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