這篇博客將會分享旋轉矩陣和歐拉角的相互轉換。
三維旋轉矩陣有三個自由度，旋轉能夠使用多種方法表示（旋轉矩陣，歐拉角，四元數，軸角，李羣與李代數），比如一個3x3的矩陣，比如四元數，甚至可以將旋轉表示爲三個數字，即繞三個軸x,y,z的旋轉角度。在原始的歐拉角公式中，旋轉是通過圍繞Z軸，Y軸，X軸分別旋轉得到的。它分別對應於偏航，俯仰和橫滾。
當定義一個旋轉時，可能還會引起歧義，比如對於一個給定的點(x,y,z),可以把這個點視爲行向量（x,y,z）或者列向量量，如果使用行向量，那麼就必須後乘旋轉矩陣（旋轉矩陣在後），如果使用列向量，就要左乘旋轉矩陣（旋轉矩陣在前）。
在MATLAB中，rotm2euler.m能夠實現旋轉矩陣到歐拉角的轉換。下文中將要敘述的轉換代碼參考於MATLAB中的rotm2euler.m實現。不同於MATLAB實現的是，它的旋轉順序是Z-Y-X，而下面的實現是X-Y-Z。
在計算將旋轉矩陣轉換成歐拉角之前，先介紹一下歐拉角轉換爲旋轉矩陣的方法。
歐拉角轉換爲旋轉矩陣
假如已知旋轉角，繞着X-Y-Z三個軸的角度分別爲 那麼三個旋轉矩陣可以表示如下：
如果旋轉順序爲Z-Y-X的順序，那麼旋轉矩陣可表示如下：
關於上面的旋轉矩陣，以z軸旋轉爲例：
第一種物理意義，座標系的旋轉。 其應用場景有SLAM，機械臂運動學等。
如上圖所示， P點不變，座標系 O-XYZ旋轉 α ,得到新的座標系O-X’Y’Z’，在新座標系下，P點的座標變爲P’ ，則有：
第二種物理意義，向量的旋轉，其應用場景有機器人的姿態估計等。
如上圖所示，座標系 O-XYZ不變， P’點旋轉α ,得到新的點P （我也想反過來，但用的教材上的圖，沒辦法），則有：
對應於上述旋轉順序的C++代碼如下：
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
在OpenCV中將旋轉矩陣轉換爲歐拉角
將旋轉矩陣轉換爲歐拉角就不是那麼容易，不同的旋轉順序對應着不同的旋轉角度。使用上面的代碼，即使歐拉角看起來非常不同，您也可以驗證與歐拉角[0.1920、2.3736、1.1170]（或[[11，136，64]度）和[-2.9496，0.7679，-2.0246]（或[-169，44，-116]度）相對應的旋轉矩陣實際上是相同的。下面的代碼顯示了在給定旋轉矩陣的情況下找到歐拉角的方法。以下代碼的輸出應與MATLAB的rotm2euler的輸出完全匹配，但x和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])