注意：

• ${}^{\mathbf{\Lambda }}$ 记做“向量拉伸成矩阵”，（见外积）
• ${}^{\mathbf{V}}$ 记做“矩阵坍缩成向量”

0. 一个例子

$今天所做的一切都是为了求解下边这个问题展开的！$

\qquad 机器人位姿$T$ ,观察到世界坐标系中的点$p$，产生观测数据$z,误差为w或e$
$z=Tp+w$
\qquad 共有N个这样的观测数据和路标，我们的任务是寻找最优的T，使误差最小化
$\underset{T}{\min }J(\mathbf{T})=\sum _{i=1}^N||{\mathbf{z}}_{\mathbf{i}}-\mathbf{Tpi}|{|}_2^2$

• 最终要求解的是目标函数$\mathbf{J}$关于变换矩阵$\mathbf{T}$的导数
• 我们经常会构建与位姿有关的函数，然后讨论该函数关于位姿的导数，以调整当前的估计值

---

1. 群和李群

• 特殊正交群与特殊欧式群：
  $SO(3)$ = {$R\in {\mathbb{R}}^{3x3}|R{R}^T=I,det(R)=1$}

$SE(3)$ = { $T=\left[\begin{array}{ccc}R& t& \\ 0^T& 1& \end{array}\right]\in {\mathbb{R}}^{4x4}|R\in SO(3),t\in {\mathbb{R}}^3$}

不难发现，$R和T$ 对加法不封闭，即$R_1+R_2\notin SO(3)$。

---

群： 只有一个运算的集合，即一种集合($记作A$ )+ 一种代数结构($记作.$)。
群有四个性质，分别是 “封闭性、结合律、幺元、逆”
常见的群：一般线性群$GL(n)$(对乘法)， 特殊正交群 $SO(n)$ ，特殊欧式群$SE(n)$

---

李群：$SO(3)$对乘法封闭，表示旋转，且连续，因为在实数空间物体的旋转是连续的。所以它是一种李群。 严谨地，李群是指具有连续（光滑）性质的群。

• 由于$SO(3)$和$SE(3)$这两个李群对SLAM较为重要，主要讨论着两个李群。

2. 李代数

2.1 推导和性质

从$R$为正交阵且$R{R}^T=I$开始推导，如下（笔记潦草，勿怪）：

\qquad\qquad\qquad 这里，可以知道$R(t)R(t{)}^T=\varphi (t)$^为反对称矩阵，上边记得 ^ 和v，分别表示把向量张成反对称矩阵（参考外积）和它的反向操作,接着对$R$的导数进行研究

以上可以看出，每次求导，仅需要左乘一个$\varphi (t)$^ 即可。
将R(t)进行泰勒展开：

$R(t)\approx R(t_0)+\dot{R}(t_0)(t-t_0)=I+\varphi (t_0)$^$(t)$

观察可得，这里$\varphi$实际反应的导数的性质。

• 特别地，研究初始时刻$t_0=0$ ，$R(0)=I$:

$\dot{R}(t)=\varphi (t_0)$^ $R(t)={\varphi }_0$^$R(t)$

解这个（一阶常线性）微分方程得 $R(t)=exp({\varphi }_0$^$t)$

• 至此，$\varphi$就正是对应$SO(3)$ 上的李代数$so(3)$。旋转矩阵R与李代数${\varphi }_0$ 通过指数关系发生了联系，反映了R在局部的导数关系。

---

李代数： 记作$g$ 描述了李群的局部性质，通用李代数定义和性质如下。定义中的二元运算被称为李括号。如，三维向量$R_3$上定义的叉积就是一种李代数$g=({\mathbb{R}}^3,\mathbb{R},$X$)$。

2.2 $so(3)$ 和 $se(3)$

so(3):三维

\qquad 因为$\varphi$ 本来代表向量，但是每个R都可以生成一个矩阵，它们关系紧密，后文不区分它是v后的矩阵，还是^后的向量，择适者用。
\qquad $so(3)$ 表示一个由三维向量组成的集合，每个向量对应到一个反对称矩阵，可以表达旋转矩阵的导数。它与 SO(3) 的关系由指数映射给定:
如下
$R=exp(\varphi$^$)$

它的李括号运算： $[{\varphi }_1,{\varphi }_2]=({\Phi }_1{\Phi }_2-{\Phi }_2{\Phi }_1{)}^V$

se(3)：六维

也表示类似局部导数的性质，但是下文的v和^仅仅指代向量和矩阵的互换，与反对称无关。
定义如下：

李括号运算：

2.3 计算李代数的幂$exp(\varphi$^$)$

so(3)的映射：
\qquad 这里称计算$exp(\varphi$^$)$ 的步骤为指数映射(Exponential Map)
矩阵指数映射的计算公式：$exp(\varphi )=\sum _{n=0}^{\infty }\frac{1}{n!}(\varphi {)}^n$

定义$\varphi =\theta a,a$是方向向量，模长1。推导可得：

和罗德里格斯公式一模一样，侧面反映了李代数$\approx$旋转向量，而李群是旋转矩阵。用迹则可以求$\theta$ 和 $a$，在$\theta \in [-\pi ,\pi ]$ 内，是一一对应的。

