Eigen一些矩阵转换

由旋转矩阵和平移得到齐次变换矩阵

1）

Eigen::Matrix3d R;
// Find your Rotation Matrix
Eigen::Vector3d T;
// Find your translation Vector
Eigen::Matrix4d Trans; // Your Transformation Matrix
Trans.setIdentity();   // Set to Identity to make bottom row of Matrix 0,0,0,1
Trans.block<3,3>(0,0) = R;
Trans.block<3,1>(0,3) = T;

2）

    // 欧式变换矩阵
    printf(">>> 旋转向量+平移向量=欧式变换矩阵");

  Eigen::Matrix3d rotation_mat;

    Eigen::Vector3d translation(1, 2, 3);
    Eigen::Isometry3d T = Eigen::Isometry3d::Identity();    // 虽然称为3d，实质上是4＊4的矩阵 齐次坐标
    T.rotate (rotation_mat);                                // 按照rotation_vec进行旋转
    T.pretranslate (translation);     // 把平移向量设成(1,2,3)
    cout << "欧式变换矩阵=\n" << T.matrix() << endl << endl;

2

四元数初始化

四元数初始化有多种形式，容易出错的是：由于Eigen中四元数存储顺序（从打印可知）是xyzw，导致赋值出现的错误：

方式一：4个标量

Quaterniond q1(1, 2, 3, 4);                               // 第一种方式  实部为1 ，虚部234

方式二：Vector4d

Quaterniond q2(Vector4d(1, 2, 3, 4));             // 第二种方式  实部为4 ，虚部123

• Eigen::Quaterniond q1(w, x, y, z);// 第一种方式

• Eigen::Quaterniond q2(Vector4d(x, y, z, w));// 第二种方式

• 以上两种方式是不同的，在Quaternion内部的保存中，虚部在前，实部在后，

  如果以第一种方式构造四元数，则实部是w， 虚部是[x,y,z]；

  对于第二种方式，则实部是w，虚部是[x,y,z]；

3

  //RotationMatrix to EulerAngles  

•     ::Eigen::Vector3d ea1 = Rx.eulerAngles(2,1,0);       

4

//Quaterniond to RotationMatrix  

•     ::Eigen::Matrix3d Rx = q.toRotationMatrix();  

5
将source_cloud经过transform_matrix转换矩阵得到新的点云filtered_pc

pcl::PointCloud::Ptr filtered_pc(new pcl::PointCloud）
pcl::tranformPointCloud(*source_cloud,*filtered_pc,transform_matrix);
 

6

四元数
4.1 初始化四元数

Eigen::Quaterniond quaternion(w,x,y,z);

四元数转旋转向量

Eigen::AngleAxisd rotation_vector(quaternion);

四元数转旋转矩阵

Eigen::Matrix3d rotation_matrix;
rotation_matrix=quaternion.matrix();

Eigen::Matrix3d rotation_matrix;
rotation_matrix=quaternion.toRotationMatrix();

四元数转欧拉角(xyz，即RPY)

Eigen::Vector3d eulerAngle=quaternion.matrix().eulerAngles(0,1,2);

齐次欧式变换

Isometry3d T=Isometry3d::Identity();
T.rotate(rotation_vector1);
T.pretranslate(t);
cout<<"齐次欧式变换：\n"<

使用Eigen实现四元数、欧拉角、旋转矩阵、旋转向量之间的转换_一抹烟霞的博客-CSDN博客_eigen欧拉角转旋转矩阵