slam学习笔记

相机按照工作方式不同，分为：

• 单目相机（Momocular）：结构简单、成本低
• 双目相机（Stereo）：双目相机的距离估计是比较左右眼的图像获得的。双目与多目的缺点是配置与标定较为复杂，其深度量程和精度受双目的基线与分辨率所限，而且视差的计算非常消耗资源，需要使用GPU和FPGA设备加速，才能实时输出整张图像的距离信息。现有条件下，计算量是双目的主要问题之一。
• 深度相机（RGB-D）：其最大特点是可以通过红外结构或Time-of-Flight(ToF)原理，像激光传感器那样，通过主动向物体发射光并接收返回的光，测出物体与相机之间的距离。相比于双目可节省大量的计算资源。缺点：测量范围窄、噪声大、视野小、易受日光干扰、无法测量透射材质等诸多问题。slam方面主要用于室内，室外较难应用。

经典视觉SLAM框架

1. 传感器信息读取。在视觉slam中主要为相机图像信息的读取和预处理。
2. 前端视觉里程计（Visual Odemetry，VO）：估计相邻图像间相机的运动，以及局部地图的样子。VO又称前端。
3. 后端（非线性）优化（Optimazition）。后端接受不同时刻视觉里程计测量的相机位姿，以及回环检测的信息，对他们进行优化，得到全局一致的轨迹和地图。
4. 回环检测（Loop Closure Detection）。回环检测判断机器人是否到达过先前的位置，如果监测到回环，他会把信息提供给后端进行处理。回环检测实质上是一种计算图像数据相似性的算法。
5. 建图（Mapping）。它根据估计的轨迹，建立与任务要求对应的地图。构建地图的过程。

如果把工作环境限定在静态、刚体、光照变化不明显、没有人为干扰的场景，这种场景下的slam技术已经相当成熟。

度量地图（Metric Map）：强调精确地表示地图中物体的位置关系，通常用稀疏（Sparse）与稠密（Dense）对其分类。定位时使用稀疏地图，导航使用稠密地图。

拓扑地图（Topological Map）：是一个图，由节点个边组成只考虑节点的连通性。

 视觉里程计

        视觉里程计关心相邻图像之间的相机运动，最简单的情况是两张图像之间的运动关系。

        视觉里程计能够通过相邻帧间的图像古遗迹相机运动，并恢复场景的空间结构。称它为‘里程计’是因为它和实际的里程计一样，只计算相邻时刻的运动，和过去的信息没有关联。

漂移：是由于视觉里程计的估计误差导致的，先前时刻的误差会传递到下一刻，导致经过一段时间后，估计的轨迹就不再准确。

后端优化和回环检测可以解决漂移问题。回环检测负责把“把机器人回到原始位置”的事情检测出来，后端优化则根据该信息校正整个轨迹的形状。

在视觉slam中，前端和计算机视觉研究领域更为相关，比如图像的特征提取与匹配等，后端则主要是滤波与非线性优化算法。

SLAM问题的本质：对运动物体自身和周围环境空间不确定性的估计。为解决slam问题，我们需要状态估计理论，把定位和见图的不确定性表达出来，然后采用滤波器或非线性优化，估计状态的均值和不确定性（方差）。

视觉回环检测实质上是一种计算图像数据相似性的算法。

SLAM问题的数学表述

离散 时刻：t=1,...,K" role="presentation" style="position: relative;">t=1,...,K$t=1,...,K$;

轨迹：x1,...,xk" role="presentation" style="position: relative;">x1,...,xk$x_1,...,x_k$;

路标：N个，用y1,...,yN" role="presentation" style="position: relative;">y1,...,yN$y_1,...,y_N$表示

时刻位于xk" role="presentation" style="position: relative;">xk$x_k$处探测到某一个路标yi" role="presentation" style="position: relative;">yi$y_i$

运动方程：                                        ​​​​​ xk=f(xk−1,uk,ωk)" role="presentation" style="position: relative;">xk=f(xk−1,uk,ωk)$x_k=f(x_{k-1},u_k,{\omega }_k)$

 这里，是运动传感器的读数或者输入，ωk" role="presentation" style="position: relative;">ωk${\omega }_k$为该过程中加入的噪声。

观测方程：在时刻位于xk" role="presentation" style="position: relative;">xk$x_k$处探测到某一个路标yi" role="presentation" style="position: relative;">yi$y_i$，产生一个观测数据zk,j" role="presentation" style="position: relative;">zk,j$z_{k,j}$。用一个抽象函数h来描述这个关系：

                                                        zk,j=h(yi,xk,vk,j)" role="presentation" style="position: relative;">zk,j=h(yi,xk,vk,j)$z_{k,j}=h(y_i,x_k,v_{k,j})$

 vk,j" role="presentation" style="position: relative;">vk,j$v_{k,j}$是这次观测里的噪声。

