視覺SLAM——第三章 Eigen幾何模組Geometry使用 四元素 歐式變換矩陣
視覺SLAM——第三章
Eigen幾何模組Geometry使用 四元素 歐式變換矩陣
github連結 點選開啟連結
* 本程式演示了 Eigen 幾何模組的使用方法 * 旋轉向量 Eigen::AngleAxisd 角度 軸 Eigen::AngleAxisd rotation_vector ( M_PI/4, Eigen::Vector3d ( 0,0,1 ) ); //沿 Z 軸旋轉 45 度 * 旋轉矩陣 Eigen::Matrix3d rotation_vector.toRotationMatrix(); //旋轉向量轉換到旋轉矩陣 * Eigen::Matrix3d R = Eigen::AngleAxisd(M_PI/2, Eigen::Vector3d(0,0,1)).toRotationMatrix();// 直接轉
* 尤拉角 Eigen::Vector3d rotation_matrix.eulerAngles ( 2,1,0 );// ( 2,1,0 )表示ZYX順序，即roll pitch yaw順序 旋轉矩陣到 尤拉角轉換到尤拉角 * 四元素 Eigen::Quaterniond q = Eigen::Quaterniond ( rotation_vector );// 旋轉向量 定義四元素 * q = Eigen::Quaterniond ( rotation_matrix );//旋轉矩陣定義四元素
* 歐式變換矩陣 Eigen::Isometry3d T=Eigen::Isometry3d::Identity();// 雖然稱為3d，實質上是4＊4的矩陣 旋轉 R+ 平移T * T.rotate ( rotation_vector ); // 按照rotation_vector進行旋轉 * 也可 Eigen::Isometry3d T(q) // 一步 按四元素表示的旋轉 旋轉 轉換矩陣 * T.pretranslate ( Eigen::Vector3d ( 1,3,4 ) ); // 把平移向量設成(1,3,4) * 輸出 cout<< T.matrix() <<endl;
#include <iostream> #include <cmath> using namespace std;
#include <Eigen/Core> // Eigen 幾何模組 #include <Eigen/Geometry>
/**************************** * 本程式演示了 Eigen 幾何模組的使用方法 ****************************/
int main ( int argc, char** argv ) { //注意一下型別名的最後一個字元為d表示雙精度型別,換成f表示單精度型別，兩種型別不能混用，必須顯示轉換 // Eigen/Geometry 模組提供了各種旋轉和平移的表示 // 3D 旋轉矩陣直接使用 Matrix3d 或 Matrix3f /****旋轉向量****/ // 旋轉向量使用 AngleAxis, 它底層不直接是Matrix，但運算可以當作矩陣（因為過載了運算子） // 乘以該向量，表示進行一個座標變換 //任意旋轉可用一個旋轉軸和一個旋轉角度來表示。 //旋轉向量，旋轉向量的方向與旋轉軸一致，長度為旋轉角度。 /*********************************/ /*旋轉向量 沿 Z 軸旋轉 45 度 角度 軸 */ 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()轉換成矩陣 // 也可以直接賦值 /*********************************/ /*旋轉矩陣*/ Eigen::Matrix3d rotation_matrix = Eigen::Matrix3d::Identity();//單位陣 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順序，即roll pitch yaw順序 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<<"(1,0,0) after Isometry3d tranformed = "<<v_transformed.transpose()<<endl;
// 對於仿射和射影變換，使用 Eigen::Affine3d 和 Eigen::Projective3d 即可，略
/*******四元數表示的旋轉***********/ // 可以直接把AngleAxis賦值給四元數，反之亦然 Quaterniond 表示雙精度 四元素 Quaternionf 表示單精度四元素 Eigen::Quaterniond q = Eigen::Quaterniond ( rotation_vector );// 表示沿Z 軸旋轉 45 度 的四元素變換 cout<<"quaternion from AngleAxis rotation_vector = \n"<<q.coeffs() <<endl; // 請注意coeffs的順序是(x,y,z,w),w為實部，前三者為虛部 // 也可以把旋轉矩陣賦給它 q = Eigen::Quaterniond ( rotation_matrix ); cout<<"quaternion from rotation_matrix = \n"<<q.coeffs() <<endl; // 使用四元數旋轉一個向量，使用過載的乘法即可 /*注意程式表達形式和實際運算的不一樣*/ v_rotated = q*v; // 注意數學上是q*v*q^{-1} 而程式為了簡化表示 直接使用 q*v代替 cout<<"(1,0,0) after Quaterniond rotation = "<<v_rotated.transpose()<<endl; /*程式設計題目 小蘿蔔１號位姿q1=[0.35,0.2,0.3,0.1],t1=[0.3,0.1,0.1]' 世界座標系到相機變換 小蘿蔔２號位姿q2=[-0.5,0.4,-0.1,0.2],t2=[-0.1,0.5,0.3]' 小蘿蔔１號看到位於自身座標系下p=[0.5,0,0.2]' 求該向量在小蘿蔔２號下的座標 */ Eigen::Quaterniond q1(0.35,0.2,0.3,0.1);//wxyz q1.coeffs() xyzw q1.vec() xyz //q1 << 0.35,0.2,0.3,0.1; Eigen::Matrix<double, 3, 1> t1;//float型別 t1 << 0.3,0.1,0.1; Eigen::Quaterniond q2(-0.5,0.4,-0.1,0.2); //q2 << -0.5,0.4,-0.1,0.2; Eigen::Matrix<double, 3, 1> t2;//float型別 t2 << -0.1,0.5,0.3; Eigen::Matrix<double, 3, 1> p1;//float型別 p1 << 0.5,0,0.2; cout<<"q1= \n"<< q1.coeffs() <<endl; cout<<"t1= \n"<< t1 <<endl; cout<<"q2= \n"<< q2.coeffs() <<endl; cout<<"t2= \n"<< t2 <<endl;
/* q1.setIdentity(); cout<<"q1 after setIdentity \n"<<q1.coeffs() <<endl; q2.setIdentity(); cout<<"q2 after setIdentity \n"<<q2.coeffs() <<endl; */ //規範化 歸一化 除以模長 q1=q1.normalized(); cout<<"q1 after normalized\n"<<q1.coeffs() <<endl; q2=q2.normalized(); cout<<"q2 after normalized \n"<<q2.coeffs() <<endl; Eigen::Matrix3d q1rotation_matrix = Eigen::Matrix3d::Identity();//單位陣 q1rotation_matrix=q1.toRotationMatrix(); Eigen::Isometry3d Tc1w=Eigen::Isometry3d::Identity();// 雖然稱為3d，實質上是4＊4的矩陣 齊次座標 Tc1w.rotate (q1rotation_matrix ); // 按照q1rotation_matrix進行旋轉 Tc1w.pretranslate ( t1); // 把平移向量設成t1
//Eigen::Isometry3d Twc1=Tc1w.inverse();//由world 到c1的逆變換 成 c1到world Eigen::Matrix<double, 3, 1> pw=Tc1w.inverse()*p1; //將c1座標系下的點p1變換到world座標系下
Eigen::Matrix3d q2rotation_matrix = Eigen::Matrix3d::Identity();//單位陣 q2rotation_matrix=q2.toRotationMatrix(); Eigen::Isometry3d Tc2w=Eigen::Isometry3d::Identity();// 雖然稱為3d，實質上是4＊4的矩陣 齊次座標 Tc2w.rotate (q2rotation_matrix ); // 按照q1rotation_matrix進行旋轉 Tc2w.pretranslate ( t2); // 把平移向量設成t1
Eigen::Matrix<double, 3, 1> p2=Tc2w*pw; //將world座標系下的點pw變換到c2座標系下 cout<<"the loc of p1 in c1 = \n"<< p1<<endl; cout<<"the loc of p1 in world = \n"<< pw<<endl; cout<<"the loc of p1 in c2 = \n"<< p2<<endl;
return 0; }