第3讲 三维空间刚体运动

1 Eigen的使用

  cpp文件内容如下，

#include 
#include 

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

using namespace std;
using namespace Eigen;

#define MATRIX_SIZE 50   //宏定义

int main()
{
     
    //声明一个2*3的float矩阵
    Matrix<float, 2, 3> matrix_23;

    Vector3d v_3d;        //等价于Matrix v_3d
    Matrix3d matrix_33;   //等价于Matrix matrix_33
    matrix_33 = Matrix3d::Zero();   //Matrix3d::Zero()为3*3阶的零矩阵

    //如果不确定矩阵大小，可以使用动态大小的矩阵
    Matrix<double, Dynamic, Dynamic> matrix_dynamic;   //实现方法1
    MatrixXd matrix_x;   //实现方法2

    //输入数据
    matrix_23 << 1, 2, 3, 4, 5, 6;
    //输出
    cout << "matrix_23 = \n" << matrix_23 << endl;

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

    //矩阵相乘
    v_3d << 3, 2, 1;
    //注意result是double型矩阵，而matrix_23是float型矩阵，因此必须将matrix_23转换成double型矩阵
    Matrix<double, 2, 1> result = matrix_23.cast<double>() * v_3d;
    cout << "result = \n" << result << endl;

    Matrix<float, 3, 1> vd_3d(4, 5, 6);
    Matrix<float, 2, 1> result2 = matrix_23 * vd_3d;
    cout << "result2 = \n" << result2 << endl;

    //一些矩阵运算
    matrix_33 = Matrix3d::Random();  //随机数矩阵
    cout << "matrix_33 = \n" << matrix_33 << endl;
    cout << "矩阵matrix_33的转置为：\n" << matrix_33.transpose() << endl;
    cout << "矩阵matrix_33中所有元素之和为：\n" << matrix_33.sum() << endl;
    cout << "矩阵matrix_33的迹为：\n" << matrix_33.trace() << endl;
    cout << "10 * matrix_33 = \n" << 10 * matrix_33 << endl;
    cout << "矩阵matrix_33的逆为：\n" << matrix_33.inverse() << endl;
    cout << "矩阵matrix_33的行列式为：\n" << matrix_33.determinant() << endl;

    //求矩阵的特征值和特征向量
    matrix_33 = matrix_33.transpose() * matrix_33;   //实对称矩阵可以保证对角化成功
    SelfAdjointEigenSolver<Matrix3d> eigen_solver(matrix_33);
    cout << "矩阵matrix_33的特征值为：\n" << eigen_solver.eigenvalues() << endl;
    cout << "矩阵matrix_33的特征向量为：\n" << eigen_solver.eigenvectors() << endl;

    //解方程A * x = b
    Matrix<double, MATRIX_SIZE, MATRIX_SIZE> A = MatrixXd::Random(MATRIX_SIZE, MATRIX_SIZE);
    A = A.transpose() * A;   //保证半正定
    Matrix<double, MATRIX_SIZE, 1> x;
    Matrix<double, MATRIX_SIZE, 1> b = MatrixXd::Random(MATRIX_SIZE, 1);

    //方法1：直接求逆解x
    clock_t start = clock(); //计时
    x = A.inverse() * b;
    cout << "直接求逆花费的时间为：" << 1000 * (clock() - start) / (double) CLOCKS_PER_SEC << "ms" << endl;
    cout << "x = \n" << x.transpose() << endl;

    //方法2：利用QR分解求x
    start = clock();
    x = A.colPivHouseholderQr().solve(b);
    cout << "利用QR分解花费的时间为：" << 1000 * (clock() - start) / (double) CLOCKS_PER_SEC << "ms" << endl;
    cout << "x = \n" << x.transpose() << endl;

    //对于正定矩阵，还可以用Cholesky分解求x
    start = clock();
    x = A.ldlt().solve(b);
    cout << "利用Cholesky分解花费的时间为：" << 1000 * (clock() - start) / (double) CLOCKS_PER_SEC << "ms" << endl;
    cout << "x = \n" << x.transpose() << endl;



    return 0;

}

  相应的CMakeLists.txt内容为：

cmake_minimum_required(VERSION 3.10)

project("3-1")

include_directories("/usr/include/eigen3")

