在Eigen中，旋转矩阵、变换矩阵、欧拉角都可以表示为Eigen库中的特定类型的矩阵。
1、旋转矩阵
旋转矩阵通常用于表示三维空间中的旋转操作。在Eigen中，可以使用Eigen::Matrix3d类型来表示三维的旋转矩阵。通常，旋转矩阵是一个正交矩阵，其转置等于逆，因此可以使用Eigen中的Rotation3D模块来创建和操作旋转矩阵。
例如，创建一个绕z轴旋转45度的旋转矩阵可以这样做：
#include <Eigen/Dense>int main() {Eigen::Matrix3d rotation_matrix;double angle = M_PI / 4; // 45度转弧度rotation_matrix = Eigen::AngleAxisd(angle, Eigen::Vector3d::UnitZ());return 0;
}
2、变换矩阵
变换矩阵用于表示在欧几里得空间中的平移、旋转和缩放操作。在三维情况下，通常使用4x4的变换矩阵来表示这些变换。Eigen中可以使用Eigen::Transform模板来创建和操作这样的变换矩阵。
例如，创建一个绕z轴旋转45度并沿x轴平移(1, 2, 3)的变换矩阵可以这样做：
#include <Eigen/Dense>int main() {Eigen::Affine3d transformation_matrix = Eigen::Affine3d::Identity();double angle = M_PI / 4; // 45度转弧度transformation_matrix.rotate(Eigen::AngleAxisd(angle, Eigen::Vector3d::UnitZ()));transformation_matrix.translation() << 1, 2, 3;return 0;
}
3、欧拉角转旋转矩阵（zyx）
// 使用eigen库，欧拉角转旋转矩阵Eigen::Matrix3d rotation_matrix1, rotation_matrix2;rotation_matrix1 =Eigen::AngleAxisd(euler_angle[2], Eigen::Vector3d::UnitZ()) *Eigen::AngleAxisd(euler_angle[1], Eigen::Vector3d::UnitY()) *Eigen::AngleAxisd(euler_angle[0], Eigen::Vector3d::UnitX());cout << "\nrotation matrix1 =\n" << rotation_matrix1 << endl << endl;
4、旋转矩阵转欧拉角
// 使用egigen将旋转矩阵转换为欧拉角
Eigen::Vector3d eulerAngle1 = rotation_matrix1.eulerAngles(2, 1, 0); //zyx顺序
cout << "roll_2 pitch_2 yaw_2 = " << eulerAngle1[2] << " " << eulerAngle1[1]<< " " << eulerAngle1[0] << endl << endl;
5、实战案例
#include<iostream>
#include<Eigen/Core>
#include<Eigen/Geometry>using namespace std;Eigen::Matrix3d eulerAnglesToRotationMatrix(Eigen::Vector3d& theta);
bool isRotationMatrix(Eigen::Matrix3d R);
Eigen::Vector3d rotationMatrixToEulerAngles(Eigen::Matrix3d& R);const double ARC_TO_DEG = 57.29577951308238;
const double DEG_TO_ARC = 0.0174532925199433;int main()
{// 设定车体欧拉角，绕固定轴double roll_deg = 0.5; // 绕x轴double pitch_deg = 0.8; // 绕y轴double yaw_deg = 108.5; // 绕z轴// 转换为弧度double roll_arc = roll_deg * DEG_TO_ARC;double pitch_arc = pitch_deg * DEG_TO_ARC; double yaw_arc = yaw_deg * DEG_TO_ARC; cout << endl;cout << "roll_arc = " << roll_arc << endl;cout << "pitch_arc = " << pitch_arc << endl;cout << "yaw_arc = " << yaw_arc << endl;// 初始化欧拉角Eigen::Vector3d euler_angle(roll_arc, pitch_arc, yaw_arc);// 使用eigen库，欧拉角转旋转矩阵Eigen::Matrix3d rotation_matrix1, rotation_matrix2;rotation_matrix1 =Eigen::AngleAxisd(euler_angle[2], Eigen::Vector3d::UnitZ()) *Eigen::AngleAxisd(euler_angle[1], Eigen::Vector3d::UnitY()) *Eigen::AngleAxisd(euler_angle[0], Eigen::Vector3d::UnitX());cout << "\nrotation matrix1 =\n" << rotation_matrix1 << endl << endl;// 使用自定义函数，欧拉角转旋转矩阵rotation_matrix2 = eulerAnglesToRotationMatrix(euler_angle);cout << "rotation matrix2 = \n" << rotation_matrix2 << endl << endl;// 使用egigen将旋转矩阵转换为欧拉角Eigen::Vector3d eulerAngle1 = rotation_matrix1.eulerAngles(2, 1, 0); //zyx顺序cout << "roll_2 pitch_2 yaw_2 = " << eulerAngle1[2] << " " << eulerAngle1[1]<< " " << eulerAngle1[0] << endl << endl;// 使用自定义函数将旋转矩阵转换为欧拉角Eigen::Vector3d eulerAngle2 = rotationMatrixToEulerAngles(rotation_matrix1);cout << "roll_2 pitch_2 yaw_2 = " << eulerAngle2[0] << " " << eulerAngle2[1]<< " " << eulerAngle2[2] << endl << endl;return 0;
}Eigen::Matrix3d eulerAnglesToRotationMatrix(Eigen::Vector3d& theta)
{Eigen::Matrix3d R_x;R_x <<1, 0, 0,0, cos(theta[0]), -sin(theta[0]),0, sin(theta[0]), cos(theta[0]);Eigen::Matrix3d R_y;R_y <<cos(theta[1]), 0, sin(theta[1]),0, 1, 0,-sin(theta[1]), 0, cos(theta[1]);Eigen::Matrix3d R_z;R_z <<cos(theta[2]), -sin(theta[2]), 0,sin(theta[2]), cos(theta[2]), 0,0, 0, 1;Eigen::Matrix3d R = R_z * R_y * R_x;return R;
}bool isRotationMatrix(Eigen::Matrix3d R)
{double err = 1e-6;Eigen::Matrix3d shouldIdentity;shouldIdentity = R * R.transpose();Eigen::Matrix3d I = Eigen::Matrix3d::Identity();return (shouldIdentity - I).norm() < err;
}Eigen::Vector3d rotationMatrixToEulerAngles(Eigen::Matrix3d& R)
{assert(isRotationMatrix(R));double sy = sqrt(R(0, 0) * R(0, 0) + R(1, 0) * R(1, 0));bool singular = sy < 1e-6;double x, y, z;if (!singular) {x = atan2(R(2, 1), R(2, 2));y = atan2(-R(2, 0), sy);z = atan2(R(1, 0), R(0, 0));}else {x = atan2(-R(1, 2), R(1, 1));y = atan2(-R(2, 0), sy);z = 0;}return { x,y,z };
}
结果
roll_arc = 0.00872665
pitch_arc = 0.0139626
yaw_arc = 1.89368rotation matrix1 =-0.317274 -0.948326 0.003845480.948231 -0.317177 0.0160091
-0.0139622 0.00872568 0.999864rotation matrix2 =-0.317274 -0.948326 0.003845480.948231 -0.317177 0.0160091
-0.0139622 0.00872568 0.999864roll_2 pitch_2 yaw_2 = 0.00872665 0.0139626 1.89368roll_2 pitch_2 yaw_2 = 0.00872665 0.0139626 1.89368
