SVO 學(xué)習(xí)筆記（三）

• 這篇博客
• Initialization
• Frame_Handler_Mono
• • processFirstFrame
  • processSecondFrame
  • processFrame
  • relocalizeFrame
• 結(jié)尾

這篇博客

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

Initialization

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

Frame_Handler_Mono

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

processFirstFrame

?這個(gè)函數(shù)用來處理最開始的圖像，直到獲得第一個(gè)關(guān)鍵幀為止。

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

processSecondFrame

?這個(gè)函數(shù)在第一關(guān)鍵幀出現(xiàn)后開始使用，一直用到出現(xiàn)第二關(guān)鍵幀為止

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

processFrame

?在成功添加了兩個(gè)關(guān)鍵幀之后，使用該函數(shù)處理后續(xù)的幀，也就是論文中描述的那幾個(gè)部分。它的大致步驟為：
?1、先用上一幀的位姿作為當(dāng)前幀的初始位姿：
?new_frame_->T_f_w_ = last_frame_->T_f_w_;
?2、進(jìn)行稀疏圖像對齊：
?size_t img_align_n_tracked = img_align.run(last_frame_, new_frame_);。
?3、將地圖點(diǎn)重投影到當(dāng)前幀中，獲得更加精確的特征匹配（關(guān)鍵幀和當(dāng)前幀的特征匹配）。
?reprojector_.reprojectMap(new_frame_, overlap_kfs_);
?如果匹配特征的數(shù)量較少，那么就將當(dāng)前幀的位姿設(shè)置為上一幀的位姿，并設(shè)置跟蹤失敗，直接結(jié)束函數(shù)。
（PS：上面這兩步的代碼解釋可參考筆記（二））
?4、如果第3步成了，就對當(dāng)前幀的估計(jì)位姿進(jìn)行優(yōu)化，通過最小化重投影誤差實(shí)現(xiàn)。先是只優(yōu)化位姿，也就是所謂的“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、之后就是結(jié)構(gòu)優(yōu)化，即優(yōu)化地圖點(diǎn)的坐標(biāo)（structure-only BA），也是根據(jù)最小化重投影誤差實(shí)現(xiàn)。這部分只優(yōu)化地圖點(diǎn)。

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

(PS：對位姿和地圖點(diǎn)分別執(zhí)行優(yōu)化，能很大地減少優(yōu)化時(shí)間。)
?6、完成位姿和結(jié)構(gòu)性優(yōu)化后，開始選擇關(guān)鍵幀。先進(jìn)行一個(gè)跟蹤效果判斷：

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

?跟蹤效果不錯(cuò)時(shí)，對當(dāng)前幀進(jìn)行關(guān)鍵幀的判斷：

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

?7、如果當(dāng)前幀被設(shè)置為關(guān)鍵幀，則增加相應(yīng)地圖點(diǎn)的觀測次數(shù)和觀測對象。

 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、最后將當(dāng)前幀作為關(guān)鍵幀添加到深度濾波器中。如果地圖和深度濾波器中的關(guān)鍵幀數(shù)量過多，則剔除掉離新關(guān)鍵幀最遠(yuǎn)的關(guān)鍵幀（這一步之前有個(gè)條件編譯：如果定義了USE_BUNDLE_ADJUSTMENT，則使用BA優(yōu)化當(dāng)前幀位姿）：

// init new depth-filters.添加新的關(guān)鍵幀能幫助濾波器產(chǎn)生新的地圖點(diǎn)，以及優(yōu)化已有地圖點(diǎn)的深度信息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()){//獲得離新關(guān)鍵幀最遠(yuǎn)的關(guān)鍵幀，并將其從深度濾波器和地圖中剔除FramePtr furthest_frame = map_.getFurthestKeyframe(new_frame_->pos());depth_filter_->removeKeyframe(furthest_frame); map_.safeDeleteFrame(furthest_frame);}map_.addKeyframe(new_frame_);

?至此整個(gè)函數(shù)結(jié)束，當(dāng)前幀處理完畢，繼續(xù)等待下一個(gè)圖像的到來。

relocalizeFrame

//以關(guān)鍵幀作為參考，完成重定位操作
//使用的是與lastframe距離最近的關(guān)鍵幀最為參考關(guān)鍵幀
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_);//若匹配上的特征個(gè)數(shù)超過30則認(rèn)為重定位成功if(img_align_n_tracked > 30){SE3 T_f_w_last = last_frame_->T_f_w_;//用參考關(guān)鍵幀作為上一幀，以便進(jìn)行processFrame()last_frame_ = ref_keyframe;//用關(guān)鍵幀做參考，進(jìn)行定位操作FrameHandlerMono::UpdateResult res = processFrame();if(res != RESULT_FAILURE){stage_ = STAGE_DEFAULT_FRAME;SVO_INFO_STREAM("Relocalization successful.");}else//重定位失敗，用上一幀位姿作為當(dāng)前幀位姿，以便繼續(xù)重定位new_frame_->T_f_w_ = T_f_w_last; // reset to last well localized pose//（其實(shí)之后很大概率還是使用這個(gè)參考關(guān)鍵幀作為重定位的參考）return res;}return RESULT_FAILURE;
}

結(jié)尾

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

總結(jié)

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

如果覺得生活随笔網(wǎng)站內(nèi)容還不錯(cuò)，歡迎將生活随笔推薦給好友。