旋转向量/矩阵、变换矩阵、欧拉角、四元数实现了对位姿的表示。在SLAM问题中，需要对位姿进行估计和优化，因此通常构建优化问题进行求解。

  在这过程涉及到求导问题，如果使用旋转矩阵，其本身带有约束（正交且行列式为1），将之作为优化变量会带来额外约束，从而使得优化求解变难。因此引出李群、李代数构建无约束优化问题，简化求解。

1．李群

  群： 一种集合$ (A) $加上一种运算$ (.) $的代数结构：

$$
G = (A; ·)
\tag{1-1}
$$

  群满足以下条件：

  矩阵中常见的群（$ SO(n) $、$ SE(n) $对乘法封闭，对加法不封闭）：

  特殊正交群、特殊欧式群对乘法封闭，对加法不封闭。

  李群： 具有连续性质的群。如整数群没有连续性质；特殊正交群、特殊欧式群是连续的，因为机器人/相机在空间中的运动时连续的，因此它们是李群。

2．李代数

2.1 李代数的引出

• 推导：

  （1）旋转函数代表机器人/相机的旋转，随着时间连续变化，即为时间的函数。旋转矩阵是正交矩阵，有：$$ \boldsymbol{R}(t) \boldsymbol{R}(t)^{T}=\boldsymbol{I} \tag{2-1}$$

  （2）等式两边对时间求导，可得出是一个反对称矩阵（2-3），又：定义向量对应的反对称矩阵，反对称矩阵对应的向量（2-4）。

$$ \dot{\boldsymbol{R}}(t) \boldsymbol{R}(t)^{T}+\boldsymbol{R}(t) \dot{\boldsymbol{R}}(t)^{T}=0 \tag{2-2}$$

$$ \dot{\boldsymbol{R}}(t) \boldsymbol{R}(t)^{T}=-\left(\dot{\boldsymbol{R}}(t) \boldsymbol{R}(t)^{T}\right)^{T} \tag{2-3}$$

$$ \boldsymbol{a}^{\wedge}=\boldsymbol{A}=\left[ \begin{array}{ccc}{0} & {-a_{3}} & {a_{2}} \\ {a_{3}} & {0} & {-a_{1}} \\ {-a_{2}} & {a_{1}} & {0}\end{array}\right], \quad \boldsymbol{A}^{\vee}=\boldsymbol{a} \tag{2-4}$$

  （3）因此可找到一个三维向量 $ \phi(t) \in \mathbb{R}^{3} $ 与 $ \dot{\boldsymbol{R}}(t) \boldsymbol{R}(t)^{T} $ 对应（2-5）。等式两边右乘 $ \boldsymbol{R}(t) $ ，可得到每对旋转矩阵求一次导数，只需左乘一个 $ \phi(t)^{\wedge} $ 矩阵即可（2-6）。

$$ \dot{\boldsymbol{R}}(t) \boldsymbol{R}(t)^{T}=\boldsymbol{\phi}(t)^{\wedge} \tag{2-5}$$

$$ \dot{\boldsymbol{R}}(t)=\boldsymbol{\phi}(t)^{\wedge} \boldsymbol{R}(t)=\left[ \begin{array}{ccc}{0} & {-\phi_{3}} & {\phi_{2}} \\ {\phi_{3}} & {0} & {-\phi_{1}} \\ {-\phi_{2}} & {\phi_{1}} & {0}\end{array}\right] \boldsymbol{R}(t) \tag{2-6}$$

  （4）令 $ t_{0}=0 $，且 $ \phi\left(t_{0}\right)=\phi_{0} $，则有（2-7）；上式为微分方程，且有初值 $\boldsymbol{R}(0)=\boldsymbol{I}$，解之得（2-8）。

$$ \dot{\boldsymbol{R}}(t)=\boldsymbol{\phi}\left(t_{0}\right)^{\wedge} \boldsymbol{R}(t)=\boldsymbol{\phi}_{0}^{\wedge} \boldsymbol{R}(t) \tag{2-7}$$

$$ \boldsymbol{R}(t)=\exp \left(\phi_{0}^{\wedge} t\right) \tag{2-8}$$

• 结论：

  （1）$ \phi $：给定某时刻的$ \boldsymbol{R} $，可求得一个$ \phi $，描述了$ \boldsymbol{R} $在局部的导数关系。其中$ \phi $即是李代数。（2.2节）
  （2）$ \exp \left(\right) $：矩阵指数。描述了李群和李代数之间的指数映射关系。（2.3节）

