学习笔记 - SLAM十四讲 - 第七讲 - g2o求解PnP问题代码解析（pose_estimation_3d2d.cpp）

看到这里忘了之前的 看完这里理解了之前的（第六讲g2o的时候） 记录一下（只截取g2o部分）

（代码是书上的 完整代码去github拿 我只是做注释解析）

/// vertex and edges used in g2o ba
class VertexPose : public g2o::BaseVertex<6, Sophus::SE3d> {
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
//节点 第二个相机的位姿节点 这个是要求的
  virtual void setToOriginImpl() override { //节点重置函数 重置为位姿矩阵即可
    _estimate = Sophus::SE3d();
  }

  /// left multiplication on SE3
  virtual void oplusImpl(const double *update) override {
//节点的更新函数 这个函数要定义的是增量如何加到现有的估计上
    Eigen::Matrix<double, 6, 1> update_eigen; // 这里先用update_eigen将double数组update转化成矩阵
    update_eigen << update[0], update[1], update[2], update[3], update[4], update[5];
    _estimate = Sophus::SE3d::exp(update_eigen) * _estimate;
// 为什么要这样更新 第四讲中的位姿李代数扰动的更新是用左乘
// 这里的exp不是数学的exp函数 而是sophus的exp表示将矩阵转化为李群格式（即能与上面重置的_estimate能够相乘的格式）可以看一下下面的图复习一下
  }

  virtual bool read(istream &in) override {} //不更新

  virtual bool write(ostream &out) const override {}
};

class EdgeProjection : public g2o::BaseUnaryEdge<2, Eigen::Vector2d, VertexPose> {
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW;

  EdgeProjection(const Eigen::Vector3d &pos, const Eigen::Matrix3d &K) : _pos3d(pos), _K(K) {}
//边：每个3d点在第二个相机中的投影  点的输入数据：pos 相机：K
  virtual void computeError() override { //计算误差的函数 误差e = 预测值 - 观测值
    const VertexPose *v = static_cast<VertexPose *> (_vertices[0]);  //创建一个顶点对象
    Sophus::SE3d T = v->estimate(); //返回当前的估计值 返回格式即_estimate的格式
    Eigen::Vector3d pos_pixel = _K * (T * _pos3d); //3d点位置转为第二个相机的像素位置（未归一化）
    pos_pixel /= pos_pixel[2]; //归一化处理
    _error = _measurement - pos_pixel.head<2>();  //预测值-观测值（实际值）_measurement为这一次的预测值  这里这个<2>有点吓人 我去输出了一下（看后面的图）发现其实就是取前两个值作为矩阵 计算出来的error为一个2维向量 保存的是像素坐标的误差
  }

  virtual void linearizeOplus() override { //边的雅可比计算函数 计算每条边相对于顶点的雅可比
    const VertexPose *v = static_cast<VertexPose *> (_vertices[0]);  //创建一个顶点对象
    Sophus::SE3d T = v->estimate();  //同上
    Eigen::Vector3d pos_cam = T * _pos3d; //第二个相机坐标系下空间点的坐标即x'y'z'
    double fx = _K(0, 0); //从相机内参矩阵中读各参数
    double fy = _K(1, 1);
    double cx = _K(0, 2);
    double cy = _K(1, 2);
    double X = pos_cam[0]; 
    double Y = pos_cam[1];
    double Z = pos_cam[2];
    double Z2 = Z * Z;
    _jacobianOplusXi //这个函数输出的为雅可比矩阵 直接把推导结果输进来即可
      << -fx / Z, 0, fx * X / Z2, fx * X * Y / Z2, -fx - fx * X * X / Z2, fx * Y / Z,
      0, -fy / Z, fy * Y / (Z * Z), fy + fy * Y * Y / Z2, -fy * X * Y / Z2, -fy * X / Z;
  }

  virtual bool read(istream &in) override {}

  virtual bool write(ostream &out) const override {}

private:
  Eigen::Vector3d _pos3d;
  Eigen::Matrix3d _K;
};

void bundleAdjustmentG2O(
  const VecVector3d &points_3d,
  const VecVector2d &points_2d,
  const Mat &K,
  Sophus::SE3d &pose) {

  // 构建图优化，先设定g2o
  typedef g2o::BlockSolver<g2o::BlockSolverTraits<6, 3>> BlockSolverType;  // pose is 6, landmark is 3
  typedef g2o::LinearSolverDense<BlockSolverType::PoseMatrixType> LinearSolverType; // 线性求解器类型
  // 梯度下降方法，可以从GN, LM, DogLeg 中选
  auto solver = new g2o::OptimizationAlgorithmGaussNewton(
    g2o::make_unique<BlockSolverType>(g2o::make_unique<LinearSolverType>()));
  g2o::SparseOptimizer optimizer;     // 图模型
  optimizer.setAlgorithm(solver);   // 设置求解器
  optimizer.setVerbose(true);       // 打开调试输出

  // vertex
  VertexPose *vertex_pose = new VertexPose(); // camera vertex_pose
  vertex_pose->setId(0);
  vertex_pose->setEstimate(Sophus::SE3d()); //设定初值
  optimizer.addVertex(vertex_pose);

  // K
  Eigen::Matrix3d K_eigen;
  K_eigen <<
          K.at<double>(0, 0), K.at<double>(0, 1), K.at<double>(0, 2),
    K.at<double>(1, 0), K.at<double>(1, 1), K.at<double>(1, 2),
    K.at<double>(2, 0), K.at<double>(2, 1), K.at<double>(2, 2);

  // edges
  int index = 1;
  for (size_t i = 0; i < points_2d.size(); ++i) { //把每条边都读入
    auto p2d = points_2d[i];
    auto p3d = points_3d[i];
    EdgeProjection *edge = new EdgeProjection(p3d, K_eigen); //创建边对象 输入的是三维点和内参矩阵
    edge->setId(index);
    edge->setVertex(0, vertex_pose); //把每条边都堆到那一个顶点上（第二个相机的位姿节点）
    edge->setMeasurement(p2d); //给出实际测量值
    edge->setInformation(Eigen::Matrix2d::Identity()); //信息矩阵 这里没有噪声 初始化一个矩阵即可 如果有噪声 则这里输入高斯的协方差矩阵之逆
    optimizer.addEdge(edge);
    index++;
  }

  chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
  optimizer.setVerbose(true);
  optimizer.initializeOptimization();
  optimizer.optimize(10); //这个数据我有改过 其实就是迭代的次数
  chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
  chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>>(t2 - t1);
  cout << "optimization costs time: " << time_used.count() << " seconds." << endl;
  cout << "pose estimated by g2o =\n" << vertex_pose->estimate().matrix() << endl;
  pose = vertex_pose->estimate(); //这就是真实的位姿了
}

图源：https://blog.csdn.net/HHB791829200/article/details/123738022