mirror/opencv
1
0
Fork 0
mirror of https://github.com/opencv/opencv.git synced 2025-01-18 14:13:15 +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)
{
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);
return cv::norm(error_vec);
}
// 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 euler(3,1,CV_64F);
@ -198,49 +201,52 @@ cv::Mat rot2euler(const cv::Mat & rotationMatrix)
double m20 = rotationMatrix.at<double>(2,0);
double m22 = rotationMatrix.at<double>(2,2);
double x, y, z;
double bank, attitude, heading;
// Assuming the angles are in radians.
if (m10 > 0.998) { // singularity at north pole
x = 0;
y = CV_PI/2;
z = atan2(m02,m22);
bank = 0;
attitude = CV_PI/2;
heading = atan2(m02,m22);
}
else if (m10 < -0.998) { // singularity at south pole
x = 0;
y = -CV_PI/2;
z = atan2(m02,m22);
bank = 0;
attitude = -CV_PI/2;
heading = atan2(m02,m22);
}
else
{
x = atan2(-m12,m11);
y = asin(m10);
z = atan2(-m20,m00);
bank = atan2(-m12,m11);
attitude = asin(m10);
heading = atan2(-m20,m00);
}
euler.at<double>(0) = x;
euler.at<double>(1) = y;
euler.at<double>(2) = z;
euler.at<double>(0) = bank;
euler.at<double>(1) = attitude;
euler.at<double>(2) = heading;
return euler;
}
// 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 rotationMatrix(3,3,CV_64F);
double x = euler.at<double>(0);
double y = euler.at<double>(1);
double z = euler.at<double>(2);
double bank = euler.at<double>(0);
double attitude = euler.at<double>(1);
double heading = euler.at<double>(2);
// Assuming the angles are in radians.
double ch = cos(z);
double sh = sin(z);
double ca = cos(y);
double sa = sin(y);
double cb = cos(x);
double sb = sin(x);
double ch = cos(heading);
double sh = sin(heading);
double ca = cos(attitude);
double sa = sin(attitude);
double cb = cos(bank);
double sb = sin(bank);
double m00, m01, m02, m10, m11, m12, m20, m21, m22;