2.2 定义

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

  二元运算被称为李括号。
  相比于群中的较为简单的二元运算，李括号表达了两个元素的差异。它不要求结合律，而要求元素和自己做李括号之后为零的性质。

• 李代数so(3)：
    $ \mathfrak{s o}(3) $是一个由三维向量组成的集合，每个向量对应一个反对称矩阵。根据2.1节推导可知，它可用于旋转矩阵的求导，李代数和李括号定义如下：

$$
\mathfrak{so}(3)=
\left\{
\phi \in \mathbb{R}^{3},
\mathbf{\Phi}=\phi^{\wedge}=
\left[
\begin{array}{ccc}
{0} & {-\phi_{3}} & {\phi_{2}} \\
{\phi_{3}} & {0} & {-\phi_{1}} \\
{-\phi_{2}} & {\phi_{1}} & {0}
\end{array}
\right]
\in \mathbb{R}^{3 \times 3}
\right\}
$$

$$
\left[\phi_{1}, \phi_{2}\right]=\left(\mathbf{\Phi}_{1} \mathbf{\Phi}_{2}-\mathbf{\Phi}_{2} \mathbf{\Phi}_{1}\right)^{\vee}
$$

• 李代数se(3)
    $ \mathfrak{s e}(3) $每个元素是一个六维向量记作$ \boldsymbol{\xi} $：前三维是平移记作$ \boldsymbol{\rho} $（这里的平移左乘一个系数矩阵才是变换矩阵中的真实平移$ \boldsymbol{t} $，见2.2.2节），后三位是旋转记作$ \phi $（实质是$ \mathfrak{s o}(3) $的元素）。
    此处符号$ \boldsymbol{^{\wedge}} $和$ \boldsymbol{^{\vee}} $不代表反对称，而是被扩展成“六位向量”与“四维矩阵”的相互转换关系（为了保持和$ \mathfrak{s o}(3) $形式一致）。
    李代数和李括号定义如下：

$$
\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\}
$$

$$
\left[\boldsymbol{\xi}_{1}, \boldsymbol{\xi}_{2}\right]=\left(\boldsymbol{\xi}_{1}^{\wedge} \boldsymbol{\xi}_{2}^{\wedge}-\boldsymbol{\xi}_{2}^{\wedge} \boldsymbol{\xi}_{1}^{\wedge}\right)^{\vee}
$$

2.3 李群和李代数关系

2.3.1 SO(3)上的指数映射

  $ SO(3) $与$ \mathfrak{s o}(3) $的关系由指数映射给定：

$$
\boldsymbol{R}=\exp \left(\boldsymbol{\phi}^{\wedge}\right)
$$

