欧拉角和旋转矩阵的转换

参考该处

这里只是纯粹记录一下

#include 
#include 
#include 
#include 
#include 
#include 

#include 
#include 
#include 
#include 

#include 

using namespace Eigen;
using namespace cv;
using namespace std;

// Calculates rotation matrix given euler angles.
Matrix3d eulerAnglesToRotationMatrix(Eigen::Vector3d &theta)
{
    // Calculate rotation about x axis
    Matrix3d R_x;
    R_x<<
        1,       0,              0,
        0,       cos(theta[0]),   -sin(theta[0]),
        0,       sin(theta[0]),   cos(theta[0]);
    // Calculate rotation about y axis
    Matrix3d R_y;
    R_y<<
        cos(theta[1]),    0,      sin(theta[1]),
        0,               1,      0,
        -sin(theta[1]),   0,      cos(theta[1]);
    // Calculate rotation about z axis
    Matrix3d R_z;
    R_z<<
        cos(theta[2]),    -sin(theta[2]),      0,
        sin(theta[2]),    cos(theta[2]),       0,
        0,               0,                  1;
    // Combined rotation matrix
    Matrix3d R = R_z * R_y * R_x;
    return R;
}

// 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 ).
Eigen::Vector3d rotationMatrixToEulerAngles(Matrix3d &R)
{
    //assert(isRotationMatrix(R));
    double sy = sqrt(R(0,0) * R(0,0) +  R(1,0) * R(1,0) );
    bool singular = sy < 1e-6; // If
    double x, y, z;
    if (!singular)
    {
        x = atan2(R(2,1) , R(2,2));
        y = atan2(-R(2,0), sy);
        z = atan2(R(1,0), R(0,0));
    }
    else
    {
        x = atan2(-R(1,2), R(1,1));
        y = atan2(-R(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 Eigen::Vector3d(x, y, z);
}

int main()
{
    cout<<endl<<endl;
    Eigen::Matrix3d R;
    Eigen::Vector3d theta(rand() % 360 - 180.0, rand() % 360 - 180.0, rand() % 360 - 180.0);
        cout<<"旋转向量是：\n"<<theta.transpose()<<endl;
    theta=theta*M_PI/180;
    cout<<"旋转向量是：\n"<<theta.transpose()<<endl;
    R=eulerAnglesToRotationMatrix(theta);
    cout<<"旋转矩阵是：\n"<<R<<endl;
//    if(! isRotationMatirx(R)){
      cout<<"旋转矩阵--->欧拉角\n"<<rotationMatrixToEulerAngles(R)<<endl;//z-y-x顺序，与theta顺序是x,y,z
//    }
//    else{
//        assert(isRotationMatirx(R));
//    }
    R<<0.0148655429818,-0.999880929698, 0.00414029679422,
                               0.999557249008, 0.0149672133247, 0.025715529948,
    -0.0257744366974, 0.00375618835797, 0.999660727178;
    
    cout<<"旋转矩阵--->欧拉角\n"<<rotationMatrixToEulerAngles(R)<<endl;
    
    return 0;
}