分别基于Egien和OpenCV实现旋转矩阵到欧拉角的转换

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
#include <iostream>
#include <Eigen/Core>
#include <Eigen/Geometry>

using namespace std;

#define PI (3.1415926535897932346f)

int main(int argc,char**argv)
{
    cout<<endl<<"********** RotationMatrix **********"<<endl;
    //旋转矩阵初始化
    Eigen::Matrix3d R;
    R<<0.9929795546761236, 0.05236994408031403, -0.1060612698030327,
       -0.01543295134771394, 0.9463450518837896, 0.3227891986850962,
       0.1172750101594792, -0.3188862363478429, 0.9405095109885926;

    //旋转矩阵转换为欧拉角
    //ZYX顺序，即先绕x轴roll,再绕y轴pitch,最后绕z轴yaw,0表示X轴,1表示Y轴,2表示Z轴
    Eigen::Vector3d euler_angles = R.eulerAngles(2, 1, 0);
    cout << "yaw(z) pitch(y) roll(x) = " << euler_angles.transpose() << endl;
    return 0;
}

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
#include <iostream>
#include <sstream>
#include <time.h>
#include <stdio.h>
#include <fstream>
#include <cmath>

#include <opencv2/core/core.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/calib3d/calib3d.hpp>
#include <opencv2/highgui/highgui.hpp>

using namespace std;
using namespace cv;
// Checks if a matrix is a valid rotation matrix.
bool isRotationMatrix(Mat &R)
{
    Mat Rt;
    transpose(R, Rt);
    Mat shouldBeIdentity = Rt * R;
    Mat I = Mat::eye(3,3, shouldBeIdentity.type());
    return  norm(I, shouldBeIdentity) < 1e-6;
}

// Calculates rotation matrix to euler angles
// The result is the same as MATLAB except the order
// of the euler angles ( x and z are swapped ).
Vec3f rotationMatrixToEulerAngles(Mat &R)
{
    //assert(isRotationMatrix(R));
    float sy = sqrt(R.at<double>(0,0) * R.at<double>(0,0) +  R.at<double>(1,0) * R.at<double>(1,0) );
    bool singular = sy < 1e-6; // If
    float x, y, z;
    if (!singular)
    {
        x = atan2(R.at<double>(2,1) , R.at<double>(2,2));
        y = atan2(-R.at<double>(2,0), sy);
        z = atan2(R.at<double>(1,0), R.at<double>(0,0));
    }
    else
    {
        x = atan2(-R.at<double>(1,2), R.at<double>(1,1));
        y = atan2(-R.at<double>(2,0), sy);
        z = 0;
    }
#if 1
    x = x*180.0f/3.141592653589793f;
    y = y*180.0f/3.141592653589793f;
    z = z*180.0f/3.141592653589793f;
#endif
    return Vec3f(x, y, z);
}

int main(){

    Vec3f eulerAngles;
    Mat R = (Mat_<double>(3,3) << 0.9929795546761236, 0.05236994408031403, -0.1060612698030327,
                                             -0.01543295134771394, 0.9463450518837896, 0.3227891986850962,
                                             0.1172750101594792, -0.3188862363478429, 0.9405095109885926);
    eulerAngles = rotationMatrixToEulerAngles(R);
    cout << "eulerAngles = " << endl;
    cout << eulerAngles << endl;
    return 0;
}