视觉slam十四讲 ---CH3 三维刚体运动
三维刚体运动，即三维空间下的刚体的运动。刚体，是指在运动中和受力作用后,形状和大小不变,而且内部各点的相对位置不变的物体。在运动过程中，机器人或者飞机和汽车的形变很小，可以近似看作刚体。三维刚体运动就是研究如何描述和表示一个刚体在三维空间下的位姿变换过程。比如装甲板的位姿描述，云台的位姿描述等。
描述刚体的位姿变换，可以将其运动分解为两个过程，旋转以及平移。旋转调整其姿态，平移调整其位置。
txt
描述姿态的工具有如下几种
旋转矩阵
变换矩阵
欧拉角
四元数1. 坐标系相关
在开始介绍描述姿态的工具之前，我们先明确一下关于坐标系的一些标准
右手笛卡尔坐标系
上图中的右边图片所示即为右手笛卡尔坐标系
可见拇指指向x轴,食指指向y轴，中指指向z轴
接下来对三维物体的位姿描述均在右手系中进行描述。
世界坐标系 相机坐标系
世界坐标选取的是一系列真实世界中的坐标系，其可以是人为规定的坐标系。其三维坐标可以使用\((X_W, Y_W, Z_W)\)来表示(W = World)。
相机坐标系则是以相机焦点为坐标系中心来规定的坐标系。使用\((X_C, Y_C, Z_C)\)来进行表示.(C = Camera)
在RM比赛中，视觉组一项重要的工作就是如何仅依靠摄像头的图片(也就是纯视觉)进行装甲板测距，测量装甲板相对于相机坐标系的坐标。这一步便是装甲板的世界坐标向相机坐标系下的坐标变换。
向量的内积与外积
对于两个向量\(\vec{a} =(a_1,a_2,a_3)\)和\(\vec{b} = (b_1,b_2,b_3)\),我们定义两种运算
-
向量的内积
\[\vec{a}\cdot\vec{b}=a_1\cdot b_1+a_2\cdot b_2+a_3\cdot b_3 \]
可见向量内积的结果是一个数
-
向量的外积
\[\vec{a}\times\vec{b} = \begin{vmatrix} i & j & k \\ a_1 & a_2 & a_3 \\ b_1 & b_2 & b_3 \\ \end{vmatrix} = \begin{bmatrix} a_2b_3 - a_3b_2\\ a_1b_3 - a_3b_1\\ a_1b_2 - a_2b_1\\ \end{bmatrix} \triangleq \verb|a^b|\\ \]
向量的外积结果仍是一个向量，其大小等于\(|a||b|sin\theta\),其方向可以这样确定：右手手掌掌尖朝向a向量，握拳方向由a向量转向b向量，此时拇指的方向即为内积向量的方向。
2. 旋转矩阵与平移向量
明确了以上几个概念，我们就可以开始展开描述三维姿态的第一种方法了，那就是旋转矩阵。
假设当前坐标系下的一组单位正交基底向量\((\vec{e_1},\vec{e_2},\vec{e_3})\)的一个向量可以使用这样的形式来表示\( \begin{bmatrix} \vec{e_1}&\vec{e_2}&\vec{e_3}\\ \end{bmatrix} \begin{bmatrix} a_1\\ a_2\\ a_3\\ \end{bmatrix} \)
其中\((a_1,a_2,a_3)\)即为\(\vec{a}\)在该组基下的坐标
不妨假设有另外一组单位正交基为\({\vec{e_1}',\vec{e_2}',\vec{e_3}'}\),向量a在该组基底的坐标为\((a_1',a_2',a_3')\),则根据向量不变性(在不同坐标系下向量的长度以及与其他向量的夹角不变)可以得出如下等式
\[\begin{bmatrix} \vec{e_1}&\vec{e_2}&\vec{e_3}\\ \end{bmatrix} \begin{bmatrix} a_1\\ a_2\\ a_3\\ \end{bmatrix} = \begin{bmatrix} \vec{e_1}'&\vec{e_2}'&\vec{e_3}'\\ \end{bmatrix} \begin{bmatrix} a_1'\\ a_2'\\ a_3'\\ \end{bmatrix} \]
两边乘以\(\begin{bmatrix} \vec{e_1}&\vec{e_2}&\vec{e_3}\\ \end{bmatrix}\)的逆，得出
\[\begin{bmatrix} a_1\\ a_2\\ a_3\\ \end{bmatrix} = \begin{bmatrix} e_1^Te_1'&e_1^Te_2'&e_1^Te_3'\\ e_2^Te_1'&e_2^Te_2'&e_2^Te_3'\\ e_3^Te_1'&e_3^Te_2'&e_3^Te_3'\\ \end{bmatrix} \begin{bmatrix} a_1'\\ a_2'\\ a_3'\\ \end{bmatrix} \triangleq Ra'\\ \text{即}\\ a = Ra',R = \begin{bmatrix} e_1^Te_1'&e_1^Te_2'&e_1^Te_3'\\ e_2^Te_1'&e_2^Te_2'&e_2^Te_3'\\ e_3^Te_1'&e_3^Te_2'&e_3^Te_3'\\ \end{bmatrix} \]
可以看出，我们可以将a'到空间点a的变换用矩阵R来描述。这里的R我们称之为旋转矩阵，它描述了同一向量在不同坐标系下的坐标变换。
值得注意的是，这里的基底向量以及变换的向量都是抽象意义上的向量，并不依赖于某个坐标系下的坐标，都保持自身的不变性。不过在我看来或者计算来看，可以取一个独立于二者之外的坐标系来进行参考，更加容易理解。
对于旋转矩阵的性质可以有如下说明
\[R\cdot R^T = E \quad\text(即R为正交阵)\\ |R| = 1 \\ \text(综上即R为单位正交阵)\\ \\ 可以将R这种矩阵的集合称作如下描述(还没学过)\\ So(n) = \{R\in R^{n\times n}|RR^T = I, det(R) = 1\}\\ \]
上述描述称为特殊正交群，满足此条件即为旋转矩阵，同理旋转矩阵也都满足此条件。
至此由坐标系1中的坐标变换到坐标系2的这一过程我们就可以使用旋转矩阵来进行描述了。值得注意的是旋转矩阵更为强调的是坐标系之间的变换，变的是坐标系而非向量，通过旋转矩阵可以知道变换之后向量在新坐标系下的坐标与位置。
text
假设坐标系1与坐标系2,向量a在1中的坐标为a1,在2中的坐标为a2| 1 -> 2 | 2 -> 1 |
|---|---|
| \(a_1 = R_{12}a_2\) | \(a_2 = R_{21}a_1\) |
需要注意的一点是，上述式子应该从右向左看，代表从右边的坐标系转换到左边的坐标系。
开始说道，描述坐标系的变换，不仅要进行旋转来调整姿态上的不同，还要进行平移来调整位置上的不同。平移的操作很简单，直接对系1原点进行平移至与系2原点重合即可。我们使用一个向量\(t\)来描述平移的过程，t就被称为平移向量
综上，对于坐标系2到坐标系1的变换，我们可以这样描述完整的变换过程
\[a_1 = R_{12}a_2 + t_{12}\\ 或者\\ \vec{a_1} = R_{12}\vec{a_2} + \vec{t_{12}} \]
3. 变换矩阵
对于多次坐标变换的情况，可以有如下描述
\[a_1 = R_{12}a_2 + t_{12}\\ a_3 = R_{31}a_1 + t_{31}\\ 则有\\ a_3 = R_{31}(R_{12}a_2 + t_{12}) + t_{31} \]
当变换的坐标越多，使用旋转矩阵和平移向量来描述过程就越加繁琐，出现括号套括号的现象。为了表示更加整洁，可以采用以下方式。
\[\begin{bmatrix} a'\\ 1 \end{bmatrix} = \begin{bmatrix} R & t \\ 0^T & 1\\ \end{bmatrix} \begin{bmatrix} a \\ 1 \\ \end{bmatrix}\triangleq T\begin{bmatrix} a\\ 1\\ \end{bmatrix}\\ \text{不难算出上述等式等于下式}\\ \begin{bmatrix} a'\\ 1 \end{bmatrix} = \begin{bmatrix} R & t \\ 0^T & 1\\ \end{bmatrix} \begin{bmatrix} a \\ 1 \\ \end{bmatrix}= \begin{bmatrix} Ra+t\\ 1 \end{bmatrix}\\ T^{-1} = \begin{bmatrix} R^T & -R^Tt \\ 0^T & 1\\ \end{bmatrix} \]
上述描述中我们给向量a增加了一个维度1(注意表示坐标的向量都视为列向量)，将其齐次化，此时的\([a\quad1]^T\)成为齐次坐标，而矩阵\(T = \begin{bmatrix} R & t \\ 0^T & 1\\ \end{bmatrix}\)被成为变换矩阵\(T\)。
变换矩阵可以这样来表示
\[SE(3) = \{ T = \begin{bmatrix} R & t \\ 0^T & 1\\ \end{bmatrix} \in R^{4\times 4} | R \in So(3),t\in R^3 \} \]
上述描述的是一个欧式变换群，满足该描述的矩阵即为变换矩阵
使用这种表示法之后，原先使用旋转矩阵和旋转向量的坐标变换便可以转化为如下表示。
\[\tilde{a} = T_1\tilde{b}\\ \tilde{c} = T_2\tilde{a}\\ \tilde{c} = T_2T_1\tilde{b} \]
其中\(\tilde{a} = [a\quad1]^T\),该种形式的坐标被称为齐次坐标。
由上述计算结果来看，无论是使用旋转矩阵还是变换矩阵，结果都是一样的，只是使用变换矩阵来描述会更加简洁。
值得注意的是，我们将a化为\(\tilde{a}\)的目的是为了使之可以与变换矩阵相乘。变换矩阵实际上是一个4*4的矩阵，a变换之前是3*1,不可乘
4. 旋转向量(轴角)与欧拉角
旋转向量
可以知道旋转矩阵是一个\(3\times3\)的矩阵，有着九个元素，而且必须满足单位正交群的约束。描述一次旋转需要用到9个量，看起来有点多。这里给出另一种描述坐标旋转的方式，叫做旋转向量 或者轴角 或者角轴 。
上图中红色的\(w\)向量即为旋转向量
使用旋转向量来描述坐标系的旋转过程只需要一个描述旋转的向量即可，该向量的方向指向转轴方向，大小或者说系数为转动角度大小。其描述形式如下
\[\vec{w} = \theta\vec{n} \]
其中的\(\theta\)即为转动角度，而\(\vec{n}\)为表征转轴方向的单位向量。
注意这里使用轴角来描述旋转时，指的是绕给定轴一步到位的旋转，而非分解到几个轴的旋转。
不难看出，轴角只需要一个旋转向量就可以描述这次旋转，且\(\vec{w}\in R^3\)。本来需要\(3\times3\)规模旋转矩阵的描述变为了使用一个三维向量来描述，更加简洁。不过旋转矩阵与旋转向量描述的变换终究是一回事，因此两者可以相互转化，对同一种旋转都是等价的。
给出如下转化公式
旋转向量转旋转矩阵
\[\text{罗德里格斯公式}\\ R = cos\theta I + (1-cos\theta)\vec{n}\vec{n}^T + sin\theta\vec{n}\verb|^|\\ \]
旋转矩阵转旋转向量
\[\text{角度}\\ \theta = arcos(\frac{tr(R)-1}{2})\\ \text{轴向量}\\ R\vec{n} = \vec{n}，求解R特征值为1时的特征向量 \]
ps:上面的公式不用太理解
欧拉角
上面的旋转矩阵以及旋转向量的描述虽然都用数学语言描述了三维空间坐标下的旋转，但是对于人类来讲还是太抽象了。但是使用欧拉角可以直观感受到坐标系或者物体的旋转。
欧拉角描述的是物体或者说坐标轴绕着当前的某个定轴旋转某个角度的变换。
在如图坐标中我们将飞机绕不同的轴转动，从而得到不同的欧拉角定义(都是作为垂直轴)
- 绕y轴转动 -- 偏航角(yaw)
- 绕x轴转动 -- 俯仰角(pitch)
- 绕z轴转动 -- 滚转角(roll)
不难体会出对于欧拉角的定义都是很直观的，左右摆动就是偏航，上下摆动就是俯仰，绕轴滚动就是滚转。这里要注意一点，欧拉角的定义和转轴名称并不绑定，即并非绕y轴转动就是yaw角，而是与转动方式相关。比如俯仰的上下变换就是yaw角，而非绕y轴转动就是yaw。
欧拉角性质
- 欧拉角变换必须指定转动角的顺序 ，如yaw-pitch-roll或者roll-pitch-yaw，并且严格按照顺序来进行转动。
- 在使用欧拉角描述姿态变换时，有两种选择，一种是变轴的欧拉角，即每次旋转都会带动其他轴的旋转，下一次旋转会在此基础上接着转。而另一种是定轴的欧拉角，转轴在一开始就已经确定不变，每次旋转都是参考选定转轴旋转。
- 变轴的欧拉角在旋转时会产生万向锁现象，导致丢失一个旋转维度。
万向锁
如图所示，万向锁是动轴的欧拉角会出现的一种现象。当三组角的中间角(这里假设为图中的y轴即yaw角，假设顺序为x_pitch - y_yaw - z_roll)转动为\(90^o\)时，会导致第三轴与第一轴重合，导致绕z轴旋转等同与绕最初的x轴旋转了。本来应该描述三个方向的旋转变成了两个方向。丢失了一个方向的信息。万向锁的出现是因为动轴的欧拉角的动轴以及欧拉角旋转的顺序性共同导致的。
这里强调一点，欧拉角的顺序性。如以pyr为顺序，(10,20,30)为pyr的转动角度，如果我们将(10,20,30)变为(12,20,30)时，发生的过程并非为在当前变换后的坐标系下的x轴转动,而是依据顺序性，重新从未旋转的状态开始进行pyr的旋转。因为当第一次旋转角度改变后，第二次旋转的轴也会有所不同。
为了编程方便，一般使用定轴的欧拉角。可由平移向量计算出
或者由旋转向量计算得出(上网搜一搜，我懒得贴出来了)
5. 四元数
我们知道在二维平面上，可以使用复数来实现旋转如\(\vec{n} * e^{\pi i}\)角度相加，相当于向量n转动了180度。那么，对于三维空间，是否有类似于复数的体系来通过运算描述三维空间中的转动呢？
答案是肯定的，那就是四元数。
对于一个标准的四元数，我们这样定义
\[\vec{q} = q_0 +q_1\vec{i}+q_2\vec{j}+q_3\vec{k} = [s\quad\vec{v}] \]
其中s为实部，v为虚部。
关于i，j，k有这样的定义运算
\[i^2 = j^2 = k^2 = ijk = -1\\ ij = k\quad ji = -k\\ jk = i\quad kj = -i\\ ki = j\quad ik = -j\\ \]
四元数运算不满足交换率
四元数的一些基本运算请见四元数运算。
四元数与轴角的转化
轴角转化为四元数
\[q = [cos\frac{\theta}{2}\quad \vec{n_x}sin\frac{\theta}{2}\quad \vec{n_y}\frac{\theta}{2}\quad \vec{n_z}\frac{\theta}{2}] \]
四元数转角轴
\[\theta = 2arccosq_0\\ [n_x,n_y,n_z]^T = \frac{[q_1,q_2,q_3]^T}{sin\frac{\theta}{2}} \]
使用四元数来描述旋转
给出点p'(x,y,z),变换到另一坐标下的点p
将p'转化成四元数的四维形式，使之可以运算
\[p = (x,y,z) -> p = [0\quad x\quad y\quad z]^T = [0\quad v]^T \]
接着给出变换需要的四元数q，从p'到p的运算如下
\[p = qpq^{-1} \]
6.Eigen库简介
Eigen库是c++的一个功能强大，性能优越的开源线性代数计算库，简单好用。大多数复杂的计算在Eigen中都有相应的接口，可以大大简化我们写代码的方式。Eigen是一个纯头文件实现的模板库，因此在使用时不需要额外链接动态或者静态库。
其所有接口封装在命名空间Eigen中，最主要的就是一个Matrix模板类，Eigen中几乎所有的类都是Matrix的typedef或者宏,给出Matrix类的构造函数
c++
Matrix(T* data_, size_t rows_, size_t cols_, size_t stride_ = 0)
/*data 元素的类型
* rows 行
* cols 列
* 初始化矩阵类对象的语法与STL中的大多数类相似，我们一般只需要指定如上的几个参数
*/
Matrix<double.3,3> matrix3d //初始化一个3*3矩阵，元素类型为double
Matrix<int,3,3> matrix3i //初始化一个3*3矩阵，元素类型为int下面给出几个常用写法
矩阵读入数据
c++
matrix3d << 1,2,3,
4,5,6,
7,8,9;
//Eigen重载了<<运算符以及逗号运算符，使之用于矩阵的读入。注意这里的读入不是覆盖而是更像push_back,因此只有第一次读入时可以这样写，否则会报错。想更改元素直接访问更改即可。
Matrix<double, 3, 3> matrix = Matrix<double, 3, 3>::Zero();//初始化一个元素全为0的矩阵。矩阵输出
c++
cout << matrix3d;
//Eigen重载了<<运算符，使之可以直接作用于矩阵类输出整个矩阵
cout << matrix3d(1,2);
//Eigen重载小括号运算符，用于访问矩阵中的各个元素。起始位依旧为0，中括号运算符不可访问Matrix类特殊类
c++
Vector3d = Matrix<double.3,1> //三列一行列向量
Matrix3d = Matrix<double,3,3> //3*3矩阵动态矩阵
c++
Matrix<double, Dynamic, Dynamic> matrix;
matrix.resize(3, 3);
matrix << 1, 2, 3,
4, 5, 6,
7, 8, 9;
cout << matrix;
//动态矩阵需要把行列参数都改为Eigen的内置参数Dynamic。这样的矩阵就是一个可变矩阵。每次改变需要使用resize接口来指定矩阵大小，我称之为半可变矩阵QAQ。注意只有可变矩阵可以使用resize，正常矩阵使用resize会runtime error。建议使用静态矩阵，动态矩阵因为维护动态开销会降低性能。矩阵乘法
c++
Matrix<int, 2, 2> matrix2i;
matrix2i << 2, 2,
2, 2;
Matrix<int, 1, 2> vector2i;
vector2i << 1, 2;
auto res = vector2i * matrix2i;
cout << res << '\n';
//Eigen重载了*运算符，使之可以直接用于矩阵乘法。注意乘法时前列等于后行以及元素对应，否则会运行时错误。描述了
\[[1\quad2]\begin{bmatrix} 2&2\\ 2&2\\ \end{bmatrix} = [6\quad6] \]
一些接口
c++
matrix_33 = Matrix3d::Random(); // 随机数矩阵
cout << "random matrix: \n" << matrix_33 << endl;
cout << "transpose: \n" << matrix_33.transpose() << endl; // 转置
cout << "sum: " << matrix_33.sum() << endl; // 各元素和
cout << "trace: " << matrix_33.trace() << endl; // 迹
cout << "times 10: \n" << 10 * matrix_33 << endl; // 数乘
cout << "inverse: \n" << matrix_33.inverse() << endl; // 逆
cout << "det: " << matrix_33.determinant() << endl; // 行列式坐标变换相关
c++
//1. 旋转矩阵
Matrix3d rotation_matrix = Matrix3d::Identity();//直接用3d当作旋转矩阵，赋值为一个单位阵I
//2.旋转向量(轴角/角轴)
AngleAxisd rotation_vector(M_PI / 4, Vector3d(0, 0, 1)); // 沿 Z 轴旋转 45 度
//3.轴角到旋转矩阵
rotation_matrix = rotation_vector.matrix();
//or
rotation_matrix = rotation_vector.toRotationMatrix();使用旋转矩阵或旋转向量变换坐标
c++
Vector3d v(1, 0, 0);
Vector3d v_rotated = rotation_vector * v;
cout << "(1,0,0) after rotation (by angle axis) = " << v_rotated.transpose() << endl;
// 或者用旋转矩阵
v_rotated = rotation_matrix * v;
cout << "(1,0,0) after rotation (by matrix) = " << v_rotated.transpose() << endl;上述过程描述了在基底为(0.707,0,0),(0,0.707,0),(0,0,1)下坐标为(1,0,0)的向量转化为基底为(1,0,0),(0,1,0),(0,0,1)下坐标为(0.707,0.707,0)的向量。当然也可以换一种视角，那就是坐标(1,0,0)向量旋转45度到坐标(0.707,0.707,0)的向量。
旋转矩阵转欧拉角
c++
Vector3d euler_angles = rotation_matrix.eulerAngles(0, 1, 2);
// 0 -> 绕x轴转动的角度
// 1 -> 绕y轴转动的角度
// 2 -> 绕z轴转动的角度变换矩阵
c++
Isometry3d T = Isometry3d::Identity(); // 虽然称为3d，实质上是4＊4的矩阵
T.rotate(rotation_vector/rotation_matrix); // 按照rotation_vector进行旋转
T.pretranslate(Vector3d(1, 3, 4));
//变换矩阵使用Isometry3d实现变换矩阵，其rotate接口来更新旋转矩阵(也可接受旋转向量参数)，pretranslate接口更新平移向量。使用变换矩阵进行坐标系变换
c++
Vector3d v_transformed = T * v; // 相当于R*v+t
//虽然v是一个三维坐标并非齐次坐标，但是Eigen重载的乘法运算符已经帮我们做了，因此直接三维坐标乘变换矩阵即可四元数类
c++
Quaterniond q = Quaterniond(rotation_vector);
//接受旋转矩阵或者旋转向量来作为初始化
v_rotated = q * v;
//重载了四元数与三维向量的*号运算符，使得我们不用将v变为四元数同时使用v' = qvq^-1的公式进行运算了。
q.coeffs(); //返回q的系数矩阵。注意Eigen中四元数的排列顺序为[x,y,z,w],w为实部，xyz对应ijk
//奇怪的是四元数类的初始化顺序为[w,x,y,z],别搞混了。
Quaternion(const Scalar& w, const Scalar& x, const Scalar& y, const Scalar& z) : m_coeffs(x, y, z, w){};
//这是其中一个接受wxyz的构造函数，可以看出输入顺序与存储顺序是不一样的。Chapter 3 三维刚体运动，完结撒花！！！！！