SVO 學習筆記（三）

• 這篇博客
• Initialization
• Frame_Handler_Mono
• • processFirstFrame
  • processSecondFrame
  • processFrame
  • relocalizeFrame
• 結尾

這篇博客

?這篇博客將介紹SVO源代碼中的frame_handler_mono、initialization兩個文件的代碼流程。前者是SVO系統類，展示了整個系統的執行過程；后者則是用于系統初始化的文件。首先介紹initialization部分代碼。

Initialization

?初始化的主要任務是使用光流法完成前幾幀的跟蹤工作,設置第一、第二關鍵幀。addFirstFrame函數處理獲得的第一幀，并提取其中特征點。如果特征點數量足夠，就將這一幀看作第一關鍵幀。
? addSecondFrame函數用于判斷能否構建第二個關鍵幀。它的步驟為：
?1、獲得當前幀，對當前幀使用光流法：
?trackKlt(frame_ref_, frame_cur, px_ref_, px_cur_, f_ref_, f_cur_, disparities_);
?完成和參考幀的匹配（因為這時還沒有地圖點，所以那些圖像對齊之類的方法還不能用）。其中disparities_保存的是各匹配點所對應的視差。
?2、double disparity = vk::getMedian(disparities_);
? if(disparity < Config::initMinDisparity())
? return NO_KEYFRAME;判斷視差大小是否滿足要求（視差足夠大時才會把當前幀設為第二關鍵幀）。
?3、若當前幀被選為第二關鍵幀，則用computeHomography計算兩關鍵幀的單應矩陣，根據內點數量判斷是否跟蹤成功。然后構建內點對應的3D地圖點。
?4、跟蹤成功后，計算地圖點在當前幀中的深度中值。用這個中值表示場景的尺度（因為是單目，所以需要計算尺度）。然后設置當前幀的位姿，要考慮尺度的影響：
?frame_cur->T_f_w_ = T_cur_from_ref_ * frame_ref_->T_f_w_;
?旋轉不用考慮地圖尺度問題，平移需要考慮：
?frame_cur->T_f_w_.translation() =-frame_cur->T_f_w_.rotation_matrix()*(frame_ref_->pos() + scale*(frame_cur->pos() - frame_ref_->pos()));
?5、最后將當前幀看到的地圖點轉換到世界坐標系中，作為第一代地圖點，并將前兩個關鍵幀與這些地圖點關聯起來。

Frame_Handler_Mono

?主要介紹這個文件中的processFirstFrame、processSecondFrame、 processFrame和relocalizeFrame這四個函數。它們代表了這樣的流程：
(獲取第一關鍵幀) > （獲取第二關鍵幀，完成地圖初始化） > (處理之后關鍵幀，完成跟蹤任務) > （跟蹤丟失后進行重定位）
?這個流程在該文件中的addImage()函數（處理每次獲得的圖像）中進行。下面介紹這四個函數代碼。

processFirstFrame

?這個函數用來處理最開始的圖像，直到獲得第一個關鍵幀為止。

//處理獲得的第一個幀，并判斷該幀能否成為關鍵幀
//這個會一直使用到第一個關鍵幀構建了為止（即函數中klt...中addFirstFrame成功）
//返回結果有三種：當前幀是KF，不是KF，結果無效
FrameHandlerMono::UpdateResult FrameHandlerMono::processFirstFrame()
{//設置當前幀的初始位姿為單位變換矩陣new_frame_->T_f_w_ = SE3(Matrix3d::Identity(), Vector3d::Zero());//將當前幀添加到初始化類中，看其是否能夠作為第一關鍵幀if(klt_homography_init_.addFirstFrame(new_frame_) == initialization::FAILURE)return RESULT_NO_KEYFRAME;new_frame_->setKeyframe();//將當前幀設置為關鍵幀map_.addKeyframe(new_frame_);//更新系統的狀態，以便下次使用其他幀處理函數stage_ = STAGE_SECOND_FRAME;SVO_INFO_STREAM("Init: Selected first frame.");return RESULT_IS_KEYFRAME;
}

processSecondFrame

