關鍵詞：空間旋轉、旋轉軸
用途：相機位姿估計、無人機位姿估計、3D游戲、3D建模
文章類型：概念、公式總結（本文不帶推導過程，若想了解公式是如何推出來的請搜索文獻），C++函數展示
@Author：VShawn(singlex@foxmail.com)
@Date:2016-11-04
@Lab: CvLab202@CSU
寫在前面的一些概念
右手系
關於這個概念，搞3D的人應該都懂，而像我這樣做圖像處理的可能就對這個知道的比較少了。右手系這個概念其實很簡單，看圖就懂了。在坐標系中，右手擺成下圖的樣子，當拇指指向X軸食指指向Y軸時，中指指向了Z軸，滿足這個條件的坐標系就是右手系。本文所有概念都在右手系下進行討論。
右手系
旋轉90°到底是怎么轉
當我要讓一個點，繞Y軸轉動了90°，並且用程序計算出了旋轉結果，為了驗證這個點是否旋轉正確，我們需要知道這個90°是怎么轉的。在網上搜索了挺多文章，都沒有對這個東西進行明確的定義，那么這里給出我的總結。從原點（0,0,0）往Y軸方向看，此時視野中的坐標系降維到二維坐標系XOZ，那么讓點繞O點順時針轉90°，即為正確的旋轉結果。
[圖待補]
問題一：XYZ空間內某點繞X、Y、Z軸旋轉一次
這個問題比較簡單，網上已經有較多總結：
設旋轉前坐標為
，旋轉后坐標為
。
1.繞Z軸旋轉γ角
首先給出向量表示：
\[\left[ x',y',z',1 \right]=\left[ x,y,z,1 \right]\left[ \begin{matrix} \cos \gamma & \sin \gamma & 0 & 0 \\ -\sin \gamma & \cos \gamma & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \\\end{matrix} \right]\]
然后是公式表示：
\[\begin{align} & x'=cos\gamma \cdot x-sin\gamma \cdot y \\ & y'=\sin \gamma \cdot x+co\gamma \cdot y \\ & z'=z \\ \end{align}\]
最后是代碼表示
//將空間點繞Z軸旋轉
//輸入參數 x y為空間點原始x y坐標
//thetaz為空間點繞Z軸旋轉多少度，角度制范圍在-180到180
//outx outy為旋轉后的結果坐標
void codeRotateByZ(double x, double y, double thetaz, double& outx, double& outy)
{
double x1 = x;//將變量拷貝一次，保證&x == &outx這種情況下也能計算正確
double y1 = y;
double rz = thetaz * CV_PI / 180;
outx = cos(rz) * x1 - sin(rz) * y1;
outy = sin(rz) * x1 + cos(rz) * y1;
}
2.繞Y軸旋轉β角
首先給出向量表示：
\[\left[ x',y',z',1 \right]=\left[ x,y,z,1 \right]\left[ \begin{matrix} \cos \beta & 0 & -\sin \beta & 0 \\ 0 & 1 & 0 & 0 \\ \sin \beta & 0 & \cos \beta & 0 \\ 0 & 0 & 0 & 1 \\\end{matrix} \right]\]
然后是公式表示：
\[\begin{align} & x'=cos\beta \cdot x+sin\beta \cdot z \\ & y'=y \\ & z'=-\sin \beta \cdot x+\cos \beta \cdot z \\ \end{align}\]
最后是代碼表示
//將空間點繞Y軸旋轉
//輸入參數 x z為空間點原始x z坐標
//thetay為空間點繞Y軸旋轉多少度，角度制范圍在-180到180
//outx outz為旋轉后的結果坐標
void codeRotateByY(double x, double z, double thetay, double& outx, double& outz)
{
double x1 = x;
double z1 = z;
double ry = thetay * CV_PI / 180;
outx = cos(ry) * x1 + sin(ry) * z1;
outz = cos(ry) * z1 - sin(ry) * x1;
}
3.繞X軸旋轉α角
首先給出向量表示：
\[\left[ x',y',z',1 \right]=\left[ x,y,z,1 \right]\left[ \begin{matrix} 1 & 0 & 0 & 0 \\ 0 & \cos \alpha & \sin \alpha & 0 \\ 0 & -\sin \alpha & \cos \alpha & 0 \\ 0 & 0 & 0 & 1 \\\end{matrix} \right]\]
然后是公式表示：
\[\begin{align} & x'=x \\ & y'=\cos \alpha \cdot y-\sin \alpha \cdot z \\ & z'=\sin \alpha \cdot y+\sin \alpha \cdot z \\ \end{align}\]
最后是代碼表示
//將空間點繞X軸旋轉
//輸入參數 y z為空間點原始y z坐標
//thetax為空間點繞X軸旋轉多少度，角度制范圍在-180到180
//outy outz為旋轉后的結果坐標
void codeRotateByX(double y, double z, double thetax, double& outy, double& outz)
{
double y1 = y;//將變量拷貝一次，保證&y == &y這種情況下也能計算正確
double z1 = z;
double rx = thetax * CV_PI / 180;
outy = cos(rx) * y1 - sin(rx) * z1;
outz = cos(rx) * z1 + sin(rx) * y1;
}
問題二：空間點繞任意軸旋轉
首先，需要定義"任意軸"的單位向量，例如X軸可以用向量
來表示。
那么假設旋轉軸的單位向量為
，旋轉前坐標為
，旋轉后坐標為
，旋轉角為
，於是有：
\[\begin{align} & x'=(vx\cdot vx\cdot (1-cos\theta )+cos\theta )\cdot x+(vx\cdot vy\cdot (1-cos\theta )-vz\cdot sin\theta )\cdot y+(vx\cdot vz\cdot (1-cos\theta )+vy\cdot sin\theta )\cdot z \\ & y'=(vx\cdot vy\cdot (1-cos\theta )+vz\cdot sin\theta )\cdot x+(vy\cdot vy\cdot (1-cos\theta )+cos\theta )\cdot y+(vy\cdot vz\cdot (1-cos\theta )-vx\cdot sin\theta )\cdot z \\ & z'=(vx\cdot vz\cdot (1-\cos \theta )-vy\cdot \sin \theta )\cdot x+(vy\cdot vz\cdot (1-\cos \theta )+vx\cdot \sin \theta )\cdot y+(vz\cdot vz\cdot (1-\cos \theta )+\cos \theta )\cdot z \\ \end{align}\]
計算時照着公式代入即可。
最后給出代碼實現：
//定義返回結構體
struct Point3f
{
Point3f(double _x, double _y, double _z)
{
x = _x;
y = _y;
z = _z;
}
double x;
double y;
double z;
};
//點繞任意向量旋轉，右手系
//輸入參數old_x，old_y，old_z為旋轉前空間點的坐標
//vx，vy，vz為旋轉軸向量
//theta為旋轉角度角度制，范圍在-180到180
//返回值為旋轉后坐標點
Point3f RotateByVector(double old_x, double old_y, double old_z, double vx, double vy, double vz, double theta)
{
double r = theta * CV_PI / 180;
double c = cos(r);
double s = sin(r);
double new_x = (vx*vx*(1 - c) + c) * old_x + (vx*vy*(1 - c) - vz*s) * old_y + (vx*vz*(1 - c) + vy*s) * old_z;
double new_y = (vy*vx*(1 - c) + vz*s) * old_x + (vy*vy*(1 - c) + c) * old_y + (vy*vz*(1 - c) - vx*s) * old_z;
double new_z = (vx*vz*(1 - c) - vy*s) * old_x + (vy*vz*(1 - c) + vx*s) * old_y + (vz*vz*(1 - c) + c) * old_z;
return Point3f(new_x, new_y, new_z);
}
問題三：空間點繞xyz軸連續旋轉
前面的問題比較基礎，到這個問題就需要一點空間想象力了。
首先我假設一個點繞x、y、z軸旋轉90°，最終它會落在哪里？這個答案不是唯一的，因為旋轉的順序將會影響到最終的結果。
以點(1,2,3)為例
A 我讓它首先繞x軸轉90°，再繞y軸轉90°，再繞z軸轉90°。
double x = 1, y = 2, z = 3;
codeRotateByX(y, z, 90, y, z);
codeRotateByY(x, z, -90, x, z);
codeRotateByZ(x, y, -90, x, y);
cout << endl << " (1,2,3) -> (" << x << ',' << y << ',' << z << ")" << endl << endl;
旋轉結果是：
B 這一次我讓它首先繞z軸轉90°，再繞y軸轉90°，最后繞z軸轉90°。
double x = 1, y = 2, z = 3;
codeRotateByZ(x, y, -90, x, y);
codeRotateByY(x, z, -90, x, z);
codeRotateByX(y, z, 90, y, z);
cout << endl << " (1,2,3) -> (" << x << ',' << y << ',' << z << ")" << endl << endl;
這次的結果是：
顯然，不同的旋轉順序導致了結果的不同，因此在處理空間內繞軸旋轉的問題時，我們需要嚴格定義每次旋轉的順序，否則會導致錯誤的答案。