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

 

posted @ 2019-04-17 22:54  feifanren  阅读(595)  评论(0编辑  收藏  举报