前言

最近阅读了高翔老师的视觉SLAM十四讲, 也算是了解了当下机器人领域最炙热的机器人领域前沿研究问题大概是怎么样的数学问题. 不得不说视频与书籍真的是深入浅出.
所有第三库无痛安装教程: 知乎链接
SLAM 主要解决的偏颇地说就是在求解一个非线性系统状态方程, 相应的后端, 回环检测等等技术则为了在某个方面比如长期环境下该方程的求解正确而提出.
因为本人与李群李代的关联更深, 因此本文主要记录从SLAM中学习到的李群李代知识. 最大的收获应该是BCH公式及其带来的相关泰勒展开公式.
Modern Robotics中关于部分关于李群李代数的内容

正文

机器人学中运动描述的两类群: 特殊正交群(SO(3)) 与 特殊欧氏群 (SE(3))
SO(3)={R∈R3×3∣RR⊤=I,det(R)=1}.SO(3) =\{ {\mathbf{R}\in \mathbb{R}^{3\times3} | \mathbf{R}\mathbf{R}^{\top}=\mathbf{I}, det({\mathbf{R}}) =1}\}. SO(3)={R∈R3×3∣RR⊤=I,det(R)=1}.
SE(3)={T=[Rt0⊤1]∈R4×4∣R∈SO(3),t∈R3}SE(3) = \{\mathbf{T} = \begin{bmatrix}\mathbf{R} && \mathbf{t} \\ \mathbf{0}^{\top} && 1\end{bmatrix} \in \mathbb{R}^{4\times 4}| \mathbf{R} \in SO(3), \mathbf{t}\in \mathbb{R}^3\} SE(3)={T=[R0⊤​​t1​]∈R4×4∣R∈SO(3),t∈R3}

作为群而言, 很大的一个特征是加法不封闭: 即 ${\mathbf{R}}_1+{\mathbf{R}}_2\notin SO(3)$; 仅有乘法成立:
$${\mathbf{R}}_1{\mathbf{R}}_2\in SO(3);{\mathbf{T}}_1{\mathbf{T}}_2\in SE(3)$$
SLAM十四讲中写有: 对于这种只有一种运算的集合,称之为群.

群是一种集合加上一种运算的代数结构 $G(A,\cdot )$, A为集合. 这一种运算需要满足:

1. 封闭性: $\forall a_1,a_2\in A,a_1{a}_2\in A$;
2. 结合律: $\forall a_1,a_2,a_3\in A,(a_1{a}_2)a_3=a_1(a_2{a}_3)$
3. 单位元: $\exists a_0\in A,s.t.\forall a\in A,a_0\cdot a=a\cdot a_0=a$
4. 逆: $\forall a\in A,\exists a^{-1}\in A,s.t.a\cdot a^{-1}=a_0$

一个对于群的补充例子: 一般线性群$GL(n)$, 指$n\times n$的可逆矩阵, 对矩阵乘法成群.

李代数的引出:
KaTeX parse error: Expected '}', got 'EOF' at end of input: …p} \end{align}
即: $\dot{\mathbf{R}}(t)\mathbf{R}(t{)}^{\top }$是反对称矩阵.
$$\mathbf{a}\times \mathbf{b}=\left|\begin{array}{ccc}\mathbf{i}& \mathbf{j}& \mathbf{k}\\ a_1& a_2& a_3\\ b_1& b_2& b_3\end{array}\right|=\left[\begin{array}{c}{a}_2{b}_3-a_3{b}_2\\ a_3{b}_1-a_1{b}_3\\ a_1{b}_2-a_2{b}_1\end{array}\right]=\left[\begin{array}{ccc}0& -a_3& a_2\\ a_3& 0& -a_1\\ -a_2& a_1& 0\end{array}\right]\mathbf{b}={\mathbf{a}}^{\wedge }\mathbf{b}$$
${\mathbf{a}}^{\wedge }$是反对称矩阵. 即$\dot{\mathbf{R}}(t)\mathbf{R}(t{)}^{\top }$可以看作是某一个三维向量 $\mathbf{\varphi }(t)\in {\mathbb{R}}^3$的展开.
$$\dot{\mathbf{R}}(t)\mathbf{R}(t{)}^{\top }=\mathbf{\varphi }(t{)}^{\wedge }$$
由于$\mathbf{R}$为正交矩阵: ${\mathbf{R}}^{\top }={\mathbf{R}}^{-1}$
$$\dot{\mathbf{R}}(t)=\mathbf{\varphi }(t{)}^{\wedge }\mathbf{R}(t)=\left[\begin{array}{ccc}0& -{\varphi }_3& {\varphi }_2\\ {\varphi }_3& 0& -{\varphi }_1\\ -{\varphi }_2& {\varphi }_1& 0\end{array}\right]\mathbf{R}(t)$$
上式的含义是: 每对旋转矩阵求一次导数, 只需左乘一个$\mathbf{\varphi }(t)$矩阵即可, 设$t_0=0$, 并设此时旋转矩阵为$\mathbf{R}(t)=\mathbf{I}$. 我们可以把$\mathbf{R}(t)$在0附近进行一阶泰勒展开:
$$\begin{array}{rl}\mathbf{R}(t)& \approx \mathbf{R}(t_0)+\dot{\mathbf{R}}(t_0)(t-t_0)\\ & =\mathbf{I}+\mathbf{\varphi }(t_0{)}^{\wedge }(t)\end{array}$$

