欧拉角转旋转矩阵
对于两个三维点
,
,由点
经过旋转矩阵
旋转到
,则有:
任何一个旋转可以表示为依次绕着三个旋转轴旋三个角度的组合。这三个角度称为欧拉角。 对于在三维空间里的一个参考系,任何坐标系的取向,都可以用三个欧拉角来表现,如下图(蓝色是起始坐标系,而红色的是旋转之后的坐标系) :
因此欧拉角转旋转矩阵如下:
则可以如下表示欧拉角:
以下代码用来实现旋转矩阵和欧拉角之间的相互变换。
C++语言:
Mat eulerAnglesToRotationMatrix(Vec3f &theta)
{
// Calculate rotation about x axis
Mat R_x = (Mat_(3,3) <<
1, 0, 0,
0, cos(theta[0]), -sin(theta[0]),
0, sin(theta[0]), cos(theta[0])
);
// Calculate rotation about y axis
Mat R_y = (Mat_(3,3) <<
cos(theta[1]), 0, sin(theta[1]),
0, 1, 0,
-sin(theta[1]), 0, cos(theta[1])
);
// Calculate rotation about z axis
Mat R_z = (Mat_(3,3) <<
cos(theta[2]), -sin(theta[2]), 0,
sin(theta[2]), cos(theta[2]), 0,
0, 0, 1
);
// Combined rotation matrix
Mat R = R_z * R_y * R_x;
return R;
}
Vec3f rotationMatrixToEulerAngles(Mat &R)
{
float sy = sqrt(R.at(0,0) * R.at(0,0) + R.at(1,0) * R.at(1,0) );
bool singular = sy < 1e-6; // If
float x, y, z;
if (!singular)
{
x = atan2(R.at(2,1) , R.at(2,2));
y = atan2(-R.at(2,0), sy);
z = atan2(R.at(1,0), R.at(0,0));
}
else
{
x = atan2(-R.at(1,2), R.at(1,1));
y = atan2(-R.at(2,0), sy);
z = 0;
}
#if 1
x = x*180.0f/3.141592653589793f;
y = y*180.0f/3.141592653589793f;
z = z*180.0f/3.141592653589793f;
#endif
return Vec3f(x, y, z);
}
Python语言实现:
def rotationMatrixToEulerAngles(rvecs):
R = np.zeros((3, 3), dtype=np.float64)
cv2.Rodrigues(rvecs, R)
sy = math.sqrt(R[0,0] * R[0,0] + R[1,0] * R[1,0])
singular = sy < 1e-6
if not singular:
x = math.atan2(R[2,1] , R[2,2])
y = math.atan2(-R[2,0], sy)
z = math.atan2(R[1,0], R[0,0])
else :
x = math.atan2(-R[1,2], R[1,1])
y = math.atan2(-R[2,0], sy)
z = 0
#print('dst:&