本文要介绍的是的旋转矩阵与欧拉角(Euler Angles)之间的相互转换方法。
本文其实和OpenCV关系不大，但是译者曾经花了一些时间解决自己在这部分知识上的困扰，看见原博客写的还不错，决定还是记录一下
一个旋转矩阵能表示三个角度自由度，即绕着三维的坐标轴的三个坐标做旋转，数学家们对三个自由度使用了不同的表示方式，有用三个数字表示、有用四个数字表示的、还有用的旋转矩阵表示的。使用较广的还是三个数字表示的方法，也有少数用四个数字表示。
在一个三维的旋转操作中，可以将其描述为分别绕X、Y、Z轴旋转的旋转角，也可以将其描述为分别绕Z、Y、X轴旋转的旋转角。这三个角度我们称之为欧拉角（Euler angles）或者泰特布莱恩角（Tait–Bryan angles）。在欧拉角中，旋转角被描述为依次绕着Z、X、Z轴或者Y-X-Y或者Z-Y-Z等得到的角度，即其即第一个旋转轴和最后一个旋转轴相同。而绕着三个不同轴得到的角度，例如Z-Y-X，应该称为泰特布莱恩角。但是欧拉角称呼比较普遍，所以文中我们将泰特布莱恩角也称之为欧拉角。
泰特布莱恩角有如下六种情况，X-Y-Z, X-Z-Y, Y-Z-X, Y-X-Z, Z-X-Y, Z-Y-X。 你可能会认为随便选一种情况来进行计算就可以了（因为不同的旋转顺序会导致不同的计算结果，这点是要注意的，所以必须旋转一种旋转方式进行计算），其实不能随便选的。工业上一般会选择Z-Y-X这样一个顺序，三个旋转角的名字分别称为yaw，pitch，roll。
这里有个关于旋转矩阵的计算问题值得注意，对于给定的一个点point(x,y,z)，利用旋转矩阵将其旋转，因为这个是矩阵运算，所以将点向量左乘矩阵和右乘矩阵得到的效果是不一样的。这两个旋转矩阵互为转置关系。
上面描述的意思是没有将一个标准定义旋转矩阵转换为欧拉角。文中将要叙述的转换代码参考于MATLAB中的rotm2euler.m实现。不同于MATLAB实现的是，它的旋转顺序是Z-Y-X，而下面的实现是X-Y-Z。
在计算将旋转矩阵转换成欧拉角之前，先介绍一下欧拉角转换为旋转矩阵的方法。
欧拉角转换为旋转矩阵
假如已知旋转角，绕着X-Y-Z三个轴的角度分别为。那么三个旋转矩阵可以表示如下
如果现在旋转顺序是Z-Y-X，那么旋转矩阵表示如下
对应代码如下
C++
// Calculates rotation matrix given euler angles.
Mat eulerAnglesToRotationMatrix(Vec3f &theta)
{
// Calculate rotation about x axis
Mat R_x = (Mat_<double>(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_<double>(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_<double>(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;
}
Python
# Calculates Rotation Matrix given euler angles.
def eulerAnglesToRotationMatrix(theta) :
R_x = np.array([[1, 0, 0 ],
[0, math.cos(theta[0]), -math.sin(theta[0]) ],
[0, math.sin(theta[0]), math.cos(theta[0]) ]
])
R_y = np.array([[math.cos(theta[1]), 0, math.sin(theta[1]) ],
[0, 1, 0 ],
[-math.sin(theta[1]), 0, math.cos(theta[1]) ]
])
R_z = np.array([[math.cos(theta[2]), -math.sin(theta[2]), 0],
[math.sin(theta[2]), math.cos(theta[2]), 0],
[0, 0, 1]
])
R = np.dot(R_z, np.dot( R_y, R_x ))
return R
将旋转矩阵转换为旋转角
将旋转矩阵转换为旋转角就有点麻烦了。因为选择不同的旋转顺序，得到的旋转角一般是不一样的。你可以证明下面两个欧拉角，[0.1920, 2.3736, 1.1170](角度制为[11, 136, 64])，[-2.9496, 0.7679, -2.0246](角度制为[-169, 44, -116])，可以得到同一个旋转矩阵。下面代码参考于MATLAB中的rotm2euler.m实现。不同于MATLAB实现的是，它的旋转顺序是Z-Y-X，而下面的实现是X-Y-Z。
C++
// Checks if a matrix is a valid rotation matrix.
bool isRotationMatrix(Mat &R)
{
Mat Rt;
transpose(R, Rt);
Mat shouldBeIdentity = Rt * R;
Mat I = Mat::eye(3,3, shouldBeIdentity.type());
return norm(I, shouldBeIdentity) < 1e-6;
}
// Calculates rotation matrix to euler angles
// The result is the same as MATLAB except the order
// of the euler angles ( x and z are swapped ).
Vec3f rotationMatrixToEulerAngles(Mat &R)
{
assert(isRotationMatrix(R));
float sy = sqrt(R.at<double>(0,0) * R.at<double>(0,0) + R.at<double>(1,0) * R.at<double>(1,0) );
bool singular = sy < 1e-6; // If
float x, y, z;
if (!singular)
{
x = atan2(R.at<double>(2,1) , R.at<double>(2,2));
y = atan2(-R.at<double>(2,0), sy);
z = atan2(R.at<double>(1,0), R.at<double>(0,0));
}
else
{
x = atan2(-R.at<double>(1,2), R.at<double>(1,1));
y = atan2(-R.at<double>(2,0), sy);
z = 0;
}
return Vec3f(x, y, z);
}
Python
# Checks if a matrix is a valid rotation matrix.
def isRotationMatrix(R) :
Rt = np.transpose(R)
shouldBeIdentity = np.dot(Rt, R)
I = np.identity(3, dtype = R.dtype)
n = np.linalg.norm(I - shouldBeIdentity)
return n < 1e-6
# Calculates rotation matrix to euler angles
# The result is the same as MATLAB except the order
# of the euler angles ( x and z are swapped ).
def rotationMatrixToEulerAngles(R) :
assert(isRotationMatrix(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
return np.array([x, y, z])
上述代码的计算原理，可以将三个矩阵展开，即将得到。展开如下：
根据此展开式就可以很容易理解上述代码里的求解过程了，到这里转换方法就介绍完了。
网友评论