【视觉SLAM十四讲】ch3笔记【逐行代码带你解析】【适合纯小白 ps:因为我就是】（持续更新中）

ch3 三维空间刚体运动

        这部分花的时间还是挺多的，如果上天再给我一次机会，我一定在大二上学期认认真真的好好的学线代。

3.1.旋转矩阵

3.1.1.点、向量和坐标系

        内积：

        外积：

3.1.2.坐标系间的欧氏变换

        两个坐标系间的运动由一个旋转加一个平移组成，这种运动我们称之为刚体运动。

        我们说这两个坐标系之间，相差了一个欧氏变换。
        我们从两个方面来学习这个破玩意儿。
        旋转
        同一个向量a在两个坐标系下的坐标为(a₁,a₂,a₃)^T和(a₁^’,a₂^’,a₃^’)^T，这两个坐标的基向量为(e₁,e₂,e₃)和(e₁^’,e₂^’,e₃^’)，所以根据坐标的定义，有：

        对等式左右同时左乘

        得到了：

        中间的这个3X3的矩阵R，我们就称之为旋转矩阵，该矩阵的各个分量是两个坐标基系的内积，由于基向量长度为1，所以实际上是各基向量夹角的余弦值，所以这个矩阵也称方向余弦矩阵。

        旋转矩阵还是一个行列式为+1的正交矩阵，反之，行列式为+1的正交矩阵也是一个旋转矩阵。将n维旋转矩阵的集合定义如下：
        
        称之为特殊正交群。

        旋转矩阵的逆（即转置）描述了一个相反的旋转，有：
        
                （正交矩阵的性质：逆=转置）

        这里附上我丑丑的手写笔记（别笑话我）

        在欧氏变换中，除了旋转，还有一个东东：平移
        平移记作向量t，那么把旋转和平移合到一起，就有了：
                                                a₁ = R₁₂a₂+t₁₂                     式1

        至此，我们终于能描述坐标系的变换啦！！

3.1.3.变换矩阵与齐次坐标

        如果，我们参照上面得出的式子a₁ = R₁₂a₂+t₁₂，当我们要做两次变换时（例如a₃变到a₂再变到a₁），就有以下推导公式：

        a₂ = R₂₃a₃+t₂₃
        a₁ = R₁₂a₂+t₁₂
        联立得：a₁ = R₁₂(R₂₃a₃+t₂₃)+t₁₂

        这样子的形式在变换多次时就会显得很麻烦，所以我们引入齐次坐标和变换矩阵，改写式1得到：
                

        矩阵T我们称之为变换矩阵。
        这时两次变换（如上）就可以改写为a₁= T₂T₃a₃

        变换矩阵的特点：左上角为旋转矩阵，右上角为平移向量，左下角为0向量，右下角为1.这种矩阵又称之为特殊欧氏群。
        
        与SO(3)一样，求解该矩阵的逆表示一个反向的变换。

        

3.2实践部分：Eigen3

        Eigen是一个用于求解矩阵问题的C++库，直接在终端通过apt去下载这个库。

sudo apt install libeigen3-dev

        如果没有定位到软件包那大概率就是你没有及时更新软件源，还是终端输入

sudo apt update
sudo apt upgrade

        如果失败那大概率是你没有换源，关于换源网络上的教程很多，这里给一个链接大家去看看就会了，比较简单这里就不写了。
链接: link

        下载成功过后这个库默认是安装在路径/usr/include/eigen3下的，下载完可以去验证一下。

        与其他的C++库比较，他有一个比较特殊的地方，就是这个库是由头文件组成的，因此在使用是，你并不需要在CMakeLists.txt中去链接库温文件（因为他就没有库文件），下面写一段代码来简单试用一下这个库。

#include <iostream>
//用于定时的库
#include <ctime>
//Eigen的核心部分
#include <Eigen/Core>
//Eigen用于矩阵运算的部分
#include <Eigen/Dense>
//使用命名空间
using namespace std;
using namespace Eigen;
//矩阵最大长度，后面用到的
#define MATRIX_SIZE 100

