视觉slam十四讲ch3三维空间刚体运动（包含实践部分）

目录

1.旋转矩阵

1.1点、向量和坐标系

1.2坐标系间的欧氏变换

1.2.1旋转矩阵

1.2.2特殊正交群SO（n）：

1.2.2平移向量

1.2.2表达方式与读法

1.3变换矩阵与齐次坐标

1.3.1特殊欧氏群SE（3）

2.实践：Eigen

3.旋转向量和欧拉角

3.1旋转向量

3.1.1罗德里格斯公式：

3.2欧拉角

 3.2.1引出欧拉角：

3.2.2slam中欧拉角叫法：

这里介绍的rpy角是比较常用的一种，只有很少的欧拉角种类会有rpy这样脍炙人口的名字。不同的欧拉角是按照旋转轴的顺序来称呼的。例如，rpy角的旋转顺序是ZYX。同样，也有XYZ、ZYZ这样的欧拉角——但是它们就没有专门的名字了。值得一提的是，大部分领域在使用欧拉角时都有各自的坐标方向和顺序上的习惯，不一定和我们这里说的相同。3.2.3欧拉角的问题：

4.四元数

4.1四元数的定义：

4.1.1为什么使用四元数：

4.1.2四元数的定义：

4.2四元数的运算：

4.2.1加法和减法：

4.2.2乘法：

4.2.3模长

4.2.4共轭

4.2.5逆

4.2.6数乘

4.3用四元数表示旋转

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

5.实践：Eigen几何模块

5.1几何模块的数据演示

5.2实际的坐标变换例子

6.可视化演示

6.1显示运动轨迹

6.2显示相机的位姿

---

1.旋转矩阵

1.1点、向量和坐标系

三维空间由三个轴组成,所以一个空间点的位置可以由三个坐标指定。不过,我们现在要考虑刚体,它
不光有位置,还有自身的姿态。相机可以看成三维空间的刚体,位置是指相机在空间中的哪个地方,姿态则是指相机的朝向。结合起来,可以说,“相机正处于空间(0, 0, 0) 点处,朝向正前方” 这样的话。但是这种自然语言很繁琐,我们更喜欢用数学语言来描述它。

点的几何意义很容易理解。向量是什么呢?它是线性空间中的一个元素,可以把它想象成从原点指向某处的一个箭头。需要提醒读者的是,请不要把向量与它的坐标两个概念混淆。但先确定一个坐标系,也就是一个线性空间的基 (e 1 , e 2 , e 3 ),那就可以谈论向量 a 在这组基下的坐标了：

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

向量的内积:

内积可以描述向量间的投影关系。

向量的外积:

外积的方向垂直于这两个向量,大小为 |a| |b| sin 〈a, b〉,是两个向量张成的四边形的有向面积。对于外积,我们引入了符号,把 a 写成一个矩阵。事实上是一个反对称矩阵。这样就把外积 a × b,写成了矩阵与向量的乘法 a ∧ b,把它变成了线性运算。这个符号将在后文经常用到,请记住它。外积只对三维向量存在定义,我们还能用外积表示向量的旋转。

1.2坐标系间的欧氏变换

设定一个惯性坐标系或者叫世界坐标系，认为它是固定不动的。同时,相机或机器人则是一个移动坐标系。我们会问:相机视野中某个向量 p,它的坐标为 p c ,而从世界坐标系下看,它的坐标 p w 。这两个坐标之间是如何转换的呢?这时,就需要先得到该点针对机器人坐标系坐标值,再根据机器人位姿转换到世界坐标系中,这个转换关系由一个矩阵 T 来描述,

相机运动是一个刚体运动,它保证了同一个向量在各个坐标系下的长度和夹角都不会发生变化。这种变换称为欧氏变换。

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

1.2.1旋转矩阵

我们首先考虑旋转。设某个单位正交基(e1,e2,e3)经过一次旋转变成了(e′1,e′2,e′3)。那么，对于同一个向量a(该向量并没有随着坐标系的旋转而发生运动)，它在两个坐标系下的坐标为(a1,a2,a3)T和 (a′1,a′2,a′3)T。因为向量本身没变，所以根据坐标的定义，有：

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

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

