ORB-SLAM作為單目SLAM，其精度很大程度上決定於幀與幀之間的位姿優化的是否準確。因此優化（optimization）在ORB-SLAM裡面扮演了很重要的角色。這一小節探討一下ORB-SLAM裡用到的優化。

一、為什麼要優化

因為攝像機標定（camera calibration）和追蹤（tracking）的精度不夠。攝像機標定的誤差會體現在重建中（比如三角法重建時），而追蹤的誤差則會體現在不同關鍵幀之間的位姿中，和重建中（單目）。誤差的不斷累積會導致後面幀的位姿離實際位姿越來越遠，最終會限制系統整體的精度。

1.1 攝像機標定

單目SLAM文獻中一般假設攝像機標定的結果是準確的，並不考慮這個因素帶來的誤差（大概因為很多時候跑標準的資料集，認為攝像機標定的誤差是相似的）。然而對於一個產品，不同型別的感測器對應的標定誤差並不相同，甚至有可能差異很大。因此，如果要評估整個系統的精度，這方面的誤差必須要考慮進去。

1.2 追蹤

無論在單目、雙目還是RGBD中，追蹤得到的位姿都是有誤差的。單目SLAM中，如果兩幀之間有足夠的對應點，那麼既可以直接得到兩幀之間的位姿（像初始化中那樣），也可以通過求解一個優化問題得到（如solvePnP）。由於單目中尺度的不確定性，還會引入尺度的誤差。由於tracking得到的總是相對位姿，前面某一幀的誤差會一直傳遞到後面去，導致tracking到最後位姿誤差有可能非常大。為了提高tracking的精度，可以1. 在區域性和全域性優化位姿；2. 利用閉環檢測（loop closure）來優化位姿。

二、如何優化

2.1 優化的目標函式在SLAM問題中，常見的幾種約束條件為： 1. 三維點到二維特徵的對映關係（通過投影矩陣）；2. 位姿和位姿之間的變換關係（通過三維剛體變換）；3. 二維特徵到二維特徵的匹配關係（通過F矩陣）；5. 其它關係（比如單目中有相似變換關係）。如果我們能夠知道其中的某些關係是準確的，那麼可以在g2o中定義這樣的關係及其對應的殘差，通過不斷迭代優化位姿來逐步減小殘差和，從而達到優化位姿的目標。

2.2 區域性優化

當新的關鍵幀加入到convisibility graph時，作者在關鍵幀附近進行一次區域性優化，如下圖所示。Pos3是新加入的關鍵幀，其初始估計位姿已經得到。此時，Pos2是和Pos3相連的關鍵幀，X2是Pos3看到的三維點，X1是Pos2看到的三維點，這些都屬於區域性資訊，共同參與Bundle Adjustment。同時，Pos1也可以看到X1，但它和Pos3沒有直接的聯絡，屬於Pos3關聯的區域性資訊，參與Bundle Adjustment，但取值保持不變。Pos0和X0不參與Bundle Adjustment。

因此，參與優化的是下圖中紅色橢圓圈出的部分，其中紅色代表取值會被優化，灰色代表取值保持不變。

2.3 全域性優化

在全域性優化中，所有的關鍵幀（除了第一幀）和三維點都參與優化。

2.4 閉環處的Sim3位姿優化

當檢測到閉環時，閉環連線的兩個關鍵幀的位姿需要通過Sim3優化（以使得其尺度一致）。優化求解兩幀之間的相似變換矩陣，使得二維對應點（feature）的投影誤差最小。

如下圖所示，Pos6和Pos2為一個可能的閉環。通過(u4,2,v4,2)(u4,2,v4,2)和(u4,6,v4,6)(u4,6,v4,6)之間的投影誤差來優化S6,2S6,2。

2.5 Sim3上的位姿優化

單目SLAM一般都會發生尺度（scale）漂移，因此Sim3上的優化是必要的。相對於SE3，Sim3的自由度要多一個，而且優化的目標是矯正尺度因子，因此優化並沒有加入更多的變數（如三維點）。

作者在檢測到閉環時在Sim3上對所有的位姿進行一次優化。定義Sim3上的殘差如下：

ei,j=logSim3(SijSjwS−1iw)ei,j=logSim3(SijSjwSiw−1)

其中SiwSiw的初值是尺度為1的Pos i相對於世界座標系的變換矩陣。Si,jSi,j為Pos i和Pos j之間的（Sim3優化之前的）相對位姿矩陣，表示SiwSiw和SjwSjw之間的測量（measurement）。此處相當於認為區域性的相對位姿是準確的，而全域性位姿有累計誤差，是不準確的。

 三、小結

個人理解，單目SLAM中的優化需要更多技巧，要有明確的優化目標，要仔細權衡其中的引數選擇、自由度、速度和穩定性。

上一篇提到，無論在單目、雙目還是RGBD中，追蹤得到的位姿都是有誤差的。隨著路徑的不斷延伸，前面幀的誤差會一直傳遞到後面去，導致最後一幀的位姿在世界座標系裡的誤差有可能非常大。除了利用優化方法在區域性和全域性調整位姿，也可以利用迴環檢測（loop closure）來優化位姿。

