视觉SLAM十四讲学习笔记（三）三维空间中的刚体运动

一、点与坐标系

内积

外积

二、旋转矩阵

三维旋转矩阵R为行列式为1的正交矩阵，定义为特殊正交群

三维变换矩阵T定义为特殊欧式群

逆矩阵为一个反向的变换

三、旋转的表示

1、旋转向量（轴/角）

只有三个自由度，旋转轴为n，角度为，则对应的旋转向量为。

罗德里格斯公式：

旋转矩阵转轴角

     

2、欧拉角

将旋转分为三个方向上的运动，根据绕固定轴还是绕旋转之后的轴，绕轴的先后次序不同，有多种表达方式，常见的是“偏航-俯仰-滚转（yaw-pitch-roll）”三个角度来描述一个旋转。等价于

1、绕物体的Z轴旋转，得到偏航角yaw

2、绕旋转之后的Y轴旋转，得到俯仰角pitch

3、绕旋转之后的X轴旋转，得到滚转角roll

但当pitch为90°时，旋转自由度减1，存在万向锁问题。

可以证明：仅用三个实数表达旋转时，不可避免地存在奇异性问题。

3、四元数

由三个实部和一个虚部组成。其中为四元数的三个虚部，满足

​​​​​​​

单位四元数可表示一个旋转

四元数之间可有以下多种运算

用四元数q来旋转点p

1、将三维空间点用虚四元数来描述 p = [0,x,y,z] = [0,v]

2、旋转之后的点

四元数与轴角之间的变换

   

 

四元数与旋转矩阵之间的变换

实际编程中，当接近0时，其余三个分量会非常大，导致解的不稳定性，此时可以再考虑使用其他方式进行转换。

四、实践

本章为Eigen实现，Eigen为矩阵库，用法类似Matlab，比较简单，可见书本。