视觉SLAM流程包括以下步骤：

小萝卜携带传感器在环境中移动，由以下两件事描述：

1. 什么是运动 ?我们必须考虑从 k ? 1 k-1 k?1时刻到 k k k时刻,小萝卜的位置 x x x它是如何改变的.

   运动方程:

   x k = f ( x k ? 1 , u k , w k ) x_k = f(x_{k-1}, u_k, w_k) xk=f(xk?1,uk,wk)

   • x k , x k ? 1 x_k, x_{k-1} xk,xk?表示小萝卜在 k k k和 k ? 1 k-1 k?1时刻的位置
   • u k u_k uk表示运动传感器的读数(有时也叫输入)
   • w k w_k wk表示噪声

2. 什么是观测 ?假设小萝卜在 k k k时刻于 x k x_k xk到处探测某个路标 y j y_j yj,我们必须考虑这件事是如何用数学语言描述的.

   观测方程:

   z k , j = h ( y j , x k , v k , j ) z_{k,j} = h(y_j, x_k, v_{k,j}) zk,j=h(yj,xk,vk,j)

   • z k , j z_{k,j} zk,j小萝卜在 x k x_k xk路标点在位置上可见 y j y_j yj,观成的观测数据
   • y j y_j yj表示第 j j j个路标点
   • v k , j v_{k,j} vk,j表示噪声

这两个方程描述了最基本的方程SLAM问:当知道运动测量的读数 u u u ,以及传感器的读数 z z z 时,如何求解定位问题(估计 x x x )和建图问题(估计 y y y)?这时,我们就把SLAM问题建模成了一个状态估计问题:如何通过带有噪声的测量数据,估计内部的、隐藏着的状态变量?

1. 向量 a a a在线性空间的基 [ e 1 , e 2 , e 3 ] [e_1, e_2, e_3] [e1​,e2​,e3​]下的坐标为 [ a 1 , a 2 , a 3 ] T [a_1, a_2, a_3]^T [a1​,a2​,a3​]T.

   a = [ e 1 , e 2 , e 3 ] [ a 1 a 2 a 3 ] = a 1 e 1 + a 2 e 2 + a 3 e 3 a = [e_1, e_2, e_3] \left[

   a1a2a3

1. 1. \right] T−1=[RT0​−RTt1​]

若点 p = ( x , y ) p=(x,y) p=(x,y)在直线 l = ( a , b , c ) l=(a,b,c) l=(a,b,c)上,则有:
a x + b y + c = [ a , b , c ] T ⋅ [ x , y , 1 ] = l T ⋅ p ′ = 0 ax+by+c = [a,b,c]^T \cdot [x,y,1] = l^T \cdot p' = 0 ax+by+c=[a,b,c]T⋅[x,y,1]=lT⋅p′=0

若点 p = ( x , y , z ) p=(x,y,z) p=(x,y,z)在平面 A = ( a , b , c , d ) A=(a,b,c,d) A=(a,b,c,d)上,则有:
a x + b y + c z + d = [ a , b , c , d ] T ⋅ [ x , y , z , 1 ] = A T ⋅ p ′ = 0 ax+by+cz+d = [a,b,c,d]^T \cdot [x,y,z,1] = A^T \cdot p' = 0 ax+by+cz+d=[a,b,c,d]T⋅[x,y,z,1]=AT⋅p′=0

在齐次坐标下,

1. 可以用两个点 p p p, q q q的齐次坐标叉乘结果表示它们的共线 l l l.
2. 可以用两条直线 l l l, m m m的齐次坐标叉乘结果表示它们的交点 x x x.

这里利用叉乘的性质: 叉乘结果与两个运算向量都垂直:

• 性质1的证明:
  l T ⋅ p = ( p × q ) ⋅ p = 0 l T ⋅ q = ( p × q ) ⋅ q = 0 l^T \cdot p = (p \times q) \cdot p = 0 \\ l^T \cdot q = (p \times q) \cdot q = 0 lT⋅p=(p×q)⋅p=0lT⋅q=(p×q)⋅q=0

• 性质2的证明:
  l T ⋅ p = l T ⋅ ( l × m ) = 0 m T ⋅ p = m T ⋅ ( l × m ) = 0 l^T \cdot p = l^T \cdot (l \times m) = 0 \\ m^T \cdot p = m^T \cdot (l \times m) = 0 lT⋅p=lT⋅(l×m)=0mT⋅p=mT⋅(l×m)=0

