三维空间刚体运动1：旋转矩阵与变换矩阵（详解加代码示例）

阅读量：684 次

发布时间：2019-03-17

本文共 15660 字，大约阅读时间需要 52 分钟。

三维空间刚体运动1：旋转矩阵与变换矩阵（详解加代码示例）

序：本篇系列文章参照高翔老师《视觉SLAM十四讲从理论到实践》，讲解三维空间刚体运动，为读者打下坚实的数学基础。博文将原第三讲分为五部分来讲解，其中四元数部分较多较复杂，又分为四部分。如果读者急于实践，可直接阅读第五部分的机器人运动轨迹，此部分详细讲解了安装准备工作。此系列总体目录如下：

四元数包括以下部分：
4-1. ；
4-2. ；
4-3. ；
4-4. ；
4-5. 。

在正式开始之前，我想先分享学习体会。之前看SLAM，看到第六讲放弃了，无他，前边理解的不深刻，后边的越来越难以理解，学了一本强化学习之后，才静下心继续学SLAM。所以在此建议SLAM小伙伴们，高翔博士该讲的都在书里，只不过太过精简，不怕各位笑话，第三讲和第四讲反反复复来回看了四遍。所以学习SLAM的关键，就是温故而知新，多多体会总结，串联起前后相关的知识点，融会贯通才能理解后边的内容。

本博文首先介绍向量及其坐标表示，并介绍了向量间的运算；然后，使用欧式变换描述坐标系之间的运动，它由旋转和平移组成，旋转由旋转矩阵 $S O (3)$ 描述，而平移直接由一个 $\mathbb{R}^{3}$ 向量描述；最后，如果将旋转和平移放在一个矩阵中，就形成了变换矩阵 $S E (3)$ ，陌生符号会在下文讲解。最后在欧氏变换基础上，讲解了相似、仿射和射影变换。

1. 点、向量和坐标系

这里讲一下刚体、点、向量、坐标和坐标系、内积和外积的概念，为了引出 $a^{\wedge }$ 。

刚体：刚体是形状和大小不发生变化的物体，我们日常生活的空间是三维的，所以一个空间点的位置可以由3个坐标指定，而刚体不光有位置，还有自身的姿态，姿态是指物体的朝向。

点：点是空间中的基本元素，没有长度没有体积，两个点连接起来，构成了向量。

向量：可以看成从某点指向另一点的箭头，他是空间中的一样东西，向量在坐标系中表示为坐标，同一向量在不同坐标系中的坐标不同。

坐标：假设在线性空间中，找到了该空间的一组基（就是张成这个空间的一组线性无关的向量，也称为基底），记为

e_{1},e_{2},e_{3})

，那么任意向量

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

轴就可以通过右手（或左手）法则由

\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 旋转

我们首先考虑旋转。由旋转引出旋转矩阵和特殊正交群 $S O (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}

刻画了一个相反的旋转。

特殊正交群 $S O (n)$ ：旋转矩阵 $R$ 是一个行列式为1的正交矩阵（即 $A^{-1} = A^{T}$ ），反之，行列式为1的正交矩阵也是一个旋转矩阵。所以，可以将 $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}$

R∈Rn×n∣RRT=I,det(R)=1}.(2.4)

S O (n)

是特殊正交群（Special Orthogonal Group）的意思。这个集合由

n

维空间的旋转矩阵，特别的，

S O (3)

就是指三维空间的旋转。通过旋转矩阵，可以直接谈论两个坐标系之间的旋转变换，而不用再从基谈起。

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}$ ： $R_{1}a+t_{1}, c = R_{2}b+t_{2}. \tag{3.1}$ 那么，从 $a$ 到 $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 = T a$ 的样子，默认其中进行了齐次坐标的转换。

特殊欧式群 $S E (3)$ ：对于变换矩阵T，它具有比较特别的结构：左上角为旋转矩阵，右上角为平移向量，左下角为 $0$ 向量，右下角为1。这种矩阵又称为特殊欧式群（Special Euclidean Group）: $\left \{ T= \begin{bmatrix} R & t\\ 0^{T} & 1 \end{bmatrix} \in \mathbb{R}^{4\times 4}|R\in SO(3), t\in \mathbb{R}^{3}\right \}.\tag{3.5}$

T=[R0Tt1]∈R4×4∣R∈SO(3),t∈R3}.(3.5)与

