视觉SLAM③：三维空间的刚体运动

目录

3.0 本章目标

3.1 点与坐标系

3.1.1 点、向量和坐标系

3.1.2 坐标系间的欧氏变换

3.1.3 变换矩阵与齐次坐标

3.2 实践：Align

3.3 旋转向量和欧拉角

3.3.1 旋转向量

3.3.2 欧拉角

3.4 四元数 

3.4.1 四元数的定义

3.4.2 四元数的运算

3.4.3 用四元数表示旋转

3.4.4 四元数到其他旋转表示的转换

---

3.0 本章目标

主要目标
1．理解三维空间的刚体运动描述方式:旋转矩阵、变换矩阵、四元数和欧拉角。

2．掌握Eigen库的矩阵、几何模块的使用方法。

3.1 点与坐标系

3.1.1 点、向量和坐标系

1.引言

        我们日常生活的空间是三维的，因此我们生来就习惯于三维空间的运动。

        三维空间由三个轴组成，所以一个空间点的位置可以由三个坐标指定。

        刚体，它不光有位置(x,y,z)，还有自身的姿态(朝向)。

        相机也可以看成三维空间的刚体，于是位置是指相机在空间中的哪个地方,而姿态则是指相机的朝向。结合起来,我们可以说,“相机正处于空间(0,0,0)点处,朝向正前方”。

        在2D的情况下：描述运动用两个坐标(x,y)加旋转角表达()（我在哪？我朝向哪？）如图3-1所示。

图3-1 二维平面下的位姿表示方法(x,y,θ)

        在三维的情况下呢???如果不好想象的话，先来了解一些基础概念吧！

2.基本概念

Ⅰ.参考系（坐标系）：一般随着关心的物体而改变，比如一个相机，相机在世界当中运动。这时候我们可以建立两个坐标系，世界坐标系和相机坐标系。

图3-2 坐标系的左手系和右手系

Ⅱ.点：点就是空间中的基本元素,没有长度,没有体积。

Ⅲ.向量：两个点连接起来，就构成了向量，向量可以看成从某点指向另一点的一个箭头

用线性代数的知识来说，三维空间中的某个点的坐标也可以用来描述。假设在这个线性空间内，我们找到了该空间的一组基，那么,任意向量α在这组基下就有一个坐标:

式 3-1   任意向量α在基下都可表示

        

        这里称为在此基下的坐标。坐标的具体取值，一是和向量本身有关，二是
和坐标系（基）的选取有关。

        坐标系通常由3个正交的坐标轴组成（尽管也可以有非正交的）。例如，当给定x和y轴时，z轴就可以通过右手（或左手）法则由定义出来。

        根据定义方式的不同，坐标系又分为左手系和右手系。左手系的第三个轴与右手系方向相反。

Ⅳ.向量的坐标：上文已提到

3.向量的运算 

        这里        

Ⅰ.向量的内积：

式3-2   向量的内积计算

其中指向量a与b的夹角，内积也可以描述向量间的投影关系。

Ⅱ.向量的外积：

式3-3   向量的外积计算

        外积的结果是一个向量，它的方向垂直于这两个向量，大小为|a||b|sin(a,b)，是两个向量张成的四边形的有向面积。对于外积运算,我们引入^符号,把a写成一个矩阵。事实上是一个反对称矩阵（Skew-symmetric Matrix)，你可以将^记成一个反对称符号。这样就把外积axb写成了矩阵与向量的乘法 a^b，把它变成了线性运算，并且此符号是一个一一映射，意味着任意向量都对应着唯一的一个反对称矩阵，反之亦然。

式3-4   a^表达式

3.1.2 坐标系间的欧氏变换

1.需求

        我们经常在实际场景中定义各种各样的坐标系。在机器人学中，你会给每一个连杆和关节定义它们的坐标系；在3D作图时，我们也会定义每一个长方体、圆柱体的坐标系。
        如果考虑运动的机器人，那么常见的做法是设定一个惯性坐标系（或者叫世界坐标系），可以认为它是固定不动的。同时，相机或机器人是一个移动坐标系。那么：相机视野中某个向量p，它在相机坐标系下的坐标为p1，而从世界坐标系下看，它的坐标为p2，那么，这两个坐标之间是如何转换的呢?。这时，就需要先得到该点针对机器人坐标系的坐标值，再根据机器人位姿变换到世界坐标系中。（顺序别记错）

图3-3 如何获得Laser相对于世界坐标系坐标

        那么可以归结为下列两个问题：

①如何描述坐标系与坐标系之间的变化？

②如何计算同一个向量在不同坐标系里的坐标？

SLAM中：

        固定的世界坐标系和移动的机器人坐标系

        不同的传感器坐标系

2.刚体运动与欧式变换

Ⅰ.刚体运动定义：两个坐标系之间的运动由一个旋转加上一个平移组成，这种运动称为刚体运动。

Ⅱ.刚体运动举例：摔手机运动就是一个刚体运动。刚体运动过程中，同一个向量在各个坐标系下的长度和夹角都不会发生变化。想象你把手机抛到空中，在它落地摔碎之前，只可能有空间位置和姿态的不同。而它自己的长度、各个面的角度等性质不会有任何变化。手机并不会像橡皮那样一会儿被挤扁、一会儿被拉长。
Ⅲ.坐标变换：此时，我们说手机坐标系到世界坐标之间，相差了一个欧氏变换。