se(3)的映射：
\quad 由上述内容，和so(3)推导一致可得
$${\xi }^{\Lambda }=\left[\begin{array}{cc}{\varphi }^{\Lambda }& \rho \\ 0^T& 1\end{array}\right]故exp({\xi }^{\Lambda })=\left[\begin{array}{cc}R& J\rho \\ 0^T& 1\end{array}\right]$$
\qquad 注意：旋转部分指数映射同$\mathbf{so}(3)$,（不同才搞笑了）平移部分在做指数映射后多了一项系数矩阵$\mathbf{J}$ ，也有点像罗德里格斯公式(仍然设$\varphi =\theta a$)：

最后上一张所有推导的总结（图源《视觉SLAM14讲》）

2.4 李代数乘法

SO(3):
\qquad 李群乘法即李代数加法，但是矩阵的指数映射不满足常数的 $e^a\cdot e^b=e^{(a+b)}$
\qquad 求解李代数指数映射乘积还得看BCH公式（Baker-Campbell-Hausdorff），如下：

$ln(e^A\cdot e^B)=A+B+\frac{1}{2}[A,B]+\frac{1}{12}[A,[A,B]]-\frac{1}{12}[B,[A,B]]+......$

其中的 [ ] 表示李括号，当A或B为极小量的时候，BCH的线性近似表达如下：

- 直观的理解上式：李群的R1和R2左乘近似（R1小）或右乘近似（R2小）而已。其实就是$\Delta R\cdot R$或$R\cdot \Delta R$。一个微小位移。
\qquad 我们以左乘为例计算一下$J_l^{-1}$：

1. 首先左乘的标注形式为：
   $J_l=J=\frac{\sin \theta }{\theta }I+(1-\frac{\sin \theta }{\theta })a{a}^T+\frac{1-\cos \theta }{\theta }{a}^{\Lambda }$
2. 计算它的逆矩阵
   $J_l^{-1}=\frac{\theta }{2}\cot \frac{\theta }{2}\cdot I+(1-\frac{\theta }{2}\cot \frac{\theta }{2})a{a}^T+\frac{\theta }{2}{a}^{\Lambda }$
3. 则右乘雅克比仅对自变量取负号即可：
   $J_r(\varphi )=J_l(-\varphi )$

SE(3)类似：

2.5 从李代数乘法到导数：

\qquad 参考前边的例子：我们知道，最终要求解的是目标函数$\mathbf{J}$关于变换矩阵$\mathbf{T}$的导数。从上边所有的铺垫到现在，有两种方法求解导数，分别介绍：

2.5.1 直接求导

对SO(3)
空间点p经过旋转
\qquad 便于理解的记法，设$R$对应的李代数为$\varphi$：
$$\begin{gathered} \frac{\partial (Rp)}{\partial R}=\frac{\partial (exp({\varphi }^{\Lambda })p)}{\partial \varphi } \\ ⟹-(Rp{)}^{\Lambda }{J}_l \end{gathered}$$

• 第二行中省略了很多推导步骤，包括导数定义展开、BCH线性近似、泰勒展开舍去高阶项近似、将反对称符号看做叉积，交换后变号。

2.5.2 扰动模型求导(常用)

对SO(3)
空间点p的旋转可以看成一次左扰动
\qquad 扰动记作$\Delta R$，李代数为 $\phi$，则
$$\begin{gathered} \frac{\partial (Rp)}{\partial \phi }=\underset{\phi \to 0}{\lim }\frac{exp({\phi }^{\Lambda })exp({\varphi }^{\Lambda })p-exp({\varphi }^{\Lambda })p}{\phi } \\ ⟹-(Rp{)}^{\Lambda } \end{gathered}$$

• 常用，简单，在位姿估计中有重要意义

对SE(3)
直接上扰动$\Delta \mathbf{T}=exp(\delta {\xi }^{\Lambda })$,它的李代数$\delta \mathbf{\xi }=[\delta \mathbf{\rho },\delta \varphi {]}^T$
直接上结果

$$\frac{\partial (Tp)}{\partial \delta \xi }=\left[\begin{array}{cc}I& -(Rp+t{)}^{\Lambda }\\ 0^T& 0^T\end{array}\right]{=}^{△}(Tp{)}^{\odot }$$

其中$\odot$表示，一个齐次坐标展开成4X6的矩阵

3. $Sim(3)$与李代数

\qquad 相似变换群($Sim$)，用于解决单目SLAM中的尺度问题，在这种情况下会显式表示出尺度$s$，此时相机坐标系下的点经过相似变换(不是欧式变换)：
描述该过程如下：

$$p^{\prime }=\left[\begin{array}{cc}\mathbf{sR}& t\\ 0^T& 1\end{array}\right]p=s\mathbf{Rp}+\mathbf{t}$$

而$Sim(3)$如下：

$$Sim(3)=\left\{\begin{array}{c}\left[\begin{array}{c}\mathbf{S}=\begin{array}{cc}\mathbf{sR}& t\\ 0^T& 1\end{array}\end{array}\right]\in {\mathbb{R}}^{4\times 4}\end{array}\right\}$$

• 同样的，尽管多了一个维度，它仍然具有李代数(7维)，指数映射，对数映射，$J$ , 导数（扰动模型），以后做单目我再自行了解吧 。