三維空間剛體運動的描述方式有很多種，大概是：
旋轉矩陣、平移向量--->歐式變換矩陣--->旋轉向量--->歐拉角--->四元數
下面對它們簡單的做一個整理，順一下思路。
歐式變換中，坐标系的變換有：旋轉、平移兩種情況（隻有旋轉、隻有平移、兩者都有）
1、旋轉矩陣、平移向量
假設此時，相機坐标系相對于世界坐标系的旋轉矩陣為R、平移向量為t，則
這裡要注意的就是“R、t是從哪個坐标系到哪個坐标系的，一定不能弄錯了”，R是一個行列式為1的正交矩陣。
2、歐式變換矩陣
用R、t來表示歐式空間的旋轉和平移存在一個問題：變換關系不是一個線性關系，多次變換之後計算公式會變得過于複雜。
于是就引入了齊次坐标：在三維向量的末尾添加一維（為數字1），将其變成四維向量，這種形式的坐标就是齊次坐标。
在齊次坐标裡邊，某個點
的具體坐标不是唯一确定的，比如說[1, 1, 1, 1]和[2, 2, 2, 2]表示的是同一個點。但是，如果最後一項不為0，我們可以将其轉換為“歸一化坐标”---所有坐标除以最後一項，也就是将齊次坐标轉換為非齊次坐标，對于每一個點來說，它的歸一化坐标一定是唯一的。用公式表示就是：
。
引入了其次坐标之後，我們可以将R、t寫在一個矩陣裡面，如下：
中間的矩陣稱為歐式變換矩陣，使用這種形式的歐式變換矩陣的好處就是它是一種線性變換的形式，多次變換直接将歐式變換矩陣左乘就可以得到最終的變換矩陣。
3、旋轉向量
歐式變換可以很好的描述的剛體的運動，并且還是線性的形式。為了還需要旋轉向量？
- 歐式變換矩陣用9個變量描述6個自由度的運動，資訊有備援
- 歐式變換矩陣本身是行列式為1的正交矩陣，這樣的限制會使得後面優化的求解變得苦難。
旋轉向量表示旋轉的思想就是：任意一個旋轉都可以用旋轉軸和旋轉角來表示。
是以，我們可以讓旋轉向量的方向和旋轉軸相同，旋轉向量的模表示旋轉角。
旋轉向量用3個自由度描述旋轉，再用平移向量來描述平移向量，這樣剛好可以用6個自由度來表達一次變換。
旋轉向量和旋轉矩陣之間的轉換---羅德裡格斯公式：
- 若旋轉軸為 ，旋轉角為
三維空間剛體運動的描述總結 ，則旋轉矩陣為： 三維空間剛體運動的描述總結 三維空間剛體運動的描述總結 - 若已經旋轉矩陣，求旋轉角：
于是：
同時，旋轉軸經過旋轉之後不變，則有
，解方程即可得到旋轉軸。
4、歐拉角
相比旋轉歐式、歐式變換矩陣、旋轉向量，歐拉角以一種更加直覺的方式來表示旋轉---三個軸上分離的轉角。
歐拉角的一個缺點就是存在“萬向鎖問題”。
5、四元數
四元數的形式如下：
或者
，其中
機關四元數可以用來表示三維空間中任意一個旋轉，這種表達方式和歐式變換矩陣、歐拉角是等價的。
（1）四元數和旋轉向量之間的轉換
假設某個旋轉是繞機關向量
進行了角度為
，那麼這個旋轉對應的四元數為：
相反，若已知一個機關四元數為
，可以從它計算出旋轉軸和夾角：
，
上面式子中的
加上
，得到一個完全相同的旋轉，但是對應的四元數卻成為一個負的。在四元數中，任意一個旋轉，都可以用兩個互為相反數的四元數來表示。
（2）用四元數表示旋轉
假設一個空間三維點
，以及一個由軸角指定的旋轉
，經過旋轉之後變為點
。
它們之間的關系可以用下列式子來表達：
- 把三維空間點用一個純四元數來描述：
三維空間剛體運動的描述總結 - 把旋轉用四元數表示：
三維空間剛體運動的描述總結 - 旋轉之後的點為：
三維空間剛體運動的描述總結
可以驗證，計算結果的實部為0，也就是一個純四元數。虛部的3個分量表示旋轉後的點的坐标。
（3）四元數到旋轉矩陣的轉換---見SLAM十四講P55。
下面是一道練習題：
機器人1号、2号分别位于世界坐标系中。
1号的位姿：q1=[0.25,0.2,0.3,0.1]，t1=[0.3,0.1,0.1]
2号的位姿：q2=[-0.5,0..4,-0.1,0.2]，t2=[-0.1,0.5,0.3]
注意：這裡的q、t表達的是Tcw，且未進行歸一化。
設點p在機器人1号的坐标系下的坐标為p=[0.5,0,0.2]，求它在機器人2号的坐标系下的坐标。
#include <iostream>
#include <Eigen\Core>
#include <Eigen\Dense>
//Geometry子產品提供了各種旋轉和平移的表示
#include <Eigen\Geometry>
using namespace std;
int main()
{
//一号的姿态
Eigen::Quaterniond q1(0.35, 0.2, 0.3, 0.1);
Eigen::Vector3d t1(0.3, 0.1, 0.1);
Eigen::Isometry3d Tcw_1 = Eigen::Isometry3d::Identity();
Tcw_1.rotate(q1.normalized().matrix()); //q1.matrix()将四元數轉為AngleAxis旋轉向量
Tcw_1.pretranslate(t1);
//點P
Eigen::Vector3d Pc1(0.5, 0, 0.2);
Eigen::Vector3d Pw = Tcw_1.inverse()*Pc1;
//二号的姿态
Eigen::Quaterniond q2(-0.5, 0.4, -0.1, 0.2);
Eigen::Vector3d t2(-0.1, 0.5, 0.3);
Eigen::Isometry3d Tcw_2 = Eigen::Isometry3d::Identity();
Tcw_2.rotate(q2.normalized().matrix());
Tcw_2.pretranslate(t2);
Eigen::Vector3d Pc2 = Tcw_2*Pw;
cout << "p向量在小蘿蔔二号的坐标系下的坐标為：" << endl;
cout << "(" << Pc2.x() << "," << Pc2.y() << "," << Pc2.z() << ")" << endl;
return 0;
}