半闲居士视觉SLAM十四讲笔记（3）三维空间刚体运动 - part 5 Eigen_Geometry、Pangolin安装

https://blog.csdn.net/youngpan1101/article/details/71087430

三维空间的刚体运动描述方式
实践
Eigen 几何模块
Eigen Geometry Document
这里贴出 eigenGeometry.cpp 的代码和运行结果：
#include
#include
using namespace std;

#include
// Eigen 几何模块
#include

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

int main ( int argc, char** argv )
{
    // Eigen/Geometry 模块提供了各种旋转和平移的表示
    // 3D 旋转矩阵直接使用 Matrix3d 或 Matrix3f
    Eigen::Matrix3d rotation_matrix = Eigen::Matrix3d::Identity();
    // 旋转向量使用 AngleAxis, 它底层不直接是Matrix，但运算可以当作矩阵（因为重载了运算符）
    Eigen::AngleAxisd rotation_vector ( M_PI/4, Eigen::Vector3d ( 0,0,1 ) );  //沿 Z 轴旋转 45 度
    cout.precision(3); // 输出精度为小数点后三位
    cout << "rotation matrix =\n" << rotation_vector.matrix() << endl;  //用matrix()转换成旋转矩阵
    // 也可以直接赋值
    rotation_matrix = rotation_vector.toRotationMatrix();
    // 用 AngleAxis 可以进行坐标变换
    Eigen::Vector3d v ( 1,0,0 );
    Eigen::Vector3d v_rotated = rotation_vector * v;
    cout << "(1,0,0) after rotation = " << v_rotated.transpose() << endl;
    // 或者用旋转矩阵
    v_rotated = rotation_matrix * v;
    cout << "(1,0,0) after rotation = " << v_rotated.transpose() << endl;

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

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

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

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

    // 四元数
    // 可以直接把 AngleAxis 赋值给四元数，反之亦然
    Eigen::Quaterniond q = Eigen::Quaterniond ( rotation_vector );
    cout << "quaternion = \n" << q.coeffs() << endl;   // 请注意coeffs的顺序是(x,y,z,w),w为实部，前三者为虚部
    // 也可以把旋转矩阵赋给它
    q = Eigen::Quaterniond ( rotation_matrix );
    cout << "quaternion = \n" << q.coeffs() <     // 使用四元数旋转一个向量，使用重载的乘法即可
    v_rotated = q*v; // 注意数学上是qvq^{-1}
    cout << "(1,0,0) after rotation = " << v_rotated.transpose() << endl;

    return 0;
}

输出结果：
rotation matrix =
    0.707    -0.707         0
    0.707     0.707         0
        0         0         1
(1,0,0) after rotation =    0.707    0.707        0
(1,0,0) after rotation =    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 = 
0
0
0.383
0.924
quaternion = 
0
0
0.383
0.924
(1,0,0) after rotation =    0.707    0.707        0
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
双精度三维运动描述对应的 Eigen 数据类型总结

三维运动变换    Eigen 库数据结构
旋转矩阵（3 x 3）    Eigen::Matrix3d
旋转向量（3 x 1）    Eigen::AngleAxisd
欧拉角（3 x 1）    Eigen::Vector3d
四元数 （4 x 1）    Eigen::Quaterniond
欧氏变换矩阵（4 x 4 ）    Eigen::Isometry3d
仿射变换（4 x 4）    Eigen::Affine3d
射影变换（4 x 4）    Eigen::Perspective3d
——————————– 分割线<< 家有小武，如有一母家有小武，如有一母 >>分割线 ——————————–

可视化演示
安装 Pangolin

Pangolin 是一个管理 OpenGL 显示/交互和抽象视频输入的小型可移植的库文件。

安装 Pangolin 之前 ，请先按照以下的命令安装一些依赖的库

$ sudo apt-get install libglew-dev    
$ sudo apt-get install cmake
$ sudo apt-get install libboost-dev libboost-thread-dev libboost-filesystem-dev
$ sudo apt-get install libpython2.7-dev
1
2
3
4
使用克隆的指令安装

$ git clone https://github.com/stevenlovegrove/Pangolin.git
1
安装好 Pangolin 后，要使用如下方法编译 Pangolin

$ cd Pangolin-master
$ mkdir build
$ cd build
$ cmake -DCPP11_NO_BOOSR=1 ..
$ make  -j4 (或者也可以使用 make –j1)
$ sudo make install 
1
2
3
4
5
6
编译 visualizeGeometry 工程并运行： 

CMakeLists.txt:

cmake_minimum_required( VERSION 2.8 )
project( visualizeGeometry )

set(CMAKE_CXX_FLAGS "-std=c++11")

# 添加Pangolin依赖

find_package( Pangolin )
include_directories( ${Pangolin_INCLUDE_DIRS} )

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