本文分别详细推导了 s e ( 3 ) \mathfrak{s e}(3) se(3) 与 s o ( 3 ) \mathfrak{s o}(3) so(3) 的左乘、右乘扰动模型的雅克比矩阵,特别提供了 s e ( 3 ) \mathfrak{s e}(3) se(3)左乘扰动模型优化特征点的重投影误差的例子。
手动码公式,如有谬误请包涵指正。
目录
一、 s e ( 3 ) \mathfrak{s e}(3) se(3) 左乘扰动模型求解点重投影误差优化问题
使用李代数对位姿进行优化的原因:
旋转矩阵自身是带有约束的(正交且行列式为1),它们作为优化变量时,会引入额外的约束,使得优化变得困难。通过李群和李代数之间的转换关系,可以把位姿估计变为无约束的优化问题,简化求解方式。
使用高斯牛顿法或者LM法对重投影误差进行优化,需要求得重投影误差的雅克比矩阵。
因为会用到链式法则,所以先求两个有用的导数。
- 投影函数:
h ( p ) = h ( [ p x p y p z ] ) = [ c x + f x p x p z c y + f y p y p z ] h(\mathbf{p})=h\left(\left[\begin{array}{l}p_x \\ p_y \\ p_z\end{array}\right]\right)=\left[\begin{array}{c}c_x+f_x \frac{p_x}{p_z} \\ c_y+f_y \frac{p_y}{p_z}\end{array}\right] h(p)=h pxpypz =[cx+fxpzpxcy+fypzpy]
投影函数对3D点坐标求导:
∂ h ( p ) ∂ p = [ f x / p z 0 − f x p x / p z 2 0 f y / p z − f y p y / p z 2 ] \frac{\partial h(\mathbf{p})}{\partial \mathbf{p}}=\left[\begin{array}{ccc}f_x / p_z & 0 & -f_x p_x / p_z^2 \\ 0 & f_y / p_z & -f_y p_y / p_z^2\end{array}\right] ∂p∂h(p)=[fx/pz00fy/pz−fxpx/pz2−fypy/pz2]
观测到的投影坐标是2维的,对3维点进行求导,所以结果是 2x3 的矩阵。 - 变换矩阵作用于3D点(使用齐次坐标表示,3D点最后一维是1),使用左乘扰动模型对李代数进行求导:
∂ ( exp ( δ ξ ∧ ) T p ) ∂ δ ξ = lim δ ξ → 0 exp ( δ ξ ∧ ) exp ( ξ ∧ ) p − exp ( ξ ∧ ) p δ ξ ≈ lim δ ξ → 0 ( I + δ ξ ∧ ) exp ( ξ ∧ ) p − exp ( ξ ∧ ) p δ ξ = lim δ ξ → 0 δ ξ ∧ exp ( ξ ∧ ) p δ ξ = lim δ ξ → 0 [ ϕ ∧ ρ 0 T 0 ] [ R p + t 1 ] δ ξ = lim δ ξ → 0 [ ϕ ∧ ( R p + t ) + ρ 0 ] δ ξ = [ I − ( R p + t ) ∧ 0 T 0 T ] ≜ ( T p ) ⊙ . \begin{aligned} \frac{\partial(\exp \left( \delta \boldsymbol{ \xi}^{\wedge}\right)\boldsymbol{T} \boldsymbol{p})}{\partial \delta \boldsymbol{\xi}}&=\lim _{ \delta \boldsymbol{ \xi} \rightarrow \mathbf{0}} \frac{\exp \left( \delta \boldsymbol{\xi}^{\wedge}\right) \exp \left(\boldsymbol{\xi}^{\wedge}\right) \boldsymbol{p}-\exp \left(\boldsymbol{\xi}^{\wedge}\right) \boldsymbol{p}}{ \delta \boldsymbol{\xi}}\\ &\approx \lim _{ \delta \boldsymbol{\xi} \rightarrow 0} \frac{\left(\boldsymbol{I}+\ \delta \boldsymbol{\xi}^{\wedge}\right) \exp \left(\boldsymbol{\xi}^{\wedge}\right) \boldsymbol{p}-\exp \left(\boldsymbol{\xi}^{\wedge}\right) \boldsymbol{p}}{ \delta \boldsymbol{\xi}}\\ &=\lim _{ \delta \boldsymbol{\xi} \rightarrow \mathbf{0}} \frac{ \delta \boldsymbol{\xi}^{\wedge} \exp \left(\boldsymbol{\xi}^{\wedge}\right) \boldsymbol{p}}{ \delta \boldsymbol{\xi}}\\ &=\lim _{ \delta \boldsymbol{\xi} \rightarrow \mathbf{0}} \frac{\left[\begin{array}{cc} \boldsymbol{\phi}^{\wedge} & \boldsymbol{\rho} \\ \mathbf{0}^T & 0 \end{array}\right]\left[\begin{array}{c} \boldsymbol{R} \boldsymbol{p}+\boldsymbol{t} \\ 1 \end{array}\right]}{ \delta \boldsymbol{\xi}}\\ &=\lim _{ \delta \boldsymbol{\xi} \rightarrow \mathbf{0}} \frac{\left[\begin{array}{c} \boldsymbol{\phi}^{\wedge}(\boldsymbol{R} \boldsymbol{p}+\boldsymbol{t})+ \boldsymbol{\rho} \\ 0 \end{array}\right]}{ \delta \boldsymbol{\xi}}=\left[\begin{array}{cc} \boldsymbol{I} & -(\boldsymbol{R} \boldsymbol{p}+\boldsymbol{t})^{\wedge} \\ \mathbf{0}^T & \mathbf{0}^T \end{array}\right] \triangleq(\boldsymbol{T} \boldsymbol{p})^{\odot} . \end{aligned} ∂δξ∂(exp(δξ∧)Tp)=δξ→0limδξexp(δξ∧)exp(ξ∧)p−exp(ξ∧)p≈δξ→0limδξ(I+ δξ∧)exp(ξ∧)p−exp(ξ∧)p=δξ→0limδξδξ∧exp(ξ∧)p=δξ→0limδξ[ϕ∧0Tρ0][Rp+t1]=δξ→0limδξ[ϕ∧(Rp+t)+ρ0]=[I0T−(Rp+t)∧0T]≜(Tp)⊙.
注: lim ξ → 0 exp ( ξ ∧ ) = [ I + ϕ ∧ ρ 0 T 1 ] = I 4 + [ ϕ ∧ ρ 0 T 0 ] = I + ξ ∧ \lim _{ \boldsymbol{\xi} \rightarrow \mathbf{0}}\exp \left(\boldsymbol{\xi}^{\wedge}\right) = \left[\begin{array}{cc} \mathbf{I} + \boldsymbol{\phi}^{\wedge} & \boldsymbol{\rho} \\ 0^T & 1\end{array}\right] = \mathbf{I}_4 + \left[\begin{array}{cc} \boldsymbol{\phi}^{\wedge} & \boldsymbol{\rho} \\ 0^T & 0\end{array}\right] = \boldsymbol{I}+ \boldsymbol{\xi}^{\wedge} limξ→0exp(ξ∧)=[I+ϕ∧0Tρ1]=I4+[ϕ∧0Tρ0]=I+ξ∧,注意这里使用的是前面提到的伪指数映射 exp ( ξ ∧ ) = [ exp ( ϕ ∧ ) ρ 0 T 1 ] \exp \left(\boldsymbol{\xi}^{\wedge}\right)=\left[\begin{array}{cc}\exp \left(\boldsymbol{\phi}^{\wedge}\right) & \boldsymbol{\rho} \\ 0^T & 1\end{array}\right] exp(ξ∧)=[exp(ϕ∧)0Tρ1]
不然根据原来的指数映射求极限后得到的不是这个结果。
- 重投影误差求雅克比矩阵(左乘扰动模型):
∂ h ( exp ( δ ξ ∧ ) T p ) − u ∂ p / ∂ δ ξ = ∂ h ( exp ( δ ξ ∧ ) T p ) ∂ p / ∂ δ ξ \frac{\partial h\left(\exp \left(\delta \boldsymbol{\xi}^{\wedge}\right) \mathbf{T} \mathbf{p}\right) - \mathbf{u}}{\partial \mathbf{p} / \partial \delta \boldsymbol{\xi}} = \frac{\partial h\left(\exp \left(\delta \boldsymbol{\xi}^{\wedge}\right) \mathbf{T} \mathbf{p}\right)}{\partial \mathbf{p} / \partial \delta \boldsymbol{\xi}} ∂p/∂δξ∂h(exp(δξ∧)Tp)−u=∂p/∂δξ∂h(exp(δξ∧)Tp)
这里 u \mathbf{u} u 是3D点 p \mathbf{p} p 的2D观测坐标。
3.1 根据链式法则,对3D点坐标求导:
J p = ∂ h ( exp ( δ ξ ∧ ) T p ) ∂ p = ∂ h ( p ′ ) ∂ p ′ ∣ p ′ = T p = g ∂ exp ( δ ξ ∧ ) T p ∂ p ∣ δ ξ = 0 = ∂ h ( p ′ ) ∂ p ′ ∣ p ′ = T p = g ∂ T p ∂ p = ( f x / g z 0 − f x g x / g z 2 0 f y / g z − f y g y / g z 2 ) R \begin{aligned} \mathbf{J}_\mathbf{p} = \frac{\partial h\left(\exp \left(\delta \boldsymbol{\xi}^{\wedge}\right) \mathbf{T} \mathbf{p}\right)}{\partial \mathbf{p}} &=\left.\frac{\partial h\left(\mathbf{p}^{\prime}\right)}{\partial \mathbf{p}^{\prime}}\right|_{\mathbf{p}^{\prime}=\mathbf{T} \mathbf{p}=\mathbf{g}} \left.\frac{\partial \exp \left(\delta \boldsymbol{\xi}^{\wedge}\right) \mathbf{T} \mathbf{p}}{\partial \mathbf{p}} \right|_{\delta \boldsymbol{\xi} = \mathbf{0}} \\ &=\left.\frac{\partial h\left(\mathbf{p}^{\prime}\right)}{\partial \mathbf{p}^{\prime}}\right|_{\mathbf{p}^{\prime}=\mathbf{T} \mathbf{p}=\mathbf{g}} \frac{\partial \mathbf{T} \mathbf{p}}{\partial \mathbf{p}} \\ &=\left(\begin{array}{ccc} f_x / g_z & 0 & -f_x g_x / g_z^2 \\ 0 & f_y / g_z & -f_y g_y / g_z^2 \end{array}\right) \mathbf{R} \end{aligned} Jp=∂p∂h(exp(δξ∧)Tp)=∂p′∂h(p′) p′=Tp=g∂p∂exp(δξ∧)Tp δξ=0=∂p′∂h(p′) p′=Tp=g∂p∂Tp=(fx/gz00fy/gz−fxgx/gz2−fygy/gz2)R
3.2 对左乘扰动量求导:
J δ ξ = ∂ h ( exp ( δ ξ ∧ ) T p ) ∂ δ ξ = ∂ h ( p ′ ) ∂ p ′ ∣ p ′ = T p = g ∂ exp ( δ ξ ∧ ) T p ∂ δ ξ = ( f x / g z 0 − f x g x / g z 2 0 f y / g z − f y g y / g z 2 ) ( I 3 − [ g ] ∧ ) = ( f x g z 0 − f x g x g z 2 − f x g x g y g z 2 f x ( 1 + g x 2 g z 2 ) − f x g y g z 0 f y g z − f y g y g z 2 − f y ( 1 + g y 2 g z 2 ) f y g x g y g z 2 f y g x g z ) \begin{aligned} \mathbf{J}_{\delta \boldsymbol{\xi}} = \frac{\partial h\left(\exp \left(\delta \boldsymbol{\xi}^{\wedge}\right) \mathbf{T} \mathbf{p}\right)}{\partial \delta \boldsymbol{\xi}} &=\left.\frac{\partial h\left(\mathbf{p}^{\prime}\right)}{\partial \mathbf{p}^{\prime}}\right|_{\mathbf{p}^{\prime}=\mathbf{T} \mathbf{p}=\mathbf{g}} \frac{\partial \exp \left(\delta \boldsymbol{\xi}^{\wedge}\right) \mathbf{T} \mathbf{p}}{\partial \delta \boldsymbol{\xi}}\\ &=\left(\begin{array}{ccc} f_x / g_z & 0 & -f_x g_x / g_z^2 \\ 0 & f_y / g_z & -f_y g_y / g_z^2 \end{array}\right)\left(\begin{array}{ll} \mathbf{I}_3 & -[\mathbf{g}]^{\wedge} \end{array}\right)\\ &=\left(\begin{array}{cccccc} \frac{f_x}{g_z} & 0 & -f_x \frac{g_x}{g_z^2} & -f_x \frac{g_x g_y}{g_z^2} & f_x\left(1+\frac{g_x^2}{g_z^2}\right) & -f_x \frac{g_y}{g_z} \\ 0 & \frac{f_y}{g_z} & -f_y \frac{g_y}{g_z^2} & -f_y\left(1+\frac{g_y^2}{g_z^2}\right) & f_y \frac{g_x g_y}{g_z^2} & f_y \frac{g_x}{g_z} \end{array}\right) \end{aligned} Jδξ=∂δξ∂h(exp(δξ∧)Tp)=∂p′∂h(p′) p′=Tp=g∂δξ∂exp(δξ∧)Tp=(fx/gz00fy/gz−fxgx/gz2−fygy/gz2)(I3−[g]∧)= gzfx00gzfy−fxgz2gx−fygz2gy−fxgz2gxgy−fy(1+gz2gy2)fx(1+gz2gx2)fygz2gxgy−fxgzgyfygzgx
这里 ∂ exp ( δ ξ ∧ ) T p ∂ δ ξ \frac{\partial \exp \left(\delta \boldsymbol{\xi}^{\wedge}\right) \mathbf{T} \mathbf{p}}{\partial \delta \boldsymbol{\xi}} ∂δξ∂exp(δξ∧)Tp 实际上应该是一个 4x6 的矩阵,因为变换得到的齐次坐标是4维的,但是投影函数只用到了前3维,所以前面求得的该式的结果相应的取前3行就好。 - 求得雅克比矩阵后就可以使用高斯牛顿法、LM法对3D点坐标和位姿进行迭代优化。求得 s 0 ( 3 ) \mathfrak{s 0}(3) s0(3) 左扰动增量后,使用左乘更新:
T n + 1 = exp ( δ ξ ∧ ) T n \mathbf{T}_{n+1} = \exp \left(\delta \boldsymbol{\xi}^{\wedge}\right)\mathbf{T}_{n} Tn+1=exp(δξ∧)Tn
除了利用左乘扰动模型优化位姿,还可以使用右乘扰动模型,又或者是对旋转(使用 s 0 ( 3 ) \mathfrak{s 0}(3) s0(3) 扰动模型)和平移量分别进行优化更新。
二、 s e ( 3 ) \mathfrak{s e}(3) se(3) 右乘扰动模型雅克比
∂ ( T exp ( δ ξ ∧ ) p ) ∂ δ ξ = lim δ ξ → 0 exp ( ξ ∧ ) exp ( δ ξ ∧ ) p − exp ( ξ ∧ ) p δ ξ ≈ lim δ ξ → 0 exp ( ξ ∧ ) ( I + δ ξ ∧ ) p − exp ( ξ ∧ ) p δ ξ = lim δ ξ → 0 exp ( ξ ∧ ) δ ξ ∧ p δ ξ = lim δ ξ → 0 [ R t 0 T 1 ] [ ϕ ∧ ρ 0 T 0 ] p δ ξ = lim δ ξ → 0 [ R ( ϕ ∧ p + ρ ) + t 0 ] δ ξ = [ R − R p ∧ 0 T 0 T ] \begin{aligned} \frac{\partial(\boldsymbol{T} \exp \left( \delta \boldsymbol{\xi}^{\wedge}\right) \boldsymbol{p})}{\partial \delta \boldsymbol{\xi}}&=\lim _{ \delta \boldsymbol{\xi} \rightarrow \mathbf{0}} \frac{ \exp \left(\boldsymbol{\xi}^{\wedge}\right) \exp \left( \delta \boldsymbol{\xi}^{\wedge}\right) \boldsymbol{p}-\exp \left(\boldsymbol{\xi}^{\wedge}\right) \boldsymbol{p}}{ \delta \boldsymbol{\xi}}\\ &\approx \lim _{ \delta \boldsymbol{\xi} \rightarrow 0} \frac{ \exp \left(\boldsymbol{\xi}^{\wedge}\right) \left(\boldsymbol{I}+\ \delta \boldsymbol{\xi}^{\wedge}\right) \boldsymbol{p}-\exp \left(\boldsymbol{\xi}^{\wedge}\right) \boldsymbol{p}}{ \delta \boldsymbol{\xi}}\\ &=\lim _{ \delta \boldsymbol{\xi} \rightarrow \mathbf{0}} \frac{ \exp \left(\boldsymbol{\xi}^{\wedge}\right) \delta \boldsymbol{\xi}^{\wedge} \boldsymbol{p}}{ \delta \boldsymbol{\xi}}\\ &=\lim _{ \delta \boldsymbol{\xi} \rightarrow \mathbf{0}} \frac{\left[\begin{array}{cc} \boldsymbol{R} & \boldsymbol{t} \\ \mathbf{0}^T & 1 \end{array}\right]\left[\begin{array}{cc} \boldsymbol{\phi}^{\wedge} & \boldsymbol{\rho} \\ \mathbf{0}^T & 0 \end{array}\right]\boldsymbol{p}}{ \delta \boldsymbol{\xi}}\\ &=\lim _{ \delta \boldsymbol{\xi} \rightarrow \mathbf{0}} \frac{\left[\begin{array}{c} \boldsymbol{R}(\boldsymbol{\phi}^{\wedge} \boldsymbol{p}+\boldsymbol{\rho})+ \boldsymbol{t} \\ 0 \end{array}\right]}{ \delta \boldsymbol{\xi}}=\left[\begin{array}{cc} \boldsymbol{R} & -\boldsymbol{R} \boldsymbol{p}^{\wedge} \\ \mathbf{0}^T & \mathbf{0}^T \end{array}\right] \end{aligned} ∂δξ∂(Texp(δξ∧)p)=δξ→0limδξexp(ξ∧)exp(δξ∧)p−exp(ξ∧)p≈δξ→0limδξexp(ξ∧)(I+ δξ∧)p−exp(ξ∧)p=δξ→0limδξexp(ξ∧)δξ∧p=δξ→0limδξ[R0Tt1][ϕ∧0Tρ0]p=δξ→0limδξ[R(ϕ∧p+ρ)+t0]=[R0T−Rp∧0T]
三、3 维平移量与 s o ( 3 ) \mathfrak{s o}(3) so(3) 左乘扰动模型雅克比
∂ ( exp ( ϕ ∧ ) R p + t ) ∂ t = I \begin{aligned} \frac{\partial(\exp(\boldsymbol{\phi}^{\wedge}) \boldsymbol{R} \boldsymbol{p} + \boldsymbol{t} )}{\partial \boldsymbol{t}}&=\boldsymbol{I} \end{aligned} ∂t∂(exp(ϕ∧)Rp+t)=I
∂ ( exp ( ϕ ∧ ) R p + t ) ∂ ϕ = lim ϕ → 0 ( exp ( ϕ ∧ ) R p + t ) − ( R p + t ) ϕ ≈ lim ϕ → 0 ( ( I + ϕ ∧ ) R p + t ) − ( R p + t ) ϕ = lim ϕ → 0 ϕ ∧ R p ϕ = − ( R p ) ∧ \begin{aligned} \frac{\partial(\exp(\boldsymbol{\phi}^{\wedge}) \boldsymbol{R} \boldsymbol{p} + \boldsymbol{t} )}{\partial \boldsymbol{\phi}} &=\lim _{ \boldsymbol{\phi} \rightarrow \mathbf{0}} \frac{(\exp(\boldsymbol{\phi}^{\wedge})\boldsymbol{R} \boldsymbol{p} + \boldsymbol{t} ) - (\boldsymbol{R} \boldsymbol{p} + \boldsymbol{t} ) }{ \boldsymbol{\phi}}\\ &\approx \lim _{ \boldsymbol{\phi} \rightarrow \mathbf{0}} \frac{((\boldsymbol{I} + \boldsymbol{\phi}^{\wedge} )\boldsymbol{R} \boldsymbol{p} + \boldsymbol{t} ) - (\boldsymbol{R} \boldsymbol{p} + \boldsymbol{t} ) }{ \boldsymbol{\phi}}\\ &=\lim _{ \boldsymbol{\phi} \rightarrow \mathbf{0}} \frac{\boldsymbol{\phi}^{\wedge} \boldsymbol{R} \boldsymbol{p} }{ \boldsymbol{\phi}}\\ &=-(\boldsymbol{R} \boldsymbol{p})^{\wedge} \end{aligned} ∂ϕ∂(exp(ϕ∧)Rp+t)=ϕ→0limϕ(exp(ϕ∧)Rp+t)−(Rp+t)≈ϕ→0limϕ((I+ϕ∧)Rp+t)−(Rp+t)=ϕ→0limϕϕ∧Rp=−(Rp)∧
四、 s o ( 3 ) \mathfrak{s o}(3) so(3) 右乘扰动模型雅克比
∂ ( R exp ( ϕ ∧ ) p + t ) ∂ ϕ = lim ϕ → 0 ( R exp ( ϕ ∧ ) p + t ) − ( R p + t ) ϕ ≈ lim ϕ → 0 ( R ( I + ϕ ∧ ) p + t ) − ( R p + t ) ϕ = lim ϕ → 0 R ϕ ∧ p ϕ = − R p ∧ \begin{aligned} \frac{\partial(\boldsymbol{R} \exp(\boldsymbol{\phi}^{\wedge}) \boldsymbol{p} + \boldsymbol{t} )}{\partial \boldsymbol{\phi}} &=\lim _{ \boldsymbol{\phi} \rightarrow \mathbf{0}} \frac{(\boldsymbol{R} \exp(\boldsymbol{\phi}^{\wedge}) \boldsymbol{p} + \boldsymbol{t} ) - (\boldsymbol{R} \boldsymbol{p} + \boldsymbol{t} ) }{ \boldsymbol{\phi}}\\ &\approx \lim _{ \boldsymbol{\phi} \rightarrow \mathbf{0}} \frac{(\boldsymbol{R} (\boldsymbol{I} + \boldsymbol{\phi}^{\wedge} ) \boldsymbol{p} + \boldsymbol{t} ) - (\boldsymbol{R} \boldsymbol{p} + \boldsymbol{t} ) }{ \boldsymbol{\phi}}\\ &=\lim _{ \boldsymbol{\phi} \rightarrow \mathbf{0}} \frac{\boldsymbol{R}\boldsymbol{\phi}^{\wedge} \boldsymbol{p} }{ \boldsymbol{\phi}}\\ &=-\boldsymbol{R} \boldsymbol{p}^{\wedge} \end{aligned} ∂ϕ∂(Rexp(ϕ∧)p+t)=ϕ→0limϕ(Rexp(ϕ∧)p+t)−(Rp+t)≈ϕ→0limϕ(R(I+ϕ∧)p+t)−(Rp+t)=ϕ→0limϕRϕ∧p=−Rp∧
附:李群与李代数的定义
-
群是一种集合加上一种运算的代数结构,群要求这个运算满足:封闭性,结合律,幺元,逆(封结幺逆):
- 封闭性: ∀ a 1 , a 2 ∈ A , a 1 ⋅ a 2 ∈ A \quad \forall a_1, a_2 \in A, \quad a_1 \cdot a_2 \in A ∀a1,a2∈A,a1⋅a2∈A.
- 结合律: ∀ a 1 , a 2 , a 3 ∈ A , ( a 1 ⋅ a 2 ) ⋅ a 3 = a 1 ⋅ ( a 2 ⋅ a 3 ) \quad \forall a_1, a_2, a_3 \in A, \quad\left(a_1 \cdot a_2\right) \cdot a_3=a_1 \cdot\left(a_2 \cdot a_3\right) ∀a1,a2,a3∈A,(a1⋅a2)⋅a3=a1⋅(a2⋅a3).
- 幺元: ∃ a 0 ∈ A , \quad \exists a_0 \in A, \quad ∃a0∈A, s.t. ∀ a ∈ A , a 0 ⋅ a = a ⋅ a 0 = a \quad \forall a \in A, \quad a_0 \cdot a=a \cdot a_0=a ∀a∈A,a0⋅a=a⋅a0=a.
- 逆: ∀ a ∈ A , ∃ a − 1 ∈ A , \quad \forall a \in A, \quad \exists a^{-1} \in A, \quad ∀a∈A,∃a−1∈A, s.t. a ⋅ a − 1 = a 0 \quad a \cdot a^{-1}=a_0 a⋅a−1=a0.
李群是指具有连续(光滑)性质的群。
特殊正交群SO(3):
S O ( 3 ) = { R ∈ R 3 × 3 ∣ R R T = I , d e t ( R ) = 1 } SO(3) = \{\boldsymbol{R} \in \mathbb{R}^{3 \times 3}| \boldsymbol{RR}^T = \boldsymbol{I}, det(\boldsymbol{R}) = 1\} SO(3)={ R∈R3×3∣RRT=I,det(R)=1}
特殊欧式群SE(3):
S E ( 3 ) = { T = [ R t 0 T 1 ] ∈ R 4 × 4 ∣ R ∈ S O ( 3 ) , t ∈ R 3 } S E(3)=\left\{\boldsymbol{T}=\left[\begin{array}{cc} \boldsymbol{R} & \boldsymbol{t} \\ \mathbf{0}^T & 1 \end{array}\right] \in \mathbb{R}^{4 \times 4} \mid \boldsymbol{R} \in S O(3), \boldsymbol{t} \in \mathbb{R}^3\right\} SE(3)={ T=[R0Tt1]∈R4×4∣R∈SO(3),t∈R3} -
李代数由一个集合V,一个数域F和一个二元运算符(李括号)[,]组成,满足以下性质:封闭性,双线性,自反性,雅可比等价。李括号表达了两个元素的差异。
- 封闭性: ∀ X , Y ∈ V , [ X , Y ] ∈ V \quad \forall \boldsymbol{X}, \boldsymbol{Y} \in \mathbb{V},[\boldsymbol{X}, \boldsymbol{Y}] \in \mathbb{V} ∀X,Y∈V,[X,Y]∈V.
- 双线性: ∀ X , Y , Z ∈ V , a , b ∈ F \quad \forall \boldsymbol{X}, \boldsymbol{Y}, \boldsymbol{Z} \in \mathbb{V}, a, b \in \mathbb{F} ∀X,Y,Z∈V,a,b∈F, 有:
[ a X + b Y , Z ] = a [ X , Z ] + b [ Y , Z ] , [ Z , a X + b Y ] = a [ Z , X ] + b [ Z , Y ] [a \boldsymbol{X}+b \boldsymbol{Y}, \boldsymbol{Z}]=a[\boldsymbol{X}, \boldsymbol{Z}]+b[\boldsymbol{Y}, \boldsymbol{Z}], \quad[\boldsymbol{Z}, a \boldsymbol{X}+b \boldsymbol{Y}]=a[\boldsymbol{Z}, \boldsymbol{X}]+b[\boldsymbol{Z}, \boldsymbol{Y}] [aX+bY,Z]=a[X,Z]+b[Y,Z],[Z,aX+bY]=a[Z,X]+b[Z,Y] - 自反性: ∀ X ∈ V , [ X , X ] = 0 \quad \forall \boldsymbol{X} \in \mathbb{V},[\boldsymbol{X}, \boldsymbol{X}]=\mathbf{0} ∀X∈V,[X,X]=0.
- 雅可比等价: ∀ X , Y , Z ∈ V , [ X , [ Y , Z ] ] + [ Z , [ Y , X ] ] + [ Y , [ Z , X ] ] = 0 \quad \forall \boldsymbol{X}, \boldsymbol{Y}, \boldsymbol{Z} \in \mathbb{V},[\boldsymbol{X},[\boldsymbol{Y}, \boldsymbol{Z}]]+[\boldsymbol{Z},[\boldsymbol{Y}, \boldsymbol{X}]]+[\boldsymbol{Y},[\boldsymbol{Z}, \boldsymbol{X}]]=\mathbf{0} ∀X,Y,Z∈V,[X,[Y,Z]]+[Z,[Y,X]]+[Y,[Z,X]]=0.
SO(3)对应的李代数:
s o ( 3 ) = { ϕ ∈ R 3 , Φ = ϕ ∧ ∈ R 3 × 3 } \mathfrak{s o}(3)=\left\{\boldsymbol{\phi} \in \mathbb{R}^3, \boldsymbol{\Phi}=\boldsymbol{\phi}^{\wedge} \in \mathbb{R}^{3 \times 3}\right\} so(3)={ ϕ∈R3,Φ=ϕ∧∈R3×3}
SE(3)对应的李代数:
s e ( 3 ) = { ξ = [ ρ ϕ ] ∈ R 6 , ρ ∈ R 3 , ϕ ∈ s o ( 3 ) , ξ ∧ = [ ϕ ∧ ρ 0 T 0 ] ∈ R 4 × 4 } \mathfrak{s e}(3)=\left\{\boldsymbol{\xi}=\left[\begin{array}{c} \boldsymbol{\rho} \\ \boldsymbol{\phi} \end{array}\right] \in \mathbb{R}^6, \boldsymbol{\rho} \in \mathbb{R}^3, \boldsymbol{\phi} \in \mathfrak{s o}(3), \boldsymbol{\xi}^{\wedge}=\left[\begin{array}{cc}\boldsymbol{\phi}^{\wedge} & \boldsymbol{\rho} \\ \mathbf{0}^T & 0\end{array}\right] \in \mathbb{R}^{4 \times 4}\right\} se(3)={ ξ=[ρϕ]∈R6,ρ∈R3,ϕ∈so(3),ξ∧=[ϕ∧0Tρ0]∈R4×4}
其中 hat(wedge) operator ( ⋅ ) ∧ (\cdot)^{\wedge} (⋅)∧:
ω = [ x y z ] , ω ∧ = ( 0 − z y z 0 − x − y x 0 ) ( ω ∧ ) ∨ = ( 0 − z y z 0 − x − y x 0 ) ∨ = [ x y z ] = ω \boldsymbol{\omega}=\left[\begin{array}{l}x \\ y \\ z\end{array}\right] ,\quad \boldsymbol{\omega}^{\wedge}=\left(\begin{array}{ccc}0 & -z & y \\ z & 0 & -x \\ -y & x & 0\end{array}\right) \quad \\ (\boldsymbol{\omega}^{\wedge})^{\vee} = \left(\begin{array}{ccc}0 & -z & y \\ z & 0 & -x \\ -y & x & 0\end{array}\right)^{\vee}=\left[\begin{array}{l}x \\ y \\ z\end{array}\right] = \boldsymbol{\omega} ω= xyz ,ω∧= 0z−y−z0xy−x0 (ω∧)∨= 0z−y−z0xy−x0 ∨= xyz =ω
附:李群与李代数的互相转换
-
so(3) -> SO(3),指数映射,即罗德里格斯公式:
exp ( ϕ ) = exp ( θ a ∧ ) = cos θ I + ( 1 − cos θ ) a a T + sin θ a ∧ exp ( ϕ ) = exp ( θ a ∧ ) = I + sin θ θ ϕ ∧ + 1 − cos θ θ 2 ( ϕ ∧ ) 2 \exp \left(\boldsymbol{\phi}\right) = \exp \left(\theta \boldsymbol{a}^{\wedge}\right)=\cos \theta \boldsymbol{I}+(1-\cos \theta) \boldsymbol{a} \boldsymbol{a}^{T}+\sin \theta \boldsymbol{a}^{\wedge}\\ \exp \left(\boldsymbol{\phi}\right) = \exp \left(\theta \boldsymbol{a}^{\wedge}\right)=\boldsymbol{I}+\frac{\sin \theta}{\theta}\boldsymbol{\phi}^{\wedge}+\frac{1-\cos \theta}{\theta^2} (\boldsymbol{\phi}^{\wedge})^2 exp(ϕ)=exp(θa∧)=cosθI+(1−cosθ)aaT+sinθa∧exp(ϕ)=exp(θa∧)=I+θsinθϕ∧+θ21−cosθ(ϕ∧)2 -
SO(3) -> so(3),对数映射:
cos θ = t r ( R ) − 1 2 , R a = a \cos \theta = \frac{tr(\mathbf{R}) - 1}{2}, \mathbf{Ra} = \mathbf{a} cosθ=2tr(R)−1,Ra=a -
se(3) -> SE(3):
exp ( ξ ∧ ) = [ exp ( ϕ ∧ ) t 0 T 1 ] , t = J ρ J = sin θ θ I + ( 1 − sin θ θ ) a a T + 1 − cos θ θ a ∧ J = I + 1 − cos θ θ 2 ϕ ∧ + θ − sin θ θ 3 ( ϕ ∧ ) 2 \exp \left(\boldsymbol{\xi}^{\wedge}\right)=\left[\begin{array}{cc}\exp \left(\boldsymbol{\phi}^{\wedge}\right) & \mathbf{t} \\ 0^T & 1\end{array}\right],\quad \mathbf{t} = \mathbf{J} \boldsymbol{\rho} \\ \boldsymbol{J}=\frac{\sin \theta}{\theta} \mathbf{I}+\left(1-\frac{\sin \theta}{\theta}\right) \boldsymbol{a} \boldsymbol{a}^T+\frac{1-\cos \theta}{\theta} \boldsymbol{a}^{\wedge} \\ \boldsymbol{J}=\mathbf{I}+\frac{1-\cos \theta}{\theta^2} \boldsymbol{\phi}^{\wedge}+\frac{\theta-\sin \theta}{\theta^3}\left(\boldsymbol{\phi}^{\wedge}\right)^2 exp(ξ∧)=[exp(ϕ∧)0Tt1],t=JρJ=θsinθI+(1−θsinθ)aaT+θ1−cosθa∧J=I+θ21−cosθϕ∧+θ3θ−sinθ(ϕ∧)2
其中
ξ ∧ = [ ϕ ∧ ρ 0 T 0 ] \boldsymbol{\xi}^{\wedge}=\left[\begin{array}{ll}\boldsymbol{\phi}^{\wedge} & \boldsymbol{\rho} \\ \boldsymbol{0}^T & 0\end{array}\right] ξ∧=[ϕ∧0Tρ0] -
SE(3)->se(3)
θ = arccos tr ( R ) − 1 2 R a = a t = J ρ \theta=\arccos \frac{\operatorname{tr}(\boldsymbol{R})-1}{2} \quad \boldsymbol{R} \boldsymbol{a}=\boldsymbol{a} \quad \boldsymbol{t}=\boldsymbol{J} \boldsymbol{\rho} θ=arccos2tr(R)−1Ra=at=Jρ
定义伪指数(pseudo-exponential):
exp ( ξ ∧ ) = [ exp ( ϕ ∧ ) ρ 0 T 1 ] \exp \left(\boldsymbol{\xi}^{\wedge}\right)=\left[\begin{array}{cc}\exp \left(\boldsymbol{\phi}^{\wedge}\right) & \boldsymbol{\rho} \\ 0^T & 1\end{array}\right] exp(ξ∧)=[exp(ϕ∧)0Tρ1]
伪指数的定义是为了简化求解雅克比矩阵。
参考
[1] 视觉SLAM14讲
[2] J.-l. Blanco, “A tutorial on SE (3) transformation parameterizations and on-manifold optimization,” no. 3, 2013.