VIO學習筆記（六）—— 前端 Frontend

學習資料是深藍學院的《從零開始手寫VIO》課程，對課程做一些記錄，方便自己以後查詢，如有錯誤還請斧正。由於習慣性心算公式，所以爲了加深理解，文章公式採用手寫的形式。
關於前端的內容可以參考高翔博士的《視覺SLAM十四講》。

VIO學習筆記（一）—— 概述
VIO學習筆記（二）—— IMU 傳感器
VIO學習筆記（三）—— 基於優化的 IMU 與視覺信息融合
VIO學習筆記（四）—— 基於滑動窗口算法的 VIO 系統:可觀性和 一致性
VIO學習筆記（五）—— 後端優化實踐:逐行手寫求解器

前端的工作

SLAM 的框架

通常的 SLAM 框架由前後端共同構成

1. 前端:提取 特徵點,追蹤相機 Pose, 定位相機
2. 後端:提供 全局優化或 滑動窗口優化

一些現實的問題

1. 前端的步驟?初始化、正常追蹤、丟失處理
2. 相機和路標的建模和參數化
   $x _{camera} = [R, t], x_{ landmark} = ?$
   可能的路標:絕對座標 xyz,逆深度 ρ,灰度值(灰度 Pattern),等等
3. 關鍵幀?
   需不需要關鍵幀?
   怎麼選關鍵幀?
   控制關鍵幀數量?
   僅在關鍵幀中補充新特徵點?還是對所有幀提取新特徵點?
   何時進行三角化?

前後端在各自領域處理效果上的差異

實際上, 前端非常能 體現一個 SLAM 的追蹤效果。
在實現中,儘管 後端存在明顯的理論 差異,但 很難直觀體驗在最終精度上。
問:假設給定相同的 Pose 和 Landmark,給定噪聲範圍,各種方法是否有明顯差異?還是僅僅有理論性質上的差異?
            
在某些理想情況下,利用仿真數據可以給出一定的結果:

這個實驗(雙目 Pose/Landmark 估計)中,UKF(SPKF) 給出最接近真值的結果。

所以說：儘管後端存在理論上的差異，但是最終處理的效果並不好從直觀的感受上體現出來，但不同的前端，針對不同的數據集會有比較好的效果，但可能並不一定適合所有數據集，或者說在處理實際情況數據時效果可能不理想。

儘管存在上述問題但是還可以問：

1. 實際情況離理論假設有多遠?(高斯噪聲)
2. 拿到的數據究竟有多好?

在很多實際場合,很難回答某種後端算法是否明確優於另一種。

例如:ORB-SLAM2 使用 Covisibility-Graph,DSO 使用帶邊緣化的滑動窗口,Tango 使用 MSCKF,等等。

實驗效果:ORB-SLAM2 具有較好的全局精度,但無迴環時 DSO具有漂移較少。Tango 計算量明顯少於其他算法。

算法的結果和數據集關係很大。例如:Kitti 屬於比較簡單的(視野開闊,動態物體少,標定準確),EUROC 一般(人工設定場景,紋理豐富,但曝光有變化),TUM-Mono 比較難(場景多樣,主要爲真實場景)

相比來說,前端的差異就比較明顯:

1. 追蹤算法是否很容易丟失?
2. 算法對干擾的魯棒性如何(光照、遮擋、動態物體)?

前端更多是範式(Paradigm)之間的比較,而非範式之內的比較。

1. 好的前端
   追蹤效果好,不容易丟
   計算速度快
   累計誤差小
2. 不好的前端
   追蹤效果差
   計算耗時
   容易出現累計誤差

VO 方法的定性比較:

幾個要點

1. 光流法最早,最成熟,缺點也明顯(抗光照干擾弱,依賴角點)
2. FAST+ 光流是比較實用的快速算法/GFTT+ 光流效果更好,也具備實時性
3. 特徵匹配需要提的特徵具有旋轉平移縮放不變性,SIFT/SURF 是最好的一類,BRISK 等次之,ORB 算比較差的
4. 特徵匹配和光流都非常依賴角點,日常生活場景中角點不明顯的很多
5. 直接法不依賴角點,但實現效果根據選點數量變化較大.
   
   DSO 在不同關鍵幀數量/不同選點數量下的表現。曲線越靠左上側越好。

