三維空間剛體運動1：旋轉矩陣與變換矩陣

前言

本篇繼續參照高翔老師《視覺SLAM十四講從理論到實踐》，講解三維空間剛體運動。博文將原第三講分爲四部分來講解：1、旋轉矩陣和變換矩陣；2、旋轉向量表示旋轉；3、歐拉角表示旋轉；4、四元數表示變換。本文相對於原文會適當精簡，同時爲便於理解，會加入一些註解和補充知識點，本篇爲第一部分：旋轉矩陣和變換矩陣，另外三部分請參照博主的其他博文。

在正式開始之前，我想先分享學習體會。之前看SLAM，看到第六講放棄了，無他，前邊理解的不深刻，後邊的越來越難以理解，學了一本強化學習之後，才靜下心繼續學SLAM。所以在此建議SLAM小夥伴們，高翔博士該講的都在書裏，只不過太過精簡，不怕各位笑話，第三講和第四講反反覆覆來回看了四遍。所以學習SLAM的關鍵，就是溫故而知新，多多體會總結，串聯起前後相關的知識點，融會貫通才能理解後邊的內容。當然，那些極其聰明的大神除外。

本博文首先介紹向量及其座標表示，並介紹了向量間的運算；然後，使用歐式變換描述座標系之間的運動，它由旋轉和平移組成，旋轉由旋轉矩陣$SO(3)$描述，而平移直接由一個$\mathbb{R}^{3}$向量描述；最後，如果將旋轉和平移放在一個矩陣中，就形成了變換矩陣$SE(3)$，陌生符號會在下文講解。

1. 點、向量和座標系

這裏講一下剛體、點、向量、座標和座標系、內積和外積的概念，爲了引出$a^{\wedge }$。

剛體：剛體是形狀和大小不發生變化的物體，我們日常生活的空間是三維的，所以一個空間點的位置可以由3個座標指定，而剛體不光有位置，還有自身的姿態，姿態是指物體的朝向。
點：點是空間中的基本元素，沒有長度沒有體積，兩個點連接起來，構成了向量。
向量：可以看成從某點指向另一點的箭頭，他是空間中的一樣東西，向量在座標系中表示爲座標，同一向量在不同座標系中的座標不同。
座標：假設在線性空間中，找到了該空間的一組基（就是張成這個空間的一組線性無關的向量，也稱爲基底），記爲$(e_{1},e_{2},e_{3})$，那麼任意向量$a$在這組基下就有一個座標：
$a = [e_{1},e_{2},e_{3}]\begin{bmatrix} a_{1}\\ a_{2}\\ a_{3} \end{bmatrix} = a_{1}e_{1} + a_{2}e_{2} + a_{3}e_{3}. \tag{1.1}$這裏$(a_{1},a_{2},a_{3})^{T }$稱爲$a$在此基下的座標。座標的具體取值，一是和向量本身有關，二是和座標系（基）的選取有關。注意：本文的向量均爲列向量，與一般數學書籍相同。
座標系：通常由3個正交的座標軸組成，當給定$x$和$y$軸，$z$軸就可以通過右手（或左手）法則由$x \times y$定義出來。根據定義方式不同，又分爲左手系和右手系。右手系中，大拇指指向$x$軸正向，食指指向$y$軸正向，中指所指方向即爲$z$軸方向。大部分3D程序庫使用右手系（如OpenGL、3D Max等），也有部分庫使用左手系（如Unity、Direct3D等）。
內積：向量的數乘、加減法不再贅述。通常意義下的內積可以寫成：$a\cdot b= a^{T}b= \sum_{i=1}^{3}a_{i}b_{}i= \left | a \right |\left | b \right |cos \left \langle a,b \right \rangle. \tag{1.2}$其中$\left \langle a,b \right \rangle$指向量$a,b$的夾角。內積也可以描述向量間的投影關係。
外積：外積是這個樣子：$a\times b= \begin{Vmatrix} e_{1} & e_{2} & e_{3}\\ a_{1} & a_{2} & a_{3}\\ b_{1} & b_{2} & b_{3} \end{Vmatrix}= \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}b \xlongequal[]{def} a^{\wedge }b. \tag{1.3}$外積的結果是一個向量，它的方向垂直於這兩個向量，大小爲$\left | a \right |\left | b \right |sin \left \langle a,b \right \rangle$，是兩個向量張成的四邊形的有向面積。對於外積運算，引入$^{\wedge }$符號，可以把$a$寫成一個矩陣，它是一個反對稱矩陣（$A^{T}=-A$）。你可以將$^{\wedge }$記成一個反對稱符號，讀作hat，這樣就把外積$a\times b$寫成了矩陣與向量的乘法$a^{\wedge}b$，把它變成了線性運算。這個符號非常重要，會經常用到，並且此符號是一個一一映射，意味着任意向量都對應着唯一的一個反對稱矩陣，反之亦然：$a^{\wedge }= \begin{bmatrix} 0 & -a_{3} & a_{2}\\ a_{3} & 0 & -a_{1}\\ -a_{2} & a_{1} & 0 \end{bmatrix}. \tag{1.4}$

