锐客网

  1. 首页 > it技术 > >

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

slam


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

  • 前言
  • 1. 点、向量和坐标系
  • 2.坐标系间的欧式变换
    • 2.1 旋转
    • 2.2 平移
  • 3.齐次坐标和变换矩阵
  • 4.实践：Eigen

前言 本篇继续参照高翔老师《视觉SLAM十四讲从理论到实践》，讲解三维空间刚体运动。博文将原第三讲分为四部分来讲解：1、旋转矩阵和变换矩阵；2、旋转向量表示旋转；3、欧拉角表示旋转；4、四元数表示变换。本文相对于原文会适当精简，同时为便于理解，会加入一些注解和补充知识点，本篇为第一部分：旋转矩阵和变换矩阵，另外三部分请参照博主的其他博文。
在正式开始之前，我想先分享学习体会。之前看SLAM，看到第六讲放弃了，无他，前边理解的不深刻，后边的越来越难以理解，学了一本强化学习之后，才静下心继续学SLAM。所以在此建议SLAM小伙伴们，高翔博士该讲的都在书里，只不过太过精简，不怕各位笑话，第三讲和第四讲反反复复来回看了四遍。所以学习SLAM的关键，就是温故而知新，多多体会总结，串联起前后相关的知识点，融会贯通才能理解后边的内容。当然，那些极其聪明的大神除外。
本博文首先介绍向量及其坐标表示，并介绍了向量间的运算；然后，使用欧式变换描述坐标系之间的运动，它由旋转和平移组成，旋转由旋转矩阵 S O ( 3 ) SO(3) SO(3)描述，而平移直接由一个 R 3 \mathbb{R}^{3} R3向量描述；最后，如果将旋转和平移放在一个矩阵中，就形成了变换矩阵 S E ( 3 ) SE(3) SE(3)，陌生符号会在下文讲解。
1. 点、向量和坐标系 这里讲一下刚体、点、向量、坐标和坐标系、内积和外积的概念，为了引出 a ∧ a^{\wedge } a∧。
刚体：刚体是形状和大小不发生变化的物体，我们日常生活的空间是三维的，所以一个空间点的位置可以由3个坐标指定，而刚体不光有位置，还有自身的姿态，姿态是指物体的朝向。
点：点是空间中的基本元素，没有长度没有体积，两个点连接起来，构成了向量。
向量：可以看成从某点指向另一点的箭头，他是空间中的一样东西，向量在坐标系中表示为坐标，同一向量在不同坐标系中的坐标不同。
坐标：假设在线性空间中，找到了该空间的一组基（就是张成这个空间的一组线性无关的向量，也称为基底），记为 ( e 1 , e 2 , e 3 ) (e_{1},e_{2},e_{3}) (e1?,e2?,e3?)，那么任意向量 a a a在这组基下就有一个坐标：
a = [ e 1 , e 2 , e 3 ] [ a 1 a 2 a 3 ] = a 1 e 1 + a 2 e 2 + a 3 e 3 . (1.1) 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=[e1?,e2?,e3?]???a1?a2?a3?????=a1?e1?+a2?e2?+a3?e3?.(1.1)这里 ( a 1 , a 2 , a 3 ) T (a_{1},a_{2},a_{3})^{T } (a1?,a2?,a3?)T称为 a a a在此基下的坐标。坐标的具体取值，一是和向量本身有关，二是和坐标系（基）的选取有关。注意：本文的向量均为列向量，与一般数学书籍相同。
坐标系：通常由3个正交的坐标轴组成，当给定 x x x和 y y y轴， z z z轴就可以通过右手（或左手）法则由 x × y x \times y x×y定义出来。根据定义方式不同，又分为左手系和右手系。右手系中，大拇指指向 x x x轴正向，食指指向 y y y轴正向，中指所指方向即为 z z z轴方向。大部分3D程序库使用右手系（如OpenGL、3D Max等），也有部分库使用左手系（如Unity、Direct3D等）。
内积：向量的数乘、加减法不再赘述。通常意义下的内积可以写成： a ? b = a T b = ∑ i = 1 3 a i b i = ∣ a ∣ ∣ b ∣ c o s ? a , b ? . (1.2) 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} a?b=aTb=i=1∑3?ai?b?i=∣a∣∣b∣cos?a,b?.(1.2)其中 ? a , b ? \left \langle a,b \right \rangle ?a,b?指向量 a , b a,b a,b的夹角。内积也可以描述向量间的投影关系。
外积：外积是这个样子： a × b = ∥ e 1 e 2 e 3 a 1 a 2 a 3 b 1 b 2 b 3 ∥ = [ a 2 b 3 ? a 3 b 2 a 3 b 1 ? a 1 b 3 a 1 b 2 ? a 2 b 1 ] = [ 0 ? a 3 a 2 a 3 0 ? a 1 ? a 2 a 1 0 ] b = d e f a ∧ b . (1.3) 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} a×b=∥∥∥∥∥∥?e1?a1?b1??e2?a2?b2??e3?a3?b3??∥∥∥∥∥∥?=???a2?b3??a3?b2?a3?b1??a1?b3?a1?b2??a2?b1?????=???0a3??a2???a3?0a1??a2??a1?0????bdef ?a∧b.(1.3)外积的结果是一个向量，它的方向垂直于这两个向量，大小为 ∣ a ∣ ∣ b ∣ s i n ? a , b ? \left | a \right |\left | b \right |sin \left \langle a,b \right \rangle ∣a∣∣b∣sin?a,b?，是两个向量张成的四边形的有向面积。对于外积运算，引入 ∧ ^{\wedge } ∧符号，可以把 a a a写成一个矩阵，它是一个反对称矩阵（ A T = ? A A^{T}=-A AT=?A）。你可以将 ∧ ^{\wedge } ∧记成一个反对称符号，读作hat，这样就把外积 a × b a\times b a×b写成了矩阵与向量的乘法 a ∧ b a^{\wedge}b a∧b，把它变成了线性运算。这个符号非常重要，会经常用到，并且此符号是一个一一映射，意味着任意向量都对应着唯一的一个反对称矩阵，反之亦然： a ∧ = [ 0 ? a 3 a 2 a 3 0 ? a 1 ? a 2 a 1 0 ] . (1.4) a^{\wedge }= \begin{bmatrix} 0 & -a_{3} & a_{2}\\ a_{3} & 0 & -a_{1}\\ -a_{2} & a_{1} & 0 \end{bmatrix}. \tag{1.4} a∧=???0a3??a2???a3?0a1??a2??a1?0????.(1.4)
2.坐标系间的欧式变换 此节是整篇甚至整本书的重中之重，请重点要理解掌握。博主也会极力详细讲清楚。首先，由刚体运动引出欧式变换。
我们经常在实际场景中定义各种各样的坐标系，如果考虑运动的机器人（即相机），那么常见的做法是设定一个惯性坐标系（或者叫世界坐标系），可以认为它是固定不动的。这时就会有这样的疑问：相机视野中某个向量p，它在相机坐标系下的坐标为 p c p_{c} pc?，而在世界坐标系下看，其坐标为 p w p_{w} pw?，那么，这两个坐标之间是如何转换的呢？这时，需要先得到该点针对机器人坐标系的坐标值，再根据机器人位姿变换到世界坐标系中，可以通过数学手段的变换矩阵 T T T来描述它。
刚体运动：两个坐标系之间的运动变换由一个旋转加上一个平移组成，这种运动就是刚体运动。相机运动就是一个刚体运动。刚体运动过程中，同一个向量在各个坐标系下的长度和夹角都不会发生变化。此时，我们说手机坐标系和世界坐标系之间，相差了一个欧式变换（Euclidean Transform）。欧式变换由旋转和平移组成。
2.1 旋转 我们首先考虑旋转。由旋转引出旋转矩阵和特殊正交群 S O ( n ) SO(n) SO(n)。
旋转矩阵：设某个单位正交基 e = ( e 1 , e 2 , e 3 ) e=(e_{1},e_{2},e_{3}) e=(e1?,e2?,e3?)经过一次旋转变成了 e ′ = ( e 1 ′ , e 2 ′ , e 3 ′ ) e{}'=(e_{1}{}',e_{2}{}',e_{3}{}') e′=(e1?′,e2?′,e3?′)。那么，对于同一个向量 a a a，它在两个坐标系下的坐标为 [ a 1 , a 2 , a 3 ] [a_{1},a_{2},a_{3}] [a1?,a2?,a3?]和 [ a 1 ′ , a 2 ′ , a 3 ′ ] [a_{1}{}',a_{2}{}',a_{3}{}'] [a1?′,a2?′,a3?′]，因为向量本身没变，所以根据坐标定义，有： [ e 1 , e 2 , e 3 ] [ a 1 a 2 a 3 ] = [ e 1 ′ , e 2 ′ , e 3 ′ ] [ a 1 ′ a 2 ′ a 3 ′ ] . (2.1) [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} [e1?,e2?,e3?]???a1?a2?a3?????=[e1?′,e2?′,e3?′]???a1?′a2?′a3?′????.(2.1)为了描述两个坐标之间的关系，对上式两边同时左乘 e T e^{T} eT，那么左侧系数变为单位矩阵，所以： [ a 1 a 2 a 3 ] = [ 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 ′ ] [ a 1 ′ a 2 ′ a 3 ′ ] = d e f R a ′ . (2.2) \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} ???a1?a2?a3?????=???e1T?e1?′e2T?e1?′e3T?e1?′?e1T?e2?′e2T?e2?′e3T?e2?′?e1T?e3?′e2T?e3?′e3T?e3?′???????a1?′a2?′a3?′????def Ra′.(2.2)矩阵 R \mathbf{R} R由两组基的内积组成，刻画了旋转前后同一个向量的坐标变换关系，矩阵 R \mathbf{R} R描述了旋转本身，因此称为旋转矩阵（Rotation Matrix）。同时，该矩阵各分量是两个坐标系基的内积，所以实际上是各基向量夹角的余弦值，故也叫方向余弦矩阵（Direction Cosine Matrix）。
同时，旋转矩阵 R \mathbf{R} R也是正交矩阵，它的逆（即转职）描述了一个相反的旋转。按照上面的定义方式，有： a ′ = R ? 1 a = R T a . (2.3) a{}'=\mathbf{R}^{-1}a=\mathbf{R}^{T}a. \tag{2.3} a′=R?1a=RTa.(2.3)显然， R ? 1 \mathbf{R}^{-1} R?1和 R T \mathbf{R}^{T} RT刻画了一个相反的旋转。
特殊正交群 S O ( n ) SO(n) SO(n)：旋转矩阵 R R R是一个行列式为1的正交矩阵（即 A ? 1 = A T A^{-1} = A^{T} A?1=AT），反之，行列式为1的正交矩阵也是一个旋转矩阵。所以，可以将 n n n维旋转矩阵的集合定义如下： S O ( n ) = { R ∈ R n × n ∣ R R T = I , d e t ( R ) = 1 } . (2.4) 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)={R∈Rn×n∣RRT=I,det(R)=1}.(2.4) S O ( n ) SO(n) SO(n)是特殊正交群（Special Orthogonal Group）的意思。这个集合由 n n n维空间的旋转矩阵，特别的， S O ( n ) SO(n) SO(n)就是指三维空间的旋转。通过旋转矩阵，可以直接谈论两个坐标系之间的旋转变换，而不用再从基谈起。
2.2 平移 在欧式变换中，除了旋转还有平移。
考虑世界坐标系中的向量 a a a，经过一次旋转矩阵 R R R和一个平移向量 t t t后，得到 a ′ a{}' a′，那么把旋转和平移合到一起，有： a ′ = R a + t . (2.5) \mathbf{a{}' }= \mathbf{R}\mathbf{a} + \mathbf{t}. \tag{2.5} a′=Ra+t.(2.5)通过上式，我们用一个旋转矩阵 R R R和一个平移向量 t t t完整的描述了一个欧式空间的坐标变换。
同时，这里对下标做一下说明。实际当中，我们会定义坐标系1，坐标系2，那么向量 a a a在两个坐标系下的坐标为 a 1 , a 2 a_{1},a_{2} a1?,a2?，它们之间的关系应该是： a 1 = R 12 a 2 + t 12 . (2.6) a_{1} = R_{12}a_{2}+t_{12}. \tag{2.6} a1?=R12?a2?+t12?.(2.6)这里的 R 12 R_{12} R12?是指“把坐标系2的向量变换到坐标系1”，即“从2到1的旋转矩阵”。由于向量乘在矩阵的右边，所以它的下标是从右读到左的。关于平移向量 t 12 t_{12} t12?，它实际对应的是坐标系1原点指向坐标系2原点的向量，在坐标系1下取的坐标，所以建议读者把它记作“从1到2的向量”，但它并不等于 ? t 21 -t_{21} ?t21?。
3.齐次坐标和变换矩阵 对于式（2.5）所表达的欧式空间的旋转和平移还存在一个问题：这里的变换关系是一个线性关系。假设我们进行了两次变换： R 1 , t 1 R_{1},t_{1} R1?,t1?和 R 2 , t 2 R_{2},t_{2} R2?,t2?： b = R 1 a + t 1 , c = R 2 b + t 2 . (3.1) b = R_{1}a+t_{1}, c = R_{2}b+t_{2}. \tag{3.1} b=R1?a+t1?,c=R2?b+t2?.(3.1)那么，从 a a a到 c c c的变换为： c = R 2 ( R 1 a + t 1 ) + t 2 . (3.2) c = R_{2}(R_{1}a+t_{1})+t_{2}.\tag{3.2} c=R2?(R1?a+t1?)+t2?.(3.2)这样的形式在变换多次之后会显得很啰嗦。因此引入齐次坐标和变换矩阵。
齐次坐标：这里使用一个数学技巧：我们在一个三维向量的末尾添加1，将其变为四维向量 a ~ = [ a 1 ] \tilde{a}= \begin{bmatrix} a\\ 1 \end{bmatrix} a~=[a1?]，称为齐次坐标。齐次坐标表示法就是用 n + 1 n+1 n+1维向量表示一个 n n n维向量。
n n n维空间中的点的位置向量用非齐次坐标表示为 ( P 1 , P 2 . . . P n ) (P_{1}, P_{2}...P_{n}) (P1?,P2?...Pn?)，它具有 n n n个分量且唯一。使用齐次坐标表示时，表示为 ( h P 1 , h P 2 . . . h P n , h ) ， (hP_{1}, hP_{2}...hP_{n},h)， (hP1?,hP2?...hPn?,h)，该向量有 n + 1 n+1 n+1个坐标分量且不唯一。
对于h，通常使 h = 1 h=1 h=1。如果 h ≠ 1 h\neq 1 h?=1且 h ≠ 0 h\neq 0 h?=0，使用h除以齐次坐标各分量，这一方法称为齐次坐标的规范化。如果 h = 0 h=0 h=0，该点表示一个无穷远点。三元组 ( 0 , 0 , 0 ) (0,0,0) (0,0,0)不表示任何点。原点表示为 ( 0 , 0 , 0 , 1 ) (0,0,0,1) (0,0,0,1)。
变换矩阵：对于齐次坐标，我们可以把旋转和平移写在一个矩阵里，使得整个关系变成线性关系： a ~ = [ a ′ 1 ] = [ R t 0 T 1 ] [ a 1 ] = d e f T [ a 1 ] = [ R a + t 1 ] . (3.3) \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} a~=[a′1?]=[R0T?t1?][a1?]def T[a1?]=[Ra+t1?].(3.3)在该式中，矩阵 T T T称为变换矩阵（Transform Matrix）。
那么依靠齐次坐标和变换矩阵，两次变换的叠加就可以有很好的形式： b ~ = T 1 a ~ , c ~ = T 2 b ~ ? c ~ = T 2 T 1 a ~ . (3.4) \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~=T1?a~,c~=T2?b~?c~=T2?T1?a~.(3.4)但是区分齐次和非齐次坐标的符号令我们厌烦，所以，在不引起歧义的情况下，以后直接把它写成 b = T a b=Ta b=Ta的样子，默认其中进行了齐次坐标的转换。
【三维空间刚体运动1（旋转矩阵与变换矩阵）】特殊欧式群 S E ( 3 ) SE(3) SE(3)：对于变换矩阵T，它具有比较特别的结构：左上角为旋转矩阵，右上角为平移向量，左下角为 0 0 0向量，右下角为1。这种矩阵又称为特殊欧式群（Special Euclidean Group）: S E ( 3 ) = { T = [ R t 0 T 0 ] ∈ R 4 × 4 ∣ R ∈ S O ( 3 ) , t ∈ R 3 } . (3.5) 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} SE(3)={T=[R0T?t0?]∈R4×4∣R∈SO(3),t∈R3}.(3.5)与 S O ( 3 ) SO(3) SO(3)一样，求解该矩阵的逆 T ? 1 T^{-1} T?1，表示一个反向的变换： T ? 1 = [ R T ? R T t 0 T 1 ] . (3.6) T^{-1}= \begin{bmatrix} R^{T} & -R^{T}t\\ 0^{T} & 1 \end{bmatrix}. \tag{3.6} T?1=[RT0T??RTt1?].(3.6)同样，我们用 T 12 T_{12} T12?这样的写法表示从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 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"<

