从旋转矩阵分解音高（轴旋转）时的歧义

Question

我得到一个机器人的 4x4 姿势，其中 z 轴朝前，x 轴朝东（右），y 轴指向下方。

现在，为了提取机器人的航向，我使用了以下函数

void inline mat2xyh(Matrix4f& pose, float &x, float &y, float &heading){
    heading = atan2(-pose(2, 0),  sqrt(pose(2, 1) * pose(2, 1) +  pose(2,2) * pose(2,2)) );
    x = pose(0, 3);
    y = pose(2, 3); 
};

为了验证，我设置了下面的代码。当我检查分解矩阵角度和我用来构造旋转矩阵的角度时，它们不匹配！

这是我发现音高有2个唯一解，或者音高的范围是-PI/2 < pitch < PI/2的时候。

现在我不知道机器人是面向 45 度还是 135 度，有什么办法解决这个问题吗？

    Vector4f v(0, 0, 1, 0);

    MatrixXf rotation = AngleAxisf(135 * M_PI/180., Vector3f::UnitY()).toRotationMatrix();
    float x,y,h;
    Matrix4f pose1 = Matrix4f::Identity();
    pose1.topLeftCorner<3,3>() = rotation;

    mat2xyh(pose1, x,y,h);

    cout << pose1 << endl;
    cout << rad2deg(h) << endl;
    cout << "Pose: \n" << pose1 * v << endl;

    cout << "====================" << endl;

    MatrixXf rotation2 = AngleAxisf(45 * M_PI/180., Vector3f::UnitY()).toRotationMatrix();
    Matrix4f pose2 = Matrix4f::Identity();
    pose2.topLeftCorner<3,3>() = rotation2;

    mat2xyh(pose2, x,y,h);

    cout << pose2 << endl;
    cout << rad2deg(h) << endl;
    cout << pose2 * v << endl;

结果

-0.707107         0  0.707107         0
        0         1         0         0
-0.707107         0 -0.707107         0
        0         0         0         1
heading: 45
Pose: 
 0.707107
        0
-0.707107
        0
====================
 0.707107         0  0.707107         0
        0         1         0         0
-0.707107         0  0.707107         0
        0         0         0         1
heading:45
Pose: 
0.707107
       0
0.707107
       0

Answer 1

如果 atan2 的第二个参数始终为非负数，您将只能得到 -pi/2 和 pi/2 之间的角度。如果你想要 -pi 和 pi 之间的角度，你可以计算，例如

heading = atan2(pose(0, 2), pose(2,2));

顺便说一句：正如@Neil 在评论中所说，'heading' 也称为 'yaw'。 'Pitch' 和 'roll' 也称为 'attitude' 和 'bank'。

另外：z 轴指向上方（或下方）和 x 轴指向前方的情况并不少见，尤其是在通常仅在表面上移动的区域中。在计算机视觉中，z 轴指向前方，而 x 轴指向右侧。即，确实确保您知道数据集使用的约定！

要了解有关欧拉角的更多信息，我建议阅读以下内容：

从旋转矩阵分解音高（轴旋转）时的歧义

ambiguity when decomposing pitch (yaxis rotation) from rotation matrix

linear-algebra

eigen

rotational-matrices