sim3群運算

Sim3=(sR01×3t1)
最小表示： [ω,ν,σ]， 其中 ω∈R3,ν∈R3,σ∈R，分別表示旋轉、位移、尺度增量。

---

逆運算：

S?1=??1sRT01×3?1sRTt1??

---

指數運算：

sim3=([w]×+σI3×301×3ν0)

expSim3(ν,ω,σ):=exp([w]×+σI3×301×3ν0)=(eσexp([ω]×)01×3Wν1)

W=(eσ?1σ)I+Aσ+(1?B)θθ(σ2+θ2)[w]×+(eσ?1σ?(B?1)σ+Aθσ2+θ2)[w]2×θ2
A=eσsin(θ)， B=eσcos(θ)， θ=∥ω∥2
則 s=eσ， R=exp([ω]×)， t=Wν

---

對數運算

log[sR01×3t1]=[log(R)+ln(s)I01×3W?1t0]

---

乘法運算：

(s1R101×3t11)?(s2R201×3t21)=(s1s2R1R201×3s1R1t2+t11)

---

閉環糾正

由于我們slam系統一般都是增量式運動估計，所以隨著系統的運行，誤差不斷的累計，譬如旋轉誤差和位移誤差，而回環檢測可以用來消除這種累計誤差。
對于視覺slam我們常用基于視覺詞典方法來檢測回環，如DBow、FABMap等方法，本文著重介紹回環檢測后的誤差糾正，因此對于回環檢測的方法不再細述。

為了糾正累計誤差，我們一般使誤差大致均勻分布在整個相機Pose鏈上，從而當前幀和回環匹配幀誤差不至于過大。如果我們優化所有的相機Pose和Point，一方面計算量過于龐大，另一方面由于要優化模型存在非凸性，因此可能做Full BA并不能達到我們我們期待收斂的結果。所以一般如圖b只對相機的Pose作PoseGraph優化，然后再做個Pose和Point的Global Bundle Adjustment。
1. 6自由度的PoseGraph優化
假設對于相機兩個Pose Tiw和Tjw，兩個Pose的相對變換Tji=TjT?1i。既然我們要優化兩個相機的Pose Tiw和Tjw，那么我們總要有測量量吧？而又不能優化特征所對應的MapPoint，所以就把初始兩個相機的位姿相對變換作為測量值，這樣優化前后整個相機Pose鏈幀間相對變換基本保持不變。
我們在 tangent space定義兩個Pose誤差為：

ei,j=log(Tji?Tiw?T?1jw)∨
Tiw∈{(Riw,tiw)|Riw∈SO3,tiw∈R3}
其中 log(.)∨:=(log(.))∨，先對SE3進行對數運算轉換成se3，再vee-operator轉裝成 R6矢量。則對整個PoseGraph作優化目標energy function：
χ2(T2,T3…Tm)=ΣTjieTi,jΩi,jei,j
其中 Ωij為信息矩陣，第一幀相機Pose固定，優化 T2,T3?Tm。
2. 7自由度的PoseGraph優化
由于單目是純相機投影模型。如下圖所示，兩個地圖點距離相機5m、6m，相機運動2m，和兩個地圖點距離相機10m、12m，相機運動4m，所觀察到的圖像平面投影點坐標變換情況一樣的。所以單目slam所構建地圖與所估計的運動均up to sclae。

相機運動帶來不只包括旋轉誤差、位移誤差，還伴隨帶來尺度飄逸，比較明顯現象如上圖隨著相機的運動，相機的軌跡越來越小，尺度也在不斷的改變。如果我們像上述方法在SE3上作6自由度優化，雖然可以糾正旋轉和位移飄逸，卻不能處理scale drift。
定義在sim3上誤差：
ei,j=log(Sji?Siw?S?1jw)∨sim3
和上述6自由度優化類似，我們定義優化初始估計值和測量值：
Siw=(Riw,tiw,s=1)，其中(Riw,tiw)=Tiw
Sji=(Rji,tji,s=1)，其中(Rji,tji)=Tji
其中對于回環幀 sloop≠1，我們不能像其他幀一樣假設s=1，否則所有幀尺度都為1，沒有尺度誤差怎么優化尺度呢。一般對于檢測到回環匹配兩幀，我們都會采用附錄【1】方法計算兩幀間Sim3。
假設優化后Pose的Sim3為 Scorjw=(Rjw,tjw,s)，則糾正之后Pose：
Tcorjw=(Rjw,1stjw)
Point的3D坐標 Pkw糾正為：
wPcork=ScorwjTjwwPk

---

對比6自由度優化和7自由度優化，會發現在7自由Sim3上優化后結果更加接近于真實值。

附錄：
[1]Closed-form solutionof absolute orientation using unit quaternions.
[2]Scale Drift-Aware Large Scale Monocular SLAM.

總結

以上是生活随笔為你收集整理的单目slam LoopClosing之Sim3优化的全部內容，希望文章能夠幫你解決所遇到的問題。

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