图3-4 坐标变换 对于同一个向量p，它在世界坐标系下的坐标pw和在相机坐标系下的坐标pc是不同的。这个变换关系由变换矩阵T来描述。

        欧氏变换由旋转和平移组成。我们首先考虑旋转。设某个单位正交基经过一次旋转变成了。

        那么，对于同一个向量(该向量并没有随着坐标系的旋转而发生运动)，它在两个坐标系下的坐标为和 。因为向量本身没变，所以根据坐标的定义，有

式3-5   同一向量在不同基底下的坐标

        为了描述两个坐标之间的关系，我们对上述等式的左右两边同时左乘，左面变成了E，所以：

式3-6   坐标变换

 

Ⅳ.旋转矩阵：我们把中间的矩阵拿出来，定义成一个矩阵R。这个矩阵由两组基之间的内积组成(R = a基的转置与a'基的矩阵乘法)，刻画了旋转前后同一个向量的坐标变换关系。只要旋转是一样的，这个矩阵就是一样的。可以说，矩阵R描述了旋转本身。因此，称为旋转矩阵(Rotation Matrix)。同时，该矩阵各分量是两个坐标系基的内积，由于基向量的长度为1，所以实际上是各基向量夹角的余弦值。所以这个矩阵也叫方向余弦矩阵(Direction Cosine Matrix)。我们后文统一称它为旋转矩阵。

        个人理解：设为世界坐标系，为机器人坐标系。上文提到，要获得物体相对于基坐标系的坐标，要用物体相对于机器人坐标的坐标再进行坐标变换得到。这个变化矩阵就是基坐标系基的转置的与机器人坐标系的基的乘法。记作R 则，a = Ra'。

图3-5 具体实例解释坐标变换

Ⅴ.旋转矩阵的性质：它是一个行列式为1的正交矩阵；反之，行列式为1的正交矩阵也是一个旋转矩阵。所以，可以将n维旋转矩阵的集合定义如下：
 

式3-7   特殊正交群

 SO(n)是特殊正交群(Special Orthogonal Group）的意思。

        由于旋转矩阵为正交矩阵,它的逆(即转置）描述了一个相反的旋转。

        于是，1到2的旋转可定义为，2到1的旋转可定义为。

矩阵关系

3.加上平移

        在欧氏变换中，除了旋转还有平移。考虑世界坐标系中的向量，经过一次旋转（用R描述）和一次平移后，得到了，那么把旋转和平移合到一起,有

式3-8   欧式变换

        其中，称为平移向量。相比于旋转，平移部分只需把平移向量加到旋转之后的坐标上。通过上式,我们用一个旋转矩阵R和一个平移向量t完整地描述了一个欧氏空间的坐标变换关系。实际当中，我们会定义坐标系1、坐标系2，那么向量a在两个坐标系下的坐标为，它们之间的关系应该是:

图3-6 不同坐标系下的平移关系

式3-9  两向量的变化关系

        这里的是指“把坐标系2的向量变换到坐标系1”中。由于向量乘在这个矩阵的右边，它的下标是从右读到左的。这也是本书的习惯写法。坐标变换很容易搞混，特别是存在多个坐标系的情况下。同理，如果我们要表达“从1到2的旋转矩阵”时，就写成。

        关于平移，它实际对应的是坐标系1原点指向坐标系2原点的向量，是在坐标系1下取的坐标，所以把它记作“从1到2的向量”。但是反过来的，即从2指向1的向量在坐标系2下的坐标，却并不等于，而是和两个系的旋转还有关系。所以，当初学者问“我的坐标在哪里”这样的问题时，我们需要清楚地说明这句话的含义。这里“我的坐标”实际上是指从世界坐标系指向自己坐标系原点的向量，是在世界坐标系下取到的坐标。

        笔者理解：这时可以讨论要获得物体相对于基坐标系的坐标，要用物体相对于机器人坐标的坐标再进行坐标变换得到的过程了:

        世界坐标系的基底是，机器人坐标系的基底是，机器人观察到物体的坐标是，在世界的坐标系下指向机器人的向量为

        则世界坐标系下物体的坐标

图3-7 解释t12为什么不等于-t21

3.1.3 变换矩阵与齐次坐标

1.变换矩阵描述刚体运动的缺陷

        式3-9描述了欧氏空间的旋转与平移，不过还存在一个小问题：

        假设我们进行了两次变换

式3-10   两次欧式变换

         这样的形式在变换多次之后会显得很啰嗦。因此，我们引入齐次坐标和变换矩阵重写式子3-10。

式3-11  齐次坐标和变换矩阵

2.定义：变换矩阵

        这是一个数学技巧:我们在一个三维向量的末尾添加1，将其变成了四维向量，称为齐次坐标。对于这个四维向量，我们可以把旋转和平移写在一个矩阵里，使得整个关系变成线性关系。该式中，矩阵T称为变换矩阵（Transform Matrix )

        我们暂时用表示的齐次坐标。那么依靠齐次坐标和变换矩阵，两次变换的叠加就可以有很好的形式:

式3-12  用变换矩阵表示两次变换叠加

3.定义：特殊欧式群

        关于变换矩阵T，它具有比较特别的结构:左上角为旋转矩阵右侧为平移向量，左下角为向量，右下角为1。这种矩阵又称为特殊欧氏群。

式 3-13  特殊欧式群

        与SO(3)一样，求解该矩阵的逆表示一个反向的变换

式 3-14  特殊欧式群的逆

        同样，我们用这样的写法来表示从2到1的变换。并且，为了保持符号的简洁，在不引起歧义的情况下，以后不刻意区别齐次坐标与普通坐标的符号，默认使用的是符合运算法则的一种。

3.2 实践：Align

1.获取Align头文件的位置

终端输入：获取elign库的位置

sudo updatedb
locate eigen3

liuhongwei@liuhongwei-virtual-machine:~$ sudo updatedb
[sudo] liuhongwei 的密码： 
liuhongwei@liuhongwei-virtual-machine:~$ locate eigen3
/usr/include/eigen3

如果没有，安装：

sudo apt-get install libeigen3-dev

2.具体实现使用

#include 

using namespace std;

#include 
// Eigen 核心部分
#include 
// 稠密矩阵的代数运算（逆，特征值等）
#include 

using namespace Eigen;

#define MATRIX_SIZE 50

/****************************
* 本程序演示了 Eigen 基本类型的使用
****************************/

int main(int argc, char **argv) {
  // Eigen 中所有向量和矩阵都是Eigen::Matrix，它是一个模板类。它的前三个参数为：数据类型，行，列
  // 声明一个2*3的float矩阵
  Matrix matrix_23;

  // 同时，Eigen 通过 typedef 提供了许多内置类型，不过底层仍是Eigen::Matrix
  // 例如 Vector3d 实质上是 Eigen::Matrix，即三维向量
  Vector3d v_3d;
  // 这是一样的
  Matrix vd_3d;

  // Matrix3d 实质上是 Eigen::Matrix
  Matrix3d matrix_33 = Matrix3d::Zero(); //初始化为零
  // 如果不确定矩阵大小，可以使用动态大小的矩阵
  Matrix matrix_dynamic;
  // 更简单的
  MatrixXd matrix_x;
  // 这种类型还有很多，我们不一一列举

  // 下面是对Eigen阵的操作
  // 输入数据（初始化）
  matrix_23 << 1, 2, 3, 4, 5, 6;
  // 输出
  cout << "matrix 2x3 from 1 to 6: \n" << matrix_23 << endl;

  // 用()访问矩阵中的元素
  cout << "print matrix 2x3: " << endl;
  for (int i = 0; i < 2; i++) {
    for (int j = 0; j < 3; j++) cout << matrix_23(i, j) << "\t";
    cout << endl;
  }

  // 矩阵和向量相乘（实际上仍是矩阵和矩阵）
  v_3d << 3, 2, 1;
  vd_3d << 4, 5, 6;

  // 但是在Eigen里你不能混合两种不同类型的矩阵，像这样是错的
  // Matrix result_wrong_type = matrix_23 * v_3d;
  // 应该显式转换
  Matrix result = matrix_23.cast() * v_3d;
  cout << "[1,2,3;4,5,6]*[3,2,1]=" << result.transpose() << endl;

  Matrix result2 = matrix_23 * vd_3d;
  cout << "[1,2,3;4,5,6]*[4,5,6]: " << result2.transpose() << endl;

  // 同样你不能搞错矩阵的维度
  // 试着取消下面的注释，看看Eigen会报什么错
  // Eigen::Matrix result_wrong_dimension = matrix_23.cast() * v_3d;

  // 一些矩阵运算
  // 四则运算就不演示了，直接用+-*/即可。
  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;    // 行列式

  // 特征值
  // 实对称矩阵可以保证对角化成功
  SelfAdjointEigenSolver eigen_solver(matrix_33.transpose() * matrix_33);
  cout << "Eigen values = \n" << eigen_solver.eigenvalues() << endl;
  cout << "Eigen vectors = \n" << eigen_solver.eigenvectors() << endl;

  // 解方程
  // 我们求解 matrix_NN * x = v_Nd 这个方程
  // N的大小在前边的宏里定义，它由随机数生成
  // 直接求逆自然是最直接的，但是求逆运算量大

  Matrix matrix_NN
      = MatrixXd::Random(MATRIX_SIZE, MATRIX_SIZE);
  matrix_NN = matrix_NN * matrix_NN.transpose();  // 保证半正定
  Matrix v_Nd = MatrixXd::Random(MATRIX_SIZE, 1);

  clock_t time_stt = clock(); // 计时
  // 直接求逆
  Matrix x = matrix_NN.inverse() * v_Nd;
  cout << "time of normal inverse is "
       << 1000 * (clock() - time_stt) / (double) CLOCKS_PER_SEC << "ms" << endl;
  cout << "x = " << x.transpose() << endl;

  // 通常用矩阵分解来求，例如QR分解，速度会快很多
  time_stt = clock();
  x = matrix_NN.colPivHouseholderQr().solve(v_Nd);
  cout << "time of Qr decomposition is "
       << 1000 * (clock() - time_stt) / (double) CLOCKS_PER_SEC << "ms" << endl;
  cout << "x = " << x.transpose() << endl;

  // 对于正定矩阵，还可以用cholesky分解来解方程
  time_stt = clock();
  x = matrix_NN.ldlt().solve(v_Nd);
  cout << "time of ldlt decomposition is "
       << 1000 * (clock() - time_stt) / (double) CLOCKS_PER_SEC << "ms" << endl;
  cout << "x = " << x.transpose() << endl;

  return 0;
}

 执行具体结果：

matrix 2x3 from 1 to 6: 
1 2 3
4 5 6
print matrix 2x3: 
1	2	3	
4	5	6	
[1,2,3;4,5,6]*[3,2,1]=10 28
[1,2,3;4,5,6]*[4,5,6]: 32 77
random matrix: 
 0.680375   0.59688 -0.329554
-0.211234  0.823295  0.536459
 0.566198 -0.604897 -0.444451
transpose: 
 0.680375 -0.211234  0.566198
  0.59688  0.823295 -0.604897
-0.329554  0.536459 -0.444451
sum: 1.61307
trace: 1.05922
times 10: 
 6.80375   5.9688 -3.29554
-2.11234  8.23295  5.36459
 5.66198 -6.04897 -4.44451
inverse: 
-0.198521   2.22739    2.8357
  1.00605 -0.555135  -1.41603
 -1.62213   3.59308   3.28973
det: 0.208598
Eigen values = 
0.0242899
 0.992154
  1.80558
Eigen vectors = 
-0.549013 -0.735943  0.396198
 0.253452 -0.598296 -0.760134
-0.796459  0.316906 -0.514998
time of normal inverse is 1.894ms
x = -55.7896 -298.793  130.113 -388.455 -159.312  160.654 -40.0416 -193.561  155.844  181.144  185.125 -62.7786  19.8333 -30.8772 -200.746  55.8385 -206.604  26.3559 -14.6789  122.719 -221.449   26.233  -318.95 -78.6931  50.1446  87.1986 -194.922  132.319  -171.78 -4.19736   11.876 -171.779  48.3047  84.1812 -104.958 -47.2103 -57.4502 -48.9477 -19.4237  28.9419  111.421  92.1237 -288.248 -23.3478  -275.22 -292.062  -92.698  5.96847 -93.6244  109.734
time of Qr decomposition is 1.832ms
x = -55.7896 -298.793  130.113 -388.455 -159.312  160.654 -40.0416 -193.561  155.844  181.144  185.125 -62.7786  19.8333 -30.8772 -200.746  55.8385 -206.604  26.3559 -14.6789  122.719 -221.449   26.233  -318.95 -78.6931  50.1446  87.1986 -194.922  132.319  -171.78 -4.19736   11.876 -171.779  48.3047  84.1812 -104.958 -47.2103 -57.4502 -48.9477 -19.4237  28.9419  111.421  92.1237 -288.248 -23.3478  -275.22 -292.062  -92.698  5.96847 -93.6244  109.734
time of ldlt decomposition is 0.64ms
x = -55.7896 -298.793  130.113 -388.455 -159.312  160.654 -40.0416 -193.561  155.844  181.144  185.125 -62.7786  19.8333 -30.8772 -200.746  55.8385 -206.604  26.3559 -14.6789  122.719 -221.449   26.233  -318.95 -78.6931  50.1446  87.1986 -194.922  132.319  -171.78 -4.19736   11.876 -171.779  48.3047  84.1812 -104.958 -47.2103 -57.4502 -48.9477 -19.4237  28.9419  111.421  92.1237 -288.248 -23.3478  -275.22 -292.062  -92.698  5.96847 -93.6244  109.734

[Done] exited with code=0 in 9.467 seconds

3.问题：

        博主遇到了一点问题：IDE提示

[Running] cd "/home/liuhongwei/桌面/slam_3/src/eigen_01/src/" && g++ eigen.cpp -o eigen && "/home/liuhongwei/桌面/slam_3/src/eigen_01/src/"eigen
eigen.cpp:7:10: fatal error: Eigen/Core: 没有那个文件或目录

        解决办法：

问题解决办法https://blog.csdn.net/CH_monsy/article/details/115461544?spm=1001.2101.3001.6650.1&utm_medium=distribute.pc_relevant.none-task-blog-2~default~CTRLIST~default-1.pc_relevant_aa&depth_1-utm_source=distribute.pc_relevant.none-task-blog-2~default~CTRLIST~default-1.pc_relevant_aa&utm_relevant_index=2

3.3 旋转向量和欧拉角

3.3.1 旋转向量

1.矩阵表示方式至少有以下两个缺点

Ⅰ.SO(3)的旋转矩阵有9个量，但一次旋转只有3个自由度。因此这种表达方式是冗余的。
同理，变换矩阵用16个量表达了6自由度的变换。那么，是否有更紧凑的表示呢?
Ⅱ.旋转矩阵自身带有约束：它必须是个正交矩阵，且行列式为1。变换矩阵也是如此。当想
估计或优化一个旋转矩阵或变换矩阵时，这些约束会使得求解变得更困难。

2.优化方法

        因此，我们希望有一种方式能够紧凑地描述旋转和平移。
        例如，用一个三维向量表达旋转,用一个六维向量表达变换，可行吗?

        事实上,任意旋转都可以用一个旋转轴和一个旋转角来刻画。于是，我们可以使用一个向量，其方向与旋转轴一致，而长度等于旋转角。这种向量称为旋转向量(或轴角/角轴，Axis-Angle )，只需一个三维向量即可描述旋转。

        同样,对于变换矩阵，我们使用一个旋转向量和一个平移向量即可表达一次变换。这时的变量维数正好是六维。

图3-8 旋转表示

3.罗德里格斯公式

        考虑某个用R表示的旋转。如果用旋转向量来描述，假设旋转轴为一个单位长度的向量n,角度为θ，那么向量θn也可以描述这个旋转。

        于是，我们要问，两种表达方式之间有什么联系吗?从旋转向量到旋转矩阵的转换过程由罗德里格斯公式( Rodrigues's Formula）表明。

式3-15  罗德里格斯公式

式3-16  罗德里格斯公式与旋转矩阵的联系

         关于转轴，旋转轴上的向量在旋转后不发生改变：

        因此，转轴是矩阵R特征值1对应的特征向量。求解此方程，再单位化，就得到了旋转轴。

3.3.2 欧拉角

1.浅介欧拉角

        无论是旋转矩阵还是旋转向量，它们虽然能描述旋转，但对人类来说是非常不直观的。当我们看到一个旋转矩阵或旋转向量时，很难想象出这个旋转究竟是什么样的。当它们变换时，我们也不知道物体是在向哪个方向转动。而欧拉角则提供了一种非常直观的方式来描述旋转——它使用了3个分离的转角，把一个旋转分解成3次绕不同轴的旋转。而人类很容易理解绕单个轴旋转的过程。

        但是，由于分解方式有许多种,所以欧拉角也存在着众多不同的、易于混淆的定义方法例如，先绕X轴，再绕Y轴，最后绕Z轴旋转，就得到了一个XYZ轴的旋转。同理，可以定义ZYZ、ZYX等旋转方式。如果讨论得更细一些,则还需要区分每次是绕固定轴旋转的，还是绕旋转之后的轴旋转的,这也会给出不一样的定义方式。

2.SLAM中欧拉角的定义

        上述定义方式上的不确定性带来了很多实际当中的困难,所幸在特定领域内，欧拉角通常有统一的定义方式。你或许在航空、航模中听说过“俯仰角”“偏航角”这些词。欧拉角当中比较常用的一种，便是用“偏航-俯仰-滚转”( yaw-pitch-roll ) 3个角度来描述一个旋转。它等价于ZYX轴的旋转,因此就以ZYX为例。

        假设一个刚体的前方朝向我们的方向)为X轴，右侧为Y轴，上方为Z轴，如图3-2所示。那么，ZYX转角相当于把任意旋转分解成以下3个轴上的转角:
1．绕物体的Z轴旋转,得到偏航角yaw。

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

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

图3-9 偏航-俯仰-滚转

        此时，可以使用[r, p, y]这样一个三维的向量描述任意旋转。这个向量十分直观，我们可以从这个向量想象出旋转的过程。其他的欧拉角也是通过这种方式，把旋转分解到3个轴上，得到一个三维的向量，只不过选用的轴及顺序不一样。这里介绍的rpy角是比较常用的一种，只有很少的欧拉角种类会有rpy这样脍炙人口的名字。不同的欧拉角是按照旋转轴的顺序来称呼的。例如，rpy角的旋转顺序是ZYX。同样，也有XYZ、ZYZ这样的欧拉角——但是它们就没有专门的名字了。值得一提的是，大部分领域在使用欧拉角时都有各自的坐标方向和顺序上的习惯，不一定和我们这里说的相同。

3.万向锁问题

        欧拉角的一个重大缺点是会碰到著名的万向锁问题(Gimbal Lock)在俯仰角为±90°时,第一次旋转与第三次旋转将使用同一个轴，使得系统丢失了一个自由度（由3次旋转变成了2次旋转)。这被称为奇异性问题，在其他形式的欧拉角中也同样存在。理论上可以证明，只要想用3个实数来表达三维旋转，都会不可避免地碰到奇异性问题。
        由于这种原理,欧拉角不适用于插值和迭代,往往只用于人机交互中。我们也很少在SLAM程序中直接使用欧拉角表达姿态，同样不会在滤波或优化中使用欧拉角表达旋转（因为它具有奇异性)。不过，若你想验证自己的算法是否有错，转换成欧拉角能够帮你快速分辨结果是否正确。
        在某些主体主要为2D运动的场合(例如扫地机、自动驾驶车辆)，我们也可以把旋转分解成三个欧拉角，然后把其中一个（例如偏航角）拿出来作为定位信息输出。
 

