从旋转矩阵分解螺距(Y轴旋转)时的歧义

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评论中已经说过的那样，"标题"也被称为"偏航"。"投球"和"滚动"也称为"态度"和"银行"。

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

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

• https://en.wikipedia.org/wiki/Euler_angles#Tait%E2%80%93Bryan_angles
• http://www.euclideanspace.com/maths/geometry/rotations/euler/index.htm