2.座標系間的歐式變換

此節是整篇甚至整本書的重中之重，請重點要理解掌握。博主也會極力詳細講清楚。首先，由剛體運動引出歐式變換。

我們經常在實際場景中定義各種各樣的座標系，如果考慮運動的機器人（即相機），那麼常見的做法是設定一個慣性座標系（或者叫世界座標系），可以認爲它是固定不動的。這時就會有這樣的疑問：相機視野中某個向量p，它在相機座標系下的座標爲$p_{c}$，而在世界座標系下看，其座標爲$p_{w}$，那麼，這兩個座標之間是如何轉換的呢？這時，需要先得到該點針對機器人座標系的座標值，再根據機器人位姿變換到世界座標系中，可以通過數學手段的變換矩陣$T$來描述它。

剛體運動：兩個座標系之間的運動變換由一個旋轉加上一個平移組成，這種運動就是剛體運動。相機運動就是一個剛體運動。剛體運動過程中，同一個向量在各個座標系下的長度和夾角都不會發生變化。此時，我們說手機座標系和世界座標系之間，相差了一個歐式變換（Euclidean Transform）。歐式變換由旋轉和平移組成。

2.1 旋轉

我們首先考慮旋轉。由旋轉引出旋轉矩陣和特殊正交羣$SO(n)$。
旋轉矩陣：設某個單位正交基$e=(e_{1},e_{2},e_{3})$經過一次旋轉變成了$e{}'=(e_{1}{}',e_{2}{}',e_{3}{}')$。那麼，對於同一個向量$a$，它在兩個座標系下的座標爲$[a_{1},a_{2},a_{3}]$和$[a_{1}{}',a_{2}{}',a_{3}{}']$，因爲向量本身沒變，所以根據座標定義，有：$[e_{1},e_{2},e_{3}]\begin{bmatrix} a_{1}\\ a_{2}\\ a_{3} \end{bmatrix} = [e_{1}{}',e_{2}{}',e_{3}{}']\begin{bmatrix} a_{1}{}'\\ a_{2}{}'\\ a_{3}{}' \end{bmatrix} . \tag{2.1}$爲了描述兩個座標之間的關係，對上式兩邊同時左乘$e^{T}$，那麼左側係數變爲單位矩陣，所以：$\begin{bmatrix} a_{1}\\ a_{2}\\ a_{3} \end{bmatrix} = \begin{bmatrix} e_{1}^{T}e_{1}{}' & e_{1}^{T}e_{2}{}' & e_{1}^{T}e_{3}{}'\\ e_{2}^{T}e_{1}{}' & e_{2}^{T}e_{2}{}' & e_{2}^{T}e_{3}{}'\\ e_{3}^{T}e_{1}{}' & e_{3}^{T}e_{2}{}' & e_{3}^{T}e_{3}{}' \end{bmatrix}\begin{bmatrix} a_{1}{}'\\ a_{2}{}'\\ a_{3}{}' \end{bmatrix} \xlongequal{def} \mathbf{R}a{}'. \tag{2.2}$矩陣$\mathbf{R}$由兩組基的內積組成，刻畫了旋轉前後同一個向量的座標變換關係，矩陣$\mathbf{R}$描述了旋轉本身，因此稱爲旋轉矩陣（Rotation Matrix）。同時，該矩陣各分量是兩個座標系基的內積，所以實際上是各基向量夾角的餘弦值，故也叫方向餘弦矩陣（Direction Cosine Matrix）。
同時，旋轉矩陣$\mathbf{R}$也是正交矩陣，它的逆（即轉職）描述了一個相反的旋轉。按照上面的定義方式，有：$a{}'=\mathbf{R}^{-1}a=\mathbf{R}^{T}a. \tag{2.3}$顯然，$\mathbf{R}^{-1}$和$\mathbf{R}^{T}$刻畫了一個相反的旋轉。

