SLAM程式碼之svo程式碼分析

在上文中我們從ROS的節點出發,一步步介紹了SVO ros節點的執行流程,下面我們將深入介紹SVO大的核心程式碼。SVO的核心主要分為3方面內容
– spase image alignment
– Feature alignment
– pose and Structure optimization
– map
首先給出總體的結構圖。
這裡寫圖片描述

首先參考論文中的內容,進行分析。使用直接法最小化影象塊重投影殘差來獲取位姿。如圖所示:其中紅色的 Tk,k−1 為位姿,即優化變數。
這裡寫圖片描述
直接法具體過程如下:
1. 準備工作。假設相鄰幀之間的位姿 Tk,k−1 已知,一般初始化為上一相鄰時刻的位姿或者假設為單位矩陣。通過之前多幀之間的特徵檢測以及深度估計,我們已經知道第k-1幀中特徵點位置以及它們的深度。
2. 重投影。知道 Ik−1 中的某個特徵在影象平面的位置 (u,v) ,以及它的深度 d ,能夠將該特徵投影到三維空間 pk−1 ,該三維空間的座標系是定義在 Ik−1 攝像機座標系的。所以,我們要將它投影到當前幀 Ik 中,需要位姿轉換 Tk,k−1 ,得到該點在當前幀座標系中的三維座標 pk 。最後通過攝像機內引數,投影到 Ik 的影象平面 (u′,v′) ,完成重投影。
3. 迭代優化更新位姿 。按理來說對於空間中同一個點,被極短時間內的相鄰兩幀拍到,它的亮度值應該沒啥變化。但由於位姿是假設的一個值,所以重投影的點不準確,導致投影前後的亮度值是不相等的。不斷優化位姿使得這個殘差最小,就能得到優化後的位姿 Tk,k−1 。
  將上述過程公式化如下:通過不斷優化位姿 Tk,k−1 最小化殘差損失函式。

Tk,k−1=argminTk,k−112∑i∈R∥δI(Tk,k−1,ui)∥2

\mathbf{T}_{k,k-1}=\arg\min_{\mathbf{T}_{k,k-1}}\frac{1}{2}\sum_{i\in R}\|\delta\mathbf{I}(\mathbf{T}_{k,k-1},\mathbf{u}_i)\|^2

δI(Tk,k−1,ui)=Ik(π(Tk,k−1⋅π(u,du)))−Ik−1(u)

\delta\mathbf{I}(\mathbf{T}_{k,k-1},\mathbf{u}_i) = \mathbf{I}_k(\pi(\mathbf{T}_{k,k-1}\cdot\pi(\mathbf{u},d_{\mathbf{u}})))-\mathbf{I}_{k-1}(\mathbf{u})
第一步為根據影象位置和深度逆投影到三維空間,第二步將三維座標點旋轉平移到當前幀座標系下,第三步再將三維座標點投影回當前幀影象座標。當然在優化過程中,殘差的計算方式不止這一種形式:有前向(forwards),逆向(inverse)之分,並且還有疊加式(additive)和構造式(compositional)之分。這方面可以讀讀光流法方面的論文,Baker的大作《Lucas-Kanade 20 Years On: A Unifying Framework》。選擇的方式不同,在迭代優化過程中計算雅克比矩陣的時候就有差別,一般為了減小計算量,都採用的是inverse compositional algorithm。
  上面的非線性最小化二乘問題,可以用高斯牛頓迭代法求解,位姿的迭代增量 ξ\xi (李代數)可以通過下述方程計算:

JTJξ=−JTδI(0)

\mathbf{J}^T\mathbf{J}\xi=-\mathbf{J}^T\delta\mathbf{I}(0)
其中雅克比矩陣為影象殘差對李代數的求導,可以通過鏈式求導得到:

∂δI(ξ,ui)∂ξ=∂Ik−1(a)∂a∣∣a=ui⋅∂π(b)∂b∣∣b=pi⋅∂T(ξ)∂ξ∣∣ξ=0⋅Pi