int main(int argc,char **argv){
	//如果不在前面使用命名空间的话这里应该谢写为Eigen::Matrix..，float是声明了你创建了的矩阵的数据类型，后面依次是行数和列数。
    Matrix<float,2,3> matrix_23;
    //这个比较特殊，这个库里面有一些给你写好的矩阵类型，例如Vector3d就是double类型的三维向量，本质上他也是Eigen::Matrix..，只不过封装的更好了。
    Vector3d v_3d;
    Matrix<float,3,1> vd_3d;
    //这个代表3X3的double型矩阵，Matrix3d::Zero()是Eigen中的一个函数，意思是将矩阵所有元素的置都赋值为0.
    Matrix3d matrix_33 = Matrix3d::Zero();
    //Dynamic是动态的意思，也可以用-1代替，Matrix<double,Dynamic,Dynamic>就是未知行列数的double型矩阵（动态的矩阵）。在我们定义矩阵的时候，如果矩阵的类型我们是已知的，那么我们就尽量在定义的时候写清楚，这样的话程序在运行的时候效率会高很多。
    Matrix<double,Dynamic,Dynamic> matrix_dynamic;
    //MatrixXd就是未知行列数的double型方阵。
    MatrixXd matrix_x;
    //这是给矩阵赋值。
    matrix_23 << 1,2,3,4,5,6 ;
    cout << "matrix 2x3 from 1 to 6: \n" << matrix_23 << "\n\n" <<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库定义的矩阵在运算时必须严格数据类型，不同的数据类型之间不能进行计算，当我们想让不同数据类型的矩阵进行运算的时候需要用.cast<type>()对矩阵进行类型转换。
    Matrix<double,2,1> result = matrix_23.cast<double>() * v_3d;
    cout << "[1,2,3;4,5,6] * [3;2;1] = \n" << result <<"\n\n" <<endl;

	//这里的赋值是给矩阵里的所有元素都赋值一个随机数。
    matrix_33 = Matrix3d::Random();
    //打印矩阵
    cout << "Random matrix :\n" << matrix_33 <<"\n\n" <<endl;
    //打印矩阵的转置
    cout << "Transpose : \n" << matrix_33.transpose() << "\n\n" <<endl;
    //打印矩阵的所有元素的和
    cout << "sum: \n" << matrix_33.sum() <<"\n\n" <<endl;
    //打印矩阵的迹（也就是主对角线所有元素的和）
    cout << "trace: \n" << matrix_33.trace() << "\n\n" <<endl;
    //打印矩阵的逆
    cout << "inverse: \n" << matrix_33.inverse() << "\n\n" <<endl;
    //打印矩阵的行列式的值
    cout << "det: \n" << matrix_33.determinant() << "\n\n" <<endl;


	//这里是求解矩阵的特征值和特征向量，eigen_solve(matrix_33.transpose() * matrix_33是为了保证对角化成功。
    SelfAdjointEigenSolver<Matrix3d> eigen_solve(matrix_33.transpose() * matrix_33);
    //打印矩阵的特征值
    cout << "Eigen value: \n" << eigen_solve.eigenvalues() <<"\n\n" << endl;
    //打印矩阵的特征向量
    cout << "Eigen vector: \n" << eigen_solve.eigenvectors() << "\n\n" <<endl;


	//这里是一个求解方程的例子，求解的方程为matrix_NN * X = v_Nd
	//我们通过三种方法来求解，并计算一下求解时间
    Matrix<double,MATRIX_SIZE,MATRIX_SIZE> matrix_NN = MatrixXd::Random(MATRIX_SIZE,MATRIX_SIZE);
    matrix_NN = matrix_NN * matrix_NN.transpose();
    Matrix<double, MATRIX_SIZE,1> v_Nd = MatrixXd::Random(MATRIX_SIZE,1);
	
	//记录当前的时间，储存在time_stt 里
    clock_t time_stt = clock();
    //利用直接求逆的方法求X
    Matrix<double,MATRIX_SIZE,1> x = matrix_NN.inverse() * v_Nd;
    //进行计时    ps：这里的CLOCKS_PER_SEC的意思是你计算机一秒计时的周期数，我刚开始也不知带这个是什么东东，如果不知道我在说啥那就去搜一下，下面我也会给出链接
    cout << "time of normal inverse is :"
         << 1000 * (clock()-time_stt) / (double) CLOCKS_PER_SEC
         << "ms"
         << endl;
    cout << "x = " << x.transpose() << "\n\n" <<endl;

    time_stt = clock();
    //利用QR分解法求X         （鬼知道这是什么东西，没有去深入了解，感觉应该也不用深入了解是怎么分解的，如果我学到后面发现真需要搞懂，我会回来补坑的）
    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() << "\n\n" <<endl;

    time_stt = clock();
    //利用cholesky分解法求X   （鬼知道这是什么东西，没有去深入了解，感觉应该也不用深入了解是怎么分解的，如果我学到后面发现真需要搞懂，我会回来补坑的）
    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() << "\n\n" <<endl;

	//书里说在QR分解法是最快的，试了一下确实如此
    return 0;

}