特殊正交羣$SO(n)$：旋轉矩陣$R$是一個行列式爲1的正交矩陣（即$A^{-1} = A^{T}$），反之，行列式爲1的正交矩陣也是一個旋轉矩陣。所以，可以將$n$維旋轉矩陣的集合定義如下：$SO(n)= \left \{ {\mathbf{R}\in \mathbb{R}^{n\times n}|\mathbf{R}\mathbf{R}^{T}= \mathbf{I},det(\mathbf{R})= 1} \right \}. \tag{2.4}$$SO(n)$是特殊正交羣（Special Orthogonal Group）的意思。這個集合由$n$維空間的旋轉矩陣，特別的，$SO(n)$就是指三維空間的旋轉。通過旋轉矩陣，可以直接談論兩個座標系之間的旋轉變換，而不用再從基談起。

2.2 平移

在歐式變換中，除了旋轉還有平移。
考慮世界座標系中的向量$a$，經過一次旋轉矩陣$R$和一個平移向量$t$後，得到$a{}'$，那麼把旋轉和平移合到一起，有：$\mathbf{a{}' }= \mathbf{R}\mathbf{a} + \mathbf{t}. \tag{2.5}$通過上式，我們用一個旋轉矩陣$R$和一個平移向量$t$完整的描述了一個歐式空間的座標變換。

同時，這裏對下標做一下說明。實際當中，我們會定義座標系1，座標系2，那麼向量$a$在兩個座標系下的座標爲$a_{1},a_{2}$，它們之間的關係應該是：$a_{1} = R_{12}a_{2}+t_{12}. \tag{2.6}$這裏的$R_{12}$是指“把座標系2的向量變換到座標系1”，即“從2到1的旋轉矩陣”。由於向量乘在矩陣的右邊，所以它的下標是從右讀到左的。關於平移向量$t_{12}$，它實際對應的是座標系1原點指向座標系2原點的向量，在座標系1下取的座標，所以建議讀者把它記作“從1到2的向量”，但它並不等於$-t_{21}$。

3.齊次座標和變換矩陣

對於式（2.5）所表達的歐式空間的旋轉和平移還存在一個問題：這裏的變換關係是一個線性關係。假設我們進行了兩次變換：$R_{1},t_{1}$和$R_{2},t_{2}$：$b = R_{1}a+t_{1}, c = R_{2}b+t_{2}. \tag{3.1}$那麼，從$a$到$c$的變換爲：$c = R_{2}(R_{1}a+t_{1})+t_{2}.\tag{3.2}$這樣的形式在變換多次之後會顯得很囉嗦。因此引入齊次座標和變換矩陣。

齊次座標：這裏使用一個數學技巧：我們在一個三維向量的末尾添加1，將其變爲四維向量$\tilde{a}= \begin{bmatrix} a\\ 1 \end{bmatrix}$，稱爲齊次座標。齊次座標表示法就是用$n+1$維向量表示一個$n$維向量。
$n$維空間中的點的位置向量用非齊次座標表示爲$(P_{1}, P_{2}...P_{n})$，它具有$n$個分量且唯一。使用齊次座標表示時，表示爲$(hP_{1}, hP_{2}...hP_{n},h)，$該向量有$n+1$個座標分量且不唯一。
對於h，通常使$h=1$。如果$h\neq 1$且$h\neq 0$，使用h除以齊次座標各分量，這一方法稱爲齊次座標的規範化。如果$h=0$，該點表示一個無窮遠點。三元組$(0,0,0)$不表示任何點。原點表示爲$(0,0,0,1)$。

變換矩陣：對於齊次座標，我們可以把旋轉和平移寫在一個矩陣裏，使得整個關係變成線性關係：$\tilde{a}= \begin{bmatrix} a{}'\\ 1 \end{bmatrix}= \begin{bmatrix} R & t\\ 0^{T} & 1 \end{bmatrix}\begin{bmatrix} a\\ 1 \end{bmatrix} \xlongequal{def} T\begin{bmatrix} a\\ 1 \end{bmatrix} = \begin{bmatrix} Ra+t\\ 1 \end{bmatrix}. \tag{3.3}$在該式中，矩陣$T$稱爲變換矩陣（Transform Matrix）。

