视觉SLAM十四讲 第3讲 三维空间刚体运动（相关知识点汇总）

本文不以书上的知识点流程进行记录，而是将初学SLAM一些相关却又比较陌生的知识点、名词进行总结，然后再记录第3讲的相关知识。

1. 刚体

刚体rigid body，指在运动或受力的过程中，形状和大小不变，各个质点的相对位置不变
​

2. 欧氏空间（euclidean space）

欧几里得空间就是对现实空间的规则抽象和推广（从n<=3推广到有限n维空间）
​

简单来说所有满足从2维、3维推出的内积、距离、角的定义的空间都叫做欧氏空间。

2.1 欧氏距离：

$$d(x,y)=\sqrt{(x_1-y_1{)}^2+(x_2-y_2{)}^2+\cdots +(x_n-y_n{)}^2}$$
​

2.2 欧氏变换：

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

非欧几里得空间：二维扁平智能生物
https://www.zhihu.com/question/27903807
​

3. 笛卡尔坐标系

又称直角坐标系、正交坐标系。笛卡尔坐标系的研究结合了代数和欧几里得几何，对于后来的解析几何、微积分和地图学的发展，具有关键性的意义。

4. 透视空间

透视从理论上直观的解释了物体在平面（2维）上呈现三维空间的基本原理和规律。方法有单点透视、空气透视和散点透视。更多的是绘画上的概念。
​

可以直接把透视空间理解为3维空间往某个方向、平面，拍扁成2维平面所形成的图像。
​

5. 齐次坐标系

5.1 问题：两条平行线可以相交于一点？

在欧式空间中，同一平面的两条平行线不能相交，但是透视空间中可以，比如火车轨道随着我们的实现越来越窄，最后两条平行线在无穷远处交于一点。
​

这个点的坐标是消失点（无穷，无穷），这在欧式空间中没有意义。
​

简而言之，齐次坐标系，就是用N+1维向量（矩阵）来表示N维向量（矩阵），而转化回笛卡尔坐标系就是用前N-1个元素除以第N个元素即可。齐次坐标系就是为了解决，平行线在透视空间相交且相交点在欧式空间没有意义的问题。
​

笛卡尔坐标系(a, b) -> 齐次坐标系(x, y, w)
其中a = x / w, b = y / w
​

5.2 为什么叫齐次？

(4, 6, 2)和(2, 3, 1)对应的是同一个欧氏点，这些点是齐次的，齐次坐标有规模不变性。
​

求平行线相交方程组：
https://blog.csdn.net/janestar/article/details/44244849
如果是普通坐标系，则方程无解，转化为齐次坐标系则有解，解在无穷远处
​

6. 关于Eigen

• 在c++程序中，我们可以把一个float数据和double数据相加、相乘，编译器会自动把数据类型转换为最合适的那种。而在eigen中，处于对性能的考虑，必须显式得对矩阵类型进行转换，不然会出错。
• 如果发现eigen出错，你可以直接找大写的部分，来看看出了什么问题。
• eigen几乎所有数据都当做矩阵来处理

7. 基础知识

7.1 点和向量

点是空间中的基本元素，没有长度，没有体积。把两个点连接起来，就构成了向量。
向量可以看成从某点指向另一个点的一个箭头。切记，不要把向量与它的坐标这两个概念啊混淆。向量就是空间中的一个东西，不需要与任何实数相关联，只有确定了空间中的某个坐标系时，才可以谈论该向量在此坐标系下的坐标，也就是找到若干个实数对应这个向量。
​

坐标的具体取值，一是和向量本身有关，二是和坐标系（基）的选取有关。基就是张成这个空间的一组线性无关的向量，有些书里也叫基底。基向量的长度为1。
​

7.2 点乘和叉乘

dot内积（点乘）：
① a在b上的投影的模乘以b的模
② 描述向量之间的夹角
$$a\cdot b=a^{T}b=|a||b|cos<a,b>$$
矩阵没有点乘一说
​

