PL-SVO公式推导及代码解析:位姿优化

通过跳过极线约束单独优化图像中每个特征的位置后，必须通过最小化3D特征与图像中相应的2D特征位置之间的重投影误差来进一步细化（3）中获得的相机姿态（ 见图5）。为此，我们考虑在世界坐标系中3D特征和相机姿势Tk，w之间的重投影误差，因为它大大减少了估计轨迹的漂移。

  // pose optimization
  SVO_START_TIMER("pose_optimizer");
  size_t sfba_n_edges_final, sfba_n_edges_final_pt, sfba_n_edges_final_ls;
  double sfba_thresh, sfba_error_init, sfba_error_final;
  pose_optimizer::optimizeGaussNewton(
      Config::poseOptimThresh(), Config::poseOptimNumIter(), false,
      new_frame_, sfba_thresh, sfba_error_init, sfba_error_final, sfba_n_edges_final_pt, sfba_n_edges_final_ls);
  SVO_STOP_TIMER("pose_optimizer");