【toc】目录

一、旋转矩阵

二、旋转向量

三、欧拉角

四、四元数

五、李群和李代数

SLAM：Simultaneous Localization and Mapping同时定位与地图构建
搭载特定传感器的主体，在没有环境先验信息的情况下，于是运动过程中建立环境的模型，同时估计自己的运动。
视觉SLAM：以相机为主要传感器的SLAM。
问题：从图像中估计相机的运动以及环境情况。
应用场景：无人驾驶，机器人，AR、VR

前言

自主运动的两大基本问题：定位，建图。定位侧重于对自身的了解，而建图侧重对外的了解。这是相互耦合的两个问题，准确的定位需要精确的地图，精确的地图来自准确的定位。

How to do SLAM？——sensors
- 安装与环境中：二维码，GPS，导轨，磁条
- 携带于机器人本体：
IMU：Inertial measurement unit，测量物体三轴姿态角(或角速率)以及加速度的装置。一般情况下，一个IMU内会装有三轴的陀螺仪和三个方向的加速度计，分别用来测量物体在三维空间中的角速度和加速度，并以此解算出物体的姿态。
激光，相机

当下基于激光传感器的建图已经比较成熟了（尤其是在学术界），但是由于其自身重量和价格应用市场受到限制，因此目前基于视觉的三维场景重建还是相当有搞头的。相机通过一定速率采集图像，最终形成视频。但是以二维投影形式记录三维视觉，此过程丢掉了一个重要的维度：距离，或者叫深度。因此需要利用图像和场景的几何关系，计算相机运动和场景结构（Motion & Structure）

• 单目相机：没有深度，必须通过移动相机产生深度Moving View Stereo。这是因为当相机运动起来时，场景和成像有几何关系，近处的物体的像运动快，远处的物体的像运动慢，从而推断距离。
• 双目相机：通过视差计算深度Stereo。左右眼的微小差异判断远近，同样远处的物体变化小，近处的物体变化大，有点类似于古代打仗，炮兵用的跳眼法估计距离，一种简易测距方法.观察者面向目标,伸出右手大拇指于两眼之间,闭上左眼,用右眼通过拇指的一侧对准目标,然后用左眼通过拇指同一侧观察,记住左眼视线对准的物体,估算出该物体与目标之间的距离,然后乘以10倍,便是目标距离.。推算距离的计算量比较大。
• RGBD：通过物理方法测量深度。物理手段测量深度，结构光ToF（Time of flight），主动测量，功耗大，深度值较准确，量程较小，易受干扰。直接测量物体的图像和距离，从而恢复三维结构。室内用的较多。

• 视觉里程计Visual Odomentry
• 相邻图像估计相机运动，通过两张图像计算运动和结构，会出现不可能避免的飘逸，因此需要后端进行非线性优化
• 方法：特征点法和直接法
• 后端优化
• 从带有噪声的数据中优化轨迹和地图状态估计问题，最大后验概率估计MAP
• 方法：前期以EKF为代表，现以图优化为代表。
• 回环检测
• 检测机器人是否回到早先位置，识别达到过的场景，计算图像间的相似性
• 方法：词袋模型
• 建图
• 用于导航、规划、通讯、可视化、交互等
• 度量地图 vs 拓扑地图
• 稀疏地图 vs 稠密地图

补充一些数学知识：

一、旋转矩阵

点存在于三维空间之中，点和点可以组成向量，点本身由原点指向它的向量所描述。那么对于三维空间点的描述，定义完坐标系滞后，向量可由  坐标表示：

向量的内积： 

向量的外积： 

有个问题：坐标系之间如何变化的？进而：如何计算在同一个向量在不同坐标系里的坐标？

在SLAM中：固定的世界坐标系和移动的机器人坐标系；机器人坐标系随着机器人运动而改变，每时每刻都有新的坐标系。

在两个坐标系中，如何描述左侧到右侧的变化？直观来看，由两部分组成，原点间的平移和三个轴的旋转，平移可以看作是一个向量，那么旋转是什么？坐标关系：

左乘  ,得到：

中间的这个矩阵  成为旋转矩阵，根据定义可以验证，  是一个正交矩阵，  的行列式为+1。因此满足这两个性质的称之为旋转矩阵。

补充1：正交矩阵的定义
定义1：  或  (E的单位矩阵)，则称n阶实矩阵A成为正交矩阵。
定义2：n阶实矩阵A，若满足  ，则称A为正交矩阵。
定义3：n阶实矩阵A的n个行（列）向量是两两正交的单位向量，则称A为正交矩阵。

