剛體就是 "剛性物體"，它在運動程序中，內部各質點間的相對位置不會改變，也即 每兩個質點間的距離 保持不變
假設剛體內任意兩個質點，坐標分別為 $(x_1, y_1, z_1)$ 和 $(x_2, y_2, z_2)$，則在剛體運動程序中，這兩個質點滿足如下條件：
$\quad \left( (x_1 - x_2)^2 + (y_1 - y_2)^2 + (z_1 - z_2)^2 \right) |_t = l^2$
例如：影視劇《西游記》中的法寶金剛琢、玉凈瓶是剛體；而幌金繩、芭蕉扇等，則不是剛體
1 剛體變換
1.1 矩陣形式
OpenCV 之 影像幾何變換 中的等距變換，實際是二維剛體變換
$\quad \begin{bmatrix} x^{\prime} \\ y^{\prime} \\ 1 \end{bmatrix} = \begin{bmatrix} \cos \theta & -\sin \theta & t_x \\ \sin \theta & \cos \theta & t_y \\ 0&0&1 \end{bmatrix} \begin{bmatrix} x \\ y \\ 1\end{bmatrix}$
從平面推及空間，三維剛體變換的矩陣形式為
$\quad \begin{bmatrix}x' \\ y' \\ z' \\ 1 \end{bmatrix}=\begin{bmatrix} r_{11} & r_{12} & r_{13} & t_x \\ r_{21} & r_{22} & r_{23} & t_y \\ r_{31} & r_{32} & r_{33} &t_z \\ 0 & 0 & 0 & 1 \end{bmatrix} \begin{bmatrix} x \\ y \\z \\ 1\end{bmatrix} $
例如：空間任一點，在相機坐標系中為 $P_{c}(x, y, z)$，世界坐標系中為 $P_{w}(X, Y, Z)$ ，則 $P_c$ -> $P_w$ 就是一個剛體變換
1.2 約束分析
R 和 T 共有 12 個未知數，但 R 是標準正交矩陣，自帶 6 個約束方程，則剛體變換有 12 - 6 = 6 個自由度 (和直觀的感受一致)
表面上看，似乎只需兩組空間對應點，聯立 6 個方程，便可求得 6 個未知數，但這 6 個方程是有冗余的 (因為這兩組對應點，在各自的坐標系下，兩點之間的距離是相等的)
因此，第二組對應點，只是提供了 2 個約束方程，加上第一組對應點的 3 個約束，共有 5 個獨立的方程
顯然，還需要第三組對應點，提供 1 個獨立的方程，才能求出 R 和 T
如圖所示，兩個剛體之間：1個點重合 => 3個自由度；2個點重合 => 1個自由度；3個點重合=> 0個自由度
OpenCV 中有一個函式 estimateAffine3D() 可求解剛體變換的矩陣
1.3 直觀理解
一個單位立方體，可在 X-Y-Z 坐標系中自由運動，則二者之間的轉換關系，可視為剛體變換
單點重合：當立方體的角點 0 和 X-Y-Z 坐標系的原點 O 重合時，立方體還能自由的旋轉 (X 軸 -> Y 軸 -> Z 軸)
兩點重合：除了立方體的角點 0 和坐標系的 原點 O 重合外，再令角點 4 和 X 軸上的某點重合，則此時立方體只能 繞 X 軸旋轉
三點重合：除了以上兩個角點 0 和 4，如果再使角點 1 和 Z 軸上的某點重合，則立方體就會和 X-Y-Z 坐標系 牢牢的連接在一起
因此，選取不共面的三組對應點，聯立方程組，便可求得 R 和 T
2 旋轉向量
任意的旋轉，都可用一個旋轉軸 (axis) 和 繞軸旋轉角 (angle) 來描述，簡稱“軸角”
因此，可用一個方向和旋轉軸一致，長度等于旋轉角的向量來表示旋轉，這個向量稱為旋轉向量 (或“旋量”)
假定旋轉軸 $\mathbf{n} = [r_{x}, r_{y}, r_{z}]$，旋轉角為 $\theta$，則旋轉向量可記為 $\theta \mathbf{n}$
旋轉向量到旋轉矩陣的轉換，可由羅德里格斯公式來實作，如下：
$\quad R = cos(\theta) I + (1 - cos \theta) \mathbf{nn^T} + sin \theta \begin{bmatrix} 0&-r_z&r_y \\ r_z&0&-r_x \\ -r_y&r_x&0 \end{bmatrix}$
反之，從旋轉矩陣到旋量，公式如下：
$\quad \sin \theta \begin{bmatrix}0 &-r_z&r_y \\r_z&0&-r_x \\-r_y & r_x &0 \end{bmatrix} = \dfrac{R - R^T}{2} $
OpenCV 中有一個 Rodrigues() 函式，可實作二者的相互轉換
void Rodrigues ( InputArray src, // 輸入旋轉向量 n(3x1 或 1x3) 或 旋轉矩陣 R(3x3) OutputArray dst, // 輸出旋轉矩陣 R 或 旋轉向量 n OutputArray jacobian = noArray() // 可選，輸出 Jacobian 矩陣(3x9 或 9x3) )
3 歐拉角
3.1 定義
假定 X-Y 平面內有一點 P， 旋轉 $\theta$ 角到 $P^{\prime}$ 位置，如下圖：
取 $\angle$POX = $\theta_0$，列方程組得
$\quad x^{\prime} = r \cos (\theta_0 + \theta) = r \cos \theta_0 \cos\theta - r \sin \theta_0 \sin\theta = x \cos\theta - y \sin \theta $
$\quad y^{\prime} = r \sin(\theta_0+ \theta) = r \sin\theta_0 cos \theta + r \cos \theta_0 \sin\theta = x \sin \theta + y \cos \theta $
轉化為矩陣形式 $ \begin{bmatrix} x' \\ y' \end{bmatrix} = \begin{bmatrix} cos \theta & -sin \theta \\ sin \theta & cos \theta \end{bmatrix} \begin{bmatrix} x \\ y \end{bmatrix}$
3.2 歐拉角
將二維旋轉矩陣 $R_{2 \times 2}$ 擴展到三維空間
1）繞 Z 軸旋轉 roll，則添加 z 坐標 $\pmb{R_z} = \begin{bmatrix} cos \theta_z & -sin \theta_z & 0 \\ sin \theta_z & cos \theta_z & 0 \\ 0&0&1 \end{bmatrix}$
2）繞 Y 軸旋轉 yaw，則添加 y 坐標 $\pmb{R_y} = \begin{bmatrix} cos \theta_y & 0 & \color{blue}{sin \theta_y} \\ 0 & 1 & 0 \\ \color{blue}{-sin \theta_y} & 0 & cos \theta_y \end{bmatrix}$
3）繞 X 軸旋轉 pitch，則添加 x 坐標 $\pmb{R_x} = \begin{bmatrix} 1 & 0 & 0 \\ 0 &cos \theta_x & -sin \theta_x \\ 0 & sin \theta_x & cos \theta_x \end{bmatrix}$
因此，當按 Z-Y-X 的順序旋轉時，一個旋轉矩陣就被分解成了繞不同軸的三次旋轉，旋轉角稱為 "歐拉角"
$\quad R = R_z \cdot R_y \cdot R_x = \begin{bmatrix} cos \theta_y cos \theta_z & sin \theta_x sin \theta_y cos \theta_z - cos \theta_x sin \theta_z & sin \theta_x sin \theta_z + cos \theta_x sin \theta_y cos \theta_z \\ cos \theta_y sin \theta_z & cos \theta_x cos \theta_z + sin \theta_x sin \theta_y sin \theta_z & cos \theta_x sin \theta_y sin \theta_z - sin \theta_x cos \theta_z \\ -sin \theta_y & sin \theta_x cos \theta_y & cos \theta_x cos \theta_y \end{bmatrix} $，
注意：在使用歐拉角時，要先指明旋轉的順序，因為繞不同的軸旋轉時得到的歐拉角也不同
反之，由旋轉矩陣求解歐拉角，則有：
$\quad \theta_x = tan^{-1} (\dfrac{r_{32}}{r_{33}})$
$\quad \theta_y = sin^{-1} (-r_{33})$
$\quad \theta_z = tan^{-1} (\dfrac{r_{21}}{r_{11}})$
3.3 代碼實作
已知繞三個軸旋轉的歐拉角，要轉換為旋轉矩陣，直接套用公式
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; }
旋轉矩陣到歐拉角的轉換，要指明旋轉順序 (Z-Y-X 或 X-Y-Z 等 6 種)，下面代碼實作了和 MATLAB 中 rotm2euler 一樣的功能，只是旋轉順序不同 (X-Y-Z)
// 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; } // 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); }
4 四元數
4.1 定義
四元數本質是一種高階的復數，普通復數有一個實部和一個虛部，而四元數有一個實部和三個虛部
$\quad q = s + x \mathbf{i} + y \mathbf{j} + z \mathbf{k}$，其中 $\mathbf{i}^2=\mathbf{j}^2=\mathbf{k}^2=\mathbf{ijk}=-1$
平面中任一點的旋轉，可通過“左乘” 旋轉向量來表示，如下：
$\quad p = a + b \mathbf{i} $，$q = cos \theta + \mathbf{i} sin \theta$
$\quad p^{\prime} = qp = a\cos\theta-b\sin\theta+(a\sin\theta+b\cos\theta)i $
推及空間中任一點的旋轉，可通過“左乘”四元數來表示，如下：
$\quad p = [0, a \mathbf{i} + b \mathbf{j} + c \mathbf{k}]$，$q = [cos \theta, sin \theta \mathbf{v} ]$，其中 $\mathbf{v}$ 由 $\mathbf{i}, \mathbf{j}, \mathbf{k}$ 組合而成
$\quad p^{\prime} = qp$
4.2 實體
例1：當向量 p 圍繞 k 軸在 i-j 平面內旋轉 45°，表示該旋轉的四元數為
$\quad q = \left[\dfrac{\sqrt{2}}{2}, \dfrac{\sqrt{2}}{2} \mathbf{k} \right] $
取 $p = [0, 2 \mathbf{i}]$，則 $p^{\prime} = qp = [0, \sqrt{2}\mathbf{i} + \sqrt{2} \mathbf{j}] $，如下圖 p' 確實是 p 圍繞 $\mathbf{k}$ 軸旋轉 45° 得到的
例2：當向量 p 圍繞 q 旋轉 45°，且 q 中的向量 v 在 i-k 平面內和 p 成 45° 時，表示該旋轉的四元數為
$\quad q = \left[ \dfrac{\sqrt{2}}{2}, \dfrac{\sqrt{2}}{2} \left(\dfrac{\sqrt{2}}{2} \mathbf{i} + \dfrac{\sqrt{2}}{2} \mathbf{k} \right) \right]$
取 $p = [0, 2 \mathbf{i}]$，則 $p^{\prime} = qp = [-1,\sqrt{2}\mathbf{i}+\mathbf{j}]$，可看出 $p^{\prime}$ 中向量模長為 $\sqrt{3}$，這不再是一個純旋轉的變換
但如果再“右乘” $q^{-1}$，則 $p^{\prime} = qpq^{-1} = [0,\mathbf{i}+\sqrt{2}\mathbf{j}+\mathbf{k} ]$
如下圖，這又變成了一個純旋轉，但是旋轉的角度是 90°，不是 45°
綜上所述，向量 p 圍繞任一軸 $\mathbf{v}$ 旋轉 $\theta$，則表示該旋轉的四元數形式為
$\quad q=\left[\cos\dfrac{\theta}{2},\sin\dfrac{\theta}{2} \mathbf{v}\right] $
4.3 轉換關系
假定四元數 $q = s + x \mathbf{i} + y \mathbf{j} + z \mathbf{k}$，則旋轉矩陣為
$\quad R = \begin{bmatrix}1-2(y^{2}+z^{2})&2(xy-sz)&2(xz+sy)\\2(xy+sz)&1-2(x^{2}+z^{2})&2(yz-sx)\\2(xz-sy)&2(yz+sx)&1-2(x^{2}+y^{2})\end{bmatrix} $
或另一種形式
$\quad R = \begin{bmatrix} s^2 + x^2 - y^2 -z^2 & 2(xy - sz) & 2(xz + sy) \\ 2(xy +sz) & s^2-x^2+y^2-z^2 & 2(yz - sx) \\ 2(xz-sy) & 2(yz+sx) & s^2-x^2-y^2+z^2 \end{bmatrix}$
附 - 歐拉角可視化
一個歐拉角的可視化鏈接 Euler Angle Visualization Tool，輸入歐拉角可實時顯示位姿變化
參考資料
《An Invitaton to 3D Vision》 ch2
《Robot Vision》ch13
OpenCV - 3D rigid/afine transforamtion
Understanding Quaternions
Rotation Matrix To Euler Angles
An Orge compatible class for euler angles
轉載請註明出處，本文鏈接：https://www.uj5u.com/qita/281143.html
標籤：其他
上一篇：均值濾波