我正在研究一个涉及Aruco标记和opencv的项目。 我在项目进展方面走得很远。我可以使用opencv中的rodrigues()读取旋转矢量并将它们转换为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将rodrigues矩阵转换为euler角度。 我将接收到的弧度转换为度数。
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]
为我提供正确的euler角度。
如果有人知道问题的解决方案,或者可以向我提供rot2euler函数如何正常工作的解释。非常感谢。
亲切的问候
Brent Convens
答案 0 :(得分:1)
不是特定于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
答案 1 :(得分:1)
我想我已经很晚了但是我会回答它。 不要引用我这个,即我不是100%肯定,但这是一个 来自openCV 3.3源代码的文件({OPENCV_INSTALLATION_DIR} /apps/interactive-calibration/rotationConverters.cpp)
在我看来,openCV正在给你Y-Z-X(类似于上面代码中显示的内容)
为什么我说我不确定因为我只是查看了cv :: Rodrigues的源代码,它似乎没有调用我上面显示的这段代码。 Rodrigues函数将数学编码到其中(我认为可以通过取两个旋转矩阵并将它们相乘 - R = Ry * Rz * Rx然后查看代码中存在{{{ 1}}或类似的东西,因为“R”的一个元素通常是cos()或正弦,它将为你提供一个解决方案,找出找到的角度。