特徵點提取、匹配和光流

特徵點提取

我們後文以傳統光流爲主來展開前端的介紹。
一個傳統的雙目光流前端流程(正常追蹤流程):
  
除了正常追蹤流程以外,還需要考慮初始化、丟失恢復的情況。
VIO 初始化比常規 VO 多一些步驟(主要爲 IMU 參數的估計)

特徵點提取

在光流中,我們通常選擇角點來追蹤。
            

爲什麼需要角點?

角點的梯度在兩個方向都有分佈:
              
利用角點附近塊的兩個特徵值大小,可以判斷該區是否爲角點。
  

FAST/GFTT 角點:

FAST:僅含像素亮度、不含計算的快速角點提取方式;
GFTT:在 Harris 基礎改進:Shi-tomasi 分數,增加固定選點數,等等
                

光流

光流可以追蹤一個時刻的角點在下個時刻的圖像位置

灰度不變假設

灰度不變假設:
            
一階 Taylor 展開:

得到:
                

帶 Warp function 的光流:

      
其中 W 爲 Warp Function,通常取仿射變換
 
其中 $p _1 − p _6$ 爲 W 的參數,需要在線估計。

金字塔式光流

      
Coarse-to-Fine:從頂層最模糊的圖像開始計算光流,一直往下迭代到最高分辨率

光流在 SLAM 中的運用

1. 光流可以追蹤上一幀的角點
2. 可以一直追蹤該角點,直到超出圖像範圍或被遮擋
3. 在單目 SLAM 中,新提出的角點沒有 3D 信息,因此可通過追蹤角點在各圖像位置,進行三角化

光流的侷限性

1. 容易受光照變化影響
2. 只適合連續圖像中的短距離追蹤,不適合更長距離
3. 圖像外觀發生明顯變化時不適用(例:遠處的角點湊近看之後不爲角點了)
4. 對角點強依賴,對 Edge 類型點表現較差
5. 稀疏光流不約束各點光流的方向統一,可能出現一些 outlier。

關鍵幀與三角化

關鍵幀

爲什麼需要關鍵幀

1. 後端通常實時性較差,不適合處理所有幀;
2. 如果相機停止,可能給後端留下無用的優化,甚至導致後端問題退化(尺度問題)

如何選擇關鍵幀

1. 關鍵幀之間不必太近(退化或三角化問題)
2. 關鍵幀之間不能太遠(共視點太少)
3. VIO 中,定期選擇關鍵幀(假設 $b ^g$ , $b^ a$ 在關鍵幀期間不變)

關鍵幀策略

對於非關鍵幀,只執行前端算法,不參與後端優化
因此,對於非關鍵幀,它的誤差會逐漸累積。只有該幀被作爲關鍵幀插入後端,BA 纔會保證窗口內的一致性

總結關鍵幀選取的策略:在計算量允許範圍內,且不引起退化時,應儘可能多地插入關鍵幀。

ORB-SLAM2

            
ORB-SLAM2 使用非常寬鬆的關鍵幀策略(大多數時候只要後端線程Idle 就會插入關鍵幀),然後在後端剔除冗餘的關鍵幀。

DSO

            
DSO 利用光度誤差插入關鍵幀(插入比較頻繁)。然後在後端計算每個關鍵幀的 Active Landmarks,Marg 對窗口貢獻最低的。因此,DSO 的關鍵幀窗口通常有一個很遠的和兩三個很近的,其他幾個分佈在中間。

三角化

在單目 SLAM 中,通常在插入關鍵幀時計算新路標點的三角化。

1. 有的 SLAM 系統在關鍵幀時提取新 Feature(DSO, SVO),也有方案對每個幀都提新 Feature(VINS, ORB)。
2. 前者節省計算量(非關鍵幀無需提點,節省 5-10ms 左右);後者效果好(在單目裏需要防止三角化 Landmark 數量不夠)。

