视觉SLAM十四讲(高翔版本)，ch4章节部分笔记
目标：理解slam的框架以及它的理论知识。供以后自己查阅。
这一章主要非常重要，也是理解后续优化的基础，它是将旋转矩阵和平移向量，转化为李代数的形式进行优化，因为它有很多好处。好处如下：
意思就是采用 R , t R,t R,t的矩阵表达形式，有冗余。因为旋转向量内部的 R R R的９个变量之间有约束，不利于优化。它可以转化为可以用三个无关变量表示的，转化为无约束的能量函数。这也是下面说的李代数。
1) 李代数：
这一章需要介绍一个基本的代数结构(大学的线性代数和离散数学里面有这方面的概念介绍)：群
集合： A A A, 运算为 ⋅ \cdot ⋅, 如果当运算满足以下性质时，称 ( A , ⋅ ) (A, \cdot) (A,⋅)为群。
性质：
1:封闭性： ∀ a 1 , a 2 ∈ A , a 1 ⋅ a 2 ∈ A \forall a_1,a_2 \in A, a_1 \cdot a_2 \in A ∀a1,a2∈A,a1⋅a2∈A
2:结合律： ∀ a 1 , a 2 , a 3 ∈ A , ( a 1 ⋅ a 2 ) ⋅ a 3 = a 1 ⋅ ( a 2 ⋅ a 3 ) \forall a_1, a_2, a_3 \in A, (a_1 \cdot a_2) \cdot a_3=a_1 \cdot(a_2 \cdot a_3) ∀a1,a2,a3∈A,(a1⋅a2)⋅a3=a1⋅(a2⋅a3)
3:幺元： ∃ a 0 ∈ A , s . t . ∀ a ∈ A , a 0 ⋅ a = a ⋅ a 0 = a \exist a_0 \in A, s.t. \space \space \forall a\in A, a_0 \cdot a=a \cdot a_0 = a ∃a0∈A,s.t. ∀a∈A,a0⋅a=a⋅a0=a
4:逆： ∀ a ∈ A , ∃ a − 1 ∈ A , s . t . a ⋅ a − 1 = a 0 \forall a \in A, \exist a^{-1} \in A, s.t. \space \space a \cdot a^{-1}=a_0 ∀a∈A,∃a−1∈A,s.t. a⋅a−1=a0
上一章节讲了旋转矩阵 R R R和平移向量 t t t，总结如下它们都满足群的性质，且是一些特殊群。是群的一部分，如下：
三维旋转矩阵构成特殊正交群 S O ( 3 ) SO(3) SO(3):
S O ( 3 ) = { R ∈ R 3 × 3 ∣ R R T = I , d e t ( R ) = 1 } ( 1 ) SO(3)= \begin{Bmatrix} \bold{R} \in R^{3\times 3} | \bold{R} \bold{R} ^T=I,det(\bold{R} )=1 \end{Bmatrix} \space \space \space \space (1) SO(3)={R∈R3×3∣RRT=I,det(R)=1} (1)
三维变换矩阵构成特殊特殊欧式群
S E ( 3 ) = { T = [ R t 0 T 1 ] ∈ R 4 × 4 ∣ R ∈ S O ( 3 ) , T ∈ R 3 } ( 2 ) SE(3) = \begin{Bmatrix} \bold{T} = \begin{bmatrix} \bold{R} & t \\ 0^T & 1 \end{bmatrix} \in R^{4 \times 4} | \bold{R} \in SO(3), T \in R^3 \end{Bmatrix} \space \space \space \space (2) SE(3)={T=[R0Tt1]∈R4×4∣R∈SO(3),T∈R3} (2)
可以验证：
旋转矩阵集合和矩阵乘法构成群。
变换矩阵和矩阵乘法构成群
可以统称旋转矩阵群，和变换矩阵群。
其它常见的群。
（私下可以验证上述４个性质）
在ＳＬＡＭ中， S O ( 2 ) , S O ( 3 ) ; S E ( 2 ) , S E ( 3 ) SO(2),SO(3); SE(2),SE(3) SO(2),SO(3);SE(2),SE(3)非常常见的群。
群论是一门课题，一般理论数学比较多。后续介绍其中的李群
李群(Lie Group)
具有连续的性质的群。
既有群也是流形
因为刚体运动中一般为空间连续运动，可以看到 S O ( 3 ) , S E ( 3 ) SO(3),SE(3) SO(3),SE(3)都是李群（连续性）。
群中的性质，只有乘法，没有加法，这个限制了很多运算。如求导和求极限。
为了解决没有加法的问题，引出李代数的概念。
李代数：和李群对应的结构，位于向量空间。是李群单位处的正切空间。(见下图使用小写的 s o ( 3 ) so(3) so(3)表示)
后面介绍上述图像表示的意思为啥成立。
引出李代数：
任意的旋转矩阵 R R R，满足：
R R T = I ( 3 ) RR^T=I \space \space \space \space (3) RRT=I (3)
考虑到 R R R随着时间的变化，有：
R ( t ) R ( t ) T = I R(t)R(t)^T = I R(t)R(t)T=I
两侧对时间求导：
R ˙ ( t ) R ( t ) T + R ( t ) R ˙ ( t ) T = 0 = > R ˙ ( t ) R ( t ) T = − ( R ˙ ( t ) R ( t ) T ) T ( 4 ) \dot{R}(t)R(t)^T +R(t) \dot{R}(t)^T=0 \\ => \dot{R}(t)R(t)^T = -(\dot{R}(t)R(t)^T)^T \space \space \space \space (4) R˙(t)R(t)T+R(t)R˙(t)T=0=>R˙(t)R(t)T=−(R˙(t)R(t)T)T (4)
通过上述的公式 ( 4 ) (4) (4)得到 R ˙ ( t ) R ( t ) T \dot{R}(t)R(t)^T R˙(t)R(t)T是反对称矩阵。可以写成一个符号( a a a为向量； A A A是矩阵)：
a ∧ = A = [ 0 − a 3 a 2 a 3 0 − a 1 − a 2 a 1 0 ] ; A ∨ = a a^{\land} = A =\begin{bmatrix} 0 & -a_3 & a_2 \\ a_3 & 0 & -a_1 \\ -a_2 & a_1 & 0 \end{bmatrix}; \space \space A^{\lor}=a a∧=A=⎣⎡0a3−a2−a30a1a2−a10⎦⎤; A∨=a
替换符号反对称矩阵（为了简便看）：
R ˙ ( t ) R ( t ) T = ϕ ∧ ( 5 ) \dot{R}(t)R(t)^T=\phi^{\land} \space \space \space \space (5) R˙(t)R(t)T=ϕ∧ (5)
将公式 ( 5 ) (5) (5)左右乘 R ( t ) R(t) R(t)可以得到如下：
R ˙ ( t ) = ϕ ∧ R ( t ) ( 6 ) \dot{R}(t)=\phi^{\land}R(t) \space \space \space \space (6) R˙(t)=ϕ∧R(t) (6)
可以看到对 R ( t ) R(t) R(t)求导其实是左乘一个反对称矩阵。
公式 ( 6 ) (6) (6)是一个非常重要的公式，也是后续计算的基础。因为有泰勒展开这个法宝。假设在 t 0 t_0 t0时刻泰勒展开，得到如下：
R ( t ) = R ( t 0 ) + R ˙ ( t 0 ) ( t − t 0 ) + ⋅ ⋅ ⋅ R(t)=R(t_0)+\dot{R}(t_0)(t-t_0)+ \cdot \cdot \cdot R(t)=R(t0)+R˙(t0)(t−t0)+⋅⋅⋅
一般情况下是求得一节求导比较多，省略后面的高阶项。
R ( t ) = R ( t 0 ) + R ˙ ( t 0 ) ( t − t 0 ) ＝ R ( t 0 ) + ϕ ( t 0 ) ∧ ( t ) ( 7 ) R(t)=R(t_0)+\dot{R}(t_0)(t-t_0) \\ ＝R(t_0)+ \phi(t_0)^{\land}(t) \space \space \space \space (7) R(t)=R(t0)+R˙(t0)(t−t0)＝R(t0)+ϕ(t0)∧(t) (7)
可以看到 ϕ ∧ \phi^{\land} ϕ∧它就是在 R ( t ) R(t) R(t)的正切空间上(见上图)。
因此在连续的空间中， t 0 t_0 t0附近， ϕ \phi ϕ不变情况下，有微分方程：
R ˙ ( t ) = ϕ ( t 0 ) ∧ R ( t ) = ϕ ( 0 ) ∧ R ( t ) ( 8 ) \dot{R}(t)=\phi(t_0)^{\land}R(t)=\phi(0)^{\land}R(t) \space \space \space \space (8) R˙(t)=ϕ(t0)∧R(t)=ϕ(0)∧R(t) (8)
如果 R ( 0 ) = I R(0)=I R(0)=I，代入到公式 ( 8 ) (8) (8),解方程得到（因为 e x p ( x ) = e x p ( x ) exp(x)=exp(x) exp(x)=exp(x)，其它为常数项，很容易得到）：
R ( t ) = e x p ( ϕ 0 ∧ t ) ( 9 ) R(t)=exp(\phi_0^{\land}t)\space \space \space \space (9) R(t)=exp(ϕ0∧t) (9)
上面的公式得到对于任意的 t t t,可以找到对应的关系 R R R和 ϕ \phi ϕ对应关系。这里称 ϕ \phi ϕ为李代数 s o ( 3 ) so(3) so(3)。
它也有很多性质和定义：
李代数由一个集合 V V V,一个数域 F F F和一个二元运算 [ , ] [,] [,]组成，同事满足下列性质，称为 ( V , F , [ , ] ) (V,F,[,]) (V,F,[,])为李代数，即为 G G G。
1:封闭性： ∀ X , Y ∈ V , [ X , Y ] ∈ V \forall X,Y \in V, [X,Y] \in V ∀X,Y∈V,[X,Y]∈V
2:双线性： ∀ X , Y , Z ∈ V ; a , b ∈ F \forall X,Y,Z \in V; a,b \in F ∀X,Y,Z∈V;a,b∈F,有： [ a X + b Y , Z ] = a [ X , Y ] + b [ Y , Z ] , [ Z , a X + b Y ] = a [ Z , X ] + b [ Z , Y ] [aX+bY, Z]=a[X,Y]+b[Y,Z], [Z,aX+bY]=a[Z,X]+b[Z,Y] [aX+bY,Z]=a[X,Y]+b[Y,Z],[Z,aX+bY]=a[Z,X]+b[Z,Y]
3:自反性： ∀ X ∈ V , [ X , X ] = 0 \forall X \in V, [X,X]=0 ∀X∈V,[X,X]=0
4:雅克比等价： ∀ X , Y , Z ∈ V , [ X , [ Y , Z ] ] + [ Z , [ Y , Z ] ] + [ Y , [ Z , X ] ] = 0 \forall X,Y,Z \in V, [X,[Y,Z]]+[Z,[Y,Z]]+[Y,[Z,X]]=0 ∀X,Y,Z∈V,[X,[Y,Z]]+[Z,[Y,Z]]+[Y,[Z,X]]=0
其中二元运算 [ , ] 被 称 为 李 括 号 [,]被称为李括号 [,]被称为李括号
在日常的SLAM中，发现三维空间向量+差积的运算构成李代数。表示如下：
s o ( 3 ) = { ϕ ∈ R 3 , Φ = ϕ ∧ ∈ R 3 × 3 } ( 10 ) so(3)=\begin{Bmatrix} \phi\in R^3, \Phi=\phi^{\land} \in R^{3 \times 3} \end{Bmatrix} \space \space \space \space (10) so(3)={ϕ∈R3,Φ=ϕ∧∈R3×3} (10)
其中
Φ = ϕ ∧ = [ 0 − ϕ 3 ϕ 2 ϕ 3 0 − ϕ 1 − ϕ 2 ϕ 1 0 ] ∈ R 3 × 3 \Phi=\phi^{\land}=\begin{bmatrix} 0 & -\phi_3 & \phi_2 \\ \phi_3 & 0 & -\phi_1 \\ -\phi_2 & \phi_1 & 0 \end{bmatrix} \in R^{3 \times 3} Φ=ϕ∧=⎣⎡0ϕ3−ϕ2−ϕ30ϕ1ϕ2−ϕ10⎦⎤∈R3×3
同理，对于李代数 s e ( 3 ) se(3) se(3):
s e ( 3 ) = { ξ = [ ρ ϕ ] ∈ R 6 , ρ ∈ R 3 , ϕ ∈ s o ( 3 ) , ξ ∧ = [ ϕ ∧ ρ 0 T 1 ] ∈ R 4 × 4 } ( 11 ) se(3)=\begin{Bmatrix} \xi = \begin{bmatrix} \rho \\ \phi \end{bmatrix} \in R^6, \rho \in R^3, \phi \in so(3), \xi ^{\land} = \begin{bmatrix} \phi^{\land} & \rho \\ 0^T & 1 \end{bmatrix} \in R^{4 \times 4} \end{Bmatrix} \space \space \space \space (11) se(3)={ξ=[ρϕ]∈R6,ρ∈R3,ϕ∈so(3),ξ∧=[ϕ∧0Tρ1]∈R4×4} (11)
其中 s e ( 3 ) se(3) se(3)是由三个平移分量和三个旋转分量组成。
旋转和 s o ( 3 ) so(3) so(3)相同
平移是一个普通的向量
这个上尖尖不再是反对称矩阵。
ξ ∧ = [ ϕ ∧ ρ 0 T 1 ] ∈ R 4 × 4 ( 12 ) \xi ^{\land} = \begin{bmatrix} \phi^{\land} & \rho \\ 0^T & 1 \end{bmatrix} \in R^{4 \times 4} \space \space \space \space (12) ξ∧=[ϕ∧0Tρ1]∈R4×4 (12)
上面的公式是基本的概念，是后续优化的基础。
下面介绍优化的formulation
2)指数映射和对数映射
指数映射反映从李代数到李群的对应关系：
R = e x p ( ϕ ∧ ) R=exp(\phi^{\land}) R=exp(ϕ∧)
其中 ϕ ∧ \phi^{\land} ϕ∧为一个矩阵，怎么定义矩阵的运算？直接用Taylor展开。
e x p ( ϕ ∧ ) = ∑ n = 0 ∞ 1 n ! ( ϕ ∧ ) n exp(\phi^{\land}) = \sum^{\infin}_{n=0} \frac{1}{n!}(\phi^{\land})^n exp(ϕ∧)=n=0∑∞n!1(ϕ∧)n
需要化简上面的泰勒公式（因为 ϕ ∧ \phi^{\land} ϕ∧是矩阵）。
利用 ϕ \phi ϕ向量的一些性质来处理 ϕ ∧ \phi^{\land} ϕ∧矩阵。
分开 ϕ \phi ϕ向量为方向和模长： ϕ = θ a \phi=\theta a ϕ=θa
因为 a a a为单位向量，所以有以下性质：
a ∧ a ∧ = a a T − I ( 13 ) a ∧ a ∧ a ∧ = − a ∧ ( 14 ) a^{\land}a^{\land}=aa^T-I \space \space \space \space (13) \\ a^{\land}a^{\land}a^{\land}=-a^{\land} \space \space \space \space (14) a∧a∧=aaT−I (13)a∧a∧a∧=−a∧ (14)
利用上述的 ( 13 ) , ( 14 ) (13),(14) (13),(14)公式化解上面的泰勒公式。
泰勒展开后得到：
e x p ( ϕ ∧ ) = e x p ( θ a ∧ ) = ∑ n = 0 ∞ 1 n ! ( θ a ∧ ) n = I + θ a ∧ + 1 2 ! θ 2 ( a ∧ a ∧ ) + 1 3 ! θ 3 ( a ∧ a ∧ a ∧ ) + . . . = a a T − a ∧ a ∧ + θ a ∧ + 1 2 ! θ 2 ( a ∧ a ∧ ) − 1 3 ! θ 3 ( a ∧ ) + . . . = a a T + ( θ − 1 3 ! θ 3 + 1 5 ! θ 5 − . . . ) a ∧ − ( 1 − 1 2 ! θ 2 + 1 4 ! θ 4 − . . . ) a ∧ a ∧ = a ∧ a ∧ + I + s i n θ a ∧ − c o s θ a ∧ a ∧ = ( 1 − c o s θ ) a ∧ a ∧ + I + s i n θ a ∧ = c o s θ I + ( 1 − c o s θ ) a a T + s i n θ a ∧ ( 15 ) exp(\phi^{\land})=exp(\theta a^{\land})= \sum^{\infin}_{n=0} \frac{1}{n!}(\theta a^{\land})^n \\ = I + \theta a^{\land}+\frac{1}{2!}\theta^2(a^{\land}a^{\land}) + \frac{1}{3!}\theta^3(a^{\land}a^{\land}a^{\land})+ ... \\ =aa^T - a^{\land}a^{\land} + \theta a^{\land} + \frac{1}{2!}\theta^2(a^{\land}a^{\land})-\frac{1}{3!}\theta^3(a^{\land})+... \\ =aa^T + (\theta -\frac{1}{3!}\theta^3 + \frac{1}{5!}\theta^5- ... )a^{\land} - (1-\frac{1}{2!}\theta^2+ \frac{1}{4!}\theta^4- ...) a^{\land}a^{\land} \\ =a^{\land}a^{\land} + I + sin\theta a^{\land} - cos\theta a^{\land}a^{\land} \\ = (1-cos \theta)a^{\land}a^{\land} + I + sin\theta a^{\land} \\ =cos \theta I + (1- cos \theta) aa^T +sin\theta a^{\land} \space \space \space \space (15) exp(ϕ∧)=exp(θa∧)=n=0∑∞n!1(θa∧)n=I+θa∧+2!1θ2(a∧a∧)+3!1θ3(a∧a∧a∧)+...=aaT−a∧a∧+θa∧+2!1θ2(a∧a∧)−3!1θ3(a∧)+...=aaT+(θ−3!1θ3+5!1θ5−...)a∧−(1−2!1θ2+4!1θ4−...)a∧a∧=a∧a∧+I+sinθa∧−cosθa∧a∧=(1−cosθ)a∧a∧+I+sinθa∧=cosθI+(1−cosθ)aaT+sinθa∧ (15)
最后得到如下：
e x p ( θ a ∧ ) = c o s θ I + ( 1 − c o s θ ) a a T + s i n θ a ∧ ( 16 ) exp(\theta a^{\land})=cos \theta I + (1- cos \theta) aa^T +sin\theta a^{\land} \space \space \space \space (16) exp(θa∧)=cosθI+(1−cosθ)aaT+sinθa∧ (16)
上述就是罗德里公式，可以看到 s o ( 3 ) so(3) so(3)的物理意义为旋转向量。
ϕ = I n ( R ) ∨ = ( ∑ n = 0 ∞ ( − 1 ) n n + 1 ( R − I ) n + 1 ) ∨ ( 17 ) \phi = In(R)^{\lor}=(\sum^{\infin}_{n=0} \frac{(-1)^n}{n+1}(R-I)^{n+1})^{\lor} \space \space \space \space (17) ϕ=In(R)∨=(n=0∑∞n+1(−1)n(R−I)n+1)∨ (17)
CH3章节介绍了，从旋转矩阵到旋转向量的转化关系如下(可以参考上一个章节)：
θ = a r c c o s ( t r ( R ) − 1 2 ) ; R n = n ( 18 ) \theta=arccos(\frac{tr(R) - 1}{2}); \space \space \space Rn=n \space \space \space \space (18) θ=arccos(2tr(R)−1); Rn=n (18)
上面就介绍了从 S O ( 3 ) SO(3) SO(3)到 s o ( 3 ) so(3) so(3)的对应关系。
同样的方式可以获取 s e ( 3 ) se(3) se(3)到 S E ( 3 ) SE(3) SE(3)的指数映射：
e x p ( ξ ∧ ) = [ ∑ n = 0 ∞ 1 n ! ( ϕ ∧ ) n ∑ n = 0 ∞ 1 ( n + 1 ) ! ( ϕ ∧ ) n ρ 0 T 1 ] = [ R J ρ 0 T 1 ] = T ( 19 ) exp(\xi ^{\land}) = \begin{bmatrix} \sum^{\infin}_{n=0} \frac{1}{n!}(\phi^{\land})^n & \sum^{\infin}_{n=0} \frac{1}{(n+1)!}(\phi^{\land})^n\rho \\ 0^T & 1 \end{bmatrix} \\ = \begin{bmatrix} R & J \rho \\ 0^T & 1 \end{bmatrix} =T \space \space \space \space (19) exp(ξ∧)=[∑n=0∞n!1(ϕ∧)n0T∑n=0∞(n+1)!1(ϕ∧)nρ1]=[R0TJρ1]=T (19)
上图中的李代数的平移部分到矩阵的平移部分相差一个线性变换(后续给出推导的过程)，由 J J J给出
J = s i n θ θ I + ( 1 − s i n θ θ ) a a T + 1 − c o s θ θ a ∧ ( 20 ) J=\frac{sin \theta}{\theta}I+(1-\frac{sin \theta}{\theta})aa^T+\frac{1 - cos \theta}{\theta}a^{\land} \space \space \space \space (20) J=θsinθI+(1−θsinθ)aaT+θ1−cosθa∧ (20)
总结上述公式(见高翔老师的ＰＰＴ中的总结)：
因为李群没法求导，因此用李代数，在李代数上用加法，定义李群上导数（因为它们一一映射）。
为了获取上述过程，需要验证，在李代数上做加法的时候在李群上是啥意义？如下？
e x p ( ϕ 1 ∧ ) e x p ( ϕ 2 ∧ ) = e x p ( ( ϕ 1 + ϕ 2 ) ∧ ) ( 21 ) ? ? ? ? ( w r o n g ) exp(\phi_1^{\land})exp(\phi_2^{\land}) = exp((\phi_1 + \phi_2)^{\land}) \space \space \space \space (21) \space \space \space \space ????(wrong) exp(ϕ1∧)exp(ϕ2∧)=exp((ϕ1+ϕ2)∧) (21) ????(wrong)
因为 ϕ 1 ∧ , ϕ 2 ∧ \phi_1^{\land},\phi_2^{\land} ϕ1∧,ϕ2∧是矩阵，它们不成立。
但是在 e x p ( ( ϕ 1 + ϕ 2 ) ∧ ) exp((\phi_1 + \phi_2)^{\land}) exp((ϕ1+ϕ2)∧) 具体是啥样子的，可以查(BCH:Baker-Campbell-Hausdorff)，里面有详细的公式推导，我也没有具体看，所以略过。给出结果公式
I n ( e x p ( ( ϕ 1 + ϕ 2 ) ∧ ) ) = A + B + 1 2 [ A , B ] + 1 12 [ A , [ A , B ] ] − 1 12 [ B , [ A , B ] ] + . . . ( 22 ) In(exp((\phi_1 + \phi_2)^{\land})) = A + B + \frac{1}{2}[A,B]+\frac{1}{12}[A,[A,B]]-\frac{1}{12}[B,[A,B]]+... \space \space \space \space (22) In(exp((ϕ1+ϕ2)∧))=A+B+21[A,B]+121[A,[A,B]]−121[B,[A,B]]+... (22)
公式 22 22 22中 [ A , B ] [A,B] [A,B]是李括号。上述的公式中，如果一个是小量的时候，可以忽略高阶项.可以得到如下：
I n ( e x p ( ( ϕ 1 + ϕ 2 ) ∧ ) ) ∨ ≈ { J l ( ϕ 2 ) − 1 ϕ 1 + ϕ 2 if ϕ 1 i s s m a l l J r ( ϕ 1 ) − 1 ϕ 2 + ϕ 1 if ϕ 2 i s s m a l l ( 23 ) In(exp((\phi_1 + \phi_2)^{\land}))^{\lor} \approx \begin{cases} J_l(\phi_2)^{-1} \phi_1 + \phi_2 &\text{if } \phi_1 \space is \space small \\ J_r(\phi_1)^{-1} \phi_2 + \phi_1 &\text{if } \phi_2 \space is \space small \end{cases} \space \space \space \space (23) In(exp((ϕ1+ϕ2)∧))∨≈{Jl(ϕ2)−1ϕ1+ϕ2Jr(ϕ1)−1ϕ2+ϕ1if ϕ1 is smallif ϕ2 is small (23)
上述公式 ( 23 ) (23) (23)中的 J l , J r J_l,J_r Jl,Jr分别表示左乘雅可比，右乘雅可比。如下：
J l = J = s i n θ θ I + ( 1 − s i n θ θ ) a a T + 1 − c o s θ θ a ∧ ( 24 ) J_l = J=\frac{sin \theta}{\theta}I+(1-\frac{sin \theta}{\theta})aa^T+\frac{1 - cos \theta}{\theta}a^{\land} \space \space \space \space (24) Jl=J=θsinθI+(1−θsinθ)aaT+θ1−cosθa∧ (24)
J l − 1 = θ 2 c o t θ 2 I + ( 1 − θ 2 c o t θ 2 ) a a T + θ 2 a ∧ ( 25 ) J_l^{-1} = \frac{\theta}{2} cot \frac{\theta}{2} I+(1-\frac{\theta}{2} cot\frac{ \theta}{2})aa^T+\frac{\theta}{2}a^{\land} \space \space \space \space (25) Jl−1=2θcot2θI+(1−2θcot2θ)aaT+2θa∧ (25)
上述 ( 24 ) (24) (24)公式是左乘雅可比，右乘雅可比如下：
J r ( ϕ ) = J l ( − ϕ ) ( 26 ) J_r(\phi)=J_l(-\phi) \space \space \space \space (26) Jr(ϕ)=Jl(−ϕ) (26)
介绍了上面的公式，因为其中一个是小量,用 Δ ϕ \Delta \phi Δϕ表示，通过公式 ( 23 ) (23) (23)可以得到如下：
e x p ( Δ ϕ ∧ ) e x p ( ϕ ∧ ) = e x p ( ( ϕ + J l − 1 ( ϕ ) Δ ϕ ) ∧ ) ( 27 ) exp(\Delta\phi^{\land})exp(\phi^{\land}) = exp((\phi + J_l^{-1} (\phi)\Delta\phi)^{\land}) \space \space \space \space (27) exp(Δϕ∧)exp(ϕ∧)=exp((ϕ+Jl−1(ϕ)Δϕ)∧) (27)
通过基本的公式 ( 27 ) (27) (27)得到，在李群上左乘小量时，李代数上的加法相差左雅可比的逆。
反之：
e x p ( ( ϕ + Δ ϕ ) ∧ ) = e x p ( ( J l Δ ϕ ) ∧ ) e x p ( ϕ ∧ ) ＝ e x p ( ϕ ∧ ) e x p ( ( J r Δ ϕ ) ∧ ) ( 28 ) exp((\phi +\Delta \phi)^{\land}) = exp((J_l \Delta \phi)^{\land})exp(\phi^{\land}) ＝ exp(\phi^{\land})exp((J_r \Delta \phi)^{\land}) \space \space \space \space (28) exp((ϕ+Δϕ)∧)=exp((JlΔϕ)∧)exp(ϕ∧)＝exp(ϕ∧)exp((JrΔϕ)∧) (28)
通过公式 ( 28 ) (28) (28)得到，在李代数上小量加法时，相当于李群上左乘一个左雅可比量(右乘一个右雅可比量)
上述是对 s o ( 3 ) so(3) so(3)运算，同样方式可以对 s e ( 3 ) se(3) se(3)运算。
e x p ( Δ ξ ∧ ) e x p ( ξ ∧ ) ≈ e x p ( ( J l − 1 Δ ξ + ξ ) ∧ ) ( 29 ) exp(\Delta\xi^{\land})exp(\xi^{\land}) \approx exp((J_l^{-1} \Delta \xi + \xi)^{\land}) \space \space \space \space (29) exp(Δξ∧)exp(ξ∧)≈exp((Jl−1Δξ+ξ)∧) (29)
e x p ( ξ ∧ ) e x p ( Δ ξ ∧ ) ≈ e x p ( ( J r − 1 Δ ξ + ξ ) ∧ ) ( 30 ) exp(\xi^{\land})exp(\Delta\xi^{\land}) \approx exp((J_r^{-1} \Delta \xi + \xi)^{\land}) \space \space \space \space (30) exp(ξ∧)exp(Δξ∧)≈exp((Jr−1Δξ+ξ)∧) (30)
总结，上述公式的作用是，通过BCH的线性近似，定义了李代数上的导数。
在slam过程中，会遇到旋转后的顶点对旋转矩阵进行求导，可以记为：
∂ R p ∂ R \frac{\partial Rp}{\partial R} ∂R∂Rp
由于 R R R没有加法，导数无法定义。
存在两种解决办法：
１：对 R R R对应的李代数加上小量，求相对于小量的变化率（导数模型）
２：对 R R R左乘或者右乘一个小量，求相对于小量的李代数的变化率（扰动模型）
下面具体介绍两种模型推导公式：
导数模型推导：
∂ exp ( ϕ ∧ ) p ∂ ϕ = lim δ ϕ − > 0 exp ( ( ϕ + δ ϕ ) ∧ ) p − exp ( ϕ ∧ ) p δ ϕ = lim δ ϕ − > 0 exp ( ( J l δ ϕ ) ∧ ) exp ( ϕ ∧ ) p − exp ( ϕ ∧ ) p δ ϕ ≈ lim δ ϕ − > 0 ( I + ( J l δ ϕ ) ∧ ) exp ( ϕ ∧ ) p − exp ( ϕ ∧ ) p δ ϕ = lim δ ϕ − > 0 ( J l δ ϕ ) ∧ exp ( ϕ ∧ ) p δ ϕ = lim δ ϕ − > 0 − ( exp ( ϕ ∧ ) p ) ∧ ( J l δ ϕ ) δ ϕ = − ( R p ) ∧ J l ( 31 ) \frac{\partial \exp(\phi^{\land})p}{\partial \phi} = \lim_{\delta \phi -> 0} \frac{\exp((\phi + \delta \phi)^{\land})p - \exp(\phi ^{\land})p}{\delta \phi } \\ = \lim_{\delta \phi -> 0} \frac{\exp((J_l \delta \phi)^{\land})\exp(\phi ^{\land})p - \exp(\phi ^{\land})p}{\delta \phi } \\ \approx \lim_{\delta \phi -> 0} \frac{(I+(J_l \delta \phi)^{\land})\exp(\phi ^{\land})p - \exp(\phi ^{\land})p}{\delta \phi } \\ = \lim_{\delta \phi -> 0} \frac{(J_l \delta \phi)^{\land}\exp(\phi ^{\land})p}{\delta \phi } \\ = \lim_{\delta \phi -> 0} \frac{-(\exp(\phi ^{\land})p)^{\land}(J_l \delta \phi)}{\delta \phi } =-(Rp)^{\land}J_l \space \space \space \space (31) ∂ϕ∂exp(ϕ∧)p=δϕ−>0limδϕexp((ϕ+δϕ)∧)p−exp(ϕ∧)p=δϕ−>0limδϕexp((Jlδϕ)∧)exp(ϕ∧)p−exp(ϕ∧)p≈δϕ−>0limδϕ(I+(Jlδϕ)∧)exp(ϕ∧)p−exp(ϕ∧)p=δϕ−>0limδϕ(Jlδϕ)∧exp(ϕ∧)p=δϕ−>0limδϕ−(exp(ϕ∧)p)∧(Jlδϕ)=−(Rp)∧Jl (31)
上述 31 31 31公式的左乘的结果比较复杂，是否可以有其它简便的（试试扰动模型？）
扰动模型推导：
左边乘一个小量，且令其李代数趋于 0 0 0
∂ ( R p ) ∂ φ = lim φ − > 0 exp ( φ ∧ ) exp ( ϕ ∧ ) p − exp ( ϕ ∧ ) p φ ≈ lim φ − > 0 ( I + φ ∧ ) exp ( ϕ ∧ ) p − exp ( ϕ ∧ ) p φ = lim φ − > 0 φ ∧ R p φ = lim φ − > 0 − ( R p ) ∧ φ φ = − ( R p ) ∧ ( 32 ) \frac{\partial (Rp)}{\partial \varphi} = \lim_{\varphi -> 0}\frac{\exp(\varphi^{\land})\exp(\phi ^{\land})p - \exp(\phi ^{\land})p}{\varphi} \\ \approx \lim_{\varphi -> 0}\frac{(I+\varphi^{\land})\exp(\phi ^{\land})p - \exp(\phi ^{\land})p}{\varphi} \\ = \lim_{\varphi -> 0}\frac{\varphi^{\land}Rp}{\varphi} = \lim_{\varphi -> 0}\frac{-(Rp)^{\land}\varphi}{\varphi} = -(Rp)^{\land} \space \space \space \space (32) ∂φ∂(Rp)=φ−>0limφexp(φ∧)exp(ϕ∧)p−exp(ϕ∧)p≈φ−>0limφ(I+φ∧)exp(ϕ∧)p−exp(ϕ∧)p=φ−>0limφφ∧Rp=φ−>0limφ−(Rp)∧φ=−(Rp)∧ (32)
( 32 ) (32) (32)这个公式可以看到它非常简约，对于三维顶点求导旋转部分，更加实用。
在 S E ( 3 ) SE(3) SE(3)的扰动模型：
∂ ( T p ) ∂ δ ξ = lim δ ξ − > 0 exp ( δ ξ ∧ ) exp ( ξ ∧ ) p − exp ( ξ ∧ ) p δ ξ ≈ lim δ ξ − > 0 ( I + δ ξ ∧ ) exp ( ξ ∧ ) p − exp ( ξ ∧ ) p δ ξ = l i m δ ξ − > 0 ( δ ξ ∧ ) exp ( ξ ∧ ) p δ ξ = lim δ ξ − > 0 [ δ ϕ ∧ δ ρ 0 T 0 ] [ ( R p + t ) 1 ] δ ξ = lim δ ξ − > 0 [ δ ϕ ∧ ( R p + t ) ∧ + δ ρ 0 T ] δ ξ = [ I − ( R p + t ) ∧ 0 T 0 T ] ( 34 ) \frac{\partial (Tp)}{\partial \delta\xi} = \lim_{\delta\xi -> 0}\frac{\exp(\delta\xi^{\land})\exp(\xi ^{\land})p - \exp(\xi ^{\land})p}{\delta\xi} \\ \approx \lim_{\delta\xi -> 0}\frac{(I+\delta\xi^{\land})\exp(\xi ^{\land})p - \exp(\xi ^{\land})p}{\delta\xi} \\ = lim_{\delta\xi -> 0}\frac{(\delta\xi^{\land})\exp(\xi ^{\land})p}{\delta\xi} \\ = \lim_{\delta \xi -> 0}\frac{\begin{bmatrix}\delta \phi ^{\land} & \delta\rho \\ 0^T & 0 \end{bmatrix}\begin{bmatrix} (Rp+t) \\ 1 \end{bmatrix}}{\delta\xi} \\ \\ = \lim_{\delta \xi -> 0}\frac{\begin{bmatrix} \delta \phi ^{\land} (Rp+t)^{\land} + \delta \rho \\ 0^T \end{bmatrix} }{\delta\xi} \\ = \begin{bmatrix} I & -(Rp+t)^{\land} \\ 0^T & 0^T \end{bmatrix} \space \space \space \space (34) ∂δξ∂(Tp)=δξ−>0limδξexp(δξ∧)exp(ξ∧)p−exp(ξ∧)p≈δξ−>0limδξ(I+δξ∧)exp(ξ∧)p−exp(ξ∧)p=limδξ−>0δξ(δξ∧)exp(ξ∧)p=δξ−>0limδξ[δϕ∧0Tδρ0][(Rp+t)1]=δξ−>0limδξ[δϕ∧(Rp+t)∧+δρ0T]=[I0T−(Rp+t)∧0T] (34)
利用ＢＣＨ线性近似，可以推导 s o ( 3 ) , s e ( 3 ) so(3),se(3) so(3),se(3)上的导数和扰动模型。
其中扰动模型更加简单和常用。
它在SOPHUS库中都有实现，大家可以下载配置这个库，然后测试一下。