SLAM过程可以总结为两个基本方程：

                                                        {xk=f(xk−1,uk,ωk)k=1,...,Kzk,j=h(yi,xk,vk,j)(k,j)∈\O" role="presentation" style="position: relative;">{xk=f(xk−1,uk,ωk)k=1,...,Kzk,j=h(yi,xk,vk,j)(k,j)∈\O$\{\begin{array}{c}{x}_k=f(x_{k-1},u_k,{\omega }_k)k=1,...,K\\ z_{k,j}=h(y_i,x_k,v_{k,j})(k,j)\in \text{O}\end{array}$

其中∅" role="presentation" style="position: relative;">∅$\varnothing$是一个集合，记录着哪个时刻观察到了哪个路标。这；两个方程描述了最基本的slam问题：

当知道运动测量的读数u" role="presentation" style="position: relative;">u$u$,以及传感器的读数z" role="presentation" style="position: relative;">z$z$,如何求解定位问题（估计x" role="presentation" style="position: relative;">x$x$）和建图问题（估计y" role="presentation" style="position: relative;">y$y$）?

我们就把slam问题建模成了一个状态估计问题：如何通过带有噪声的测量数据，估计内部的、隐藏着的状态变量？

第3讲：三维空间刚体运动

三位空间的刚体运动描述方式：旋转矩阵、变换矩阵、四元数和欧拉角。

3.1旋转矩阵

1、内积

                                       a⋅b=aTb=∑i=13aibi=|a||b|cos⟨a,b⟩" role="presentation" style="position: relative;">a⋅b=aTb=∑3i=1aibi=|a||b|cos⟨a,b⟩$a\cdot b=a^{T}b=\sum _{i=1}^3{a}_i{b}_i=\left|a\right|\left|b\right|cos⟨a,b⟩$

⟨a,b⟩" role="presentation" style="position: relative;">⟨a,b⟩$⟨a,b⟩$指向量a→,b→" role="presentation" style="position: relative;">a⃗ ,b⃗ $\overrightarrow{a},\overrightarrow{b}$的夹角。 也可以描述向量间的投影关系。

2、外积

a×b=‖e1e2e3a1a2a3b1b2b3‖=[a2b3−a3b2a3b1−a1b3a1b2−a2b1]=[0−a3a2a30−a1−a2a10]b=defa∧b" role="presentation" style="position: relative;">a×b=∥∥∥∥e1a1b1e2a2b2e3a3b3∥∥∥∥=⎡⎣⎢a2b3−a3b2a3b1−a1b3a1b2−a2b1⎤⎦⎥=⎡⎣⎢0a3−a2−a30a1a2−a10⎤⎦⎥bdef=a∧b$a\times b=\Vert \begin{array}{ccc}{e}_1& e_2& e_3\\ a_1& a_2& a_3\\ b_1& b_2& b_3\end{array}\Vert =\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]b_{=}^{def}{a}^{\wedge }b$

 外积的结果是一个向量，他的方向垂直于这两个向量，大小为|a||b|sin⟨a,b⟩" role="presentation" style="position: relative;">|a||b|sin⟨a,b⟩$\left|a\right|\left|b\right|sin⟨a,b⟩$，是两个向量张成的四边形的有向面积。

反对称符号：^

反对称矩阵：

                                                                  a∧=[0−a3a2a30a1−a2a10]" role="presentation" style="position: relative;">a∧=⎡⎣⎢0a3−a2−a30a1a2a10⎤⎦⎥$a^{\wedge }=\left[\begin{array}{ccc}0& -a_3& a_2\\ a_3& 0& a_1\\ -a_2& a_1& 0\end{array}\right]$

 任意向量都对应着唯一的一个反对称矩阵，反之亦然。向量的加减法和内外积与坐标系无关。

3、刚体运动 ：两个坐标之间的运动由一个旋转加上一个平移组成，这种运动成为刚体运动。

欧式变换由旋转和平移组成。

 矩阵R描述了旋转本身。成为旋转矩阵（Rotation Matrix）。同时，该矩阵各分量是两个坐标系基的内积，由于基向量的程度为1，所以是各基向量夹角的余弦值。所以这个矩阵也叫方向余弦矩阵（Direction Cosine Matrix).

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

                                SO(n)={ℜ∈Rn×n|ℜℜT=I,det(ℜ)=1}" role="presentation" style="position: relative;">SO(n)={R∈Rn×n|RRT=I,det(R)=1}$SO(n)=\left\{\mathrm{\Re }\in {\mathbb{R}}^{n\times n}|\mathrm{\Re }{\mathrm{\Re }}^T=I,det(\mathrm{\Re })=1\right\}$

 SO(n)" role="presentation" style="position: relative;">SO(n)$SO(n)$是特殊正交群。这个集合是由n维空间的旋转矩阵组成，特别地，SO(3)" role="presentation" style="position: relative;">SO(3)$SO(3)$指三维空间的旋转。

由于旋转矩阵为正交矩阵，它的逆（转置）描述了一个相反的旋转。按照上面的定义方式，有

        ​​​​​​​        ​​​​​​​        ​​​​​​​        ​​​​​​​        ​​​​​​​        ​​​​​​​        a′=R−1a=RTa" role="presentation" style="position: relative;">a′=R−1a=RTa$a^{{}^{\prime }}=R^{-1}a=R^{T}a$

显然，RT" role="presentation" style="position: relative;">RT$R^T$刻画了一个相反的旋转。 

加上一次平移之后，

         ​​​​​​​        ​​​​​​​        ​​​​​​​        ​​​​​​​        ​​​​​​​        ​​​​​​​        a′=Ra+t" role="presentation" style="position: relative;">a′=Ra+t$a^{{}^{\prime }}=Ra+t$

