視覺SLAM（三) 李群與李代數

群

群的性質

• 旋轉矩陣集合與旋轉乘法構成群
• 變換矩陣與矩陣乘法構成群
• 因此可以稱為旋轉矩陣群和變換矩陣群
  三維旋轉矩陣構成了特殊正交群

其他群的例子:

1. 一般線性群GL(n) n*n的可逆矩陣 它們對矩陣乘法構成群
2. 特殊正交群SO(n)
3. 特殊歐式群SE(n)

\[SO(3)=\{R\in R^{3*3}|RR^T=I,det(R)=1\}. \]

三維變換矩陣構成了特殊矩陣群

李群與李代數

李群:

1. 具有連續(光滑)性質的群
2. 既是群也是流形
3. 直觀上看,一個剛體能連續地在空間中運動,故SO(3)和SE(3)都是李群
4. 但是,SO(3)和SE(3)只有定義良好的乘法,沒有加法,所以難以進行取極限\求導等操作.

李代數:與李群對應的一種結構,位於向量空間.

• 通常記作小寫的SO(3)和SE(3),書中以哥特體突出顯示.
• 事實上是李群單位元處的正切空間
• 每個李群都有與之對應的李代數。李代數描述了李群單位元附近的正切空間性質

從旋轉矩陣引出李代數

• 考慮任意旋轉矩陣R,滿足 \(RR^T=I\)
  

令R隨時間變換(連續運動),有\(R(t){R(t)}^T=I\)
量測對時間求導:\(\hat{R(t)}{R(t)}^T+R(t){\hat{R(t)}}^T=0\)
整理:\(\hat{R(t)=\Phi(t)^{\times}R(t)}\)
兩側右乘R(t):\(\hat{R(t)=\Phi(t)^{\times}R(t)}\)
課看成對R求導之后,左側多出一個\(\Phi(t)\)

單位元附近:\(t_0=0\),\(R(0)=I\)

\[R(t)\approx R(t_0)+\dot R(t_0)(t-t_0)=I+\phi(t_0)^{\times}(t) \]

已知初始情況：\(R(0)=I\),解之，得:

\[R(t)=exp(\phi_0^{\times}t). \]

該式說明，對任意t，都可以找到一個R和一個\(\phi\)得對應關系
該關系指指數映射（Exponetial Map）
這里得\(\phi\)稱為SO（3）對應得李代數：SO(3)

指數映射與對數映射

指數映射反映了從李代數到李群的對應關系：\(R=\exp(\phi^{\wedge})\)
問題：但是\(\phi^{\wedge}\)是一個矩陣，對於矩陣，如何定義求指數運算？
\(\exp(\phi^{\wedge})=\sum^{}_{n=0} \frac{1}{n!}(\phi^{\wedge})^n\)

由於\(\phi\)得向量，定義其角度和模長：
角度乘單位向量：\(\phi=\theta a\)

\[a^{\wedge}a^{\wedge}=aa^T-I \]

\[a^{\wedge}a^{\wedge}a^{\wedge} = -a^{\wedge} \]

a的性質化解taylor展示中的高階項提供了有效方法

Taylor展開：

結果：\(\exp(\theta a^{\wedge}) = \cos \theta I+(1-cos\theta)aa^T+sin\theta a^{\wedge}\)

這說明SO(3)的物理意義就是旋轉向量；
反之，給的旋轉矩陣是，亦能求李代數：

\[\phi = ln(R)^{\vee} = (\sum^{\infty}_{n=0}\frac{(-1)^n}{n+1}(R-I)^{n+1})^{\vee} \]

但實際中沒必要這樣求，在旋轉向量小姐已經介紹了矩陣到向量的轉換關系：
$\theta = arccos(\frac{tr(R)-1}{2}) $ \(Rn=n\)
至此，說明了\(SO(3)\)與\(so(3)\)的對應關系。

求導與擾動模型

SLAM的定位即位姿估計
但李群無加法：\(R_1+R_2\notin SO(3)\)倒數無從定義
解決方法：

• 利用李代數上加法定義李群元素的導數？
• 使用指數映射和對數映射完成變換關系
  基本問題：當在李代數中做加法時，是否等價於在李群上做乘法？
  \(exp(\phi_1^{\wedge})exp(\phi_2^{\vee})=exp((\phi_1+\phi_2)^{\wedge})\)
  在使用標量的情況下，該式明顯成立，但這里的\(\phi^{\wedge}\)為矩陣，完整形式由BCH公式給出

BCH公式

前面的幾項：

通過 BCH 線性近似，可以定義李代數上的導數
考慮一個基本問題：旋轉后的點關於旋轉的導數可以不嚴謹地記為：\(\frac{\partial (Rp)}{\partial R}\)
由於 R 沒有加法，導數無從定義，此時存在兩種解決辦法：
• 對 R 對應的李代數加上小量，求相對於小量的變化率（導數模型）；
• 對 R 左乘或右乘一個小量，求相對於小量的李代數的變化率（擾動模型）。

導數模型

擾動模型

左乘一個小量，令其李代數為0

SE(3)上的擾動模型

小結
• 利用 BCH 線性近似，可以推導 so(3) 與 se(3) 上的導數和擾動模型
• 通常情況下，擾動模型更為簡潔實用

實踐：Sophus庫

#include "sophus/so3.hpp"
#include "sophus/se3.hpp"