编译好程序后，运行它，可以看到各矩阵运算结果如下：
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.6803750.59688 -0.329554 -0.2112340.8232950.536459 0.566198 -0.604897 -0.444451 transpose: 0.680375 -0.2112340.566198 0.596880.823295 -0.604897 -0.3295540.536459 -0.444451 sum: 1.61307 trace: 1.05922 times 10: 6.803755.9688 -3.29554 -2.112348.232955.36459 5.66198 -6.04897 -4.44451 inverse: -0.1985212.227392.8357 1.00605 -0.555135-1.41603 -1.622133.593083.28973 det: 0.208598 Eigen values= 0.0242899 0.992154 1.80558 Eigen vectors= -0.549013 -0.7359430.396198 0.253452 -0.598296 -0.760134 -0.7964590.316906 -0.514998 time of normal inverse is 1.967ms x = -55.7896 -298.793130.113 -388.455 -159.312160.654 -40.0416 -193.561155.844181.144185.125 -62.778619.8333 -30.8772 -200.74655.8385 -206.60426.3559 -14.6789122.719 -221.44926.233-318.95 -78.693150.144687.1986 -194.922132.319-171.78 -4.1973611.876 -171.77948.304784.1812 -104.958 -47.2103 -57.4502 -48.9477 -19.423728.9419111.42192.1237 -288.248 -23.3478-275.22 -292.062-92.6985.96847 -93.6244109.734 time of Qr decomposition is 2.409ms x = -55.7896 -298.793130.113 -388.455 -159.312160.654 -40.0416 -193.561155.844181.144185.125 -62.778619.8333 -30.8772 -200.74655.8385 -206.60426.3559 -14.6789122.719 -221.44926.233-318.95 -78.693150.144687.1986 -194.922132.319-171.78 -4.1973611.876 -171.77948.304784.1812 -104.958 -47.2103 -57.4502 -48.9477 -19.423728.9419111.42192.1237 -288.248 -23.3478-275.22 -292.062-92.6985.96847 -93.6244109.734 time of ldlt decomposition is 0.667ms x = -55.7896 -298.793130.113 -388.455 -159.312160.654 -40.0416 -193.561155.844181.144185.125 -62.778619.8333 -30.8772 -200.74655.8385 -206.60426.3559 -14.6789122.719 -221.44926.233-318.95 -78.693150.144687.1986 -194.922132.319-171.78 -4.1973611.876 -171.77948.304784.1812 -104.958 -47.2103 -57.4502 -48.9477 -19.423728.9419111.42192.1237 -288.248 -23.3478-275.22 -292.062-92.6985.96847 -93.6244109.734 time of lu decomposition is 0.787ms x = -55.7896 -298.793130.113 -388.455 -159.312160.654 -40.0416 -193.561155.844181.144185.125 -62.778619.8333 -30.8772 -200.74655.8385 -206.60426.3559 -14.6789122.719 -221.44926.233-318.95 -78.693150.144687.1986 -194.922132.319-171.78 -4.1973611.876 -171.77948.304784.1812 -104.958 -47.2103 -57.4502 -48.9477 -19.423728.9419111.42192.1237 -288.248 -23.3478-275.22 -292.062-92.6985.96847 -93.6244109.734

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

    推荐阅读


    上一篇：机器学习笔试面试题——day1

    下一篇：NumPy 迭代数组