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

基于Eigen:

#include 
#include 
#include 

using namespace std;

#define PI (3.1415926535897932346f)

int main(int argc,char**argv)
{
    cout<#include 
#include 
#include 
#include 
#include 
#include 

#include 
#include 
#include 
#include 

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(0,0) * R.at(0,0) +  R.at(1,0) * R.at(1,0) );
    bool singular = sy < 1e-6; // If
    float x, y, z;
    if (!singular)
    {
        x = atan2(R.at(2,1) , R.at(2,2));
        y = atan2(-R.at(2,0), sy);
        z = atan2(R.at(1,0), R.at(0,0));
    }
    else
    {
        x = atan2(-R.at(1,2), R.at(1,1));
        y = atan2(-R.at(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_(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;
}