视觉slam十四讲第ch7-PNP-3d2d手写位姿估计代码详解

视觉slam十四讲第ch7 PNP 3d2d手写位姿估计代码详解

最近在学习视觉slam十四讲，不管是书上内容还是高博视频，描述的都很细致。关于此书的资料博客也可以比较方便的在网上找到。但是因为此书除了第二版，一些代码什么的是第二版独有的，网上资料还没那么丰富。
今天在学到手写位姿估计的内容时就发现内容和代码都似懂非懂，网上的资料又不充分，因此花了很多时间去解决，现把pose_estimation_3d2d.cpp的代码手写高斯牛顿法片段和我的注释版附上，不一定对，但是应该会对初学者有一些帮助。

void bundleAdjustmentGaussNewton(
  const VecVector3d &points_3d,
  const VecVector2d &points_2d,
  const Mat &K,
  Sophus::SE3d &pose)//传入P点，P点像素在I2投影，内参;初始pose,在此程序中初始pose为0;
  {
  typedef Eigen::Matrix<double, 6, 1> Vector6d;
  const int iterations = 10;
  double cost = 0, lastCost = 0;
  double fx = K.at<double>(0, 0);
  double fy = K.at<double>(1, 1);
  double cx = K.at<double>(0, 2);
  double cy = K.at<double>(1, 2);
  for (int iter = 0; iter < iterations; iter++) {
    Eigen::Matrix<double, 6, 6> H = Eigen::Matrix<double, 6, 6>::Zero();
    Vector6d b = Vector6d::Zero();
    cost = 0;
    // compute cost
    for (int i = 0; i < points_3d.size(); i++) {
      Eigen::Vector3d pc = pose * points_3d[i];//pose即待优化的运动的位姿，pc是P撇，即变换到I2相机坐标系下的空间点P
      // Vector6d se3 = pose.log();
      // cout<<"se3 = "<<se3.transpose()<<endl;一开始se3为0,三四次迭代后趋于稳定
      double inv_z = 1.0 / pc[2];
      double inv_z2 = inv_z * inv_z;
      Eigen::Vector2d proj (fx * pc[0] / pc[2] + cx, fy * pc[1] / pc[2] + cy);//P点做投影到I2平面，存入proj中
      Eigen::Vector2d e = points_2d[i] - proj;//作差，得到差值;是观测值-预测值！
      cost += e.squaredNorm();//误差的二范数的平方
      Eigen::Matrix<double, 2, 6> J;//2*6的雅克比矩阵，即误差相对于位姿求导，通过导数可以知道有了误差以后我们应该往哪个方向去优化
      J << -fx * inv_z,
        0,
        fx * pc[0] * inv_z2,
        fx * pc[0] * pc[1] * inv_z2,
        -fx - fx * pc[0] * pc[0] * inv_z2,
        fx * pc[1] * inv_z,
        0,
        -fy * inv_z,
        fy * pc[1] * inv_z,
        fy + fy * pc[1] * pc[1] * inv_z2,
        -fy * pc[0] * pc[1] * inv_z2,
        -fy * pc[0] * inv_z;
      H += J.transpose() * J;//高斯牛顿方法，H*dx=b
      b += -J.transpose() * e;
    }
    Vector6d dx;
    dx = H.ldlt().solve(b);//对H做LDLT分解，并求解dx
    // dx = H.qr().solve(b);//对H做LDLT分解，并求解 //显然不行
    cout <<"dx:" << endl << dx <<endl;
    cout <<"dx.norm():" << endl << dx.norm() <<endl;
    if (isnan(dx[0])) {
      cout << "result is nan!" << endl;
      break;
    }
    if (iter > 0 && cost >= lastCost) {
      // cost increase, update is not good//发散的情况
      cout << "cost: " << cost << ", last cost: " << lastCost << endl;
      break;
    }
    // update your estimation
    pose = Sophus::SE3d::exp(dx) * pose;
    lastCost = cost;
    cout << "iteration " << iter << " cost=" << cout.precision(12) << cost << endl;
    if (dx.norm() < 1e-6) {
      // converge 当dx<1e-6时停止迭代
      break;
    }
  }
  cout << "pose by g-n: \n" << pose.matrix() << endl;
}