mirror/opencv
1
0
Fork 0
mirror of https://github.com/opencv/opencv.git synced 2025-06-08 01:53:19 +08:00
Code Issues Actions Packages Projects Releases Wiki Activity

Merge pull request #11506 from catree:tutorial_euler_angles_doc

Browse Source
This commit is contained in:
Alexander Alekhin 2018-05-13 17:36:13 +00:00
commit b7a9d966d7
1 changed files with 29 additions and 23 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

52
samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/src/Utils.cpp
View File

@ -179,13 +179,16 @@ double get_translation_error(const cv::Mat &t_true, const cv::Mat &t)
double get_rotation_error(const cv::Mat &R_true, const cv::Mat &R) double get_rotation_error(const cv::Mat &R_true, const cv::Mat &R)
{ {
cv::Mat error_vec, error_mat; cv::Mat error_vec, error_mat;
error_mat = R_true * cv::Mat(R.inv()).mul(-1); error_mat = -R_true * R.t();
cv::Rodrigues(error_mat, error_vec); cv::Rodrigues(error_mat, error_vec);
return cv::norm(error_vec); return cv::norm(error_vec);
} }
// Converts a given Rotation Matrix to Euler angles // Converts a given Rotation Matrix to Euler angles
// Convention used is Y-Z-X Tait-Bryan angles
// Reference code implementation:
// https://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToEuler/index.htm
cv::Mat rot2euler(const cv::Mat & rotationMatrix) cv::Mat rot2euler(const cv::Mat & rotationMatrix)
{ {
cv::Mat euler(3,1,CV_64F); cv::Mat euler(3,1,CV_64F);
@ -198,49 +201,52 @@ cv::Mat rot2euler(const cv::Mat & rotationMatrix)
double m20 = rotationMatrix.at<double>(2,0); double m20 = rotationMatrix.at<double>(2,0);
double m22 = rotationMatrix.at<double>(2,2); double m22 = rotationMatrix.at<double>(2,2);
double x, y, z; double bank, attitude, heading;
// Assuming the angles are in radians. // Assuming the angles are in radians.
if (m10 > 0.998) { // singularity at north pole if (m10 > 0.998) { // singularity at north pole
x = 0; bank = 0;
y = CV_PI/2; attitude = CV_PI/2;
z = atan2(m02,m22); heading = atan2(m02,m22);
} }
else if (m10 < -0.998) { // singularity at south pole else if (m10 < -0.998) { // singularity at south pole
x = 0; bank = 0;
y = -CV_PI/2; attitude = -CV_PI/2;
z = atan2(m02,m22); heading = atan2(m02,m22);
} }
else else
{ {
x = atan2(-m12,m11); bank = atan2(-m12,m11);
y = asin(m10); attitude = asin(m10);
z = atan2(-m20,m00); heading = atan2(-m20,m00);
} }
euler.at<double>(0) = x; euler.at<double>(0) = bank;
euler.at<double>(1) = y; euler.at<double>(1) = attitude;
euler.at<double>(2) = z; euler.at<double>(2) = heading;
return euler; return euler;
} }
// Converts a given Euler angles to Rotation Matrix // Converts a given Euler angles to Rotation Matrix
// Convention used is Y-Z-X Tait-Bryan angles
// Reference:
// https://www.euclideanspace.com/maths/geometry/rotations/conversions/eulerToMatrix/index.htm
cv::Mat euler2rot(const cv::Mat & euler) cv::Mat euler2rot(const cv::Mat & euler)
{ {
cv::Mat rotationMatrix(3,3,CV_64F); cv::Mat rotationMatrix(3,3,CV_64F);
double x = euler.at<double>(0); double bank = euler.at<double>(0);
double y = euler.at<double>(1); double attitude = euler.at<double>(1);
double z = euler.at<double>(2); double heading = euler.at<double>(2);
// Assuming the angles are in radians. // Assuming the angles are in radians.
double ch = cos(z); double ch = cos(heading);
double sh = sin(z); double sh = sin(heading);
double ca = cos(y); double ca = cos(attitude);
double sa = sin(y); double sa = sin(attitude);
double cb = cos(x); double cb = cos(bank);
double sb = sin(x); double sb = sin(bank);
double m00, m01, m02, m10, m11, m12, m20, m21, m22; double m00, m01, m02, m10, m11, m12, m20, m21, m22;