需要根据旋转矩阵求出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; // Iffloat 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;
}