补充2：正交矩阵的性质
性质1：  ，  存在，并且也为正交矩阵。
性质2：  也是正交矩阵
性质3：若B也是正交矩阵，则  都为正交矩阵。

因此，对与三维空间的平移 和旋转的数学描述：两个坐标系的刚体运动完全可以由R，t完全描述：

其实用旋转+平移方式有一定的不便之处，比如发生了两次变换：

 ，此时 

叠加起来过于复杂,非齐次，于是，对齐稍微改变一下形式：向量化

经过多次变换： 

这种用四个数表达三维向量的做法成为齐次坐标，引入齐次坐标后，旋转和平移可以放入到同一个矩阵下，称为变换矩阵。

那么反向变换：

二、旋转向量

三维旋转：三自由度用  向量表示，方向为旋转轴、长度为转过的角度，称为角轴（Angle-Axis）或旋转向量（Rotation Vector）

那么这个角轴和旋转矩阵有什么不同？它们只是表达方式不同

• 旋转矩阵：9个量，有正交性约束
• 角轴：3个量，没有约束

那么二者的转换关系如何？轴角转旋转矩阵：罗德里格斯公式

• 角度： 
• 轴： 

三、欧拉角（Euler Angles）

将旋转分解为三次不同轴上的转动，但是轴可以是定轴或者动轴，而且顺序也可不同，因此存在许多种定义方式不同的欧拉角，常见的有yaw-pitch-roll（偏航-俯仰-滚转）角等等。

万向锁：Gimbal Lock，ZYX顺序中，若pitch为正负90度，则第三次旋转和第一次绕同一个轴，使得系统丢失了一个自由度，存在奇异性问题，使得计算中会出现一些奇怪的问题。因此，由于万向锁，欧拉角不适于插值和迭代，往往只用于人机交互中，可以证明，用三个实数表达三维旋转时，会不可避免地出现奇异性问题。在SLAM程序中很少直接使用欧拉角表达姿态。

四、四元数

四元数（Quaternion）是一种扩展的附属，四元数有三个虚部，可以表达三维空间中的旋转 

或者记作：  其中 

虚部之间的关系： 

那么，四元数和角轴的关系可以表示为：

• 角轴到四元数： 
• 四元数到角轴： 

类似地，四元数也可以转换为旋转矩阵、欧拉角。

那么如何使用四元数旋转一个空间点？四元数相较于角轴、欧拉角的优势：紧凑、无奇异性。

五、李群与李代数

• 什么是群？一种集合加上一种运算的代数结构。记集合为A，运算为 
• 1.封闭性： 
• 2.结合律： 
• 3.幺元： 
• 4. 逆： 

可以验证：旋转矩阵群和变换矩阵群

• 旋转矩阵集合和矩阵乘法构成群
• 变换矩阵和矩阵乘法也构成群
• 其他常见的群：
• 一般线性群GL(n)。指nxn的可逆矩阵，他们对矩阵乘法成群
• 特殊正交群SO(n)。所谓的旋转矩阵群，其中SO(2)和SO(3)最为常见
• 特殊欧式群SE(n)。即n维欧式变换，如SE(2)和SE(3)

群结构保证了群上的运算具有良好的性质。群论是研究群的各种结构和性质的理论。

• 李群(Lie Group)
• 具有连续(光滑)性质的群
• 群也是流形
• 直观上，一个刚体能够连续在空间中运动，故SO(2)和SO(3)都是李群
• 但是SO(3)和SE(3)只有定义域良好的乘法，可以有加法，所以难以进行取极限，求导等操作。
• 李代数：与李群对应的一种结构，位于向量空间
• 通常记作小写的so(3)和se(3)
• 事实上，李群单位元处的正切空间
• 李代数的引出
• 任意旋转矩阵R，满足： 

考虑到，R随时间进行变化，且考虑为光滑变化，有： 

两侧对时间求导： 

整理得： 

左边  是一个反对称矩阵，两侧右乘  得：

可以看出对R求导后，左侧多出一个 

考虑简单的情况：  ,泰勒展开，只保留一阶。

可见  反映了一阶导数性质，它位于正切空间上

在  附近，假设不变，有微分方程：

已知初始情况，  得到：

• 李代数
• 封闭性
• 双线性
• 自反性
• 雅可比等价性
• 李括号

每日一句毒鸡汤：

岁月是把杀猪刀是针对那些长得好看的说的，对于那些丑的，岁月拿他们一点办法也没有…

本次推荐：MouseInc 2.10.21