AprilTag - Obtaining rotation information as Euler angles

Question

I am trying to take AprilTag's 3x3 rotation matrix output and turn that into Euler angles.

I understand that using Euler angles can present some problems such as axes ordering and gimbal lock, etc. But my use case is very simple and that really shouldn't be a problem.

I have AprilTag successfully detecting tags, and estimating a pose like so:

    apriltag_detection_t* det = [obtained externally]
    apriltag_detection_info_t info;
    info.det = det;
    info.tagsize = [a constant];
    info.fx = [a constant];
    info.fy = [a constant];
    info.cx = [a constant];
    info.cy = [a constant];

    apriltag_pose_t pose;
    estimate_tag_pose(&info, &pose);

I can pull the translational data off that pose without any issue, like so. (And it seems to be quite accurate, about 1/3" error at 18" away)

    double translation_on_axes_1 = pose.t->data[0];
    double translation_on_axes_2 = pose.t->data[1];
    double translation_on_axes_3 = pose.t->data[3];

I can access the estimated 3x3 rotation matrix (of type matd_t) at pose.R. The problem is, I cannot figure out how to take that 3x3 matrix and turn it into "normal" euler angles. I've tried 3 different methods for converting a 3x3 matrix to euler angles that I found online, and none work.

First method I found:

void rotationMatrixToEulerAngles(matd_t* matrix, double out[])
{
    double sy = sqrt(MATD_EL(matrix, 0, 0) * MATD_EL(matrix, 0, 0) +  MATD_EL(matrix, 1, 0) * MATD_EL(matrix, 1, 0) );

    bool singular = sy < 1e-6; // If

    double x, y, z;
    if (!singular)
    {
        x = atan2(MATD_EL(matrix, 2, 1) , MATD_EL(matrix, 2, 2));
        y = atan2(-MATD_EL(matrix, 2, 0), sy);
        z = atan2(MATD_EL(matrix, 1, 0), MATD_EL(matrix, 0, 0));
    }
    else
    {
        x = atan2(-MATD_EL(matrix, 1, 2), MATD_EL(matrix, 1, 1));
        y = atan2(-MATD_EL(matrix, 2, 0), sy);
        z = 0;
    }

    out[0] = x;
    out[1] = y;
    out[2] = z;
}

Second method I found:

void rotationMatrixToEulerAngles2(matd_t* matrix, double out[])
{
    double y_rot = asin(MATD_EL(matrix, 2, 0));
    double x_rot = acos(MATD_EL(matrix, 2, 2)/cos(y_rot));
    double z_rot = acos(MATD_EL(matrix, 0, 0)/cos(y_rot));

    double y_rot_angle = y_rot * (180.0/M_PI);
    double x_rot_angle = x_rot * (180.0/M_PI);
    double z_rot_angle = z_rot * (180.0/M_PI);

    out[0] = y_rot_angle;
    out[1] = x_rot_angle;
    out[2] = z_rot_angle;
}

Third method I found:

void rotationMatrixToEulerAngles3(matd_t* matrix, double out[])
{
    double m00 = MATD_EL(matrix, 0, 0);
    double m02 = MATD_EL(matrix, 0, 2);
    double m10 = MATD_EL(matrix, 1, 0);
    double m11 = MATD_EL(matrix, 1, 1);
    double m12 = MATD_EL(matrix, 1, 2);
    double m20 = MATD_EL(matrix, 2, 0);
    double m22 = MATD_EL(matrix, 2, 2);

    double x, y, z;

    if (m10 > 0.998) {
        x = 0;
        y = CV_PI/2;
        z = atan2(m02,m22);
    }
    else if (m10 < -0.998) {
        x = 0;
        y = -CV_PI/2;
        z = atan2(m02,m22);
    }
    else
    {
        x = atan2(-m12,m11);
        y = asin(m10);
        z = atan2(-m20,m00);
    }

    out[0] = x;
    out[1] = y;
    out[2] = z;
}

All of those methods returned totally garbage data. I then attempted to use OpenCV's decomposeProjectionMatrix function as described here.

