旋转矩阵到旋转角的计算

需要根据旋转矩阵求出x,y,z三个轴的旋转角度    参考

vector<float> matrix2angle(Eigen::Matrix4f rotateMatrix)
{
	float sy = (float)sqrt(rotateMatrix(0,0) * rotateMatrix(0,0) + rotateMatrix(1,0)*rotateMatrix(1,0));
	bool singular = sy < 1e-6; // If
	float x, y, z;
	if (!singular)
	{
		x = (float)atan2(rotateMatrix(2,1), rotateMatrix(2,2));
		y = (float)atan2(-rotateMatrix(2,0), sy);
		z = (float)atan2(rotateMatrix(1, 0), rotateMatrix(0, 0));
	}
	else
	{
		x = (float)atan2(-rotateMatrix(1, 2), rotateMatrix(1, 1));
		y = (float)atan2(-rotateMatrix(2, 0), sy);
		z = 0;
	}
	vector<float> i;
	i.push_back((float)(x * (180.0f / M_PI)));
	i.push_back((float)(y * (180.0f / M_PI)));
	i.push_back((float)(z * (180.0f / M_PI)));
	return i;
}