\frac{\partial \delta\mathbf{I}(\xi,\mathbf{u}_i)}{\partial \xi}=\frac{\partial \mathbf{I}_{k-1}(\mathbf{a})}{\partial \mathbf{a}}\big\vert_{\mathbf{a}=\mathbf{u}_i}\cdot\frac{\partial \pi(\mathbf{b})}{\partial \mathbf{b}}\big\vert_{\mathbf{b}=\mathbf{p}_i}\cdot\frac{\partial \mathbf{T}(\xi)}{\partial \xi}\big\vert_{\xi=0}\cdot\mathbf{P}_i
我們已經能夠估計位姿了,但是這個位姿肯定不是完美的。導致重投影預測的特徵點在 Ik 中的位置並不和真正的吻合,也就是還會有殘差的存在。如下圖所示:

這裡寫圖片描述

圖中灰色的特徵塊為真實位置,藍色特徵塊為預測位置。幸好,他們偏差不大,可以構造殘差目標函式,和上面直接法類似,不過優化變數不再是相機位姿,而是畫素的位置 (u’,v’)(u′,v′) ,通過迭代對特徵塊的預測位置進行優化。這就是svo中提到的Feature Alignment。

接下來我們在看程式碼中的情況。這裡我們首先介紹image spase alignment ,這個類繼承自類 vk::NLLSSolver<6, SE3>
該類的宣告和實現在rpg-vikit包中metapackage vikit-common 中的檔案nlls_sovler.h和nlls_sovler_impl.h
中。該類中定義了若干虛擬函式,

  virtual double   computeResiduals      (const ModelType& model,
bool linearize_system,
bool compute_weight_scale) = 0;
virtual int   solve () = 0;
virtual void   update (const ModelType& old_model, ModelType& new_model) = 0;
virtual void startIteration        () { }
virtual void finishIteration       () { }

這裡回顧一下(純)虛擬函式的用法

繼承與物件導向設計

  • 繼承可以是單一繼承或多重繼承
  • 每一個繼承連線可以是public, protected, private, 也可以是virtual, pure virtual.
  • 每個成員函式可以是virtual, non-virutal, pure virtual.
  • virtual意味這介面必須被繼承,non-virtual 意味著介面和實現都必須被繼承。

公開繼承意味這is-a的關係。
derived class,類D; base class 類B, 每個類D 的物件也是一個類B 的物件。反之不成立。B比D表現出更為一般化的概念, D物件比B表現出更為特殊化的概念。

pure virtual 函式的目的是為了讓derived class制繼承介面。這意味這derived class必須提供一個對應該介面的實現,但base class不關心使怎樣實現的。
宣告非純虛擬函式的目的是讓derived class繼承該函式的介面和預設實現。這意味這derived class必須支援該介面,可以自己實現,也可以使用預設版本。

這裡寫圖片描述
這裡可以看出優化函式optimize, optimizeGaussNewton, optimizeLevenbergMarquardt 是基類中定義和實現,不是虛擬函式,因此最終執行過成中呼叫的是這裡的函式,而計算殘差更新等與自己的資料型別相關的函式,需要派生類中實現,覆蓋基類中的定義。派生類spaseImgAlign中的run函式傳入的引數參考幀和當前幀的frame,計算兩者之間的變換T_cur_from_ref,最後呼叫基類的optimize函式,這裡需要指定優化的方法是Gauss-Newton 還是LM,這個是在類frame-handler-mono中指定的。

SparseImgAlign img_align(Config::kltMaxLevel(), Config::kltMinLevel(),
30, SparseImgAlign::GaussNewton, false, false);

所以我們進入優化函式optimizeGaussNewton中去看具體的過程如何。

optimizeGaussNewton

首先是計算殘差,這個函式時呼叫了spasre-img-align中重新定義的函式computeResiduals,然後是開始迭代。每一次迭代中包含一下步驟
– 執行開始函式startIteration
– computeResiduals
– applyPrior
– solve
– update
– finishIteration
執行開始函式startIteration和finishIteration沒有內容,update中完成了

