視覺SLAM十四講：第3講 三維空間剛體運動

第3講：三維空間剛體運動

三維空間中剛體運動的描述方式：旋轉矩陣、變換矩陣、四元數和歐拉角

3.1 旋轉矩陣

3.1.1 點和向量，座標系

三維空間中，給定線性空間基$(\mathbf{e}_{1}, \mathbf{e}_{2}, \mathbf{e}_{3})$，則向量$\mathbf{a}, \mathbf{b} \in \R^{3}$，

$\mathbf{a} = \begin{bmatrix} \mathbf{e}_{1} & \mathbf{e}_{2} & \mathbf{e}_{3} \end{bmatrix} \begin{bmatrix} a_{1} \\ a_{2} \\ a_{3} \end{bmatrix} = a_{1}\mathbf{e}_{1} + a_{2}\mathbf{e}_{2} + a_{3}\mathbf{e}_{3} \tag {3-1}$

內積：

$\mathbf{a} \cdot \mathbf{b} = \mathbf{a}^{\text{T}} \mathbf{b} = \sum_{i = 1}^{3} a_{i} b_{i} = |\mathbf{a}| |\mathbf{b}| \cos \langle \mathbf{a}, \mathbf{b} \rangle \tag {3-2}$

內積描述向量間的投影關係。外積爲：

$\mathbf{a} \times \mathbf{b} = \begin{bmatrix} \mathbf{i} & \mathbf{j} & \mathbf{k} \\ a_{1} & a_{2} & a_{3} \\ b_{1} & b_{2} & b_{3} \\ \end{bmatrix} = \begin{bmatrix} a_{2} b_{3} - a_{3} b_{2} \\ a_{3} b_{1} - a_{1} b_{3} \\ a_{1} b_{2} - a_{2} b_{1} \\ \end{bmatrix} = \begin{bmatrix} 0 & - a_{3} & a_{2} \\ a_{3} & 0 & - a_{1} \\ - a_{2} & a_{1} & 0 \\ \end{bmatrix} \mathbf{b} \triangleq \mathbf{a}^{\land} \mathbf{b} \tag {3-3}$

外積的方向垂直於這兩個向量，大小爲$|\mathbf{a}| |\mathbf{b}|\langle \mathbf{a}, \mathbf{b} \rangle$，表示兩個向量張成的四邊形的有向面積。符號$\land$表示反對稱符號，將$\mathbf{a}$改寫成反對稱矩陣（skew-symmetric）形式，其作用爲將外積$\mathbf{a} \times \mathbf{b}$寫成矩陣與向量的乘積$\mathbf{a}^{\land} \mathbf{b}$，從而將外積變成線性運算。外積只對三維向量存在定義，外積可以表示向量的旋轉。

在右手法則下，用右手的四指從$\mathbf{a}$轉向$\mathbf{b}$，其拇指方向就是旋轉向量的方向，即$\mathbf{a} \times \mathbf{b}$的方向，其大小由 $\mathbf{a}$和$\mathbf{b}$的夾角決定。該方式構造了從$\mathbf{a}$到$\mathbf{b}$的旋轉向量，該向量同樣位於三維空間中，在此座標系下，可以用三個實數描述。

3.1.2 座標系間的歐氏變換

兩個座標系間的變換關係：旋轉、平移。機器人在運動過程中，通常設定一個慣性座標系（世界座標系），認爲其固定不動，如圖中$x_{W}, y_{W}, z_{W}$；相機或機器人則是一個移動座標系，如$x_{C}, y_{C}, z_{C}$。相機視野中某個向量$\mathbf{p}$，其座標爲$\mathbf{p}_{c}$，在世界座標系中，其座標爲$\mathbf{p}_{w}$。

剛體運動：同一個向量在各個座標系下的長度和夾角不會改變，這種變換稱爲歐氏變換。

旋轉：假設一組單位正交基$(\mathbf{e}_{1}, \mathbf{e}_{2}, \mathbf{e}_{3})$經一次旋轉，變成$(\mathbf{e}_{1}^{\prime}, \mathbf{e}_{2}^{\prime}, \mathbf{e}_{3}^{\prime})$，對於同一個向量$\mathbf{a}$（該向量並沒有隨座標系旋轉而運動），其在兩個座標系下的座標分別爲$[ a_{1}, a_{2}, a_{3} ]^{\text{T}}$和$[ a_{1}^{\prime}, a_{2}^{\prime}, a_{3}^{\prime} ]^{\text{T}}$，則：

