文档维护：Arvin

网页部署：Arvin

▶

写在前面：这篇的内容主要来自于《slam十四讲：从入门到实践》中的第三、四章，是为了引出李群李代数，文章大多记录了一些结论，如果对推导有兴趣可以去看书中详细的过程。

0

基础知识

三维空间刚体运动

旋转矩阵

1.反对称矩阵

设A=(aij)n×nA=(a_{ij})_{n \times n}，若其中元素满足aij=aji,∀i,j⇔AT=Aa_{ij}=a_{ji},\forall i,j\Leftrightarrow A^T=A，则称AA是对称矩阵；若其元素满足aij=−aji,∀i,j⇔AT=−Aa_{ij}=-a_{ji},\forall i,j\Leftrightarrow A^T=-A，则称AA为反对称矩阵。

若AA是反对称矩阵，则aij=−aija_{ij}=-a_{ij}，当i=ji=j时，便有aij=0a_{ij}=0，即反对称矩阵对角线上的元全为零，而位于主对角线两侧对称的元素反号。

2.旋转矩阵

欧氏变换由旋转和平移组成。我们首先考虑旋转。设某个单位正交基(e1,e2,e3)(e_1, e_2, e_3)经过一次旋转变成了(e1′,e2′,e3′)(e^{\prime}_1, e^{\prime}_2, e^{\prime}_3)。那么，对于同一个向量aa，它在两个坐标系下的坐标分别为[a1,a2,a3]T[a_1, a_2, a_3]^T和[a1′,a2′,a3′]T[a^{\prime}_1, a^{\prime}_2, a^{\prime}_3]^T。有：

