slam 求解相機的位姿求解核心思想：將有約束的李群問題轉換成無約束的李代數問題，然后使用高斯牛頓算法或者LM(列文伯格-馬夸爾特法)求解。

人們找了很多以相機位姿為變量的誤差函數，比如光度誤差，重投影誤差，3D幾何誤差等等，希望使得誤差最小，使用BA（又稱位最小化重投影誤差），進而求得比較準確的相機位姿。舉一個重投影例子：?

?

T?=min12∑i=0n||ui?1siKTPi||2(1)

Pi是3D點，?ui是Pi對應的像素位置，?K為相機內參矩陣，si為ui對應的深度值。T就是我們要求的變量。上式只是一個粗略的表達，其中沒有顯式說明齊次與非齊次，我們認為自動滿足。有人可能會想對于這樣的式子直接求導為零，不就可以解出T了么。但是不要忘記了T是滿足如下約束的：?

?

T=[R0Tt1],RTR=I,det(R)=1,t∈R3(2)

R為旋轉矩陣，它是正交矩陣并且行列式為1，t是平移向量。在求解1式的時候，必須考慮到T滿足的約束，那么這個問題就變成了有約束優化問題。但是我們不想去求有約束的問題，因為不好求。我們更喜歡的是去求無約束問題，而且無約束問題有很多現成的解法。?
可去另外一個空間求解，比如李代數。最后研究表明確實通過李代數可以將（1）式轉化為無約束問題，然后很方便的通過高斯牛頓法，列文伯格-馬夸爾特法（簡稱LM法）等優化算法求解了。當然不僅僅是上面這個優化問題了，我們以相機位姿為變量的建立的目標函數都可以在李代數上求解。下面就簡單介紹一下李群和李代數。

?

?

李群和李代數

群：是一種集合加上一種運算的代數結構，我們把集合記為A，運算記為?.，那么群可以記為G=(A,.)。群要求這個運算要滿足封閉性，結合律，幺元和可逆。矩陣中常見的群有：

• 一般線性群GL（n） 指n×n的可逆矩陣，它們對矩陣乘法成群
• 特殊正交群SO（n） 就是旋轉矩陣群，其中SO（2），SO（3）最為常見。
• 特殊歐式群SE（n） 就是n維歐式變換，其中SE（2），SE（3）最為常見。

群結構保證了在群上的運算具有良好的性質，李群是指具有連續性質的群。SO（n），SE（n）在實數空間上是連續的，我們可以直觀的感覺到一個剛體能夠在空間中連續運動，所以SO（n）和SE（n）都是李群。那么相機的位姿就可以表示為：

?

SO(3)={R∈R3×3|RRT=I,det(R)=1}SE(3)={T=[R0Tt1]∈R4×4|R∈SO(3),t∈R3}

?

現在我們只是說明了相機位姿是特殊歐式群，旋轉矩陣是特殊正交群。但是現在還不能直接去求解因為約束依然存在。正如前面所說的，我們希望將有約束的問題轉化為無約束問題求解。數學上常用的方法就是去另外一個空間計算，就像分類問題一樣，也許在這個空間沒法分開，變換一個空間說不定就可以分開了。橫看成嶺側成峰就是這個道理。數學家們研究表明每一個李群都已與之對應的李代數，因為李群是連續的。真如我們所期望的，在李代數求解（1）式時可以將其化為無約束問題，通過高斯牛頓法等方法迭代求解。這是因為旋轉矩陣對加法不封閉，而李代數對加法封閉。

李群李代數相互轉化

旋轉矩陣對應的李代數是一個三維向量，相機位姿（剛性變換矩陣，沒有尺度因子）是一個六位向量。那么我們想知道的是這些向量是怎么和對應的李群聯系，或者說怎么變換。可參考高翔博士的《視覺slam十四講從理論到實踐》。下面這張圖也是來自這本書的：?

這張圖給出了李群和李代數之間的轉換公式。有了這個轉換，離我們求解1式問題還有些距離。在使用類似高斯牛頓法這樣的迭代算法時，目標函數關于變量的導數很重要，因為它提供了目標函數變小的方向。其實就是變量選擇的方向，那么我們就想知道目標函數關于李代數的導數。

李代數求導

使用李代數解決問題的求導思路分為兩種：?
1. 用李代數表示位姿，然后根據李代數加法來對李代數求導?
2. 對李群左乘或者右乘微小擾動量，然后對該擾動求導，成為左擾動和右擾動模型。

我們先說關于旋轉矩陣對應的李代數SO(3)求導，第一種的求導過程，如下：?

這樣我們就得到了旋轉后點相對于李代數的導數：?

Jl的形式如下：?

由于旋轉矩陣對應的李代數是一個三維向量，所以我們可以用模長與單位向量的乘積表示。?=θa，?θ為模長，a為單位向量，上式中θ?和 a 就是這個含義。

下來我們在來看看旋轉矩陣李代數的左擾動模型，左乘一個微小擾動，對微小擾動求導：?

這種方式省去了計算雅克比Jl，所以使用更為常見。?
最后直接給出SE(3)上的李代數求導，使用左擾動模型：?

李代數上求解相機位姿

有了李群和李代數相互轉換公式，李代數上的導數，那么我們就可以將1式用李代數重新表達。?

上式仍然只是一個粗略的表達，其中沒有顯式說明齊次與非齊次，我們認為自動滿足。使用李代數我們可以成功的將位姿約束問題轉化為無約束問題，但要用高斯牛頓法或者LM方法等迭代優化算法，我們還需要知道這個誤差關于位姿李代數的導數。使用左擾動模型，利用鏈式法則展開：?

相機投影模型相對于P‘為：?

那么就可以得到：?

求導的第二項我們前面已經給出了：?

取出這部分的前三行，然后兩個導數項相乘，就可以得到一個2*6的雅克比矩陣：?

如果變換矩se(3)的旋轉在前，平移在后，只需要將上式的前面3列和后面3列對調即可。有了李代數表示，也有了李代數上的表示，下一步我們就可以利用一些優化庫（ceres,?g2o）去計算了。

總結

以上是生活随笔為你收集整理的SLAM学习--------相机位姿表示-李群李代数的全部內容，希望文章能夠幫你解決所遇到的問題。

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