$\begin{bmatrix} \mathbf{e}_{1} & \mathbf{e}_{2} & \mathbf{e}_{3} \end{bmatrix} \begin{bmatrix} a_{1} \\ a_{2} \\ a_{3} \end{bmatrix} = \begin{bmatrix} \mathbf{e}_{1}^{\prime} & \mathbf{e}_{2}^{\prime} & \mathbf{e}_{3}^{\prime} \end{bmatrix} \begin{bmatrix} a_{1}^{\prime} \\ a_{2}^{\prime} \\ a_{3}^{\prime} \end{bmatrix} \tag {3-4}$

由單位正交基性質可知：

$\begin{bmatrix} a_{1} \\ a_{2} \\ a_{3} \end{bmatrix} = \begin{bmatrix} \mathbf{e}_{1}^{\text{T}} \\ \mathbf{e}_{2}^{\text{T}} \\ \mathbf{e}_{3}^{\text{T}} \end{bmatrix} \begin{bmatrix} \mathbf{e}_{1}^{\prime} & \mathbf{e}_{2}^{\prime} & \mathbf{e}_{3}^{\prime} \end{bmatrix} \begin{bmatrix} a_{1}^{\prime} \\ a_{2}^{\prime} \\ a_{3}^{\prime} \end{bmatrix} = \begin{bmatrix} \mathbf{e}_{1}^{\text{T}} \mathbf{e}_{1}^{\prime} & \mathbf{e}_{1}^{\text{T}} \mathbf{e}_{2}^{\prime} & \mathbf{e}_{1}^{\text{T}} \mathbf{e}_{3}^{\prime} \\ \mathbf{e}_{2}^{\text{T}} \mathbf{e}_{1}^{\prime} & \mathbf{e}_{2}^{\text{T}} \mathbf{e}_{2}^{\prime} & \mathbf{e}_{2}^{\text{T}} \mathbf{e}_{3}^{\prime} \\ \mathbf{e}_{3}^{\text{T}} \mathbf{e}_{1}^{\prime} & \mathbf{e}_{3}^{\text{T}} \mathbf{e}_{2}^{\prime} & \mathbf{e}_{3}^{\text{T}} \mathbf{e}_{3}^{\prime} \\ \end{bmatrix} \begin{bmatrix} a_{1}^{\prime} \\ a_{2}^{\prime} \\ a_{3}^{\prime} \end{bmatrix} \triangleq \mathbf{R} \mathbf{a}^{\prime} \tag {3-5}$

矩陣$\mathbf{R}$由兩組基之間的內積組成，表示旋轉前後同一個向量的座標變換關係，即$\mathbf{R}$表示旋轉，又稱爲旋轉矩陣（rotation matrix）。旋轉矩陣是一個行列式爲1的正交矩陣；反之，行列式爲1的正交矩陣也是一個旋轉矩陣。所以，旋轉矩陣的集合定義爲：

$SO(n) = \{ \mathbf{R} \in \R^{n \times n} | \mathbf{R} \mathbf{R}^{\text{T}} = \mathbf{I}, \det(\mathbf{R}) = 1 \} \tag {3-6}$

$SO(n)$表示特殊正交羣（special orthogonal group），$SO(3)$表示三維空間中的旋轉。旋轉矩陣可以描述相機的旋轉。

旋轉矩陣爲正交陣，其逆（即轉置）表示方向相反的旋轉：

$\mathbf{a}^{\prime} = \mathbf{R}^{-1} \mathbf{a} = \mathbf{R}^{\text{T}} \mathbf{a} \tag {3-7}$

假設世界座標系中的向量$\mathbf{a}$，經過旋轉（$\mathbf{R}$）和平移$\mathbf{t}$後，得到$\mathbf{a}^{\prime}$，則：

$\mathbf{a}^{\prime} = \mathbf{R} \mathbf{a} + \mathbf{t} \tag {3-8}$

其中，$\mathbf{t}$爲平移向量。旋轉矩陣$\mathbf{R}$和平移向量$\mathbf{t}$共同完整地描述一個歐氏空間的座標變換關係。

3.1.3 變換矩陣與齊次座標

引入齊次座標和變換矩陣重寫方程（3-8）：

$\begin{bmatrix} \mathbf{a}^{\prime} \\ 1 \end{bmatrix} = \begin{bmatrix} \mathbf{R} & \mathbf{t} \\ \mathbf{0}^{\text{T}} & 1 \end{bmatrix} \begin{bmatrix} \mathbf{a} \\ 1 \end{bmatrix} \triangleq \mathbf{T} \begin{bmatrix} \mathbf{a} \\ 1 \end{bmatrix} \tag {3-9}$

