DSO(3)——幀間跟蹤

寫在前面

上一篇記錄了DSO的初始化階段，本文主要記錄下對於新幀的追蹤，即tracking，整個部分其實不是很難，主要的思路也簡單，就是由粗到細的進行光度誤差的最小化，以此來計算位姿。

特點

雖然都是最小化光度誤差，這裏先列出DSO做的很不同的地方：

放棄使用了像素塊的方法，這個個人深有感觸，一般而言雖然像素塊的大小不大，但是耐不住點多啊，多數情況下如果用像素塊的話這個地方就變成了十分花費時間；
當某一層的初始位姿不好的時候，DSO並不放棄"治療"，反而會給足了機會去優化，但是如果優化出來的結果與閾值差距太大，那麼就直接放棄了；
作者假設了5種運動模型：勻速、倍速、半速、零速以及沒有運動；
與此同時，作者假定了有26×N種旋轉情況（四元數的26種旋轉情況，N個比較小的角度），作者認爲是在丟失的情況下，這樣的方法會十分有用，如果沒有丟失應該前五種假設就夠了；

深入探討

整個跟蹤函數僅有一個，即代碼中的trackNewCoarse函數，該函數主要做了四件事：

準備相應的運動初值，詳細來說就是五種運動假設和N種旋轉假設；
對每一個運動假設進行L-M迭代，求得最佳的位姿以及對應的能量；
更新最優結果；
更新其它變量；

下面逐步進行說明：

A. 準備運動初值

這個部分用文字說明比較蒼白，這裏配上一張圖會比較清晰：

如圖所示，圖中的變量使用的都是程序中的變量名稱，最終需要的運動初值爲紅線所示的LastF_2_fh，然後作者使用的參考運動值爲sprelast_2_slast，假設爲slast_2_fh，因此五種模型如下：

勻速模型： $T_{lastF}^{fh} = T_{slast}^{fh}*T_{lastF}^{slast}$ ；
倍速模型： $T_{lastF}^{fh} = T_{slast}^{fh}*T_{lastF}^{fh}$ ，相當於說勻速的從slast幀運動到當前幀，之後以當前幀爲起點再勻速運動一次；
半速模型： $T_{lastF}^{fh} = 0.5×T_{slast}^{fh}*T_{lastF}^{slast}$ ；
零速模型： $T_{lastF}^{fh} = T_{lastF}^{slast}$ ；
不動模型： $T_{lastF}^{fh} = T_I$ ；

隨後就是26×3種旋轉模型，這裏不在贅述，代碼如下：

shared_ptr<FrameHessian> lastF = coarseTracker->lastRef; // last key frame
shared_ptr<Frame> slast = allFrameHistory[allFrameHistory.size() - 2];
shared_ptr<Frame> sprelast = allFrameHistory[allFrameHistory.size() - 3];

SE3 slast_2_sprelast;
SE3 lastF_2_slast;
{    // lock on global pose consistency!
    unique_lock<mutex> crlock(shellPoseMutex);
    slast_2_sprelast = sprelast->getPose() * slast->getPose().inverse();
    lastF_2_slast = slast->getPose() * lastF->frame->getPose().inverse();
    aff_last_2_l = slast->aff_g2l;
}
SE3 fh_2_slast = slast_2_sprelast;// assumed to be the same as fh_2_slast.

// get last delta-movement.
lastF_2_fh_tries.push_back(fh_2_slast.inverse() * lastF_2_slast);    // assume constant motion.
lastF_2_fh_tries.push_back(fh_2_slast.inverse() * fh_2_slast.inverse() * lastF_2_slast);    // assume double motion (frame skipped)
lastF_2_fh_tries.push_back(SE3::exp(fh_2_slast.log() * 0.5).inverse() * lastF_2_slast); // assume half motion.
lastF_2_fh_tries.push_back(lastF_2_slast); // assume zero motion.
lastF_2_fh_tries.push_back(SE3()); // assume zero motion FROM KF.


// just try a TON of different initializations (all rotations). In the end,
// if they don't work they will only be tried on the coarsest level, which is super fast anyway.
// also, if tracking rails here we loose, so we really, really want to avoid that.
for (float rotDelta = 0.02;rotDelta < 0.05; rotDelta += 0.01) {    // TODO changed this into +=0.01 where DSO writes ++
    lastF_2_fh_tries.push_back(fh_2_slast.inverse() * lastF_2_slast *
                               SE3(Sophus::Quaterniond(1, rotDelta, 0, 0),
                                   Vec3(0, 0, 0)));            // assume constant motion.
    ...
    lastF_2_fh_tries.push_back(fh_2_slast.inverse() * lastF_2_slast *
                                   SE3(Sophus::Quaterniond(1, rotDelta, rotDelta, rotDelta),
                                       Vec3(0, 0, 0)));    // assume constant motion.
}