那麼依靠齊次座標和變換矩陣，兩次變換的疊加就可以有很好的形式：$\tilde{b}= T_{1}\tilde{a}, \tilde{c}= T_{2}\tilde{b} \Rightarrow \tilde{c}= T_{2}T_{1}\tilde{a} . \tag{3.4}$但是區分齊次和非齊次座標的符號令我們厭煩，所以，在不引起歧義的情況下，以後直接把它寫成$b=Ta$的樣子，默認其中進行了齊次座標的轉換。

特殊歐式羣$SE(3)$：對於變換矩陣T，它具有比較特別的結構：左上角爲旋轉矩陣，右上角爲平移向量，左下角爲$0$向量，右下角爲1。這種矩陣又稱爲特殊歐式羣（Special Euclidean Group）:$SE(3)= \left \{ T= \begin{bmatrix} R & t\\ 0^{T} & 0 \end{bmatrix} \in \mathbb{R}^{4\times 4}|R\in SO(3), t\in \mathbb{R}^{3}\right \}.\tag{3.5}$與$SO(3)$一樣，求解該矩陣的逆$T^{-1}$，表示一個反向的變換：$T^{-1}= \begin{bmatrix} R^{T} & -R^{T}t\\ 0^{T} & 1 \end{bmatrix}. \tag{3.6}$同樣，我們用$T_{12}$這樣的寫法表示從2到1的變換。在不引起歧義的情況下，以後不可以區別齊次座標與普通座標的符號，默認使用的是符合運算法則的那一種，因爲齊次座標與非齊次座標之間的轉換事實上非常容易。

4.實踐：Eigen

本節講解如何使用Eigen表示矩陣和向量，隨後引申至旋轉矩陣與變換矩陣的運算。KDevelop工程形式的代碼在附件中。

Eigen:Eigen是一個C++開源線性代數庫，它提供了快速的有關矩陣的線性代數運算，還包括解方程等功能。許多上層的軟件庫也使用Eigen進行矩陣運算，包括g2o、Sopus等。與其他庫相比，Eigen的特殊之處在於，它是一個純用頭文件搭建起來的庫，這意味着你只能找到它的頭文件，而沒有類似.so或.a的二進制文件。在使用時，只需引入頭文件即可，不需要鏈接庫文件。例程只是介紹了基本的矩陣運算，你可以通過Eigen官網教程學習更多Eigen知識。

如果沒有安裝Eigen，請輸入以下命令進行安裝：

sudo apt install libeigen3-dev

下面寫一段代碼來實際練習Eigen的使用（已添加註釋）：

#include<iostream>
using namespace std;

#include<ctime>
#include<eigen3/Eigen/Core>
#include<eigen3/Eigen/Dense>  //稠密矩陣的代數運算，如逆、特徵值等
using namespace Eigen;

#define MATRIX_SIZE 50

