g2o--ba代码解析

概要

g2o是常用的图优化理论c++库，其自带了很多example讲解如何使用该库文件，本文分析其中ba的示例代码。

所谓的图优化，就是把一个常规的优化问题，以图（Graph）的形式来表述。

在图中，以顶点表示优化变量，以边表示观测方程。于是总体优化问题变为n条边加和的形式(边是约束)。在具体编写g2o代码时，我们也需要明确哪些是顶点（优化项），哪些是边（约束项）。

业务场景

如上图所示，存在以下变量

f -- 路标，或者观察点，或者物体的特征点，在ba中作为顶点出现；

b -- 观测者的位置和姿态，在ba中也是作为顶点出现；

z -- f在相机所在的b的投影，在ba中也是作为边出现；

代码解析

设定求解器

  g2o::SparseOptimizer optimizer;
  optimizer.setVerbose(false);
  string solverName = "lm_fix6_3";
  if (DENSE) {
    solverName = "lm_dense6_3";
  } else {
#ifdef G2O_HAVE_CHOLMOD
    solverName = "lm_fix6_3_cholmod";
#else
    solverName = "lm_fix6_3";
#endif
  }

  g2o::OptimizationAlgorithmProperty solverProperty;
  optimizer.setAlgorithm(
      g2o::OptimizationAlgorithmFactory::instance()->construct(solverName,
                                                               solverProperty));

初始化路标位置

  vector<Vector3d> true_points;
  for (size_t i = 0; i < 500; ++i) { // 随机生成500个路标
    true_points.push_back(
        Vector3d((g2o::Sampler::uniformRand(0., 1.) - 0.5) * 3,
                 g2o::Sampler::uniformRand(0., 1.) - 0.5,
                 g2o::Sampler::uniformRand(0., 1.) + 3));
  }

设置相机的相关参数

  double focal_length = 1000.;
  Vector2d principal_point(320., 240.);

  vector<g2o::SE3Quat, aligned_allocator<g2o::SE3Quat> > true_poses;
  g2o::CameraParameters* cam_params =
      new g2o::CameraParameters(focal_length, principal_point, 0.);
  cam_params->setId(0);

  if (!optimizer.addParameter(cam_params)) {
    assert(false);
  }

初始化观测者的位置

  int vertex_id = 0;
  for (size_t i = 0; i < 15; ++i) { // 15个观测者位置
    Vector3d trans(i * 0.04 - 1., 0, 0); // 平移的距离

    Eigen::Quaterniond q;
    q.setIdentity(); // 无旋转
    g2o::SE3Quat pose(q, trans);
    g2o::VertexSE3Expmap* v_se3 = new g2o::VertexSE3Expmap();
    v_se3->setId(vertex_id);
    if (i < 2) {
      v_se3->setFixed(true); // 设置不动点，在g2o的实现中，至少存在一个不动点
    }
    v_se3->setEstimate(pose); // 设置初始化位姿，引擎在迭代过程中会更新它
    optimizer.addVertex(v_se3); // 添加至顶点，作为优化项
    true_poses.push_back(pose);
    vertex_id++;
  }

添加观测者所见路标在相机中的投影点

  for (size_t i = 0; i < true_points.size(); ++i) { // 遍历所有路标
    g2o::VertexPointXYZ* v_p = new g2o::VertexPointXYZ();
    v_p->setId(point_id);
    v_p->setMarginalized(true);
    v_p->setEstimate(true_points.at(i) +
                     Vector3d(g2o::Sampler::gaussRand(0., 1),
                              g2o::Sampler::gaussRand(0., 1),
                              g2o::Sampler::gaussRand(0., 1))); // 对初始化点进行扰动
    int num_obs = 0;
    for (size_t j = 0; j < true_poses.size(); ++j) { // 遍历观测者位姿
      Vector2d z = cam_params->cam_map(true_poses.at(j).map(true_points.at(i)));
      if (z[0] >= 0 && z[1] >= 0 && z[0] < 640 && z[1] < 480) {
        ++num_obs; // 在相机中能观察到该路标
      }
    }
    if (num_obs >= 2) { // 如果只有一个位置能观察到该路标，无法建立回环检测
      optimizer.addVertex(v_p); // 将该路标添加到顶点中
      bool inlier = true;
      for (size_t j = 0; j < true_poses.size(); ++j) { // 遍历观测者位姿 
        Vector2d z =
            cam_params->cam_map(true_poses.at(j).map(true_points.at(i))); // 计算在相机中的投影点位置，是一个二维量
        if (z[0] >= 0 && z[1] >= 0 && z[0] < 640 && z[1] < 480) {
          z += Vector2d(g2o::Sampler::gaussRand(0., PIXEL_NOISE),
                        g2o::Sampler::gaussRand(0., PIXEL_NOISE)); // 添加误差
          g2o::EdgeProjectXYZ2UV* e = new g2o::EdgeProjectXYZ2UV();
          e->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*>(v_p)); // 第一个点为当前路标
          e->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex*>(
                              optimizer.vertices().find(j)->second)); // 第二个点为当前的位姿
          e->setMeasurement(z); // 设置边的值
          e->information() = Matrix2d::Identity();
if (ROBUST_KERNEL) {
            g2o::RobustKernelHuber* rk = new g2o::RobustKernelHuber; // 设定核函数
            e->setRobustKernel(rk);
          }
          e->setParameterId(0, 0);
          optimizer.addEdge(e); // 添加边
        }
      }