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
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(); 使用Eigen实现四元数、欧拉角、旋转矩阵、旋转向量之间的转换_一抹烟霞的博客-CSDN博客_eigen欧拉角转旋转矩阵
T.rotate(rotation_vector1);
T.pretranslate(t);
cout<<"齐次欧式变换：\n"<