当然，代码有了，编译的时候也要记得去CmakeLists.txt里面添加一下这个头文件的路径

include_directories("/usr/include/eigen3")

完整的CMakeLists.txt文件如下：

cmake_minimum_required( VERSION 2.8 )

project( useEigen )

set( CMAKE_BUILD_TYPE "Release")

add_executable( main /home/psj/Desktop/slam_study/ch3/src/eigenMatrix.cpp)

include_directories("/usr/include/eigen3")

后面我的学习代码可能会同步在github上，现在先不搞这个。

终端打印出来的结果：

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

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
inverse: 
-0.198521   2.22739    2.8357
  1.00605 -0.555135  -1.41603
 -1.62213   3.59308   3.28973
det: 
0.208598
Eigen value: 
0.0242899
 0.992154
  1.80558
Eigen vector: 
-0.549013 -0.735943  0.396198
 0.253452 -0.598296 -0.760134
-0.796459  0.316906 -0.514998

time of normal inverse is :0.11ms
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 :0.092ms
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.028ms
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

3.3.旋转向量与欧拉角

3.3.1.旋转向量（或轴角/角轴，Axis-Angle）

        So(3)的旋转矩阵有九个量，九个量表示一个旋转（一次旋转只有三个自由度），这种表达方式是冗余的，同理，变换矩阵用16个量表达了6个自由度的变换，也是冗余的，所以我们引入更加紧凑的表达——旋转向量。

        任何旋转都可以用一个旋转轴和一个旋转角来表示，如图：

        这个向量我们就称之为旋转向量。

        旋转向量也可以装换为旋转矩阵，用罗德里格斯公式：

        其中R为旋转矩阵，西塔为旋转的角度，n为旋转轴。

        反之，我们也可以从旋转矩阵求出旋转向量，这个也有公式：

        解这些式子就得出了旋转向量的两个值：旋转角（西塔）和旋转轴（n）。

        转轴n是矩阵R特征值1对应的特征向量。

3.3.2.欧拉角（euler angles）

        还有另外一种表示旋转的就是欧拉角，欧拉角使用了三个分离的转角，大部分领域在使用欧拉角的时候都有各自的坐标方向和顺序上的习惯。

        欧拉角里面比较常用的一种，便是使用“偏航-俯仰-滚转”（yaw-pitch-roll)三个角度来描述一个旋转。他等价于ZYX轴的旋转。

        欧拉角有一个重大的缺点，就是会遇到著名的万向锁问题，这被称之为奇异性问题，理论上证明，只要想用三个实数来表达三维旋转，都会不可避免的遇到奇异性问题，因此我们很少在SLAM程序中直接使用欧拉角表示姿态。同样不会在滤波或优化中使用欧拉角表示旋转，因为它具有奇异性。不过当我们验证自己的算法时，转换成欧拉角能够帮你快速分辨结果是否正确。某些主体主要为2D运动的场合，我们也可以把旋转分解为三个欧拉角，然后把其中一个（例如偏航角）拿出来作为定位信息输出。旋转向量也具有奇异性。

        万像锁问题不太好理解，建议大家去搜个视屏看看，能很直观的一下子就了解了。

3.4.四元数