三角化的數學描述

1. 考慮某路標點 y 在若干個關鍵幀 k = 1, · · · , n 中看到。
2. $y ∈ R^4$ ,取齊次座標。每次觀測爲 $x _k = [u _k , v _k , 1] ^⊤$ ,取歸一化平面座標(這樣可以忽略掉內參)。
3. 記投影矩陣 $P _k = [R _k , t_ k ] ∈ R ^{3×4}$ ,爲 World 繫到 Camera 系
4. 投影關係:
   $∀k, λ _k x_ k = P_ k y.$
   其中$λ _k$ 爲觀測點的深度值(未知)
   根據上式第三行:
   $λ_ k = P ^⊤_{k,3} y$
   其中 $P ^⊤_{k,3}$爲 $P _k$ 的第 3 行。
5. 將上式代入上上式的前兩行:
   $u_k P^ ⊤_{k,3} y = P_{ k,1}^T y\\ u_k P^ ⊤_{k,3} y = P_{ k,2}^T y$
6. 每次觀測將提供兩個這樣的方程,視 y 爲未知量,並將 y 移到等式一側:
             
7. 於是,y 爲 D 零空間中的一個非零元素。
8. 由於 $D ∈ R ^{2n×4}$ ,在觀測次於大於等於兩次時,很可能 D 滿秩,無零空間。
9. 尋找最小二乘解:
   $\min_y ∥Dy∥ ^2_2 , s.t. ∥y∥ = 1$
   解法:對 $D ^⊤ D$ 進行 SVD(再去研究一下矩陣論的內容):
   $D ^⊤ D =∑^4_{i=1}σ_ i^ 2 u_ i u_ j^ ⊤$
   其中 $σ _i$ 爲奇異值,且由大到小排列,$u_ i$ , $u_ j$ 正交。
10. 此時,取 $y = u _4$ (爲什麼?),那麼該問題的目標函數值爲 $σ _4$ 。
11. 判斷該解有效性的條件:$σ _4 ≪ σ_ 3$ 。若該條件成立,認爲三角化有效,否則認爲三角化無效。
12. Rescale:在某此場合(比如我們使用 UTM 座標的平移),D 的數值大小差異明顯,導致解在數值上不穩定。此時對 D 做一個縮放:
                   
    其中 S 爲一個對角陣,取 D 最大元素之逆即可。
13. 實用當中,還需要加上 y 投影正負號的判定作爲依據

以上,我們推導了對任意次觀測的三角化算法,可作爲通用的依據。

三角化測試代碼

//
// Created by hyj on 18-11-11.
//
#include <iostream>
#include <vector>
#include <random>  
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <Eigen/Eigenvalues>

struct Pose
{
    Pose(Eigen::Matrix3d R, Eigen::Vector3d t):Rwc(R),qwc(R),twc(t) {};
    Eigen::Matrix3d Rwc;
    Eigen::Quaterniond qwc;
    Eigen::Vector3d twc;
    // 這幀圖像觀測到的特徵座標
    Eigen::Vector2d uv;    
};

