算法一：直接法

1. bool isRotationMatrix(cv::Mat &R)
2. {
3. cv::Mat R_t;
4. cv::transpose(R,R_t);
5. cv::Mat shouldBeIdentity=R_t*R;
6. cv::Mat I = cv::Mat::eye(3,3,shouldBeIdentity.type());
7. return norm(I,shouldBeIdentity)< 1e-6;
8. }
9. void getEulerAngles(cv::Mat matrix)
10. {
11. assert(isRotationMatrix(matrix));
12. float sy=sqrt(matrix.at<double>(0,0)*matrix.at<double>(0,0)+matrix.at<double>(1,0)*matrix.at<double>(1,0));
13. bool singular = sy<1e-6;
14. if(!singular)
15. {
16. theta_x=atan2(matrix.at<double>(2,1),matrix.at<double>(2,2));
17. //theta_x= theta_x*180.0/3.1416 ;
18. theta_y=atan2(-matrix.at<double>(2,0), sy);
19. //theta_y= theta_y*180.0/3.1416 ;
20. theta_z=atan2(matrix.at<double>(1,0), matrix.at<double>(0,0));
21. //theta_z= theta_z*180.0/3.1416 ;
22. }
23. else
24. {
25. theta_x=atan2(-matrix.at<double>(1,2), matrix.at<double>(1,1));
26. //theta_x= theta_x*180.0/3.1416 ;
27. theta_y=atan2(-matrix.at<double>(2,0), sy);
28. //theta_y= theta_y*180.0/3.1416 ;
29. theta_z=0;
30. //theta_z= theta_z*180.0/3.1416 ;
31. }
32. Debug("theta_x");
33. Debug(theta_x);
34. Debug("theta_y");
35. Debug(theta_y);
36. Debug("theta_z");
37. Debug(theta_z);
38. }

NOTE：需要判断奇异性（singularity）！

算法二：拆分法

1. void getEulerAngles(cv::Mat &R,cv::Mat &t,cv::Mat &euler_angles)
2. {
3. cv::Mat camMatrix,rotMatrix,transVect,theta_x,theta_y,theta_z;
4. cv::Mat rotation_vec;
5. cv::Mat projMatrix = cv::Mat(3,4,CV_64FC1);
6. //cv::Mat euler_angles = cv::Mat(3,1,CV_64FC1);
7. cv::Mat out_intrinsics = cv::Mat(3,3,CV_64FC1);
8. cv::Mat out_rotation = cv::Mat(3,3,CV_64FC1);
9. cv::Mat out_translation = cv::Mat(4,1,CV_64FC1);
10. cv::hconcat(R,t,projMatrix);//将R、t拼接维投影矩阵
11. cv::decomposeProjectionMatrix(projMatrix,out_intrinsics,out_rotation,out_translation,
12. cv::noArray(),cv::noArray(),cv::noArray(),euler_angles);
13. //将投影矩阵分解为旋转矩阵和相机(内参)矩阵
14. Debug("Euler Angle");
15. Debug(euler_angles);
16. }

无需判断奇异性，直接调用库函数，相对简单