AprilTag - 以欧拉角形式获取旋转信息

Question

我正在尝试采用 AprilTag 的 3x3 旋转矩阵 output 并将其转换为欧拉角。

我知道使用欧拉角可能会出现一些问题，例如轴排序和云台锁定等。但是我的用例非常简单，这确实不应该成为问题。

我让 AprilTag 成功检测到标签，并估计如下姿势：

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

我可以毫无问题地从那个姿势中提取平移数据，就像这样。 （而且它似乎相当准确，在 18 英寸外大约有 1/3 英寸的误差）

    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];

我可以在pose.R访问估计的3x3 旋转矩阵（ matd_t类型）。 问题是，我无法弄清楚如何将 3x3 矩阵转换为“正常”欧拉角。 我已经尝试了 3 种不同的方法来将 3x3 矩阵转换为我在网上找到的欧拉角，但都没有奏效。

我发现的第一种方法：

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

我发现的第二种方法：

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

我发现的第三种方法：

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

所有这些方法都返回了完全垃圾数据。 然后我尝试使用 OpenCV 的decomposeProjectionMatrix function ，如此处所述。

我将 AprilTag 的数据输入如下：

    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];

这也产生了垃圾数据。 我认为这很简单，但显然不是。 有谁知道如何从 AprilTag 中正确获取欧拉角？

Answer 1

我不确定欧拉，但这里有一个 function 我用来生成四元数（可以很容易地转换为欧拉）。

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