方向向量转欧拉角_欧拉角、旋转向量和旋转矩阵的相互转换

欧拉角转旋转矩阵

对于两个三维点

，

，由点

经过旋转矩阵

旋转到

，则有：

任何一个旋转可以表示为依次绕着三个旋转轴旋三个角度的组合。这三个角度称为欧拉角。 对于在三维空间里的一个参考系，任何坐标系的取向，都可以用三个欧拉角来表现，如下图(蓝色是起始坐标系，而红色的是旋转之后的坐标系) ：

因此欧拉角转旋转矩阵如下：

则可以如下表示欧拉角：

以下代码用来实现旋转矩阵和欧拉角之间的相互变换。

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:&