视觉SLAM十四讲(高翔版本),ch4章节部分笔记

目标:理解slam的框架以及它的理论知识。供以后自己查阅。

这一章主要非常重要,也是理解后续优化的基础,它是将旋转矩阵和平移向量,转化为李代数的形式进行优化,因为它有很多好处。好处如下:
视觉SLAM十四讲(高翔版本),ch4章节部分笔记_第1张图片

意思就是采用 R , t R,t R,t的矩阵表达形式,有冗余。因为旋转向量内部的 R R R的9个变量之间有约束,不利于优化。它可以转化为可以用三个无关变量表示的,转化为无约束的能量函数。这也是下面说的李代数。

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,a2A,a1a2A
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,a3A,(a1a2)a3=a1(a2a3)
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 a0A,s.t.  aA,a0a=aa0=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 aA,a1A,s.t.  aa1=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)={RR3×3RRT=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×4RSO(3),TR3}    (2)

可以验证:
旋转矩阵集合和矩阵乘法构成群。
变换矩阵和矩阵乘法构成群
可以统称旋转矩阵群,和变换矩阵群。
其它常见的群。
(私下可以验证上述4个性质)
在SLAM中, 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)表示)
视觉SLAM十四讲(高翔版本),ch4章节部分笔记_第2张图片

后面介绍上述图像表示的意思为啥成立。
引出李代数:
任意的旋转矩阵 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=0a3a2a30a1a2a10;  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)(tt0)+

一般情况下是求得一节求导比较多,省略后面的高阶项。
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)(tt0)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(ϕ0t)    (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,YV,[X,Y]V
2:双线性: ∀ X , Y , Z ∈ V ; a , b ∈ F \forall X,Y,Z \in V; a,b \in F X,Y,ZV;a,bF,有: [ 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 XV,[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,ZV,[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ϕ10R3×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=0n!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) aa=aaTI    (13)aaa=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=0n!1(θa)n=I+θa+2!1θ2(aa)+3!1θ3(aaa)+...=aaTaa+θa+2!1θ2(aa)3!1θ3(a)+...=aaT+(θ3!1θ3+5!1θ5...)a(12!1θ2+4!1θ4...)aa=aa+I+sinθacosθaa=(1cosθ)aa+I+sinθa=cosθI+(1cosθ)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+(1cosθ)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=0n+1(1)n(RI)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=0n!1(ϕ)n0Tn=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+θ1cosθa    (20)

总结上述公式(见高翔老师的PPT中的总结):
视觉SLAM十四讲(高翔版本),ch4章节部分笔记_第3张图片

因为李群没法求导,因此用李代数,在李代数上用加法,定义李群上导数(因为它们一一映射)。

为了获取上述过程,需要验证,在李代数上做加法的时候在李群上是啥意义?如下?

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+θ1cosθ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) Jl1=2θcot2θI+(12θ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((ϕ+Jl1(ϕ)Δϕ))    (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((Jl1Δξ+ξ))    (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((Jr1Δξ+ξ))    (30)

总结,上述公式的作用是,通过BCH的线性近似,定义了李代数上的导数

在slam过程中,会遇到旋转后的顶点对旋转矩阵进行求导,可以记为:

∂ R p ∂ R   \frac{\partial Rp}{\partial R}  RRp 

由于 R R R没有加法,导数无法定义。
存在两种解决办法:
1:对 R R R对应的李代数加上小量,求相对于小量的变化率(导数模型
2:对 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((ϕ+δϕ))pexp(ϕ)p=δϕ>0limδϕexp((Jlδϕ))exp(ϕ)pexp(ϕ)pδϕ>0limδϕ(I+(Jlδϕ))exp(ϕ)pexp(ϕ)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(ϕ)pexp(ϕ)pφ>0limφ(I+φ)exp(ϕ)pexp(ϕ)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(ξ)pexp(ξ)pδξ>0limδξ(I+δξ)exp(ξ)pexp(ξ)p=limδξ>0δξ(δξ)exp(ξ)p=δξ>0limδξ[δϕ0Tδρ0][(Rp+t)1]=δξ>0limδξ[δϕ(Rp+t)+δρ0T]=[I0T(Rp+t)0T]    (34)

利用BCH线性近似,可以推导 s o ( 3 ) , s e ( 3 ) so(3),se(3) so(3),se(3)上的导数和扰动模型。
其中扰动模型更加简单和常用。

它在SOPHUS库中都有实现,大家可以下载配置这个库,然后测试一下。

你可能感兴趣的:(c++)