1.2.2特殊正交群SO（n）：

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

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

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

1.2.2平移向量

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

其中，t称为平移向量。通过上式,我们用一个旋转矩阵R和一个平移向量t完整地描述了一个欧氏空间的坐标变换关系。

1.2.2表达方式与读法

实际当中，我们会定义坐标系1、坐标系2，那么向量a在两个坐标系下的坐标为a1,a2，它们之间的关系应该是:

通用表达式

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

关于平移t12，它实际对应的是坐标系1原点指向坐标系2原点的向量，是在坐标系1下取的坐标，所以把它记作“从1到2的向量”。但是反过来的t21，即从2指向1的向量在坐标系2下的坐标，却并不等于−t12，而是和两个系的旋转还有关系。

所以，当初学者问“我的坐标在哪里”这样的问题时，我们需要清楚地说明这句话的含义。这里“我的坐标”实际上是指从世界坐标系指向自己坐标系原点的向量，是在世界坐标系下取到的坐标。

1.3变换矩阵与齐次坐标

 前式描述了欧氏空间的旋转与平移，不过还存在一个小问题：假设我们进行了两次变换：

那么，从a到c的变换为：

这样在多次变换后会很繁琐，我们引入齐次坐标和变换矩阵，重写通用表达式：

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

多次变换的形式直接写为b=Ta。默认其中进行了齐次变换。

1.3.1特殊欧氏群SE（3）

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

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

2.实践：Eigen

Eigen是一个c++开源线性代数库，提供了有关矩阵的线性代数运算，还包括解方程等功能。

先安装Eigen库：

sudo apt-get install libeigen3-dev

通过此命令找到Eigen库位置：

sudo updatedb
locate eigen3

Eigen是一个纯用头文件搭起来的库，使用时只需引入头文件，不用链接库文件

创建一个文本并写以下代码：

#include 
using namespace std;

#include 
#include 
#include 
using namespace Eigen;

#define MATRIX_SIZE 50

int main(int argc, char **argv){
        Matrixmatrix_23;
        Vector3d v_3d;
        Matrixvd_3d;
        Matrix3d matrix_33 = Matrix3d::Zero();
        Matrix matrix_dynamic;
        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;
        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;

        srand(time(0));
        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 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;
        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;

        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;
}

（其中记得设随机数种子，否则随机数矩阵与书上的例子会一模一样）

CMakeLists.txt内容为：

# 声明要求的 cmake 最低版本
cmake_minimum_required(VERSION 3.0.2)
 
# 声明一个 cmake 工程
project(eigenMatrix)
 
# 添加一个可执行程序
# 语法：add_executable( 程序名 源代码文件 ）
add_executable(eigenMatrix eigenMatrix.cpp)

#添加头文件
include_directories("/usr/include/eigen3")

然后跟上一讲一样在build文件夹里编译并运行：

3.旋转向量和欧拉角

3.1旋转向量

用矩阵表示刚体运动有至少两个缺点：

1.旋转矩阵9个量表达3自由度的旋转，变换矩阵16个量表达6自由度的变换，繁琐

2.旋转矩阵自身带有约束，必须是正交阵且行列式为一。变换矩阵也带有约束，使求解变困难。

事实上，任意旋转都可以用一个旋转轴和一个旋转角来刻画。于是，我们可以使用一个向量，其方向与旋转轴一致，而长度等于旋转角。这种向量称为旋转向量(或轴角/角轴，Axis-Angle )，只需一个三维向量即可描述旋转。同样,对于变换矩阵，我们使用一个旋转向量和一个平移向量即可表达一次变换。这时的变量维数正好是六维。

3.1.1罗德里格斯公式：

考虑某个用R表示的旋转。如果用旋转向量来描述，假设旋转轴为一个单位长度的向量n,角度为θ，那么向量θn也可以描述这个旋转。两种表达方式的变换可以用罗德里格斯公式表明：

反之：

3.2欧拉角

 3.2.1引出欧拉角：

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

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

3.2.2slam中欧拉角叫法：