• 点 ( x , y , z ) (x,y,z) (x,y,z)的齐次坐标为 ( x , y , z , 1 ) (x,y,z,1) (x,y,z,1)
• 向量 ( x , y , z ) (x,y,z) (x,y,z)的齐次坐标为 ( x , y , z , 0 ) (x,y,z,0) (x,y,z,0)

对于平行直线 l = ( a , b , c ) l=(a,b,c) l=(a,b,c)和 m = ( a , b , d ) m=(a,b,d) m=(a,b,d),求取其交点的齐次坐标 x = l × m = ( k b , − k a , 0 ) x=l \times m=(kb, -ka, 0) x=l×m=(kb,−ka,0),将其转为非齐次坐标,得到 x = ( k b / 0 , − k a / 0 ) = ( inf ⁡ , − inf ⁡ ) x = (kb/0, -ka/0) = (\inf, -\inf) x=(kb/0,−ka/0)=(inf,−inf),这表示无穷远点.

使用齐次坐标,可以将加法运算转化为乘法运算.

\right] =\left[

100010txty1

\right] * \left[

xy1

\right] =\left[

sx000sy0001

\right] * \left[

xy1

\right] =\left[

1hy0hx10001

\right] * \left[

xy1

\right] =\left[

cosθ−sinθ0sinθcosθ0001

\right] * \left[

xy1

• 旋转矩阵的缺点:

  1. 旋转矩阵有9个量,但一次旋转只有3个自由度,这种表达方式是冗余的.
  2. 旋转矩阵自带约束(必须是行列式为1的正交矩阵),这些约束会给估计和优化带来困难.

• 旋转向量: 任意旋转都可以用一个旋转轴和一个旋转角 来刻画.于是,我们可以使用一个向量,其方向表示旋转轴而长度表示旋转角.这种向量称为旋转向量(或轴角,Axis-Angle).

  假设有一个旋转轴为 n n n,角度为 θ \theta θ的旋转,其对应的旋转向量为 θ n \theta n θn.

• 旋转向量和旋转矩阵之间的转换:

  设旋转向量 R R R表示一个绕单位向量 n n n,角度为 θ θ θ的旋转.

  • 旋转向量到旋转矩阵:
    R = cos ⁡ θ I + ( 1 − cos ⁡ θ ) n n T + sin ⁡ θ   n ∧ R = \cos\theta I + (1-\cos\theta) n n^T + \sin\theta \, n^\wedge R=cosθI+(1−cosθ)nnT+sinθn∧

  • 旋转矩阵到旋转向量:

    • 旋转角 θ = arccos ⁡ ( t r ( R ) − 1 2 ) \theta = \arccos \left( \frac{tr(R)-1}{2} \right) θ=arccos(2tr(R)−1​)
    • 旋转轴 n n n是矩阵 R R R特征值1对应的特征向量

为什么需要四元数: 对于三维旋转,找不到不带奇异性的三维向量描述方式.因此引入四元数.
四元数是一种扩展的复数,既是紧凑的,也没有奇异性.

1. 四元数的定义

   一个四元数 q q q拥有一个实部和三个虚部
   q = q 0 + q 1 i + q 2 j + q 3 k q = q_0 + q_1 i + q_2 j + q_3 k q=q0​+q1​i+q2​j+q3​k

   其中 i i i, j j j, k k k,为四元数的3个虚部,它们满足以下关系式(自己和自己的运算像复数,自己和别人的运算像叉乘):
   { i 2 = j 2 = k 2 = − 1 i j = k , j i = − k j k = i , k j = − i k i = j , i k = − j \left\{

   i2=j2=k2=−1ij=k,ji=−kjk=i,kj=−iki=j,ik=−j

• \right. ⎩⎪⎪⎪⎪⎨⎪⎪⎪⎪⎧​​i2=j2=k2=−1ij=k,ji=−kjk=i,kj=−iki=j,ik=−j​

  也可以用一个标量和一个向量来表达四元数:
  q = [ s , v ] , s = q 0 ∈ R v = [ q 1 , q 2 , q 3 ] T ∈ R 3 q = [s, v], \quad s=q_0\in\mathbb{R} \quad v=[q_1, q_2, q_3]^T \in \mathbb{R}^3 q=[s,v],s=q0​∈Rv=[q1​,q2​,q3​]T∈R3

  s s s为四元数的实部, v v v为四元数的虚部.有实四元数和虚四元数的概念.

• 四元数与旋转角度的关系:

  • 在二维情况下,任意一个旋转都可以用单位复数来描述,乘 i i i就是绕 i i i轴旋转90°.
  • 在三维情况下,任意一个旋转都可以用单位四元数来描述,乘 i i i就是绕 i i i轴旋转180°.

• 单位四元数和旋转向量之间的转换:

  设单位四元数 q q q表示一个绕单位向量 n = [ n x , n y , n z ] T n =[n_x,n_y,n_z]^T n=[nx​,ny​,nz​]T,角度为 θ θ θ的旋转.

  • 从旋转向量到单位四元数:

  q = [ cos ⁡ ( θ 2 ) , n sin ⁡ ( θ 2 ) ] T = [ cos ⁡ ( θ 2 ) , n x sin ⁡ ( θ 2 ) , n y sin ⁡ ( θ 2 ) , n z sin ⁡ ( θ 2 ) ] T q = \left[ \cos(\frac{\theta}{2}), n\sin(\frac{\theta}{2}) \right]^T= \left[ \cos(\frac{\theta}{2}), n_x\sin(\frac{\theta}{2}), n_y\sin(\frac{\theta}{2}), n_z\sin(\frac{\theta}{2}) \right]^T q=[cos(2θ​),nsin(2θ​)]T=[cos(2θ​),nx​sin(2θ​),ny​sin(2θ​),nz​sin(2θ​)]T

  • 从单位四元数到旋转向量:
    { θ = 2 arccos ⁡ q 0 [ n x , n y , n z ] = [ q 1 , q 2 , q 3 ] T / sin ⁡ θ 2 \left\{

    θ=2arccosq0[nx,ny,nz]=[q1,q2,q3]T/sinθ2

1. • \right. ⎩⎨⎧​​θ=2arccosq0​[nx​,ny​,nz​]=[q1​,q2​,q3​]T/sin2θ​​

给定一个空间三维点 p = [ x , y , z ] ∈ R 3 p=[x,y,z ]\in \R^3 p=[x,y,z]∈R3,以及一个由轴角 n n n, θ θ θ指定的旋转,三维点 p p p经过旋转后变为 p ′ p′ p′.如何使用单位四元数 q q q表达旋转?

1. 把三维空间点用一个虚四元数 p p p表示:
   p = [ 0 , x , y , z ] = [ 0 , v ] p = [0, x, y, z] = [0, v] p=[0,x,y,z]=[0,v]

2. 把旋转用单位四元数 q q q表示:
   q = [ cos ⁡ θ 2 , n sin ⁡ θ 2 ] q = [\cos{\frac{\theta}{2}}, n\sin{\frac{\theta}{2}} ] q=[cos2θ​,nsin2θ​]

3. 旋转后的点 p ′ p' p′可表示为:
   p ′ = q p q − 1 p' = qpq^{-1} p′=qpq−1

这样得到的点 p ′ p' p′仍为一个纯虚四元数,其虚部的3个分量表示旋转后3D点的坐标.

只有单位四元数才能表示旋转,因此在程序中创建四元数后,要记得调用normalize()以将其单位化

旋转矩阵构成特殊正交群 S O ( 3 ) SO(3) SO(3),变换矩阵构成了特殊欧氏群 S E ( 3 ) SE(3) SE(3).
S O ( 3 ) = { R ∈ R 3 × 3 ∣ R R T = I , det ⁡ ( R ) = 1 } S E ( 3 ) = { T = [ R t 0 T 1 ] ∈ R 4 × 4 ∣ R ∈ S O ( 3 ) , t ∈ R 3 }

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

SO(3)SE(3)​={R∈R3×3∣RRT=I,det(R)=1}={T=[R0T​t1​]∈R4×4∣R∈SO(3),t∈R3}​

• 群(Group)是一种集合加上一种运算的代数结构.把集合记作 A A A,运算记作 ⋅ \cdot ⋅ ,那么群可以记作 G = ( A , ⋅ ) G =(A,\cdot) G=(A,⋅).群要求这个运算满足如下条件(封结幺逆):

  1. 封闭性: ∀ a 1 , a 2 ∈ A , a 1 ⋅ a 2 ∈ A \forall{a_1, a_2} \in A, \quad 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, \quad (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 \exists{a_0} \in A, \quad \mathrm{s.t.} \quad \forall a \in A, \quad 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, \quad \exists{a^{-1}} \in A, \quad \mathrm{s.t.} a\cdot a^{-1}=a_0 ∀a∈A,∃a−1∈A,s.t.a⋅a−1=a0​

• 李群是指具有连续(光滑)性质的群. S O ( 3 ) SO(3) SO(3)和 S E ( 3 ) SE(3) SE(3)都是李群

每个李群都有与之对应的李代数,李代数描述了李群的局部性质.

通用的李代数的定义如下:
李代数由一个集合 V V V,一个数域 F F F和一个二元运算 [ , ] [, ] [,]组成.如果它们满足以下几条性质,则称 ( V , F , [ , ] ) (V, F, [, ]) (V,F,[,])为一个李代数,记作 g \mathfrak{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. 双线性: $\forall X,Y,Z \in V, a,b \in F $有:
   [ a X + b Y , Z ] = a [ X , Z ] + b [ Y , Z ] , [ Z , a X + b Y ] = a [ Z , X ] + b [ Z , Y ] [aX+bY,Z]=a[X,Z]+b[Y,Z], \quad [Z, aX+bY]=a[Z,X]+b[Z,Y] [aX+bY,Z]=a[X,Z]+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 , [ X , Y ] ] + [ Y , [ Z , X ] ] = 0 \forall X,Y,Z \in V, \quad [X, [Y,Z ]]+[Z, [X,Y ]]+[Y, [Z,X ]]=0 ∀X,Y,Z∈V,[X,[Y,Z]]+[Z,[X,Y]]+[Y,[Z,X]]=0.

其中的二元运算 [ , ] [,] [,]被称为李括号.例如三维向量空间 R 3 \mathbb{R^3} R3上定义的叉积 × \times ×是一种李括号.

• 李群 S O ( 3 ) SO(3) SO(3)对应的李代数 s o ( 3 ) \mathfrak{so}(3) so(3)是定义在 R 3 \mathbb{R^3} R3上的向量,记作 ϕ \phi ϕ.

  s o ( 3 ) = { ϕ ∈ R 3 , Φ = ϕ ∧ = [ 0 − ϕ 3 ϕ 2 ϕ 3 0 − ϕ 1 − ϕ 2 ϕ 1 0 ] ∈ R 3 × 3 } \mathfrak{so}(3) = \left\{ \phi \in \mathbb{R^3}, \Phi=\phi^\wedge = \left[

  0ϕ3−ϕ2−ϕ30ϕ1ϕ2−ϕ10

• \right] \in \mathbb{R^{3 \times 3}} \right\} so(3)=⎩⎨⎧​ϕ∈R3,Φ=ϕ∧=⎣⎡​0ϕ3​−ϕ2​​−ϕ3​0ϕ1​​ϕ2​−ϕ1​0​⎦⎤​∈R3×3⎭⎬⎫​

• 李代数 s o ( 3 ) \mathfrak{so}(3) so(3)的李括号为
  [ ϕ 1 , ϕ 2 ] = ( Φ 1 Φ 2 − Φ 2 Φ 1 ) ∨ [\phi_1, \phi_2] = (\Phi_1 \Phi_2 - \Phi_2 \Phi_1) ^\vee [ϕ1​,ϕ2​]=(Φ1​Φ2​−Φ2​Φ1​)∨
  其中 ∨ ^\vee ∨是 ∧ ^\wedge ∧的逆运算,表示将反对称矩阵还原为向量

• s o ( 3 ) \mathfrak{so}(3) so(3)和 S O ( 3 ) SO(3) SO(3)间的映射关系为

  李 群 R = exp ⁡ ( ϕ ∧ ) = exp ⁡ ( Φ ) 李 代 数 ϕ = ln ⁡ ( R ) ∨

  李群R李代数ϕ=exp(ϕ∧)=exp(Φ)=ln(R)∨

• 李群R李代数ϕ​=exp(ϕ∧)=exp(Φ)=ln(R)∨​

• 类似地,李群 S E ( 3 ) SE(3) SE(3)的李代数 s e ( 3 ) \mathfrak{se}(3) se(3)是定义在 R 6 \mathbb{R^6} R6上上的向量.记作 ξ \xi ξ:

  s e ( 3 ) = { ξ = [ ρ ϕ ] ∈ R 6 , ρ ∈ R 3 , ϕ ∈ s o ( 3 ) , ξ ∧ = [ ϕ ∧ ρ 0 T 0 ] ∈ R 4 × 4 } \mathfrak{se}(3) = \left\{ \xi = \left[

  ρϕ

\right] \in \mathbb{R^6}, \rho \in \mathbb{R^3}, \phi \in \mathfrak{so}(3), \xi^\wedge = \left[

ϕ∧0Tρ0

• \right] \in \mathbb{R^{4\times 4}} \right\} se(3)={ξ=[ρϕ​]∈R6,ρ∈R3,ϕ∈so(3),ξ∧=[ϕ∧0T​ρ0​]∈R4×4}
  s e ( 3 ) \mathfrak{se}(3) se(3)中的每个元素 ξ \xi ξ,是一个六维向量.前三维 ρ \rho ρ表示平移;后三维 ϕ \phi ϕ表示旋转,本质上是 s o ( 3 ) \mathfrak{so}(3) so(3)元素.

• 在这里同样使用 ∧ ^\wedge ∧符号将六维向量扩展成为四维矩阵,但不再表示反对称

  ξ ∧ = [ ϕ ∧ ρ 0 T 0 ] ∈ R 4 × 4 \xi^\wedge = \left[

  ϕ∧0Tρ0

• \right] \in \mathbb{R^{4 \times 4}} ξ∧=[ϕ∧0T​ρ0​]∈R4×4

• 李代数 s e ( 3 ) \mathfrak{se}(3) se(3)的李括号和 s o ( 3 ) \mathfrak{so}(3) so(3)类似:
  [ ξ 1 , ξ 2 ] = ( ξ 1 ∧ ξ 2 ∧ − ξ 2 ∧ ξ 1 ∧ ) ∨ [\xi_1, \xi_2] = (\xi^\wedge_1 \xi^\wedge_2 - \xi^\wedge_2 \xi^\wedge_1) ^\vee [ξ1​,ξ2​]=(ξ1∧​ξ2∧​−ξ2∧​ξ1∧​)∨

• s e ( 3 ) \mathfrak{se}(3) se(3)和 S E ( 3 ) SE(3) SE(3)间映射关系为
  李 群 T = exp ⁡ ( ξ ∧ ) 李 代 数 ξ = ln ⁡ ( T ) ∨

  李群T李代数ξ=exp(ξ∧)=ln(T)∨

• 李群T李代数ξ​=exp(ξ∧)=ln(T)∨​

• 将三维向量 ϕ \phi ϕ分解为其模长 θ \theta θ和方向向量 α \alpha α,即 ϕ = θ α \phi=\theta\alpha ϕ=θα.则从 s o ( 3 ) \mathfrak{so}(3) so(3)到 S O ( 3 ) SO(3) SO(3)的指数映射可表示为:

  R = exp ⁡ ( ϕ ) = exp ⁡ ( θ α ∧ ) = cos ⁡ θ I + ( 1 − cos ⁡ θ ) α α T + sin ⁡ θ α ∧ R = \exp(\phi) = \exp(\theta \alpha ^\wedge) = \cos \theta I + (1-\cos\theta) \alpha \alpha^T + \sin \theta \alpha ^\wedge R=exp(ϕ)=exp(θα∧)=cosθI+(1−cosθ)ααT+sinθα∧

  上式即为旋转向量到旋转矩阵的罗德里格斯公式,可见** s o ( 3 ) \mathfrak{so}(3) so(3)本质上是旋转向量组成的空间**.

• 从 S O ( 3 ) SO(3) SO(3)到 s o ( 3 ) \mathfrak{so}(3) so(3)的对数映射可表示为:
  ϕ = ln ⁡ ( R ) ∨ \phi = \ln(R)^\vee ϕ=ln(R)∨

  实际计算时可以通过迹的性质分别求出转角 θ \theta θ和转轴 α \alpha α
  θ = arccos ⁡ t r ( R ) − 1 2 , R α = α \theta = \arccos \frac{tr(R)-1}{2}, \qquad R \alpha = \alpha θ=arccos2tr(R)−1​,Rα=α

• 从 s e ( 3 ) \mathfrak{se}(3) se(3)到 S E ( 3 ) SE(3) SE(3)的指数映射可表示为:

  T = exp ⁡ ( ξ ∧ ) = [ R J ρ 0 T 1 ] T = \exp(\xi ^\wedge) = \left[

  R0TJρ1

• \right] T=exp(ξ∧)=[R0T​Jρ1​]

  其中
  J = sin ⁡ θ θ I + ( 1 − sin ⁡ θ θ ) α α T + 1 − cos ⁡ θ θ α ∧ J = \frac{\sin\theta}{\theta} I + (1-\frac{\sin\theta}{\theta}) \alpha \alpha^T + \frac{1- \cos\theta}{\theta} \alpha^\wedge J=θsinθ​I+(1−θsinθ​)ααT+θ1−cosθ​α∧

  可以看到,平移部分经过指数映射之后,发生了一次以 J J J为系数矩阵的线性变换.

• 从 S E ( 3 ) SE(3) SE(3)到 s e ( 3 ) \mathfrak{se}(3) se(3)的对数映射可表示为:
  ξ = ln ⁡ ( T ) ∨ \xi = \ln(T)^\vee ξ=ln(T)∨

  实际计算时 ϕ \phi ϕ可以由 S O ( 3 ) SO(3) SO(3)到 s o ( 3 ) \mathfrak{so}(3) so(3)的映射得到, ρ \rho ρ可以由 t = J ρ t=J\rho t=Jρ计算得到.

1. BCH公式及其近似形式

   • 很遗憾地,李群乘积和李代数加法并不等价,即:
     R 1 R 2 = exp ⁡ ( ϕ 1 ∧ ) exp ⁡ ( ϕ 1 ∧ ) ≠ exp ⁡ ( ( ϕ 1 + ϕ 2 ) ∧ ) R_1 R_2 = \exp(\phi_1^\wedge) \exp(\phi_1^\wedge) \ne \exp((\phi_1 + \phi_2)^\wedge) R1​R2​=exp(ϕ1∧​)exp(ϕ1∧​)​=exp((ϕ1​+ϕ2​)∧)

     李群乘积与李代数运算的对应关系由BCH公式给出:

     ln ⁡ ( exp ⁡ ( A ) exp ⁡ ( B ) ) = A + B + 1 2 [ A , B ] + 1 12 [ A , [ A , B ] ] − 1 12 [ B , [ A , B ] ] + . . . \ln(\exp(A) \exp(B)) = A+B +\frac{1}{2} [A,B] +\frac{1}{12} [A, [A,B]] -\frac{1}{12} [B, [A,B]] + ... ln(exp(A)exp(B))=A+B+21​[A,B]+121​[A,[A,B]]−121​[B,[A,B]]+...

     上式中 [ , ] [,] [,]表示李括号运算.

   • 当 ϕ 1 \phi_1 ϕ1​或 ϕ 2 \phi_2 ϕ2​为小量时,可以对BCH公式进行线性近似,得到李群乘积对应的李代数的表达式:
     R 1 ⋅ R 2 对 应 的 李 代 数 = ln ⁡ ( exp ⁡ ( ϕ 1 ∧ ) exp ⁡ ( ϕ 1 ∧ ) ) ∨ ≈ { J l ( ϕ 2 ) − 1 ϕ 1 + ϕ 2 当 ϕ 1 为小量时 J r ( ϕ 1 ) − 1 ϕ 2 + ϕ 1 当 ϕ 2 为小量时 R_1 \cdot R_2 对应的李代数 = \ln (\exp(\phi_1^\wedge) \exp(\phi_1^\wedge))^\vee \approx \left\{

     Jl(ϕ2)−1ϕ1+ϕ2当ϕ1为小量时Jr(ϕ1)−1ϕ2+ϕ1当ϕ2为小量时

1. • \right. R1​⋅R2​对应的李代数=ln(exp(ϕ1∧​)exp(ϕ1∧​))∨≈{Jl​(ϕ2​)−1ϕ1​+ϕ2​当ϕ1​为小量时Jr​(ϕ1​)−1ϕ2​+ϕ1​当ϕ2​为小量时​

     其中左乘雅可比矩阵 J l J_l Jl​即为从 S E ( 3 ) SE(3) SE(3)到 s e ( 3 ) \mathfrak{se}(3) se(3)对数映射中的雅可比矩阵
     J l = sin ⁡ θ θ I + ( 1 − sin ⁡ θ θ ) α α T + 1 − cos ⁡ θ θ α ∧ J_l = \frac{\sin\theta}{\theta} I + (1-\frac{\sin\theta}{\theta}) \alpha \alpha^T + \frac{1- \cos\theta}{\theta} \alpha^\wedge Jl​=θsinθ​I+(1−θsinθ​)ααT+θ1−cosθ​α∧

     其逆为
     J l − 1 = θ 2 cot ⁡ θ 2 I + ( 1 − θ 2 cot ⁡ θ 2 ) α α T + θ 2 α ∧ J_l^{-1} = \frac{\theta}{2} \cot{\frac{\theta}{2}} I + (1-\frac{\theta}{2} \cot{\frac{\theta}{2}}) \alpha \alpha^T + \frac{\theta}{2} \alpha^\wedge Jl−1​=2θ​cot2θ​I+(1−2θ​cot2θ​)ααT+2θ​α∧

     右乘雅可比矩阵只需对自变量取负号即可
     J r ( ϕ ) = J l ( − ϕ ) J_r(\phi) = J_l(-\phi) Jr​(ϕ)=Jl​(−ϕ)

2. 李群 S O ( 3 ) SO(3) SO(3)乘法与李代数 s o ( 3 ) \mathfrak{so}(3) so(3)加法的关系:

   • 对旋转 R R R(李代数为 ϕ \phi ϕ)左乘一个微小旋转 Δ R \Delta R ΔR(李代数为 Δ ϕ \Delta \phi Δϕ),得到的旋转李群 Δ R ⋅ R \Delta R\cdot R ΔR⋅R对应的李代数为:
     Δ R ⋅ R 对 应 的 李 代 数 = ln ⁡ ( exp ⁡ ( Δ ϕ ∧ ) exp ⁡ ( ϕ ∧ ) ) = ϕ + J l − 1 ( ϕ ) Δ ϕ \Delta R \cdot R 对应的李代数 = \ln \left( \exp(\Delta \phi^\wedge) \exp(\phi^\wedge) \right) = \phi + J_l^{-1}(\phi)\Delta \phi ΔR⋅R对应的李代数=ln(exp(Δϕ∧)exp(ϕ∧))=ϕ+Jl−1​(ϕ)Δϕ

   • 反之,李代数加法 ( ϕ + Δ ϕ ) (\phi+\Delta \phi) (ϕ+Δϕ)对应的李群元素可表示为:
     ( ϕ + Δ ϕ ) 对 应 的 李 群 = exp ⁡ ( ( ϕ + Δ ϕ ) ∧ ) = exp ⁡ ( ( J l Δ ϕ ) ∧ ) exp ⁡ ( ϕ ∧ ) = exp ⁡ ( ϕ ∧ ) exp ⁡ ( ( J r Δ ϕ ) ∧ ) (\phi+\Delta \phi)对应的李群 = \exp((\phi+\Delta \phi)^\wedge) = \exp((J_l \Delta \phi)^\wedge) \exp(\phi^\wedge)= \exp(\phi^\wedge) \exp((J_r \Delta \phi)^\wedge) (ϕ+Δϕ)对应的李群=exp((ϕ+Δϕ)∧)=exp((Jl​Δϕ)∧)exp(ϕ∧)=exp(ϕ∧)exp((Jr​Δϕ)∧)

3. 同理,李群 S E ( 3 ) SE(3) SE(3)乘法与李代数 s e ( 3 ) \mathfrak{se}(3) se(3)加法的关系:
   exp ⁡ ( Δ ξ ∧ ) exp ⁡ ( ξ ∧ ) ≈ exp ⁡ ( ( J l − 1 Δ ξ + ξ ) ∧ ) exp ⁡ ( ξ ∧ ) exp ⁡ ( Δ ξ ∧ ) ≈ exp ⁡ ( ( J r − 1 Δ ξ + ξ ) ∧ ) \exp(\Delta \xi^\wedge) \exp(\xi^\wedge) \approx \exp\left( (J_l^{-1}\Delta \xi + \xi)^\wedge \right) \\ \exp(\xi^\wedge) \exp(\Delta \xi^\wedge) \approx \exp\left( (J_r^{-1}\Delta \xi + \xi)^\wedge \right) exp(Δξ∧)exp(ξ∧)≈exp((Jl−1​Δξ+ξ)∧)exp(ξ∧)exp(Δξ∧)≈exp((Jr−1​Δξ+ξ)∧)

对空间点 p p p进行旋转,得到 R p Rp Rp,旋转之后点的坐标对旋转的导数可表示为:
∂ ( R p ) ∂ R \frac{\partial(Rp)}{\partial R} ∂R∂(Rp)​

对于上式的求导,有两种方式:

1. 用李代数 ϕ \phi ϕ表示姿态 R R R,然后根据李代数加法对 ϕ \phi ϕ求导.
2. 用李代数 φ \varphi φ表示微小扰动 ∂ R \partial R ∂R,然后根据李群左乘对 φ \varphi φ求导.

其中扰动模型表达式简单,更为实用.

用李代数 ϕ \phi ϕ表示姿态 R R R,求导得到
∂ ( R p ) ∂ R = ∂ ( exp ⁡ ( ϕ ∧ ) p ) ∂ ϕ = − ( R p ) ∧ J l \frac{\partial(Rp)}{\partial R} = \frac{\partial( \exp(\phi^\wedge) p)}{\partial \phi} = -(Rp) ^\wedge J_l ∂R∂(Rp)​=∂ϕ∂(exp(ϕ∧)p)​=−(Rp)∧Jl​

另一种求导方式是对 R R R进行一次左乘扰动 ∂ R \partial R ∂R,设左乘扰动 ∂ R \partial R ∂R对应的李代数为 φ \varphi φ,对 φ \varphi φ求导,得到
∂ ( R p ) ∂ R = exp ⁡ ( ( ϕ + φ ) ∧ ) p − exp ⁡ ( ϕ ∧ ) p φ = − ( R p ) ∧ \frac{\partial(Rp)}{\partial R} = \frac{ \exp((\phi+\varphi)^\wedge)p - \exp(\phi^\wedge)p }{\varphi} =-(Rp) ^\wedge ∂R∂(Rp)​=φexp((ϕ+φ)∧)p−exp(ϕ∧)p​=−(Rp)∧

类似地,空间点 p p p经过变换 T T T得到 T p Tp Tp,给 T T T左乘一个扰动 Δ T = exp ⁡ ( δ ξ ∧ ) \Delta T = \exp (\delta \xi ^\wedge) ΔT=exp(δξ∧),则有
∂ ( R p ) δ ξ = [ I − ( R p + t ) ∧ 0 T 0 T ] = ( T P ) ⊙ \frac{\partial(Rp)}{\delta \xi} = \left[

I0T−(Rp+t)∧0T

\right]= (TP) ^ \odot δξ∂(Rp)​=[I0T​−(Rp+t)∧0T​]=(TP)⊙

O − x − y − z O-x-y-z O−x−y−z为相机坐标系,现实空间点 P P P的相机坐标为 [ X , Y , Z ] T [X,Y,Z]^T [X,Y,Z]T,投影到 O ′ − x ′ − y ′ O'-x'-y' O′−x′−y′平面上的点 P ′ P' P′,坐标为 [ X ′ , Y ′ , Z ′ ] T [X',Y',Z']^T [X′,Y′,Z′]T.

• 将成像平面对称到相机前方,根据几何相似关系 Z f = X X ′ = Y Y ′ \frac{Z}{f} = \frac{X}{X'} = \frac{Y}{Y'} fZ​=X′X​=Y′Y​,整理得到投影点 P ′ P' P′在投影平面上的坐标 P ′ = [ X ′ , Y ′ ] P'=[X',Y'] P′=[X′,Y′]:

  { X ′ = f X Z Y ′ = f Y Z \left\{

  X′=fXZY′=fYZ