$\mathbf{\varphi }$ 反映了 $\mathbf{R}$的导数性质, 故称它在$SO(3)$原点附近的正切空间. 在$t_0$附近, SLAM十四讲中设$\mathbf{\varphi }$保持为常数$\mathbf{\varphi }(t_0)={\mathbf{\varphi }}_0$. 而如果用流形的观点来看的话, 我认为应该是指切向基 (tangent base) 保持不变.
$$\dot{\mathbf{R}}(t)=\mathbf{\varphi }(t_0{)}^{\wedge }\mathbf{R}(t)={{\mathbf{\varphi }}_0}^{\wedge }\mathbf{R}(t)$$
利用矩阵微分方程有:
$$\mathbf{R}(t)=\exp ({{\mathbf{\varphi }}_0}^{\wedge }\mathrm{t})$$
(此即为指数映射)

李代数的定义: 李代数由一个集合$\mathbb{V}$, 一个数域$\mathbb{F}$和一个二元运算符[,]组成. 它们需要满足以下性质, 则称($\mathbb{V},\mathbb{F},[,]$)为一个李代数, 记为$\mathfrak{g}$.

1. 封闭性: $\forall X,Y\in \mathbb{V},[X,Y]\in \mathbb{V}$;
2. 双线性: $\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];$$
3. 自反性: $\forall X\in \mathbb{V},[X,X]=0$;
4. 雅可比等价: $\forall X,Y,Z\in \mathbb{V},[X,[Y,Z]]+[Z,[X,Y]]+[Y,[Z,X]]=0$.
   此处的二元运算符为李括号 (Lie bracket).

李括号: $[w_1\times w_2]=[w_1][w_2]-[w_2][w_1]$
$$\begin{array}{rl}[{\mathcal{V}}_1][{\mathcal{V}}_2]-[{\mathcal{V}}_2][{\mathcal{V}}_1]& =\left[\begin{array}{cc}[w_1]& v_1\\ 0& 0\end{array}\right]\left[\begin{array}{cc}[w_2]& v_2\\ 0& 0\end{array}\right]-\left[\begin{array}{cc}[w_2]& v_2\\ 0& 0\end{array}\right]\left[\begin{array}{cc}[w_1]& v_1\\ 0& 0\end{array}\right]\\ & =\left[\begin{array}{cc}[w_1][w_2]-[w_2][w_1]& [w_1]v_2-[w_2]v_1\\ 0& 0\end{array}\right]\\ & =\left[\begin{array}{cc}[w^{\prime }]& v^{\prime }\\ 0& 0\end{array}\right]\end{array}$$
$$\left[\begin{array}{c}{w}^{\prime }\\ v^{\prime }\end{array}\right]=\left[\begin{array}{cc}[w_1]& 0\\ [v_1]& [w_1]\end{array}\right]\left[\begin{array}{c}{w}_2\\ v_2\end{array}\right]$$
需要注意的是: 这里摘抄自 Modern Robotics , 这份教材里对于角速度与线速度的放置与移动机器人领域中的设定是相反的.