3.4.1.四元数的定义

        上面我们知道了旋转矩阵跟旋转向量还有欧拉角都带有奇异性，所以我们引入四元数。用四元数来表达三维空间旋转时它既是紧凑的，也没有奇异性。它是一种类似于复数的代数。但是四元数也有缺点，它不够直观，而且运算稍微比较复杂。

        想要了解什么是四元数，我们得先了解复平面，下面给出了一个复平面的学习链接。大家如果不清楚什么是复平面的话，可以去看一下：
        链接: link
        以下是我学习到这里的时候做的一点小笔记（不许嫌弃我字丑！！！）：

        下面开始讲四元数：

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

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

        它类似于三维空间中的差积。

        有时人们也用一个标量和一个向量来表达四元数：

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

        我们可以用单位四元数表示三维空间中任意一个旋转。

3.4.2.四元数的运算（这一部分直接用了书上的截图）

        四元数常见的有四则运算、共轭、求逆、数乘等。

3.4.3.四元数表示旋转（这一部分直接用了书上的截图）

3.4.4.四元数表示的旋转与其他表示方式的转换

        看过一遍知道怎么回事就行了，因为在我们写代码的时候，很多转换都给我们封装好了。

        四元数相比于角轴、欧拉角的优势：紧凑、无奇异性

3.5.相似、仿射、射影变换

（这部分书里标了星号，本人只是看了一遍，没有去细究他就往下了，如果学到后面发现他用处很大，那我再回来补坑。）

3.6.实践：Eigen几何模块

3.6.1.Eigen几何模块的数据演示

#include <iostream>
#include <cmath>
#include <Eigen/Core>
#include <Eigen/Geometry>

using namespace std;
using namespace Eigen;