這裏說明一點的是，雖然註釋上寫說旋轉模型僅僅在金字塔最頂層進行優化，但是在代碼上個人並沒有發現。

B. 位姿優化

對於每一個運動假設，算法從金字塔的最頂層由粗到細的進行位姿優化，大致步驟如下（主要是coarseTracker->trackNewestCoarse函數部分）：

從上到下遍歷每一層；

對於每一層使用L-M優化的方式，不過對於初始化的殘差和增量方程，作者採取了及其容忍的態度，這大概率是因爲在計算光度誤差的時候，作者僅僅使用一個點而不是像素塊的方式，具體方法是每次都會判斷超出閾值的點所佔的比例，如果比例過大（60%），則增加閾值（2倍）並重新來過，直到滿足條件或者閾值增大到一定程度，代碼如下：

Vec6 resOld = calcRes(lvl, refToNew_current, aff_g2l_current, setting_coarseCutoffTH * levelCutoffRepeat);
// 如果誤差大的點的比例大於60%， 那麼增大閾值再計算N次
while (resOld[5] > 0.6 && levelCutoffRepeat < 50) {
    // more than 60% is over than threshold, then increate the cut off threshold
    levelCutoffRepeat *= 2;
    resOld = calcRes(lvl, refToNew_current, aff_g2l_current, setting_coarseCutoffTH * levelCutoffRepeat);
}

// Compute H and b
// 內部也是用SSE實現的，公式可以參考上一篇文章
// 其中用到的參考幀的東西都在參考幀添加的時候準備好了
calcGSSSE(lvl, H, b, refToNew_current, aff_g2l_current);

進行L-M算法，這裏都比較正常，代碼如下：代碼中不太明白的是作者在求解出增量了之後，爲什麼又與權重做了積？

for (int iteration = 0; iteration < maxIterations[lvl]; iteration++) {
    Mat88 Hl = H;
    for (int i = 0; i < 8; i++) Hl(i, i) *= (1 + lambda);
    Vec8 inc = Hl.ldlt().solve(-b);

    // depends on the mode, if a,b is fixed, don't estimate them
    if (setting_affineOptModeA < 0 && setting_affineOptModeB < 0)    // fix a, b
    {
        inc.head<6>() = Hl.topLeftCorner<6, 6>().ldlt().solve(-b.head<6>());
        inc.tail<2>().setZero();
    }
    if (!(setting_affineOptModeA < 0) && setting_affineOptModeB < 0)    // fix b
    {
        inc.head<7>() = Hl.topLeftCorner<7, 7>().ldlt().solve(-b.head<7>());
        inc.tail<1>().setZero();
    }
    if (setting_affineOptModeA < 0 && !(setting_affineOptModeB < 0))    // fix a
    {
        Mat88 HlStitch = Hl;
        Vec8 bStitch = b;
        HlStitch.col(6) = HlStitch.col(7);
        HlStitch.row(6) = HlStitch.row(7);
        bStitch[6] = bStitch[7];
        Vec7 incStitch = HlStitch.topLeftCorner<7, 7>().ldlt().solve(-bStitch.head<7>());
        inc.setZero();
        inc.head<6>() = incStitch.head<6>();
        inc[6] = 0;
        inc[7] = incStitch[6];
    }

    float extrapFac = 1;
    if (lambda < lambdaExtrapolationLimit)
        extrapFac = sqrtf(sqrt(lambdaExtrapolationLimit / lambda));
    inc *= extrapFac;

    // 這裏爲什麼要再乘一個scale
    Vec8 incScaled = inc;
    incScaled.segment<3>(0) *= SCALE_XI_ROT;
    incScaled.segment<3>(3) *= SCALE_XI_TRANS;
    incScaled.segment<1>(6) *= SCALE_A;
    incScaled.segment<1>(7) *= SCALE_B;

    if (!std::isfinite(incScaled.sum())) incScaled.setZero();

    // left multiply the pose and add to a,b
    SE3 refToNew_new = SE3::exp((Vec6) (incScaled.head<6>())) * refToNew_current;
    AffLight aff_g2l_new = aff_g2l_current;
    aff_g2l_new.a += incScaled[6];
    aff_g2l_new.b += incScaled[7];

    // calculate new residual after this update step
    Vec6 resNew = calcRes(lvl, refToNew_new, aff_g2l_new, setting_coarseCutoffTH * levelCutoffRepeat);

    // decide whether to accept this step
    // res[0]/res[1] is the average energy
    bool accept = (resNew[0] / resNew[1]) < (resOld[0] / resOld[1]);

    if (accept) {
        // decrease lambda
        calcGSSSE(lvl, H, b, refToNew_new, aff_g2l_new);
        resOld = resNew;
        aff_g2l_current = aff_g2l_new;
        refToNew_current = refToNew_new;
        lambda *= 0.5;
    } else {
        // increase lambda in LM
        lambda *= 4;
        if (lambda < lambdaExtrapolationLimit) lambda = lambdaExtrapolationLimit;
    }

    // terminate if increment is small
    if (!(inc.norm() > 1e-3)) {
        break;
    }
} // end of L-M iteration