图3-10  欧拉角的旋转示意图为pitch = 90°时，第三次旋转与第一次滚转角相同，使得系统丢失了一个自由度。

3.4 四元数 

3.4.1 四元数的定义

1.为什么需要四元数

        旋转矩阵用9个量描述3自由度的旋转，具有冗余性;

        欧拉角和旋转向量是紧凑的，但具有奇异性。

        事实上，我们找不到不带奇异性的三维向量描述方式。这有点类似于用两个坐标表示地球表面（如经度和纬度)，将必定存在奇异性(纬度为±90°时经度无意义)。

        有关复数和坐标变换的知识：请温习高中数学的知识

高中数学有关复数与坐标变换的知识https://www.bilibili.com/video/BV1kE411S7Mg?spm_id_from=333.337.search-card.all.click

        回忆以前学习过的复数。我们用复数集表示复平面上的向量，而复数的乘法则表示复平面上的旋转:例如，乘上复数i相当于逆时针把一个复向量旋转 90°。类似地，在表达三维空间旋转时，也有一种类似于复数的代数:四元数（Quaternion )。四元数是Hamilton找到的一种扩展的复数。

        它既是紧凑的，也没有奇异性。如果说缺点，四元数不够直观，其运算稍复杂些。

2.用四元数表示旋转