add_executable(main main.cpp)

  结果如下所示，

matrix_23 = 
1 2 3
4 5 6
matrix_23 = 
1 2 3 
4 5 6 
result = 
10
28
result2 = 
32
77
matrix_33 = 
 0.680375   0.59688 -0.329554
-0.211234  0.823295  0.536459
 0.566198 -0.604897 -0.444451
矩阵matrix_33的转置为：
 0.680375 -0.211234  0.566198
  0.59688  0.823295 -0.604897
-0.329554  0.536459 -0.444451
矩阵matrix_33中所有元素之和为：
1.61307
矩阵matrix_33的迹为：
1.05922
10 * matrix_33 = 
 6.80375   5.9688 -3.29554
-2.11234  8.23295  5.36459
 5.66198 -6.04897 -4.44451
矩阵matrix_33的逆为：
-0.198521   2.22739    2.8357
  1.00605 -0.555135  -1.41603
 -1.62213   3.59308   3.28973
矩阵matrix_33的行列式为：
0.208598
矩阵matrix_33的特征值为：
0.0242899
 0.992154
  1.80558
矩阵matrix_33的特征向量为：
-0.549013 -0.735943  0.396198
 0.253452 -0.598296 -0.760134
-0.796459  0.316906 -0.514998
直接求逆花费的时间为：1.953ms
x = 
-1164.69 -381.519  372.996  795.185  380.903 -58.7773  239.368 -529.252  217.491  -45.514 -645.898  728.862 -231.748  48.7265 -13.2979  290.475  416.367 -669.864  413.731  250.427 -243.593 -476.972 -170.741 -920.584 -388.211  191.107 -457.996 -473.321  74.7525 -341.087 -356.935 -589.916  362.278  366.526 -75.5243  -34.571  -587.88  315.681  -304.92  784.666  106.104  300.407   -433.7 -645.511  152.065  113.522  259.167  24.6086 -517.915 -83.5159
利用QR分解花费的时间为：2.404ms
x = 
-1164.69 -381.519  372.996  795.185  380.903 -58.7773  239.368 -529.252  217.491  -45.514 -645.898  728.862 -231.748  48.7265 -13.2979  290.475  416.367 -669.864  413.731  250.427 -243.593 -476.972 -170.741 -920.584 -388.211  191.107 -457.996 -473.321  74.7525 -341.087 -356.935 -589.916  362.278  366.526 -75.5243  -34.571  -587.88  315.681  -304.92  784.666  106.104  300.407   -433.7 -645.511  152.065  113.522  259.167  24.6086 -517.915 -83.5159
利用Cholesky分解花费的时间为：0.717ms
x = 
-1164.69 -381.519  372.996  795.185  380.903 -58.7773  239.368 -529.252  217.491  -45.514 -645.898  728.862 -231.748  48.7265 -13.2979  290.475  416.367 -669.864  413.731  250.427 -243.593 -476.972 -170.741 -920.584 -388.211  191.107 -457.996 -473.321  74.7525 -341.087 -356.935 -589.916  362.278  366.526 -75.5243  -34.571  -587.88  315.681  -304.92  784.666  106.104  300.407   -433.7 -645.511  152.065  113.522  259.167  24.6086 -517.915 -83.5159

2 Eigen几何模块

  cpp文件内容如下，

#include 
#include 

using namespace std;

#include        //Eigen核心部分
#include    //Eigen几何模块

using namespace Eigen;

