???????? 只知道算法描述和代碼，而不知道原理是比較扯的事情，還是把原理轉載一下。

???????? 原文鏈接： http://www.cnblogs.com/luyb/p/5447497.html

??????? 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矩陣）；

????????? 4. 4呢？

????????? 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?)?(u4,2,v4,2)和(u?4,6?,v?4,6?)?(u4,6,v4,6)之間的投影誤差來優化S?6,2??S6,2。

2.5 Sim3上的位姿優化

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

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

e?i,j?=log?Sim3?(S?ij?S?jw?S??1?iw?)?ei,j=logSim3(SijSjwSiw?1)

其中S?iw??Siw的初值是尺度為1的Pos i相對于世界坐標系的變換矩陣。S?i,j??Si,j為Pos i和Pos j之間的（Sim3優化之前的）相對位姿矩陣，表示S?iw??Siw和S?jw??Sjw之間的測量（measurement）。此處相當于認為局部的相對位姿是準確的，而全局位姿有累計誤差，是不準確的。

?

三、小結

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

?

該系列的其它文章：

ORB－SLAM（一）簡介

ORB－SLAM（二）性能

ORB－SLAM（三）初始化

ORB－SLAM（四）追蹤

ORB－SLAM（六）回環檢測



總結

以上是生活随笔為你收集整理的SLAM：ORB-SLAM 位姿优化描述的全部內容，希望文章能夠幫你解決所遇到的問題。

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