優化完成之後查看一下該層最終的標準差，如果標準差大於閾值的1.5倍時，認爲該次優化失敗了，直接退出，這裏閾值是動態調節的，假設這是對第k+1個運動假設進行優化，0～k次得到的最優運動假設誤差爲E（閾值就是這個），那麼該次優化的誤差如果超過了NE（作者使用N=1.5），那就沒必要再優化了，直接用最優的結果就好了，幫助刪除一些不必要的優化時間，但是如果沒有超過NE，就認爲該運動假設可以繼續進行優化；除此之外，如果該層的初始誤差狀態並不佳，但是最終的誤差確實在NE範圍中，那麼說明這個初值還有希望，就再優化一遍，不過這個機會是整個運動假設優化過程中唯一的一次機會，用掉了就沒有了。代碼如下：
```
// set last residual for that level, as well as flow indicators.
// 看一下標準差，如果標準差大於1.5倍的閾值，那麼認爲優化失敗
lastResiduals[lvl] = sqrtf((float) (resOld[0] / resOld[1]));
lastFlowIndicators = resOld.segment<3>(2);
if (lastResiduals[lvl] > 1.5 * minResForAbort[lvl])
   return false;

// repeat this level level
// 當初始位姿不好的時候，
if (levelCutoffRepeat > 1 && !haveRepeated) {
   lvl++;
   haveRepeated = true;
}
```

C. 更新最優變量

經歷上面的優化過程後，如果沒有什麼問題，此時我們就獲得了一個能量（也就是整體的誤差水平），如果本次優化在金字塔第0層的誤差小於上次的第0層誤差（第0層着實很重要），那麼算法認爲這是一個更好的結果，就更新（B4）步驟中的閾值爲當前的各層誤差；進一步，如果本次的金字塔第0層誤差水平在上一幀的誤差水平的1.5倍之內，那麼就認爲這個就是最優的，直接退出計算，代碼如下：

if (trackingIsGood && 
    std::isfinite((float) coarseTracker->lastResiduals[0]) &&
    !(coarseTracker->lastResiduals[0] >= achievedRes[0])) {
    flowVecs = coarseTracker->lastFlowIndicators;
    aff_g2l = aff_g2l_this;
    lastF_2_fh = lastF_2_fh_this;
    haveOneGood = true;
}

// take over achieved res (always).
if (haveOneGood) {
    for (int i = 0; i < 5; i++) {
        if (!std::isfinite((float) achievedRes[i]) ||
            achievedRes[i] > coarseTracker->lastResiduals[i])    // take over if achievedRes is either bigger or NAN.
            achievedRes[i] = coarseTracker->lastResiduals[i];
    }
}

// 如果當次的優化結果是上一幀結果的N倍之內，認爲這就是最優的
if (haveOneGood && achievedRes[0] < lastCoarseRMSE[0] * setting_reTrackThreshold)
    break;

D. 更新其他變量

最後，如果上面三個步驟都能如期運行，那麼我們就已經跟蹤上了前一個關鍵幀；但是如果上面的步驟並沒有給出一個很好的結果，那麼算法將勻速假設作爲最好的假設並設置爲當前的位姿。最後就是講當前的誤差水平更新爲保存變量供之後的過程使用。代碼如下：

if (!haveOneGood) {
    LOG(WARNING) << "BIG ERROR! tracking failed entirely. Take predicted pose and hope we may somehow recover." << endl;
    flowVecs = Vec3(0, 0, 0);
    aff_g2l = aff_last_2_l;
    lastF_2_fh = lastF_2_fh_tries[0];
}

lastCoarseRMSE = achievedRes;

總結

整體來看，DSO在進行位姿跟蹤的時候確實花了不少心思，一是準備了很多運動假設，二是給每一個假設很足的機會，能初始化就初始化，實在不行才放棄。不過這麼做也必然會花費不少時間。

DSO(3)——幀間跟蹤

寫在前面

特點

深入探討

A. 準備運動初值

B. 位姿優化

C. 更新最優變量

D. 更新其他變量

總結

VINS-Mono理論推導2

VINS-Mono理論推導1

DSO(2)——初始化階段

概率相關總結

DSO(4)——窗口優化

Mac下配置sublime實現LaTeX

https://yachay.unat.edu.pe/blog/index.php?comment_area=format_blog&comment_component=blog&comment_co

linux以太網驅動總結