在三維向量的末尾添加1，組成四維向量，稱爲齊次座標。對於齊次向量，可以將旋轉和平移包含在一個矩陣裏，其中矩陣$\mathbf{T}$稱爲變換矩陣（transform matrix）。用$\tilde{\mathbf{a}}$表示$\mathbf{a}$的齊次座標。

在射影幾何中，通過添加最後一維，用四個實數描述一個三維向量。在齊次座標中，點$\mathbf{x}$的每個分量同乘一個非零常數$k$後，仍然表示同一個點。因此，點的座標值並是唯一，如$[1, 1, 1, 1]^{\text{T}}$和$[2, 2, 2, 2]^{\text{T}}$表示同一個點。因此，當最後一項不爲零時，將所有座標除以最後一項，強制最後一項爲 1，從而得到一個點唯一的座標表示（即轉換成非齊次座標）：

$\tilde{\mathbf{x}} = [x, y, z, w]^{\text{T}} = [x/w, y/w, z/w, 1]^{\text{T}} \tag {3-10}$

多次歐氏變換可表示爲：

$\tilde{\mathbf{b}} = \mathbf{T}_{1} \tilde{\mathbf{a}}, \tilde{\mathbf{c}} = \mathbf{T}_{2} \tilde{\mathbf{b}} \Rightarrow \tilde{\mathbf{c}} = \mathbf{T}_{2} \mathbf{T}_{1} \tilde{\mathbf{a}} \tag {3-11}$

變換矩陣$\mathbf{T}$的結構：左上角爲旋轉矩陣、右側爲平移向量、左下角爲零向量、右下角爲1，這種矩陣又稱爲特殊歐氏羣（special euclidean group）：

$SE(3) = \left\{ \mathbf{T} = \begin{bmatrix} \mathbf{R} & \mathbf{t} \\ \mathbf{0}^{\text{T}} & 1 \end{bmatrix} \in \R^{4 \times 4} | \mathbf{R} \in SO(3), \mathbf{t} \in \R^{3} \right\} \tag {3-12}$

變換矩陣$\mathbf{T}$的逆表示反向變換：

$\mathbf{T}^{-1} = \begin{bmatrix} \mathbf{R}^{\text{T}} & - \mathbf{R}^{\text{T}} \mathbf{t} \\ \mathbf{0}^{\text{T}} & 1 \end{bmatrix} \tag {3-13}$

在不引起歧義的情況下，齊次座標與普通座標符號一般不做區分，默認爲符合所用運算法則的那一種。

3.2 實踐：Eigen

// eigen_matrix.cpp
#include <iostream>
#include <ctime>

using namespace std;

// eigen
#include <Eigen/Core>
// 稠密矩陣代數運算（逆，特徵值等）
#include <Eigen/Dense>

#define MATRIX_SIZE 100

/******************************
* 本程序演示了 Eigen 基本類型的使用
******************************/