Lie bracket的定义: 给定两个twist ${\mathcal{V}}_1=(w_1,v_1)$ 以及 ${\mathcal{V}}_2=(w_2,v_2)$, ${\mathcal{V}}_1$以及${\mathcal{V}}_2$的Lie bracket可以写作 $[{\mathrm{ad}}_{{\mathcal{V}}_1}]{\mathcal{V}}_2$ 或者 ${\mathrm{ad}}_{{\mathcal{V}}_1}({\mathcal{V}}_2)$ 被定义为如下形式:
$$\left[\begin{array}{cc}[w_1]& 0\\ [v_1]& [w_1]\end{array}\right]\left[\begin{array}{c}{w}_2\\ v_2\end{array}\right]=[{\mathrm{ad}}_{{\mathcal{V}}_1}]{\mathcal{V}}_2={\mathrm{ad}}_{{\mathcal{V}}_1}({\mathcal{V}}_2)\in {\mathbb{R}}^6$$

李代数$\mathfrak{so}(3)$
$$\Phi ={\varphi }^{\wedge }=\left[\begin{array}{ccc}0& -{\varphi }_3& {\varphi }_2\\ {\varphi }_3& 0& -{\varphi }_1\\ -{\varphi }_2& {\varphi }_1& 0\end{array}\right]\in {\mathbb{R}}^{3\times 3}$$
两个向量${\varphi }_1$与${\varphi }_2$的李括号为:
$$[{\varphi }_1,{\varphi }_2]=({\mathbf{\varphi }}_1{\mathbf{\varphi }}_2-{\mathbf{\varphi }}_2{\mathbf{\varphi }}_1{)}^{\vee }$$

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

$\mathfrak{so}(3)$通过指数映射与$SO(3)$进行联系:
$$\mathbf{R}=\exp ({\varphi }^{\wedge }).$$

李代数$\mathfrak{se}(3)$
se(3)={ξ=[ρϕ]∈R6,ρ∈R3,ϕ∈so(3),ξ∧=[ϕ∧ρ0⊤0]∈R4×4}\mathfrak{se}(3) = \{\xi=\begin{bmatrix}\rho \\ \boldsymbol{\phi} \end{bmatrix} \in \mathbb{R}^{6}, \rho\in \mathbb{R}^3, \boldsymbol{\phi}\in \mathfrak{so}(3), \xi^{\wedge} = \begin{bmatrix} \phi^{\wedge} & \rho \\ 0^{\top} & 0\end{bmatrix}\in \mathbb{R}^{4\times4} \} se(3)={ξ=[ρϕ​]∈R6,ρ∈R3,ϕ∈so(3),ξ∧=[ϕ∧0⊤​ρ0​]∈R4×4}

$\mathfrak{se}(3)$的李括号:
$$[{\xi }_1,{\xi }_2]=({\xi }_1^{\wedge }{\xi }_2^{\wedge }-{\xi }_2^{\wedge }{\xi }_1^{\wedge }{)}^{\vee }$$
其验证过程可见上面从Modern Robotics中摘抄的部分, 只是角速度与线速度的位置上下需要颠倒.

---

指数与对数映射
任意矩阵的指数映射可以写成一个泰勒展开, 但是只有在收敛(矩阵中每个元素都收敛)的情况下才会有结果, 其结果仍是一个矩阵:
$$\exp (\mathrm{A})=\sum _{\mathrm{n}=0}^{\infty }\frac{1}{\mathrm{n}!}{\mathrm{A}}^{\mathrm{n}}$$
同样地,对$\mathfrak{so}(3)$中的任意元素$\mathbf{\varphi }$, 我们也可按此方式定义它的指数映射
$$\exp ({\mathbf{\varphi }}^{\wedge })=\sum _{\mathrm{n}=0}^{\infty }\frac{1}{\mathrm{n}!}({\mathbf{\varphi }}^{\wedge }{)}^{\mathrm{n}}$$
$\mathbf{\varphi }$是一个三维向量,可以定义出它的长度$\theta$ 与方向向量 $\mathbf{a}$, $\mathbf{\varphi }=\theta \mathbf{a}$.
$${\mathbf{a}}^{\wedge }{\mathbf{a}}^{\wedge }=\mathbf{a}{\mathbf{a}}^{\top }-\mathbf{I}$$
$${\mathbf{a}}^{\wedge }{\mathbf{a}}^{\wedge }{\mathbf{a}}^{\wedge }=-{\mathbf{a}}^{\wedge }$$

