推薦開源項目:簡單的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(θ)]