小试旋转向量、欧拉角、四元数之间的变换

 1     Matrix3d R_mat = Matrix3d::Identity();//初始化为单位阵
 2     AngleAxisd R_vec(M_PI/4,Vector3d(0,0,1));//用旋转角和旋转轴表示的旋转向量
 3     cout.precision(3);//输出精确到三位小数
 4     R_mat = R_vec.toRotationMatrix();//旋转向量转化为旋转矩阵
 5     cout << "旋转矩阵1：\n"<<R_vec.matrix() << endl //旋转向量转化为旋转矩阵
 6          << "旋转矩阵1：\n" << R_mat << endl;
 7     
 8     Vector3d v(1, 0, 0);
 9     Vector3d v_r = R_vec * v;//用旋转轴和角表示的坐标变换
10     cout << "旋转向量2：\n" << v_r.transpose() << endl;//transpose()为转置
11     v_r = R_mat * v;//用旋转矩阵表示的坐标变换
12     cout << "旋转向量2：\n" << v_r.transpose() << endl;
13 
14     Vector3d eul_ang = R_mat.eulerAngles(2, 1, 0);//ZYX顺序，RPY
15     cout << "欧拉角rpy为：\n" << eul_ang << endl;//绕Z旋转π/4
16 
17     Isometry3d T = Isometry3d::Identity();//欧氏变换，4x4矩阵
18     T.rotate(R_vec);//按照R_vec旋转向量来旋转
19     T.pretranslate(Vector3d(1, 2, 3));//平移向量为（1.2.3）
20     cout << "变换矩阵为：\n" <<T.matrix() << endl;
21 
22     Vector3d v_transform = T * v;//就是通常的R*v+t,旋转和平移都包括进去了对于坐标第四个数默认补0
23     cout << "变换后的坐标为：\n" << v_transform << endl;
24 
25     Quaterniond q = Quaterniond(R_vec);//用四元数表示旋转向量
26     cout << "四元数表示的旋转向量为：\n" << q.coeffs().transpose() << endl;//(i,j,k,w),前三个为虚部，最后一个为实部
27      q = Quaterniond(R_mat);//用四元数表示旋转矩阵
28     cout << "四元数表示的旋转矩阵为：\n" << q.coeffs().transpose() << endl;//(i,j,k,w),前三个为虚部，最后一个为实部      
29     
30     v_r = q * v;//用四元数表示的旋转变换，数学中是q*v*q.inverse()
31     cout << "四元数旋转向量为：\n" << v_r.transpose() << endl;