$$\begin{array}{rl}\exp ({\varphi }^{\wedge })& =exp(\theta {\mathbf{a}}^{\wedge })=\sum _{n=0}^{\infty }\frac{1}{n!}(\theta {\mathbf{a}}^{\wedge }{)}^n\\ & =I+\theta {\mathbf{a}}^{\wedge }+\frac{1}{2!}{\theta }^2{\mathbf{a}}^{\wedge }{\mathbf{a}}^{\wedge }+\frac{1}{3!}{\theta }^3{\mathbf{a}}^{\wedge }{\mathbf{a}}^{\wedge }{\mathbf{a}}^{\wedge }+\frac{1}{4!}{\theta }^4({\mathbf{a}}^{\wedge }{)}^4+...\\ & =\mathbf{a}{\mathbf{a}}^{\top }-{\mathbf{a}}^{\wedge }{\mathbf{a}}^{\wedge }+\theta {\mathbf{a}}^{\wedge }+\frac{1}{2!}{\theta }^2{\mathbf{a}}^{\wedge }{\mathbf{a}}^{\wedge }-\frac{1}{3!}{\theta }^3{\mathbf{a}}^{\wedge }-\frac{1}{4!}{\theta }^4({\mathbf{a}}^{\wedge }{)}^4+...\\ & =\mathbf{a}{\mathbf{a}}^{\top }+(\theta -\frac{1}{3!}{\theta }^3+\frac{1}{5!}{\theta }^5-...){\mathbf{a}}^{\wedge }-(1-\frac{1}{2!}{\theta }^2+\frac{1}{4!}{\theta }^4-...){\mathbf{a}}^{\wedge }{\mathbf{a}}^{\wedge }\\ & ={\mathbf{a}}^{\wedge }{\mathbf{a}}^{\wedge }+I+sin\theta {\mathbf{a}}^{\wedge }-cos\theta {\mathbf{a}}^{\wedge }{\mathbf{a}}^{\wedge }\\ & =(1-cos\theta ){\mathbf{a}}^{\wedge }{\mathbf{a}}^{\wedge }+sin\theta {\mathbf{a}}^{\wedge }\\ & =cos\theta I+(1-cos\theta )\mathbf{a}{\mathbf{a}}^{\top }+sin\theta {\mathbf{a}}^{\wedge }\end{array}$$
此即为罗德里格斯公式, 也称为指数映射

对数映射:
$$\mathbf{\varphi }=ln(R{)}^{\vee }=(\sum _{n=0}^{\infty }\frac{-1^n}{n+1}(R-I{)}^{n+1}{)}^{\vee }.$$
然而实际上, 更多的情况下是采用下面的方式进行转换
$$R=cos\theta I+(1-cos\theta )\mathbf{a}{\mathbf{a}}^{\top }+sin\theta {\mathbf{a}}^{\wedge }$$
$$\begin{array}{rl}tr(R)& =cos\theta tr(I)+(1-cos\theta )tr(\mathbf{a}{\mathbf{a}}^{\top })+sin\theta tr({\mathbf{a}}^{\wedge })\\ & =3cos\theta +(1-cos\theta )\\ & =1+2cos\theta \end{array}$$
即
$$\theta =arccos(\frac{tr(R)-1}{2})$$

SE(3)上的指数映射:
参考链接
$$\exp ({\xi }^{\wedge })=\left[\begin{array}{cc}{\sum }_{n=0}^{\infty }\frac{1}{n!}({\varphi }^{\wedge }{)}^n& {\sum }_{n=0}^{\infty }\frac{1}{(n+1)!}({\varphi }^{\wedge }{)}^n\rho \\ 0^{\top }& 1\end{array}\right]=\left[\begin{array}{cc}R& J\rho \\ 0^{\top }& 1\end{array}\right]=\mathrm{T}$$
此处的J矩阵: $J=\frac{sin\theta }{\theta }I+(1-\frac{sin\theta }{\theta })\mathbf{a}{\mathbf{a}}^{\top }+\frac{1-cos\theta }{\theta }{\mathbf{a}}^{\wedge }$

起初这里的J矩阵我认为是Modern Robotics中的$G(\theta )$, 但这样却导不出SLAM十四讲中的相似结果, 主要是J比$G(\theta )$多除了一项$\theta$, 后来经过对比发现, MR中是旋量$\mathcal{S}\theta$转换为$SE(3)$而此处是$se(3)$转换为$SE(3)$.

