SVO 從名字來看，是半直接視覺里程計，所謂半直接是指通過對圖像中的特征點圖像塊進行直接匹配來獲取相機位姿，而不像直接匹配法那樣對整個圖像使用直接匹配。整幅圖像的直接匹配法常見于RGBD傳感器，因為RGBD傳感器能獲取整幅圖像的深度。?
??雖然semi-direct方法使用了特征，但它的思路主要還是通過direct method來獲取位姿，這和feature-method不一樣。同時，semi-direct方法和direct method不同的是它利用特征塊的配準來對direct method估計的位姿進行優化。?
??和常規的單目一樣，SVO算法分成兩部分: 位姿估計，深度估計。本文對論文內容用自己的理解進行解讀，并對一些關鍵內容會推薦一些參考文獻幫助大家更進一步的學習SVO。

位姿估計

??svo 方法中motion estimation的步驟可以簡單概括如下:

1. 對稀疏的特征塊使用direct method 配準，獲取相機位姿；
2. 通過獲取的位姿預測參考幀中的特征塊在當前幀中的位置，由于深度估計的不準導致獲取的位姿也存在偏差，從而使得預測的特征塊位置不準。由于預測的特征塊位置和真實位置很近，所以可以使用牛頓迭代法對這個特征塊的預測位置進行優化。
3. 特征塊的預測位置得到優化，說明之前使用直接法預測的有問題。利用這個優化后的特征塊預測位置，再次使用直接法，對相機位姿(pose)以及特征點位置（structure）進行優化。

??下面結合作者forster的原論文對motion estimation進行更詳細的討論。

1.sparse model-based image alignment

??使用直接法最小化圖像塊重投影殘差來獲取位姿。如圖所示：其中紅色的Tk,k?1為位姿，即優化變量。

?

直接法具體過程如下：?
??step1. 準備工作。假設相鄰幀之間的位姿Tk,k?1已知，一般初始化為上一相鄰時刻的位姿或者假設為單位矩陣。通過之前多幀之間的特征檢測以及深度估計，我們已經知道第k-1幀中特征點位置以及它們的深度。?
??step2. 重投影。知道Ik?1中的某個特征在圖像平面的位置(u,v)，以及它的深度d，能夠將該特征投影到三維空間pk?1，該三維空間的坐標系是定義在Ik?1攝像機坐標系的。所以，我們要將它投影到當前幀Ik中，需要位姿轉換Tk,k?1，得到該點在當前幀坐標系中的三維坐標pk。最后通過攝像機內參數，投影到Ik的圖像平面(u′,v′)，完成重投影。?
??step3. 迭代優化更新位姿 。按理來說對于空間中同一個點，被極短時間內的相鄰兩幀拍到，它的亮度值應該沒啥變化。但由于位姿是假設的一個值，所以重投影的點不準確，導致投影前后的亮度值是不相等的。不斷優化位姿使得這個殘差最小，就能得到優化后的位姿Tk,k?1。?
??將上述過程公式化如下：通過不斷優化位姿Tk,k?1最小化殘差損失函數。?

其中

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

其中雅克比矩陣為圖像殘差對李代數的求導，可以通過鏈式求導得到：

這中間最復雜的部分是位姿矩陣對李代數的求導。很多文獻都有提到過，比如DTAM作者Newcombe的博士論文，gtsam的作者Dellaert的數學筆記。不在這里展開(有兩篇博客的篇幅)，可以參看清華大學王京的李代數筆記。?
??好了，先別在旁枝末葉上耗費精力，繼續回到主題。到這里，我們已經能夠估計位姿了，但是這個位姿肯定不是完美的。導致重投影預測的特征點在Ik中的位置并不和真正的吻合，也就是還會有殘差的存在。如下圖所示：

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

2.Relaxation Through Feature Alignment

??通過第一步的幀間匹配能夠得到當前幀相機的位姿，但是這種frame to frame估計位姿的方式不可避免的會帶來累計誤差從而導致漂移。所以，應該通過已經建立好的地圖模型，來進一步約束當前幀的位姿。?
??地圖模型通常來說保存的就是三維空間點，因為每一個Key frame通過深度估計能夠得到特征點的三維坐標，這些三維坐標點通過特征點在Key Frame中進行保存。所以SVO地圖上保存的是Key Frame 以及還未插入地圖的KF中的已經收斂的3d點坐標（這些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中的亮度差不多。所以可以重新構造一個殘差，對特征預測位置進行優化：

注意這里的優化變量是像素位置，這過程就是光流法跟蹤嘛。并且注意，光度誤差的前一部分是當前圖像中的亮度值，后一部分不是Ik?1而是Ir，即它是根據投影的3d點追溯到的這個3d點所在的key frame中的像素值，而不是相鄰幀。由于是特征塊對比并且3d點所在的KF可能離當前幀new frame比較遠，所以光度誤差和前面不一樣的是還加了一個仿射變換，需要對KF幀中的特征塊進行旋轉拉伸之類仿射變換后才能和當前幀的特征塊對比。?
??這時候的迭代量計算方程和之前是一樣的，只不過雅克比矩陣變了，這里的雅克比矩陣很好計算：

J=[?r?u′?r?v′]=[?I(u′,v′)?u′?I(u′,v′)?v′]

這不就是圖像橫縱兩個方向的梯度嘛。?
??通過這一步我們能夠得到優化后的特征點預測位置，它比之前通過相機位姿預測的位置更準，所以反過來，我們利用這個優化后的特征位置，能夠進一步去優化相機位姿以及特征點的三維坐標。所以位姿估計的最后一步就是Pose and Structure Refinement。

3.Pose and Structure Refinement

??在一開始的直接法匹配中，我們是使用的光度誤差，這里由于優化后的特征位置和之前預測的特征位置存在差異，這個能用來構造新的優化目標函數。