• 推导：
    （1）将指数映射写成泰勒展开式：
  $$
  \exp \left(\phi^{\wedge}\right)=\sum_{n=0}^{\infty} \frac{1}{n !}\left(\phi^{\wedge}\right)^{n}
  $$
    （2）因为是三维向量，将之表示为模长（）和方向（单位方向向量）的乘积，且具有以下性质：
  $$
  \begin{cases}
  \phi=\theta \boldsymbol{a}\
  \boldsymbol{a}^{\wedge} \boldsymbol{a}^{\wedge}=\boldsymbol{a} \boldsymbol{a}^{T}-\boldsymbol{I}\
  \boldsymbol{a}^{\wedge} \boldsymbol{a}^{\wedge} \boldsymbol{a}^{\wedge}=-\boldsymbol{a}^{\wedge}
  \end{cases}
  $$
    （3）将上述表示带入指数映射：
  $$
  \begin{aligned} \exp \left(\phi^{\wedge}\right) &=\exp \left(\theta \boldsymbol{a}^{\wedge}\right)=\sum_{n=0}^{\infty} \frac{1}{n !}\left(\theta \boldsymbol{a}^{\wedge}\right)^{n} \\ &=\boldsymbol{I}+\theta \boldsymbol{a}^{\wedge}+\frac{1}{2 !} \theta^{2} \boldsymbol{a}^{\wedge} \boldsymbol{a}^{\wedge}+\frac{1}{3 !} \theta^{3} \boldsymbol{a}^{\wedge} \boldsymbol{a}^{\wedge} \boldsymbol{a}^{\wedge}+\frac{1}{4 !} \theta^{4}\left(\boldsymbol{a}^{\wedge}\right)^{4}+\ldots \\ &=\boldsymbol{a} \boldsymbol{a}^{T}-\boldsymbol{a}^{\wedge} \boldsymbol{a}^{\wedge}+\theta \boldsymbol{a}^{\wedge}+\frac{1}{2 !} \theta^{2} \boldsymbol{a}^{\wedge} \boldsymbol{a}^{\wedge}-\frac{1}{3 !} \theta^{3} \boldsymbol{a}^{\wedge}-\frac{1}{4 !} \theta^{4}\left(\boldsymbol{a}^{\wedge}\right)^{2}+\ldots \\ &=\boldsymbol{a} \boldsymbol{a}^{T}+\left(\theta-\frac{1}{3 !} \theta^{3}+\frac{1}{5 !} \theta^{5}-\ldots\right) \boldsymbol{a}^{\wedge}-\left(1-\frac{1}{2 !} \theta^{2}+\frac{1}{4 !} \theta^{4}-\ldots\right) \boldsymbol{a}^{\wedge} \boldsymbol{a}^{\wedge} \\ &=\boldsymbol{a}^{\wedge} \boldsymbol{a}^{\wedge}+\boldsymbol{I}+\sin \theta \boldsymbol{a}^{\wedge}-\cos \theta \boldsymbol{a}^{\wedge} \boldsymbol{a}^{\wedge} \\ &=(1-\cos \theta) \boldsymbol{a}^{\wedge} \boldsymbol{a}^{\wedge}+\boldsymbol{I}+\sin \theta \boldsymbol{a}^{\wedge}\ &=\cos \theta \boldsymbol{I}+(1-\cos \theta) \boldsymbol{a} \boldsymbol{a}^{T}+\sin \theta \boldsymbol{a}^{\wedge} \end{aligned}
  $$

• 结论：
    上式同罗德里格斯公式，即旋转向量与旋转矩阵的转换公式（见《刚体运动与位姿表示》6.2节）。因此如下结论：
    （1）$ \mathfrak{s o}(3) $到$ SO(3) $的指数映射：由罗德里格斯公式给出，$ \mathfrak{s o}(3) $实际上就是旋转向量组成的空间。
    （2）$ SO(3) $到$ \mathfrak{s o}(3) $到的映射：由如下对数映射给出。但实际中常用《刚体运动与位姿表示》6.2节所述方法，根据旋转矩阵计算旋转向量。
  $$
  \phi=\ln (\boldsymbol{R})^{\vee}=\left(\sum_{n=0}^{\infty} \frac{(-1)^{n}}{n+1}(\boldsymbol{R}-\boldsymbol{I})^{n+1}\right)^{\vee}
  $$

2.3.2 SE(3)上的指数映射

  $ SE(3) $与$ \mathfrak{s e}(3) $的相互转换，可通过下述映射进行计算：
$$
\begin{aligned} \exp \left(\boldsymbol{\xi}^{\wedge}\right) &=\left[\begin{array}{ccc}{\sum_{n=0}^{\infty} \frac{1}{n !}\left(\boldsymbol{\phi}^{\wedge}\right)^{n}} & {\sum_{n=0}^{\infty} \frac{1}{(n+1) !}\left(\boldsymbol{\phi}^{\wedge}\right)^{n} \boldsymbol{\rho}} \\ {\mathbf{0}^{T}} & {1}\end{array}\right] \\ & \triangleq\left[\begin{array}{cc}{\boldsymbol{R}} & {\boldsymbol{J} \rho} \\ {\mathbf{0}^{T}} & {1}\end{array}\right]=\boldsymbol{T} \end{aligned}
$$

$$
J=\frac{\sin \theta}{\theta} \boldsymbol{I}+\left(1-\frac{\sin \theta}{\theta}\right) \boldsymbol{a} \boldsymbol{a}^{T}+\frac{1-\cos \theta}{\theta} \boldsymbol{a}^{\wedge}
$$

3．李代数求导

3.1 BCH公式与近似形式

  对于矩阵的指数函数，有BCH公式：