int main(int argc ,char **argv){
	//Eigen/Geometry模块提供了各种旋转和平移的表示
	//3d旋转矩阵直接使用Matrix3d或Matrix3f，这里的Identity()函数将矩阵初始化为单位矩阵
    Matrix3d rotation_matrix = Matrix3d::Identity();
    //AngleAxisd（旋转向量），此句创建了一个绕着z轴旋转45度的旋转向量。其中，M_PI/4表示旋转角度，即π/4，Vector3d(0,0,1)表示旋转轴，即z轴的单位向量。
    AngleAxisd rotation_vector(M_PI / 4 , Vector3d(0,0,1));
    //将输出流中浮点数的小数部分保留3位，其余部分按默认方式处理。不懂的我后面给个链接嘻嘻
    cout.precision(3);
    //打印出这个旋转向量
    cout << "rotation_matrix = \n" << rotation_vector.matrix() << endl;
     
    //一条语句，将旋转向量直接转换为旋转矩阵，是不是很方便哈哈哈 
    rotation_matrix = rotation_vector.toRotationMatrix();
    //再打印出来
    cout << "rotation_matrix = \n" << rotation_matrix << endl;
	
	//这条语句定义了旋转前点v的坐标v（1，0，0）
    Vector3d v(1,0,0);
    //看不懂这一句的回去前面讲旋转向量那里再看看，旋转向量乘以原坐标得出旋转后的坐标（这是因为重载了运算符）
    Vector3d v_rotated = rotation_vector * v;
    cout << "(1,0,0) after rotation (by rotation_vector) = " << v_rotated.transpose() << endl;
	
	//旋转矩阵乘以原坐标得出旋转后的坐标（这是因为重载了运算符）
    v_rotated = rotation_matrix * v;
    cout << "(1,0,0) after rotation (by rotation_matrix) = " << v_rotated.transpose() << endl;

	//欧拉角：可以直接将旋转矩阵转换成欧拉角
	/*
	在Eigen库中，Matrix3d类和Quaterniond类都提供了eulerAngles()方法，用于将
	旋转矩阵或四元数转换为欧拉角（Euler angles）。其中，eulerAngles()方法接受
	三个参数，分别表示旋转顺序。在这个三个参数中，每个参数都是一个整数，用于表示绕
	哪个坐标轴旋转的角度。数值从0到2，分别代表x轴、y轴、z轴。例如，(2,1,0)表示先
	绕z轴旋转，再绕y轴旋转，最后绕x轴旋转。这个旋转顺序通常被称为“ZYX”旋转顺序。
	使用不同的旋转顺序，会得到不同的欧拉角表示。在机器人控制和导航中，常常需要根据
	具体的应用场景，选择合适的旋转顺序来描述物体的旋转姿态。
	*/
    Vector3d euler_angles = rotation_matrix.eulerAngles(2,1,0);
    //打印出欧拉角（“偏航-俯仰-滚转”（yaw-pitch-roll)）
    cout << "yaw pitch roll = " << euler_angles.transpose() <<endl; 

	//欧氏变换矩阵用Eigen::Isomestry   
	//忘了什么是欧氏变换矩阵的回去看3.1.3
    Isometry3d T = Isometry3d::Identity();  //三维欧氏变换矩阵是一个4X4的矩阵。这里虽然为3d，但是实质上T是4X4的矩阵
    //还记得欧氏变换矩阵有旋转矩阵和平移向量吧
    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_transformed = " << v_transformed.matrix() << endl;

	//四元数

	//可以将旋转向量直接转换成相对应的四元数，反之亦然
    Quaterniond q = Quaterniond(rotation_vector);
    //打印出来，这里有一个要注意的地方，就是coeffs（）这个函数将这个四元数转换成了矩阵，并且顺序是（x，y，z，w）其中w为实部，前三者为虚部。
    cout << "quaternion from rotation vector = " << q.coeffs().transpose() << endl;
	
	//也可以将旋转矩阵赋值给他
    q = Quaterniond(rotation_matrix);
    cout << "quaternion from rotation matrix = " << q.coeffs().transpose() << endl;
	
	//使用四元数旋转一个向量，使用重载了打乘法
    v_rotated = q * v;//这里的乘法是重载了的，数学上是 q * v * q的逆
    cout << "(1,0,0) after rotation (by quaternion) = " << v_rotated.transpose() << endl;
	
	//这里也给出了不使用重载的函数，用常规的数学运算来求出坐标，但是不建议，看起来就复杂的头皮发麻QAQ
    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
rotation_matrix = 
 0.707 -0.707      0
 0.707  0.707      0
     0      0      1
(1,0,0) after rotation (by rotation_vector) = 0.707 0.707     0
(1,0,0) after rotation (by rotation_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_transformed = 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 (by quaternion) = 0.707 0.707     0
should be equal to 0.707 0.707     0     0

关于cout.precision(3):
链接: link

        Eigen库里面一些定义好了的矩阵：

3.6.2.实际的坐标变换例子

        下面这个图片是书上的原例题：

以下是对这道题的解析：

        首先，题目中有三个坐标系，分别是世界、一号、二号（W、R₁、R₂），P_R1代表在R₁坐标系下的坐标，我们要把他转换成在R₂坐标系中的坐标，但是我们题目中对于一号和二号给出的四元数和平移向量是相对于世界坐标系的，我们能先把P_R1转换成P_W，也就是所求点相对于世界的坐标系：

                                                        P_W = T_W,R1 * P_R1

        进一步，我们就能求出所求点相对于世界的坐标：

                                                        P_R2 = T_R2,W * P_W

        综合这两式子：

                                                        P_R2 = T_R2,W * T_W,R1 * P_R1

        接下来是程序部分：

//include各种库
#include <iostream>
#include <cmath>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <algorithm>

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);
	/*
		不要忘记了欧氏变换矩阵的形式
		| R, T |
		| 0, 1 |
	*/
	//定义两个欧氏变换矩阵并把四元数赋值给他们（也就是相当于初始化变换矩阵的旋转矩阵）【四元数表示旋转】
    Isometry3d T1w(q1),T2w(q2);

	/*
		当然，你也可以这么写
		//将四元数转换为旋转矩阵
		Matrix3d R1 = q1.toRotationMatrix();
		Matrix3d R2 = q2.toRotationMatrix();
		//将旋转矩阵赋值给欧氏变换矩阵
		Isometry3d T1w.rotate(R1);
		Isometry3d T2w.rotate(R2);
	*/

    //将平移向量给到变换矩阵
    T1w.pretranslate(t1);
    T2w.pretranslate(t2);
    //根据刚才推出来的公式进行计算
    //这里为什么要对T1w取逆再运算呢，如果理解不了这一个地方的建议回到3.1去复习一下
    Vector3d p2 = T2w*T1w.inverse() *p1;
    cout << endl << p2.transpose() << endl;
    return 0;
}

