使用Geometry模块进行各种变换

旋转角度以及向量的表示：

Matrix3d rotation_matrix = Matrix3d::Identity();//初始化一个3*3的矩阵
AngleAxisd rotation_vector( M_PI/ 4, Vector3d(0, 0, 1));     //沿 Z 轴旋转 45 度  旋转角度+旋转轴 
cout.precision(3);//控制输出精度为3位
//rotation_vector.matrix()和rotation_vector.toRotationMatrix()都表示是对应的旋转矩阵
cout << "rotation matrix =\n" << rotation_vector.matrix() << endl;  
rotation_matrix = rotation_vector.toRotationMatrix();

在旋转矩阵的基础上进行进行向量的转换：

Vector3d v(1, 0, 0);
Vector3d v_rotated = rotation_vector * v;
//旋转矩阵*向量  等价于  AngleAxisd*向量，这是因为AngleAxisd重载了运算符*
cout << "(1,0,0) after rotation (by angle axis) = " << v_rotated.transpose() << endl;//使用transpose是为了将矩阵由竖向表示转为横向表示；
	v_rotated = rotation_matrix * v;//用旋转矩阵实现

将旋转矩阵转换为欧拉角,因为AngleAxisd并不能直接得到，它是通过转换矩阵而得到各种信息的

Vector3d euler_angles = rotation_matrix.eulerAngles(2, 1, 0); // ZYX顺序，即yaw(偏航)-pitch(倾斜)-roll(翻滚)顺序
	cout << "yaw pitch roll = " << euler_angles.transpose() << endl;//输出的是弧度

对欧拉角的解释：

Each of the three parameters \a a0,\a a1,\a a2 represents the respective rotation axis as an integer in {0,1,2}.

mat == AngleAxisf(ea[0], Vector3f::UnitZ())

AngleAxisf(ea[1], Vector3f::UnitX())

AngleAxisf(ea[2], Vector3f::UnitZ()); \endcode

使用欧式变换转换坐标，也是使用旋转矩阵rotation_vector进行转换：

Isometry3d T1 = Isometry3d::Identity();                // 虽然称为3d，实质上是4＊4的矩阵
T1.rotate(rotation_vector);                                   
T1.pretranslate(Vector3d(1, 3, 4));                    //进行平移
cout << "Transform matrix = \n" << T1.matrix() << endl;
// 用变换矩阵进行坐标变换
Vector3d v_transformed = T1 * v;                              // 相当于R*v+t
cout << "v tranformed = " << v_transformed.transpose() << endl;//换为行的表示打印

使用四元数进行转换：

如何使用Eigen库中的Quaterniond类来进行四元数的转换和旋转操作。首先，通过旋转向量或旋转矩阵创建一个四元数对象，然后可以使用coeffs()方法获取四元数的系数，其中w为实部，前三者为虚部。接着，可以使用重载的乘法运算符来将四元数应用于向量，得到旋转后的向量。需要注意的是，数学上是qvq^{-1}，即先将向量转换为四元数，然后进行四元数乘法，最后再将结果转换回向量。最后，代码还演示了如何使用常规向量乘法来验证四元数旋转的正确性。

Quaterniond q = Quaterniond(rotation_vector);
cout << "quaternion from rotation vector = " << q.coeffs().transpose()
    << endl;   // 请注意coeffs的顺序是(x,y,z,w),w为实部，前三者为虚部
// 也可以把旋转矩阵赋给它
q = Quaterniond(rotation_matrix);
cout << "quaternion from rotation matrix = " << q.coeffs().transpose() << endl;
// 使用四元数旋转一个向量，使用重载的乘法即可
v_rotated = q * v; // 注意数学上是qvq^{-1}
cout << "(1,0,0) after rotation = " << v_rotated.transpose() << endl;
// 用常规向量乘法表示，则应该如下计算
cout << "should be equal to " << (q * Quaterniond(0, 1, 0, 0) * q.inverse()).coeffs().transpose() << endl;

这个问题涉及到四元数的乘法运算和旋转的本质。在四元数表示的旋转中，旋转矩阵可以通过四元数表示为 $qRq^{-1}$ 的形式，其中q是旋转对应的四元数，R是旋转矩阵， $q^{-1}$ 是q的逆四元数。这个公式的意义是，先将向量转换为四元数，然后将四元数q和四元数v相乘，得到旋转后的四元数，最后再将结果乘以q的逆四元数，得到旋转后的向量。

为什么要使用逆四元数呢？因为四元数乘法不满足交换律，即q1q2≠q2q1，但是逆四元数满足 $q^{-1}q=qq^{-1}=1$ ，其中1是四元数乘法的单位元。因此，使用逆四元数可以保证旋转的正确性。

具体来说，旋转矩阵对应的四元数可以通过Quaterniond类的fromRotationMatrix方法创建，向量可以通过Quaterniond类的fromVector3d方法创建。然后，可以使用重载的乘法运算符将四元数应用于向量进行旋转，得到旋转后的向量。

使用Geometry模块进行各种变换

猜你喜欢