所幸在特定领域内，欧拉角通常有统一的定义方式。欧拉角当中比较常用的一种，便是用“偏航-俯仰-滚转”( yaw-pitch-roll ) 3个角度来描述一个旋转。它等价于ZYX轴的旋转,因此就以ZYX为例。

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

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

3．绕旋转之后的X轴旋转，得到滚转角roll。
 此时，可以使用[r, p, y]这样一个三维的向量描述任意旋转。这个向量十分直观，我们可以从这个向量想象出旋转的过程。

这里介绍的rpy角是比较常用的一种，只有很少的欧拉角种类会有rpy这样脍炙人口的名字。不同的欧拉角是按照旋转轴的顺序来称呼的。例如，rpy角的旋转顺序是ZYX。同样，也有XYZ、ZYZ这样的欧拉角——但是它们就没有专门的名字了。值得一提的是，大部分领域在使用欧拉角时都有各自的坐标方向和顺序上的习惯，不一定和我们这里说的相同。
3.2.3欧拉角的问题：

 欧拉角的一个重大缺点是会碰到著名的万向锁问题(Gimbal Lock)在俯仰角为±90°时,第一次旋转与第三次旋转将使用同一个轴，使得系统丢失了一个自由度（由3次旋转变成了2次旋转)。这被称为奇异性问题，在其他形式的欧拉角中也同样存在。理论上可以证明，只要想用3个实数来表达三维旋转，都会不可避免地碰到奇异性问题。
        由于这种原理,欧拉角不适用于插值和迭代,往往只用于人机交互中。我们也很少在SLAM程序中直接使用欧拉角表达姿态，同样不会在滤波或优化中使用欧拉角表达旋转（因为它具有奇异性)。

4.四元数

4.1四元数的定义：

4.1.1为什么使用四元数：

旋转矩阵用9个量描述3自由度的旋转，具有冗余性;欧拉角和旋转向量是紧凑的，但具有奇异性。事实上，我们找不到不带奇异性的三维向量描述方式。这有点类似于用两个坐标表示地球表面（如经度和纬度)，将必定存在奇异性(纬度为±90°时经度无意义)。

回忆以前学习过的复数。我们用复数集表示复平面上的向量，而复数的乘法则表示复平面上的旋转:例如，乘上复数i相当于逆时针把一个复向量旋转 90°。类似地，在表达三维空间旋转时，也有一种类似于复数的代数:四元数（Quaternion )。四元数是Hamilton找到的一种扩展的复数。 它既是紧凑的，也没有奇异性。如果说缺点，四元数不够直观，其运算稍复杂些。

4.1.2四元数的定义：

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

这正是一个单位长度的复数。所以，二维情况下，旋转可以由单位复数来描述。三维情况下，三维旋转由单位四元数来描述

一个四元数q拥有一个实部和三个虚部。

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

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

这里，s称为四元数的实部，而v称为它的虚部。如果一个四元数的虚部为0，则称为实四元数;反之，若它的实部为0，则称为虚四元数。

可以用单位四元数表示三维空间中任意一个旋转，不过这种表达方式和复数有着微妙的不同。在复数中，乘以i意味着旋转90°。四元数中，乘以i对应着旋转180°,这样才能保证ij=k的性质。而i^2= -1，意味着绕i轴旋转360°后得到一个相反的东西。这个东西要旋转两周才会和它原先的样子相等。
（自己用笔来试一下）

4.2四元数的运算：

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

那么运算可表示如下：

4.2.1加法和减法：

4.2.2乘法：

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

如果写成向量形式并利用内外积运算，表示为：

由于最后一项外积的存在，四元数乘法通常是不可交换的。

4.2.3模长

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

4.2.4共轭

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

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

4.2.5逆

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

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

4.2.6数乘

4.3用四元数表示旋转

 我们可以用四元数表达对一个点的旋转。假设有一个空间三维点p = [x,y,z] ，以从一个由单位四元数q指定的旋转。三维点p经过旋转之后变成p'。如果使用矩阵描述，那么有p' = Rp。

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

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

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

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

任意单位四元数描述了一个旋转，该旋转也可用旋转矩阵或旋转向量描述。四元数与旋转向量、旋转矩阵之间的转换关系，在实际编程中，程序库通常会为我们准备好各种形式间的变换。

