半闲居士视觉SLAM十四讲笔记（3）三维空间刚体运动

本系列文章由 youngpan1101 出品，转载请注明出处。
文章链接： http://blog.csdn.net/youngpan1101/article/details/71087430
作者：宋洋鹏（youngpan1101）
邮箱： [e-mail]

---

该讲详细资料下载链接 【Baidu Yun】【Video】【Code】

若您觉得本博文对您有帮助，请支持高博的新书《视觉SLAM十四讲》，【点击购买】
若您觉得本博文对您有帮助，请支持高博的新书《视觉SLAM十四讲》，【点击购买】
若您觉得本博文对您有帮助，请支持高博的新书《视觉SLAM十四讲》，【点击购买】

---

三维空间的刚体运动描述方式

实践

Eigen 几何模块

• Eigen Geometry Document
• 这里贴出 eigenGeometry.cpp 的代码和运行结果：

#include <iostream>#include <cmath>using namespace std;#include <Eigen/Core>// Eigen 几何模块#include <Eigen/Geometry>/***************************** 本程序演示了 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() <<endl;    // 用变换矩阵进行坐标变换    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() <<endl;    // 使用四元数旋转一个向量，使用重载的乘法即可    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        0yaw pitch roll = 0.785 -0  0Transform matrix =     0.707    -0.707         0         1    0.707     0.707         0         3        0         0         1         4        0         0         0         1v tranformed =    1.71    3.71       4quaternion = 000.3830.924quaternion = 000.3830.924(1,0,0) after rotation =    0.707    0.707        0

• 双精度三维运动描述对应的 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

  • 使用克隆的指令安装

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

  • 安装好 Pangolin 后，要使用如下方法编译 Pangolin

    $ cd Pangolin-master$ mkdir build$ cd build$ cmake -DCPP11_NO_BOOSR=1 ..$ make  -j4 (或者也可以使用 make –j1)$ sudo make install 

  • 编译 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} )