3.7.可视化演示

3.7.1.Ubuntu20下Pangolin的安装

        笔者的虚拟机版本是ubuntu20，安装过程中踩了挺多坑的。。。。。建议看一遍下面的再进行安装。

        1.下载功能包、打开终端

git clone https://github.com/stevenlovegrove/Pangolin.git

        2.开始编译

先进入你下载的Pangolin文件夹
mkdir build
cd build
cmake ..
cmake --build .

        这个是编译过程中的报错信息：

        踩了半天的坑，看了很多资料都无法解决。

        看回到报错：

        进入报错的文件夹发现并没有这个文件，看了一圈可能是版本问题

于是去到github上找到他的历史版本：（0.5版本）
GitHub链接: link

        再编译一次：

先进入你下载的Pangolin文件夹
mkdir build
cd build
cmake ..
cmake --build .

        一大丢新的报错，参考了以下链接: link

        谢天谢地，终于编译成功了。。。。。

        3. 编译结束后记得安装

sudo make install

        4.然后就完成了，可以执行例子进行验证

cd Pangolin/build/examples/HelloPangolin
./HelloPangolin

        成功运行

3.7.2.实践：可视化轨迹

CMakeLists.txt

#指定cmake版本
cmake_minimum_required( VERSION 2.8 )
#工程名字
project( plotTrajectory )

set( CMAKE_BUILD_TYPE "Release")
# set( CMAKE_CXX_FLAGS ".03")

add_executable( main /home/psj/Desktop/slam_study/ch3_4/src/plotTrajectory.cpp)

include_directories("/usr/include/eigen3")


find_package(Pangolin REQUIRED)
include_directories(${Pangolin_INCLUDE_DIRS})
target_link_libraries(main ${Pangolin_LIBRARIES})

#set( CMAKE_BUILD_TYPE "Debug")

PlotTrajectory.cpp

//导入pangolin的库
#include <pangolin/pangolin.h>
//导入Eigen的相关库
#include <Eigen/Core>
#include <Eigen/Geometry>
//导入Linux系统中使用的一些系统调用函数。比如usleep函数
#include <unistd.h>

// 本例演示了如何画出一个预先存储的轨迹
using namespace std;
using namespace Eigen;

//轨迹文件的路径
string trajectory_file = "/home/psj/Desktop/slam_study/ch3_4/src/trajectory.txt";

//函数声明
void DrawTrajectory(vector<Isometry3d, Eigen::aligned_allocator<Isometry3d>>);

