Matrice de rotation vers Euler (C++ 11)

Question

J'utilise cv :: Mat pour créer une matrice de transformation entre les points de repère. Je voudrais obtenir les angles euler de ce tapis mais je ne suis pas capable de le faire ...

mes "données" (qui sont la rotation pure sur l'axe x, y ou z) pour tester ma fonction:

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); 

comment j'appelle ma fonction:

 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; 

Ce que j'ai tryed pour obtenir l'angle de euler (rotation sur 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; 
    } 

Est-ce que quelqu'un a un bon moyen de faire cela? Réservoirs pour votre aide!

Answer 1

J'ai trouvé comment, j'utilise eigen. Ma fonction est maintenant la suivante:

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; 
}