实际中，我们会定义坐标系1、坐标系2，那么向量a→" role="presentation" style="position: relative;">a⃗ $\overrightarrow{a}$在两个坐标系下的坐标为a1→" role="presentation" style="position: relative;">a1→$\overrightarrow{a_1}$,a2→" role="presentation" style="position: relative;">a2→$\overrightarrow{a_2}$，它们之间的关系是：

        ​​​​​​​        ​​​​​​​        ​​​​​​​        ​​​​​​​        ​​​​​​​        ​​​​​​​        ​​​​​​​  a1=R12a2+t12" role="presentation" style="position: relative;">a1=R12a2+t12$a_1=R_{12}{a}_2+t_{12}$ 

R12" role="presentation" style="position: relative;">R12$R_{12}$是指“把坐标系2的向量变换到坐标系1”中。

关于平移t12" role="presentation" style="position: relative;">t12$t_{12}$它实际对应的是坐标系1原点指向坐标系2原点的向量，在坐标系1下去的坐标。

4、变换矩阵与齐次坐标

假设进行了两次变换：R1,t1" role="presentation" style="position: relative;">R1,t1$R_1,t_1$和R2,t2" role="presentation" style="position: relative;">R2,t2$R_2,t_2$：

        ​​​​​​​        ​​​​​​​        ​​​​​​​        ​​​​​​​        ​​​​​​​        b=R1a+t1,c=R2b+t2" role="presentation" style="position: relative;">b=R1a+t1,c=R2b+t2$b=R_{1}a+t_1,c=R_{2}b+t_2$

那么，从a→" role="presentation" style="position: relative;">a⃗ $\overrightarrow{a}$到c→" role="presentation" style="position: relative;">c⃗ $\overrightarrow{c}$的变换为

        ​​​​​​​        ​​​​​​​        ​​​​​​​        ​​​​​​​        ​​​​​​​        c=R2(R1a+t1)+t2" role="presentation" style="position: relative;">c=R2(R1a+t1)+t2$c=R_2(R_{1}a+t_1)+t_2$ 

引入齐次坐标和变换矩阵：

                                         [a′1]=[Rt0T1][a1]def=T[a1]" role="presentation" style="position: relative;">[a′1]=[R0Tt1][a1]def=T[a1]$\left[\begin{array}{c}{a}^{{}^{\prime }}\\ 1\end{array}\right]=\left[\begin{array}{cc}R& t\\ 0^T& 1\end{array}\right]\left[\begin{array}{c}a\\ 1\end{array}\right]\begin{array}{c}def\\ =\end{array}T\left[\begin{array}{c}a\\ 1\end{array}\right]$

数学技巧：在三维向量的末位添加1，将其变为四维向量，称为齐次坐标。对于四维向量可以把旋转和平移写在一个矩阵里，使得整个关系变成线性关系，矩阵T" role="presentation" style="position: relative;">T$T$称为变换矩阵（Transform Matrix）。

关于变换矩阵T" role="presentation" style="position: relative;">T$T$具有特殊的结构：左上角为旋转矩阵，右侧为平移变量，左下角为0→" role="presentation" style="position: relative;">0⃗ $\overrightarrow{0}$，右下角为1.这种矩阵又称为特殊欧氏群（Special Euclidean Group）：

        ​​​​​​​        ​​​​​​​        ​​​​​​​        ​​​​​​​SE(3)={T=[Rt01]∈Rn×n|R∈SO(3),t∈R3}" role="presentation" style="position: relative;">SE(3)={T=[R0t1]∈Rn×n|R∈SO(3),t∈R3}$SE(3)=\left\{T=\left[\begin{array}{cc}R& t\\ 0& 1\end{array}\right]\in {\mathbb{R}}^{n\times n}|R\in SO(3),t\in {\mathbb{R}}^3\right\}$

与SO(3)一样，求解该矩阵的逆表示一个反向的变换：

        ​​​​​​​        ​​​​​​​        ​​​​​​​        ​​​​​​​        ​​​​​​​        T−1=[RT−RTt0T1]" role="presentation" style="position: relative;">T−1=[RT0T−RTt1]$T^{-1}=\left[\begin{array}{cc}{R}^T& -R^{T}t\\ 0^T& 1\end{array}\right]$

当写Ta" role="presentation" style="position: relative;">Ta$Ta$时，使用的是齐次坐标。而写时，使用的是非齐次坐标。 

Eigen是一个C++开源线性代数库。它提供了快速的有关矩阵的线性代数运算，还包括解方程等。

它是一个纯用头文件搭建起来的库，只需要引入Eigen的头文件即可，不需要链接库文件。

Ubuntu 安装：

sudo apt install libeigen3-dev

查找命令：

sudo updatedb
locate eigen3

3.3旋转向量和欧拉角

矩阵表示的缺点：

1. SO(3)的旋转有9个量，但一次旋转只有3个自由度。因此表达式是冗余的。
2. 旋转矩阵自身带有约束：他必须是个正交矩阵，且行列式为1.变换矩阵也是如此，这让求解变得更加困难。 

任意一个旋转都可以用一个旋转轴和一个旋转角来刻画。