int main(int argc, char const *argv[])
{
    /* code */
    // Eigen以矩陣爲基本數據單元。Matrix是一個模板類。前三個參數爲：數據類型，行，列
    Eigen::Matrix<float, 2, 3> matrix_23;
    // Eigen通過`typedef`定義了許多內置類型，其底層仍爲｀Eigen::Matrix｀
    // 例如`Vector3d`實質上是`Eigen::Matrix<double, 3, 1>`
    Eigen::Vector3d v_3d;
    Eigen::Matrix3d matrix_33 = Eigen::Matrix3d::Zero();    // 初始化爲零

    // 使用動態尺寸矩陣時，可以不指定矩陣大小
    Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> matrix_dynamic;
    Eigen::MatrixXd matrix_x;

    // 矩陣操作
    // 輸入數據
    matrix_23 << 1, 2, 3, 4, 5, 6;
    cout << matrix_23 << endl;

    // 用()訪問矩陣中的元素
    for (int i = 0; i < 1; i++)
    {
        for (int j = 0; j < 2; j ++)
        {
            cout << matrix_23(i, j) << endl;
        }
    }

    v_3d << 3, 2, 1;

    // 矩陣和向量相乘（實際上仍是矩陣和矩陣）
    // 但是不能混合數據類型不同的矩陣，需要顯式轉換
    Eigen::Matrix<double, 2, 1> result = matrix_23.cast<double>() * v_3d;
    cout << result << endl;

    // 矩陣運算
    matrix_33 = Eigen::Matrix3d::Random();
    cout << matrix_33 << endl;

    // 轉置
    cout << matrix_33.transpose() << endl;
    // 各元素和
    cout << matrix_33.sum() << endl;
    // 跡
    cout << matrix_33.trace() << endl;
    // 標量相乘
    cout << 10 * matrix_33 << endl;
    // 逆
    cout << matrix_33.inverse() << endl;
    // 行列式
    cout << matrix_33.determinant() << endl;

    // 特徵值
    // 實對稱矩陣可以保證成功對角化
    Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> eigen_solver ( matrix_33.transpose() * matrix_33 );
    cout << "eigen values = " << eigen_solver.eigenvalues() << endl;
    cout << "eigen vectors = " << eigen_solver.eigenvectors() << endl;

    // 解方程
    // 求解`matrix_NN * x = v_Nd`
    // 直接求逆運算量大
    Eigen::Matrix<double, MATRIX_SIZE, MATRIX_SIZE> matrix_NN;
    matrix_NN = Eigen::MatrixXd::Random(MATRIX_SIZE, MATRIX_SIZE);
    Eigen::Matrix<double, MATRIX_SIZE, 1> v_Nd;
    v_Nd = Eigen::VectorXd::Random(MATRIX_SIZE);

    // 計時
    clock_t time_stt = clock();
    // 直接求逆
    Eigen::Matrix<double, MATRIX_SIZE, 1> x = matrix_NN.inverse() * v_Nd;
    cout << "time use in normal inverse is " << 1000 * (clock() - time_stt) / (double)CLOCKS_PER_SEC << "ms" << endl;

    // 通常用矩陣分解求逆，例如`QR`分解，速度會快很多
    time_stt = clock();
    x = matrix_NN.colPivHouseholderQr().solve(v_Nd);
    cout << "time use in Qr compsition is " << 1000 * (clock() - time_stt) / (double)CLOCKS_PER_SEC << "ms" << endl;

    return 0;
}

# CMakeLists.txt

# 聲明要求的 cmake 最低版本
cmake_minimum_required( VERSION 2.8 )

# 聲明一個 cmake 工程
project( Eigen )

# 添加頭文件
include_directories( "/usr/include/eigen3" )

# 添加一個可執行程序
# 語法：add_executable( 程序名 源代碼文件 ）
add_executable( eigen_matrix eigen_matrix.cpp )

# 設置編譯模式
set( CMAKE_BUILD_TYPE "Debug" )

3.3 旋轉向量和歐拉角

3.3.1 旋轉向量

矩陣表示方式的缺點：

1. $SO(3)$旋轉矩陣有九個量，但一次旋轉只有三個自由度，因此這種表達方式是冗餘的。同理，變換矩陣用十六個量表達了六自由度變換；

2. 旋轉矩陣自帶約束：必須爲正交矩陣，且行列式爲1。變換矩陣也是如此。約束會使估計、優化旋轉矩陣（變換矩陣）變得困難。

任意旋轉都可以用一個旋轉軸和一個旋轉角表示，因此旋轉可以用一個向量表示旋轉，其方向與旋轉軸一致、長度等於旋轉角，這種向量稱爲旋轉向量（rotation vector）（軸角，axis-angle）。同理，變換可以用一個旋轉向量和一個平移向量表示，此時，維數恰好爲6。

旋轉向量和旋轉矩陣之間的轉換：假設有一個旋轉軸爲$\mathbf{n}$、角度爲$\theta$的旋轉，其對應的旋轉向量爲$\theta \mathbf{n}$，由羅德里格斯公式（Rodrigues’s formula）：

$\mathbf{R} = \cos \theta \mathbf{I} + (1 - \cos \theta) \mathbf{n} \mathbf{n}^{\text{T}} + \sin \theta \mathbf{n}^{\land} \tag {3-14}$

其中，$\land$表示向量到反對稱的轉換符。旋轉矩陣到旋轉向量的轉換：

$\begin{aligned} \text{tr}(\mathbf{R}) & = \cos \theta \text{tr}(\mathbf{I}) + (1 - \cos \theta) \text{tr}(\mathbf{n} \mathbf{n}^{\text{T}}) + \sin \theta \text{tr}(\mathbf{n}^{\land}) \\ & = 1 + 2 \cos \theta \end{aligned}$

因此，轉角$\theta$爲：

$\theta = \arccos(\frac{\text{tr}(\mathbf{R}) - 1}{2}) \tag {3-16}$

由於轉軸向量$\mathbf{n}$旋轉不變，因此

