Eigen實現尤拉角、四元數和旋轉矩陣之間的變換
阿新 • • 發佈:2018-12-13
include相應的標頭檔案
#include <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() 轉換成矩陣 // 用 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 即可
四元數
q=[cos(θ/2),nxsin(θ/2),nysin(θ/2),nzsin(θ/2)]
// 可以直接把 AngleAxis 賦值給四元數,反之亦然 Eigen::Quaterniond q = Eigen::Quaterniond ( rotation_vector ); cout<<"quaternion = \n"<<q.coeffs() <<endl; // 請注意 coeffs 的順序是 (x,y,z,w), w 為實部,前三者為虛 部 // 使用四元數旋轉一個向量,使用過載的乘法即可 v_rotated = q*v; // 注意數學上是 qvq^{-1} cout<<"(1,0,0) after rotation = "<<v_rotated.transpose()<<endl;
想進一步瞭解 Eigen 的幾何模組可以參考(http://eigen.
tuxfamily.org/dox/group__TutorialGeometry.html)