詳解坐標變換矩陣-有解無憂

在高級駕駛輔助系統（ADAS）領域，存在多種常用的坐標系：LiDAR坐標系、車輛坐標系、相機坐標系、影像坐標系等，在筆者最近的實習程序中，和這些坐標系頻繁打交道，作為第一次在CSDN發文，本文將詳細總結坐標變換矩陣，

1. 何為坐標變換矩陣（Transformation Matrix）

2. 旋轉變換矩陣（Rotation Matrix）

3. 縮放變換矩陣（Scale Matrix）

4. 平移變換矩陣（Translation Matrix）

5. 綜合變換

6. 小結

1. 何為坐標變換矩陣（Transformation Matrix）

首先要回答一個問題，何為坐標變換矩陣呢？

“橫看成嶺側成峰，遠近高低各不同”，這說明了參照系的選取對我們觀察事物的重要性，在以上所舉例的坐標系變換的語境下，點是客觀存在的，而坐標系則是根據不同的應用場景人為選擇的，處理pcd點云資料時，需要三維的LiDAR坐標系；查看影像時，需要二維的影像坐標系（通常為1920*1080等尺寸），即：

“點不變，坐標系進行變換，”

而坐標變換矩陣，就是在這種變換坐標系的前后，點的數值的變換映射關系矩陣，注意，點不動，坐標系動，下文討論三種變換矩陣：旋轉變換矩陣、縮放變換矩陣和平移變換矩陣，

2. 旋轉變換矩陣（Rotation Matrix）

2.1 二維情形

如圖1所示，在二維平面xoy上，由綠色坐標系逆時針旋轉θ°到藍色坐標系，可以看到，點A是沒有移動的，變化的是點A分別在前后兩個坐標系中的坐標，即從 $(x_{g}, y_{g})$ 變換到了 $(x_{b}, y_{b})$ ，

如圖1中黑色虛線的分解方式所示，通過矢量分解（類似于物理中力、速度等矢量的分解），將綠色坐標系中的 $(x_{g}, y_{g})$ 分別分解到藍色坐標系的x軸和y軸上，可以得到：

$\\x_{b} = cos\theta * x_{g} + sin\theta * y_{g} \\ y_{b} = -sin\theta * x_{g} + cos\theta* y_{g}$

用矩陣表示為：

$\begin{bmatrix} x_{b}\\ y_{b} \end{bmatrix} = \begin{bmatrix} cos\theta & sin\theta\\ -sin\theta & cos\theta \end{bmatrix} \begin{bmatrix} x_{g}\\ y_{g} \end{bmatrix}$

其中R則為二維情形下的旋轉變換矩陣，它表示了A點在前后坐標系中的值的映射關系，

$R = \begin{bmatrix} cos\theta & sin\theta\\ -sin\theta & cos\theta \end{bmatrix}$

2.2 三維情形

有了上述在二維平面旋轉的基礎，三維空間的旋轉矩陣也就不難得出了，

即繞x軸，y軸，z軸分別進行旋轉，最后將這三個旋轉變換矩陣相乘，就能得到在三維空間任意角度的旋轉變換矩陣了，（xyz軸滿足右手系關系）

在繞x軸旋轉的時候，可以看作在yoz二維平面上的旋轉，此時x的值不變，

$R_{x} =\begin{bmatrix} 1 &0& 0\\ 0 &cos\theta &sin\theta \\ 0 &-sin\theta & cos\theta \end{bmatrix}$

在繞y軸旋轉的時候，可以看作在zox二維平面上的旋轉，此時y的值不變，

$R_{y} =\begin{bmatrix} cos\theta &0& -sin\theta\\ 0 &1&0 \\ sin\theta &0& cos\theta \end{bmatrix}$

在繞z軸旋轉的時候，可以看作在xoy二維平面上的旋轉，此時z的值不變，

$R_{z} =\begin{bmatrix} cos\theta &sin\theta& 0 \\ -sin\theta &cos\theta&0 \\0&0&1 \end{bmatrix}$

最終的三維旋轉變換矩陣就是上面三個矩陣相乘，意為三維坐標系分別繞x軸、y軸和z軸旋轉相應的角度，

$R = R_{x}R_{y}R_{z} =\begin{bmatrix} 1 &0& 0\\ 0 &cos\theta_{x} &sin\theta_{x}\\ 0 &-sin\theta_{x} & cos\theta_{x} \end{bmatrix}\begin{bmatrix} cos\theta_{y}&0& -sin\theta_{y}\\ 0 &1&0 \\ sin\theta_{y} &0& cos\theta_{y} \end{bmatrix}\begin{bmatrix} cos\theta_{z} &sin\theta_{z}& 0 \\ -sin\theta_{z} &cos\theta_{z}&0 \\0&0&1 \end{bmatrix}$

2.3 順時針？逆時針？

在筆者初次接觸旋轉概念之時，常常對何時順時針，何時逆時針十分頭疼，

高中就接觸到點的旋轉矩陣R（rotation matrix of a point），在這種情況下是坐標系不變，點繞坐標原點順時針旋轉 θ°，

$R = \begin{bmatrix} cos\theta & sin \theta\\ -sin\theta & cos \theta \end{bmatrix}$

而在本文的情形下，旋轉變換矩陣的形式完全一致，但是方向卻相反了——變成了逆時針！究其原因，還是上文老生常談的那點，高中的矩陣是點動系不動，而本文是系動點不動，因此方向正好相反了，

回到本文的系動點不動，此時再從兩種角度推匯出順時針旋轉的公式，

1）θ變為-θ：此時就是選擇了相反的旋轉角度，變成順時針，

2）求原矩陣的逆矩陣：先逆時針，再順時針，相當于回到原系，也就是坐標和單位矩陣相乘，