?這個函數在第一關鍵幀出現后開始使用，一直用到出現第二關鍵幀為止

//返回結果有三種：結果是KF，不是KF，結果無效
FrameHandlerBase::UpdateResult FrameHandlerMono::processSecondFrame()
{
//klt...是用于系統初始化的類對象initialization::InitResult res = klt_homography_init_.addSecondFrame(new_frame_);if(res == initialization::FAILURE)//使用兩幀初始化失敗return RESULT_FAILURE;else if(res == initialization::NO_KEYFRAME)//第二幀不能作為關鍵幀return RESULT_NO_KEYFRAME;//條件編譯：如果需要使用BA，就用BA優化頭兩個關鍵幀的位姿#ifdef USE_BUNDLE_ADJUSTMENTba::twoViewBA(new_frame_.get(), map_.lastKeyframe().get(), Config::lobaThresh(), &map_);#endifnew_frame_->setKeyframe();double depth_mean, depth_min;//此時第一代地圖點以由前兩個關鍵幀構建了//獲得所有地圖點在當前幀中的平均、最小深度值frame_utils::getSceneDepth(*new_frame_, depth_mean, depth_min);//將當前這個新的關鍵幀添加到深度濾波器中depth_filter_->addKeyframe(new_frame_, depth_mean, 0.5*depth_min);// add frame to mapmap_.addKeyframe(new_frame_);//更新系統狀態stage_ = STAGE_DEFAULT_FRAME;klt_homography_init_.reset();SVO_INFO_STREAM("Init: Selected second frame, triangulated initial map.");return RESULT_IS_KEYFRAME;
}

processFrame

?在成功添加了兩個關鍵幀之后，使用該函數處理后續的幀，也就是論文中描述的那幾個部分。它的大致步驟為：
?1、先用上一幀的位姿作為當前幀的初始位姿：
?new_frame_->T_f_w_ = last_frame_->T_f_w_;
?2、進行稀疏圖像對齊：
?size_t img_align_n_tracked = img_align.run(last_frame_, new_frame_);。
?3、將地圖點重投影到當前幀中，獲得更加精確的特征匹配（關鍵幀和當前幀的特征匹配）。
?reprojector_.reprojectMap(new_frame_, overlap_kfs_);
?如果匹配特征的數量較少，那么就將當前幀的位姿設置為上一幀的位姿，并設置跟蹤失敗，直接結束函數。
（PS：上面這兩步的代碼解釋可參考筆記（二））
?4、如果第3步成了，就對當前幀的估計位姿進行優化，通過最小化重投影誤差實現。先是只優化位姿，也就是所謂的“motion-only BA“。

 pose_optimizer::optimizeGaussNewton(Config::poseOptimThresh(), Config::poseOptimNumIter(), false,new_frame_, sfba_thresh, sfba_error_init, sfba_error_final, sfba_n_edges_final);

?5、之后就是結構優化，即優化地圖點的坐標（structure-only BA），也是根據最小化重投影誤差實現。這部分只優化地圖點。

optimizeStructure(new_frame_, Config::structureOptimMaxPts(), Config::structureOptimNumIter());

(PS：對位姿和地圖點分別執行優化，能很大地減少優化時間。)
?6、完成位姿和結構性優化后，開始選擇關鍵幀。先進行一個跟蹤效果判斷：

core_kfs_.insert(new_frame_);setTrackingQuality(sfba_n_edges_final);//跟蹤效果判斷if(tracking_quality_ == TRACKING_INSUFFICIENT){//跟蹤得不好，就返回“跟蹤失敗”//個人認為這樣設置是為了方便在重定位的時候找參考關鍵幀，因為之后newframe的成員會給到lastframe中//然后使用與lastframe距離最近的一部分關鍵幀來實現重定位new_frame_->T_f_w_ = last_frame_->T_f_w_; // reset to avoid crazy pose jumpsreturn RESULT_FAILURE;}

?跟蹤效果不錯時，對當前幀進行關鍵幀的判斷：

