在使用Eigen时，经常会遇到旋转矩阵，旋转向量，四元数，欧拉角之间的两两相互转换。这里最常见、最容易出错的是欧拉角和旋转矩阵之间的相互转换。下面就欧拉角和旋转矩阵之间的转换进行详细分析。

图1 旋转顺序Z-Y-X，正方向内 v2-9062617be078bd5e96252a4f59ad0689_r 旋

欧拉角（1）欧拉角的叫法：

欧拉角的叫法不固定，跟坐标轴的定义强相关。在图1中，假设X是车头，Y是车左方，Z是车上方，那么绕X轴旋转得到的是roll，绕Y轴旋转得到的是pitch，绕Z轴得到的是yaw。在图1中，假设Y是车头，X是车右方，Z是车上方，那么绕X轴旋转得到的是pitch，绕Y轴旋转得到的是roll，绕Z轴得到的是yaw。（2）欧拉角正负：

如果是右手系，旋转轴正方向面对观察者时，逆时针方向的旋转是正、顺时针方向的旋转是负。亦可这样描述：使用右手的大拇指指向旋转轴正方向，其他4个手指在握拳过程中的指向便是正方向。如图1中的三次旋转都是正向旋转。（3）欧拉角的范围：

这个要具体问题具体对待。假如是车体坐标系（x-前，y-左，z-上），那么roll和pitch应该定义在（-90°，+90°），yaw应该定义在（-180°，+180°）。假如是飞机坐标系，那么roll、pitch和yaw都应该定义在（-180°，+180°）。 Eigen中的默认范围roll、pitch和yaw都是（-180°，+180°）。（4）明确旋转顺序和旋转轴：

对于x,y,z三个轴的不同旋转顺序一共有(x-y-z,y-z-x,z-x-y,x-z-y,z-y-x,y-x-z)六种组合，在旋转相同的角度的情况下不同的旋转顺序得到的姿态是不一样的。比如，先绕x轴旋转alpha，再绕y轴旋转beta；先绕y轴旋转beta，再绕x轴旋转alpha。这两种顺序得到的姿态是不一样的。（5）内旋和外旋：

每次旋转是绕固定轴（一个固定参考系，比如世界坐标系）旋转，称为外旋。每次旋转是绕自身旋转之后的轴旋转，称为内旋。下图说明了内旋和外旋的区别。 v2-54aa5fb93eb991ef96e47c63fc0b807a_720w

旋转矩阵

假设绕XYZ三个轴旋转的角度分别为 [公式] [公式] [公式] ，则三次旋转的旋转矩阵计算方法如下：

[公式]

按照内旋方式，Z-Y-X旋转顺序（指先绕自身轴Z，再绕自身轴Y，最后绕自身轴X），可得旋转矩阵（内旋是右乘）：

[公式]

按照外旋方式，X-Y-Z旋转顺序（指先绕固定轴X，再绕固定轴Y，最后绕固定轴Z），可得旋转矩阵（外旋是左乘）：

[公式]

故R1=R2，具体不在此证明，记住即可。这个结论说明ZYX顺序的内旋等价于XYZ顺序的外旋。

slam十四讲中提到的常用旋转顺序是Z-Y-X，对应rpy，指的就是内旋（绕自身轴）Z-Y-X顺序。而欧拉角转换成旋转矩阵（相对于世界坐标系的旋转矩阵）通常是按外旋方式（绕固定轴），即X-Y-Z顺序，所以旋转矩阵为：

[公式]

欧拉角和旋转矩阵之间的转换程序示例下面程序分别使用Eigen和自定义函数测试欧拉角和旋转矩阵之间的转换：

include

include <Eigen/Core>

include <Eigen/Geometry>

using namespace std;

Eigen::Matrix3d eulerAnglesToRotationMatrix(Eigen::Vector3d &theta); bool isRotationMatirx(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;    // 绕X轴
double pitch_arc = pitch_deg * DEG_TO_ARC;  // 绕Y轴
double yaw_arc = yaw_deg * DEG_TO_ARC;      // 绕Z轴

cout << endl;
cout << "roll_arc = " << roll_arc << endl;
cout << "pitch_arc = " << pitch_arc << endl;
cout << "yaw_arc = " << yaw_arc << endl;

// 初始化欧拉角（rpy）,对应绕x轴，绕y轴，绕z轴的旋转角度
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;

// 使用Eigen将旋转矩阵转换为欧拉角
Eigen::Vector3d eulerAngle1 = rotation_matrix1.eulerAngles(2,1,0); // ZYX顺序，yaw,pitch,roll
cout << "roll_1 pitch_1 yaw_1 = " << eulerAngle1[2] << " " << eulerAngle1[1] 
     << " " << eulerAngle1[0] << endl << endl;

// 使用自定义函数将旋转矩阵转换为欧拉角
Eigen::Vector3d eulerAngle2 = rotationMatrixToEulerAngles(rotation_matrix1); // roll,pitch,yaw
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; // 计算旋转矩阵的X分量 R_x << 1, 0, 0, 0, cos(theta[0]), -sin(theta[0]), 0, sin(theta[0]), cos(theta[0]);

Eigen::Matrix3d R_y;    // 计算旋转矩阵的Y分量
R_y <<
        cos(theta[1]),   0, sin(theta[1]),
        0,   1,             0,
        -sin(theta[1]),  0, cos(theta[1]);

Eigen::Matrix3d R_z;    // 计算旋转矩阵的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 isRotationMatirx(Eigen::Matrix3d R) { double err=1e-6; Eigen::Matrix3d shouldIdenity; shouldIdenity=R*R.transpose(); Eigen::Matrix3d I=Eigen::Matrix3d::Identity(); return (shouldIdenity - I).norm() < err; }

Eigen::Vector3d rotationMatrixToEulerAngles(Eigen::Matrix3d &R) { assert(isRotationMatirx(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.89368

rotation matrix1 =
 -0.317274  -0.948326 0.00384548
  0.948231  -0.317177  0.0160091
-0.0139622 0.00872568   0.999864

rotation matrix2 =
 -0.317274  -0.948326 0.00384548
  0.948231  -0.317177  0.0160091
-0.0139622 0.00872568   0.999864

roll_1 pitch_1 yaw_1 = 0.00872665 0.0139626 1.89368

roll_2 pitch_2 yaw_2 = 0.00872665 0.0139626 1.89368
可以看出二者的输出结果是一样的。

Sslishy / PointcloudLib

欧拉角详细解释 #1

include

include <Eigen/Core>

include <Eigen/Geometry>