C++中如何从旋转矩阵恢复构造用的最小XYZ欧拉角

我有一组由小角度XYZ欧拉角构造的3D旋转矩阵，所有原始角度范围为 -20° < angle < 20°，旋转矩阵构造方式如下：

我需要从该旋转矩阵反向求解原始欧拉角，但欧拉角存在多组可行解，目标是在C++中恢复出可构造该旋转矩阵的最小幅值角度。

目前我在C++中使用Eigen::eulerAngles()接口求解角度，但该接口返回的角度范围为 x∈[0:pi] y∈[-pi:pi] z∈[-pi:pi]，无法返回负的X轴角度：例如原始X角为 -0.2 弧度时，接口返回的X角为 2.94159 弧度，不符合小角度预期。

测试代码 #

void eigen_test(float x, float y, float z) {
  auto mat = AngleAxisf(x, Vector3f::UnitX())
    * AngleAxisf(y, Vector3f::UnitY())
    * AngleAxisf(z, Vector3f::UnitZ());

  auto angle = mat.toRotationMatrix().eulerAngles(0,1,2);

  cout << "(" <<  x << ", " << y << ", " << z << ") 
-> (" << angle[2] << ", " << angle[1] << ", " << angle[0] << ")" << endl;

}

int main() {
  std::vector<float> num = { -0.2,0.,0.2 };
  for (auto x : num) {
    for (auto y : num) {
      for (auto z : num) {
        eigen_test(x, y, z);
      }
    }
  }

  return 0;
}

完整测试结果 #

X    , Y   , Z     -> X'   , Y'   , Z'
__________________________________________________
(-0.2, -0.2, -0.2) -> (2.94159, -2.94159, 2.94159)
(-0.2, -0.2, 0   ) -> (2.94159, -2.94159, -3.14159)
(-0.2, -0.2, 0.2 ) -> (2.94159, -2.94159, -2.94159)
(-0.2, 0,    -0.2) -> (2.94159, -3.14159, 2.94159)
(-0.2, 0,    0   ) -> (2.94159, 3.14159, -3.14159)
(-0.2, 0,    0.2 ) -> (2.94159, 3.14159, -2.94159)
(-0.2, 0.2,  -0.2) -> (2.94159, 2.94159, 2.94159)
(-0.2, 0.2,  0   ) -> (2.94159, 2.94159, 3.14159)
(-0.2, 0.2,  0.2 ) -> (2.94159, 2.94159, -2.94159)
(0,   -0.2,  -0.2) -> (0, -0.2, -0.2)
(0,   -0.2,  0   ) -> (0, -0.2, 0)
(0,   -0.2,  0.2 ) -> (3.14159, -2.94159, -2.94159)
(0,   0,     -0.2) -> (0, 0, -0.2)
(0,   0,     0   ) -> (-0, 0, -0)
(0,   0,     0.2 ) -> (-0, 0, 0.2)
(0,   0.2,   -0.2) -> (3.14159, 2.94159, 2.94159)
(0,   0.2,   0   ) -> (-0, 0.2, 0)
(0,   0.2,   0.2 ) -> (0, 0.2, 0.2)
(0.2, -0.2,  -0.2) -> (0.2, -0.2, -0.2)
(0.2, -0.2,  0   ) -> (0.2, -0.2, -0)
(0.2, -0.2,  0.2 ) -> (0.2, -0.2, 0.2)
(0.2, 0,     -0.2) -> (0.2, 0, -0.2)
(0.2, 0,     0   ) -> (0.2, 0, 0)
(0.2, 0,     0.2 ) -> (0.2, 0, 0.2)
(0.2, 0.2,   -0.2) -> (0.2, 0.2, -0.2)
(0.2, 0.2,   0   ) -> (0.2, 0.2, -0)
(0.2, 0.2,   0.2 ) -> (0.2, 0.2, 0.2)

解决方法 #

XYZ顺序欧拉角本身存在两组数学等价解，Eigen接口默认返回X角落在[0, pi]区间的那组，不符合小角度场景需求。由于已知所有原始角度都在±20°区间，不存在万向锁问题，只需要生成两组等价解，选择角度幅值和最小的一组即可。

两组等价解的转换规则：

• 第一组为Eigen直接返回的解：(x1, y1, z1)
• 第二组等价解为：(x1 - π, π - y1, z1 - π)，所有角度需要规整到[-π, π]区间

可直接使用的实现代码 #

#include <cmath>
#include <Eigen/Core>
#include <Eigen/Geometry>

// 将角度规整到[-pi, pi]区间
inline float wrap_pi(float angle) {
    angle = fmodf(angle + M_PI, 2 * M_PI);
    if (angle < 0) angle += 2 * M_PI;
    return angle - M_PI;
}

// 从旋转矩阵提取最小幅值的XYZ顺序欧拉角
Eigen::Vector3f extract_small_xyz_euler(const Eigen::Matrix3f& rot) {
    Eigen::Vector3f ang = rot.eulerAngles(0, 1, 2);
    float x1 = ang.x(), y1 = ang.y(), z1 = ang.z();
    // 生成第二组等价解
    float x2 = wrap_pi(x1 - M_PI);
    float y2 = wrap_pi(M_PI - y1);
    float z2 = wrap_pi(z1 - M_PI);
    // 比较两组解的角度绝对值和，返回更小的那组
    float cost1 = fabsf(x1) + fabsf(y1) + fabsf(z1);
    float cost2 = fabsf(x2) + fabsf(y2) + fabsf(z2);
    return cost2 < cost1 ? Eigen::Vector3f(x2, y2, z2) : ang;
}

注意：测试代码中打印角度的顺序有误，eulerAngles(0,1,2)返回的向量顺序已经是[X轴角度, Y轴角度, Z轴角度]，不需要倒序取angle[2], angle[1], angle[0]。

使用上述函数替换原生eulerAngles调用后，所有测试用例都能正确恢复出原始的-0.2、0、0.2小角度结果，不会返回接近π的大角度值。

内容的提问来源于stack exchange，提问作者Maxime Charrière