//定义主函数
int main(int argc, char **argv) 
{
  /*    使用Eigen库中的Isometry3d类型作为元素的向量。Isometry3d是一个3D变换矩阵类型。

        Eigen::aligned_allocator<Isometry3d> 是一个用于分配 Isometry3d 对象的自定义分配器。它是在使用
        Eigen 库时为了保证内存对齐而提供的。在 C++ 中，内存对齐是指在分配内存时，确保某些类型的对象的地址
        是对齐的，以提高访问效率。对于 Eigen 库中的一些特殊类型，如 Isometry3d，它们需要按照特定的对齐要
        求进行存储，以避免性能损失。

        poses是定义的变量
  */  
  vector<Isometry3d, Eigen::aligned_allocator<Isometry3d>> poses;
  /*
        ifstream fin(trajectory_file) 是在 C++ 中使用输入文件流对象 ifstream 打开一个文件。在这个代码
        中，它用于打开存储轨迹信息的文件 trajectory_file。

        ifstream是C++标准库中用于从文件中读取数据的类。它提供一组用于读取文件内容的成员函数和操作符重载。
        此处, trajectory_file 是一个表示文件路径的字符串，用于指定要打开的轨迹文件的位置。ifstream 对象
        fin 被创建并与该文件关联。

        一旦文件被成功打开，您就可以使用 fin 对象来读取文件中的数据。通常，您可以使用 >> 操作符重载来逐个
        读取文件中的数据项。

        请注意，打开文件之后，应该检查文件是否成功打开。可以通过检查 fin 对象的状态来确定文件是否成功打开。
        例如，可以使用 fin.is_open() 来检查文件是否打开成功。
  */
  ifstream fin(trajectory_file);
  if (!fin) 
  {
    cout << "cannot find trajectory file at " << trajectory_file << endl;
    return 1;
  }
  /*
        !fin.eof() 是对输入文件流对象 fin 的 eof() 成员函数的逻辑非操作。在C++中，eof() 函数用于检查文件
        流的结束标志。
        
        eof() 返回一个布尔值，表示文件流是否已达到文件末尾。当文件流到达文件末尾时，eof() 返回 true，否则
        返回 false。

        此处，!fin.eof() 用于检查文件流 fin 是否还未到达文件末尾。如果文件流还未到达文件末尾，!fin.eof() 
        的结果为真，表示文件流尚未结束。
  */
  while (!fin.eof()) 
  {
    /*
        time：时间
        tx：平移向量的 x 分量
        ty：平移向量的 y 分量
        tz：平移向量的 z 分量
        qx：四元数的 x 分量
        qy：四元数的 y 分量
        qz：四元数的 z 分量
        qw：四元数的 w 分量
    */
    double time, tx, ty, tz, qx, qy, qz, qw;
    //赋值
    fin >> time >> tx >> ty >> tz >> qx >> qy >> qz >> qw;
    //定义欧氏变换矩阵Twr并用四元数初始化他的旋转矩阵
    Isometry3d Twr(Quaterniond(qw, qx, qy, qz));
    //初始化他的平移矩阵
    Twr.pretranslate(Vector3d(tx, ty, tz));
    //调用 push_back(Twr)，将 Twr 添加到 poses 容器的末尾，扩展容器的大小，并将 Twr 插入到新的位置。
    poses.push_back(Twr);
  }
  //size() 是容器的成员函数之一，用于返回容器中元素的数量。
  cout << "read total " << poses.size() << " pose entries" << endl;

  //画出轨迹
  DrawTrajectory(poses);
  return 0;
}

