3d-2d g2o基本流程 以及 BlockSolverTraits的定义

讲解

在使用g2o的时候，通常要线设定g2o的顶点类型和边类型。
其中BlockSolverTraits<M,N>参数总是让人琢磨不透。
以高翔SLAM14讲，利用g2o进行BA优化，求解PnP为例
直接给出解答。

在这个问题中，给出的信息有图像上的特征点的二维像素坐标，这些特征点所对应的世界坐标系中的3维坐标，相机参数，最后求解的是世界坐标系到相机坐标系的相机位姿T（李群）。

首先要明确问题，节点有哪些，边（误差方程）怎么定义。

在这个问题中，特征点的三维坐标，看作是固定不动的。

节点就包括6维的转换矩阵T（需要优化）和3维的特征点三维坐标P（不优化）
（BlockSolverTraits<M,N>的参数就为6和3）
查了国外的文档，M，N分别为两个顶点块的维度，第二个参数并不是边的维度。

边，所连接的顶点为T（节点、需要优化）和P（作为初始参数传入）

误差方程就是 slam14讲2 上写的（7.35式）

这里，图像上的二维像素坐标，就可以作为 _measurement 传入边中，接着把三维坐标P和相机外参K，都作为初始参数传入，就可以计算出 _error了。

边中还要在 linearizeOplus中，得到 _jacobianOplusXi 参数，即雅可比函数。需要根据估计的T值计算得到。

代码

/// vertex and edges used in g2o ba
//VertexPose 顶点，维度6，类型 SE3d
class VertexPose : public g2o::BaseVertex<6, Sophus::SE3d> {
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW;

//估计得到的结果 SE3d
  virtual void setToOriginImpl() override {
    _estimate = Sophus::SE3d();
  }

  //左乘，来更新
  virtual void oplusImpl(const double *update) override {
  //更新的量，Eigen矩阵 6*1
    Eigen::Matrix<double, 6, 1> update_eigen;
    update_eigen << update[0], update[1], update[2], update[3], update[4], update[5];
    //Sophus李代数exp 左乘
    _estimate = Sophus::SE3d::exp(update_eigen) * _estimate;
  }

  virtual bool read(istream &in) override {}

  virtual bool write(ostream &out) const override {}
};
//EdgeProjection BaseUnaryEdge模板 误差维度2,误差类型Vector2d,连接的顶点为VerterxPose
class EdgeProjection : public g2o::BaseUnaryEdge<2, Eigen::Vector2d, VertexPose> {
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
//初始化输入参数为，三维坐标position和相机参数K
  EdgeProjection(const Eigen::Vector3d &pos, const Eigen::Matrix3d &K) : _pos3d(pos), _K(K) {}

//计算误差
  virtual void computeError() override {
  //得到顶点
    const VertexPose *v = static_cast<VertexPose *> (_vertices[0]);
    //估计的T
    Sophus::SE3d T = v->estimate();
    //像素坐标系下坐标
    Eigen::Vector3d pos_pixel = _K * (T * _pos3d);
    //归一化结果（u,v,1）
    pos_pixel /= pos_pixel[2];
    //测量误差，由_measurement得到
    _error = _measurement - pos_pixel.head<2>();
  }

//线性化，得到雅可比矩阵
  virtual void linearizeOplus() override {
  //得到顶点
    const VertexPose *v = static_cast<VertexPose *> (_vertices[0]);
    //估计的的T
    Sophus::SE3d T = v->estimate();
    //相机坐标系下，三维坐标
    Eigen::Vector3d pos_cam = T * _pos3d;
    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;
};

//求解g2o

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;  //相机位姿维度6，特征点三维坐标维度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
  //设置id
  vertex_pose->setId(0);
  //初始估计值为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);
    //设置id
    edge->setId(index);
    //设置顶点，将vertex_pose连接到_vertex[0]
    edge->setVertex(0, vertex_pose);
    //设置观察值，_measureament=p2d
    edge->setMeasurement(p2d);
    //_information为协方差矩阵的逆，信息矩阵。
    edge->setInformation(Eigen::Matrix2d::Identity());
    optimizer.addEdge(edge);
    index++;
  }

  chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
  optimizer.setVerbose(true);
  //初始化优化器
  optimizer.initializeOptimization();
  //优化10次
  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();
}