《视觉SLAM十四讲》读书笔记（二）

李群与李代数

引入李群与李代数的原因：旋转矩阵自身是带有约束的（正交且行列式为1）.它们作为优化变量时，会引入额外的约束，使优化变得困难。通过李群——李代数间的转换关系，我们希望把位姿估计变成无约束的优化问题，简化求解方式。

4.1 李群与李代数基础

三维旋转矩阵构成了特殊正交群SO(3)
变换矩阵构成了特殊欧式群SE(3)
$$\begin{array}{l}\mathrm{S}\mathrm{O}(3)=\left\{\mathbf{R}\in {\mathbb{R}}^{3\times 3}|\mathbf{R}{\mathbf{R}}^{\mathrm{T}}=\mathbf{I},\det (\mathbf{R})=1\right\}\\ \mathrm{S}\mathrm{E}(3)=\left\{\mathbf{T}=\left[\begin{array}{l}\mathbf{R}\mathbf{t}\\ 0^{\mathrm{T}}1\end{array}\right]\in {\mathbb{R}}^{4\times 4}|\mathbf{R}\in \mathrm{S}\mathrm{O}(3),\mathbf{t}\in {\mathbb{R}}^3\right\}\end{array}$$
乘法对应着旋转或变换的集合，两个旋转矩阵相乘表示做了两次旋转。对于这种只有一个（良好的，封闭的）运算的集合，我们称之为群。

4.1.1 群

群的定义：群是一种集合加上一种运算的代数结构。
运算对象：集合记作$A$。
运算符：运算记作$\cdot$
群的标记：$G=(A,\cdot )$

群对运算的要求（封结幺逆 = 丰俭由你）：

1. 封闭性： $\forall a_1,a_2\in A,a_1\cdot a_2\in A$.

2. 结合律: $\forall a_1,a_2,a_3\in A,\left(a_1\cdot a_2\right)\cdot a_3=a_1\cdot \left(a_2\cdot a_3\right)$.

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$.

李群的定义：李群是指具有连续（光滑）性质的群。

参考：旋转之十一 - SE(3) 和 se(3)

4.1.2 李代数的引出

$\mathbf{R}$是某个相机的旋转，它会随时间连续地变化，即为时间的函数：$\mathbf{R}(t)$，有：
$$\mathbf{R}(t)\mathbf{R}(t{)}^T=\mathbf{I}\text{\hspace{1em}(4.5)}$$
在等式两边对时间求导，得到
$$\dot{\mathbf{R}}(t)\mathbf{R}(t{)}^{\mathrm{T}}+\mathbf{R}(t)\dot{\mathbf{R}}(t{)}^{\mathrm{T}}=0$$
整理得
$$\dot{\mathbf{R}}(t)\mathbf{R}(t{)}^{\mathrm{T}}=-{\left(\dot{\mathbf{R}}(t)\mathbf{R}(t{)}^{\mathrm{T}}\right)}^{\mathrm{T}}\text{\hspace{1em}(4.6)}$$
$\dot{\mathbf{R}}(t)\mathbf{R}(t{)}^{\mathrm{T}}$是一个反对称矩阵。${}^{\wedge }$符号可以将一个向量转成反对称矩阵。反对称矩阵，也能找到唯一与之对应的向量。
$${\mathbf{a}}^{\wedge }=\mathbf{A}=\left[\begin{array}{ccc}0& -a_3& a_2\\ a_3& 0& -a_1\\ -a_2& a_1& 0\end{array}\right],{\mathbf{A}}^{\vee }=\mathbf{a}\text{\hspace{1em}(4.7)}$$
于是，由于$\dot{\mathbf{R}}(t)\mathbf{R}(t{)}^{\mathrm{T}}$是一个反对称矩阵，我们可以找到一个三维向量$\mathbf{\varphi }(t)\in {\mathbb{R}}^3$与之对应：
$$\dot{\mathbf{R}}(t)\mathbf{R}(t{)}^{\mathrm{T}}=\mathbf{\varphi }(t{)}^{\wedge }$$

等式两边右乘 $\mathbf{R}(t)$, 由于 $\mathbf{R}$ 为正交阵, 可以消去左边的${\mathbf{R}}^T$
$$\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)\text{\hspace{1em}(4.8)}$$
旋转矩阵的导数：只需左乘一个$\mathbf{\varphi }(t{)}^{\wedge }$即可。考虑$t_0=0$时，设此时旋转矩阵为$\mathbf{R}(0)=\mathbf{I}$。按照导数定义，可以把$\mathbf{R}(t)$在$t=0$附近进行一阶泰勒展开：
$$\begin{gathered} \mathbf{R}(t)\approx \mathbf{R}\left(t_0\right)+\dot{\mathbf{R}}\left(t_0\right)\left(t-t_0\right) \\ =\mathbf{I}+\mathbf{\varphi }{\left(t_0\right)}^{\wedge }\mathbf{R}(t_0)(t) \\ =\mathbf{I}+\mathbf{\varphi }{\left(t_0\right)}^{\wedge }(t)\text{\hspace{1em}(4.9)} \end{gathered}$$
$\mathbf{\varphi }$的定义：在SO(3)原点附近的正切空间，它反映了$\mathbf{R}$的导数性质

在$t_0$附近，设$\mathbf{\varphi }$保持为常数$\mathbf{\varphi }(t_0)={\mathbf{\varphi }}_0$。根据式(4.8)，有
$$\dot{\mathbf{R}}(t)=\mathbf{\varphi }{\left(t_0\right)}^{\wedge }\mathbf{R}(t)={\mathbf{\varphi }}_0^{\wedge }\mathbf{R}(t)$$
上式是一个关于$\mathbf{R}$的微分方程，而且有初始值$\mathbf{R}(0)=\mathbf{I}$，解得
$$\mathbf{R}(t)=\exp \left({\mathbf{\varphi }}_0^{\wedge }t\right)\text{\hspace{1em}(4.10)}$$
式(4.10)推导：

$$\dot{\mathbf{R}}(t)=\mathbf{\varphi }{\left(t_0\right)}^{\wedge }\mathbf{R}(t)={\mathbf{\varphi }}_0^{\wedge }\mathbf{R}(t)=\frac{d\mathbf{R}(t)}{dt}$$
方法1. 转为一阶齐次线性微分方程标准形式
$$\frac{d\mathbf{R}(t)}{dt}-{\mathbf{\varphi }}_0^{\wedge }\mathbf{R}(t)=0$$
得到通解形式：
$$\mathbf{R}(t)=\exp \left({\mathbf{\varphi }}_0^{\wedge }t\right)$$
方法2. 或者逐步推导，由
$$\frac{d\mathbf{R}(t)}{R(t)}={\mathbf{\varphi }}_0^{\wedge }dt$$
$$ln|\mathbf{R}(t)|=\int {\mathbf{\varphi }}_0^{\wedge }dt+C$$
$$|\mathbf{R}(t)|=e^{{\mathbf{\varphi }}_0^{\wedge }dt+C}=e^C{e}^{{\mathbf{\varphi }}_0^{\wedge }dt}=\mathbf{R}(0)e^{{\mathbf{\varphi }}_0^{\wedge }dt}$$
由于初始值$\mathbf{R}(0)=\mathbf{I}$，因此同样可以得到结果
$$\mathbf{R}(t)=\exp \left({\mathbf{\varphi }}_0^{\wedge }t\right)$$

总结

1. 给定某时刻的$\mathbf{R}$，我们就能求得一个$\mathbf{\varphi }$，它描述了$\mathbf{R}$在局部的倒数关系。与$\mathbf{R}$对应的$\mathbf{\varphi }$正是对应到$SO(3)$的李代数$\mathfrak{s}\mathfrak{o}(3)$
2. 李群与李代数间的指数/对数映射，给出了向量$\mathbf{\varphi }$与$\mathbf{R}$一一对应的关系。
3. 有朋友可能还在困惑，到底$\mathbf{\varphi }$怎么求取。实际上，利用式(4.9)，只需要知道$\mathbf{R}(t)$和$\mathbf{R}(t_0)$即可求得$\mathbf{\varphi }$。

4.1.3 李代数的定义

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

