part 5 Eigen_Geometry、Pangolin安裝
三維空間的剛體運動描述方式 實踐 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 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} )