$\mathbf{R} \mathbf{n} = \mathbf{n}$

轉軸$\mathbf{n}$是矩陣$\mathbf{R}$特徵值1對應的特徵向量。

3.3.2 歐拉角

**歐拉角（Euler angle）**用三個分離的轉角把一個旋轉分解成三次繞不同軸的旋轉，提供了一種直觀方式描述旋轉。

根據不同的分解方式，歐拉角存在不同的定義方法。例如先繞X軸旋轉，再繞Y軸，最後繞Z軸，就得到了一個XYZ軸的旋轉。同理，可以定義ZYZ、ZYX等旋轉方式。此外，還需區分每次旋轉是繞固定軸，還是繞旋轉後的軸。

歐拉角常用“偏航-俯仰-滾轉”（yaw-pitch-roll）三個角度描述一個旋轉，其等價於ZYX軸旋轉。假設一個剛體的前方爲X軸，右側爲Y軸，上方爲Z軸，ZYX轉角把旋轉分解成：

1. 繞物體的Z軸旋轉，得到偏航角（yaw）；

2. 繞旋轉之後的Y軸旋轉，得到俯仰角（pitch）；

3. 繞旋轉之後的X軸旋轉，得到滾轉角（roll）。

此時，使用三維向量$[r, p, y]^{\text{T}}$描述任意旋轉。歐拉角的缺點是萬向鎖問題（gimbal lock）：當俯仰角爲$\pm90^{\circ}$時，第一次旋轉與第三次旋轉將使用同一個軸，使得系統丟失了一個自由度（三次旋轉變成兩次旋轉），稱爲奇異性問題。理論上，只要用三個實數表示三維旋轉時，都會碰到奇異性問題。因此，歐拉角不適於插值和迭代，只能用於人機交互。

3.4 四元數

3.4.1 四元數的定義

三維旋轉是一個三維流形，若要無奇異性地表示，三個量不夠，因此，三維向量表示旋轉的無奇異性方式不存在。

四元數（quaternion）：Hamilton提出的一種擴展複數，緊湊且無奇異性。一個四元數$\mathbf{q}$有一個實部和三個虛部，通常實部在前（也有實部在後的）：

$\mathbf{q} = q_{0} + q_{1} i + q_{2} j + q_{3} k \tag {3-17}$

其中，$i, j, k$爲四元數的三個虛部，三個虛部滿足：

$\begin{cases} i^{2} = j^{2} = k^{2} = -1 \\ ij = k, ji = -k \\ jk = i, kj = -i \\ ki = j, ij = -j \end{cases} \tag {3-18}$

四元數也用一個標量和一個向量表示：

$\mathbf{q} = [s, \mathbf{v}], \ s = q_{0} \in \R, \mathbf{v} = [q_{1}, q_{2}, q_{3}]^{\text{T}} \in \R^{3}$

其中，$s$爲四元數實部，而$\mathbf{v}$爲虛部。如果一個四元數虛部爲$0$，稱之爲實四元數；反之，若其實部爲$0$，稱之爲虛四元數。

單位四元數可以表示三維空間中任意旋轉，假設一個旋轉繞單位向量$\mathbf{n} = [n_{x}, n_{y}, n_{z}]^{\text{T}}$旋轉$\theta$，則該旋轉的四元數爲：

$\mathbf{q} = \left[ \cos \frac{\theta}{2}, n_{x} \sin \frac{\theta}{2}, n_{y} \sin \frac{\theta}{2}, n_{z} \sin \frac{\theta}{2} \right]^{\text{T}} \tag {3-19}$

單位四元數對應旋轉軸與旋轉角分別爲：

$\begin{cases} \theta = 2 \arccos q_{0} \\ [n_{x}, n_{y}, n_{z}]^{\text{T}} = \frac{[q_{1}, q_{2}, q_{3}]^{\text{T}}}{\sin \frac{\theta}{2}} \end{cases} \tag {3-20}$

在四元數中，任意的旋轉都可以由兩個互爲相反數的四元數表示。當$\theta = 0$時，得到一個沒有旋轉的實四元數：

$\mathbf{q}_{0} = \left[ \pm 1, 0, 0, 0 \right]^{\text{T}} \tag {3-21}$

3.4.2 四元數的運算

四元數包括四則運算、數乘、求逆、共軛等。

給定兩個四元數

$\mathbf{q}_{a} = [s_{a}, \mathbf{v}_{a}] = s_{a} + x_{a} i + y_{a} j + z_{a} k$