int main()
{
     
    Matrix3d rotation_matrix = Matrix3d::Identity();  //Matrix3d::Identity()表示3*3单位矩阵
    cout << "rotation_matrix = \n" << rotation_matrix << endl;
    AngleAxisd rotation_vector(M_PI / 4, Vector3d(0, 0, 1));   //沿Z轴旋转45度
    cout << "rotation_vector = \n" << rotation_vector.matrix() << endl;

    cout.precision(3);   //设置cout输出保留三位有效数字

    cout << "将旋转向量转换为旋转矩阵：\n" << rotation_vector.matrix() << endl;

    rotation_matrix = rotation_vector.toRotationMatrix();

    //用角轴进行坐标变换
    Vector3d p(1, 0, 0);
    Vector3d q1 = rotation_vector * p;
    cout << "q1 = \n" << q1 << endl;
    //用旋转矩阵进行坐标变换
    q1 = rotation_matrix * p;
    cout << "q1 = \n" << q1 << endl;

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

    //欧式变换矩阵表示
    Isometry3d T = Isometry3d::Identity();   //Isometry3d::Identity()表示4*4单位矩阵
    cout << "T = \n" << T.matrix() << endl;  //输出T
    T.rotate(rotation_vector);   //设置T中的旋转部分
    T.pretranslate(Vector3d(1, 3, 4));   //设置T中的平移部分
    cout << "T = \n" << T.matrix() << endl;

    //用变换矩阵进行坐标变换
    Vector3d q2 = T * p;  //相当于R*p+t
    cout << "q2 = \n" << q2 << endl;

    //用单位四元数表示旋转
    Quaterniond quaternion = Quaterniond(rotation_vector);  //用角轴进行赋值
    cout << "quaternion = \n" << quaternion.coeffs() << endl;
    cout << "请注意quaternion.coeffs()中的顺序为(x, y, z, w)，w为实部，前三者为虚部！" << endl;
    cout << "请注意使用Quaterniond()初始化时却是(w, x, y, z)的顺序！" << endl;
    quaternion = Quaterniond(rotation_matrix);   //用旋转矩阵进行赋值
    cout << "quaternion = \n" << quaternion.coeffs() << endl;
    //使用四元数旋转一个向量，重载乘法
    Vector3d q3 = quaternion * p;   //注意数学上是q = quaternion * p * quaternion.inverse();
    cout << "q3 = \n" << q3 << endl;
    //使用四元数乘以四元数
    Quaterniond q4 = quaternion * Quaterniond(0, 1, 0 , 0) * quaternion.inverse();
    cout << "q4 = \n" << q4.coeffs() << endl;

    cout << "Quaterniond(0, 1, 0 , 0) = \n" << Quaterniond(0, 1, 0 , 0).coeffs() << endl;

    return 0;
}

  CMakeLists.txt文件内容如下，

include_directories("/usr/include/eigen3")

add_executable(main main.cpp)

  结果如下所示，

rotation_matrix = 
1 0 0
0 1 0
0 0 1
rotation_vector = 
 0.707107 -0.707107         0
 0.707107  0.707107         0
        0         0         1
将旋转向量转换为旋转矩阵：
 0.707 -0.707      0
 0.707  0.707      0
     0      0      1
q1 = 
0.707
0.707
    0
q1 = 
0.707
0.707
    0
yaw pitch roll = 0.785    -0     0
T = 
1 0 0 0
0 1 0 0
0 0 1 0
0 0 0 1
T = 
 0.707 -0.707      0      1
 0.707  0.707      0      3
     0      0      1      4
     0      0      0      1
q2 = 
1.71
3.71
   4
quaternion = 
    0
    0
0.383
0.924
请注意quaternion.coeffs()中的顺序为(x, y, z, w)，w为实部，前三者为虚部！
请注意使用Quaterniond()初始化时却是(w, x, y, z)的顺序！
quaternion = 
    0
    0
0.383
0.924
q3 = 
0.707
0.707
    0
q4 = 
0.707
0.707
    0
    0
Quaterniond(0, 1, 0 , 0) = 
1
0
0
0

3 坐标变换例子

  cpp文件内容为，

#include 

using namespace std;

#include        //Eigen核心模块
#include    //Eigen几何模块

using namespace Eigen;

int main()
{
     
    Quaterniond q1(0.35, 0.2, 0.3, 0.1), q2(-0.5, 0.4, -0.1, 0.2); //位姿，以Tcw形式存储
    q1.normalize();  //四元数归一化
    q2.normalize();  //四元数归一化

    Vector3d t1(0.3, 0.1, 0.1), t2(-0.1, 0.5, 0.3);
    Vector3d P(0.5, 0, 0.2);

    Isometry3d Tc1w(q1), Tc2w(q2);
    Tc1w.pretranslate(t1);
    Tc2w.pretranslate(t2);

    Vector3d Q = Tc2w * Tc1w.inverse() * P;

    cout << "Q = \n" << Q << endl;
    
    return 0;
}

  CMakeLists.txt文件内容为，

