两种由旋转矩阵计算欧拉角的办法

算法一：直接法

bool isRotationMatrix(cv::Mat &R)
{
	cv::Mat R_t;
	cv::transpose(R,R_t);
	cv::Mat shouldBeIdentity=R_t*R;
	cv::Mat I = cv::Mat::eye(3,3,shouldBeIdentity.type());
	return norm(I,shouldBeIdentity)< 1e-6;
} 

 void getEulerAngles(cv::Mat matrix)
{

	assert(isRotationMatrix(matrix));
	
	float sy=sqrt(matrix.at<double>(0,0)*matrix.at<double>(0,0)+matrix.at<double>(1,0)*matrix.at<double>(1,0));

	bool singular = sy<1e-6;
	
	if(!singular)
	{
		theta_x=atan2(matrix.at<double>(2,1),matrix.at<double>(2,2));
		//theta_x= theta_x*180.0/3.1416 ;
		theta_y=atan2(-matrix.at<double>(2,0), sy);
		//theta_y= theta_y*180.0/3.1416 ;
		theta_z=atan2(matrix.at<double>(1,0), matrix.at<double>(0,0));
		//theta_z= theta_z*180.0/3.1416 ;
	}
	else
	{
		theta_x=atan2(-matrix.at<double>(1,2), matrix.at<double>(1,1));
		//theta_x= thet