//關鍵幀的判斷double depth_mean, depth_min;//獲得當前幀中所有地圖點深度的平均和最小值frame_utils::getSceneDepth(*new_frame_, depth_mean, depth_min);//根據兩個條件判斷當前幀能否做關鍵幀：//1、跟蹤的質量好//2、當前幀距離其他共視幀的距離遠//第二條由neddNewKF決定。這個函數由將共視關鍵幀的位置坐標投影到當前幀坐標系，并通過距離判斷是否加入需要關鍵幀if(!needNewKf(depth_mean) || tracking_quality_ == TRACKING_BAD){//雖然不能做關鍵幀，但還是能用于更新深度濾波器中點的深度depth_filter_->addFrame(new_frame_);return RESULT_NO_KEYFRAME;}

?7、如果當前幀被設置為關鍵幀，則增加相應地圖點的觀測次數和觀測對象。

 for(Features::iterator it=new_frame_->fts_.begin(); it!=new_frame_->fts_.end(); ++it)if((*it)->point != NULL)(*it)->point->addFrameRef(*it);map_.point_candidates_.addCandidatePointToFrame(new_frame_);

?8、最后將當前幀作為關鍵幀添加到深度濾波器中。如果地圖和深度濾波器中的關鍵幀數量過多，則剔除掉離新關鍵幀最遠的關鍵幀（這一步之前有個條件編譯：如果定義了USE_BUNDLE_ADJUSTMENT，則使用BA優化當前幀位姿）：

// init new depth-filters.添加新的關鍵幀能幫助濾波器產生新的地圖點，以及優化已有地圖點的深度信息depth_filter_->addKeyframe(new_frame_, depth_mean, 0.5*depth_min);// if limited number of keyframes, remove the one furthest apartif(Config::maxNKfs() > 2 && map_.size() >= Config::maxNKfs()){//獲得離新關鍵幀最遠的關鍵幀，并將其從深度濾波器和地圖中剔除FramePtr furthest_frame = map_.getFurthestKeyframe(new_frame_->pos());depth_filter_->removeKeyframe(furthest_frame); map_.safeDeleteFrame(furthest_frame);}map_.addKeyframe(new_frame_);

?至此整個函數結束，當前幀處理完畢，繼續等待下一個圖像的到來。

relocalizeFrame

//以關鍵幀作為參考，完成重定位操作
//使用的是與lastframe距離最近的關鍵幀最為參考關鍵幀
FrameHandlerMono::UpdateResult FrameHandlerMono::relocalizeFrame(const SE3& T_cur_ref,FramePtr ref_keyframe)
{SVO_WARN_STREAM_THROTTLE(1.0, "Relocalizing frame");if(ref_keyframe == nullptr){SVO_INFO_STREAM("No reference keyframe.");return RESULT_FAILURE;}SparseImgAlign img_align(Config::kltMaxLevel(), Config::kltMinLevel(),30, SparseImgAlign::GaussNewton, false, false);size_t img_align_n_tracked = img_align.run(ref_keyframe, new_frame_);//若匹配上的特征個數超過30則認為重定位成功if(img_align_n_tracked > 30){SE3 T_f_w_last = last_frame_->T_f_w_;//用參考關鍵幀作為上一幀，以便進行processFrame()last_frame_ = ref_keyframe;//用關鍵幀做參考，進行定位操作FrameHandlerMono::UpdateResult res = processFrame();if(res != RESULT_FAILURE){stage_ = STAGE_DEFAULT_FRAME;SVO_INFO_STREAM("Relocalization successful.");}else//重定位失敗，用上一幀位姿作為當前幀位姿，以便繼續重定位new_frame_->T_f_w_ = T_f_w_last; // reset to last well localized pose//（其實之后很大概率還是使用這個參考關鍵幀作為重定位的參考）return res;}return RESULT_FAILURE;
}

結尾

?總算是將SVO的代碼筆記延續到了第三期，可喜可賀X3。感謝屏幕前的你耐心地看完，也感謝自己堅持了下來。如果文中有什么不足，勞煩各位博友指出，十分感謝。

總結

以上是生活随笔為你收集整理的SVO 学习笔记(三)的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。