include_directories("/usr/include/eigen3")

add_executable(main main.cpp)

  结果如下所示，

Q = 
-0.0309731
   0.73499
  0.296108

4 显示运动轨迹

  cpp文件内容如下，

#include 
#include 
#include 

using namespace std;

#include 

#include   //Eigen核心模块

using namespace Eigen;

//以Twc格式存储位姿
//该txt文件中每行的格式为：time, tx, ty, tx, qx, qy, qz, qw
string path = "../trajectory.txt";

void DrawTrajectory(vector<Isometry3d, Eigen::aligned_allocator<Isometry3d>>);
//Eigen::aligned_allocator表示内存管理方法
//向量类容器中的元素是Isometry3d，内存管理方法是Eigen::aligned_allocator

int main()
{
     
    vector<Isometry3d, Eigen::aligned_allocator<Isometry3d>> poses;
    ifstream fin(path);   //等价于：ifstream fin; fin.open(path);
    if(!fin)   //如果打开文件失败则执行
    {
     
        cout << "打开文件" << path << "失败！" << endl;
        return 1;
    }

    while(!fin.eof())  //如果此时fin并不是end of file，那么执行以下内容
    {
     
        double time, tx, ty, tz, qx, qy, qz, qw;
        fin >> time >> tx >> ty >> tz >> qx >> qy >> qz >> qw;
        Isometry3d Twc(Quaterniond(qw, qx, qy, qz));
        Twc.pretranslate(Vector3d(tx, ty, tz));
        poses.push_back(Twc);
    }

    cout << "一共有" << poses.size() << "个位姿！" << endl;

    //调用DrawTrajectory()函数进行绘图
    DrawTrajectory(poses);

    return 0;
}


