目錄
座標變換基礎知識
外積
外積大小爲 
，可以表示兩個向量張成四邊形的有向面積，引入符號，定義a^表示爲一個反對稱矩陣，這樣就把向量的外積變換成了矩陣和向量的乘法。
同時外積可以表示向量的旋轉，在右手法則下，用右手的四個指頭從a轉到 b，其大拇指所指向的方向就是旋轉向量的方向，旋轉向量的大小爲 a與 b的夾角。
歐氏變換
在機器人的運動中，通常先固定一個座標系，稱爲世界座標系，如圖中的 
定義的座標系；同時相機也是一個座標系，如圖中的定義的座標系。在相機視野中的某個向量P座標爲Pc，在世界座標系下座標則爲Pw。相機運動時，兩個座標之間的轉換關係由變換矩陣 T來表示。
因爲相機可以看作剛體，剛體運動時能保證同一個向量在各個座標系下的長度和夾角不會發生變化，這種變換稱作歐氏變換，一個歐式變換由一個旋轉和一個平移兩部分組成。
旋轉矩陣
首先看旋轉，設單位正交基 
經過一次旋轉變成了 
，對於同一個向量a ，它在兩個座標系下的的座標分別爲和 
，則有：
爲了描述兩個座標之間的關係，對等式左右同乘 
，左式係數即變爲單位矩陣，右式係數則變成由兩組標準正交基的內積組成的矩陣：
稱右式矩陣R 爲旋轉矩陣，旋轉矩陣作用於向量 a的座標得到向量 a`旋轉後的座標。
仔細觀察旋轉矩陣 R，可以發現旋轉矩陣爲一個行列式爲1的正交矩陣。反之，行列式爲1的正交矩陣也是一個旋轉矩陣R。這條性質在計算增量座標變化時常用作一條約束條件來計算新的旋轉矩陣。
由於R 爲正交矩陣，由正交矩陣性質知旋轉矩陣R的轉置和其逆矩陣等價
，所以 
可以描述一個相反的旋轉過程：
歐式變換除了旋轉還有平移，平移就簡單多了，直接在旋轉後的向量座標上加一個平移向量 t就可以，把旋轉和平移合在一起有：
通過旋轉矩陣R和平移向量 t 可以完整描述一個歐氏變換。
變換矩陣和齊次座標
雖然上式可以完整描述一個歐氏變換，但是這裏的變換不是一個線性關係，當做兩次變換時有：
那麼用 a 表述 c時則有：
可以想象在n多次變換後式子會變得多麼複雜，爲了簡化形式，引入了齊次座標和變換矩陣：
給三維向量加一維變成四維向量，稱爲齊次座標；把旋轉和平移寫在同一個矩陣裏，稱爲變換矩陣。
首先介紹齊次座標：
- 通過添加最後一維，用四個數表示一個三維向量，這樣就多了一個自由度，多了一個自由度就可以寫出線性的形式。
- 齊次座標的每一個分量同時乘一個非零常數仍然表示同一個點，所以一個點的齊次座標不唯一。
- 用齊次座標運算得到的也是齊次座標，將齊次座標轉換爲非齊次座標的方法爲把所有座標除以最後一項，強制最後一項爲1，然後忽略最後一項即可得到歐氏空間中的非齊次座標。
然後介紹變換矩陣：
- 變換矩陣左上角爲旋轉矩陣，右上角爲平移向量，左下角爲 0向量，右下角爲1.
- 變換矩陣的逆也表示一個反向的變換
通過引入齊次座標和旋轉矩陣，就大大簡化了歐氏變換的表示：
旋轉向量
- 旋轉矩陣R用9個量來表示三個自由度的旋轉，變換矩陣T用16個量來表示6個自由度的歐式變換，這樣會有數據冗餘。
- 旋轉矩陣具有約束：之前講過，旋轉矩陣必須是一個行列式det（R）爲1的正交矩陣，這使得估計或優化一個旋轉矩陣時會使得求解變得困難。
其實最開始提到的外積就可以表示向量的旋轉。座標系的旋轉可以由一個旋轉軸和一個旋轉角來刻畫，我們選擇方向與旋轉軸相同、長度等於旋轉角的向量，來描述旋轉，稱爲旋轉向量。易知旋轉向量用三個量來表示三個自由度的旋轉。
反之也可得出從旋轉矩陣到旋轉向量的公式：
由於旋轉軸上的向量在旋轉後不發生改變，所以有:
因此轉軸 n 是旋轉矩陣 R 特徵值爲1對應的特徵向量，所以求解方程
det(λI - R) = 1可得旋轉軸。
歐拉角
除了旋轉矩陣和旋轉向量，還可以用歐拉角來直觀的表式旋轉。
歐拉角使用三個分離的轉角來表述旋轉，分別爲：
1. 繞物體的 Z 軸旋轉，得到偏航角 yaw；
2. 繞旋轉之後的 Y 軸旋轉，得到俯仰角 pitch；
3. 繞旋轉之後的 X 軸旋轉，得到滾轉角 roll。
這樣我們就可以通過
這樣一個三維向量來描述任意旋轉。
但是使用歐拉角會遇到著名的萬向鎖問題：當俯仰角 
時，第三次旋轉與第一次旋轉將會使用同一個軸，這樣會使系統丟失一個自由度，稱爲奇異性問題。所以歐拉角不適用於插值和迭代，也很少用於SLAM程序，往往只用於人機交互中，方便直觀理解。
四元數
旋轉矩陣冗餘，歐拉角奇異。事實上我們找不到不帶奇異性的三維向量描述方式。比如，當使用兩個座標來表示地球表面時，如經緯度，當緯度爲 
時，經度沒有意義。
回憶剛學過的複數，我們可以用複數集 C 來表示複平面上的向量，乘上 i相當於把復向量旋轉90度。那麼類似的，在表達三維空間的旋轉時，也有一種類似於複數的代數：四元數。四元數是一種擴展複數，在表示旋轉時具有緊湊性，缺點是運算時比較複雜。
一個四元數q具有一個實部和三個虛部：
，
其中 
爲三個虛部，且滿足以下關係式：
有時也通過一個標量和一個向量來表達四元數：
s稱爲實部， v稱爲虛部。當虛部爲0時稱爲實四元數，實部爲0時稱爲虛四元數，虛四元數可以表示三維空間中的一個點。
爲了滿足 
的關係，定義乘上一個虛部對應旋轉180度。
四元數和旋轉向量之間的轉換關係爲：
和
第一個式子中當 θ加上2π 時，易得旋轉沒有改變，但是得到的四元數 q卻變成了 -q 。因此兩個互爲相反數的四元數表示着同一個旋轉。
用四元數表示點的旋轉時，設空間中一點 
；
若要表示點n繞轉軸旋轉 θ ，由上式得 
；
旋轉後的 
，可驗證得 p`也是純虛數，表示三維空間中的點。
最後給出四元數到旋轉矩陣的轉換公式，設四元數 ，則有：
反之有：