Ⅰ.把四元数与复数类比可以帮助你更快地理解四元数。例如,当我们想要将复平面的向量旋转
θ角时，可以给这个复向量乘以。这是极坐标表示的复数，它也可以写成普通的形式，只要使用欧拉公式即可:

式3-17  欧拉公式

 Ⅱ.2D的情况下，可以用单位复数表达旋转

式3-18 二维情况下的旋转

 Ⅲ.3D情况下，四元数可作为复数的扩充。一个四元数q拥有一个实部和三个虚部。

式3-19  四元数的表示

 其中,i,j, k为四元数的三个虚部。这三个虚部满足以下关系式:

式3-20 四元数运算法则

如果把i, j, k看成三个坐标轴,那么它们与自己的乘法和复数一样，相互之间的乘法和外积一样。有时，人们也用一个标量和一个向量来表达四元数:

式3-21  用一个标量和一个向量来表达四元数

        这里，s称为四元数的实部，而v称为它的虚部。如果一个四元数的虚部为0，则称为实四元数;反之，若它的实部为0，则称为虚四元数。
        可以用单位四元数表示三维空间中任意一个旋转，不过这种表达方式和复数有着微妙的不同。在复数中，乘以i意味着旋转90°。这是否意味着四元数中，乘i就是绕i轴旋转90°？那么，ij=k是否意味着，先绕i轴转90°，再绕j轴转 90°，就等于绕k轴转 90°?

        可以找一部手机比划一下——然后你会发现情况并不是这样的。正确的情形应该是，乘以i对应着旋转180°,这样才能保证ij=k的性质。而i^2= -1，意味着绕i轴旋转360°后得到一个相反的东西。这个东西要旋转两周才会和它原先的样子相等。