S O (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. 相似、仿射和射影变换

除了欧式变换，3D空间还存在其他几种变换方式，只不过欧氏变换是最简单的。它们一部分和测量几何有关，因为在之后的讲解中可能会提到，所以先罗列出来。欧氏变换保持了向量的长度和夹角，相当于我们把一个刚体原封不动地进行了移动或旋转，不改变它自身的样子。但现实中由于角度问题，总会发生畸变，所以需要相似、仿射、射影变换，它们都会改变物体的外形。它们都有类似的矩阵表示。

4.1 相似变换

相似变换比欧式变换多了一个自由度，它允许物体进行均匀缩放，其矩阵表示为： $T_{S}=\begin{bmatrix} sR & t\\ 0^{T} &1 \end{bmatrix}\tag{1.1}$

注意，旋转部分多了一个缩放因子

s

，它表示我们在对向量旋转之后，可以在

x, y, z

三个坐标上进行均匀缩放。由于含有缩放，相似变换不再保持图形的面积不变。你可以想象一个边长为1的立方体经过相似变换后，变成边长为10的立方体。

三维相似变换的集合也叫做相似变换群，记作

S i m (3)

。

4.2 仿射变换

仿射变换的矩阵形式如下： $T_{A}=\begin{bmatrix} A & t\\ 0^{T} &1 \end{bmatrix}\tag{2.1}$ 与欧氏变换不同二十，仿射变换只要求 $A$ 是一个可逆矩阵，而不必是正交矩阵。仿射变换也叫正交投影，经过仿射变换之后，立方体就不再是方的了，但是各个方面仍然是平行四边形。

4.3 射影变换

射影变换是最一般的变换，又称为投影变换。它的矩阵形式为： $T_{S}=\begin{bmatrix} A & t\\ a^{T} &v \end{bmatrix}\tag{3.1}$ 它的左上角为可逆矩阵 $A$ ，右上角为平移 $t$ ，左下角为缩放 $a^{T}$ ，右下角为整体的变换比例 $v$ 。由于采用了齐次坐标，当 $v\neq 0$ 时，我们可以对整个矩阵除以 $v$ 得到一个右下角为1的矩阵；否则当 $v = 0$ 时，得到右下角为0的矩阵。因此，2D的射影变换一共有8个自由度，3D则共有15个自由度。

射影变换是讲过的变换中，形式最一般的。从真实世界到相机照片的变换可以看成一个射影变换。读者可以想象一个原本方形的地板砖，在照片中是什么样子？首先，它不再是方形的，由于近大远小的关系，它甚至不是平行四边形，而是一个不规则的四边形。这也是位姿中常遇到的情况。

4.4 总结

下面对比总结下讲到的四种变换的性质。注意在“不变性质”中，从上到下是有包含关系的。例如，欧氏变换除了保体积，也具有保平行、相交等性质。

表4-1 常见变换的性质比较

变换名称	矩阵形式	自由度	不变性质	变换形态
欧氏变换	$T_{E}=\begin{bmatrix} R & t\\ 0^{T} &1 \end{bmatrix}$	6	长度、夹角、体积	位置，方向改变
相似变换	$T_{S}=\begin{bmatrix} sR & t\\ 0^{T} &1 \end{bmatrix}$	7	体积比	按比例缩放
仿射变换	$T_{A}=\begin{bmatrix} A & t\\ 0^{T} &1 \end{bmatrix}$	12	平行性、体积比	正交投影，平行性不变
射影变换	$T_{P}=\begin{bmatrix} A & t\\ a^{T} &v \end{bmatrix}$	15	接触平面的相交和相切	大小、平行性均发生改变

从真实世界到相机照片的变换是一个射影变换。如果相机的焦距为无穷远，那么这个变换为仿射变换。在详细学习相机模型之前，只要对它们有个大致了解即可。

5.实践：Eigen

本节讲解如何使用Eigen表示矩阵和向量，随后引申至旋转矩阵与变换矩阵的运算。KDevelop工程形式的代码在附件中。

Eigen:Eigen是一个C++开源线性代数库，它提供了快速的有关矩阵的线性代数运算，还包括解方程等功能。许多上层的软件库也使用Eigen进行矩阵运算，包括g2o、Sopus等。与其他库相比，Eigen的特殊之处在于，它是一个纯用头文件搭建起来的库，这意味着你只能找到它的头文件，而没有类似.so或.a的二进制文件。在使用时，只需引入头文件即可，不需要链接库文件。例程只是介绍了基本的矩阵运算，你可以通过学习更多Eigen知识。

如果没有安装Eigen，请输入以下命令进行安装：

sudo apt install libeigen3-dev

下面写一段代码来实际练习Eigen的使用（已添加注释）：

#include
   
    using namespace std;#include
    
     #include
     
      #include
      
         //稠密矩阵的代数运算，如逆、特征值等using namespace Eigen;#define MATRIX_SIZE 50int main(int argc, char **argv){
       //Eigen中所有向量和矩阵都是Eigen::Matrix，它是一个模板类，前三个参数为数据类型、行、列。下式为声明一个2*3的float矩阵    Matrix
       
         matrix_23f;    //同时，Eigen通过typedef提供了许多内置类型，不过底层仍是Eigen::Matrix，例如Vector3d实质上是Eigen::Matrix
        
         ，即三维向量。 Vector3d v_3d; Matrix
         
           matrix_31f; Matrix3d matrix_33d = Matrix3d::Zero(); //如果不确定大小，可使用动态大小的矩阵，Matrix
          
           与MatrixXd相同。 Matrix
           
             matrix_dynamic; MatrixXd matrix_x; //下面是对Eigen矩阵的操作 //输入数据进行初始化 matrix_23f<<1,2,3,4,5,6; cout<<"matrix 2*3 from 1 to 6:\n"<
            
             <
             
               result = matrix_23f.cast
              
               () * v_3d; cout<<"[1,2,3;4,5,6]*[3,2,1]="<
               
                <
                
                  result2 = matrix_23f * matrix_31f; cout<<"[1,2,3;4,5,6]*[4,5,6]="<
                 
                  <
                  
                    result_wrong_dimension = matrix_23f.cast
                   
                    ()*v_31d; //一些矩阵运算 matrix_33d = Matrix3d::Random(); //随机数矩阵 cout<<"random matrix: \n"<
                    
                     <
                     
                       eigen_solver(matrix_33d.transpose()*matrix_33d); cout<<"Eigen values=\n"<
                      
                       <
                       
                         matrix_NN = MatrixXd::Random(MATRIX_SIZE, MATRIX_SIZE); matrix_NN = matrix_NN * matrix_NN.transpose(); Matrix
                        
                          v_N1d = MatrixXd::Random(MATRIX_SIZE, 1); //计时 clock_t time_stt = clock(); //直接求逆，运算量大 Matrix
                         
                           x = matrix_NN.inverse()*v_N1d; cout<<"time of normal inverse is "<<1000*(clock()-time_stt)/(double)CLOCKS_PER_SEC<<"ms"<

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 34 5 6print 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 77random matrix:  0.680375   0.59688 -0.329554-0.211234  0.823295  0.536459 0.566198 -0.604897 -0.444451transpose:  0.680375 -0.211234  0.566198  0.59688  0.823295 -0.604897-0.329554  0.536459 -0.444451sum: 1.61307trace: 1.05922times 10:  6.80375   5.9688 -3.29554-2.11234  8.23295  5.36459 5.66198 -6.04897 -4.44451inverse: -0.198521   2.22739    2.8357  1.00605 -0.555135  -1.41603 -1.62213   3.59308   3.28973det: 0.208598Eigen values=0.0242899 0.992154  1.80558Eigen vectors=-0.549013 -0.735943  0.396198 0.253452 -0.598296 -0.760134-0.796459  0.316906 -0.514998time of normal inverse is 1.967msx = -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.734time of Qr decomposition is 2.409msx = -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.734time of ldlt decomposition is 0.667msx = -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.734time of lu decomposition is 0.787msx = -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

附件包含了第三讲所有代码。

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

本文基于《视觉SLAM十四讲：从理论到实践》和《Quaternions, Interpolation and Animation》编写，但相对于原文会适当精简，同时为便于全面理解，会收集其他网络好文，根据作者理解，加入一些注解和扩展知识点，如果您觉得还不错，请一键四连（点赞关注收藏评论），让更多的人看到。

参考文献：

《视觉SLAM十四讲：从理论到实践》，高翔、张涛等著，中国工信出版社

转载地址：http://akvhz.baihongyu.com/

你可能感兴趣的文章