cross product外积（叉乘）：
① aOb平面的法向量
② ab组成的平行四边形的面积和法向量长度$$|a||b|sin<a,b>$$
法向量的方向成右手法则，所以叉乘的顺序是会有影响的。
​

反对成矩阵（skew-symmetric matrix），如a = [a1, a2, a3]
$$a\hat{}=\left[\begin{array}{ccc}0& -a_3& a_2\\ a_3& 0& -a_1\\ -a_2& a_1& 0\end{array}\right]$$
​

在机器人学中，你会给每个连杆和关节定义它们的坐标系。如果考虑运动的机器人，那么常见的做法是设定一个惯性坐标系（或者世界坐标系），可以认为它是固定不动的。
​

8. 旋转矩阵相乘的含义

$a_1=R_{12}{a}_2+t_{12}$
旋转矩阵$R_{12}$代表的意思是“把坐标系2的向量变换到坐标系1”。由于向量乘在了这个矩阵的右边，它的下表是从右读到左的。关于平移$t_{12}$，它实际对应的是坐标系1原点指向坐标系2原点的向量，在坐标系1下取得坐标。但是反过来的$t_{21}$不等于$-t_{12}$，t指平移，运算时为向量，但不能默许相反数的规则，这回导致后续的数学运算出错。
​

9. 变换矩阵Transform Matrix

SO(n)是特殊正交群（special orthogonal group），集合的定义如下：
S O ( N ) = { R ∈ R n × n ∣ R R T = I , d e t ( R ) = 1 } SO(N) = \{R \in \mathbb{R}^{n\times n} | RR^T = I, det(R) = 1\} SO(N)={R∈Rn×n∣RRT=I,det(R)=1}
特殊正交群要求正交，且行列式为1.
​

我们在一个三维向量的末尾添加1，将其变成了四位向量，称为其次坐标。对于这个四位向量，我们可以把旋转和平移卸载一个矩阵里，使得整个关系变成线性关系。
$$T=\left[\begin{array}{cc}R& t\\ 0& 1\end{array}\right]$$

SE(3)是特殊欧氏群（special euclidean group），定义如下：
S E ( 3 ) = { T = [ R t 0 T 1 ] ∈ R 4 × 4 ∣ R ∈ S O ( 3 ) , t ∈ R 3 } SE(3) = \{ T = \begin{bmatrix} R & t \\ 0^T & 1 \end{bmatrix} \in \mathbb{R}^{4 \times 4} | R \in SO(3), t \in R^3\} SE(3)={T=[R0T​t1​]∈R4×4∣R∈SO(3),t∈R3}
反向的变换.inverse()为：
$$T^{-1}=\left[\begin{array}{cc}{R}^T& -R^{T}t\\ 0^T& 1\end{array}\right]$$

10. 旋转向量、欧拉角、四元数

10.1 旋转向量和旋转矩阵

任意旋转都可以用一个旋转轴和一个旋转角来刻画。于是，我们可以使用一个向量，其方向与旋转轴一致，而长度等于旋转角。这种向量称为旋转向量（或轴角/角轴，axis-angle），只需要一个三维向量即可描述旋转。
​

如果用一个旋转矩阵R来表示旋转，可以从旋转向量转化过来，假设旋转向量的单位向量n，以及角度（长度）Θ，则可以通过罗德里格斯公式（Rodrigues’s Formula）来表明：
$$R=cos\theta I+(1-cos\theta )n{n}^T+sin\theta \hat{n}$$
如果从旋转矩阵转回旋转向量，则可以两边取迹，方向向量则是R特征值为1的特征向量。
罗德里格斯公式的详细推导过程我自己总结了一份，写的会比其他博客详细一些。

10.1.1 旋转向量和旋转矩阵Eigen