3.4.2 四元数的运算

1.四元数的运算

        现有两个四元数,，它们的向量表示为,，或者原始四元数表示为

式3-22 原始四元数

 Ⅰ.加减法

式3-23  四元数的加减法

Ⅱ.乘法

乘法是把qa的每一项与qb的每项相乘,最后相加,虚部要按照式3-16进行

式3-24  四元数的乘法

 虽然稍为复杂，但形式上是整齐有序的。如果写成向量形式并利用内外积运算，该表达会更加简洁:

式3-25  四元数的乘法

Ⅲ.模长

式3-26  四元数的模长

 可以验证，两个四元数乘积的模即模的乘积。这使得单位四元数相乘后仍是单位四元数

Ⅳ.共轭

四元数的共轭是把虚部取成相反数
 

式3-27  四元数的共轭

 四元数共辄与其本身相乘，会得到一个实四元数，其实部为模长的平方

式3-28 四元数的共轭与其本身相乘

Ⅴ.逆

一个四元数的逆为

式3-29  四元数的逆

 按此定义，四元数和自己的逆的乘积为实四元数1

式3-30 四元数和自己的逆的乘积为实四元数1

 如果q为单位四元数,其逆和共轭就是同一个量。同时,乘积的逆具有和矩阵相似的性质:

式3-31 四元数乘积的逆

Ⅵ.数乘

和向量相似，四元数可以与数相乘
 

式3-32 四元数乘积的数乘

3.4.3 用四元数表示旋转

        我们可以用四元数表达对一个点的旋转。假设有一个空间三维点，以从一个由单位四元数指定的旋转。三维点经过旋转之后变成。

        如果使用矩阵描述，那么有。

        而如果用四元数描述旋转，它们的关系又如何表达呢?
        首先，把三维空间点用一个虚四元数来描述:

式3-33   三维空间点用一个虚四元数来描述

         相当于把四元数的3个虚部与空间中的3个轴相对应。那么，旋转后的点可表示为这样的乘积：

式3-34    三维空间点用一个虚四元数来描述

         这里的乘法均为四元数乘法，结果也是四元数。最后把的虚部取出，即得旋转之后点的坐标。

        并且,可以验证，计算结果的实部为0，故为纯虚四元数。

3.4.4 四元数到其他旋转表示的转换

         任意单位四元数描述了一个旋转，该旋转也可用旋转矩阵或旋转向量描述。现在来考察四元数与旋转向量、旋转矩阵之间的转换关系。

        在此之前，我们要说，四元数乘法也可以写成一种矩阵的乘法。设，那么，定义如下的符号⊕和+

式3-35 将四元数映射成为一个4×4的矩阵

 经推导：

式3-36 四元数的性质

考虑使用四元数对空间点进行旋转的问题，根据前面的说法，有：
                                              

Ⅰ.四元数到旋转矩阵的变换关系

式3-37  四元数到旋转矩阵的变换关系

 Ⅱ.四元数到旋转向量的变换关系：为四元数

式 (3-27) 四元数到旋转向量的变换关系

3.6 实践：Elign几何模块

3.6.1 Eigen 几何模块的数据演示

#include 
#include 

using namespace std;

#include 
#include 

using namespace Eigen;

// 本程序演示了 Eigen 几何模块的使用方法