$\mathbf{q}_{b} = [s_{b}, \mathbf{v}_{b}] = s_{b} + x_{b} i + y_{b} j + z_{b} k$

1. 加法和減法

$\mathbf{q}_{a} \pm \mathbf{q}_{b} = [s_{a} \pm s_{b}, \mathbf{v}_{a} \pm \mathbf{v}_{b}] \tag {3-22}$

2. 乘法：$\mathbf{q}_{a}$的各項與$\mathbf{q}_{b}$的各項分別相乘，然後相加，虛部遵從方程（3-18）

$\begin{aligned} \mathbf{q}_{a} \mathbf{q}_{b} & = s_{a} s_{b} - x_{a} x_{b} - y_{a} y_{b} - z_{a} z_{b} \\ & + (s_{a} x_{b} + x_{a} s_{b} + y_{a} z_{b} - z_{a} y_{b}) i \\ & + (s_{a} y_{b} - x_{a} z_{b} + y_{a} s_{b} + z_{a} x_{b}) j \\ & + (s_{a} z_{b} + x_{a} y_{b} - y_{a} x_{b} + z_{a} s_{b}) k \\ & = \left[ s_{a} s_{b} - \mathbf{v}_{a}^{\text{T}} \mathbf{v}_{b}, s_{a} \mathbf{v}_{a} + s_{b} \mathbf{v}_{b} + \mathbf{v}_{a} \times \mathbf{v}_{b} \right] \end{aligned} \tag {3-24}$

四元數乘法通常不可交換，除非$\mathbf{v}_{a}$和$\mathbf{v}_{b}$在$\R^{3}$中共線，此時外積項爲零。

3. 共軛：四元數共軛是把虛部取成相反數

$\mathbf{q}^{\ast} = [s, - \mathbf{v}] = s - x i - y j - z k \tag {3-25}$

四元數共軛與自身相乘，得到一個實四元數，其實部爲模長的平方：

$\mathbf{q}^{\ast} \mathbf{q} = \mathbf{q} \mathbf{q}^{\ast} = \left[ s^{2} - \mathbf{v}^{\text{T}} \mathbf{v}, \mathbf{0} \right] \tag {3-26}$

4. 模長

$\| \mathbf{q} \| = \sqrt{ s^{2} + x^{2} + y^{2} + z^{2} } \tag {3-27}$

兩個四元數乘積的模即爲模的乘積，單位四元數相乘後仍是單位四元數。

$\| \mathbf{q}_{a} \mathbf{q}_{b} \| = \| \mathbf{q}_{a} \| \| \mathbf{q}_{b} \| \tag {3-28}$

5. 逆

$\mathbf{q}^{-1} = \frac{\mathbf{q}^{\ast}}{\| \mathbf{q} \|} \tag {3-29}$

則四元數與其逆的乘積爲實四元數$\mathbf{1}$：

$\mathbf{q}^{-1} \mathbf{q} = \mathbf{q} \mathbf{q}^{-1} = \mathbf{1} \tag {3-30}$

單位四元數的逆和共軛相同。乘積的逆滿足

$(\mathbf{q}_{a} \mathbf{q}_{b})^{-1} = \mathbf{q}_{b}^{-1} \mathbf{q}_{a}^{-1} \tag {3-31}$

6. 數乘與點乘

四元數與標量相乘

$k \mathbf{q} =[ks, k \mathbf{v}] \tag {3-32}$

點乘：兩個四元數對應位置元素分別相乘

$\mathbf{q}_{a} \cdot \mathbf{q}_{b} = s_{a} s_{b} + x_{a} x_{b} i + y_{a} y_{b} j + z_{a} z_{b} k \tag {3-33}$

3.4.3 用四元數表示旋轉

給定三維空間中點$\mathbf{p} = [x, y, z] \in \R^{3}$和軸角$\mathbf{n}, \theta$指定的旋轉，點$\mathbf{p}$經旋轉後記爲$\mathbf{p}^{\prime}$，則點的旋轉四元數表示爲：

1. 將點$\mathbf{p}$表示爲虛：

$\mathbf{p} = [0, x, y, z] = [0, \mathbf{v}]$

2. 由方程（3-19）旋轉的四元數$\mathbf{q}$表示爲：

$\mathbf{q} = [\cos \frac{\theta}{2}, \mathbf{n} \sin \frac{\theta}{2}]$

則旋轉後點$\mathbf{p}^{\prime}$的表示爲：

$\mathbf{p}^{\prime} = \mathbf{q} \mathbf{p} \mathbf{q}^{-1} \tag {3-34}$