旋转向量：方向与旋转轴一致，长度等于旋转角。

 考虑某个旋转用R表示。如果用旋转向量来描述，假设旋转轴为一个单位长度的向量n→" role="presentation" style="position: relative;">n⃗ $\overrightarrow{n}$,角度为θ" role="presentation" style="position: relative;">θ$\theta$，那么向量θn→" role="presentation" style="position: relative;">θn⃗ $\theta \overrightarrow{n}$也可以描述这个旋转。

从旋转向量到旋转矩阵的转换过程由罗德里格斯公式表明，

        ​​​​​​​        ​​​​​​​        ​​​​​​​        ​​​​​​​        R=cosθI+(1−cosθ)n→n→T+sinθn∧" role="presentation" style="position: relative;">R=cosθI+(1−cosθ)n⃗ n⃗ T+sinθn∧$R=cos\theta I+(1-cos\theta )\overrightarrow{n}{\overrightarrow{n}}^T+sin\theta n^{\wedge }$

符号^是向量到反对称矩阵的转换符。反之。我们可以从一个旋转矩阵到旋转向量的转换。对于转角θ" role="presentation" style="position: relative;">θ$\theta$，取两边的迹，有

        ​​​​​​​        ​​​​​​​        tr(R)=cosθtr(I)+(1−cosθ)tr(nnT+sinθtr(n∧))" role="presentation" style="position: relative;">tr(R)=cosθtr(I)+(1−cosθ)tr(nnT+sinθtr(n∧))$tr(R)=cos\theta tr(I)+(1-cos\theta )tr(n{n}^T+sin\theta tr(n^{\wedge }))$

        ​​​​​​​        ​​​​​​​        ​​​​​​​        ​​​​​​​   =3cosθ+(1−cosθ)=1+2cosθ" role="presentation" style="position: relative;">=3cosθ+(1−cosθ)=1+2cosθ$=3cos\theta +(1-cos\theta )=1+2cos\theta$

 因此

        ​​​​​​​        ​​​​​​​        ​​​​​​​        θ=arccostr(R)−12" role="presentation" style="position: relative;">θ=arccostr(R)−12$\theta =arccos\frac{tr(R)-1}{2}$

关于旋转轴n→" role="presentation" style="position: relative;">n⃗ $\overrightarrow{n}$,旋转轴上的向量在旋转后不发生改变，说明：

        ​​​​​​​        ​​​​​​​        ​​​​​​​        ​​​​​​​        ​​​​​​​        ​​​​​​​        Rn=n" role="presentation" style="position: relative;">Rn=n$Rn=n$ 

因此，旋转轴是矩阵R特征值1对应的特征向量。求解此方程，再归一化，就得到了旋转轴。

欧拉角： 

        使用3个分离的转角，把一个旋转分解成3次绕不同轴的旋转。

“偏航-俯仰-滚转”（yaw-pitch-roll）的旋转顺序等价于ZYX轴的旋转。

[r,p,y]T" role="presentation" style="position: relative;">[r,p,y]T${\left[r,p,y\right]}^T$这样一个三维向量可以描述任意旋转。

欧拉角的重大缺点是会碰到万向锁问题（Gimbal Lock）：在俯仰角为±90∘" role="presentation" style="position: relative;">±90∘$\pm {90}^{\circ }$时，第一次旋转与第三次旋转将使用同一个轴。

3.4四元数 

四元数是Hamilton找到的一种扩展的复数。它既是紧凑的，也没有奇异性。

一个四元数有1个实部3个虚部。

        ​​​​​​​        ​​​​​​​        ​​​​​​​        ​​​​​​​        q=q0+q1i+q2j+q3k" role="presentation" style="position: relative;">q=q0+q1i+q2j+q3k$q=q_0+q_{1}i+q_{2}j+q_{3}k$ 