T_curnew_from_ref =  T_curold_from_ref * SE3::exp(-x_);

solve中代用Eigen中的ldlt的solver函式。

主要內容在computeResiduals,首先對映(扭曲)到參考影象的空間,注意該函式分為四種模式,是否是線性系統和是否應用權重不同型別,如果是首次執行的化需要計算權迭代的權重,然後對於參考幀中的每一特徵進行
– 判斷該特徵是否在該影象中
– 計算當前幀的畫素位置
– 計算殘差
– 如果是非線性系統那麼計算雅克比,權重hessian和梯度下降的權值。
– 如果是首次迭代,那麼計算用於第一次迭代的權值。

Relaxation Through Feature Alignment

地圖模型通常來說儲存的就是三維空間點,因為每一個Key frame通過深度估計能夠得到特徵點的三維座標,這些三維座標點通過特徵點在Key Frame中進行儲存。所以SVO地圖上儲存的是Key Frame 以及還未插入地圖的KF中的已經收斂的3d點座標,也就是說地圖map不需要自己管理所有的3d點,它只需要管理KF就行了。先看看選取KF的標準是啥?KF中儲存了哪些東西?當新的幀new frame和相鄰KF的平移量超過場景深度平均值的12%時(比如四軸上升),new frame就會被當做KF,它會被立即插入地圖。同時,又在這個新的KF上檢測新的特徵點作為深度估計的seed,這些seed會不斷融合新的new frame進行深度估計。但是,如果有些seed點3d點位姿通過深度估計已經收斂了,怎麼辦?map用一個point_candidates來儲存這些尚未插入地圖中的點。所以map這個資料結構中儲存了兩樣東西,以前的KF以及新的尚未插入地圖的KF中已經收斂的3d點。
  通過地圖我們儲存了很多三維空間點,很明顯,每一個new frame都是可能看到地圖中的某些點的。由於new frame的位姿通過上一步的直接法已經計算出來了,按理來說這些被看到的地圖上的點可以被投影到這個new frame中,即圖中的藍色方框塊。上圖中分析了,所有位姿誤差導致這個方框塊在new frame中肯定不是真正的特徵塊所處的位置。所以需要Feature Alignment來找到地圖中特徵塊在new frame中應該出現的位置,根據這個位置誤差為進一步的優化做準備。基於光度不變性假設,特徵塊在以前參考幀中的亮度應該和new frame中的亮度差不多。所以可以重新構造一個殘差,對特徵預測位置進行優化:

u′i=argminu′i12∥Ik(u′i)−AiIr(u′i∥2

\mathbf{u}_i’=\arg\min_{\mathbf{u}_i’}\frac{1}{2}\|\mathbf{I}_{k}(\mathbf{u}_i’)-\mathbf{A}_i\mathbf{I}_{r}(\mathbf{u}_i’\|^2
述注意這裡的優化變數是畫素位置,這過程就是光流法跟蹤嘛。並且注意,光度誤差的前一部分是當前影象中的亮度值,後一部分不是 Ik−1 而是 Ir ,即它是根據投影的3d點追溯到的這個3d點所在的key frame中的畫素值,而不是相鄰幀。由於是特徵塊對比並且3d點所在的KF可能離當前幀new frame比較遠,所以光度誤差和前面不一樣的是還加了一個仿射變換,需要對KF幀中的特徵塊進行旋轉拉伸之類仿射變換後才能和當前幀的特徵塊對比。
  這時候的迭代量計算方程和之前是一樣的,只不過雅克比矩陣變了,這裡的雅克比矩陣很好計算:

J=[∂r∂u′∂r∂v′]

\mathbf{J}=\begin{bmatrix}\frac{\partial r}{\partial u’}&\frac{\partial r}{\partial v’}\end{bmatrix}
這不就是影象橫縱兩個方向的梯度嘛。
  通過這一步我們能夠得到優化後的特徵點預測位置,它比之前通過相機位姿預測的位置更準,所以反過來,我們利用這個優化後的特徵位置,能夠進一步去優化相機位姿以及特徵點的三維座標。所以位姿估計的最後一步就是Pose and Structure Refinement。

然後繼續看程式碼,這裡首先介紹介個基礎的類,其中frame類是地圖的關鍵組成,上述的理論的實現在reprojecter類中,首先給出示意圖
這裡寫圖片描述
從processFrame的函式流程中順序執行到這裡,上一步是使用Gauss-Newton方法優化相鄰幀間的變化,使用鏈式求導的方法計算雅克比矩陣。那麼接下來,把地圖中跟當前幀有重疊的關鍵幀拿出來,遍歷對應的特徵點優化當前的位姿。
– 通過當前幀查詢到地圖中當前幀最近的n各關鍵幀影象。
– 按照與 當前幀的距離(靠近程度)排序
– 對於地圖中當前幀最近的n幀影象的每一個,遍歷其中的特徵點,構造一個overlay關鍵幀容器(std::vector< std::pair

struct Candidate {
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
Point* pt;       //!< 3D point.
Vector2d px;     //!< projected 2D pixel location.
Candidate(Point* pt, Vector2d& px) : pt(pt), px(px) {}
};

可以看出這是一個一個點對,包含了世界中的3D點和影象的2D點。

  struct Grid
{
CandidateGrid cells;
vector<int> cell_order;
int cell_size;
int grid_n_cols;
int grid_n_rows;
};

接下來就是重投影candidate points,這些點在map中定義。
Grid除了包含Candidate另外增加了一個cell的索引,以及影象的寬和高。
最後是執行projectcell,這這其中首先按照pointquality 進行排序,對於每個cell,findMatchDirect,如果找到,構造新的特徵,在當前幀中增加特徵。findMatchDirect中首先找到有類似視角的幀,然後根據當前幀和參考幀的camera資訊計算affine matrix,並使用該矩陣warp on a large patch. 最後是使用fature_alignment中的函式align2D函式。該函式包含了針對SSE2指令和ARM的的分別優化,因此在編譯的時候要分來處理。
為了理解其中原理,我們來看未優化的版本
– 計算梯度和hessian
– 亞畫素計算直至收斂
關於ARM-neon和x86的SSE指令集可以參考,這裡不在展開
1. http://blog.csdn.net/hemmingway/article/details/44828303
2. https://developer.arm.com/docs/dui0472/l/using-neon-support/neon-intrinsics-for-operations-with-a-scalar-value
3. https://community.arm.com/groups/processors/blog/2010/03/17/coding-for-neon–part-1-load-and-stores

Frame member

  • 抽象相機
  • SE3
  • 影象金字塔
  • 特徵
  • 特徵容器
  • g2o節點指標

Point member

這裡寫圖片描述

Feature member

這裡寫圖片描述

map member

  • list 關鍵幀list
  • list 要刪除的點list
  • MapPointCandidates 預選點

成員函式

  • 增加關鍵幀
  • 安全刪除幀
SVO stepoptimized objectresidual error objectmatched framecharacteristic
sparse model-based image alignmentTkk−1T^k_{k-1}光度值(I)current vs previouslie algebra derivative
Relaxation Through Feature Alignmentfeature predicted positionu′ku_k’光度值(I)current vs key framecontains affine transform
Pose and Structure RefinementTkk−1T^k_{k-1}camera pose && 3d postionallmotion-only Bundl Structure -only Bundler Adjustment

refer

  1. https://sholmes9091.blogspot.com/2015/12/svo.html
  2. http://www.voidcn.com/blog/heyijia0327/article/p-5804373.html
  3. https://sites.google.com/site/scarabotix/tutorial-on-visual-odometry
  4. https://sholmes9091.blogspot.com/2015/12/svo.html