SLAM学习：Eigen几何模块测试

 #include <iostream>

    
 #include <cmath>
    
 using namespace std;
    
  
    
 #include <Eigen/Core>
    
 // Eigen 几何模块
    
 #include <Eigen/Geometry>
    
  
    
 /**************************** * 本程序演示了 Eigen 几何模块的使用方法
    
 ****************************/
    
  
    
 int main ( int argc, char** argv )
    
 {
    
     // 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()转换成矩阵
    
    
    
    // 旋转向量转为旋转矩阵
    
     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<<"v tranformed = "<<v_transformed.transpose()<<endl;
    
  
    
     // 对于仿射和射影变换，使用 Eigen::Affine3d 和 Eigen::Projective3d 即可，略
    
  
    
     // 四元数
    
     // 可以直接把AngleAxis赋值给四元数，反之亦然
    
     Eigen::Quaterniond q = Eigen::Quaterniond ( rotation_vector );
    
     cout<<"quaternion = \n"<<q.coeffs() <<endl;   // 请注意coeffs的顺序是(x,y,z,w),w为实部，前三者为虚部
    
     // 也可以把旋转矩阵赋给它
    
     q = Eigen::Quaterniond ( rotation_matrix );
    
     cout<<"quaternion = \n"<<q.coeffs() <<endl;
    
     // 使用四元数旋转一个向量，使用重载的乘法即可
    
     v_rotated = q*v; // 注意数学上是qvq^{-1}
    
     cout<<"(1,0,0) after rotation = "<<v_rotated.transpose()<<endl;
    
  
    
     return 0;
    
 }
    
    
    
    
    AI助手

 rotation matrix =

    
     0.707    -0.707         0
    
     0.707     0.707         0
    
     0         0         1
    
 (1,0,0) after rotation =    0.707    0.707        0
    
 (1,0,0) after rotation =    0.707    0.707        0
    
 yaw pitch roll = 0.785 -0  0
    
 Transform matrix = 
    
     0.707    -0.707         0         1
    
     0.707     0.707         0         3
    
     0         0         1         4
    
     0         0         0         1
    
 v tranformed =    1.71    3.71       4
    
 quaternion = 
    
 0
    
 0
    
 0.383
    
 0.924
    
 quaternion = 
    
 0
    
 0
    
 0.383
    
 0.924
    
 (1,0,0) after rotation =    0.707    0.707        0
    
    
    
    
    AI助手