推薦開源項目：簡單的SLAM與機器人教程與編程實踐-github
我們在做幾何變換的時候經常需要把某個坐標系上的所有點都進行一個旋轉，這個操作就叫做剛體旋轉（所有的點相對位置不變的發生旋轉）。下圖是一個典型的二維坐標系下剛體旋轉。我們把藍色的坐標系旋轉了
θ度，新坐標系就是紅色的坐標系。我們現在已知一個點相對紅色那個坐標系的坐標
(xred,yred)，和已知旋轉角度
θ，然后我們想求得該點相對於藍色那個坐標系的坐標
(xblue,yblue)。
這個其實很簡單我們利用高中學的三角幾何就可以輕松解決。從上圖可以發現不變量是黑色的那個線段的長度。而這個長度我們是可以根據黑色點相對紅色坐標系下的坐標
(xred,yred)算出來的。黑色線段長度為
r=xred2+yred2
.
然后我們可以根據紫色那個三角形計算出黑色點相對藍色坐標系下的坐標
(xblue,yblue)。
根據高中學的三角幾何我們可以知道：
xblue=r∗cos(α+θ)yblue=r∗sin(α+θ)
現在這個
α我們是不知道的。但是我們能找到一個關於它的線索。
xred=r∗cos(α)yred=r∗sin(α)。
為了用上這個線索我們需要對下面這個式子進行展開。
xblue=r∗cos(α+θ)=r∗(cos(α)∗cos(θ)−sin(α)∗sin(θ))=xred∗cos(θ)−yred∗sin(θ)yblue=r∗sin(α+θ)=r∗(sin(α)∗cos(θ)+cos(α)∗sin(θ))=yred∗cos(θ)+xred∗sin(θ)
所以我們把上面那個式子總結成向量相乘的方式那就是：
xblue=[cos(θ),−sin(θ)][xred,yred]T
yblue=[sin(θ),cos(θ)][xred,yred]T
然后可以進一步整理成矩陣相乘的形式：
[cos(θ)sin(θ)−sin(θ)cos(θ)]