3.4.4 四元數到旋轉矩陣的轉換

四元數$\mathbf{q} = q_{0} + q_{1} i + q_{2} j + q_{3} k$對應的旋轉矩陣$\mathbf{R}$爲：

$\mathbf{R} = \begin{bmatrix} 1 - 2 q_{2}^{2} - 2 q_{3}^{2} & 2 q_{1} q_{2} + 2 q_{0} q_{3} & 2 q_{1} q_{3} - 2 q_{0} q_{2} \\ 2 q_{1} q_{2} - 2 q_{0} q_{3} & 1 - 2 q_{1}^{2} - 2 q_{3}^{2} & 2 q_{2} q_{3} + 2 q_{0} q_{1} \\ 2 q_{1} q_{3} + 2 q_{0} q_{2} & 2 q_{2} q_{3} - 2 q_{0} q_{1} & 1 - 2 q_{1}^{2} - 2 q_{2}^{2} \end{bmatrix} \tag {3-35}$

反之，旋轉矩陣$\mathbf{R} = [ m_{ij} ], i, j \in \{1, 2, 3\}$對應的四元數$\mathbf{q}$爲：

$q_{0} = \frac{\sqrt{\text{tr}(\mathbf{R}) + 1}}{2}, q_{1} = \frac{m_{23} - m{32}}{4 q_{0}}, q_{2} = \frac{m_{31} - m{13}}{4 q_{0}}, q_{3} = \frac{m_{12} - m{21}}{4 q_{0}} \tag {3-36}$

3.5 相似、仿射、射影變換

歐氏變換保持向量的長度和夾角不變，相當於將一個剛體進行了移動或旋轉，不改變其自身的樣子。

1. 相似變換

相似變換比歐氏變換多了一個自由度，它允許物體進行均勻縮放，其矩陣表示爲：

$\mathbf{T}_{S} = \begin{bmatrix} s \mathbf{R} & \mathbf{t} \\ \mathbf{0}^{\text{T}} & 1 \end{bmatrix} \tag {3-37}$

旋轉矩陣部分增加的縮放因子$s$，表示在對向量旋轉之後，還可以在$x, y, z$三個座標上對物體均勻縮放。例如，邊長爲1的立方體通過相似變換後，變成邊長爲10的立方體。

2. 仿射變換

仿射變換的矩陣形式爲：

$\mathbf{T}_{A} = \begin{bmatrix} \mathbf{A} & \mathbf{t} \\ \mathbf{0}^{\text{T}} & 1 \end{bmatrix} \tag {3-38}$

仿射變換要求$\mathbf{A}$是一個可逆矩陣，不必是正交矩陣。仿射變換也叫正交投影。例如，立方體經過仿射變換後，變成平行六面體。

3. 射影變換

射影變換形式最爲一般：

$\mathbf{T}_{P} = \begin{bmatrix} \mathbf{A} & \mathbf{t} \\ \mathbf{a}^{\text{T}} & 1 \end{bmatrix} \tag {3-39}$

其左上角爲可逆矩陣$\mathbf{A}$、右上爲平移$\mathbf{t}$、左下縮放$\mathbf{a}^{\text{T}}$。由於採用齊次座標，當$v \neq 0$時，可以對矩陣各元素除以$v$得到右下角爲1的矩陣；否則，得到右下角爲0的矩陣。因此，2D的射影變換共有8個自由度，3D共有15個自由度。從真實世界到相機照片的變換是一個射影變換。如果相機的焦距爲無窮遠，這個變換則爲仿射變換。

在“不變性質”中，從上到下是包含關係。例如，歐氏變換除了保體積之外，也具有保平行、相交等性質。

實踐：Eigen幾何模塊

// use_geometry.cpp

#include <iostream>
#include <cmath>

using namespace std;

#include <Eigen/Core>
#include <Eigen/Geometry>

/**********************************
* 本程序演示了 Eigen 幾何模塊的使用方法
**********************************/