I fed it the data from AprilTag like so:

    double eulerAngles[3];

    double projMatrix[12] = {MATD_EL(pose.R, 0, 0),MATD_EL(pose.R, 0, 1),MATD_EL(pose.R, 0, 2),pose.t->data[0],
                             MATD_EL(pose.R, 1, 0),MATD_EL(pose.R, 1, 1),MATD_EL(pose.R, 1, 2),pose.t->data[1],
                             MATD_EL(pose.R, 2, 0),MATD_EL(pose.R, 2, 1),MATD_EL(pose.R, 2, 2),pose.t->data[2]};

    cv::Mat projection_mat_obj(3,4,CV_64FC1,projMatrix);
    cv::Mat cameraMatrixBogusOutput(3,3,CV_64FC1);
    cv::Mat rotMatrixBogusOutput(3,3,CV_64FC1);
    cv::Mat transMatrixBogusOutput(4,1,CV_64FC1);
    cv::Mat rotMatrixXBogusOutput(3,3,CV_64FC1);
    cv::Mat rotMatrixYBogusOutput(3,3,CV_64FC1);
    cv::Mat rotMatrixZBogusOutput(3,3,CV_64FC1);
    cv::Vec3d eulerAngles_vec;

    cv::decomposeProjectionMatrix(projection_mat_obj,
            cameraMatrixBogusOutput,
            rotMatrixBogusOutput,
            transMatrixBogusOutput,
            rotMatrixXBogusOutput,
            rotMatrixYBogusOutput,
            rotMatrixZBogusOutput,
            eulerAngles_vec);

    eulerAngles[0] = eulerAngles_vec.val[0];
    eulerAngles[1] = eulerAngles_vec.val[1];
    eulerAngles[2] = eulerAngles_vec.val[2];

That also yielded garbage data. I thought this would be simple, but apparently not. Does anyone know how to properly get euler angles out of AprilTag?

Answer 1

I'm not sure about Euler, but here's a function I use to generate quaternions (which could easily be converted to Euler).

Quaternion matrix_to_quat(const matd_t *R) {
    double T = MATD_EL(R, 0, 0) + MATD_EL(R, 1, 1) + MATD_EL(R, 2, 2) + 1;
    double S = 0;

    double m0  = MATD_EL(R, 0, 0);
    double m1  = MATD_EL(R, 1, 0);
    double m2  = MATD_EL(R, 2, 0);
    double m4  = MATD_EL(R, 0, 1);
    double m5  = MATD_EL(R, 1, 1);
    double m6  = MATD_EL(R, 2, 1);
    double m8  = MATD_EL(R, 0, 2);
    double m9  = MATD_EL(R, 1, 2);
    double m10 = MATD_EL(R, 2, 2);

    Quaternion q;

    if(T > 0.0000001) {
        S = sqrt(T) * 2;
        q.x = -(m9 - m6) / S;
        q.y = -(m2 - m8) / S;
        q.z = -(m4 - m1) / S;
        q.w = 0.25 * S;
    } else if(m0 > m5 && m0 > m10)  {
        // Column 0:
        S  = sqrt(1.0 + m0 - m5 - m10) * 2;
        q.x = -0.25 * S;
        q.y = -(m4 + m1) / S;
        q.z = -(m2 + m8) / S;
        q.w = (m9 - m6) / S;
    } else if(m5 > m10) {
        // Column 1:
        S  = sqrt(1.0 + m5 - m0 - m10) * 2;
        q.x = -(m4 + m1) / S;
        q.y = -0.25 * S;
        q.z = -(m9 + m6) / S;
        q.w = (m2 - m8) / S;
    } else {
        // Column 2:
        S  = sqrt(1.0 + m10 - m0 - m5) * 2;
        q.x = -(m2 + m8) / S;
        q.y = -(m9 + m6) / S;
        q.z = -0.25 * S;
        q.w = (m4 - m1) / S;
    }

    // Normalize
    double mag2 = 0;
    mag2 += q.x*q.x;
    mag2 += q.y*q.y;
    mag2 += q.z*q.z;
    mag2 += q.w*q.w;
    double norm = 1.0 / sqrt(mag2);
    q.x *= norm;
    q.y *= norm;
    q.z *= norm;
    q.w *= norm;

    return q;
}