\symbfit{a}=[\boldsymbol{e}_1,\boldsymbol{e}_2,\boldsymbol{e}_3]\begin{bmatrix}a_1\\\\a_2\\\\a_3\end{bmatrix}=[\boldsymbol{e}_1',\boldsymbol{e}_2',\boldsymbol{e}_3']\begin{bmatrix}a_1'\\\\a_2'\\\\a_3'\end{bmatrix}

左乘[e1Te2Te3T]\begin{bmatrix}e_1^\mathrm{T}\\e_2^\mathrm{T}\\e_3^\mathrm{T}\end{bmatrix}，得到：

[a1a2a3]=[e1Te1′e1Te2′e1Te3′e2Te1′e2Te2′e2Te3′e3Te1′e3Te2′e3Te3′][a1′a2′a3′a3′]=⁡defRa′\begin{bmatrix}a_1\\a_2\\a_3\end{bmatrix}=\begin{bmatrix}e_1^{\mathrm{T}}e_1'&e_1^{\mathrm{T}}e_2'&e_1^{\mathrm{T}}e_3'\\e_2^{\mathrm{T}}e_1'&e_2^{\mathrm{T}}e_2'&e_2^{\mathrm{T}}e_3'\\e_3^{\mathrm{T}}e_1'&e_3^{\mathrm{T}}e_2'&e_3^{\mathrm{T}}e_3'\end{bmatrix}\begin{bmatrix}a_1'\\a_2'\\a_3'\\a_3'\end{bmatrix}\overset{\mathrm{def}}{\operatorname*{=}}\boldsymbol{R}\boldsymbol{a}'

我们称RR为旋转矩阵，也叫做方向余弦矩阵。

旋转矩阵是一个行列式为1的正交矩阵。反之，行列式为1的正交矩阵也是一个旋转矩阵。所以，可以将nn维旋转矩阵的集合定义如下：

SO(n)={R∈Rn×n∣RRT=I,det⁡(R)=1}\mathrm{SO}(n)=\{\boldsymbol{R}\in\mathbb{R}^{n\times n}|\boldsymbol{R}\boldsymbol{R}^\mathrm{T}=\boldsymbol{I},\det(\boldsymbol{R})=1\}

SO(n)SO(n)是一个特殊正交群。

3.变换矩阵

欧式变换中，除了旋转还有平移。用公式表示：

a′=Ra+t\boldsymbol{a'}=\boldsymbol{Ra}+\boldsymbol{t}

其中RR为旋转矩阵，tt为平移向量。但若经过多次变换，这里并非完全线性关系，如：

b=R1a+t1,c=R2b+t2b=R_1\boldsymbol{a}+\boldsymbol{t}_1,\quad c=\boldsymbol{R}_2\boldsymbol{b}+\boldsymbol{t}_2

c=R2(R1a+t1)+t2c=R_2\left(R_1\boldsymbol{a}+\boldsymbol{t}_1\right)+\boldsymbol{t}_2

为了化简，我们引入齐次坐标和变换矩阵：

[a′1]=[Rt0T1][a1]=⁡defT[a1]\begin{bmatrix}a'\\\\1\end{bmatrix}=\begin{bmatrix}R&t\\\mathbf{0}^\mathrm{T}&1\end{bmatrix}\begin{bmatrix}a\\\\1\end{bmatrix}\overset{\mathrm{def}}{\operatorname*{=}}T\begin{bmatrix}a\\\\1\end{bmatrix}

在一个三维向量的末尾添加1，将其变成四维矩阵，成为齐次坐标。矩阵TT成为变换矩阵。这种矩阵又被称为特殊欧式群：

SE(3)={T=[Rt0T1]∈R4×4∣R∈SO(3),t∈R3}\mathrm{SE}(3)=\left\{\boldsymbol{T}=\begin{bmatrix}\boldsymbol{R}&t\\\boldsymbol{0}^\mathrm{T}&1\end{bmatrix}\in\mathbb{R}^{4\times4}|\boldsymbol{R}\in\mathrm{SO}(3),\boldsymbol{t}\in\mathbb{R}^3\right\}

旋转向量和欧拉角

1.旋转向量

任意旋转都可以用一个旋转轴和一个旋转角来刻画。我们使用一个向量，其方向与旋转轴一致，而长度等于旋转角，这种向量称为旋转向量。

假设有一个旋转矩阵RR。假设旋转轴为一个单位长度的向量nn，角度为θ\theta，那么θn\theta n也可以描述这个旋转。从旋转向量到旋转矩阵的转换过程有罗德里格斯公式表明：

R=cos⁡θI+(1−cos⁡θ)nnT+sin⁡θn∧\boldsymbol{R}=\cos\theta\boldsymbol{I}+(1-\cos\theta)\boldsymbol{n}\boldsymbol{n}^\mathrm{T}+\sin\theta\boldsymbol{n}^\mathrm{\wedge}

从旋转矩阵到旋转向量：

因为

tr(R)=cos⁡θtr⁡(I)+(1−cos⁡θ)tr⁡(nnT)+sin⁡θtr⁡(n∧)=3cos⁡θ+(1−cos⁡θ)=1+2cos⁡θ\begin{aligned} tr\left(R\right)& =\cos\theta\operatorname{tr}\left(\boldsymbol{I}\right)+\left(1-\cos\theta\right)\operatorname{tr}\left(\boldsymbol{n}\boldsymbol{n}^\mathrm{T}\right)+\sin\theta\operatorname{tr}(\boldsymbol{n}^{\wedge}) \\ &=3\cos\theta+(1-\cos\theta) \\ &=1+2\cos\theta \end{aligned}

所以

θ=arccos⁡tr⁡(R)−12\theta=\arccos\frac{\operatorname{tr}(\boldsymbol{R})-1}2

关于转轴nn，旋转轴上的向量在旋转后不发生改变，说明：

Rn=nRn=n

因此，转轴nn是矩阵RR特征值1对应的特征向量。

2.欧拉角

欧拉角使用了3个分离的转角，把一个旋转分解成3次不同轴的旋转。一般情况下，我们使用”偏航-俯仰-滚转“（yaw-pitch-roll）3个角度来描述一个旋转。它等价于ZYX轴的旋转：

  • 绕物体的Z轴旋转，得到偏航角yaw。
  • 绕旋转之后的Y轴旋转，得到俯仰角pitch。
  • 绕旋转之后的X轴旋转，得到滚转角。

四元数

1.定义

在二维情况下，旋转可以由单位复数来描述。类似地，我们用四元数描述三维旋转：

一个四元数qq拥有一个实部和三个虚部：

q=q0+q1i+q2j+q3k\mathbf{q}=q_0+q_1\mathbf{i}+\mathbf{q}_2\mathbf{j}+\mathbf{q}_3\mathbf{k}

其中，i,h,ki,h,k为四元数的三个虚部。满足：

{i2=j2=k2=−1ij=k,ji=−kjk=i,kj=−iki=j,ik=−j\begin{cases}\mathrm{i^2=j^2=k^2=-1}\\[2ex]\mathrm{ij=k,ji=-k}\\[2ex]\mathrm{jk=i,kj=-i}\\[2ex]\mathrm{ki=j,ik=-j}\end{cases}

2.转换成旋转矩阵

用四元数表示旋转

把三维空间点pp用一个虚四元数来描述：

p=[0,x,y,z]T=[0,v]T\boldsymbol{p}=[0,x,y,z]^\mathrm{T}=[0,\boldsymbol{v}]^\mathrm{T}

则旋转后的p′p^{\prime}可表示为：

p′=qpq−1p^{\prime}=qpq^{-1}

我们换另一种写法，先定义如下的符号q+\boldsymbol{q}^+和q⊕\boldsymbol{q}^\mathrm{\oplus}

q+=[s−vTvsI+v∧],q⊕=[s−vTvsI−v∧]\boldsymbol{q}^+=\begin{bmatrix}s&-\boldsymbol{v}^\mathrm{T}\\\\\boldsymbol{v}&s\boldsymbol{I}+\boldsymbol{v}^\mathrm{\wedge}\end{bmatrix},\quad\boldsymbol{q}^\mathrm{\oplus}=\begin{bmatrix}s&-\boldsymbol{v}^\mathrm{T}\\\\\boldsymbol{v}&s\boldsymbol{I}-\boldsymbol{v}^\mathrm{\wedge}\end{bmatrix}

有：

q1+q2=[s1−v1Tv1s1I+v1∧][s2v2]=[−v1Tv2+s1s2s1v2+s2v1+v1∧v2]=q1q2q_1^+q_2=\begin{bmatrix}s_1&-\boldsymbol{v}_1^\mathrm{T}\\\\v_1&s_1\boldsymbol{I}+\boldsymbol{v}_1^\mathrm{\wedge}\end{bmatrix}\begin{bmatrix}s_2\\\\v_2\end{bmatrix}=\begin{bmatrix}-\boldsymbol{v}_1^\mathrm{T}\boldsymbol{v}_2+s_1s_2\\\\s_1\boldsymbol{v}_2+s_2\boldsymbol{v}_1+\boldsymbol{v}_1^\mathrm{\wedge}\boldsymbol{v}_2\end{bmatrix}=\boldsymbol{q}_1\boldsymbol{q}_2

则：

p′=qpq−1=q+p+q−1=q+q−1⊕p\begin{aligned}p^{\prime}&=qpq^{-1}\\&=q^{+}p^{+}q^{-1}\\&=q^{+}q^{-1\oplus}p\end{aligned}

q+(q−1)⊕=[s−vTvsI+v∧][svT−vsI+v∧]=[100TvvT+s2I+2sv∧+(v∧)2]\begin{align} \boldsymbol{q}^+\left(\boldsymbol{q}^{-1}\right)^\oplus & = \begin{bmatrix}s&-\boldsymbol{v}^\mathrm{T}\\\\\boldsymbol{v}&s\boldsymbol{I}+\boldsymbol{v}^\mathrm{\wedge}\end{bmatrix}\begin{bmatrix}s&\boldsymbol{v}^\mathrm{T}\\\\-\boldsymbol{v}&s\boldsymbol{I}+\boldsymbol{v}^\mathrm{\wedge}\end{bmatrix}\\ & = \begin{bmatrix}1&\boldsymbol{0}\\\boldsymbol{0}^\mathrm{T}&\boldsymbol{v}\boldsymbol{v}^\mathrm{T}+s^2\boldsymbol{I}+2s\boldsymbol{v}^\mathrm{\wedge}+\left(\boldsymbol{v}^\mathrm{\wedge}\right)^2\end{bmatrix} \end{align}

得：

R=vvT+s2I+2sv∧+(v∧)2(1)R=vv^{\mathrm{T}}+s^2I+2s\boldsymbol{v}^{\wedge}+\left(\boldsymbol{v}^{\wedge}\right)^2 \tag{1}

3.转换成旋转向量

对式(1)两侧求迹，得

tr(R)=tr⁡(vvT+3s2+2s⋅0+tr⁡((v∧)2)=v12+v22+v32+3s2−2(v12+v22+v32)=(1−s2)+3s2−2(1−s2)=4s2−1\begin{aligned} \text{tr(R)}& =\operatorname{tr}(\boldsymbol{v}\boldsymbol{v}^\mathrm{T}+3s^2+2s\cdot0+\operatorname{tr}((\boldsymbol{v}^\wedge)^2) \\ &=v_1^2+v_2^2+v_3^2+3s^2-2(v_1^2+v_2^2+v_3^2) \\ &=(1-s^2)+3s^2-2(1-s^2) \\ &=4s^2-1 \end{aligned}

由旋转向量和旋转矩阵之间得转换公式得

θ=arccos⁡tr(R−1)2=arccos⁡(2s2−1).\begin{aligned}\theta&=\arccos\frac{\mathrm{tr}(\boldsymbol{R}-1)}{2}\\&=\arccos(2s^2-1).\end{aligned}

即

cos⁡θ=2s2−1=2cos⁡2θ2−1\cos\theta=2s^2-1=2\cos^2\frac{\theta}{2}-1

所以

θ=2arccos⁡s\theta=2\arccos s

总之，四元数到旋转向量的转换公式如下：

{θ=2arccos⁡q0[nx,ny,nz]T=[q1,q2,q3]T/sin⁡θ2\left\{\begin{array}{l} \theta=2 \arccos q_{0} \\ {\left[n_{x}, n_{y}, n_{z}\right]^{\mathrm{T}}=\left[q_{1}, q_{2}, q_{3}\right]^{\mathrm{T}} / \sin \frac{\theta}{2}} \end{array}\right.

李群李代数

群

SO(3)={R∈R3×3∣RRT=I,det⁡(R)=1},SE(3)={T=[Rt0T1]∈R4×4∣R∈SO(3),t∈R3}\begin{aligned} &\text{SO(3)} =\{\boldsymbol{R}\in\mathbb{R}^{3\times3}|\boldsymbol{RR}^{\mathrm{T}}=\boldsymbol{I},\det(\boldsymbol{R})=1\}, \\ &\text{SE(3)} =\left\{\boldsymbol{T}=\begin{bmatrix}\boldsymbol{R}&\boldsymbol{t}\\\\\boldsymbol{0}^\mathrm{T}&1\end{bmatrix}\in\mathbb{R}^{4\times4}|\boldsymbol{R}\in\mathrm{SO}(3),\boldsymbol{t}\in\mathbb{R}^3\right\} \end{aligned}

上式分别为三维旋转矩阵集合和三维变换矩阵集合，分别构成了特殊正交群SO(3)和特殊欧式群SE(3)。

群是一种集合加上一种运算的代数结构。我们把集合记作AA，运算记作$\cdot ，那么群可以记作，那么群可以记作G=(A,\cdot)$。群要求这个运算满足以下几个条件：

  1. 封闭性：∀a1,a2∈A,a1⋅a2∈A\forall a_1,a_2\in A,\quad a_1\cdot a_2\in A
  2. 结合律：∀a1,a2,a3∈A,(a1⋅a2)⋅a3=a1⋅(a2⋅a3)\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)
  3. 幺元：∃a0∈A,s.t.∀a∈A,a0⋅a=a⋅a0=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
  4. 逆：∀a∈A,∃a−1∈A,s.t.a⋅a−1=a0\forall a\in A,\quad\exists a^{-1}\in A,\quad\text{s.t.}\quad a\cdot a^{-1}=a_0

李群是指具有连续光滑性质的群。

李代数

李代数描述了李群的局部性质。准确的讲是单位元附近的正切空间。

李代数由一个集合VV、一个数域FF和一个二元运算[,][,]组成。如果满足以下几条性质，则称(V,F,[,])(\mathbb{V},\mathbb{F},[,])为一个李代数，记作g\mathfrak{g}。

  1. 封闭性：∀X,Y∈V,[X,Y]∈V\forall X,\boldsymbol{Y}\in\mathbb{V},[\boldsymbol{X},\boldsymbol{Y}]\in\mathbb{V}

  2. 双线性：∀X,Y,Z∈V,a,b∈F\forall X,Y,Z\in\mathbb{V},a,b\in\mathbb{F}，有

    [aX+bY,Z]=a[X,Z]+b[Y,Z],[Z,aX+bY]=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}]

  3. 自反性：∀X∈V,[X,X]=0\forall X\in\mathbb{V},[X,X]=\mathbf{0}

  4. 雅可比等价：∀X,Y,Z∈V,[X,[Y,Z]]+[Z,[X,Y]]+[Y,[Z,X]]=0\forall X,Y,Z\in\mathbb{V},[X,[Y,Z]]+[Z,[X,Y]]+[Y,[Z,X]]=\mathbf{0}

李代数so(3)\mathfrak{so}(3)

SO(3)SO(3)对应的李代数是定义在R3\mathbb{R}^3上的向量，我们记作ϕ\phi，其反对称矩阵为：

Φ=ϕ∧=[0−ϕ3ϕ2ϕ30−ϕ1−ϕ2ϕ10]∈R3×3\boldsymbol{\Phi}=\boldsymbol{\phi}^\wedge=\begin{bmatrix}0&-\phi_3&\phi_2\\\\\phi_3&0&-\phi_1\\\\-\phi_2&\phi_1&0\end{bmatrix}\in\mathbb{R}^{3\times3}

此定义下，李括号为：

[ϕ1,ϕ2]=(Φ1Φ2−Φ2Φ1)∨[\phi_1,\phi_2]=(\boldsymbol{\Phi}_1\boldsymbol{\Phi}_2-\boldsymbol{\Phi}_2\boldsymbol{\Phi}_1)^\vee

则so(3)\mathfrak{so}(3)为：

so(3)={ϕ∈R3,Φ=ϕ∧∈R3×3}\mathfrak{so}(3)=\left\{\phi\in\mathbb{R}^3,\boldsymbol{\Phi}=\boldsymbol{\phi}^\wedge\in\mathbb{R}^{3\times3}\right\}

三维旋转矩阵RR（李群）与l李代数so(3)\mathfrak{so}(3)的关系由指数映射恒定：

R=exp⁡(ϕ∧)R=\exp(\phi^{\wedge})

李代数se(3)\mathfrak{se}(3)

与旋转矩阵类似地，对于SE(3)SE(3)也有对应的李代数se(3)\mathfrak{se}(3)。其定义为：

sc(3)={ξ=[ρϕ]∈R6,ρ∈R3,ϕ∈so(3),ξ∧=[ϕ∧ρ0T0]∈R4×4}\mathfrak{sc}(3)=\left\{\boldsymbol{\xi}=\begin{bmatrix}\rho\\\phi\end{bmatrix}\in\mathbb{R}^6,\boldsymbol{\rho}\in\mathbb{R}^3,\boldsymbol{\phi}\in\mathfrak{so}(3),\boldsymbol{\xi}^\wedge=\begin{bmatrix}\boldsymbol{\phi}^\wedge&\boldsymbol{\rho}\\\\\boldsymbol{0}^\mathrm{T}&0\end{bmatrix}\in\mathbb{R}^{4\times4}\right\}

我们把每个se(3)\mathfrak{se}(3)元素记作ξ\xi，它是一个六维向量。前三维为平移（但含义与变换矩阵中的平移不同），记作ρ\rho；后三维为旋转，记作ϕ\phi，实质上是元素so(3)\mathfrak{so}(3)。同时，我们拓展了^符号的含义。在se(3)\mathfrak{se}(3)中，同样使用＾符号，将一个六维向量转换成四维矩阵，但这里不再表示反对称：

ξ∧=[ϕ∧ρ0T0]∈R4×4\boldsymbol{\xi}^\wedge=\begin{bmatrix}\boldsymbol{\phi}^\wedge&\boldsymbol{\rho}\\\boldsymbol{0}^\mathrm{T}&0\end{bmatrix}\in\mathbb{R}^{4\times4}

李括号定义为：

[ξ1,ξ2]=(ξ1∧ξ2∧−ξ2∧ξ1∧)∨[\boldsymbol{\xi}_1,\boldsymbol{\xi}_2]=(\boldsymbol{\xi}_1^\wedge\boldsymbol{\xi}_2^\wedge-\boldsymbol{\xi}_2^\wedge\boldsymbol{\xi}_1^\wedge)^\vee

指数映射与对数映射

1

其中，由于ϕ\phi是三维向量，我们可以定义它的模长和方向，分别记作θ\theta和aa

ϕ=θa\phi=\theta\boldsymbol{a}

BCH公式与近似形式

ln⁡(exp⁡(ϕ1∧)exp⁡(ϕ2∧))∨≈{Jl(ϕ2)−1ϕ1+ϕ2当ϕ1为小量,Jr(ϕ1)−1ϕ2+ϕ1当ϕ2为小量.\ln\left(\exp\left(\phi_1^{\wedge}\right)\exp\left(\phi_2^{\wedge}\right)\right)^{\vee}\approx\begin{cases}J_l\left(\phi_2\right)^{-1}\phi_1+\phi_2&\text{当}\phi_1\text{为小量},\\J_r\left(\phi_1\right)^{-1}\phi_2+\phi_1&\text{当}\phi_2\text{为小量}.\end{cases}

以第一个近似为例。该式告诉我们，当对一个旋转矩阵R2R2（李代数为ϕ2\phi_2）左乘一个微小旋转矩阵R1R1（李代数为ϕ1\phi_1）时，可以近似地看作，在原有的李代数ϕ2\phi_2上加上了一项Jl(ϕ2)−1ϕ1\boldsymbol{J}_l(\boldsymbol{\phi}_2)^{-1}\boldsymbol{\phi}_1。同理，第二个近似描述了右乘一个微小位移的情况。于是，李代数在BCH近似下，分成了左乘近似和右乘近似两种，在使用时我们须注意使用的是左乘模型还是右乘模型。

以左乘为例：

Jl=J=sin⁡θθI+(1−sin⁡θθ)aaT+1−cos⁡θθa∧\boldsymbol{J}_l=\boldsymbol{J}=\frac{\sin\theta}\theta\boldsymbol{I}+\left(1-\frac{\sin\theta}\theta\right)\boldsymbol{a}\boldsymbol{a}^\mathrm{T}+\frac{1-\cos\theta}\theta\boldsymbol{a}^\wedge

它的逆为：

Jr(ϕ)=Jl(−ϕ)J_r(\phi)=\boldsymbol{J}_l(-\boldsymbol{\phi})

所以在SO(3)SO(3)中，有

exp⁡(Δϕ∧)exp⁡(ϕ∧)=exp⁡((ϕ+Jl−1(ϕ)Δϕ)∧)\exp\left(\Delta\phi^{\wedge}\right)\exp\left(\phi^{\wedge}\right)=\exp\left(\left(\phi+J_{l}^{-1}\left(\phi\right)\Delta\phi\right)^{\wedge}\right)

exp⁡((ϕ+Δϕ)↑)=exp⁡((JlΔϕ)↑)exp⁡(ϕ↑)=exp⁡(ϕ↑)exp⁡((JrΔϕ)↑)\exp\left((\phi+\Delta\phi)^{\uparrow}\right)=\exp\left((J_l\Delta\phi)^{\uparrow}\right)\exp\left(\phi^{\uparrow}\right)=\exp\left(\phi^{\uparrow}\right)\exp\left((J_r\Delta\phi)^{\uparrow}\right)

同样的在SE(3)SE(3)中，有

exp⁡(Δξ∧)exp⁡(ξ∧)≈exp⁡((Jl−1Δξ+ξ)∧)exp⁡(ξ∧)exp⁡(Δξ∧)≈exp⁡((Jr−1Δξ+ξ)∧)\begin{aligned}\exp\left(\Delta\boldsymbol{\xi}^\wedge\right)\exp\left(\boldsymbol{\xi}^\wedge\right)&\approx\exp\left(\left(\boldsymbol{J}_l^{-1}\Delta\boldsymbol{\xi}+\boldsymbol{\xi}\right)^\wedge\right)\\\exp\left(\boldsymbol{\xi}^\wedge\right)\exp\left(\Delta\boldsymbol{\xi}^\wedge\right)&\approx\exp\left(\left(\boldsymbol{J}_r^{-1}\Delta\boldsymbol{\xi}+\boldsymbol{\xi}\right)^\wedge\right)\end{aligned}

李代数求导

**SO(3)**上的李代数求导：

∂(exp⁡(ϕ∧)p)∂ϕ=lim⁡δϕ→0exp⁡((ϕ+δϕ)∧)p−exp⁡(ϕ∧)pδϕ=lim⁡δϕ→0exp⁡((Jlδϕ)∧)exp⁡(ϕ∧)p−exp⁡(ϕ∧)pδϕ=lim⁡δϕ→0(I+(Jlδϕ)∧)exp⁡(ϕ∧)p−exp⁡(ϕ∧)pδϕ=lim⁡δϕ→0(Jlδϕ)∧exp⁡(ϕ∧)pδϕ=lim⁡δϕ→0−(exp⁡(ϕ∧)p)∧Jlδϕδϕ=−(Rp)∧Jl\begin{aligned} \frac{\partial\left(\exp \left(\boldsymbol{\phi}^{\wedge}\right) \boldsymbol{p}\right)}{\partial \boldsymbol{\phi}} & =\lim _{\delta \boldsymbol{\phi} \rightarrow \mathbf{0}} \frac{\exp \left((\boldsymbol{\phi}+\delta \boldsymbol{\phi})^{\wedge}\right) \boldsymbol{p}-\exp \left(\boldsymbol{\phi}^{\wedge}\right) \boldsymbol{p}}{\delta \boldsymbol{\phi}} \\ & =\lim _{\delta \boldsymbol{\phi} \rightarrow \mathbf{0}} \frac{\exp \left(\left(\boldsymbol{J}_{l} \delta \boldsymbol{\phi}\right)^{\wedge}\right) \exp \left(\boldsymbol{\phi}^{\wedge}\right) \boldsymbol{p}-\exp \left(\boldsymbol{\phi}^{\wedge}\right) \boldsymbol{p}}{\delta \boldsymbol{\phi}} \\ & =\lim _{\delta \boldsymbol{\phi} \rightarrow \mathbf{0}} \frac{\left(\boldsymbol{I}+\left(\boldsymbol{J}_{l} \delta \boldsymbol{\phi}\right)^{\wedge}\right) \exp \left(\boldsymbol{\phi}^{\wedge}\right) \boldsymbol{p}-\exp \left(\boldsymbol{\phi}^{\wedge}\right) \boldsymbol{p}}{\delta \boldsymbol{\phi}} \\ & =\lim _{\delta \boldsymbol{\phi} \rightarrow \mathbf{0}} \frac{\left(\boldsymbol{J}_{l} \delta \boldsymbol{\phi}\right)^{\wedge} \exp \left(\boldsymbol{\phi}^{\wedge}\right) \boldsymbol{p}}{\delta \boldsymbol{\phi}} \\ & =\lim _{\delta \boldsymbol{\phi} \rightarrow \mathbf{0}} \frac{-\left(\exp \left(\boldsymbol{\phi}^{\wedge}\right) \boldsymbol{p}\right)^{\wedge} \boldsymbol{J}_{l} \delta \boldsymbol{\phi}}{\delta \boldsymbol{\phi}} \\ & =-(\boldsymbol{R} \boldsymbol{p})^{\wedge} \boldsymbol{J}_{l} \end{aligned}

扰动模型（左乘）：

∂(Rp)∂φ=lim⁡φ→0exp⁡(φ∧)exp⁡(ϕ∧)p−exp⁡(ϕ∧)pφ=lim⁡φ→0(I+φ∧)exp⁡(ϕ∧)p−exp⁡(ϕ∧)pφ=lim⁡φ→0φ∧Rpφ=lim⁡φ→0−(Rp)∧φφ=−(Rp)∧\begin{aligned} \frac{\partial(\boldsymbol{R} \boldsymbol{p})}{\partial \boldsymbol{\varphi}} & =\lim _{\boldsymbol{\varphi} \rightarrow 0} \frac{\exp \left(\boldsymbol{\varphi}^{\wedge}\right) \exp \left(\phi^{\wedge}\right) \boldsymbol{p}-\exp \left(\boldsymbol{\phi}^{\wedge}\right) \boldsymbol{p}}{\boldsymbol{\varphi}} \\ & =\lim _{\boldsymbol{\varphi} \rightarrow 0} \frac{\left(\boldsymbol{I}+\boldsymbol{\varphi}^{\wedge}\right) \exp \left(\phi^{\wedge}\right) \boldsymbol{p}-\exp \left(\boldsymbol{\phi}^{\wedge}\right) \boldsymbol{p}}{\boldsymbol{\varphi}} \\ & =\lim _{\boldsymbol{\varphi} \rightarrow 0} \frac{\boldsymbol{\varphi}^{\wedge} \boldsymbol{R} \boldsymbol{p}}{\varphi}=\lim _{\boldsymbol{\varphi} \rightarrow 0} \frac{-(\boldsymbol{R} \boldsymbol{p})^{\wedge} \boldsymbol{\varphi}}{\boldsymbol{\varphi}} \\ & =-(\boldsymbol{R} \boldsymbol{p})^{\wedge} \end{aligned}

**SE(3)**上的李代数求导

∂(Tp)∂δξ=lim⁡δξ→0exp⁡(δξ∧)exp⁡(ξ∧)p−exp⁡(ξ∧)pδξ=lim⁡δξ→0(I+δξ∧)exp⁡(ξ∧)p−exp⁡(ξ∧)pδξ=lim⁡δξ→0δξ∧exp⁡(ξ∧)pδξ=lim⁡δξ→0[δϕ∧δρ0T0][Rp+t1]δξ=lim⁡δξ→0[δϕ∧(Rp+t)+δρ0T][δρ,δϕ]T=[I−(Rp+t)∧0T0T]=⁡def(Tp)⊙\begin{align} \frac{\partial\left(\boldsymbol{Tp}\right)}{\partial\delta\boldsymbol{\xi}} &=\lim_{\delta\boldsymbol{\xi}\to\boldsymbol{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}} \\ &=\lim_{\delta\boldsymbol{\xi}\to\mathbf{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}\to\mathbf{0}}\frac{\delta\boldsymbol{\xi}^\wedge\exp\left(\boldsymbol{\xi}^\wedge\right)\boldsymbol{p}}{\delta\boldsymbol{\xi}} \\ &=\lim_{\delta\boldsymbol{\xi}\to\mathbf{0}}\frac{\begin{bmatrix}\delta\boldsymbol{\phi}^\wedge&\delta\boldsymbol{\rho}\\\mathbf{0}^\mathrm{T}&0\end{bmatrix}\begin{bmatrix}\boldsymbol{Rp}+\boldsymbol{t}\\\\1\end{bmatrix}}{\delta\boldsymbol{\xi}} \\ &=\lim_{\delta\boldsymbol{\xi}\to\boldsymbol{0}}\frac{\begin{bmatrix}\delta\boldsymbol{\phi}^{\wedge}\left(\boldsymbol{R}\boldsymbol{p}+\boldsymbol{t}\right)+\delta\boldsymbol{\rho}\\\mathbf{0}^{\mathrm{T}}\end{bmatrix}}{[\delta\boldsymbol{\rho},\delta\boldsymbol{\phi}]^{\mathrm{T}}} \\ &=\begin{bmatrix}\boldsymbol{I}&-(\boldsymbol{R}\boldsymbol{p}+\boldsymbol{t})^{\wedge}\\\mathbf{0}^{\mathrm{T}}&\boldsymbol{0}^{\mathrm{T}}\end{bmatrix} \\ &\overset{\mathrm{def}}{\operatorname*{=}}(\boldsymbol{T}\boldsymbol{p})^{\odot} \end{align}