# 向量和旋转向量是不一样的,向量的base是Eigen::Matrix，而旋转向量是Eigen::AngleAxis
# 初始化旋转向量
Eigen::AngleAxisd rotation_vector(M_PI / 2, Eigen::Vector3d(1,2,3).normalized());
# 注意normalized()返回向量，norm返回L2范数，normalize()是归一化处理，不返回值

# 旋转向量转旋转矩阵
Eigen::Matrix3d rotation_matrix = rotation_vector.matrix();

# 旋转矩阵转旋转向量
Eigen::Vector3d rotate;
rotate.fromRotationMatrix(rotation_matrix);

10.2 欧拉角

无论是旋转矩阵还是旋转向量，虽然能描述旋转，但是对人类来说很不直观。
​

欧拉角提供了一种非常直观的方式来描述旋转——使用3个分离的转角，把一个旋转分解成3次不同的旋转。假设一个刚体的前方为x轴，右侧为Y轴，上方为Z轴。通常使用ypr（yaw-pitch-row）旋转顺序。
​

10.3 万向锁

欧拉角的一个重大缺点就是会碰到著名的万向锁问题（gimbal lock）：当第二个旋转角为±90°时，第一次旋转和第三次旋转将会使用同一个轴，使得系统丢失一个自由度（由三次旋转变为2次旋转）。这被称为奇异性问题。理论上可以证明，只要想用三个实数来表达三维旋转，都会不可避免的遇到奇异性问题。

10.3.1 如何理解万向锁？

首先，万向锁（gimbal lock），是使用动态欧拉角表示旋转会出现的问题。
​

首先搞清楚一个概念，欧拉角有两种旋转：

• 静态：世界坐标系（惯性坐标系），绕着世界坐标系三个轴旋转，因为物体旋转过程坐标轴保持静止，所以称为静态。
• 动态：绕着物体坐标系旋转，因为旋转过程中坐标轴随着物体做相同的运动，所以称为动态。

使用动态欧拉角会出现万向锁现象，静态欧拉角不会出现万向锁现象。
​

当使用动态欧拉角旋转时，无论3个轴的旋转顺序是什么，只要第二个旋转轴的旋转角度为90度，那么第三个旋转轴（物体坐标系）的旋转效果和第一次旋转轴（世界坐标系）的旋转效果是一样的。这样，旋转系统丢失了一个自由度，原本需要3次旋转表示的旋转变成了2次。这被称为奇异性问题。实际上，只要想用三个实数来表达三维旋转，都会不可避免的遇到奇异性问题，这点有点类似于经纬度的表示，当维度为±90°时，经度没有意义。
​

口头描述（面试回答）：
万向锁，英文名gimbal clock，是使用动态欧拉角表示旋转时会出现的问题。具体是什么问题呢，当使用动态欧拉角表示旋转的时候，不管旋转顺序如何，当第二个旋转轴旋转90°时，第一次旋转和第三次旋转的效果是一样的，也就是说旋转系统丢失了一个自由度（一个由3次旋转才能表示的旋转变成2次就是表示），这就是万向锁问题。也被称为奇异性问题。

10.4 四元数

定义：四元数是简单的超复数。超复数的意思是一个实数加三个复数。
意义：用四元数可以用来表示旋转，相对于欧拉角这种有奇异性的三维向量描述方式，四元数可以避免万向锁问题。
​

意义：① 增量旋转
② 避免万向锁问题
③ 给定方位的表达方式有两种，互为负（欧拉角有无数种表达方式）
​

10.4.1 加法

各项加

10.4.2 乘法

乘法是把qa的每一项与qb的每一项相乘，最后相加。

10.4.3 模长

$||q_a||=\sqrt{s_a^2+x_a^2+y_a^2+z_a^2}$
$||q_a{q}_b||=||q_a||||q_b||$

10.4.4 共轭

虚部取反

10.4.5 逆

$q^{-1}=\frac{q^{\star }}{||q|{|}^2}$