J矩阵的引出过程:
$$\begin{array}{rl}\exp ({\xi }^{\wedge })& =\sum _{n=0}^{\infty }\frac{1}{n!}{\left[\begin{array}{cc}{\mathbf{\varphi }}^{\wedge }& \rho \\ 0^{\top }& 0\end{array}\right]}^n\\ & =I+\sum _{n=1}^{\infty }\frac{1}{n!}{\left[\begin{array}{cc}\theta {\mathbf{a}}^{\wedge }& \rho \\ 0^{\top }& 0\end{array}\right]}^n\\ & =\left[\begin{array}{cc}{\sum }_{n=0}^{\infty }\frac{1}{n!}(\theta {\mathbf{a}}^{\wedge }{)}^n& {\sum }_{n=1}^{\infty }\frac{1}{n!}(\theta {\mathbf{a}}^{\wedge }{)}^{n-1}\rho \\ 0^{\top }& 1\end{array}\right]\end{array}$$
令$n=n-1$, 从而$n=n+1$, 带入上式右上角的一项有:
$$\exp ({\xi }^{\wedge })=\left[\begin{array}{cc}{\sum }_{n=0}^{\infty }\frac{1}{n!}(\theta {\mathbf{a}}^{\wedge }{)}^n& {\sum }_{n=0}^{\infty }\frac{1}{(n+1)!}(\theta {\mathbf{a}}^{\wedge }{)}^n\rho \\ 0^{\top }& 1\end{array}\right]$$

$$\begin{array}{rl}J& =\sum _{n=0}^{\infty }\frac{1}{(n+1)!}(\theta {\mathbf{a}}^{\wedge }{)}^n\\ & =I+\frac{1}{2!}(\theta {\mathbf{a}}^{\wedge })+\frac{1}{3!}(\theta {\mathbf{a}}^{\wedge }{)}^2+\cdots +\frac{1}{(n+1)!}(\theta {\mathbf{a}}^{\wedge }{)}^n\\ & =I+\frac{1}{2!}(\theta {\mathbf{a}}^{\wedge })+\frac{1}{3!}(\theta {\mathbf{a}}^{\wedge }{)}^2-\frac{1}{4!}{\theta }^3{\mathbf{a}}^{\wedge }-\frac{1}{5!}{\theta }^4({\mathbf{a}}^{\wedge }{)}^2+\frac{1}{6!}{\theta }^5({\mathbf{a}}^{\wedge })+\frac{1}{7!}{\theta }^7({\mathbf{a}}^{\wedge }{)}^2+\cdots \\ & =\frac{1}{\theta }(\frac{1}{2!}{\theta }^2-\frac{1}{4!}{\theta }^4+\cdots ){\mathbf{a}}^{\wedge }+\frac{1}{\theta }(\frac{1}{3!}{\theta }^3-\frac{1}{5!}{\theta }^5+\cdots )({\mathbf{a}}^{\wedge }{)}^2+I\end{array}$$
借由$sin\theta$与$cos\theta$的展开以及${\mathbf{a}}^{\wedge }{\mathbf{a}}^{\wedge }=\mathbf{a}{\mathbf{a}}^{\top }-I$可以有:
$$\begin{array}{rl}sin\theta & =\theta -\frac{1}{3!}{\theta }^3+\frac{1}{5!}{\theta }^5-\frac{1}{7!}{\theta }^7+\cdots \\ cos\theta & =1-\frac{1}{2!}{\theta }^2+\frac{1}{4!}{\theta }^4-\frac{1}{6!}{\theta }^6+\cdots \end{array}$$
$$\begin{array}{rl}J& =\frac{1}{\theta }(1-cos\theta )({\mathbf{a}}^{\wedge })+\frac{\theta -sin\theta }{\theta }(\mathbf{a}{\mathbf{a}}^{\top }-I)+I\\ & =\frac{sin\theta }{\theta }I+(1-\frac{sin\theta }{\theta })\mathbf{a}{\mathbf{a}}^{\top }+\frac{1-cos\theta }{\theta }{\mathbf{a}}^{\wedge }\end{array}$$

---

Baker-Campbell-Hausdoff公式: BCH公式的引入是为了指出$exp({\varphi }_1^{\wedge })exp({\varphi }_2^{\wedge })\ne exp(({\varphi }_1+{\varphi }_2{)}^{\wedge }).$ 即$ln(exp({\varphi }_1^{\wedge })exp({\varphi }_2^{\wedge }))\ne {\varphi }_1^{\wedge }+{\varphi }_2^{\wedge }$ 同时给出可以用于描述增量的截断公式: $ln(exp(A)exp(B))=A+B+\frac{1}{2}[A,B]+\frac{1}{12}[A,[A,B]]-\frac{1}{12}[B,[A,B]]+\cdots$