int main(int argc, char **argv){
    //Eigen中所有向量和矩陣都是Eigen::Matrix，它是一個模板類，前三個參數爲數據類型、行、列。下式爲聲明一個2*3的float矩陣
    Matrix<float, 2, 3> matrix_23f;
    //同時，Eigen通過typedef提供了許多內置類型，不過底層仍是Eigen::Matrix，例如Vector3d實質上是Eigen::Matrix<double, 3, 1>，即三維向量。
    Vector3d v_3d;
    Matrix<float, 3, 1> matrix_31f;
    
    Matrix3d matrix_33d = Matrix3d::Zero();
    //如果不確定大小，可使用動態大小的矩陣，Matrix<double, Dynamic, Dynamic>與MatrixXd相同。
    Matrix<double, Dynamic, Dynamic> matrix_dynamic;
    MatrixXd matrix_x;
    
    //下面是對Eigen矩陣的操作
    
    //輸入數據進行初始化
    matrix_23f<<1,2,3,4,5,6;
    cout<<"matrix 2*3 from 1 to 6:\n"<<matrix_23f<<endl;
    
    //用（）訪問矩陣中的元素
    cout<<"print matrix 2*3:"<<endl;
    for (int i=0; i<2; i++) {
        for (int j=0; j<3; j++) {
            cout<<matrix_23f(i, j)<<"\t";
        }
        cout<<endl;
    }
    
    v_3d << 3,2,1;
    matrix_31f<<4,5,6;
    
    //在Eigen中，不能混合兩種不同類型的矩陣，必須進行顯式轉換。同樣，不能搞混維度
    Matrix<double, 2, 1> result = matrix_23f.cast<double>() * v_3d;
    cout<<"[1,2,3;4,5,6]*[3,2,1]="<<result.transpose()<<endl;
    
    Matrix<float, 2, 1> result2 = matrix_23f * matrix_31f;
    cout<<"[1,2,3;4,5,6]*[4,5,6]="<<result2.transpose()<<endl;

    //同樣，不能搞混維度,下面是個錯誤例子。當你在編譯程序，出現莫名其妙的錯誤時，請首先仔細檢查你所進行運算矩陣的維度，這點相當重要。
    //Eigen::Matrix<double, 2, 3> result_wrong_dimension = matrix_23f.cast<double>()*v_31d;
    
    //一些矩陣運算
    matrix_33d = Matrix3d::Random();    //隨機數矩陣
    cout<<"random matrix: \n"<<matrix_33d<<endl;
    cout<<"transpose: \n"<<matrix_33d.transpose()<<endl;    //轉置
    cout<<"sum: "<<matrix_33d.sum()<<endl;    //各元素和
    cout<<"trace: "<<matrix_33d.trace()<<endl;    //跡
    cout<<"times 10: \n"<<10 * matrix_33d<<endl;    //數乘
    cout<<"inverse: \n"<<matrix_33d.inverse()<<endl;    //逆
    cout<<"det: "<<matrix_33d.determinant()<<endl;    //行列式
    
    //特徵值和特徵向量，實對稱矩陣可保證對角化成功。
    SelfAdjointEigenSolver<Matrix3d> eigen_solver(matrix_33d.transpose()*matrix_33d);
    cout<<"Eigen values=\n"<<eigen_solver.eigenvalues()<<endl;
    cout<<"Eigen vectors=\n"<<eigen_solver.eigenvectors()<<endl;
    
    //解方程，這裏求解方程matrix_NN * x = v_N1d
    Matrix<double, MATRIX_SIZE, MATRIX_SIZE> matrix_NN = MatrixXd::Random(MATRIX_SIZE, MATRIX_SIZE);
    matrix_NN = matrix_NN * matrix_NN.transpose();
    Matrix<double, MATRIX_SIZE, 1> v_N1d = MatrixXd::Random(MATRIX_SIZE, 1);
    
    //計時
    clock_t time_stt = clock();
    //直接求逆，運算量大
    Matrix<double, MATRIX_SIZE, 1> x = matrix_NN.inverse()*v_N1d;
    cout<<"time of normal inverse is "<<1000*(clock()-time_stt)/(double)CLOCKS_PER_SEC<<"ms"<<endl;
    cout<<"x = "<<x.transpose()<<endl;
    
    time_stt = clock();
    //通常用矩陣分解來求解，例如QR分解，速度會快很多
    x = matrix_NN.colPivHouseholderQr().solve(v_N1d);
    cout<<"time of Qr decomposition is "<<1000*(clock()-time_stt)/(double)CLOCKS_PER_SEC<<"ms"<<endl;
    cout<<"x = "<<x.transpose()<<endl;
    
    time_stt = clock();
    //對於正定矩陣，還可以用cholesky分解來解方程
    x = matrix_NN.ldlt().solve(v_N1d);
    cout<<"time of ldlt decomposition is "<<1000*(clock()-time_stt)/(double)CLOCKS_PER_SEC<<"ms"<<endl;
    cout<<"x = "<<x.transpose()<<endl;
    
    time_stt = clock();
    //此外還有lu分解
    x = matrix_NN.lu().solve(v_N1d);
    cout<<"time of lu decomposition is "<<1000*(clock()-time_stt)/(double)CLOCKS_PER_SEC<<"ms"<<endl;
    cout<<"x = "<<x.transpose()<<endl;
}

CMakeLists.txt文件內容如下：

