《SLAM十四讲》Ch7 VO1 对PnP结果BA优化的一点改进

bundleAdjustmentG2O( )函数实现

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();
}

主要步骤：
1、构建图优化，设定g2o
2、添加顶点vertex
3、添加边edges
4、optimizer.optimize( )优化

注意到设置顶点时：

vertex_pose->setEstimate(Sophus::SE3d());

这里初始值的设定“Sophus::SE3d( )”不是待优化的结果

更改如下：

Matrix3d R_eigen;
  for(int i=0;i<3;++i)
  {
    for(int j=0;j<3;++j)
    {
      R_eigen(i,j)=R.at<double>(i,j);
    }
  }  
  Vector3d t_eigen;
  for(int i=0;i<3;++i)
  {
    t_eigen<<t.at<double>(i,0);
  }

	cout << "calling bundle adjustment by g2o" << endl;
  Sophus::SE3d pose_g2o(R_eigen,t_eigen);

	vertex_pose->setEstimate(pose);

更改后显著提高优化速度，前后对比如下：

更改前：
optimization cost time为0.001844386s
g2o BA优化的时间为0.002059048s
更改后：

optimization cost time为0.000413748s
g2o BA优化的时间为0.000505369s
速度提高约5倍左右