void DrawTrajectory(vector<Isometry3d, Eigen::aligned_allocator<Isometry3d>> poses)
{
     
    //创建pangolin窗口和绘制轨迹
    pangolin::CreateWindowAndBind("Trajectory Viewer", 1024, 768);  //分别表示窗口名、窗口宽度和窗口高度
    glEnable(GL_DEPTH_TEST);  //启用深度渲染，当需要显示3D模型时需要打开，根据目标的远近自动隐藏被遮挡的模型
    glEnable(GL_BLEND);       //表示窗口使用颜色混合模式，让物体显示半透明效果
    glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
    //GL_SRC_ALPHA表示使用源颜色的alpha值作为权重因子，GL_ONE_MINUS_SRC_ALPHA表示用1.0-源颜色的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)
            );
    //ProjectionMatrix中各参数依次为图像宽度、图像高度、fx、fy、cx、cy、最近距离和最远距离
    //ModelViewLookAt中各参数依次为相机的三维位置，观看视点的三维位置和设置相机各轴的正方向，右下前为000
    //比如，ModelViewLookAt(0, -0.1, -1.8, 0, 0, 0, 0.0, -1.0, 0.0)表示相机在(0, -0.1, -1.8)位置处观看视点(0, 0, 0)，并设置相机XYZ轴正方向为0-10，即右上前


    //创建交互视图，用于显示上一步相机所观测到的内容
    pangolin::Handler3D handler(s_cam);   //交互相机视图句柄
    pangolin::View &d_cam = pangolin::CreateDisplay()
            .SetBounds(0.0, 1.0, 0.0, 1.0, -1024.0f / 798.0f)
            .SetHandler(&handler);
    //SetBounds()内的前4个参数分别表示交互视图的大小，均为相对值，范围在0.0至1.0之间
    //第1个参数表示bottom，即为视图最下面在整个窗口中的位置
    //第2个参数为top，即为视图最上面在整个窗口中的位置
    //第3个参数为left，即视图最左边在整个窗口中的位置
    //第4个参数为right，即为视图最右边在整个窗口中的位置
    //第5个参数为aspect，表示横纵比，负号和正号表示什么意思呢？

    while(pangolin::ShouldQuit() == false)   //ShouldQuit()检测你是否关闭OpenGL窗口，如果没有关闭则执行
    {
     
        glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); //清空颜色和深度缓存，这样每次都会刷新显示，不至于前后帧的颜色信息相互干扰
        d_cam.Activate(s_cam); //激活显示，并设置状态矩阵
        glClearColor(1.0f, 1.0f, 1.0f, 1.0f); //将背景显示为白色


        //绘制每个时刻的相机坐标系
        for(int i = 0; i < poses.size(); i++)
        {
     
            Vector3d O = poses[i].translation(); //相机坐标系的坐标原点(0, 0, 0)在世界系下的坐标
            Vector3d A = poses[i] * Vector3d(0.02, 0, 0);  //相机坐标系中的点(0.1, 0, 0)在世界坐标系下的坐标
            Vector3d B = poses[i] * Vector3d(0, 0.02, 0);  //相机坐标系中的点(0, 0.1, 0)在世界坐标系下的坐标
            Vector3d C = poses[i] * Vector3d(0, 0, 0.02);  //相机坐标系中的点(0, 0, 0.1)在世界坐标系下的坐标

            //绘制直线OA、OB和OC分别表示该时刻相机坐标系的X轴、Y轴和Z轴
            glBegin(GL_LINES);
            glLineWidth(2); //线宽设置为2
            glColor3f(1.0, 0.0, 0.0); //X轴用红色表示
            glVertex3d(O[0], O[1], O[2]);
            glVertex3d(A[0], A[1], A[2]);
            glColor3f(0.0, 1.0, 0.0); //Y轴用绿色表示
            glVertex3d(O[0], O[1], O[2]);
            glVertex3d(B[0], B[1], B[2]);
            glColor3d(0.0, 0.0, 1.0); //Z轴用蓝色表示
            glVertex3d(O[0], O[1], O[2]);
            glVertex3d(C[0], C[1], C[2]);
            glEnd(); //对应于glBegin()

            //绘制点O
            glBegin(GL_POINTS);
            glColor3f(1.0, 1.0, 1.0);
            glVertex3f(O[0], O[1], O[2]);
            glEnd();
        }

        //画出相邻时刻相机光心连线O1O2，表示在世界坐标系下
        for(int i = 0; i < poses.size() - 1; i++)
        {
     
            glBegin(GL_LINES); //绘制直线O1O2
            glColor3f(0.0, 0.0, 0.0); //设置直线颜色为黑色
            glLineWidth(2); //线宽设置为2
            Vector3d O1 = poses[i].translation();
            Vector3d O2 = poses[i+1].translation();
            glVertex3d(O1[0], O1[1], O1[2]);
            glVertex3d(O2[0], O2[1], O2[2]);
            glEnd();
        }

        pangolin::FinishFrame();  //执行后期渲染、事件处理和帧交换，按照前面的设置进行最终的显示
        usleep(5000);    //程序停止执行5000微秒，即5毫秒

    }




}

  CMakeLists.txt文件内容为，

include_directories("/usr/include/eigen3")

find_package(Pangolin REQUIRED)
include_directories(${
     Pangolin_DIRECTORIES})

add_executable(main main.cpp)

target_link_libraries(main ${
     Pangolin_LIBRARIES})

  运行结果如下，

5 显示相机的位姿

  cpp文件内容为，

#include 
#include 

using namespace std;

#include       //Eigen核心模块
#include   //Eigen几何模块

using namespace Eigen;

#include 

struct RotationMatrix
{
     
    Matrix3d matrix = Matrix3d::Identity();   //Matrix3d::Identity()表示3*3单位阵
};

//重载结构体RotationMatrix的输出流运算符<<
ostream &operator << (ostream& out, const RotationMatrix& r)
{
     
    out.setf(ios::fixed);  //fixed表示用正常的记数方法显示浮点数
    Matrix3d matrix = r.matrix;
    out << "=";
    //先out.setf(ios::fixed)，后setprecision(2)，效果为保留小数点后两位
    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;
}

//重载结构体RotationMatrix的输入流运算符>>
istream &operator >> (istream& in, RotationMatrix& r)
{
     
    return in;
}

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

//重载结构体TranslationVector的输出流运算符<<
ostream &operator << (ostream& out, const TranslationVector& t)
{
     
    out << fixed << setprecision(2);  //保留小数点后两位
    out << "= [" << t.trans(0) << ", " << t.trans(1) << ", " << t.trans(2) << "]";
    return out;
}

