오일러 회전 행렬 (C++ 11)

Question

랜드 마크 사이의 변환 행렬을 생성하기 위해 cv :: Mat를 사용하고 있습니다. 나는이 매트에서 오일러 각도를 좀하고 싶습니다하지만 난

(X, Y 또는 Z 축에서 순수 회전이다) 내 "데이터는"내 기능을 테스트 ... 그것을 할 수 아니에요 :

를

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

내 함수를 호출하고있어 방법 :

 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; 

나는 (X, Y, Z에 회전) 오일러 각도를 얻을 tryed했습니다 무엇 :

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

누군가 이렇게 할 수있는 좋은 방법이 있습니까? 도와 주실 탱크!

Answer 1

나는 고유를 사용하는 방법을 발견했습니다. 이제 내 기능은 다음과 같습니다.

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