//画出轨迹的函数
void DrawTrajectory(vector<Isometry3d, Eigen::aligned_allocator<Isometry3d>> poses) 
{
  // create pangolin window and plot the trajectory
  //创建显示窗口
  pangolin::CreateWindowAndBind("Trajectory Viewer", 1024, 768);
  /*
        glEnable(GL_DEPTH_TEST); 是OpenGL函数调用，用于启用深度测试。
        在OpenGL中，深度测试是一种用于确定像素是否应该被绘制的技术。它通过比较每个像素的深度值（即离相机的距离）
        与当前已绘制像素的深度值进行判断。只有当像素的深度值小于当前已绘制像素的深度值时，才会将该像素绘制到屏幕
        上，从而实现正确的遮挡关系。
        通过调用 glEnable(GL_DEPTH_TEST)，我们启用了深度测试功能。这意味着在绘制场景时，OpenGL会自动处理深度值，
        确保正确的遮挡关系，并按照深度值绘制像素。  
  */
  glEnable(GL_DEPTH_TEST);
  /*
        glEnable(GL_BLEND); 是OpenGL函数调用，用于启用混合功能。
        在OpenGL中，混合（Blending）是一种将新像素颜色与已存在的像素颜色进行混合的技术。通过启用混合功能，可以实
        现透明效果、颜色混合以及其他特殊效果。
        调用 glEnable(GL_BLEND) 启用了混合功能。这意味着在绘制过程中，OpenGL将根据特定的混合方程式，将新的像素颜
        色与已存在的像素颜色进行混合，并将混合后的结果绘制到屏幕上。
  */
  glEnable(GL_BLEND);
  /*
        glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA); 是 OpenGL 函数调用，用于设置混合函数。
        在 OpenGL 中，混合函数用于指定混合因子，控制混合操作中源颜色和目标颜色的权重。glBlendFunc() 函数接受两个参
        数，分别是源混合因子和目标混合因子。
        在这个特定的调用中，GL_SRC_ALPHA 指定源混合因子，表示使用源颜色的alpha值作为权重。GL_ONE_MINUS_SRC_ALPHA
        指定目标混合因子，表示使用目标颜色的 alpha 值的补数作为权重。
        因此，这个混合函数设置将根据源颜色的 alpha 值和目标颜色的 alpha 值的补数来进行混合操作。这通常用于实现透明效
        果，其中源颜色的透明度决定了最终像素的透明度，并与背景进行混合。
        需要注意的是，在调用 glBlendFunc() 之前，应该先启用混合功能（通过调用 glEnable(GL_BLEND)）
  */
  glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
  /*
        pangolin::OpenGlRenderState 是 Pangolin 库中的一个类，它用于管理 OpenGL 渲染状态，包括投影矩阵、模型视图
        矩阵和其他与渲染相关的状态。
        通过调用 pangolin::OpenGlRenderState() 的无参数构造函数，创建了一个名为 s_cam 的 OpenGlRenderState 对象。
        这个对象将用于在 Pangolin 窗口中设置和管理 OpenGL 渲染状态，以便进行后续的渲染操作。
  */
  pangolin::OpenGlRenderState s_cam
  (
  /*
        pangolin::ProjectionMatrix(1024, 768, 500, 500, 512, 389, 0.1, 1000) 是一个函数调用，用于创建 Pangolin 
        中的投影矩阵。
        pangolin::ProjectionMatrix() 是 Pangolin 库中的一个函数，它用于创建一个投影矩阵，用于定义场景的投影变换。
        在这个特定的调用中，传递了一系列参数 (1024, 768, 500, 500, 512, 389, 0.1, 1000)，用于定义投影矩阵的属性。
        具体参数的含义如下：
            1024 和 768 是视口的宽度和高度（以像素为单位）。
            -500 和 500 是投影平面的左右边界。
            -512 和 389 是投影平面的上下边界。
            0.1 和 1000 是近裁剪面和远裁剪面的位置。
        通过调用 pangolin::ProjectionMatrix(1024, 768, 500, 500, 512, 389, 0.1, 1000)，会创建一个投影矩阵，定义了
        场景的投影变换，用于后续的渲染操作和绘制。
  */
    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中的模
        型视图矩阵。
        pangolin::ModelViewLookAt() 是 Pangolin 库中的一个函数，它用于创建一个 LookAt 视图矩阵，用于定义场景的观察变换。
        在这个特定的调用中，传递了一系列参数 (0, -0.1, -1.8, 0, 0, 0, 0.0, -1.0, 0.0)，用于定义模型视图矩阵的属性。
        具体参数的含义如下：
            0, -0.1, -1.8 是相机位置的 x、y、z 坐标。
            0, 0, 0 是观察目标的 x、y、z 坐标。
            0.0, -1.0, 0.0 是相机的上向量的 x、y、z 分量。
        通过调用 pangolin::ModelViewLookAt(0, -0.1, -1.8, 0, 0, 0, 0.0, -1.0, 0.0)，会创建一个模型视图矩阵，定义了场景
        的观察变换，用于后续的渲染操作和绘制。这个观察变换将相机位置设置为 (0, -0.1, -1.8)，观察目标设置为原点 (0, 0, 0)，
        并指定了相机的上向量。
  */
    pangolin::ModelViewLookAt(0, -0.1, -1.8, 0, 0, 0, 0.0, -1.0, 0.0)
  );
  /*
        在这个语句中，首先通过调用 pangolin::CreateDisplay()创建一个显示窗口，并将返回的pangolin::View对象赋值给d_cam引用。
        接下来，通过调用 SetBounds() 方法设置了窗口的边界。具体的参数 (0.0, 1.0, 0.0, 1.0, -1024.0f / 768.0f) 定义了窗口边
        界的范围。前两个参数 0.0 和 1.0 表示窗口的水平边界范围，从左边界到右边界。后两个参数 0.0 和 1.0 表示窗口的垂直边界范围
        ，从下边界到上边界。最后一个参数 -1024.0f / 768.0f 是窗口的纵横比（aspect ratio），用于保持窗口显示的宽高比。
        最后，通过调用 SetHandler() 方法设置了一个 pangolin::Handler3D 对象作为窗口的事件处理器。该处理器使用之前创建的s_cam
        对象作为参数，用于处理与三维交互相关的事件和操作。
        通过这个语句，创建了一个窗口对象 d_cam，并设置了窗口的边界和事件处理器，以便进行后续的窗口渲染和交互操作。
  */
  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
  }
}

执行效果：