旋转矩阵、旋转向量（轴角）、四元数、欧拉角之间相互转换的代码实现(利用Eigen实现)
1 #include <iostream> 2 #include <cmath> 3 using namespace std; 4 5 #include <Eigen/Core> 6 // Eigen 几何模块 7 #include <Eigen/Geometry> 8 9 10 int main ( int argc, char** argv ) 11 { 12 13 //旋转矩阵R 14 //3X3的旋转矩阵可以用Matrix3d，也Matrix3f 15 Eigen::Matrix3d R=Eigen::Matrix3d::Identity();//将其赋值为单位矩阵 16 // Eigen::Matrix3d R; 17 // R.setIdentity(); //也是将旋转矩阵赋值为单位矩阵，与上面效果一样 18 cout<<"R= \n"<<R<<endl; 19 20 //旋转向量v（轴角） 21 //旋转向量使用AngleAxisd(f),底层不直接是Matrix，但因为重载了运算符，运算可以当作矩阵 22 Eigen::AngleAxisd v(M_PI/4,Eigen::Vector3d(0,0,1));//沿z轴旋转了45度 23 cout << "rotation vector: Angle is: " << v.angle() * (180 / M_PI)<<endl//旋转角 24 << " Axis is: " << v.axis().transpose() << endl<<endl;//旋转轴 25 //将旋转向量转换为旋转矩阵 26 //方式一：用matrix() 27 // R=v.matrix(); 28 //方式二：用toRotationMatrix() 29 R=v.toRotationMatrix(); 30 cout<<"R=\n"<<R<<endl<<endl; 31 //将旋转向量转化为四元数q 32 Eigen::Quaterniond q = Eigen::Quaterniond(v); 33 cout<<"q=\n"<<q.coeffs()<<endl<<endl;//coeffs的顺序:(x,y,z,w) 34 cout<<"q=\n"<<q.x()<<endl<<q.y()<<endl<<q.z()<<endl<<q.w()<<endl<<endl;//四元数的另一种输出方式 35 36 37 //将旋转矩阵转化为四元数 38 q = Eigen::Quaterniond(R); 39 cout<<"q=\n"<<q.coeffs()<<endl<<endl; 40 //将旋转矩阵转化为欧拉角 41 Eigen::Vector3d euler_angles=R.eulerAngles(2,1,0); 42 cout<<"yaw(Z) pitch(Y) roll(X)=\n"<<euler_angles.transpose()<<endl<<endl; 43 44 45 //将四元数转化为旋转矩阵 46 R=Eigen::Matrix3d(q); 47 cout<<"R=\n"<<R<<endl<<endl; 48 49 //将四元数转化为旋转向量 50 v=Eigen::AngleAxisd(q); 51 cout<<v.matrix()<<endl<<endl; 52 return 0; 53 }
运行结果：R=
1 0 0
0 1 0
0 0 1
rotation vector: Angle is: 45
Axis is: 0 0 1
R=
0.707107 -0.707107 0
0.707107 0.707107 0
0 0 1
q=
0
0
0.382683
0.92388
q=
0
0
0.382683
0.92388
q=
0
0
0.382683
0.92388
yaw(Z) pitch(Y) roll(X)=
0.785398 -0 0
R=
0.707107 -0.707107 0
0.707107 0.707107 0
0 0 1
0.707107 -0.707107 0
0.707107 0.707107 0
0 0 1