其中，i，j，k 为四元数的三个虚部。这三个虚部满足以下关系式：

        ​​​​​​​        ​​​​​​​        ​​​​​​​        ​​​​​​​        {i2=j2=k2=−1ij=k,ji=−kjk=i,kj=−iki=j,ik=−j" role="presentation" style="position: relative;">⎧⎩⎨⎪⎪⎪⎪⎪⎪i2=j2=k2=−1ij=k,ji=−kjk=i,kj=−iki=j,ik=−j$\{\begin{array}{c}{i}^2=j^2=k^2=-1\\ ij=k,ji=-k\\ jk=i,kj=-i\\ ki=j,ik=-j\end{array}$

人们也用一个标量和一个向量来表达四元数：

        ​​​​​​​        ​​​​​​​        ​​​​​​​        q=[s,v→]T,s=q0∈R,v→=[q1,q2,q3]T∈R3" role="presentation" style="position: relative;">q=[s,v⃗ ]T,s=q0∈R,v⃗ =[q1,q2,q3]T∈R3$q={\left[s,\overrightarrow{v}\right]}^T,s=q_0\in \mathbb{R},\overrightarrow{v}={\left[q_1,q_2,q_3\right]}^T\in {\mathbb{R}}^3$ 

s称为四元数的实部，v→" role="presentation" style="position: relative;">v⃗ $\overrightarrow{v}$称为四元数的虚部。若虚部为0→" role="presentation" style="position: relative;">0⃗ $\overrightarrow{0}$，则称为实四元数 。若实部为0，则称虚四元数。

可以用单位四元数表示三维空间中任意一个旋转。

1、四元数加减法：

        ​​​​​​​        ​​​​​​​        ​​​​​​​        ​​​​​​​        qa±qb=[sa±sb,va±vb]T" role="presentation" style="position: relative;">qa±qb=[sa±sb,va±vb]T$q_a\pm q_b={\left[s_a\pm s_b,v_a\pm v_b\right]}^T$

2、乘法(向量外积运算形式)：

         ​​​​​​​        ​​​​​​​        ​​​​​​​        qaqb=[sasb−vaTvb,savb+sbva+va×vb]T" role="presentation" style="position: relative;">qaqb=[sasb−vTavb,savb+sbva+va×vb]T$q_a{q}_b={\left[s_a{s}_b-v_a^T{v}_b,s_a{v}_b+s_b{v}_a+v_a\times v_b\right]}^T$

 四元数乘法不可交换的，除非va" role="presentation" style="position: relative;">va$v_a$和vb" role="presentation" style="position: relative;">vb$v_b$在R3" role="presentation" style="position: relative;">R3${\mathbb{R}}^3$中共线，外积项为0.

两个四元数的乘积仍是实的，这与复数一致。

3、模长

        ​​​​​​​        ​​​​​​​        ​​​​​​​        ​​​​​​​        ‖q‖=sa2+xa2+ya2+za2" role="presentation" style="position: relative;">∥q∥=s2a+x2a+y2a+z2a−−−−−−−−−−−−−−√$\Vert q\Vert =\sqrt{s_a^2+x_a^2+y_a^2+z_a^2}$

        ​​​​​​​        ​​​​​​​        ​​​​​​​        ​​​​​​​        ‖qaqb‖=‖qa‖‖qb‖" role="presentation" style="position: relative;">∥qaqb∥=∥qa∥∥qb∥$\Vert q_a{q}_b\Vert =\Vert q_a\Vert \Vert q_b\Vert$

四元数乘积的模即模的乘积。

4、四元数的共轭是把虚部取成相反数：

        ​​​​​​​        ​​​​​​​        ​​​​​​​        qa∗=sa−xai−yaj−zak=[sa,−va]T" role="presentation" style="position: relative;">q∗a=sa−xai−yaj−zak=[sa,−va]T$q_a^{\ast }=s_a-x_{a}i-y_{a}j-z_{a}k={\left[s_a,-v_a\right]}^T$ 

        ​​​​​​​        ​​​​​​​        ​​​​​​​        q∗q=qq∗=[sa2+vTv,0]T" role="presentation" style="position: relative;">q∗q=qq∗=[s2a+vTv,0]T$q^{\ast }q=q{q}^{\ast }={\left[s_a^2+v^{T}v,0\right]}^T$ 

5、逆

        ​​​​​​​        ​​​​​​​        ​​​​​​​        ​​​​​​​ q−1=q∗/‖q‖2" role="presentation" style="position: relative;">q−1=q∗/∥q∥2$q^{-1}=q^{\ast }/{\Vert q\Vert }^2$ 

        ​​​​​​​        ​​​​​​​        ​​​​​​​        ​​​​​​​ q−1q=qq−1=1" role="presentation" style="position: relative;">q−1q=qq−1=1$q^{-1}q=q{q}^{-1}=1$         

          ​​​​​​​        ​​​​​​​        ​​​​​​​       (qaqb)−1=qb−1qa−1" role="presentation" style="position: relative;">(qaqb)−1=q−1bq−1a$(q_a{q}_b{)}^{-1}=q_b^{-1}{q}_a^{-1}$

 6、数乘、

                                 kq→=[ks,kv→]T" role="presentation" style="position: relative;">kq⃗ =[ks,kv⃗ ]T$k\overrightarrow{q}={\left[ks,k\overrightarrow{v}\right]}^T$

四元数到其他旋转表示的转换

设q=[s,v→]T" role="presentation" style="position: relative;">q=[s,v⃗ ]T$q={\left[s,\overrightarrow{v}\right]}^T$,那么定义如下的符号+和⊕" role="presentation" style="position: relative;">⊕$\oplus$为：

        ​​​​​​​        ​​​​​​​        ​​​​​​​        ​​​​​​​q+=[s−vTvsI+v∧],q⊕=[s−vTvsI−v∧]" role="presentation" style="position: relative;">q+=[sv−vTsI+v∧],q⊕=[sv−vTsI−v∧]$q^{+}=\left[\begin{array}{cc}s& -v^T\\ v& sI+v^{\wedge }\end{array}\right],q^{\oplus }=\left[\begin{array}{cc}s& -v^T\\ v& sI-v^{\wedge }\end{array}\right]$

   可证：                              q1q2=q1+q2=q1⊕q1" role="presentation" style="position: relative;">q1q2=q+1q2=q⊕1q1$q_1{q}_2=q_1^{+}{q}_2=q_1^{\oplus }{q}_1$

 四元数到旋转矩阵的变换：

        ​​​​​​​        ​​​​​​​        ​​​​​​​        ​​​​​​​        ​​​​​​​        R=vvT+s2I+2sv∧+(v∧)2" role="presentation" style="position: relative;">R=vvT+s2I+2sv∧+(v∧)2$R=v{v}^T+s^{2}I+2s{v}^{\wedge }+(v^{\wedge }{)}^2$

四元数到旋转向量的转换公式：

        ​​​​​​​        ​​​​​​​        ​​​​​​​        ​​​​​​​        ​​​​​​​        ​​​​​​​{θ=2arccosq0[nx,ny,nz]T=[q1,q2,q3]T/sinθ2" role="presentation" style="position: relative;">{θ=2arccosq0[nx,ny,nz]T=[q1,q2,q3]T/sinθ2$\{\begin{array}{c}\theta =2arccos{q}_0\\ [n_x,n_y,n_z{]}^T=[q_1,q_2,q_3{]}^T/sin\frac{\theta }{2}\end{array}$ 

李群与李代数

 什么样的相机位姿最符合当前观测数据？求得最优的R,t使得误差最小化。

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

三维旋转矩阵构成了特殊正交群SO(3); 变换矩阵构成了特殊欧式群SE(3).

        ​​​​​​​        ​​​​​​​        ​​​​​​​        SO(3)=R∈R3×3|RRT=I,det(R)=1" role="presentation" style="position: relative;">SO(3)=R∈R3×3|RRT=I,det(R)=1$SO(3)=R\in {\mathbb{R}}^{3\times 3}|R{R}^T=I,det(R)=1$

        ​​​​​​​        ​​​​​​​        ​​​​​​​        SE(3)={T=[Rt0T1]∈R4×4|R∈SO(3),T∈R3}" role="presentation" style="position: relative;">SE(3)={T=[R0Tt1]∈R4×4|R∈SO(3),T∈R3}$SE(3)=\left\{T=\left[\begin{array}{cc}R& t\\ 0^T& 1\end{array}\right]\in {\mathbb{R}}^{4\times 4}|R\in SO(3),T\in {\mathbb{R}}^3\right\}$

旋转矩阵和变换矩阵对加法不封闭：任意两个旋转矩阵的和不再是旋转矩阵。SO(3)和SE(3)关于乘法封闭。

        ​​​​​​​        ​​​​​​​        ​​​​​​​        ​​​​​​​  R1R2∈SO(3),T1T2∈SE(3)" role="presentation" style="position: relative;">R1R2∈SO(3),T1T2∈SE(3)$R_1{R}_2\in SO(3),T_1{T}_2\in SE(3)$

乘法对应着旋转或变换的复合，两个旋转矩阵相乘表示做了两次旋转。对于这种只有一个（良好）运算的集合，称为群。

群（Group）：一种集合加上一种运算的代数结构。集合记作A" role="presentation" style="position: relative;">A$A$，运算记作⋅" role="presentation" style="position: relative;">⋅$\cdot$，那么G=(A,⋅)" role="presentation" style="position: relative;">G=(A,⋅)$G=(A,\cdot )$

1. 封闭性：∀a1,a2∈A,a1⋅a2∈A" role="presentation" style="position: relative;">∀a1,a2∈A,a1⋅a2∈A$\mathrm{\forall }{a}_1,a_2\in A,a_1\cdot a_2\in A$
2. 结合律：∀a1,a2,a3∈A,(a1⋅a2)⋅a3=a1⋅(a2⋅a3)" role="presentation" style="position: relative;">∀a1,a2,a3∈A,(a1⋅a2)⋅a3=a1⋅(a2⋅a3)$\mathrm{\forall }{a}_1,a_2,a_3\in A,(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" role="presentation" style="position: relative;">∃a0∈A,s.t.∀a∈A,a0⋅a=a⋅a0=a$\mathrm{\exists }{a}_0\in A,s.t.\mathrm{\forall }a\in A,a_0\cdot a=a\cdot a_0=a$
4. 逆：

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

每个李群都有与之对应的李代数。李代数描述了李群的局部性质，准确的说，是单位元附近的正切空间。一般的李代数定义如下：

李代数由一个集合V、一个数域和一个二元运算[,]组成。如果满足一下几条性质，则称（V,F,[,]）为一个李代数，记作g。

1. 封闭性   ∀X,Y∈V,[X,Y]∈V" role="presentation" style="position: relative;">∀X,Y∈V,[X,Y]∈V$\mathrm{\forall }X,Y\in V,[X,Y]\in V$
2. 双线性    ∀X,YZ∈V,a,b∈F," role="presentation" style="position: relative;">∀X,YZ∈V,a,b∈F,$\mathrm{\forall }X,YZ\in V,a,b\in F,$有[aX+bY,Z]=a[X,Z]+b[Y,Z],[Z,aX+bY]=a[Z,X]+b[Z,Y]" role="presentation" style="position: relative;">[aX+bY,Z]=a[X,Z]+b[Y,Z],[Z,aX+bY]=a[Z,X]+b[Z,Y]$[aX+bY,Z]=a[X,Z]+b[Y,Z],[Z,aX+bY]=a[Z,X]+b[Z,Y]$
3. 自反性   ∀X∈V,[X,X]=0" role="presentation" style="position: relative;">∀X∈V,[X,X]=0$\mathrm{\forall }X\in V,[X,X]=0$
4. 雅可比等价  ∀X,Y,Z∈V,[X,[Y,Z]]+[Z,[X,Y]]+[Y,[Z,X]]=0" role="presentation" style="position: relative;">∀X,Y,Z∈V,[X,[Y,Z]]+[Z,[X,Y]]+[Y,[Z,X]]=0$\mathrm{\forall }X,Y,Z\in V,[X,[Y,Z]]+[Z,[X,Y]]+[Y,[Z,X]]=0$

其中，二元运算被称为李括号。

李代数so(3)" role="presentation" style="position: relative;">so(3)$so(3)$

SO(3)对应的李代数是定义在R3" role="presentation" style="position: relative;">R3${\mathbb{R}}^3$上的向量，我们记作ϕ" role="presentation" style="position: relative;">ϕ$\varphi$。每个ϕ" role="presentation" style="position: relative;">ϕ$\varphi$​​​​​​​

都可以生成一个反对称矩阵：

        ​​​​​​​        ​​​​​​​        ​​​​​​​        \Phi =\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\times 3}" role="presentation" style="position: relative;">\Phi =\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\times 3}$\text{Phi =phi ^{wedge }=begin{bmatrix } 0 \& -phi \_3 \& phi \_2 phi \_3 \& 0 \& -phi \_1 -phi \_2 \& phi \_1 \& 0 end{bmatrix}in mathbb{R}^{3times 3}}$ 

        ​​​​​​​        ​​​​​​​        ​​​​​​​        so(3)={ϕ∈R3,Φ=ϕ∧∈R3×3}" role="presentation" style="position: relative;">so(3)={ϕ∈R3,Φ=ϕ∧∈R3×3}$so(3)=\left\{\varphi \in {\mathbb{R}}^3,\mathrm{\Phi }={\varphi }^{\wedge }\in {\mathbb{R}}^{3\times 3}\right\}$

 so(3)" role="presentation" style="position: relative;">so(3)$so(3)$是一个由三维向量组成的集合，每个向量对应一个反对称矩阵，可以用于表达旋转矩阵的倒数。它与SO(3)的关系由指数映射给定：

        ​​​​​​​        ​​​​​​​        ​​​​​​​        R=exp(ϕ∧)" role="presentation" style="position: relative;">R=exp(ϕ∧)$R=exp({\varphi }^{\wedge })$

 李代数se(3)" role="presentation" style="position: relative;">se(3)$se(3)$

对于SE(3)，有对应的李代数se(3)" role="presentation" style="position: relative;">se(3)$se(3)$，se(3)" role="presentation" style="position: relative;">se(3)$se(3)$位于R6" role="presentation" style="position: relative;">R6${\mathbb{R}}^6$空间中：

        ​​​​​​​        ​​​​​​​        ​​​​​​​ se(3)={ξ=[ρϕ]∈R6,ϱ∈R3,ϕ∈so(3),ξ∧=[ϕ∧ρ0T0]∈R4×4}" role="presentation" style="position: relative;">se(3)={ξ=[ρϕ]∈R6,ϱ∈R3,ϕ∈so(3),ξ∧=[ϕ∧0Tρ0]∈R4×4}$se(3)=\left\{\xi =\left[\begin{array}{c}\rho \\ \varphi \end{array}\right]\in {\mathbb{R}}^6,\varrho \in {\mathbb{R}}^3,\varphi \in so(3),{\xi }^{\wedge }=\left[\begin{array}{cc}{\varphi }^{\wedge }& \rho \\ 0^T& 0\end{array}\right]\in {\mathbb{R}}^{4\times 4}\right\}$

 我们把每个se(3)" role="presentation" style="position: relative;">se(3)$se(3)$元素记作，它是一个6维向量。前三维为平移（但含义与变换矩阵中的平移不同）记作ρ" role="presentation" style="position: relative;">ρ$\rho$；后三维为旋转，记作ϕ" role="presentation" style="position: relative;">ϕ$\varphi$，实质上是 so(3)" role="presentation" style="position: relative;">so(3)$so(3)$元素。

使用∧" role="presentation" style="position: relative;">∧$\wedge$和∨" role="presentation" style="position: relative;">∨$\vee$符号指代“从向量到矩阵”和“从矩阵到向量”的关系。

指数与对数映射

任意矩阵的指数映射可以写成一个泰勒展开，但是只有在收敛的情况下才会有结果，其结果仍是一个矩阵：

        ​​​​​​​        ​​​​​​​        ​​​​​​​        exp(A)=∑n=0∞1n!An" role="presentation" style="position: relative;">exp(A)=∑∞n=01n!An$exp(A)=\sum _{n=0}^{\mathrm{\infty }}\frac{1}{n!}{A}^n$

对于so(3)" role="presentation" style="position: relative;">so(3)$so(3)$中的任意元素ϕ" role="presentation" style="position: relative;">ϕ$\varphi$，也可按此方式定义它的指数映射：

        ​​​​​​​        ​​​​​​​        ​​​​​​​        exp(ϕ∧)=∑n=0∞1n!(ϕ∧)n" role="presentation" style="position: relative;">exp(ϕ∧)=∑∞n=01n!(ϕ∧)n$exp({\varphi }^{\wedge })=\sum _{n=0}^{\mathrm{\infty }}\frac{1}{n!}({\varphi }^{\wedge }{)}^n$

ϕ" role="presentation" style="position: relative;">ϕ$\varphi$三维向量，定义其模长和方向，分别记作θ" role="presentation" style="position: relative;">θ$\theta$和a" role="presentation" style="position: relative;">a$a$,   于是ϕ=θa" role="presentation" style="position: relative;">ϕ=θa$\varphi =\theta a$,这里a是一个长度为1 的方向向量，即‖a‖=1" role="presentation" style="position: relative;">∥a∥=1$\Vert a\Vert =1$.首先，对于a∧" role="presentation" style="position: relative;">a∧$a^{\wedge }$，有一下两条性质：

        ​​​​​​​        ​​​​​​​        ​​​​​​​        a∧a∧=aa∧−I" role="presentation" style="position: relative;">a∧a∧=aa∧−I$a^{\wedge }{a}^{\wedge }=a{a}^{\wedge }-I$

                                 a∧a∧a∧=a∧(aaT−I)=−a∧" role="presentation" style="position: relative;">a∧a∧a∧=a∧(aaT−I)=−a∧$a^{\wedge }{a}^{\wedge }{a}^{\wedge }=a^{\wedge }(a{a}^T-I)=-a^{\wedge }$

        ​​​​​​​        ​​​​​​​        ​​​​​​​        exp(ϕ∧)=exp(θa∧)=∑n=0∞1n!(θa∧)n" role="presentation" style="position: relative;">exp(ϕ∧)=exp(θa∧)=∑∞n=01n!(θa∧)n$exp({\varphi }^{\wedge })=exp(\theta a^{\wedge })=\sum _{n=0}^{\mathrm{\infty }}\frac{1}{n!}(\theta a^{\wedge }{)}^n$

        ​​​​​​​        ​​​​​​​        ​​​​​​​        ​​​​​​​        ​​​​​​​        =cosθ+(1−cosθ)aaT+sinθa∧" role="presentation" style="position: relative;">=cosθ+(1−cosθ)aaT+sinθa∧$=cos\theta +(1-cos\theta )a{a}^T+sin\theta a^{\wedge }$

最后得到一个似曾相识的式子：

        ​​​​​​​        ​​​​​​​        ​​​​​​​        exp(θa∧)=cosθI+(1−cosθ)aaT+sinθa∧" role="presentation" style="position: relative;">exp(θa∧)=cosθI+(1−cosθ)aaT+sinθa∧$exp(\theta a^{\wedge })=cos\theta I+(1-cos\theta )a{a}^T+sin\theta a^{\wedge }$ 

它和罗德里格斯公式如出一辙。 so(3)" role="presentation" style="position: relative;">so(3)$so(3)$实际上就是所谓的旋转向量组成的空间，而指数映射即罗德里格斯公式。通过他们，我们把​​​​​​​so(3)" role="presentation" style="position: relative;">so(3)$so(3)$中任意一个向量对应到了一个位于SO(3)中的旋转矩阵。反之，如果定义对数映射，也能把SO(3)中的元素对应到so(3)" role="presentation" style="position: relative;">so(3)$so(3)$ 中：

        ​​​​​​​        ​​​​​​​        ​​​​​​​        ϕ=ln(R)∨=(∑n=0∞(−1)nn+1(R−I)n+1)∨" role="presentation" style="position: relative;">ϕ=ln(R)∨=(∑∞n=0(−1)nn+1(R−I)n+1)∨$\varphi =ln(R{)}^{\vee }={\left(\sum _{n=0}^{\mathrm{\infty }}\frac{(-1{)}^n}{n+1}(R-I{)}^{n+1}\right)}^{\vee }$ 

和指数映射一样，我们没有必要直接用泰勒展开计算对数映射。利用迹的性质分别求解转角和转轴，采用这种方式更省事。

se(3)" role="presentation" style="position: relative;">se(3)$se(3)$上的指数映射形式如下：

        ​​​​​​​        ​​​​​​​        ​​​​​​​        exp(ξ∧)=[∑n=0∞1n!(ϕ∧)n∑n=0∞1(n+1)!(ϕ∧)nρ0T1]" role="presentation" style="position: relative;">exp(ξ∧)=[∑∞n=01n!(ϕ∧)n0T∑∞n=01(n+1)!(ϕ∧)nρ1]$exp({\xi }^{\wedge })=\left[\begin{array}{cc}\sum _{n=0}^{\mathrm{\infty }}\frac{1}{n!}({\varphi }^{\wedge }{)}^n& \sum _{n=0}^{\mathrm{\infty }}\frac{1}{(n+1)!}({\varphi }^{\wedge }{)}^n\rho \\ 0^T& 1\end{array}\right]$

        ​​​​​​​        ​​​​​​​        ​​​​​​​        ​​​​​​​        ​​​​​​​        =[RJρ0T1]=T" role="presentation" style="position: relative;">=[R0TJρ1]=T$=\left[\begin{array}{cc}R& J\rho \\ 0^T& 1\end{array}\right]=T$

        ​​​​​​​        ​​​​​​​        ​​​​​​​        J=sinθθI+(1−sinθθ)aaT+1−cosθθa∧" role="presentation" style="position: relative;">J=sinθθI+(1−sinθθ)aaT+1−cosθθa∧$J=\frac{sin\theta }{\theta }I+(1-\frac{sin\theta }{\theta })a{a}^T+\frac{1-cos\theta }{\theta }{a}^{\wedge }$

        ​​​​​​​        ​​​​​​​        ​​​​​​​        ​​​​​​​t=Jρ" role="presentation" style="position: relative;">t=Jρ$t=J\rho$