版权声明：本文为博主原创文章，未经博主允许不得转载。 https://blog.csdn.net/youngpan1101/article/details/71087430
本系列文章由 youngpan1101 出品，转载请注明出处。
文章链接： http://blog.csdn.net/youngpan1101/article/details/71087430
作者：宋洋鹏（youngpan1101）
邮箱： [email protected]
该讲详细资料下载链接 【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 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双精度三维运动描述对应的 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} )