int main(int argc, char const *argv[])
{
    /* code */
    // `Eigen/Geometry`模塊提供各種旋轉和平移表示
    // 3D旋轉矩陣直接使用`Matrix3d`或`Matrix3f`
    Eigen::Matrix3d rotation_matrix = Eigen::Matrix3d::Identity();

    // 旋轉向量使用`AngleAxis`，其底層不｀Matrix｀，但通過重載運算符，可以進行矩陣運算可
    // 沿Z軸旋轉45度
    Eigen::AngleAxisd rotation_vector(M_PI / 4, Eigen::Vector3d(0, 0, 1));
    cout.precision(3);
    // 用`matrix()`轉換成矩陣
    cout << "rotation matrix = \n" << rotation_vector.matrix() << endl;
    // 直接賦值
    rotation_matrix = rotation_vector.toRotationMatrix();

    // 用`AngleAxis`進行座標變換
    Eigen::Vector3d v(1, 0, 0);
    Eigen::Vector3d v_rotated = rotation_vector * v;
    cout << "(1,0,0) after rotation = " << v_rotated.transpose() << endl;
    // 用旋轉矩陣
    v_rotated = rotation_matrix * v;
    cout << "(1,0,0) after rotation = " << v_rotated.transpose() << endl;

    // 歐拉角：將旋轉矩陣轉換成歐拉角
    // ZYX順序，`yaw-pitch-roll`
    Eigen::Vector3d euler_angle = rotation_matrix.eulerAngles(2, 1, 0);
    cout << "yaw pitch roll = " << euler_angle.transpose() << endl;

    // 歐氏變換矩陣使用`Eigen::Isometry`
    // 實質是4＊4的矩陣
    Eigen::Isometry3d T = Eigen::Isometry3d::Identity();
    // 按rotation_vector旋轉
    T.rotate(rotation_vector);
    // 平移向量設成(1,3,4)
    T.pretranslate(Eigen::Vector3d(1, 3, 4));

    // 用變換矩陣進行座標變換
    // R * v + t
    Eigen::Vector3d v_transformed = T * v;
    cout << "v tranformed = " << v_transformed.transpose() << endl;

    // 仿射和射影變換，`Eigen::Affine3d`和`Eigen::Projective3d`

    // 四元數
    // `AngleAxis`可直接賦值四元數，反之亦然
    Eigen::Quaterniond q = Eigen::Quaterniond(rotation_vector);
    // coeffs的順序是`(x,y,z,w)`，`w`爲實部，前三者爲虛部
    cout << "quaternion = \n" << q.coeffs() << endl;
    // 旋轉矩陣賦值四元數
    q = Eigen::Quaterniond(rotation_matrix);
    cout << "quaternion = \n" << q.coeffs() << endl;

    // 使用四元數旋轉一個向量，用重載的乘法
    // 數學表達式爲｀qvq^{-1}｀
    v_rotated = q * v;
    cout << "(1,0,0) after rotation = " << v_rotated.transpose() << endl;


    return 0;
}

# CMakeLists.txt

# 聲明要求的 cmake 最低版本
cmake_minimum_required( VERSION 2.8 )

# 聲明一個 cmake 工程
project( Eigen )

# 添加頭文件
include_directories( "/usr/include/eigen3" )

# 添加一個可執行程序
# 語法：add_executable( 程序名 源代碼文件 ）
add_executable( use_geometry use_geometry.cpp )

# 設置編譯模式
set( CMAKE_BUILD_TYPE "Debug" )

p.s.

scipy部分實現了Eigen

scipy.spatial.transform.Rotation(quat, normalize=True, copy=True)類：實現3D旋轉

• 接口：

  • 四元數（quaternion）

  • 旋轉矩陣（rotation matrix）

  • 旋轉向量（rotation vector）

  • 歐拉角（Euler angle）

• 支持：

  • Application on vectors

  • Rotation Composition

  • Rotation Inversion

  • Rotation Indexing

from scipy.spatial.transform import Rotation as R
import numpy as np

# 沿Z軸旋轉45度
# 旋轉向量
r = R.from_rotvec(np.pi / 4 * np.array([0, 0, 1]))

print(r.as_matrix())

[[ 0.70710678 -0.70710678  0.        ]
 [ 0.70710678  0.70710678  0.        ]
 [ 0.          0.          1.        ]]

v = np.array([1, 0, 0])

# 旋轉矩陣
rotation_matrix = r.as_matrix()
rotation_vector = r.as_rotvec()

v_rotated = np.matmul(rotation_matrix, v)
print("(1,0,0) after rotation = ", v_rotated.T)

v_rotated = r.apply(v)
print("(1,0,0) after rotation = ", v_rotated.T)

(1,0,0) after rotation =  [0.70710678 0.70710678 0.        ]
(1,0,0) after rotation =  [0.70710678 0.70710678 0.        ]

# 歐拉角
euler_angles = r.as_euler(seq="zyx", degrees=True)

print("yaw pitch roll = ", euler_angles)

yaw pitch roll =  [45.  0.  0.]