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

巴特西

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

最新文章

热门文章