5.实践：Eigen几何模块

5.1几何模块的数据演示

Eigen几何模块可以表示四元数，欧拉角，旋转矩阵，各个形式之间可以变换。

用vim创建一个useGeometry.cpp文本并编辑：

#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;
}

编写CMakeLists.txt，执行程序后结果为：

通过读这段代码可知，Eigen 中对各种形式的表达方式总结如下。请注意每种类型都有单精度和双精度两种数据类型， 而且和之前一样， 不能由编译器自动转换。 下面以双精度为例， 你可以把最后 的 d 改成 f ， 即得到单精度的数据结构。

• 旋转矩阵（ 3 × 3 ） ： Eigen::Matrix3d 。

• 旋转向量（ 3 × 1 ） ： Eigen::AngleAxisd 。

• 欧拉角 （ 3 × 1 ）： Eigen::Vector3d 。

• 四元数（ 4 × 1 ）： Eigen::Quaterniond 。

• 欧氏变换矩阵（ 4 × 4 ）： Eigen::Isometry3d 。

5.2实际的坐标变换例子

例子：小萝卜一号、小萝卜二号，世界坐标系W，小萝卜坐标系分别为R1、R2，小萝卜一号的位姿q1=[0.35,0.2,0.3,0.1]T,  t1=[0.3,0.1,0.1]T，  小萝卜二号的位姿为q2=[-0.5,0.4,-0.1,0.2]T,   t2=[-0.1,0.5,0.3]T，小萝卜一号看到某个点在自身坐标系下的坐标为p1=[0.5,0,0.2]T，问该点在小萝卜二号坐标系下的坐标

跟之前一样的流程，code：

#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;
}

CMakeLists.txt为：

# 声明要求的 cmake 最低版本
cmake_minimum_required(VERSION 3.0.2)
 
# 声明一个 cmake 工程
project(coordinateTransform)
include_directories("/usr/include/eigen3")
add_executable(coordinateTransform coordinateTransform.cpp)

运行完结果：

注意，使用四元数前要对其进行归一化处理。

6.可视化演示

6.1显示运动轨迹

首先安装Pangolin

先安装依赖：

sudo apt-get install libgl1-mesa-dev
sudo apt-get install libglew-dev
sudo apt-get install libboost-dev libboost-thread-dev libboost-filesystem-dev
sudo apt-get install libpython2.7-dev

Pangolin0.5版本下载地址：选择tar.gz

tar -zxvf Pangolin-0.5.tar.gz

然后在Pangolin-0.5文件夹下编译安装：

mkdir build
cd build
cmake  ..
make 

（记录一下0.5版本怎么装，之后orb-slam2需要这个版本）

Pangolin0.6安装：

git clone https://github.com/stevenlovegrove/Pangolin.git      
cd Pangolin
mkdir build && cd build
cmake ..
make
sudo make install

验证是否安装成功：

cd ../examples/HelloPangolin
mkdir build && cd build
cmake ..
make
./HelloPangolin

会报这个错：

error while loading shared libraries: libpango_windowing.so: cannot open shared object file: No such file or directory error while loading shared libraries: libpango_windowing.so: cannot open shared object file: No such file or directory

网上搜索，发现是没有执行 sudo ldconfig，用于刷新共享库的原因。执行命令：

sudo ldconfig

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

                                ​​​​​​​        ​​​​​​​        time,t_{x},t_{y},t_{z},q_{x},q_{y},q_{z},q_{w}

其中，time指该位姿的记录时间，t为平移，q为旋转四元数，均是以世界坐标系到机器人坐标系记录。下面我们从文件中读取这些轨迹，并显示到一个窗口中。

code：

#include 
#include 
#include 

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

using namespace std;
using namespace Eigen;

// path to trajectory file
string trajectory_file = "/home/shikai/slamshijian/第三讲/example/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]);
      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
  }
}
                                                                                                                                                                                                                                                           

编译完执行：

 

6.2显示相机的位姿

code：

#include 
#include 

using namespace std;

#include 
#include 

using namespace Eigen;

#include 

struct RotationMatrix {
  Matrix3d matrix = Matrix3d::Identity();
};

