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