1. 封闭性： $\forall \mathbf{X},\mathbf{Y}\in \mathbb{V},[\mathbf{X},\mathbf{Y}]\in \mathbb{V}.$

2. 双线性： $\forall \mathbf{X},\mathbf{Y},\mathbf{Z}\in \mathbb{V},a,b\in \mathbb{F}$ , 有
   $[a\mathbf{X}+b\mathbf{Y},\mathbf{Z}]=a[\mathbf{X},\mathbf{Z}]+b[\mathbf{Y},\mathbf{Z}],[\mathbf{Z},a\mathbf{X}+b\mathbf{Y}]=a[\mathbf{Z},\mathbf{X}]+b[\mathbf{Z},\mathbf{Y}]$

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

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

其中二元运算被称为李括号。作为例子，三维向量${\mathbb{R}}^3$上定义的叉积$\times$是一种李括号，因此$\mathfrak{g}=({\mathbb{R}}^3,\mathbb{R},\times )$构成了一种李代数。

4.1.4 李代数$\mathfrak{s}\mathfrak{o}(3)$

前面提到的$\mathbf{\varphi }$，事实上是一种李代数。SO(3)对应的李代数是定义在${\mathbb{R}}^3$上的向量，我们记作$\mathbf{\varphi }$。每个$\mathbf{\varphi }$都可以生成一个反对称矩阵：
$$\mathbf{\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}\text{\hspace{1em}(4.11)}$$
在此定义下，两个向量${\mathbf{\varphi }}_1$,${\mathbf{\varphi }}_2$的李括号为：
$$\left[{\mathbf{\varphi }}_1,{\mathbf{\varphi }}_2\right]={\left({\mathbf{\Phi }}_1{\mathbf{\Phi }}_2-{\mathbf{\Phi }}_2{\mathbf{\Phi }}_1\right)}^{\vee }\text{\hspace{1em}(4.12)}$$
由于向量$\mathbf{\varphi }$与反对称矩阵是一一 对应的, 在不引起歧义的情况下, 就说 $\mathfrak{s}\mathfrak{o}(3)$ 的元素是三维向量或者三维反对称矩阵, 不加区别:
$$\mathfrak{s}\mathfrak{o}(3)=\left\{\varphi \in {\mathbb{R}}^3,\mathbf{\Phi }={\varphi }^{\wedge }\in {\mathbb{R}}^{3\times 3}\right\}\text{\hspace{1em}(4.13)}$$
$\mathfrak{s}\mathfrak{o}(3)$是一个由三维向量组成的集合, 每个向量对应一个反对称矩阵, 可以用于表达旋转矩阵的导数。它与 $\mathrm{S}\mathrm{O}(3)$ 的关系由指数映射给定:
$$\mathbf{R}=\exp \left({\mathbf{\varphi }}^{\wedge }\right)\text{\hspace{1em}(4.14)}$$
由于$\varphi$与反对称矩阵关系很紧密，在不引起歧义的情况下，就说$\mathfrak{s}\mathfrak{o}(3)$的元素是3维向量或者3维反对称矩阵，不加区别：
$$\mathfrak{s}\mathfrak{o}(3)=\left\{\Phi ={\varphi }^{\wedge }\in {\mathbb{R}}^{3\times 3}\mid \varphi \in {\mathbb{R}}^3\right\}$$

4.1.5 李代数$\mathfrak{s}\mathfrak{e}(3)$

对于SE(3)，它也有对应的李代数$\mathfrak{s}\mathfrak{e}(3)$。$\mathfrak{s}\mathfrak{e}(3)$位于${\mathbb{R}}^6$空间中：
$$\mathfrak{s}\mathfrak{e}(3)=\left\{\mathbf{\xi }=\left[\begin{array}{l}\rho \\ \varphi \end{array}\right]\in {\mathbb{R}}^6,\mathbf{\rho }\in {\mathbb{R}}^3,\varphi \in \mathfrak{s}\mathfrak{o}(3),{\mathbf{\xi }}^{\wedge }=\left[\begin{array}{cc}{\varphi }^{\wedge }& \rho \\ 0^{\mathrm{T}}& 0\end{array}\right]\in {\mathbb{R}}^{4\times 4}\right\}$$
我们把每个 $\mathfrak{s}\mathfrak{e}(3)$ 元素记作 $\mathbf{\xi }$ , 它是一个六维向量。前三维为平移（但含义与变换矩阵中的平移 不同, 分析见后 ), 记作 $\rho$ ; 后三维为旋转, 记作 $\varphi$ , 实质上是 $\mathfrak{s}\mathfrak{o}(3)$ 元素 ${}^{\odot }$ 。同时, 我们拓展了 ${}^{\wedge }$ 符号的含义。在 $\mathfrak{s}\mathfrak{e}(3)$ 中, 同样使用 ${}^{\wedge }$ 符号, 将一个六维向量转换成四维矩阵, 但这里不再表示反对称:
$${\mathbf{\xi }}^{\wedge }=\left[\begin{array}{ll}{\varphi }^{\wedge }& \rho \\ 0^{\mathrm{T}}& 0\end{array}\right]\in {\mathbb{R}}^{4\times 4}$$
我们仍使用 ${}^{\wedge }$ 和 $\vee$ 符号指代 “从向量到矩阵”和 “从矩阵到向量”的关系, 以保持和 $\mathfrak{s}\mathfrak{o}(3)$ 上 的一致性。它们依旧是一一对应的。读者可以简单地把 $\mathfrak{s}\mathfrak{e}(3)$ 理解成 “由一个平移加上一个 $\mathfrak{s}\mathfrak{o}(3)$ 元素构成的向量” ( 尽管这里的 $\rho$ 还不直接是平移)。同样, 李代数 $\mathfrak{s}\mathfrak{e}(3)$ 也有类似于 $\mathfrak{s}\mathfrak{o}(3)$ 的李括号：
$$\left[{\mathbf{\xi }}_1,{\mathbf{\xi }}_2\right]={\left({\mathbf{\xi }}_1^{\wedge }{\mathbf{\xi }}_2^{\wedge }-{\mathbf{\xi }}_2^{\wedge }{\mathbf{\xi }}_1^{\wedge }\right)}^{\vee }$$

4.2 指数与对数映射

4.2.1 SO(3)上的指数映射

首先，回忆任意矩阵的指数映射。它可以写成一个泰勒展开，但是只有在收敛的情况下才会有结果，其结果仍是一个矩阵。
$$\exp (\mathbf{A})=\sum _{n=0}^{\infty }\frac{1}{n!}{\mathbf{A}}^n\text{\hspace{1em}(4.18)}$$
同样地，对$\mathfrak{s}\mathfrak{o}(3)$中任意一元素$\varphi$，我们亦可按此方式定义它的指数映射：
$$\exp ({\varphi }^{\wedge })=\sum _{n=0}^{\infty }\frac{1}{n!}({\varphi }^{\wedge }{)}^n\text{\hspace{1em}(4.19)}$$
现在我们来仔细看看它的含义。由于$\varphi$是三维向量，我们可以定义它的模长和它的方向，分别记作$\theta$和$\mathbf{a}$（$|\mathbf{a}|=1$），那么按照上式，可以推出式(4.20)，注意中间使用了上面讲到了两个反对称矩阵的性质：

反对称矩阵有一些重要的性质，重点包括以下两条：
$$\varphi {\varphi }^T={\varphi }^{\wedge }{\varphi }^{\wedge }+\Vert \varphi {\Vert }^2{\mathbf{I}}_{3\times 3}$$
当$\varphi$为单位向量时，进而有：
$${\varphi }^{\wedge }{\varphi }^{\wedge }=\left[\begin{array}{ccc}-a_2^2-a_3^2& a_1{a}_2& a_1{a}_3\\ a_1{a}_2& -a_1^2-a_3^2& a_2{a}_3\\ a_1{a}_3& a_2{a}_3& -a_1^2-a_2^2\end{array}\right]=\varphi {\varphi }^T-\mathbf{I}$$
以及
$${\varphi }^{\wedge }{\varphi }^{\wedge }{\varphi }^{\wedge }={\varphi }^{\wedge }(\varphi {\varphi }^T-\mathbf{I})=-{\varphi }^{\wedge }$$