$$
\ln (\exp (\boldsymbol{A}) \exp (\boldsymbol{B}))=\boldsymbol{A}+\boldsymbol{B}+\frac{1}{2}[\boldsymbol{A}, \boldsymbol{B}]+\frac{1}{12}[\boldsymbol{A},[\boldsymbol{A}, \boldsymbol{B}]]-\frac{1}{12}[\boldsymbol{B},[\boldsymbol{A}, \boldsymbol{B}]]+...
$$

• SO(3)上的近似形式
    取其近似形式，分为左乘近似和右乘近似：

$$
\ln \left(\exp \left(\phi_{1}^{\wedge}\right) \exp \left(\phi_{2}^{\wedge}\right)\right)^{\vee} \approx\left\{\begin{array}{ll}{\boldsymbol{J}_{l}\left(\boldsymbol{\phi}_{2}\right)^{-1} \phi_{1}+\phi_{2}} & {\text { if } \boldsymbol{\phi}_{1} \text { is small, }} \\ {\boldsymbol{J}_{r}\left(\boldsymbol{\phi}_{1}\right)^{-1} \boldsymbol{\phi}_{2}+\boldsymbol{\phi}_{1}} & {\text { if } \boldsymbol{\phi}_{2} \text { is small. }}\end{array}\right.
$$

$$
\boldsymbol{J}_{l}=\boldsymbol{J}=\frac{\sin \theta}{\theta} \boldsymbol{I}+\left(1-\frac{\sin \theta}{\theta}\right) \boldsymbol{a} \boldsymbol{a}^{T}+\frac{1-\cos \theta}{\theta} \boldsymbol{a}^{\wedge}
$$

$$
\boldsymbol{J}_{r}(\boldsymbol{\phi})=\boldsymbol{J}_{l}(-\boldsymbol{\phi})
$$

• SE(3)上的近似形式：
    这里的雅可比矩阵形式复杂，是6×6的矩阵，实际计算中不会用到。
  $$
  \begin{array}{l}{\exp \left(\Delta \boldsymbol{\xi}^{\wedge}\right) \exp \left(\boldsymbol{\xi}^{\wedge}\right) \approx \exp \left(\left(\mathcal{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(\mathcal{J}_{r}^{-1} \Delta \boldsymbol{\xi}+\boldsymbol{\xi}\right)^{\wedge}\right)}\end{array}
  $$

3.2 李代数求导

3.2.1 李代数求导的实际意义

3.2.2 两种模型

  第一章提到，$SO$ 和 $SE$ 没有加法，因此无法根据导数的定义对李群求导。可使用李代数解决该问题。

• 求导模型
    用李代数表示姿态，然后对根据李代数加法来对李代数求导（该模型计算中有较为复杂的雅可比矩阵，不常用）。

• 扰动模型
    对李群左乘或右乘微小扰动，然后对该扰动求导，称为左扰动和右扰动模型。

3.3 扰动模型

• SO(3)上的求导：
    是对 $\boldsymbol{R}$ 进行一次扰动 $∆\boldsymbol{R}$ 。设左扰动 $∆\boldsymbol{R}$ 对应的李代数为 $\boldsymbol{φ}$ 。然后，对 $\boldsymbol{φ}$ 求导：

$$
\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(\boldsymbol{\phi}^{\wedge}\right) \boldsymbol{p}-\exp \left(\boldsymbol{\phi}^{\wedge}\right) \boldsymbol{p}}{\boldsymbol{\varphi}} \\
& \approx \lim _{\boldsymbol{\varphi} \rightarrow 0} \frac{\left(1+\boldsymbol{\varphi}^{\wedge}\right) \exp \left(\boldsymbol{\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}}{\boldsymbol{\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)上的求导：

$$
\begin{aligned}
\frac{\partial(\boldsymbol{T} p)}{\partial \delta \boldsymbol{\xi}}
&=\lim _{\delta \boldsymbol{\xi} \rightarrow 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(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 0} \frac{\delta \boldsymbol{\xi}^{\wedge} \exp \left(\boldsymbol{\xi}^{\wedge}\right) \boldsymbol{p}}{\delta \boldsymbol{\xi}} \\
& =\lim _{\delta \boldsymbol{\xi} \rightarrow 0} \frac{\left[ \begin{array}{cc}{\delta \boldsymbol{\phi}^{\wedge}} & {\delta \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 0} \frac{\left[ \begin{array}{c}{\delta \boldsymbol{\phi}^{\wedge}(\boldsymbol{R} \boldsymbol{p}+\boldsymbol{t})+\delta \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}
$$