這件事情就好比一個人走在陌生的城市裡，一開始還能分清東南西北，但隨著在小街小巷轉來轉去，已經不知道自己在什麼地方了。通過認真辨識周邊環境，他可以建立起區域性的地圖資訊（區域性優化）。再回憶以前走過的路徑，他可以糾正一些以前的地圖資訊（全域性優化）。然而他還是不敢確定自己在城市的精確方位。直到他看到了一個之前路過的地方，就會恍然大悟，“噢！原來我回到了這個地方。”此時，將這個資訊傳遞迴整個地圖，就可以得到相當準確的地圖資訊。這就是迴環檢測。

因此，迴環檢測在大尺度地圖構建上是一個非常有用的方法。迴環檢測可以從二維影象出發，也可以從三維點雲出發。目前大家更推薦基於二維影象的方法。

DBoW2

基於二維影象的方法本質上是一個場景識別的問題。我沒有深入研究過，因此直接介紹一下ORB－SLAM中用到的DBoW2的方法。

BoW（bag of words，詞袋模型），可以理解為一個以特徵描述作為元素的詞典。如果是ORB特徵，那就是ORB詞典；如果是SIFT特徵，那就是SIFT詞典。詞典可以從影象資料集中訓練出來。下面舉一個簡單的例子。假如我們有一個一萬幅影象的資料集，並認為它基本上涵蓋了我們所面臨的場景。

1. 從每幅影象中提取特徵點和特徵描述；特徵描述一般是一個多維向量，因此可以計算兩個特徵描述之間的距離；

2. 將這些特徵描述進行聚類（比如k－means），類別的個數就是詞典的單詞數，比如1000；也可以用Beyes、SVM等；

3. DBoW2將這個詞典組織成樹的形式，方便搜尋。

在實際應用中，每一幅影象都在詞典中搜索其最近鄰的單詞，並在該單詞下留下標記。如果A、B兩幅影象定位到同一個單詞時，說明這兩幅影象有可能有相似的特徵點。當A、B有一定量的相似點時，可以認為這兩幅影象之間存在著一定的相似性。

ORB－SLAM的作者修改了DBoW2，輸出一系列候選影象（candidate）而不是一幅最相似的影象。

基於BoW的方法有一些非常好的優勢：

1. 詞典可以離線訓練。在實時應用中能離線的東西越多越好。作者提供了通過大量資料訓練出來的BRIEF和SIFT的詞典。

2. 搜尋速度飛快。小尺寸的影象可以在毫秒級別完成。作者提供了正向（direct index）和反向（inverse index）兩種輔助指標。反向指標在節點（單詞）上儲存到達這個節點的影象特徵的權重資訊和影象編號，因此可用於快速尋找相似影象。正向指標則儲存每幅影象上的特徵以及其對應的節點在詞典樹上的某一層父節點的位置，因此可用於快速特徵點匹配（只需要匹配該父節點下面的單詞）。

3. 很多slam應用本身就需要計算特徵點和描述，因此可以用特徵來搜尋。

4. ORB－SLAM的作者還用詞典的特性做快速的特徵篩選，減少特徵匹配需要的時間（特別是在大尺度上搜索特徵時）。​

當然它也有自己的劣勢：

1. 如果應用的場景比較特殊，請訓練自己的詞典。一般的詞典會不太好用。

2. BoW一般不太考慮特徵之間的幾何關係（有人在做，但不清楚效果和計算量如何）

還有幾個comments：

1. 如果應用本身不需要計算特徵，那要考慮額外的計算時間。

2. 推薦像ORB－SLAM一樣，BoW只用來快速篩選影象，後續需要通過其它方法一一驗證、嚴格驗證。如果迴環選錯了，那，就跪了。

3. 如果場景特徵很少，或者重複的特徵太多，比較難辦。

4. 在稠密點雲重建中，如果場景本身有豐富的幾何紋理，那麼可以利用兩幀之間（包括相鄰位置）的三維點雲匹配去驗證迴環。如果匹配的誤差足夠小，那麼迴環是比較準確的。

迴環驗證和Sim3優化

對每個候選的迴環幀，作者先匹配其和當前幀上的特徵點，然後用特徵點對應的三維點去求解一個相似變換矩陣（RANSAC框架下）。如果某個迴環幀對應的矩陣有足夠多的內點，那去做Sim3優化。利用優化結果再去尋找更多的特徵匹配，再做一遍優化。如果內點足夠多，那麼接受這個迴環。

Sim3優化詳見上一篇。

迴環融合（fusion）

這裡有一個隱含假設，即誤差隨著時間不斷累積，相對而言，我們更信任之前的資訊而不是當前的資訊。這部分主要是把迴環幀的資訊融合到當前幀裡面，包括匹配的特徵點對應的三維資訊（深度、尺度等），世界座標系下的位姿（通過Sim3的結果轉化過去）等等。融合也包括迴環幀的鄰域和當前幀的鄰域。

全域性優化

全域性優化時，固定迴環幀及其鄰域，當前幀及其鄰域，優化剩餘幀在世界座標系的位姿。詳見上一篇。

預告：

這個系列寫到這裡就暫時告一段落啦。回頭來看，前面幾篇寫的並不滿意，尤其追蹤那部分，將來爭取重寫一下。

接下來，本來想寫關於LSD的文章，估計會暫緩一下，先寫一兩篇關於網格生成的文章。