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