int main(int argc, char **argv) {

  // Eigen/Geometry 模块提供了各种旋转和平移的表示
  // 3D 旋转矩阵直接使用 Matrix3d 或 Matrix3f
  Matrix3d rotation_matrix = Matrix3d::Identity();
  // 旋转向量使用 AngleAxis, 它底层不直接是Matrix，但运算可以当作矩阵（因为重载了运算符）
  AngleAxisd rotation_vector(M_PI / 4, Vector3d(0, 0, 1));     //沿 Z 轴旋转 45 度
  cout.precision(3);
  cout << "rotation matrix =\n" << rotation_vector.matrix() << endl;   //用matrix()转换成矩阵
  // 也可以直接赋值
  rotation_matrix = rotation_vector.toRotationMatrix();
  // 用 AngleAxis 可以进行坐标变换
  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;

  // 欧拉角: 可以将旋转矩阵直接转换成欧拉角
  Vector3d euler_angles = rotation_matrix.eulerAngles(2, 1, 0); // ZYX顺序，即yaw-pitch-roll顺序
  cout << "yaw pitch roll = " << euler_angles.transpose() << endl;

  // 欧氏变换矩阵使用 Eigen::Isometry
  Isometry3d T = Isometry3d::Identity();                // 虽然称为3d，实质上是4＊4的矩阵
  T.rotate(rotation_vector);                                     // 按照rotation_vector进行旋转
  T.pretranslate(Vector3d(1, 3, 4));                     // 把平移向量设成(1,3,4)
  cout << "Transform matrix = \n" << T.matrix() << endl;

  // 用变换矩阵进行坐标变换
  Vector3d v_transformed = T * v;                              // 相当于R*v+t
  cout << "v tranformed = " << v_transformed.transpose() << endl;

  // 对于仿射和射影变换，使用 Eigen::Affine3d 和 Eigen::Projective3d 即可，略

  // 四元数
  // 可以直接把AngleAxis赋值给四元数，反之亦然
  Quaterniond q = Quaterniond(rotation_vector);
  cout << "quaternion from rotation vector = " << q.coeffs().transpose()
       << endl;   // 请注意coeffs的顺序是(x,y,z,w),w为实部，前三者为虚部
  // 也可以把旋转矩阵赋给它
  q = Quaterniond(rotation_matrix);
  cout << "quaternion from rotation matrix = " << q.coeffs().transpose() << endl;
  // 使用四元数旋转一个向量，使用重载的乘法即可
  v_rotated = q * v; // 注意数学上是qvq^{-1}
  cout << "(1,0,0) after rotation = " << v_rotated.transpose() << endl;
  // 用常规向量乘法表示，则应该如下计算
  cout << "should be equal to " << (q * Quaterniond(0, 1, 0, 0) * q.inverse()).coeffs().transpose() << endl;

  return 0;
}

 执行结果：

rotation matrix =
 0.707 -0.707      0
 0.707  0.707      0
     0      0      1
(1,0,0) after rotation (by angle axis) = 0.707 0.707     0
(1,0,0) after rotation (by matrix) = 0.707 0.707     0
yaw pitch roll = 0.785    -0     0
Transform matrix = 
 0.707 -0.707      0      1
 0.707  0.707      0      3
     0      0      1      4
     0      0      0      1
v tranformed = 1.71 3.71    4
quaternion from rotation vector =     0     0 0.383 0.924
quaternion from rotation matrix =     0     0 0.383 0.924
(1,0,0) after rotation = 0.707 0.707     0
should be equal to 0.707 0.707     0     0

[Done] exited with code=0 in 4.926 seconds

总结：

        Eigen中对各种形式的表达方式总结如下。请注意每种类型都有单精度和双精度两种数据类型，而且和之前一样，不能由编译器自动转换。下面以双精度为例，你可以把最后的d改成f,即得到单精度的数据结构。
·旋转矩阵(3×3): Eigen:Matrix3d。

·旋转向量(3×1): Eigen::AngleAxisd。

·欧拉角（3×1):Eigen::Vector3d

·四元数(4×1):Eigen::Quaterniond。
·欧氏变换矩阵（4×4):Eigen::Isometry3d

·仿射变换（4×4):Eigen:Affine3d
·射影变换（4×4):Eigen::Projective3d。
        参考代码中对应的CMakeLists即可编译此程序。在这个程序中，演示了如何使用Eigen 中的旋转矩阵、旋转向量、欧拉角和四元数。我们用这几种旋转方式旋转一个向量v ,发现结果是一样的。同时,也演示了如何在程序中转换这几种表达方式。
        程序代码通常和数学表示有一些细微的差别。例如，通过运算符重载，四元数和三维向量可以直接计算乘法，但在数学上则需要先把向量转成虚四元数，再利用四元数乘法进行计算，同样的情况也适用于变换矩阵乘三维向量的情况。总体而言，程序中的用法会比数学公式更灵活。

3.6.2 实际的坐标变换例子

        下面我们举一个小例子来演示坐标变换。
例子：设有小萝卜一号和小萝卜二号位于世界坐标系中。记世界坐标系为，小萝卜们的坐标系为和。

        小萝卜一号的位姿为，。

        小萝卜二号的位姿为，。

        这里的和表达的是，也就是世界坐标系到相机坐标系的变换关系。

        现在，小萝卜一号看到某个点在自身的坐标系下坐标为，求该向量在小萝卜二号坐标系下的坐标。

code:

#include 
#include 
#include 
#include 
#include 

using namespace std;
using namespace Eigen;

int main(int argc, char** argv) {
  Quaterniond q1(0.35, 0.2, 0.3, 0.1), q2(-0.5, 0.4, -0.1, 0.2);
  q1.normalize();
  q2.normalize();
  Vector3d t1(0.3, 0.1, 0.1), t2(-0.1, 0.5, 0.3);
  Vector3d p1(0.5, 0, 0.2);

  Isometry3d T1w(q1), T2w(q2);
  T1w.pretranslate(t1);
  T2w.pretranslate(t2);

  Vector3d p2 = T2w * T1w.inverse() * p1;
  cout << endl << p2.transpose() << endl;
  return 0;
}

