Learn OpenCV之Rotation Matrix To Euler Angles
本文要介紹的是 的旋轉矩陣與尤拉角(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])
上述程式碼的計算原理，可以將三個矩陣展開，即將