ostream &operator<<(ostream &out, const RotationMatrix &r) {
  out.setf(ios::fixed);
  Matrix3d matrix = r.matrix;
  out << '=';
  out << "[" << setprecision(2) << matrix(0, 0) << "," << matrix(0, 1) << "," << matrix(0, 2) << "],"
      << "[" << matrix(1, 0) << "," << matrix(1, 1) << "," << matrix(1, 2) << "],"
      << "[" << matrix(2, 0) << "," << matrix(2, 1) << "," << matrix(2, 2) << "]";
  return out;
}

istream &operator>>(istream &in, RotationMatrix &r) {
  return in;
}

struct TranslationVector {
  Vector3d trans = Vector3d(0, 0, 0);
};

ostream &operator<<(ostream &out, const TranslationVector &t) {
  out << "=[" << t.trans(0) << ',' << t.trans(1) << ',' << t.trans(2) << "]";
  return out;
}

istream &operator>>(istream &in, TranslationVector &t) {
  return in;
}

struct QuaternionDraw {
  Quaterniond q;
};

ostream &operator<<(ostream &out, const QuaternionDraw quat) {
  auto c = quat.q.coeffs();
  out << "=[" << c[0] << "," << c[1] << "," << c[2] << "," << c[3] << "]";
  return out;
}

istream &operator>>(istream &in, const QuaternionDraw quat) {
  return in;
}

int main(int argc, char **argv) {
  pangolin::CreateWindowAndBind("visualize geometry", 1000, 600);
  glEnable(GL_DEPTH_TEST);
  pangolin::OpenGlRenderState s_cam(
    pangolin::ProjectionMatrix(1000, 600, 420, 420, 500, 300, 0.1, 1000),
    pangolin::ModelViewLookAt(3, 3, 3, 0, 0, 0, pangolin::AxisY)
  );

  const int UI_WIDTH = 500;

  pangolin::View &d_cam = pangolin::CreateDisplay().
    SetBounds(0.0, 1.0, pangolin::Attach::Pix(UI_WIDTH), 1.0, -1000.0f / 600.0f).
    SetHandler(new pangolin::Handler3D(s_cam));

  // ui
  pangolin::Var rotation_matrix("ui.R", RotationMatrix());
  pangolin::Var translation_vector("ui.t", TranslationVector());
  pangolin::Var euler_angles("ui.rpy", TranslationVector());
  pangolin::Var quaternion("ui.q", QuaternionDraw());
  pangolin::CreatePanel("ui").SetBounds(0.0, 1.0, 0.0, pangolin::Attach::Pix(UI_WIDTH));

  while (!pangolin::ShouldQuit()) {
    glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

    d_cam.Activate(s_cam);

    pangolin::OpenGlMatrix matrix = s_cam.GetModelViewMatrix();
    Matrix m = matrix;

    RotationMatrix R;
    for (int i = 0; i < 3; i++)
      for (int j = 0; j < 3; j++)
        R.matrix(i, j) = m(j, i);
    rotation_matrix = R;

    TranslationVector t;
    t.trans = Vector3d(m(0, 3), m(1, 3), m(2, 3));
    t.trans = -R.matrix * t.trans;
    translation_vector = t;

    TranslationVector euler;
    euler.trans = R.matrix.eulerAngles(2, 1, 0);
    euler_angles = euler;

    QuaternionDraw quat;
    quat.q = Quaterniond(R.matrix);
    quaternion = quat;

    glColor3f(1.0, 1.0, 1.0);

    pangolin::glDrawColouredCube();
    // draw the original axis
    glLineWidth(3);
    glColor3f(0.8f, 0.f, 0.f);
    glBegin(GL_LINES);
    glVertex3f(0, 0, 0);
    glVertex3f(10, 0, 0);
    glColor3f(0.f, 0.8f, 0.f);
    glVertex3f(0, 0, 0);
    glVertex3f(0, 10, 0);
    glColor3f(0.2f, 0.2f, 1.f);
    glVertex3f(0, 0, 0);
    glVertex3f(0, 0, 10);
    glEnd();

    pangolin::FinishFrame();
  }
}

执行结果：