$$\begin{array}{rl}\exp \left({\varphi }^{\wedge }\right)& =\exp \left(\theta {\mathbf{a}}^{\wedge }\right)=\sum _{n=0}^{\infty }\frac{1}{n!}{\left(\theta {\mathbf{a}}^{\wedge }\right)}^n\\ & =\mathbf{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{\left({\mathbf{a}}^{\wedge }\right)}^4+\cdots \\ & =\mathbf{a}{\mathbf{a}}^{\mathrm{T}}-{\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{\left({\mathbf{a}}^{\wedge }\right)}^2+\cdots \\ & =\mathbf{a}{\mathbf{a}}^{\mathrm{T}}+\underset{\sin \theta }{\underbrace{\left(\theta -\frac{1}{3!}{\theta }^3+\frac{1}{5!}{\theta }^5-\cdots \right)}}{\mathbf{a}}^{\wedge }-\underset{\cos \theta }{\underbrace{\left(1-\frac{1}{2!}{\theta }^2+\frac{1}{4!}{\theta }^4-\cdots \right)}}{\mathbf{a}}^{\wedge }{\mathbf{a}}^{\wedge }\\ & ={\mathbf{a}}^{\wedge }{\mathbf{a}}^{\wedge }+\mathbf{I}+\sin \theta {\mathbf{a}}^{\wedge }-\cos \theta {\mathbf{a}}^{\wedge }{\mathbf{a}}^{\wedge }\\ & =(1-\cos \theta ){\mathbf{a}}^{\wedge }{\mathbf{a}}^{\wedge }+\mathbf{I}+\sin \theta {\mathbf{a}}^{\wedge }\\ & =\cos \theta \mathbf{I}+(1-\cos \theta )\mathbf{a}{\mathbf{a}}^{\mathrm{T}}+\sin \theta {\mathbf{a}}^{\wedge }\end{array}\text{\hspace{1em}(4.20)}$$

式(4.20)详细推导过程（实际上也是罗德里格斯公式的推导过程）：

先再次观察 so(3)，so(3) 中的元素可以看成矩阵，这个矩阵为反对称矩阵，我们用$\Omega$来表示:

$$\Omega =[\mathbf{\omega }{]}_x=\left[\begin{array}{ccc}0& -{\omega }_3& {\omega }_2\\ {\omega }_3& 0& -{\omega }_1\\ -{\omega }_2& {\omega }_1& 0\end{array}\right]$$

这个矩阵可以看成是
$$E_1=\left[\begin{array}{ccc}0& 0& 0\\ 0& 0& -1\\ 0& 1& 0\end{array}\right],E_2=\left[\begin{array}{ccc}0& 0& 1\\ 0& 0& 0\\ -1& 0& 0\end{array}\right],E_3=\left[\begin{array}{ccc}0& -1& 0\\ 1& 0& 0\\ 0& 0& 0\end{array}\right]$$
组合而成, $\Omega ={\omega }_1{E}_1+{\omega }_2{E}_2+{\omega }_3{E}_3$ 也就是:
$$\begin{array}{rl}& \mathbf{\omega }\in R^3\\ {\omega }_1{E}_1+{\omega }_2{E}_2& +{\omega }_3{E}_3\in so(3)\end{array}$$
观察 $\mathbf{\omega }=\left[\begin{array}{l}{\omega }_1\\ {\omega }_2\\ {\omega }_3\end{array}\right]$ , 作为向量, 它的模长:
$$\begin{array}{c}{\theta }^2={\omega }_1^2+{\omega }_2^2+{\omega }_3^2\\ {\theta }^2=\left[\begin{array}{lll}{\omega }_1& {\omega }_2& {\omega }_3\end{array}\right]\cdot \left[\begin{array}{c}{\omega }_1\\ {\omega }_2\\ {\omega }_3\end{array}\right]={\omega }^T\mathbf{\omega }\end{array}$$
计算 ${\Omega }^n,n=1,2,\dots$ :

$$\begin{array}{c}{\Omega }^0=I\\ \Omega =\left[\begin{array}{ccc}0& -{\omega }_3& {\omega }_2\\ {\omega }_3& 0& -{\omega }_1\\ -{\omega }_2& {\omega }_1& 0\end{array}\right]\\ {\Omega }^2=\left[\begin{array}{ccc}0& -{\omega }_3& {\omega }_2\\ {\omega }_3& 0& -{\omega }_1\\ -{\omega }_2& {\omega }_1& 0\end{array}\right]\left[\begin{array}{ccc}0& -{\omega }_3& {\omega }_2\\ {\omega }_3& 0& -{\omega }_1\\ -{\omega }_2& {\omega }_1& 0\end{array}\right]=\left[\begin{array}{ccc}-{\omega }_3^2-{\omega }_2^2& {\omega }_1{\omega }_2& {\omega }_1{\omega }_3\\ {\omega }_2{\omega }_1& -{\omega }_3^2-{\omega }_1^2& {\omega }_2{\omega }_3\\ {\omega }_3{\omega }_1& {\omega }_2{\omega }_3& -{\omega }_1^2-{\omega }_2^2\end{array}\right]\end{array}$$
由于我们知道反对称矩阵的通用性质：
$$\varphi {\varphi }^T={\varphi }^{\wedge }{\varphi }^{\wedge }+\Vert \varphi {\Vert }^2{\mathbf{I}}_{3\times 3}$$
$${\varphi }^{\wedge }{\varphi }^{\wedge }=\varphi {\varphi }^T-\Vert \varphi {\Vert }^2{\mathbf{I}}_{3\times 3}$$
左右两边左乘一个${\varphi }^{\wedge }$：
$${\varphi }^{\wedge }{\varphi }^{\wedge }{\varphi }^{\wedge }={\varphi }^{\wedge }\varphi {\varphi }^T-\Vert \varphi {\Vert }^2{\varphi }^{\wedge }$$
由于${\varphi }^{\wedge }\varphi =0$，因此得
$${\varphi }^{\wedge }{\varphi }^{\wedge }{\varphi }^{\wedge }=-\Vert \varphi {\Vert }^2{\varphi }^{\wedge }$$
由于有$||\mathbf{\omega }|{|}^2=\left({\omega }_1^2+{\omega }_2^2+{\omega }_3^2\right)={\theta }^2$。让$\Omega$代入上式，所以有 ${\Omega }^3=-{\theta }^2\Omega$ ，继续：
继续让式子左右两边左乘一个${\varphi }^{\wedge }$：
$${\varphi }^{\wedge }{\varphi }^{\wedge }{\varphi }^{\wedge }{\varphi }^{\wedge }=-\Vert \varphi {\Vert }^2{\varphi }^{\wedge }{\varphi }^{\wedge }$$
$${\varphi }^{\wedge }{\varphi }^{\wedge }{\varphi }^{\wedge }{\varphi }^{\wedge }{\varphi }^{\wedge }=-\Vert \varphi {\Vert }^2{\varphi }^{\wedge }{\varphi }^{\wedge }{\varphi }^{\wedge }=\Vert \varphi {\Vert }^4{\varphi }^{\wedge }$$
如此类推可得到
$$\begin{array}{c}{\Omega }^4=\Omega {\Omega }^3=-{\theta }^2{\Omega }^2\\ {\Omega }^5={\Omega }^2{\Omega }^3=-{\Omega }^2{\theta }^2\Omega =-{\theta }^2{\Omega }^3={\theta }^4\Omega \\ \cdots \end{array}$$
所以有
$$\begin{array}{rl}{\theta }^2& ={\mathbf{\omega }}^T\mathbf{\omega }\\ {\Omega }^{2i+1}& =(-1{)}^i{\theta }^{2i}\Omega \\ {\Omega }^{2i+2}& =(-1{)}^i{\theta }^{2i}{\Omega }^2\end{array}$$
展开：
$$\begin{array}{rl}\exp (\Omega )& =\sum _{n=0}^{\infty }\frac{1}{n!}{\Omega }^n\\ & =I+\left(\sum _{i=0}^{\infty }\frac{(-1{)}^i{\theta }^{2i}}{(2i+1)!}\right)\Omega +\left(\sum _{i=0}^{\infty }\frac{(-1{)}^i{\theta }^{2i}}{(2i+2)!}\right){\Omega }^2\\ & =I+\left(1-\frac{{\theta }^2}{3!}+\frac{{\theta }^4}{5!}+\cdots \right)\Omega +\left(\frac{1}{2!}-\frac{{\theta }^2}{4!}+\frac{{\theta }^4}{6!}+\cdots \right){\Omega }^2\end{array}$$
有欧拉公式:
$$\begin{array}{l}\sin x=x-\frac{x^3}{3!}+\frac{x^5}{5!}-\frac{x^7}{7!}+\cdots \\ \cos x=1-\frac{x^2}{2!}+\frac{x^4}{4!}-\frac{x^6}{6!}+\cdots \end{array}$$
所以：
$$\exp (\Omega )=I+\left(\frac{\sin \theta }{\theta }\right)\Omega +\left(\frac{1-\cos \theta }{{\theta }^2}\right){\Omega }^2$$
如果我们需要更严密一点的话 $\mathrm{expso}(3)\to \mathrm{S}\mathrm{O}(3)$ :

$$\begin{array}{c}\Omega \in so(3)\\ \theta =\sqrt{-\frac{1}{2}\mathrm{tr}\left({\Omega }^2\right)}\\ \exp (\Omega )=\{\begin{array}{ll}I& \theta \simeq 0\\ I+\left(\frac{\sin \theta }{\theta }\right)\Omega +\left(\frac{1-\cos \theta }{{\theta }^2}\right){\Omega }^2& \theta \ne 0\end{array}\end{array}$$
同样 $\mathrm{S}\mathrm{O}(3)\to \mathrm{s}\mathrm{o}(3)$ ，也就是旋转矩阵到旋转向量。如果也更严密一点，首先就是我们限制旋转角 店在 $[0,\pi ]$ 间, 毕竟可能要用$acos$函数和保证一对一, 否则角度加上$2\pi$ 计算出来的 $\sin \cos$ 总是 相等，其次就是当 $\theta$ 足够小的时候的处理：
$$\begin{array}{c}R\in SO(3)\\ \theta =\arccos \left(\frac{1}{2}[\mathrm{tr}(R)-1]\right)\\ \log (R)=\{\begin{array}{ll}\theta \left[\begin{array}{ccc}0& -1& 1\\ 1& 0& -1\\ -1& 1& 0\end{array}\right]\theta \simeq 0& \\ \frac{\theta }{\sin \theta }\left(R-R^T\right)& \theta \ne 0\end{array}\end{array}$$

最后，得到罗德里格斯公式
$$\exp \left(\theta {\mathbf{a}}^{\wedge }\right)=\cos \theta \mathbf{I}+(1-\cos \theta )\mathbf{a}{\mathbf{a}}^{\mathrm{T}}+\sin \theta {\mathbf{a}}^{\wedge }\text{\hspace{1em}(4.22)}$$
这表明, $\mathfrak{s}\mathfrak{o}(3)$ 实际上就是 由所谓的旋转向量组成的空间, 而指数映射即罗德里格斯公式。通过它们, 我们把 $\mathfrak{s}\mathfrak{o}(3)$ 中任意一个向量对应到了一个位于 $\mathrm{S}\mathrm{O}(3)$ 中的旋转矩阵。反之, 如果定义对数映射, 也能把 $\mathrm{S}\mathrm{O}(3)$ 中的元素对应到 $\mathfrak{s}\mathfrak{o}(3)$ 中:
$$\varphi =\ln (\mathbf{R}{)}^{\vee }={\left(\sum _{n=0}^{\infty }\frac{(-1{)}^n}{n+1}(\mathbf{R}-\mathbf{I}{)}^{n+1}\right)}^{\vee }\text{\hspace{1em}(4.23)}$$
和指数映射医院，我们没必要直接用泰勒展开计算对数映射，只需要使用常规的旋转矩阵转旋转向量公式即可：
转角：
$$\begin{array}{rl}\mathrm{tr}(\mathbf{R})=& \cos \theta \mathrm{tr}(\mathbf{I})+(1-\cos \theta )\mathrm{tr}\left(\mathbf{n}{\mathbf{n}}^{\mathrm{T}}\right)+\sin \theta \mathrm{tr}\left({\mathbf{n}}^{\wedge }\right)\\ =& 3\cos \theta +(1-\cos \theta )\\ =& 1+2\cos \theta \end{array}\text{\hspace{1em}(3.16)}$$
$$\theta =\arccos \frac{\mathrm{tr}(\mathbf{R})-1}{2}\text{\hspace{1em}(3.17)}$$
转轴：
转轴$\mathbf{n}$是矩阵$\mathbf{R}$特征值1对应的特征向量。

如果我们把旋转角度固定在$\pm \pi$之间，那么李群和李代数元素是一一对应的。

4.2.2 SE(3)上的指数映射

$\mathfrak{s}\mathfrak{e}(3)$上的指数映射形式：
$$\begin{array}{rl}\exp \left({\mathbf{\xi }}^{\wedge }\right)& =\left[\begin{array}{cc}{\sum }_{n=0}^{\infty }\frac{1}{n!}{\left({\varphi }^{\wedge }\right)}^n& {\sum }_{n=0}^{\infty }\frac{1}{(n+1)!}{\left({\varphi }^{\wedge }\right)}^n\rho \\ 0^{\mathrm{T}}& 1\end{array}\right]\\ & \triangleq \left[\begin{array}{cc}\mathbf{R}& \mathbf{J}\mathbf{\rho }\\ 0^{\mathrm{T}}& 1\end{array}\right]=\mathbf{T}\end{array}\text{\hspace{1em}(4.25)}$$
只要有一点耐心, 可以照着 $\mathfrak{s}\mathfrak{o}(3)$ 上的做法推导, 把 $\exp$ 进行泰勒展开推导此式。令 $\varphi =\theta \mathbf{a}$ , 其中 $\mathbf{a}$ 为单位向量, 则
$$\begin{array}{rl}\sum _{n=0}^{\infty }\frac{1}{(n+1)!}{\left({\mathbf{\varphi }}^{\wedge }\right)}^n& =\mathbf{I}+\frac{1}{2!}\theta {\mathbf{a}}^{\wedge }+\frac{1}{3!}{\theta }^2{\left({\mathbf{a}}^{\wedge }\right)}^2+\frac{1}{4!}{\theta }^3{\left({\mathbf{a}}^{\wedge }\right)}^3+\frac{1}{5!}{\theta }^4{\left({\mathbf{a}}^{\wedge }\right)}^4\cdots \\ & =\frac{1}{\theta }\left(\frac{1}{2!}{\theta }^2-\frac{1}{4!}{\theta }^4+\cdots \right)\left({\mathbf{a}}^{\wedge }\right)+\frac{1}{\theta }\left(\frac{1}{3!}{\theta }^3-\frac{1}{5}{\theta }^5+\cdots \right){\left({\mathbf{a}}^{\wedge }\right)}^2+\mathbf{I}\\ & =\frac{1}{\theta }(1-\cos \theta )\left({\mathbf{a}}^{\wedge }\right)+\frac{\theta -\sin \theta }{\theta }\left(\mathbf{a}{\mathbf{a}}^{\mathrm{T}}-\mathbf{I}\right)+\mathbf{I}\\ & =\frac{\sin \theta }{\theta }\mathbf{I}+\left(1-\frac{\sin \theta }{\theta }\right)\mathbf{a}{\mathbf{a}}^{\mathrm{T}}+\frac{1-\cos \theta }{\theta }{\mathbf{a}}^{\wedge }\\ & \stackrel{\text{ def }}{=}\mathbf{J}\end{array}\text{\hspace{1em}(4.26)}$$

李群和李代数的定义与相互转换关系总结

4.3 李代数求导与扰动模型

4.3.1 BCH公式与近似形式

高等数学告诉我们，如果有标量$A$和$B$，有下式：
$$\ln \left(\exp \left(A\right)\exp \left(B\right)\right)=A+B$$
但是，该式在矩阵时并不成立。两个李代数指数映射乘积的完整形式，由Baker-Campbell-Hausdorff公式（BCH公式）给出：
$$\ln (\exp (\mathbf{A})\exp (\mathbf{B}))=\mathbf{A}+\mathbf{B}+\frac{1}{2}[\mathbf{A},\mathbf{B}]+\frac{1}{12}[\mathbf{A},[\mathbf{A},\mathbf{B}]]-\frac{1}{12}[\mathbf{B},[\mathbf{A},\mathbf{B}]]+\cdots \text{\hspace{1em}(4.29)}$$
其中$[]$为李括号。特别地，考虑$\mathfrak{s}\mathfrak{o}(3)$上地李代数$\ln {\left(\exp \left({\varphi }_1^{\wedge }\right)\exp \left({\varphi }_2^{\wedge }\right)\right)}^{\vee }$，当${\varphi }_1^{\wedge }$或${\varphi }_2^{\wedge }$为小量时，小量二次以上的项都可以被忽略。此时，BCH拥有线性近似表达：
$$\ln {\left(\exp \left({\varphi }_1^{\wedge }\right)\exp \left({\varphi }_2^{\wedge }\right)\right)}^{\vee }\approx \{\begin{array}{ll}{J}_l{\left({\varphi }_2\right)}^{-1}{\varphi }_1+{\varphi }_2& \text{ 当 }{\varphi }_1\text{ 为小量, }\\ J_r{\left({\varphi }_1\right)}^{-1}{\varphi }_2+{\varphi }_1& \text{ 当 }{\varphi }_2\text{ 为小量. }\end{array}\text{\hspace{1em}(4.30)}$$
于是，李代数在BCH近似下，分成了左乘近似和右乘近似两种。其中，左乘BCH近似雅可比$J_l$为：
$${\mathbf{J}}_l(\theta )=\mathbf{J}=\frac{\sin \theta }{\theta }\mathbf{I}+\left(1-\frac{\sin \theta }{\theta }\right)\mathbf{a}{\mathbf{a}}^{\mathrm{T}}+\frac{1-\cos \theta }{\theta }{\mathbf{a}}^{\wedge }\text{\hspace{1em}(4.31)}$$
它的逆为
$${\mathbf{J}}_l(\theta {)}^{-1}=\frac{\theta }{2}\cot \frac{\theta }{2}\mathbf{I}+\left(1-\frac{\theta }{2}\cot \frac{\theta }{2}\right)\mathbf{a}{\mathbf{a}}^{\mathrm{T}}-\frac{\theta }{2}{\mathbf{a}}^{\wedge }\text{\hspace{1em}(4.32)}$$
其中$\cot ()$为余切函数。

而右乘雅可比仅需要对自变量取负号即可:
$$J_r(\varphi )=J_l(-\varphi )\text{\hspace{1em}(4.33)}$$
BCH近似的意义：假定对某个旋转$\mathbf{R}$，对应的李代数为$\mathbf{\varphi }$。我们给它左乘一个微小旋转，记作$\Delta \mathbf{R}$，对应的李代数为$\Delta \mathbf{\varphi }$。那么，在李群上，得到的结果就是$\Delta \mathbf{R}\cdot \mathbf{R}$，而在李代数上，根据BCH近似，为$\varphi +J_l^{-1}(\varphi )\Delta \varphi$。合并起来，可以简单地写成：
$$\exp \left(\Delta {\varphi }^{\wedge }\right)\exp \left({\varphi }^{\wedge }\right)=\exp \left({\left(\varphi +J_l^{-1}(\varphi )\Delta \varphi \right)}^{\wedge }\right)\text{\hspace{1em}(4.34)}$$
反之, 如果我们在李代数上进行加法, 让一个 $\varphi$ 加上 $\Delta \varphi$ , 那么可以近似为李群上带左右雅可比的乘法（尤其需要注意左乘雅可比和右乘雅可比是不同的）:
$$\exp \left((\varphi +\Delta \varphi {)}^{\wedge }\right)=\exp \left({\left({\mathbf{J}}_l\Delta \varphi \right)}^{\wedge }\right)\exp \left({\mathbf{\varphi }}^{\wedge }\right)=\exp \left({\mathbf{\varphi }}^{\wedge }\right)\exp \left({\left({\mathbf{J}}_r\Delta \mathbf{\varphi }\right)}^{\wedge }\right)\text{\hspace{1em}(4.35)}$$
这就为之后李代数上做微积分提供了理论基础。同样地, 对于 $\mathrm{S}\mathrm{E}(3)$ , 也有类似的BCH近似:
$$\begin{array}{l}\exp \left(\Delta {\mathbf{\xi }}^{\wedge }\right)\exp \left({\mathbf{\xi }}^{\wedge }\right)\approx \exp \left({\left({\mathcal{J}}_l^{-1}\Delta \mathbf{\xi }+\mathbf{\xi }\right)}^{\wedge }\right)\\ \exp \left({\mathbf{\xi }}^{\wedge }\right)\exp \left(\Delta {\mathbf{\xi }}^{\wedge }\right)\approx \exp \left({\left({\mathcal{J}}_r^{-1}\Delta \mathbf{\xi }+\mathbf{\xi }\right)}^{\wedge }\right)\end{array}\text{\hspace{1em}(4.36)}$$
这里的${\mathcal{J}}_l$形式比较复杂，它是一个6X6矩阵。

4.3.2 SO(3)上的李代数求导

假设某个时刻机器人的位姿为$\mathbf{T}$。它观察到了一个世界坐标位于$\mathbf{p}$的点，产生了一个观测数据$\mathbf{z}$。那么，由坐标变换关系知：
$$\mathbf{z}=\mathbf{T}\mathbf{p}+\mathbf{w}\text{\hspace{1em}(4.38)}$$
其中 $\mathbf{w}$ 为随机噪声。由于它的存在, $\mathbf{z}$ 往往不可能精确地满足 $\mathbf{z}=\mathbf{T}\mathbf{p}$ 的关系。所以, 我们通常会计算理想的观测与实际数据的误差:
$$\mathbf{e}=\mathbf{z}-\mathbf{T}\mathbf{p}\text{\hspace{1em}(4.39)}$$
假设一共有 $N$ 个这样的路标点和观测, 于是就有 $N$ 个上式。那么, 对机器人进行位姿估计, 相当于寻找一个最优的 $\mathbf{T}$ , 使得整体误差最小化:
$$\underset{\mathbf{T}}{\min }J(\mathbf{T})=\sum _{i=1}^N{\Vert {\mathbf{z}}_i-\mathbf{T}{\mathbf{p}}_i\Vert }_2^2\text{\hspace{1em}(4.40)}$$
SO(3)，SE(3) 上并没有良好定义的加法，它们只是群。如果我们把$\mathbf{T}$当成一个普通矩阵来处理优化，那就必须对优化问题添加约束（一旦有约束就变得很复杂了）。而从李代数角度来说，由于李代数由向量组成，具有良好的加法运算。因此，使用李代数解决求导问题的思路分为两种：

1. 用李代数表示姿态，然后对根据李代数加法来对李代数求导。
2. 对李群左乘或右乘微小扰动，然后对该扰动求导，称为左扰动和右扰动模型。

4.3.3 李代数求导

首先，考虑SO(3)上的情况。假设我们对一个空间点$\mathbf{p}$进行了旋转，得到了$\mathbf{R}\mathbf{p}$。现在，要计算旋转之后点的坐标相对于旋转的导数，我们非正式地记为：
$$\frac{\partial (\mathbf{R}\mathbf{p})}{\partial \mathbf{R}}$$
由于$\mathrm{S}\mathrm{O}(3)$没有加法, 所以该导数无法按照导数的定义进行计算。设 $\mathbf{R}$对应的李代数为$\varphi$, 我们转而计算。
$$\frac{\partial \left(\exp \left({\varphi }^{\wedge }\right)\mathbf{p}\right)}{\partial \varphi }$$
按照导数的定义, 有
$$\begin{array}{rl}\frac{\partial \left(\exp \left({\mathbf{\varphi }}^{\wedge }\right)\mathbf{p}\right)}{\partial \mathbf{\varphi }}& =\underset{\delta \mathbf{\varphi }\to 0}{\lim }\frac{\exp \left((\mathbf{\varphi }+\delta \mathbf{\varphi }{)}^{\wedge }\right)\mathbf{p}-\exp \left({\mathbf{\varphi }}^{\wedge }\right)\mathbf{p}}{\delta \mathbf{\varphi }}\\ & =\underset{\delta \mathbf{\varphi }\to 0}{\lim }\frac{\exp \left({\left({\mathbf{J}}_l\delta \mathbf{\varphi }\right)}^{\wedge }\right)\exp \left({\mathbf{\varphi }}^{\wedge }\right)\mathbf{p}-\exp \left({\mathbf{\varphi }}^{\wedge }\right)\mathbf{p}}{\delta \mathbf{\varphi }}\\ & =\underset{\delta \varphi \to 0}{\lim }\frac{\left(\mathbf{I}+{\left({\mathbf{J}}_l\delta \mathbf{\varphi }\right)}^{\wedge }\right)\exp \left({\mathbf{\varphi }}^{\wedge }\right)\mathbf{p}-\exp \left({\mathbf{\varphi }}^{\wedge }\right)\mathbf{p}}{\delta \mathbf{\varphi }}\\ & =\underset{\delta \varphi \to 0}{\lim }\frac{{\left({\mathbf{J}}_l\delta \mathbf{\varphi }\right)}^{\wedge }\exp \left({\mathbf{\varphi }}^{\wedge }\right)\mathbf{p}}{\delta \mathbf{\varphi }}\\ & =\underset{\delta \mathbf{\varphi }\to 0}{\lim }\frac{-{\left(\exp \left({\mathbf{\varphi }}^{\wedge }\right)\mathbf{p}\right)}^{\wedge }{\mathbf{J}}_l\delta \mathbf{\varphi }}{\delta \mathbf{\varphi }}=-(\mathbf{R}\mathbf{p}{)}^{\wedge }{\mathbf{J}}_l\end{array}$$
第二行地近似为BCH线性近似，第三行为泰勒展开社区高阶项后地近似（$\exp ()$括号内的矩阵范数很小时才可以这样近似），第四行至第五行将反对称符号看作叉积，交换之后变号。于是，我们推导出了旋转后的点相对于李代数的导数：
$$\frac{\partial (\mathbf{R}\mathbf{p})}{\partial \mathbf{R}}=-(\mathbf{R}\mathbf{p}{)}^{\wedge }{\mathbf{J}}_l\text{\hspace{1em}(4.41)}$$
不过，由于这里仍然含有形式比较复杂的${\mathbf{J}}_l$，我们不太希望计算它。而下面要讲的扰动模型则提供了更简单的导数计算方式。

4.3.4 扰动模型（左乘）

另一种求导方式是对$\mathbf{R}$进行一次扰动$\Delta \mathbf{R}$ , 看结果相对于扰动的变化率。这个扰动可以乘 在左边也可以乘在右边, 最后结果会有一点儿微小的差异, 我们以左扰动为例。设左扰动$\Delta \mathbf{R}$对应的李代数为 ${\phi }_{\circ }$ 然后, 对$\phi$求导, 即
$$\frac{\partial (\mathbf{R}\mathbf{p})}{\partial \mathbf{\phi }}=\underset{\phi \to 0}{\lim }\frac{\exp \left({\mathbf{\phi }}^{\wedge }\right)\exp \left({\mathbf{\varphi }}^{\wedge }\right)\mathbf{p}-\exp \left({\mathbf{\varphi }}^{\wedge }\right)\mathbf{p}}{\mathbf{\phi }}\text{\hspace{1em}(4.42)}$$
该式的求导比上面更简单:
$$\begin{array}{rl}\frac{\partial (\mathbf{R}\mathbf{p})}{\partial \mathbf{\phi }}& =\underset{\mathbf{\phi }\to 0}{\lim }\frac{\exp \left({\mathbf{\phi }}^{\wedge }\right)\exp \left({\mathbf{\varphi }}^{\wedge }\right)\mathbf{p}-\exp \left({\mathbf{\varphi }}^{\wedge }\right)\mathbf{p}}{\mathbf{\phi }}\\ & =\underset{\mathbf{\phi }\to 0}{\lim }\frac{\left(\mathbf{I}+{\mathbf{\phi }}^{\wedge }\right)\exp \left({\mathbf{\varphi }}^{\wedge }\right)\mathbf{p}-\exp \left({\mathbf{\varphi }}^{\wedge }\right)\mathbf{p}}{\mathbf{\phi }}\\ & =\underset{\mathbf{\phi }\to 0}{\lim }\frac{{\mathbf{\phi }}^{\wedge }\mathbf{R}p}{\mathbf{\phi }}=\underset{\mathbf{\phi }\to 0}{\lim }\frac{-(\mathbf{R}\mathbf{p}{)}^{\wedge }\mathbf{\phi }}{\mathbf{\phi }}=-(\mathbf{R}\mathbf{p}{)}^{\wedge }\end{array}$$
可见, 相比于直接对李代数求导, 省去了一个雅可比 ${\mathbf{J}}_l$ 的计算。这使得扰动模型更为实用。 请读者务必理解这里的求导运算, 这在位姿估计中具有重要的意义。

4.3.4扩展1： 扰动模型（右乘）

右扰动模型定义：
$$\frac{\partial (\mathbf{R}\mathbf{p})}{\partial \mathbf{\phi }}=\underset{\phi \to 0}{\lim }\frac{\exp \left({\mathbf{\varphi }}^{\wedge }\right)\exp \left({\mathbf{\phi }}^{\wedge }\right)\mathbf{p}-\exp \left({\mathbf{\varphi }}^{\wedge }\right)\mathbf{p}}{\mathbf{\phi }}\text{\hspace{1em}(4.42)}$$
进一步推算求导
$$\begin{array}{rl}\frac{\partial (\mathbf{R}\mathbf{p})}{\partial \mathbf{\phi }}& =\underset{\mathbf{\phi }\to 0}{\lim }\frac{\exp \left({\mathbf{\varphi }}^{\wedge }\right)\exp \left({\mathbf{\phi }}^{\wedge }\right)\mathbf{p}-\exp \left({\mathbf{\varphi }}^{\wedge }\right)\mathbf{p}}{\mathbf{\phi }}\\ & =\underset{\mathbf{\phi }\to 0}{\lim }\frac{\exp \left({\mathbf{\varphi }}^{\wedge }\right)\left(\mathbf{I}+{\mathbf{\phi }}^{\wedge }\right)\mathbf{p}-\exp \left({\mathbf{\varphi }}^{\wedge }\right)\mathbf{p}}{\mathbf{\phi }}\\ & =\underset{\mathbf{\phi }\to 0}{\lim }\frac{\mathbf{R}{\mathbf{\phi }}^{\wedge }p}{\mathbf{\phi }}=\underset{\mathbf{\phi }\to 0}{\lim }\frac{-\mathbf{R}{\mathbf{p}}^{\wedge }\mathbf{\phi }}{\mathbf{\phi }}=-\mathbf{R}{\mathbf{p}}^{\wedge }\end{array}$$

4.3.4扩展2： 伴随

在SO(3) 和SE(3) 上，有⼀个东西称为伴随（Adjoint）。对于SO(3)，有：
$$\mathbf{R}\exp \left({\mathbf{p}}^{\wedge }\right){\mathbf{R}}^{\mathrm{T}}=\exp \left((\mathbf{R}\mathbf{p}{)}^{\wedge }\right)$$
此时称 $\mathrm{Ad}(\mathbf{R})=\mathbf{R}$ 。

提示: 首先你需要证明 $\forall a\in {\mathbb{R}}^3,{\mathbf{R}\mathbf{a}}^{\wedge }{\mathbf{R}}^{\mathrm{T}}=(\mathbf{R}\mathbf{a}{)}^{\wedge }$ , 页面Derivation of Adjoint for SO(3)提示了一种简洁的途径。如下：

设有任意向量$\mathbf{v}$，与式子右边内积：
$$\begin{array}{rl}(\mathbf{R}\mathbf{a}{)}^{\wedge }\mathbf{v}& =(\mathbf{R}\mathbf{a})\times \mathbf{v}\\ & =(\mathbf{R}\mathbf{a})\times (\mathbf{R}{\mathbf{R}}^{\mathbf{T}}\mathbf{v})\\ & =\mathbf{R}\left(\mathbf{a}\times \left({\mathbf{R}}^{\mathbf{T}}\mathbf{v}\right)\right)\\ & =\mathbf{R}\left({\mathbf{a}}^{\wedge }{\mathbf{R}}^{\mathbf{T}}\mathbf{v}\right)\\ & =\mathbf{R}{\mathbf{a}}^{\wedge }{\mathbf{R}}^{\mathbf{T}}\mathbf{v}\end{array}$$
证毕，$\forall a\in {\mathbb{R}}^3,{\mathbf{R}\mathbf{a}}^{\wedge }{\mathbf{R}}^{\mathrm{T}}=(\mathbf{R}\mathbf{a}{)}^{\wedge }$。

对于$\mathrm{S}\mathrm{E}(3)$, 有:
$$\mathbf{T}\exp \left({\mathbf{\xi }}^{\wedge }\right){\mathbf{T}}^{-1}=\exp \left((\mathrm{Ad}(\mathbf{T})\mathbf{\xi }{)}^{\wedge }\right)$$
其中$\mathrm{Ad}(\mathbf{T})$定义为:
$$\mathrm{Ad}(\mathbf{T})=\left[\begin{array}{cc}\mathbf{R}& {\mathbf{t}}^{\wedge }\mathbf{R}\\ 0& \mathbf{R}\end{array}\right]$$
这个性质将在后文的 Pose Graph 优化中用到。但是 $\mathrm{S}\mathrm{E}(3)$ 的证明较为复杂。

4.3.4扩展3： 扰动求导的例子

参考李代数扰动模型求导总结
例子1
$$\frac{\partial R_1{R}_2}{\partial R_1}=I$$
求$R_1{R}_2$对$R_1$的左扰动导数：
$$\begin{array}{rl}\frac{\partial R_1{R}_2}{\partial R_1}& =\frac{\partial \log (\exp (\delta )R_1{R}_2(R_1{R}_2{)}^{-1})}{\partial \delta }{|}_{\delta =0}\\ & =\frac{\partial \log (\exp (\delta )R_1{R}_2{R}_2^{-1}{R}_1^{-1})}{\partial \delta }{|}_{\delta =0}\\ & =\frac{\partial \log (\exp (\delta ))}{\partial \delta }{|}_{\delta =0}\\ & =I\end{array}$$

例子2
$$\frac{\partial R_1{R}_2}{\partial R_2}=R_1$$
求$R_1{R}_2$对$R_2$的左扰动导数：
$$\begin{array}{rl}\frac{\partial R_1{R}_2}{\partial R_2}& =\frac{\partial \log (R_1\exp (\delta )R_2(R_1{R}_2{)}^{-1})}{\partial \delta }{|}_{\delta =0}\\ & =\frac{\partial \log (R_1\exp (\delta )R_2{R}_2^{-1}{R}_1^{-1})}{\partial \delta }{|}_{\delta =0}\\ & =\frac{\partial \log (R_1\exp (\delta )R_1^{-1})}{\partial \delta }{|}_{\delta =0}\\ & =\frac{\partial \log (\exp (R_1\delta ))}{\partial \delta }{|}_{\delta =0}\\ & =R_1\end{array}$$

例子3
$$\frac{\partial R^{-1}}{\partial R}=-R^{-1}=-R^{⊺}$$
求$R^{-1}$对$R$的左扰动导数：
$$\begin{array}{rl}\frac{\partial R^{-1}}{\partial R}& =\frac{\partial \log ((\exp (\delta )R{)}^{-1}R)}{\partial \delta }{|}_{\delta =0}\\ & =\frac{\partial \log ((R^{-1}\exp (\delta )R{)}^{-1})}{\partial \delta }{|}_{\delta =0}\\ & =\frac{\partial \log (\exp (R^{-1}\delta {)}^{-1})}{\partial \delta }{|}_{\delta =0}\\ & =\frac{\partial \log (\exp (-R^{-1}\delta ))}{\partial \delta }{|}_{\delta =0}\\ & =-R^{-1}=-R^{⊺}\end{array}$$

$$\exp (\varphi {)}^{-1}=\exp (-\varphi )$$
证明：
已知：
$$\begin{array}{r}\exp (\varphi )=\exp (\theta \cdot a)=\cos (\theta )\cdot I+(1-\cos (\theta ))a\cdot a^{⊺}+\sin (\theta )a^{\wedge }\end{array}$$
和：
$$(a^{\wedge }{)}^{⊺}=-a^{\wedge }$$
则：
$$\begin{array}{rl}\exp (\varphi {)}^{-1}=\exp (\theta \cdot a{)}^{⊺}& =\cos (\theta )\cdot I^{⊺}+(1-\cos (\theta ))(a\cdot a^{⊺}{)}^{⊺}+\sin (\theta )(a^{\wedge }{)}^{⊺}\\ & =\cos (\theta )\cdot I+(1-\cos (\theta ))a\cdot a^{⊺}-\sin (\theta )a^{\wedge }\\ & \\ \exp (-\varphi )=\exp (-\theta \cdot a)& =\cos (-\theta )\cdot I+(1-\cos (-\theta ))a\cdot a^{⊺}+\sin (-\theta )a^{\wedge }\\ & =\cos (\theta )\cdot I+(1-\cos (\theta ))a\cdot a^{⊺}-\sin (\theta )a^{\wedge }\end{array}$$
因此：
$$\exp (\varphi {)}^{-1}=\exp (-\varphi )$$

4.3.5 SE(3)上的扰动模型

最后, 我们给出 $\mathrm{S}\mathrm{E}(3)$ 上的扰动模型, 而直接李代数上的求导就不再介绍了。假设某空间点 $p$ 经过一次变换 $\mathbf{T}$ (对应李代数为 $\mathbf{\xi }$) , 得到 $\mathbf{T}{\mathbf{p}}^{}$ 。现在, 给 $\mathbf{T}$ 左乘一个扰动 $\Delta \mathbf{T}=\exp \left(\delta {\mathbf{\xi }}^{\wedge }\right)$ , 我们设扰动项的李代数为 $\delta \mathbf{\xi }=[\delta \mathbf{\rho },\delta \varphi {]}^{\mathrm{T}}$ , 那么：
$$\frac{\partial (\mathbf{T}\mathbf{p})}{\partial \delta \mathbf{\xi }}=\underset{\delta \xi \to 0}{\lim }\frac{\exp \left(\delta {\mathbf{\xi }}^{\wedge }\right)\exp \left({\mathbf{\xi }}^{\wedge }\right)\mathbf{p}-\exp \left({\mathbf{\xi }}^{\wedge }\right)\mathbf{p}}{\delta \mathbf{\xi }}$$
$$\begin{array}{l}={\lim }_{\delta \xi \to 0}\frac{\left(\mathbf{I}+\delta {\mathbf{\xi }}^{\wedge }\right)\exp \left({\mathbf{\xi }}^{\wedge }\right)\mathbf{p}-\exp \left({\mathbf{\xi }}^{\wedge }\right)\mathbf{p}}{\delta \mathbf{\xi }}\\ ={\lim }_{\delta \mathbf{\xi }\to 0}\frac{\delta {\mathbf{\xi }}^{\wedge }\exp \left({\mathbf{\xi }}^{\wedge }\right)\mathbf{p}}{\delta \mathbf{\xi }}\\ ={\lim }_{\delta \mathbf{\xi }\to 0}\frac{\left[\begin{array}{cc}\delta {\mathbf{\varphi }}^{\wedge }& \delta \mathbf{\rho }\\ 0^{\mathrm{T}}& 0\end{array}\right]\left[\begin{array}{c}\mathbf{R}\mathbf{p}+\mathbf{t}\\ 1\end{array}\right]}{\delta \mathbf{\xi }}\\ ={\lim }_{\delta \mathbf{\xi }\to 0}\frac{\left[\begin{array}{c}\delta {\mathbf{\varphi }}^{\wedge }(\mathbf{R}\mathbf{p}+\mathbf{t})+\delta \mathbf{\rho }\\ 0^{\mathrm{T}}\end{array}\right]}{[\delta \mathbf{\rho },\delta \mathbf{\varphi }{]}^{\mathrm{T}}}=\left[\begin{array}{cc}\mathbf{I}& -(\mathbf{R}\mathbf{p}+\mathbf{t}{)}^{\wedge }\\ 0^{\mathrm{T}}& 0^{\mathrm{T}}\end{array}\right]\stackrel{\mathrm{def}}{=}(\mathbf{T}\mathbf{p}{)}^{\odot }.\end{array}$$
我们把最后的结果定义成一个算符 ${}^{\odot }$ , 它把一个齐次坐标的空间点变换成一个 $4\times 6$ 的矩 阵。此式稍微需要解释的是矩阵求导方面的顺序, 假设 $\mathbf{a},\mathbf{b},\mathbf{x},\mathbf{y}$ 都是列向量, 那么在我们的符号写法下, 有如下的规则:
$$\frac{\mathrm{d}\left[\begin{array}{l}a\\ b\end{array}\right]}{\mathrm{d}\left[\begin{array}{l}\mathbf{x}\\ \mathbf{y}\end{array}\right]}={\left(\begin{array}{c}\frac{\mathrm{d}[\mathbf{a},\mathbf{b}{]}^{\mathrm{T}}}{\mathrm{d}\left[\begin{array}{l}\mathbf{x}\\ \mathbf{y}\end{array}\right]}\end{array}\right)}^{\mathrm{T}}={\left[\begin{array}{cc}\frac{\mathrm{d}\mathbf{a}}{\mathrm{d}\mathbf{x}}& \frac{\mathrm{d}\mathbf{b}}{\mathrm{d}\mathbf{x}}\\ \frac{\mathrm{d}\mathbf{a}}{\mathrm{d}\mathbf{y}}& \frac{\mathrm{d}\mathbf{b}}{\mathrm{d}\mathbf{y}}\end{array}\right]}^{\mathrm{T}}=\left[\begin{array}{cc}\frac{\mathrm{d}\mathbf{a}}{\mathrm{d}\mathbf{x}}& \frac{\mathrm{d}\mathbf{a}}{\mathrm{d}\mathbf{y}}\\ \frac{\mathrm{d}\mathbf{b}}{\mathrm{d}\mathbf{x}}& \frac{\mathrm{d}\mathbf{b}}{\mathrm{d}\mathbf{y}}\end{array}\right]$$

4.4 实践：Sopuhs

待续…

4.5 相似变换群与李代数

最后, 我们要提一下在单目视觉中使用的相似变换群$\mathrm{Sim}(3)$, 以及对应的李代数 $\mathrm{sim}(3)$ 。如果在单目 SLAM 中使用 $\mathrm{S}\mathrm{E}(3)$表示位姿, 那么由于尺度不确定性与尺度漂移，整个SLAM过程中的尺度会发生变化, 这在$\mathrm{S}\mathrm{E}(3)$中末能体现出来。 因此, 在单目情况下我们一般会显式地把尺度因子表达出来。用数学语言来说, 对于位于空间的点 $\mathbf{p}$ , 在相机坐标系下要经过一个相似变换, 而非欧氏变换:
$${\mathbf{p}}^{\prime }=\left[\begin{array}{cc}s\mathbf{R}& t\\ 0^{\mathrm{T}}& 1\end{array}\right]\mathbf{p}=s\mathbf{R}\mathbf{p}+\mathbf{t}$$
在相似变换中, 我们把尺度$s$表达出来了。它同时作用在$\mathbf{p}$的 3 个坐标之上, 对 $\mathbf{p}$进行了一次缩 放。与 $\mathrm{S}\mathrm{O}(3)$ 、 $\mathrm{S}\mathrm{E}(3)$相似, 相似变换也对矩阵乘法构成群, 称为相似变换群 $\mathrm{Sim}(3)$ :
$$\mathrm{Sim}(3)=\left\{S=\left[\begin{array}{cc}s\mathbf{R}& t\\ 0^{\mathrm{T}}& 1\end{array}\right]\in {\mathbb{R}}^{4\times 4}\right\}$$
同样地, $\mathrm{Sim}(3)$也有对应的李代数、指数映射、对数映射等。李代数$\mathrm{sim}(3)$元素是一个 7 维 向量 $\zeta$。它的前6维与$\mathfrak{s}\mathfrak{e}(3)$相同, 最后多了一项${\sigma }_{\text{。 }}$
$$\mathrm{sim}(3)=\left\{\mathbf{\zeta }\mid \mathbf{\zeta }=\left[\begin{array}{l}\rho \\ \varphi \\ \sigma \end{array}\right]\in {\mathbb{R}}^7,{\mathbf{\zeta }}^{\wedge }=\left[\begin{array}{cc}\sigma I+{\varphi }^{\wedge }& \rho \\ 0^{\mathrm{T}}& 0\end{array}\right]\in {\mathbb{R}}^{4\times 4}\right\}$$
它比$\mathfrak{s}\mathfrak{e}(3)$多了一项${\sigma }_{\circ }$关联$\mathrm{Sim}(3)$和 $\mathrm{sim}(3)$的仍是指数映射和对数映射。指数映射为
$$\exp \left({\mathbf{\zeta }}^{\wedge }\right)=\left[\begin{array}{cc}{\mathrm{e}}^{\sigma }\exp \left({\mathbf{\varphi }}^{\wedge }\right)& {\mathbf{J}}_s\rho \\ 0^{\mathrm{T}}& 1\end{array}\right]$$
其中, ${\mathbf{J}}_s$的形式为
$$\begin{array}{rl}{J}_s=& \frac{{\mathrm{e}}^{\sigma }-1}{\sigma }\mathbf{I}+\frac{\sigma {\mathrm{e}}^{\sigma }\sin \theta +\left(1-{\mathrm{e}}^{\sigma }\cos \theta \right)\theta }{{\sigma }^2+{\theta }^2}{\mathbf{a}}^{\wedge }\\ & +\left(\frac{{\mathrm{e}}^{\sigma }-1}{\sigma }-\frac{\left({\mathrm{e}}^{\sigma }\cos \theta -1\right)\sigma +\left({\mathrm{e}}^{\sigma }\sin \theta \right)\theta }{{\sigma }^2+{\theta }^2}\right){\mathbf{a}}^{\wedge }{\mathbf{a}}^{\wedge }\end{array}$$
通过指数映射, 我们能够找到李代数与李群的关系。对于李代数$\mathbf{\zeta }$ , 它与李群的对应关系为
$$s={\mathrm{e}}^{\sigma },\mathbf{R}=\exp \left({\mathbf{\varphi }}^{\wedge }\right),\mathbf{t}={\mathbf{J}}_s\mathbf{\rho }$$
旋转部分和 $\mathrm{S}\mathrm{O}(3)$ 是一致的。平移部分，在$\mathfrak{s}\mathfrak{e}(3)$中需要乘一个雅可比 $\mathcal{J}$ , 而相似变换的雅 可比更复杂。对于尺度因子, 可以看到李群中的$s$即为李代数中 $\sigma$ 的指数函数。

$\mathrm{Sim}(3)$ 的 $\mathrm{B}\mathrm{C}\mathrm{H}$ 近似与$\mathrm{S}\mathrm{E}(3)$是类似的。我们可以讨论一个点$\mathbf{p}$经过相似变换$\mathbf{S}\mathbf{p}$后, 相对于$S$的导数。同样地，存在微分模型和扰动模型两种方式, 而扰动模型较为简单。我们省略推导 过程，直接给出扰动模型的结果。设给予$S\mathbf{p}$左侧一个小扰动 $\exp \left({\mathbf{\zeta }}^{\wedge }\right)$ , 并求 $\mathbf{S}\mathbf{p}$ 对于扰动的导 数。因为 $Sp$ 是 4 维的齐次坐标, $\zeta$ 是 7 维向量, 所以该导数应该是 $4\times 7$ 的雅可比。方便起见, 记 $S\mathbf{p}$ 的前 3 维组成向量为 $\mathbf{q}$ , 那么:
$$\frac{\partial \mathbf{S}\mathbf{p}}{\partial \mathbf{\zeta }}=\left[\begin{array}{ccc}\mathbf{I}& -{\mathbf{q}}^{\wedge }& \mathbf{q}\\ 0^{\mathrm{T}}& 0^{\mathrm{T}}& 0\end{array}\right]$$