ORB-SLAM作為單目SLAM，其精度很大程度上決定於幀與幀之間的位姿優化的是否准確。因此優化（optimization）在ORB-SLAM里面扮演了很重要的角色。這一小節探討一下ORB-SLAM里用到的優化。
ORB-SLAM選用g2o作為圖優化的方法，關於g2o可以參考http://www.cnblogs.com/gaoxiang12/p/5304272.html。
一、為什么要優化
因為攝像機標定（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。
因此，參與優化的是下圖中紅色橢圓圈出的部分，其中紅色代表取值會被優化，灰色代表取值保持不變。(u,v)是X在Pos下的二維投影點，即X在Pos下的測量（measurement）。優化的目標是讓投影誤差最小。
2.3 全局優化
在全局優化中，所有的關鍵幀（除了第一幀）和三維點都參與優化。
2.4 閉環處的Sim3位姿優化
當檢測到閉環時，閉環連接的兩個關鍵幀的位姿需要通過Sim3優化（以使得其尺度一致）。優化求解兩幀之間的相似變換矩陣，使得二維對應點（feature）的投影誤差最小。
如下圖所示，Pos6和Pos2為一個可能的閉環。通過$(u_{4,2},v_{4,2})$和$(u_{4,6},v_{4,6})$之間的投影誤差來優化$S_{6,2}$。
2.5 Sim3上的位姿優化
單目SLAM一般都會發生尺度（scale）漂移，因此Sim3上的優化是必要的。相對於SE3，Sim3的自由度要多一個，而且優化的目標是矯正尺度因子，因此優化並沒有加入更多的變量（如三維點）。
作者在檢測到閉環時在Sim3上對所有的位姿進行一次優化。定義Sim3上的殘差如下：
$e_{i,j}=log_{Sim3}(S_{ij}S_{jw}S_{iw}^{-1})$
其中$S_{iw}$的初值是尺度為1的Pos i相對於世界坐標系的變換矩陣。$S_{i,j}$為Pos i和Pos j之間的（Sim3優化之前的）相對位姿矩陣，表示$S_{iw}$和$S_{jw}$之間的測量（measurement）。此處相當於認為局部的相對位姿是准確的，而全局位姿有累計誤差，是不准確的。
三、小結
個人理解，單目SLAM中的優化需要更多技巧，要有明確的優化目標，要仔細權衡其中的參數選擇、自由度、速度和穩定性。
該系列的其它文章：