结果：

[Running] cd "/home/liuhongwei/桌面/slambook2/ch3/examples/" && g++ coordinateTransform.cpp -o coordinateTransform && "/home/liuhongwei/桌面/slambook2/ch3/examples/"coordinateTransform

-0.0309731    0.73499   0.296108

[Done] exited with code=0 in 2.443 seconds

计算过程只需计算：

即可。注意四元数使用之前需要归一化。

3.7可视化演示

3.7.1显示运动轨迹

        首先，假设我们通过某种方式记录了一个机器人的运动轨迹，现在想把它画到一个窗口中，每一行用下面的格式存储：

                                ​​​​​​​        ​​​​​​​        

        其中，time指该位姿的记录时间，为平移，为旋转四元数，均是以世界坐标系到机器人坐标系记录。

        下面我们从文件中读取这些轨迹，并显示到一个窗口中。

        原则上，如果只是谈论“机器人的位姿”，那么你可以使用或者，事实上它们也只差一个逆而已，这意味着知道其中一个就可以很轻松地得到另一个。如果你想要存储机器人的轨迹,那么可以存储所有时刻的、或者，这并没有太大的差别。

         在画轨迹的时候，我们可以把“轨迹”画成一系列点组成的序列，这和我们想象中的“轨迹”比较相似。严格说来，这其实是机器人（相机）坐标系的原点在世界坐标系中的坐标。考虑机器人坐标系的原点O，此时的​​​​​​​就是这个原点在世界坐标系下的坐标：

                                                      

        这正是 的平移部分。因此，可以从中直接看到相机在何处，这也是我们说更为直观的原因。因此,在可视化程序里，轨迹文件存储了而不是。

        code：

#include 
#include 
#include 

// 本例演示了如何画出一个预先存储的轨迹

using namespace std;
using namespace Eigen;

// path to trajectory file
string trajectory_file = "./examples/trajectory.txt";

void DrawTrajectory(vector>);

int main(int argc, char **argv) {

  vector> poses;
  ifstream fin(trajectory_file);
  if (!fin) {
    cout << "cannot find trajectory file at " << trajectory_file << endl;
    return 1;
  }

  while (!fin.eof()) {
    double time, tx, ty, tz, qx, qy, qz, qw;
    fin >> time >> tx >> ty >> tz >> qx >> qy >> qz >> qw;
    Isometry3d Twr(Quaterniond(qw, qx, qy, qz));
    Twr.pretranslate(Vector3d(tx, ty, tz));
    poses.push_back(Twr);
  }
  cout << "read total " << poses.size() << " pose entries" << endl;

  // draw trajectory in pangolin
  DrawTrajectory(poses);
  return 0;
}

/*******************************************************************************************/
void DrawTrajectory(vector> poses) {
  // create pangolin window and plot the trajectory
  pangolin::CreateWindowAndBind("Trajectory Viewer", 1024, 768);
  glEnable(GL_DEPTH_TEST);
  glEnable(GL_BLEND);
  glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);

  pangolin::OpenGlRenderState s_cam(
    pangolin::ProjectionMatrix(1024, 768, 500, 500, 512, 389, 0.1, 1000),
    pangolin::ModelViewLookAt(0, -0.1, -1.8, 0, 0, 0, 0.0, -1.0, 0.0)
  );

  pangolin::View &d_cam = pangolin::CreateDisplay()
    .SetBounds(0.0, 1.0, 0.0, 1.0, -1024.0f / 768.0f)
    .SetHandler(new pangolin::Handler3D(s_cam));

  while (pangolin::ShouldQuit() == false) {
    glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
    d_cam.Activate(s_cam);
    glClearColor(1.0f, 1.0f, 1.0f, 1.0f);
    glLineWidth(2);
    for (size_t i = 0; i < poses.size(); i++) {
      // 画每个位姿的三个坐标轴
      Vector3d Ow = poses[i].translation();
      Vector3d Xw = poses[i] * (0.1 * Vector3d(1, 0, 0));
      Vector3d Yw = poses[i] * (0.1 * Vector3d(0, 1, 0));
      Vector3d Zw = poses[i] * (0.1 * Vector3d(0, 0, 1));
      glBegin(GL_LINES);
      glColor3f(1.0, 0.0, 0.0);
      glVertex3d(Ow[0], Ow[1], Ow[2]);
      glVertex3d(Xw[0], Xw[1], Xw[2]);
      glColor3f(0.0, 1.0, 0.0);
      glVertex3d(Ow[0], Ow[1], Ow[2]);
      glVertex3d(Yw[0], Yw[1], Yw[2]);
      glColor3f(0.0, 0.0, 1.0);
      glVertex3d(Ow[0], Ow[1], Ow[2]);
      glVertex3d(Zw[0], Zw[1], Zw[2]);
      glEnd();
    }
    // 画出连线
    for (size_t i = 0; i < poses.size(); i++) {
      glColor3f(0.0, 0.0, 0.0);
      glBegin(GL_LINES);
      auto p1 = poses[i], p2 = poses[i + 1];
      glVertex3d(p1.translation()[0], p1.translation()[1], p1.translation()[2]);
      glVertex3d(p2.translation()[0], p2.translation()[1], p2.translation()[2]);
      glEnd();
    }
    pangolin::FinishFrame();
    usleep(5000);   // sleep 5 ms
  }
}

图3-11 位姿可视化结果