使用opencv将旋转矩阵旋转到欧拉角

Posted 2023-02-22

【中文标题】使用opencv将旋转矩阵旋转到欧拉角

【英文标题】：Rotation matrix to euler angles with opencv

【发布时间】：2017-04-12 08:58:48

【问题描述】：

我正在做一个涉及 Aruco 标记和 opencv 的项目。 我在项目进展方面还很远。我可以读取旋转向量并使用 opencv 中的 rodrigues() 将它们转换为罗德里格斯矩阵。

这是我得到的罗德里格斯矩阵的示例：

[0,1,0;

1,0,0;

0,0,-1]

我使用以下代码。

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;

我使用预定义的 rot2euler 将罗德里格斯矩阵转换为欧拉角。 我将接收到的弧度转换为度数。

rot2euler 如下所示。

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;

如果我使用我给出的罗德里格斯矩阵作为示例，我会得到以下欧拉角。

[0; 90; -180]

但我想得到以下内容。

[-180; 0; 90]

什么时候使用这个工具http://danceswithcode.net/engineeringnotes/rotations_in_3d/demo3D/rotations_in_3d_tool.html

你可以看到 [0; 90; -180] 与罗德里格斯矩阵不匹配，但 [-180; 0; 90] 确实如此。 （我知道该工具适用于 ZYX 坐标）

所以问题是我得到了正确的值但顺序错误。

另一个问题是情况并非总是如此。 例如罗德里格斯矩阵：

[1,0,0;

0,-1,0;

0,0,-1]

为我提供正确的欧拉角。

如果有人知道问题的解决方案或者可以向我解释 rot2euler 函数是如何工作的。将不胜感激。

亲切的问候

布伦特会议

【问题讨论】：

两个不同的欧拉旋转可以代表同一个旋转。尝试使用逆变换得到 3x3 矩阵。

看来你是奇点的情况。据我所知，将旋转矩阵用于 90 度或 180 度（无论是正角还是负角）这样的角度并不方便，因为您有出现奇点的风险。而且看起来公式是错误的......have a look at this paper

【参考方案1】：

我想我来晚了，但我还是会回答。 不要引用我的话，即我不是 100% 确定，但这是一个 openCV 3.3 源代码中的文件（OPENCV_INSTALLATION_DIR/apps/interactive-calibration/rotationConverters.cpp）

在我看来，openCV 给了你 Y-Z-X（类似于上面代码中显示的内容）

为什么我说我不确定，因为我只是查看了 cv::Rodrigues 的源代码，它似乎没有调用我上面显示的这段代码。 Rodrigues 函数将数学编码到其中（我认为可以通过将 2 个旋转矩阵乘以 - R = Ry * Rz * Rx 然后查看代码中存在 @987654323 的位置来检查它@ 或类似的东西，因为“R”的元素之一通常是 cos() 或正弦，它会给你一个关于找到哪个角度的解决方案。

【参考方案2】：

不特定于 OpenCV，但你可以这样写：

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

在这里，您可以探索更多：

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

【参考方案3】：

我建议使用 PCL 库来完成这个公式

pcl::getEulerAngles(transformatoin,roll,pitch,yaw);

你只需要初始化roll、pitch、yaw和一个预先计算好的变换矩阵就可以了