声明：本文是深蓝学院 高翔博士主讲的《SLAM理论与实践》的学习笔记。
点与坐标关系
向量可由坐标系中的点来表达。向量运算可由坐标运算表达：
- 加法、减法：对应坐标相加减
- 内积：
- 外积：
机器人在运动中涉及多种坐标系，例如 固定的世界坐标系、移动的机器人坐标系
、不同的传感器坐标系。如下图所示。
那么问题来了：
- 如何描述坐标系与坐标系之间的变化？答案：旋转＋平移
- 如何计算同一个向量在不同坐标系里的坐标？
旋转矩阵与变换矩阵表达运动
旋转
考虑一次旋转,
此处R称为旋转矩阵，可以验证Ｒ是一个正交矩阵且行列式为+1 （满足这两个性质的称为旋转矩阵）。
旋转矩阵的集合，称为特殊正交群SO(3)：Special Orthogonal Group
于是，１到２的旋转可以表达为：
反之２到１的旋转可以表达为：
矩阵关系：
变换矩阵
两个坐标系间的运动可用Ｒ,t完全描述。欧拉定理（Euler’s rotation theorem）：刚体在三维空间里的一般运动，可分解为刚体上某一点的平移，以及绕经过此点的旋转轴的转动。
旋转加平移在表达复合情况下有不便之处。
可以使用齐次形式的坐标（Homogeneous）+变换矩阵
此时可以方便表达复合运动。
变换矩阵的集合称为特殊欧氏群 SE(3) （Special Euclidean Group）
逆形式：
更好的表达方式：旋转向量/欧拉角/四元数
除了旋转矩阵/变换矩阵之外，还存在其他的表示方式，旋转矩阵 R 有九个元素，但仅有三个自由度，能否以更少的元素表达旋转？
旋转向量
方向为旋转轴，长度为转过的角度。称为角轴/轴角（Angle Axis）或旋转向量（Rotation Vector）
旋转向量与矩阵的不同：
- 仅有三个量
- 无约束
- 更直观
它们是同一个东西的不同表达方式
旋转向量->旋转矩阵：罗德里格斯公式（Rodrigues’s Formula）
旋转矩阵->旋转向量：
欧拉角
欧拉角（Euler Angles）将旋转分解为三个方向上的转动。例，按Z-Y-X顺序转动。
- 绕物体的Z轴旋转，得到偏航角yaw；
- 绕旋转之后的Y轴旋转，得到俯仰角pitch；
- 绕旋转之后的X轴旋转，得到滚转角roll
万向锁（Gimbal Lock）
欧拉角的奇异性问题，在特定值时，旋转自由度减1；Yaw-pitch-roll顺序下，当pitch为90度时，存在奇异性
由于万向锁的存在，欧拉角不适合插值或迭代，多用于人机交互中
可以证明：仅用三个实数表达旋转时，不可避免地存在奇异性问题
SLAM中亦很少用欧拉角表达姿态
四元数
2D 情况下，可用单位复数表达旋转,乘 i 即转90度，乘 –i 转-90度。只有一个旋转方向故而只有一个虚部。
3D情况下，可用四元数Quaternion（作为复数的扩充），四元数有三个虚部（对应三个旋转方向）和一个实部。
虚部之间满足关系：（自己和自己的运算像复数，自己和别人的运算像叉乘）
单位四元数可表达旋转，为理解旋转的计算方式，先看四元数间如何运算。
加法：
数乘：
求模：
共轭：
求倒：
对应元素乘法：
特殊乘法：
四元数到旋转向量（角轴）：
旋转向量到四元数：
问：如何用四元数旋转一个空间点？
设点 p经过一次以 q表示的旋转后，得到了 p’,它们关系如何表示？
答：将的坐标用四元数表示（虚四元数）
旋转之后的关系为：
四元数相比于旋转向量(角轴)、欧拉角的优势：紧凑、无奇异性