//重载结构体TranslationVector的输入流运算符>>
istream &operator >> (istream& in, TranslationVector& t)
{
     
    return in;
}

struct QuaternionDraw
{
     
    Quaterniond q;
};

//重载结构体QuaternionDraw的输出流运算符<<
ostream &operator << (ostream& out, const QuaternionDraw quat)
{
     
    Vector4d c = quat.q.coeffs();   //c中的格式为x,y,z,w
    out << fixed << setprecision(2);  //保留小数点后两位
    out << "= [" << c[0] << ", " << c[1] << ", " << c[2] << ", " << c[3] << "]";
    return out;
}

//重载结构体QuaternionDraw的输入流运算符>>
istream &operator >> (istream& in, const QuaternionDraw& quat)
{
     
    return in;
}

int main()
{
     
    //创建pangolin窗口
    pangolin::CreateWindowAndBind("visualize geometry", 1000, 600);  //分别表示窗口名、窗口宽度和窗口高度
    glEnable(GL_DEPTH_TEST);  //启用深度渲染，当需要显示3D模型时需要打开，根据目标的远近自动隐藏被遮挡的模型

    //创建一个观察相机视图，包括投影矩阵和相机视角
    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)
            );
    //ProjectionMatrix()中各参数依次为图像宽度、图像高度、fx、fy、cx、cy、最近距离和最远距离
    //ModelViewLookAt()中各参数为相机位置，被观察点位置和相机哪个轴朝上

    const int UI_WIDTH = 500;  //用户界面所占宽度，从左边算起

    //创建交互视图，用于显示上一步相机所观测到的内容
    pangolin::Handler3D handler(s_cam);  //交互相机视图句柄
    pangolin::View& d_cam = pangolin::CreateDisplay().
            SetBounds(0.0, 1.0, pangolin::Attach::Pix(UI_WIDTH), 1.0, -1000.0f / 600.0f).
            SetHandler(&handler);
    //SetBounds()中各参数表示交互视图的位置和窗口的横纵比

    //设置UI界面
    pangolin::Var<RotationMatrix> rotation_matrix("ui.R", RotationMatrix());  //显示旋转矩阵
    pangolin::Var<TranslationVector> translation_vector("ui.t", TranslationVector());  //显示平移向量
    pangolin::Var<TranslationVector> euler_angles("ui.rpy_X-Y-Z", TranslationVector());  //显示欧拉角
    pangolin::Var<QuaternionDraw> quaternion("ui.q_x-y-z-w", QuaternionDraw());  //显示四元数
    pangolin::CreatePanel("ui").SetBounds(0.0, 1.0, 0.0, pangolin::Attach::Pix(UI_WIDTH));  //设置用户界面的大小

    while(!pangolin::ShouldQuit())  //如果没有关闭OpenGL窗口，则执行
    {
     
        glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);  //清空颜色和深度缓存，使每一帧之间互不干扰

        d_cam.Activate(s_cam);  //激活显示，设置相机状态

        pangolin::OpenGlMatrix matrix = s_cam.GetModelViewMatrix();  //将模型可视化矩阵赋值给matrix
        Matrix<double, 4, 4> 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();

        //绘制坐标系
        glLineWidth(3);  //设置线宽为3
        glBegin(GL_LINES);
        glColor3f(0.8f, 0.0f, 0.0f);  //设置X轴为红色
        glVertex3f(0, 0, 0);
        glVertex3f(10, 0, 0);
        glColor3f(0.0f, 0.8f, 0.0f);  //设置Y轴为绿色
        glVertex3f(0, 0, 0);
        glVertex3f(0, 10, 0);
        glColor3f(0.2f, 0.2f, 1.0f);  //设置Z轴颜色
        glVertex3f(0, 0, 0);
        glVertex3f(0, 0, 10);
        glEnd();

        pangolin::FinishFrame();  //执行后期渲染、事件处理和帧交换，按照前面的设置进行最终的显示
    }
    
    return 0;
}

  CMakeLists.txt文件内容为，

include_directories("/usr/include/eigen3")

find_package(Pangolin REQUIRED)
include_directories( ${
     Pangolin_DIRECTORIES} )

add_executable(main main.cpp)
target_link_libraries(main ${
     Pangolin_LIBRARIES})

  结果为，