int main()
{

    int poseNums = 10;
    double radius = 8;
    double fx = 1.;
    double fy = 1.;
    std::vector<Pose> camera_pose;
    for(int n = 0; n < poseNums; ++n ) {
        double theta = n * 2 * M_PI / ( poseNums * 4); // 1/4 圓弧
        // 繞 z軸 旋轉
        Eigen::Matrix3d R;
        R = Eigen::AngleAxisd(theta, Eigen::Vector3d::UnitZ());
        Eigen::Vector3d t = Eigen::Vector3d(radius * cos(theta) - radius, radius * sin(theta), 1 * sin(2 * theta));
        camera_pose.push_back(Pose(R,t));
    }

    // 隨機數生成 1 個 三維特徵點
    std::default_random_engine generator;
    std::uniform_real_distribution<double> xy_rand(-4, 4.0);
    std::uniform_real_distribution<double> z_rand(8., 10.);
    double tx = xy_rand(generator);
    double ty = xy_rand(generator);
    double tz = z_rand(generator);

    Eigen::Vector3d Pw(tx, ty, tz);
    // 這個特徵從第三幀相機開始被觀測，i=3
    int start_frame_id = 3;
    int end_frame_id = poseNums;
    for (int i = start_frame_id; i < end_frame_id; ++i) {
        Eigen::Matrix3d Rcw = camera_pose[i].Rwc.transpose();
        Eigen::Vector3d Pc = Rcw * (Pw - camera_pose[i].twc);

        double x = Pc.x();
        double y = Pc.y();
        double z = Pc.z();

        camera_pose[i].uv = Eigen::Vector2d(x/z,y/z);
    }
    
    /// TODO::homework; 請完成三角化估計深度的代碼
    // 遍歷所有的觀測數據，並三角化
    Eigen::Vector3d P_est;           // 結果保存到這個變量
    P_est.setZero();
    /* your code begin */
    
    //step1:constuct D
    Eigen::Matrix<double, 14, 4> D;
    for (int i = start_frame_id; i < end_frame_id; ++i) {

        Eigen::Matrix<double,3,4> Tcw;
        Eigen::Matrix3d Rcw = camera_pose[i].Rwc.transpose();
        Eigen::Vector3d tcw = -Rcw*camera_pose[i].twc;//注意是-:Pc=Pc0-tcw
        Tcw<< Rcw, tcw;

        //rows = mat.rows(2);  //  獲取第3行
        Eigen::Vector2d uv = camera_pose[i].uv ;

        D.block(2*(i - start_frame_id), 0, 1,4) = uv[0]*Tcw.row(2) - Tcw.row(0);
        D.block(2*(i - start_frame_id)+1, 0, 1,4) = uv[1]*Tcw.row(2) - Tcw.row(1);

    }
    
    std::cout<<"D Matrix is :"<<D<<std::endl;

    //step2:recale D,找到D中最大元素
    Eigen::MatrixXd::Index maxRow, maxCol;
    double max = D.maxCoeff(&maxRow,&maxCol);
    //注意是D的絕對值中最大元素的D.cwiseAbs().maxCoeff()
    std::cout << "Max = \n" << max <<"行:"<<maxRow<<"列"<<maxCol<< std::endl;

    Eigen::MatrixXd DtD((D/max).transpose()*(D/max));

    clock_t time_stt = clock();

    Eigen::JacobiSVD<Eigen::MatrixXd> svd_holder(DtD, Eigen::ComputeThinU | Eigen::ComputeThinV );

    clock_t time_stt1 = clock();
    
    std::cout<<"SVD分解，耗時:\n"<<(clock()-time_stt1)/(double) CLOCKS_PER_SEC<<"ms"<<std::endl; 

    // 構建SVD分解結果 Eigen::MatrixXd U = svd_holder.matrixU(); Eigen::MatrixXd V = svd_holder.matrixV(); 
    Eigen::MatrixXd S(svd_holder.singularValues()); 
    std::cout<<"singularValues :\n"<<S<<std::endl;

    //step3:判斷分解是否有效,然後求解y
    if(std::abs(svd_holder.singularValues()[3]/svd_holder.singularValues()[2]) < 1e-2){

	Eigen::Vector4d U4 = (max*svd_holder.matrixU().rightCols(1));//最後一列
	P_est = (U4/U4[3]).head(3);

    }
    else{

	std::cout<<"這次求解無效"<<std::endl;
    }
    
    /* your code end */
    
    std::cout <<"ground truth: \n"<< Pw.transpose() <<std::endl;
    std::cout <<"your result: \n"<< P_est.transpose() <<std::endl;
    // TODO:: 請如課程講解中提到的判斷三角化結果好壞的方式，繪製奇異值比值變化曲線
    return 0;
}

參考博客：

1. https://blog.csdn.net/orange_littlegirl/article/details/103512138