Rotation matrix to Euler (c++11)

Question

I'm using cv::Mat to create transformation matrix between landmark. I would like to get the euler angles from this mat but I'm not able to do it...

my "data" (which are pure rotation on x, y or z axis) to test my function :

    float rotPIx[16] = {1,0,0,0,0,cos(CV_PI),-1*sin(CV_PI),0,0,sin(CV_PI),cos(CV_PI),0,0,0,0,1};
    float rotPIy[16] = {cos(CV_PI),0,sin(CV_PI),0,0,1,0,0,-1*sin(CV_PI),0,cos(CV_PI),0,0,0,0,1};
    float rotPIz[16] = {cos(CV_PI),-1*sin(CV_PI),0,0,sin(CV_PI),cos(CV_PI),0,0,0,0,1,0,0,0,0,1};
    const cv::Mat T_ORB_TO_WORLD = cv::Mat(4,4,CV_32F,rotPIx);

how I'm calling my function :

        cout << "full mat " << endl << T_ORB_TO_WORLD << endl;
        cv::Mat m_rot = cv::Mat(T_ORB_TO_WORLD, cv::Range(0,3), cv::Range(0,3));
        cout << "my rot :" << endl << m_rot << endl ;
        cv:: Mat euler = getEuler(m_rot);
        cout << " euler = " << euler << endl;

What I've tryed to get the euler angle (rotation on x,y,z) :

// Converts a given Rotation Matrix to Euler angles
cv::Mat rot2euler(const cv::Mat & rotationMatrix){
    cv::Mat euler(1,3,CV_32F);

    float m00 = rotationMatrix.at<float>(0,0);
    float m02 = rotationMatrix.at<float>(0,2);
    float m10 = rotationMatrix.at<float>(1,0);
    float m11 = rotationMatrix.at<float>(1,1);
    float m12 = rotationMatrix.at<float>(1,2);
    float m20 = rotationMatrix.at<float>(2,0);
    float m22 = rotationMatrix.at<float>(2,2);

    float x, y, z;

    // Assuming the angles are in radians.
    if (m10 > 0.998) { // singularity at north pole
        x = 0;
        y = CV_PI/2;
        z = atan2(m02,m22);
    }
    else if (m10 < -0.998) { // singularity at south pole
        x = 0;
        y = -CV_PI/2;
        z = atan2(m02,m22);
    }
    else
    {
        x = atan2(-m12,m11);
        y = asin(m10);
        z = atan2(-m20,m00);
    }

    euler.at<float>(0) = x;
    euler.at<float>(1) = y;
    euler.at<float>(2) = z;

    return euler;
}

    cv::Mat getEuler(cv::Mat R){
        float r_x =0, r_y=0, r_z=0;

//try 0
//return rot2euler(R); //from opencv, doesn't work for pure y rot

        //try 1 https://d3cw3dd2w32x2b.cloudfront.net/wp-content/uploads/2012/07/euler-angles1.pdf //work for rot on x,z but not pure on y
//        r_x = atan2(R.at<float>(1,2), R.at<float>(2,2));

//        float c2 = sqrt(R.at<float>(0,0)*R.at<float>(0,0)+R.at<float>(10,1)*R.at<float>(0,1));
//        r_y = atan2(-1*R.at<float>(0,2), c2);

//        float s1 = sin(r_x), c1 = cos(r_x);
//        r_z = atan2(s1*R.at<float>(2,0)-c1*R.at<float>(1,0), c1*R.at<float>(1,1)-s1*R.at<float>(2,1));

        //try 2 http://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToEuler/ //don't rly work
//        r_y = asin(R.at<float>(1,0));

//        if(R.at<float>(1,0) == 1 || R.at<float>(1,0) ==-1){
//            r_x = atan2(R.at<float>(0,2),R.at<float>(2,2));
//            r_z=0;
//        }else{
//            r_x = atan2(-1*R.at<float>(2,0),R.at<float>(0,0));
//            r_z = atan2(-1*R.at<float>(1,2),R.at<float>(1,1));
//        }

        //try 3 https://www.geometrictools.com/Documentation/EulerAngles.pdf //work for rot on x,z but not pure on y
//        if(R.at<float>(0,2) < 1){
//            if(R.at<float>(0,2) > -1){
//                r_y = asin(R.at<float>(0,2));
//                r_x = atan2(-1*R.at<float>(1,2),R.at<float>(2,2));
//                r_z = atan2(-1*R.at<float>(0,1),R.at<float>(0,0));
//            }else{
//                r_y = -1*CV_PI/2;
//                r_x = atan2(R.at<float>(1,0), R.at<float>(1,2));
//                r_z = 0;
//            }
//        }

        //try 4 https://gamedev.stackexchange.com/questions/50963/how-to-extract-euler-angles-from-transformation-matrix // I probably misswrite it because rly don't work
        //We suppose that we get the full transformation matrix and not only the rotation part as in the others try
//        if(R.at<float>(0,0) == 1){
//            r_x = atan2(R.at<float>(0,2), R.at<float>(2,3));
//            r_y = 0;
//            r_z = 0;
//        }else{
//            if(R.at<float>(0,0) == -1){
//                r_x = atan2(R.at<float>(0,2), R.at<float>(2,3));
//                r_y = 0;
//                r_z = 0;
//            }else{
//                r_x = atan2(-1*R.at<float>(2,0),R.at<float>(0,0));
//                r_y = asin(R.at<float>(1,0));
//                r_z = atan2(-1*R.at<float>(1,2), R.at<float>(1,1));
//            }
//        }

        cv::Mat euler(1,3,CV_32F);
        euler.at<float>(0) = r_x;
        euler.at<float>(1) = r_y;
        euler.at<float>(2) = r_z;

        return euler;
    }

Does someone has a good way to do this ? Tanks for your help !

see Is there a way to calculate 3D rotation on X and Y axis from a 4x4 matrix — Spektre

Bastienm Bastienm · Accepted Answer · 2017-05-22T17:01:32

I found how to, I'm using eigen. My function is now the following :

cv::Mat getEuler(cv::Mat R){
    float r_x =0, r_y=0, r_z=0;
    Eigen::Matrix<float, 3, 3> m;
    Eigen::Vector3f eulerAngle;

    for(int i=0; i < R.rows ; i++){
        for(int j=0; j < R.cols ; j++){
            m(i,j) = R.at<float>(i,j);
        }
    }

    eulerAngle = m.eulerAngles(0,1,2);

    cv::Mat euler(1,3,CV_32F);
    euler.at<float>(0) = eulerAngle[0];
    euler.at<float>(1) = eulerAngle[1];
    euler.at<float>(2) = eulerAngle[2];

    return euler;
}

Rotation matrix to Euler (c++11)

1 Answers