Search code examples
linear-algebraeigenrotational-matrices

ambiguity when decomposing pitch (yaxis rotation) from rotation matrix

I am given a 4x4 pose of a robot where the z axis is facing forward, x-axis is facing east (right) and the y axis is point downwards.

Now, to extract the heading of the robot, I used the following function

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); 
};

To validate, I setup the code below. When I examined the decomposed matrix angle and the angle i used to construct the rotation matrix, they don't match!

This was when I found out that there is 2 unique solutions for pitch, or that the range for pitch is -PI/2 < pitch < PI/2.

Now I have no idea the robot is facing 45 degrees or 135 degrees, is there any ways around this? 

    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;

Results

-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
Solution

  • If the second argument of of atan2 is always non-negative, you will only get angles between -pi/2 and pi/2. If you want angles between -pi and pi, you can calculate, e.g.,

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

    Btw: As @Neil said in the comments already, 'heading' is also known as 'yaw'. 'Pitch' and 'roll' are also referred to as 'attitude' and 'bank'.

    Also: It is not uncommon to have the z-axis point upwards (or downwards), and the x-axis forward, especially in areas which usually only move on a surface. And in computer vision, the z-axis points forward, while the x-axis points right. I.e., really make sure that you know what convention your data set uses!

    To learn more about Euler-Angles I recommend reading these: