旋轉向量

從上一篇中已經知道，旋轉可以用旋轉矩陣來表示，變換可以用變換矩陣來表示，那麼為什麼還需要旋轉向量呢？

仔細想一下，矩陣表示方式至少有以下幾個缺點：

1. 的旋轉矩陣有9個量，但是一次旋轉只有3個自由度，因此這種表達方式是冗餘的。同理，變換矩陣用16個量來表示6自由度的變換也是冗餘的。我們需要一種更緊湊的表達方式。
2. 旋轉矩陣自身帶有約束：它必須是個正交矩陣，且行列式為1。變換矩陣也是如此。當想要估計或者優化一個旋轉矩陣/變換矩陣時，這些約束會使得求解變得十分困難。

綜合上面幾點原因，我們希望找到一種更緊湊的方式來表達旋轉和平移。

通過前面的學習，我們已經知道了可以用外積來表示兩個向量間的旋轉。

順著這個思路下去，看一下如何用一個三維向量來表示旋轉？

       我們知道，任意一個旋轉都可以用一個旋轉軸和一個旋轉角來刻畫。於是，我們可以使用一個向量，其方向和旋轉軸一致，長度等於旋轉角。這種向量就稱為旋轉向量。通過這種表達方式，通過一個三維向量就可以表示旋轉了。

       同理，對於變換矩陣，我們使用一個旋轉向量+平移向量即可表達一次變換。

思路有了，剩下的問題就是旋轉矩陣和旋轉向量之間是如何轉換的？

       從旋轉向量到旋轉矩陣的轉換過程由Rodrigues's Formula表示：。（符號^是從向量到反對稱矩陣的轉換符，前面已經說明過這樣的轉換方式）。通過這個公式，由一個旋轉向量和旋轉角就可以得到旋轉矩陣了。

       反之，我們也可以計算從一個旋轉矩陣到旋轉向量的轉換。

尤拉角

       無論是旋轉矩陣還是旋轉向量，它們雖然能夠描述旋轉，但是對我們人類是非常不直觀的。當我們看到一個旋轉矩陣或者旋轉向量時，很難想象出這個旋轉究竟是什麼樣的。當它們變換時，我們也不知道物體是朝哪個方向轉動。

       尤拉角提供了一種非常直觀的方式來描述旋轉---它使用了三個分離的轉角，把一個旋轉分解成3次繞不同軸的旋轉。

下面介紹一種比較常用的尤拉角：用 偏航角 - 俯仰角 - 翻滾角（yaw - pitch - roll）三個角度來描述一個旋轉。

由於它等價於ZYX軸的旋轉，因此就以ZYX為例。

假設一個剛體的前方（朝向我們的方向）為X軸，右側為Y軸，上方為Z軸，如下圖所示：

那麼，ZYX轉角相當於把任意旋轉分解成以下三個軸上的轉角：

1、繞物體的Z軸旋轉，得到偏航角yaw

2、繞旋轉之後的Y軸旋轉，得到俯仰角pitch

3、繞旋轉之後的X軸旋轉，得到翻滾角roll

此時，可以使用這樣一個三維的向量來描述任意旋轉。這個向量是非常直觀的，我們可以從這個向量中想象出旋轉的過程。

其他定義的尤拉角也是通過這種方式，把旋轉分解到3個軸上，得到一個三維向量，只是選用的軸和順序不同。

尤拉角存在一個重大的缺點：著名的萬向鎖問題

可以看一下這裡，幫助理解什麼是萬向鎖。

可以證明：只要想用3個實數來表達三維旋轉時，都會不可避免的碰到萬向鎖問題。因此很少在SLAM程式中直接使用尤拉角來表達姿態。

四元數

       單位四元數（unit quaternion）可以用於表示三維空間裡的旋轉。它與常用的另外兩種表示方式（三維正交矩陣和尤拉角）是等價的，但是避免了尤拉角表示法中的萬向鎖問題，比起三維正交矩陣表示，四元數表示能夠更方便的給出旋轉的轉軸和旋轉角。

暫時先不管四元數的含義，先來看看四元數的形式：

       一個四元數擁有1個實部和3個虛部，像下面這樣：，其中i、j、k為四元數的3個虛部，這三個虛部滿足：

由於四元數的這種特殊的表示形式，有時也會有人用一個標量和一個向量來表達四元數：

如果一個四元數的實部為0，則將它稱為虛四元數（純四元數）；如果一個四元數的虛部為0，則將它稱為實四元數。

四元數的含義（關於四元數的更多資料可以看這裡）

四元數可以用來表示三維空間裡的點，也可以用來表示三維空間的旋轉

1、四元數表示三維空間中的點

若三維空間中的一個點的笛卡爾座標為，則用純四元數表示為：

2、單位四元數表示一個三維空間旋轉

設 q 為一個單位四元數，而 p 是一個純四元數，定義，

則 Rq(p) 也是一個純四元數，可以證明 Rq 確實表示一個旋轉，這個旋轉將空間的點 p 旋轉為空間的另一個點 Rq(p)。

四元數表示旋轉

用單元四元數表示旋轉和用正交矩陣表示旋轉是等價的，這可以通過直接的代數計算得到。

1、從旋轉向量到四元數的轉換

假設某個旋轉是繞單位向量進行了角度為的旋轉，那麼這個旋轉的四元數形式為