BCH公式告诉我们, 当处理两个矩阵指数之积时, 它们会产生一些由李括号组成的余项. 特别地, 考虑$SO(3)$上的李代数$ln(exp({\varphi }_1^{\wedge })exp({\varphi }_2^{\wedge }){)}^{\vee }$, 当${\varphi }_1$或${\varphi }_2$为小量, 小量二次以上的项都可以被忽略掉, 此时BCH公式:
$$ln(exp({\varphi }_1^{\wedge })exp({\varphi }_2^{\wedge }){)}^{\vee }\approx \{\begin{array}{cc}{J}_l({\varphi }_2{)}^{-1}{\varphi }_1+{\varphi }_2& 当{\varphi }_1为小量\\ J_r({\varphi }_1{)}^{-1}{\varphi }_2+{\varphi }_1& 当{\varphi }_2为小量\end{array}$$
BCH公式的含义是: 当对一个旋转矩阵${\mathbf{R}}_2$(李代数为${\mathbf{\varphi }}_2$)左乘一个微小旋转矩阵${\mathbf{R}}_1$(李代数为${\mathbf{\varphi }}_1)$时,可以近似地看作,在原有的李代数${\mathbf{\varphi }}_2$上加上一项$J_l({\varphi }_2{)}^{-1}{\varphi }_1$, 同理, 第二个近似描述了右乘一个微小位移的情况. 于是, 李代数在BCH近似下,分为了左乘近似与右乘近似两种. 在使用时, 我们必须注意是使用的是左乘模型还是右乘模型.

$J_l$实际上就是前面提到的$J$
$$J_l=J=\frac{sin\theta }{\theta }I+(1-\frac{sin\theta }{\theta })\mathbf{a}{\mathbf{a}}^{\top }+\frac{1-cos\theta }{\theta }{\mathbf{a}}^{\wedge }$$
它的逆为:
$${\mathbf{J}}_l^{-1}=\frac{\theta }{2}cot\frac{\theta }{2}I+(1-\frac{\theta }{2}cot\frac{\theta }{2})\mathbf{a}{\mathbf{a}}^{\top }-\frac{\theta }{2}{\mathbf{a}}^{\wedge }$$

右乘Jacobian仅需要对自变量取负号即可:
$${\mathbf{J}}_r(\mathbf{\varphi })={\mathbf{J}}_l(-\mathbf{\varphi })$$

假定对某个旋转$\mathbf{R}$, 对应的李代数为$\mathbf{\varphi }$, 左乘一个微小旋转, 记作$△\mathbf{R}$, 对应的李代数为$△\varphi$. 在李群上,得到的结果是$△\mathbf{R}\cdot \mathbf{R}$, 而在李代数上, 根据BCH近似, 为${\mathbf{J}}_l^{-1}(\varphi )△\varphi +\varphi$

$$exp(△{\varphi }^{\wedge })exp({\varphi }^{\wedge })=exp((\varphi +{\mathbf{J}}_l^{-1}(\varphi )△\varphi {)}^{\wedge })$$

反之, 如果在李代数上进行加法, 让一个$\varphi$加上$△\varphi$, 可以近似为李群上带左右雅可比的乘法:
$$exp((\varphi +△\varphi {)}^{\wedge })=exp(({\mathbf{J}}_l△\varphi {)}^{\wedge })exp({\varphi }^{\wedge })=exp({\varphi }^{\wedge })exp(({\mathbf{J}}_r△{\varphi }^{\wedge })$$

对于SE(3)亦有类似的BCH近似公式:
$$\begin{array}{rl}exp(△{\xi }^{\wedge })exp({\xi }^{\wedge })& \approx exp(({\mathcal{J}}_l^{-1}△\xi +\xi {)}^{\wedge })\\ exp({\xi }^{\wedge })exp(△{\xi }^{\wedge })& \approx exp(({\mathcal{J}}_r^{-1}△\xi +\xi {)}^{\wedge })\end{array}$$

---

误差与扰动模型

在SLAM问题中, 相机的位置与姿态是需要去估计的量, 位姿由SO(3)上的旋转矩阵或SE(3)上的变换矩阵进行描述. 设相机的位姿为$T$, 它观察到一个世界坐标位于$p$的点, 产生一个观测数据$z$, $z=Tp+w$, 由于观测噪声w的存在, z往往不可能精确地满足z=Tp的关系,所以, 计算理想的观测与实际数据的误差:
$$e=z-Tp$$

假设一共有N个这样的路标点与观测, 于是就有N个上式. 对相机的位姿进行估计, 相当于寻找一个最优的T, 使得整体误差最小化:
$$\underset{T}{\min }J(T)=\sum _{i=1}^N||z_i-T{p}_i|{|}_2^2$$
构建与位姿相关的函数, 然后讨论该函数关于位姿的导数, 以调整当前的估计值.
SLAM十四讲中提到了两种解决方法:

1. 用李代数表示姿态, 然后根据李代数加法来对李代数进行求导 ( A Micro Lie Theory论文里的那一套)
2. 对李群左乘或右乘微小扰动, 然后对该扰动求导, 称为左/右扰动模型

李代数求导:
对一个空间点p进行旋转, 得到$Rp$, 要计算旋转之后的坐标相对于旋转的导数, 不严谨地记为:
$$\frac{\partial (Rp)}{\partial R}$$
由于$SO(3)$没有加法,所以该导数无法按照导数的定义进行计算.设R对应的李代数为$\varphi$, 转而计算:
$$\begin{array}{rl}\frac{\partial (exp({\varphi }^{\wedge })p)}{\partial \varphi }& =\underset{\partial \varphi \to 0}{\lim }\frac{exp((\varphi +\delta \varphi {)}^{\wedge })p-exp({\varphi }^{\wedge })p}{\delta \varphi }\\ & =\underset{\partial \varphi \to 0}{\lim }\frac{exp((J_l\delta \varphi {)}^{\wedge })p-exp({\varphi }^{\wedge })p}{\delta \varphi }\\ & \approx \underset{\partial \varphi \to 0}{\lim }\frac{(I+(J_l\delta \varphi {)}^{\wedge })exp({\varphi }^{\wedge })p-exp({\varphi }^{\wedge })p}{\delta \varphi }\\ & =\underset{\partial \varphi \to 0}{\lim }\frac{(J_l\delta \varphi {)}^{\wedge }exp({\varphi }^{\wedge })p}{\delta \varphi }\\ & =\underset{\partial \varphi \to 0}{\lim }\frac{-(exp({\varphi }^{\wedge })p{)}^{\wedge }{J}_l\delta \varphi }{\delta \varphi }\\ & =-(Rp{)}^{\wedge }{J}_l\end{array}$$

第二行为BCH线性近似, 第三行为泰勒展开, 第五行为向量叉乘前后掉转后添加负号. 故

$$\frac{\partial (Rp)}{\partial \varphi }=-(Rp{)}^{\wedge }{J}_l$$

扰动模型:
对R进行一次$△R$, 这个扰动可以乘在左边也可以乘在右边. 以左扰动为例, 左扰动$△R$对应的李代数为$\phi$.
$$\frac{\partial (Rp)}{\partial \phi }=\underset{\phi \to 0}{\lim }\frac{exp({\phi }^{\wedge })exp({\varphi }^{\wedge })p-exp({\varphi }^{\wedge })p}{\phi }$$

$$\begin{array}{rl}\frac{\partial (Rp)}{\partial \phi }& =\underset{\phi \to 0}{\lim }\frac{exp({\phi }^{\wedge })exp({\varphi }^{\wedge })p-exp({\varphi }^{\wedge })p}{\phi }\\ & \approx \underset{\phi \to 0}{\lim }\frac{(I+{\phi }^{\wedge })exp({\varphi }^{\wedge })p-exp({\varphi }^{\wedge })p}{\phi }\\ & =\underset{\phi \to 0}{\lim }\frac{{\phi }^{\wedge }Rp}{\phi }\\ & =\underset{\phi \to 0}{\lim }\frac{-(Rp{)}^{\wedge }\phi }{\phi }=-(Rp{)}^{\wedge }\end{array}$$

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

假设某空间点p经过一次变换T(对应的李代数为$\xi$), 得到$Tp$. 现在,给T左乘一个扰动$△T=exp(\delta {\xi }^{\wedge })$. 扰动项的李代数为$\delta \xi =[\delta \rho ,\delta \varphi {]}^{\top }$.

$$\begin{array}{rl}\frac{\partial (Tp)}{\partial \delta \xi }& =\underset{\delta \xi \to 0}{\lim }\frac{exp(\delta \xi {)}^{\wedge }exp({\xi }^{\wedge })p-exp({\xi }^{\wedge })p}{\delta \xi }\\ & =\underset{\delta \xi \to 0}{\lim }\frac{(I+\delta {\xi }^{\wedge })exp({\xi }^{\wedge })p-exp({\xi }^{\wedge })p}{\delta \xi }\\ & =\underset{\delta \xi \to 0}{\lim }\frac{\delta {\xi }^{\wedge }exp({\xi }^{\wedge })p}{\delta \xi }\\ & =\underset{\delta \xi \to 0}{\lim }\frac{\left[\begin{array}{cc}\delta {\varphi }^{\wedge }& \delta \rho \\ 0^{\top }& 0\end{array}\right]\left[\begin{array}{c}Rp+t\\ 1\end{array}\right]}{\delta \xi }\\ & =\underset{\delta \xi \to 0}{\lim }\frac{\left[\begin{array}{c}\delta {\varphi }^{\wedge }(Rp+t)+\delta \rho \\ 0\end{array}\right]}{\delta \xi }\\ & =\left[\begin{array}{cc}I& -(Rp+t{)}^{\wedge }\\ 0^{\top }& 0^{\top }\end{array}\right]\underset{=}{△}(Tp{)}^{\odot }\end{array}$$

SO(3)与SE(3)的伴随性质可见于链接

相似变换群与相应李代数
单目相机具有尺度不确定性.
$$p^{\prime }=\left[\begin{array}{cc}sR& t\\ 0^{\top }& 1\end{array}\right]=sRp+t$$
同时作用于p的3个坐标上, 对p进行了一次放缩. 与SO(3),SE(3)相似, 相似变换亦对矩阵乘法构成群, 称为相似变换群Sim(3):
$$Sim(3)=\left\{S=\left[\begin{array}{cc}sR& t\\ 0^{\top }& 1\end{array}\right]\in {\mathbb{R}}^{4\times 4}\right\}$$
李代数$\mathfrak{sim}(3)$元素为7维向量. 前6维与$\mathfrak{se}(3)$相同, 最后多了一项$\sigma$.

$$\mathfrak{sim}(3)=\left\{\zeta |\zeta =\left[\begin{array}{c}\rho \\ \varphi \\ \sigma \end{array}\right]\in {\mathbb{R}}^7,{\zeta }^{\wedge }=\left[\begin{array}{cc}\sigma I+{\varphi }^{\wedge }& \rho \\ 0^{\top }& 0\end{array}\right]\in {\mathbb{R}}^{4\times 4}\right\}$$
$\zeta$的指数映射为
$$exp({\zeta }^{\wedge })=\left[\begin{array}{cc}{e}^{\sigma }exp({\varphi }^{\wedge })& J_s\rho \\ 0^{\top }& 1\end{array}\right]$$
其中$J_s$的形式为:
$$J_s=\frac{e^{\sigma }-1}{\sigma }I+\frac{\sigma e^{\sigma }sin\theta +(1-e^{\sigma }cos\theta )\theta {\mathbf{a}}^{\wedge }}{{\sigma }^2+{\theta }^2}+(\frac{e^{\sigma }-1}{\sigma }-\frac{(e^{\sigma }cos\theta -1)\sigma +(e^{\sigma }sin\theta )\theta }{{\sigma }^2+{\theta }^2}){\mathbf{a}}^{\wedge }{\mathbf{a}}^{\wedge }$$

旋转部分与SO(3)一致, 平移部分在$\mathfrak{se}(3)$中需要乘以一个雅可比$\mathcal{J}$, 而相似变换的雅可比更加复杂一些. 对于尺度因子, 可以看到李群中的s即为李代数中的$\sigma$的指数函数.

$Sim(3)$的扰动模型:
给予Sp左侧一个小扰动$exp({\zeta }^{\wedge })$,并求Sp对于扰动的导数, 因为Sp是4维的齐次坐标,$\zeta$是7维向量.该导数可以为$4\times 7$的Jacobian.

$$\frac{\partial Sp}{\partial \zeta }=\left[\begin{array}{ccc}I& -q^{\wedge }& q\\ 0^{\top }& 0^{\top }& 0\end{array}\right]$$