視覺裡程計的重定位問題1——SVO的重定位部分

SVO的重定位部分代碼解析與分析

SVO的重定位功能展現在：運動跟蹤丢失後通過與上一關鍵幀比對以及地圖點投影，找回目前相機位姿。由于沒有後端和回環，SVO的重定位并不是回環校正後的重定位。

代碼部分被放在運動跟蹤線程裡，隻有寥寥幾行，作用效果十分有限。

原文代碼部分如下：

FrameHandlerMono::UpdateResult FrameHandlerMono::relocalizeFrame(  const SE3 &T_cur_ref, FramePtr ref_keyframe)
    {
        SVO_WARN_STREAM_THROTTLE(1.0, "Relocalizing frame");
        if (ref_keyframe == nullptr) {              //先判斷一下last_kf是否存在，如果上個關鍵幀是空  就直接傳回RESULT_FAILURE   <重定位失敗>
            SVO_INFO_STREAM("No reference keyframe.");
            return RESULT_FAILURE;
          }
          //這裡類似運動跟蹤的第一步： 使用上一關鍵幀對目前圖像進行對齊
        SparseAlign img_align(Config::kltMaxLevel(), Config::kltMinLevel(), 30, true, false);//false false  
        size_t img_align_n_tracked = img_align.run(ref_keyframe, new_frame_);
    //接下來進行判斷1：如果跟蹤到超過30個點
        if (img_align_n_tracked > 30) {
            SE3 T_f_w_last = last_frame_->T_f_w_; 
            last_frame_ = ref_keyframe;                      //把last_kf作為上一幀
            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;   //把上一幀的位姿賦給目前幀
                SVO_INFO_STREAM("Relocalization failure.");
            }
            return res;
        }
    //2. 如果跟蹤小于30個點   <重定位失敗>
        return RESULT_FAILURE;
    }
           

從上面的代碼看來，SVO的重定位成功的條件必要滿足：1.上一關鍵幀存在；2目前幀與上一關鍵幀的圖像對齊，跟蹤到>30個特征點；3能夠通過processFrame()的重重考驗（相鄰幀位姿優化、重投影位姿優化後的内點>門檻值）

要滿足前兩點非常容易，但重定位往往在第三點上吊死。

在實際運作過程中發現，重定位在圖像對齊環節還是可以跟蹤到的點是足夠的，但是經過重投影之後剩下的比對點數太少，以至于重定位失敗！！

再來看看啟動重定位的條件有哪些：

在整個frame_handler_mono.cpp裡面 我隻找到了當res== result_faliure 時才會進行下一幀的重定位。是以在運動跟蹤過程中，一旦某一步失敗——下一幀馬上開始重定位，要是重定位又失敗呢？——繼續下一幀重定位·· 重定位···

~ 如果載體在跟蹤丢失點附近小範圍慢速運動，由于觀測的地圖點相差并不太大，即便多次重定位失敗，還是有機會跟蹤回來的。

~ 但是更真實的情況是，機體運動一下子超出範圍，開足馬力拍屁股走人，然後重定位再也無法跟蹤上，視覺跟蹤宣布失敗！！！

~ 并且重定位一直假定目前幀位姿為上一個有效跟蹤點地位姿，當一直重定位不上時，可以通過可視化看見機體位姿不再移動也不再增加，而是一直在丢失點閃啊閃啊閃···

跟蹤品質不佳的判定

在運動跟蹤過程中，若不滿足給定名額要求，則會判定tracking_quality_ = TRACKING_INSUFFICIENT

出現以下幾種情況都将被判定為跟蹤品質不佳：

1.num_observation < Config::qualityMinFts()

[num_observations實為位姿優化後保留下來的特征點數]

2.feature_drop / init_match_number >0.6

[feature_drop = init_match_number -num_observation ]

重投影後目前幀與地圖的比對點數 - 優化後剩下的特征點數 = 優化過程中特征丢失的數量

即：位姿優化後丢失的特征點數不能超過優化前的3/5，不然就認為跟蹤品質不佳

3.repr_n_new_references <Config::qualityMinFts()

此處為重投影之後，若投影成功的地圖點數量小于門檻值，也判定為跟蹤效果不佳。這裡有點小疑問，此處的repr_n_new_references 應該就是第二個條件裡的init_match_number，個人認為這裡的門檻值應該要比條件1的門檻值要高才行，否則丢特征前後的特征點數量名額相同，這有點不太合理？？

4. if( dropout == RESULT_FAILURE && (stage_ == STAGE_DEFAULT_FRAME || stage_ == STAGE_RELOCALIZING))

{stage_ = STAGE_RELOCALIZING;

tracking_quality_ = TRACKING_INSUFFICIENT }

在每一幀結束處理的時候，也會進行一次判斷:如果處理預設幀或處理重定位幀的結果為 res 等于 RESULT_FAILURE ；那麼判定跟蹤品質不好，并在下一幀進行重定位。

在判定目前跟蹤品質不好之後，為了避免位姿出現較大跳變，一般會令目前幀位姿為上一幀位姿

并且在下一幀的時候開啟重定位！！

總結起來，SVO 的重定位部分問題比較多，丢失之後基本無法重定位回來，有很多地方值得修改