如果寫成向量的形式就是：

2、從四元數到旋轉軸、旋轉角的轉換

假設某個旋轉用單位四元數表示，可以通過下列公式計算出旋轉軸和夾角：

3、用四元數表示旋轉

上面已經介紹了四元數和旋轉軸、旋轉角之間的相互轉換，那麼如果已經知道了點p，以及旋轉q（用單位四元數表示的），如果計算旋轉後得到的點的座標呢？

假設一個空間三維點，以及一個由軸角指定的旋轉，它們之間的關係可以用下列式子來表達：

• 首先，把三維空間點用一個純四元數來描述：
• 然後，用四元數來表示旋轉：   （用上面的1中給出的公式計算得到）
• 旋轉之後的點可以表示為：

可以驗證，計算結果的實部為0，也就是一個純四元數。虛部的3個分量表示旋轉後的點的座標。

四元數和旋轉矩陣之間的轉換

這一部分有較多的公式的推導，暫時先不看了。先對整體框架有個完整的瞭解之後在補把。

做個標記，書本的55頁。

實踐部分：Eigen幾何模組

現在，我們通過程式設計在Eigen中使用四元數、尤拉角和旋轉矩陣，演示它們之間的變換方式。

這裡，要接觸到Eigen中的一個新的模組Geometry，Eigen/Geometry模組提供了各種旋轉和平移的表示。

#include <iostream>
#include "Eigen/Geometry"

using namespace std;

int main(int argc, char **argv)
{
    //使用Matrix3d定義一個旋轉矩陣
    Eigen::Matrix3d rotation_matrix=Eigen::Matrix3d::Identity();    //Eigen::Matrix3d::Identity()返回一個單位矩陣，不一定是方陣

    //使用AngleAxisd來定義一個旋轉向量，它底層不直接是Matrix，但是可以當做矩陣來進行運算（因為過載了運算子）
    Eigen::AngleAxisd rotation_vector(M_PI/4, Eigen::Vector3d(0, 0, 1));

    cout.precision(3);  //輸出時，小數點之後保留3位小數

    //將旋轉向量轉換為旋轉矩陣輸出
    cout<<"rotation_vector ---> rotation_matrix"<<endl;
    cout<<rotation_vector.matrix()<<endl;

    //也可以通過呼叫toRotationMatrix()方法轉換為旋轉矩陣直接賦值
    rotation_matrix=rotation_vector.toRotationMatrix();
    cout<<"rotation_vector ---> rotation_matrix"<<endl;
    cout<<rotation_matrix<<endl;

    //用旋轉向量進行座標轉換
    Eigen::Vector3d v(1, 0, 0);
    Eigen::Vector3d v_ratated=rotation_vector*v;
    cout<<"made a rotation by rotation_vector:"<<endl;
    cout<<"(1, 0, 0) after rotation = "<<v_ratated.transpose()<<endl;

    //用旋轉矩陣來進行旋轉
    v_ratated=rotation_matrix*v;
    cout<<"made a rotation by rotation_matrix:"<<endl;
    cout<<"(1, 0, 0) after rotation = "<<v_ratated.transpose()<<endl;

    //尤拉角：將旋轉矩陣直接轉換為尤拉角
    Eigen::Vector3d euler_angles=rotation_matrix.eulerAngles(2,1,0);    //引數2,1,0表示安裝ZYX順序，即yaw,pitch,roll順序
    cout<<"rotation_matrix ---> eulerAngles:"<<endl;
    cout<<"yaw pitch roll = "<<euler_angles.transpose()<<endl;

    //歐式變換矩陣使用Eigen::Isometry
    Eigen::Isometry3d T=Eigen::Isometry3d::Identity();    //雖然是3d，實際上是4*4矩陣
    cout<<"Transform matrix :"<<endl;
    T.rotate(rotation_vector);  //進行旋轉
    cout<<"Transform matrix = \n"<<T.matrix()<<endl;
    T.pretranslate(Eigen::Vector3d(1, 3, 4));   //進行平移
    cout<<"Transform matrix = \n"<<T.matrix()<<endl;

    //用變換矩陣進行座標變換
    Eigen::Vector3d v_transformed = T*v;    //這裡的變換包括了上面的兩種：旋轉和平移
    cout<<"v transformed = "<<v_transformed.transpose()<<endl;

    //四元數的使用：可以直接把旋轉向量賦值給四元數，反之亦然
    Eigen::Quaterniond q=Eigen::Quaterniond(rotation_vector);
    cout<<"quaternion = \n"<<q.coeffs()<<endl;    //coeffs的順序為（x,y,z,w），前三者為虛部，w為實部

    //也可以把旋轉矩陣賦值給四元數
    q=Eigen::Quaterniond(rotation_matrix);
    cout<<"quaternion = \n"<<q.coeffs()<<endl;

    //使用四元數來旋轉一個向量
    v_ratated=q*v;
    cout<<"(1, 0, 0) after rotation by quaternion = "<<v_ratated.transpose()<<endl;

    return 0;
}

執行該程式可以發現：通過旋轉向量、旋轉矩陣、四元數來表示旋轉的計算結果是相同的，也就印證了上面講的：這三種方式是等價的。

第三講學習到這裡，明天開始學習第四講：李群與李代數。