前言

本篇继续参照高翔老师《视觉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

附件包含了第三讲所有代码。
后续会介绍刚体运动第二部分：旋转向量和欧拉角，以及第三部分：四元数表示旋转。请继续学习，欢迎留言讨论，你的关注是我更新下去的动力。

shao918516

发布了7 篇原创文章 · 获赞 18 · 访问量 2165

私信关注

三维空间刚体运动1：旋转矩阵与变换矩阵