可以得到，坐標系順時針旋轉θ°的旋轉變換矩陣為：

$R = \begin{bmatrix} cos\theta & -sin \theta\\ sin\theta & cos \theta \end{bmatrix}$

3. 縮放變換矩陣（Scale Matrix）

除了旋轉變換，還有坐標數值的純粹放大縮小變換，即縮放變換，下面直接給出縮放變換的公式：

$S = \begin{bmatrix} Scale.x & 0 & 0\\ 0 & Scale.y & 0 \\ 0 & 0 & Scale.z \end{bmatrix}$

上式中點的x, y, z坐標值分別擴大（縮小）了Scale.x, Scale.y, Scale.z倍，

縮放矩陣同樣存在“系動點不動”還是“點動系不動”的問題，如果是“點動系不動”，那么S矩陣中的Scale.x, Scale.y, Scale.z就是單純的點x, y, z的擴大（縮小）倍數，

如果是本文重點探討的“系動點不動”，那么S矩陣中的Scale.x, Scale.y, Scale.z就是坐標系的x, y, z軸的單位擴大（縮小）倍數的倒數，換言之，如果坐標軸單位放大Scale倍，那么點x, y, z的值就要縮小Scale倍，可以用千米和米的轉換來思考這個問題，如果單位是m，一個物體長1000m，當單位變為km后，這個物體就長1km了，單位擴大的同時，數值上從1000縮小為了1，

4. 平移變換矩陣（Translation Matrix）

坐標系的旋轉和縮放可以通過3*3的變換矩陣完成，但是平移就需要將3*3擴展到4*4，引入齊次變換矩陣，下面直接給出平移變換矩陣的公式：

$T = \begin{bmatrix} 1 & 0 & 0 & Translation.x \\ 0 & 1 & 0 & Translation.y \\ 0 & 0 & 1 & Translation.z\\ 0 & 0 & 0 & 1 \end{bmatrix}$

具體程序即：

$\begin{bmatrix} x+Translation.x\\ y+Translation.y\\ z+Translation.z\\ 1 \end{bmatrix} = \begin{bmatrix} 1 & 0 & 0 & Translation.x \\ 0 & 1 & 0 & Translation.y \\ 0 & 0 & 1 & Translation.z\\ 0 & 0 & 0 & 1 \end{bmatrix} \begin{bmatrix} x\\ y\\ z\\ 1 \end{bmatrix}$

平移矩陣同樣存在“系動點不動”還是“點動系不動”的問題，

如果是本文重點探討的“系動點不動”，如果系向左（x軸負方向）/后（y軸負方向）/下（z軸負方向）平移，那么T矩陣中的Translation.x, Translation.y, Translation.z為正；如果系向右（x軸正方向）/前（y軸正方向）/上（z軸正方向）平移，那么T矩陣中的Translation.x, Translation.y, Translation.z為負，可以借助爬樓梯來理解，小明在地面抬頭看著5樓，五樓相當于+5，當小明爬到5樓的時候，此時五樓相當于0了，小明爬到10樓的時候，此時五樓就相當于-5了，小明就是坐標系的原點，5樓就是不動的一個點，

5. 綜合變換

將變換矩陣和點向量全部齊次化：

$R = R_{x}R_{y}R_{z} = \begin{bmatrix} 1 &0& 0 & 0\\ 0 &cos\theta_{x} &sin\theta_{x}& 0\\ 0 &-sin\theta_{x} & cos\theta_{x} & 0\\0&0&0&1\end{bmatrix} \begin{bmatrix} cos\theta_{y}&0& -sin\theta_{y}& 0\\ 0 &1&0& 0 \\ sin\theta_{y} &0& cos\theta_{y}& 0\\0&0&0&1 \end{bmatrix} \begin{bmatrix} cos\theta_{z} &sin\theta_{z}& 0& 0 \\ -sin\theta_{z} &cos\theta_{z}&0& 0 \\0&0&1 & 0\\0&0&0&1\end{bmatrix}$

$S = \begin{bmatrix} Scale.x & 0 & 0& 0\\ 0 & Scale.y & 0 & 0\\ 0 & 0 & Scale.z & 0\\0& 0&0&1\end{bmatrix}$

$T = \begin{bmatrix} 1 & 0 & 0 & Translation.x \\ 0 & 1 & 0 & Translation.y \\ 0 & 0 & 1 & Translation.z\\ 0 & 0 & 0 & 1 \end{bmatrix}$

$A = \begin{bmatrix} x& y& z& 1 \end{bmatrix}^{T}$

那么此時，對于不動點，如果按照“旋轉-縮放-平移”的順序變換坐標系后，此點的值產生如下變換：

${A}' = TSRA$

6. 小結

ADAS中涉及多種坐標系的變換，點的坐標會隨著坐標系的變換而變化，坐標變換矩陣就是在坐標系變換前后，點的數值的映射關系矩陣，主要有旋轉變換矩陣、縮放變換矩陣和平移變換矩陣等，極其重要和容易混淆的是，在具體的應用場景中，是什么在變，什么不變，是系還是點，矩陣的具體實作可以采用python中的numpy模塊，

轉載請註明出處，本文鏈接：https://www.uj5u.com/qita/397518.html

標籤：其他

上一篇：三維視覺 | 03 MATLAB 單、雙目相機標定

下一篇：《人工智能及其應用》課程筆記（四）第4章非經典推理

詳解坐標變換矩陣

1. 何為坐標變換矩陣 （Transformation Matrix）

2. 旋轉變換矩陣（Rotation Matrix）

3. 縮放變換矩陣（Scale Matrix）

4. 平移變換矩陣（Translation Matrix）

5. 綜合變換

6. 小結

1. 何為坐標變換矩陣（Transformation Matrix）