Rotation matrix to euler angles with opencv

I am working on a project wich involves Aruco markers and opencv. I am quite far in the project progress. I can read the rotation vectors and convert them to a rodrigues matrix using rodrigues() from opencv.

This is a example of a rodrigues matrix I get:

[0,1,0;

1,0,0;

0,0,-1]

I use the following code.

Mat m33(3, 3, CV_64F);
Mat measured_eulers(3, 1, CV_64F);
Rodrigues(rotationVectors, m33);

measured_eulers = rot2euler(m33);

Degree_euler = measured_eulers * 180 / CV_PI;

I use the predefined rot2euler to convert from rodrigues matrix to euler angles. And I convert the received radians to degrees.

rot2euler looks like the following.

Mat rot2euler(const Mat & rotationMatrix)
{
    Mat euler(3, 1, CV_64F);

    double m00 = rotationMatrix.at<double>(0, 0);
    double m02 = rotationMatrix.at<double>(0, 2);
    double m10 = rotationMatrix.at<double>(1, 0);
    double m11 = rotationMatrix.at<double>(1, 1);
    double m12 = rotationMatrix.at<double>(1, 2);
    double m20 = rotationMatrix.at<double>(2, 0);
    double m22 = rotationMatrix.at<double>(2, 2);

    double x, y, z;

    // Assuming the angles are in radians.
    if (m10 > 0.998) { // singularity at north pole
        x = 0;
        y = CV_PI / 2;
        z = atan2(m02, m22);
    }
    else if (m10 < -0.998) { // singularity at south pole
        x = 0;
        y = -CV_PI / 2;
        z = atan2(m02, m22);
    }
    else
    {
        x = atan2(-m12, m11);
        y = asin(m10);
        z = atan2(-m20, m00);
    }

    euler.at<double>(0) = x;
    euler.at<double>(1) = y;
    euler.at<double>(2) = z;

    return euler;
}

If I use the rodrigues matrix I give as an example I get the following euler angles.

[0; 90; -180]

But I am suppose to get the following.

[-180; 0; 90]

When is use this tool http://danceswithcode.net/engineeringnotes/rotations_in_3d/demo3D/rotations_in_3d_tool.html

You can see that [0; 90; -180] doesn't match the rodrigues matrix but [-180; 0; 90] does. (I am aware of the fact that the tool works with ZYX coordinates)

So the problem is I get the correct values but in a wrong order.

Another problem is that this isn't always the case. For example rodrigues matrix:

[1,0,0;

0,-1,0;

0,0,-1]

Provides me the correct euler angles.

If someone knows a solution to the problem or can provide me with a explanation how the rot2euler function works exactly. It will be higly appreciated.

Kind Regards

Brent Convens

Solution 1:

I guess I am quite late but I'll answer it nonetheless. Dont quote me on this, ie I'm not 100 % certain but this is one of the files ( {OPENCV_INSTALLATION_DIR}/apps/interactive-calibration/rotationConverters.cpp ) from the source code of openCV 3.3

It seems to me that openCV is giving you Y-Z-X ( similar to what is being shown in the code above )

Why I said I wasn't sure because I just looked at the source code of cv::Rodrigues and it doesnt seem to call this piece of code that I have shown above. The Rodrigues function has the math harcoded into it ( and I think it can be checked by Taking the 2 rotation matrices and multiplying them as - R = Ry * Rz * Rx and then looking at the place in the code where there is a acos(R(2,0)) or asin(R(0,2) or something similar,since one of the elements of "R" will usually be a cos() or sine which will give you a solution as to which angle is being found.

Solution 2:

Not specific to OpenCV, but you could write something like this:

cosine_for_pitch = math.sqrt(pose_mat[0][0] ** 2 + pose_mat[1][0] ** 2)
is_singular = cosine_for_pitch < 10**-6
if not is_singular:
    yaw = math.atan2(pose_mat[1][0], pose_mat[0][0])
    pitch = math.atan2(-pose_mat[2][0], cosine_for_pitch)
    roll = math.atan2(pose_mat[2][1], pose_mat[2][2])
else:
    yaw = math.atan2(-pose_mat[1][2], pose_mat[1][1])
    pitch = math.atan2(-pose_mat[2][0], cosine_for_pitch)
    roll = 0

Here, you could explore more:

https://www.learnopencv.com/rotation-matrix-to-euler-angles/ http://www.staff.city.ac.uk/~sbbh653/publications/euler.pdf