「SLAM14講義」Ch7VO1PnP結果の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.頂点頂点を追加し
ます3.エッジエッジを追加します
4. Optimizer.optimize（）optimization

頂点を設定するときの注意：

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

変更後、最適化速度が大幅に向上します。比較前後は以下のとおりです。

変更前：
最適化コスト時間は0.001844386s
g2o BA最適化時間は0.002059048s
変更後：

最適化コスト時間は0.000413748sg2oBA
最適化時間は0.000505369s
速度は約5倍高速