上式中誤差變成了像素重投影以后位置的差異（不是像素值的差異），優化變量還是相機位姿，雅克比矩陣大小為2×6(橫縱坐標u，v分別對六個李代數變量求導)。這一步是就叫做motion-only Bundler Adjustment。同時根據根據這個誤差定義，我們還能夠對獲取的三維點的坐標(x,y,z)進行優化，還是上面的誤差像素位置誤差形式，只不過優化變量變成三維點的坐標，這一步叫Structure -only Bundler Adjustment，優化過程中雅克比矩陣大小為2×3(橫縱坐標u，v分別對點坐標(x,y,z)變量求導)。

Discussion:

??作者在論文discussion中簡單闡述了他的motion estimationg 方法和直接使用直接法的優點，也說明了對比直接使用光流跟蹤再優化位姿的優點，可以簡單看看。

depth估計

??最基本的深度估計就是三角化，這是多視角幾何的基礎內容(可以參看圣經Hartly的《Multiple View Geometry in Computer Vision》中的第十二章structure computation；可以參看我的相應博客)。我們知道通過兩幀圖像的匹配點就可以計算出這一點的深度值，如果有多幅圖像，那就能計算出這一點的多個深度值。這就像對同一個狀態變量我們進行了多次測量，因此，可以用貝葉斯估計來對多個測量值進行融合，使得估計的不確定性縮小。如下圖所示：

一開始深度估計的不確定性較大(淺綠色部分)，通過三角化得到一個深度估計值以后，能夠極大的縮小這個不確定性(墨綠色部分)。?
??在這里，先簡單介紹下svo中的三角化計算深度的過程，主要是極線搜索確定匹配點。在參考幀Ir中，我們知道了一個特征的圖像位置，假設它的深度值在[dmin,dmax]之間，那么根據這兩個端點深度值，我們能夠計算出他們在當前幀Ik中的位置，如上圖中草綠色圓圈中的線段。確定了特征出現的極線段位置，就可以進行特征搜索匹配了。如果極線段很短，小于兩個像素，那直接使用上面求位姿時提到的Feature Alignment光流法就可以比較準確地預測特征位置。如果極線段很長，那分兩步走，第一步在極線段上間隔采樣，對采樣的多個特征塊一一和參考幀中的特征塊匹配，用Zero mean Sum of Squared Differences 方法對各采樣特征塊評分，那個得分最高，說明他和參考幀中的特征塊最匹配。第二步就是在這個得分最高點附近使用Feature Alignment得到次像素精度的特征點位置。像素點位置確定了，就可以三角化計算深度了。?
??得到一個新的深度估計值以后，用貝葉斯概率模型對深度值更新。在LSD slam中，假設深度估計值服從高斯分布，用卡爾曼濾波(貝葉斯的一種)來更新深度值。這種假設中，他認為深度估計值效果很棒，很大的概率出現在真實值(高斯分布均值)附近。而SVO的作者采用的是Vogiatzis的論文《Video-based, real-time multi-view stereo》提到的概率模型：

這個概率模型是一個高斯分布加上一個設定在最小深度dmin和最大深度dmax之間的均勻分布。這個均勻分布的意義是假設會有一定的概率出現錯誤的深度估計值。有關這個概率模型來由更嚴謹的論證去看看Vogiatzis的論文。同時，有關這個概率模型遞推更新的過程具體可以看Vogiatzis在論文中提到的Supplementary material，論文中告知了下載地址。知道了這個貝葉斯概率模型的遞推過程，程序就可以實現深度值的融合了，結合supplementary material去看svo代碼中的updateSeeds(frame)這個程序就容易了，整個程序里的那些參數的計算遞歸過程的推導，我簡單截個圖，這部分我也沒細看(公式19是錯誤的，svo作者指出了)，現在有幾篇博客對該部分進行了推導，盧彥斌：svo原理解析,東北大學孫志明：svo的Supplementary matterial 推導過程。?
?
??在深度估計的過程中，除了計算深度值外，這個深度值的不確定性也是需要計算的，它在很多地方都會用到，如極線搜索中確定極線的起始位置和長度，如用貝葉斯概率更新深度的過程中用它來確定更新權重(就像卡爾曼濾波中協方差矩陣扮演的角色)，如判斷這個深度點是否收斂了，如果收斂就插入地圖等等。SVO的作者Forster作為第二作者發表的《REMODE: Probabilistic, Monocular Dense Reconstruction in Real Time》中對由于特征定位不準導致的三角化深度誤差進行了分析，如下圖：?
?
它是通過假設特征點定位差一個像素偏差，來計算深度估計的不確定性。具體推導見原論文，簡單的幾何關系。?
??最后，簡單說下SVO的初始化過程：它假設前兩個關鍵幀所拍到的特征點在一個平面上(四軸飛行棋對地面進行拍攝)，然后估計單應性H矩陣，并通過三角化來估計初始特征點的深度值。SVO初始化時triangulation的方法具體代碼是vikit/math_utils.cpp里的triangulateFeatureNonLin()函數，使用的是中點法，關于這個三角化代碼算法的推導見github issue。還有就是SVO適用于攝像頭垂直向下的情況（也就是無人機上,垂直向上也可以,朝著一面墻也可以），為什么呢？1.初始化的時候假設的是平面模型 2.KF的選擇是個極大的限制，除了KF的選擇原因外攝像頭水平朝前運動的時候，SVO中的深度濾波做的不好，這個討論可以看看github issue，然而在我的測試中，不知道修改了哪些參數，稍微改動了部分代碼，發現前向運動，并且對著非平面SVO也是很溜的。?

總結

以上是生活随笔為你收集整理的SVO 半直接视觉里程计的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。