si
m3=(
sr01
×3t1
) 最小表示：[ω
,ν,σ
] ， 其中ω∈
r3,ν
∈r3,
σ∈r ，分別表示旋轉、位移、尺度增量。
逆運算： s−
1=⎛⎝
1srt
01×3
−1sr
tt1⎞
⎠ 指數運算： si
m3=(
[w]×
+σi3
×301
×3ν0
) ex
psim
3(ν,
ω,σ)
:=exp
([w]
×+σi
3×30
1×3ν
0)=(
eσex
p([ω
]×)0
1×3w
ν1)
w=(e
σ−1σ
)i+a
σ+(1
−b)θ
θ(σ2
+θ2)
[w]×
+(eσ
−1σ−
(b−1
)σ+a
θσ2+
θ2)[
w]2×
θ2 a
=eσs
in(θ
) ， b=
eσco
s(θ)
， θ=∥
ω∥2
則s=e
σ ， r=
exp(
[ω]×
) ， t=
wν對數運算 lo
g[sr
01×3
t1]=
[log
(r)+
ln(s
)i01
×3w−
1t0]
乘法運算： (s
1r10
1×3t
11)∗
(s2r
201×
3t21
)=(s
1s2r
1r20
1×3s
1r1t
2+t1
1)由於我們slam系統一般都是增量式運動估計，所以隨著系統的執行，誤差不斷的累計，譬如旋轉誤差和位移誤差，而回環檢測可以用來消除這種累計誤差。
對於視覺slam我們常用基於視覺詞典方法來檢測回環，如dbow、fabmap等方法，本文著重介紹回環檢測後的誤差糾正，因此對於回環檢測的方法不再細述。
為了糾正累計誤差，我們一般使誤差大致均勻分布在整個相機pose鏈上，從而當前幀和回環匹配幀誤差不至於過大。如果我們優化所有的相機pose和point，一方面計算量過於龐大，另一方面由於要優化模型存在非凸性，因此可能做full ba並不能達到我們我們期待收斂的結果。所以一般如圖b只對相機的pose作posegraph優化，然後再做個pose和point的global bundle adjustment。
1. 6自由度的posegraph優化
假設對於相機兩個pose ti
w 和tj
w ，兩個pose的相對變換tj
i=tj
t−1i
。既然我們要優化兩個相機的pose ti
w 和tj
w ei
,j=l
og(t
ji∗t
iw∗t
−1jw
)∨ t
iw∈
其中log
(.)∨
:=(lo
g(.)
)∨，先對se3進行對數運算轉換成se3，再vee-operator轉裝成r6
向量。則對整個posegraph作優化目標energy function： χ2
(t2,
t3…t
m)=σ
tjie
ti,j
ωi,j
ei,j
其中ωi
j為資訊
矩陣，第一幀相機pose固定，優化t2
,t3⋯
tm。 2. 7自由度的posegraph優化
由於單目是純相機投影模型。如下圖所示，兩個地圖點距離相機5m、6m，相機運動2m，和兩個地圖點距離相機10m、12m，相機運動4m，所觀察到的影象平面投影點座標變換情況一樣的。所以單目slam所構建地圖與所估計的運動均up to sclae。
相機運動帶來不只包括旋轉誤差、位移誤差，還伴隨帶來尺度飄逸，比較明顯現象如上圖隨著相機的運動，相機的軌跡越來越小，尺度也在不斷的改變。如果我們像上述方法在se3上作6自由度優化，雖然可以糾正旋轉和位移飄逸，卻不能處理scale drift。
定義在sim3上誤差： ei
,j=l
og(s
ji∗s
iw∗s
−1jw
)∨si
m3和上述6自由度優化類似，我們定義優化初始估計值和測量值： si
w=(r
iw,t
iw,s
=1)，
其中(r
iw,t
iw)=
tiw
sji=
(rji
,tji
,s=1
)，其中
(rji
,tji
)=tj
i 其中對於回環幀sl
oop≠
1 ，我們不能像其他幀一樣假設s=1，否則所有幀尺度都為1，沒有尺度誤差怎麼優化尺度呢。一般對於檢測到回環匹配兩幀，我們都會採用附錄方法計算兩幀間sim3。
假設優化後pose的sim3為sc
orjw
=(rj
w,tj
w,s)
，則糾正之後pose： tc
orjw
=(rj
w,1s
tjw)
point的3d座標pk
w 糾正為： wp
cork
=sco
rwjt
jwwp
k 對比6自由度優化和7自由度優化，會發現在7自由sim3上優化後結果更加接近於真實值。
附錄：
[1]closed-form solutionof absolute orientation using unit quaternions.
[2]scale drift-aware large scale monocular slam.
單目相機標定
主要 執行前需要先準備標定和記錄標定列表的文字檔案，並放入程式所在目錄下 文字檔案的內容如下 calibdata.txt 裡面填寫好需要標定的名稱 for int ii 0 iiif 0 ii cornernum 24 是每幅的角點個數。此判斷語句是為了輸出 號，便於控制台 輸出所有的角點 cout...
單目相機標定
1 matlbe 單目標定步驟 常用術語 中文名 英文名 內參矩陣 intrinsic matrix 焦距 focal length 主點 principal point 徑向畸變 radial distortion 切向畸變 tangential distortion 旋轉矩陣 rotation ...
單目操作符
單目操作符 非 邏輯反操作，負值，正值，取位址，sizeof 不是函式 運算元的型別長度，以位元組為單位 按位取反 包括符號位 前置 後置 簡接訪問操作符 解引用操作符 型別 強制型別轉換 int main printf d n a 0 printf d n b 1 printf p n a 00d...