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 }