C++与Python从旋转矩阵计算欧拉角结果不一致问题咨询

问题背景 #

我有一段Python代码，用于将3x3旋转矩阵转换为Z-Y-X顺序的欧拉角（yaw, pitch, roll）：

from scipy.spatial.transform import Rotation   

r = Rotation.from_matrix(R)
angles = r.as_euler("zyx", degrees=True)
print(f'angles = {angles}')

其中R是3x3旋转矩阵。

对应的C++代码预期实现相同功能，但结果不一致：

Eigen::Map<Eigen::Matrix<float, 3, 3, Eigen::RowMajor>> eigen_R(R.ptr<float>());

Eigen::Quaternionf q(eigen_R);
Eigen::Vector3f euler = q.toRotationMatrix().eulerAngles(0, 1, 2);

double angle_x = euler[0];
double angle_y = euler[1];
double angle_z = euler[2];

// Convert angles to degrees
//double deg_factor = 180.0 / M_PI;
double deg_factor = 1.0;

float half_circle = M_PI * deg_factor;
float yaw_dim = angle_y > 0 ? half_circle : -half_circle;

_headPose[0] = (angle_y * deg_factor);      // yaw
_headPose[1] = (angle_x * deg_factor);      // pitch
_headPose[2] = (angle_z * deg_factor);      // roll

测试现象 #

• 当使用旋转矩阵R1时，两段代码结果一致：

  R1 = [[ 0.99640366 -0.05234712  0.06662979]
        [ 0.05348801  0.99844889 -0.01545452]
        [-0.06571744  0.01896283  0.99765807]]
  

  结果：

  Yaw = 3.82043, Pitch = 0.887488, Roll = 3.00733
  

• 当使用旋转矩阵R2时，结果出现巨大差异：

  R2 = [[ 0.99321075 -0.08705603  0.07715995]
        [ 0.08656997  0.99619924  0.00962846]
        [-0.0777049  -0.00288336  0.99697223]]
  

  Python输出：

  yaw = 4.425338029132474, pitch = -0.5533284872549677, roll = 5.0092369943283375
  

  C++输出：

  Yaw = 175.575, Pitch = 179.447, Roll = -174.991
  

实际场景中，当角度从(0,0,0)细微变化时，C++计算结果会莫名反向，临时调整代码可得到正确结果，但无法理解背后原因。

问题根源 #

1. 旋转顺序不匹配：

   • Python代码使用extrinsic Z-Y-X顺序（先绕固定Z轴转yaw，再绕固定Y轴转pitch，最后绕固定X轴转roll），旋转矩阵为 R = R_x(roll) * R_y(pitch) * R_z(yaw)。
   • C++代码使用Eigen的eulerAngles(0,1,2)，对应intrinsic X-Y-Z顺序（先绕自身X轴转，再绕自身Y轴转，最后绕自身Z轴转），旋转矩阵为 R = R_z(roll) * R_y(pitch) * R_x(yaw)。两者的旋转矩阵乘法顺序相反，导致角度结果完全不同。

2. 角度范围处理逻辑不同：

   • Scipy会选择最短路径的欧拉角解，强制pitch（Y轴）范围在[-90°, 90°]，yaw和roll范围在[-180°, 180°]。
   • Eigen默认的欧拉角范围是[0°, 360°]或[0°, 180°]，当旋转接近奇异点（pitch接近±90°）时，会切换到另一个等价解，导致角度跳变到180°附近，出现"反向"现象。

修复方案 #

直接按照Scipy的Z-Y-X extrinsic欧拉角计算逻辑实现C++代码，确保旋转顺序和角度范围完全匹配：

// 假设R是3x3旋转矩阵，存储为RowMajor的float类型
Eigen::Map<Eigen::Matrix<float, 3, 3, Eigen::RowMajor>> eigen_R(R.ptr<float>());

float pitch = asinf(-eigen_R(2, 0)); // Y轴旋转角，范围[-π/2, π/2]
float yaw, roll;

const float cos_pitch = cosf(pitch);
if (fabs(cos_pitch) > 1e-6) { // 非奇异情况
    yaw = atan2f(eigen_R(1, 0), eigen_R(0, 0));   // Z轴旋转角，范围[-π, π]
    roll = atan2f(eigen_R(2, 1), eigen_R(2, 2));  // X轴旋转角，范围[-π, π]
} else { // 奇异情况（pitch接近±90°），固定yaw为0，计算roll
    yaw = 0.0f;
    roll = atan2f(-eigen_R(0, 1), eigen_R(1, 1));
}

// 转换为角度
const float deg_factor = 180.0f / M_PI;
_headPose[0] = yaw * deg_factor;    // yaw
_headPose[1] = pitch * deg_factor;  // pitch
_headPose[2] = roll * deg_factor;   // roll

说明 #

• 该代码直接从旋转矩阵计算欧拉角，完全对齐Scipy的as_euler("zyx", degrees=True)逻辑。
• atan2f返回的角度范围是[-π, π]，转换为角度后自然落在[-180°, 180°]，无需额外调整。
• 奇异情况的处理和Scipy保持一致，避免出现角度跳变。

内容的提问来源于stack exchange，提问作者Eugene Alexeev