cmake_minimum_required(VERSION 3.0)
project(rigidMotion)
add_executable(useEigen useEigen.cpp)
set(CMAKE_BUILD_TYPE "Debug")

編譯好程序後，運行它，可以看到各矩陣運算結果如下：

matrix 2*3 from 1 to 6:
1 2 3
4 5 6
print matrix 2*3:
1	2	3	
4	5	6	
[1,2,3;4,5,6]*[3,2,1]=10 28
[1,2,3;4,5,6]*[4,5,6]=32 77
random matrix: 
 0.680375   0.59688 -0.329554
-0.211234  0.823295  0.536459
 0.566198 -0.604897 -0.444451
transpose: 
 0.680375 -0.211234  0.566198
  0.59688  0.823295 -0.604897
-0.329554  0.536459 -0.444451
sum: 1.61307
trace: 1.05922
times 10: 
 6.80375   5.9688 -3.29554
-2.11234  8.23295  5.36459
 5.66198 -6.04897 -4.44451
inverse: 
-0.198521   2.22739    2.8357
  1.00605 -0.555135  -1.41603
 -1.62213   3.59308   3.28973
det: 0.208598
Eigen values=
0.0242899
 0.992154
  1.80558
Eigen vectors=
-0.549013 -0.735943  0.396198
 0.253452 -0.598296 -0.760134
-0.796459  0.316906 -0.514998
time of normal inverse is 1.967ms
x = -55.7896 -298.793  130.113 -388.455 -159.312  160.654 -40.0416 -193.561  155.844  181.144  185.125 -62.7786  19.8333 -30.8772 -200.746  55.8385 -206.604  26.3559 -14.6789  122.719 -221.449   26.233  -318.95 -78.6931  50.1446  87.1986 -194.922  132.319  -171.78 -4.19736   11.876 -171.779  48.3047  84.1812 -104.958 -47.2103 -57.4502 -48.9477 -19.4237  28.9419  111.421  92.1237 -288.248 -23.3478  -275.22 -292.062  -92.698  5.96847 -93.6244  109.734
time of Qr decomposition is 2.409ms
x = -55.7896 -298.793  130.113 -388.455 -159.312  160.654 -40.0416 -193.561  155.844  181.144  185.125 -62.7786  19.8333 -30.8772 -200.746  55.8385 -206.604  26.3559 -14.6789  122.719 -221.449   26.233  -318.95 -78.6931  50.1446  87.1986 -194.922  132.319  -171.78 -4.19736   11.876 -171.779  48.3047  84.1812 -104.958 -47.2103 -57.4502 -48.9477 -19.4237  28.9419  111.421  92.1237 -288.248 -23.3478  -275.22 -292.062  -92.698  5.96847 -93.6244  109.734
time of ldlt decomposition is 0.667ms
x = -55.7896 -298.793  130.113 -388.455 -159.312  160.654 -40.0416 -193.561  155.844  181.144  185.125 -62.7786  19.8333 -30.8772 -200.746  55.8385 -206.604  26.3559 -14.6789  122.719 -221.449   26.233  -318.95 -78.6931  50.1446  87.1986 -194.922  132.319  -171.78 -4.19736   11.876 -171.779  48.3047  84.1812 -104.958 -47.2103 -57.4502 -48.9477 -19.4237  28.9419  111.421  92.1237 -288.248 -23.3478  -275.22 -292.062  -92.698  5.96847 -93.6244  109.734
time of lu decomposition is 0.787ms
x = -55.7896 -298.793  130.113 -388.455 -159.312  160.654 -40.0416 -193.561  155.844  181.144  185.125 -62.7786  19.8333 -30.8772 -200.746  55.8385 -206.604  26.3559 -14.6789  122.719 -221.449   26.233  -318.95 -78.6931  50.1446  87.1986 -194.922  132.319  -171.78 -4.19736   11.876 -171.779  48.3047  84.1812 -104.958 -47.2103 -57.4502 -48.9477 -19.4237  28.9419  111.421  92.1237 -288.248 -23.3478  -275.22 -292.062  -92.698  5.96847 -93.6244  109.734

附件包含了第三講所有代碼。
後續會介紹剛體運動第二部分：旋轉向量和歐拉角，以及第三部分：四元數表示旋轉。請繼續學習，歡迎留言討論，你的關注是我更新下去的動力。