本人讲解关于slam一系列文章汇总链接:史上最全slam从零开始
有兴趣的朋友可以加微信 17575010159 相互讨论技术 - 文末公众号也可关注
一、前言(线性变换)
该篇博客主要讲解一个 slam 中最基础的几个东西，那就是旋转矩阵，缩放矩阵以及偏移矩阵。本人会做一个比较细致的讲解。首先从二维平面开始引入，等大家略微了解之后，再扩展到三维。
在讲解之前，聊一下其他的东西，那就是线性变换。在学习线性代数的时候，如果矩阵 A A A 左乘一个向量 v ⃗ \vec v v , 就说成矩阵 A A A 对向量 v ⃗ \vec v v 进行了线性变换。直观上的感觉改变了向量 v ⃗ \vec v v 的坐标。但是并不知道其形象的几何意义是什么，那么现在就来更深层次的理解一下。
首先，向量的表示有三种方式: ①带有箭头的有线线段。②符号 v ⃗ \vec v v ， x ⃗ \vec x x 等。③ v ⃗ = [ v 1 v 2 ] \vec{v} =\left[\begin{array}{l} v_{1} \\ v_{2} \end{array}\right] v =[v1v2]。以该为基础，进行具体的分析。任何一个空间都是由一组基构成的，言外之意就是说，在这个空间的任何一点(向量)，都可以由这组基以线性组合的方式得到。比如在二维中的 x y xy xy 平面，其实他就是组基 ( e 1 → = [ 1 0 ] , e 2 → = [ 0 1 ] ) \left(\overrightarrow{e_{1}}=\left[\begin{array}{l} 1 \\ 0 \end{array}\right], \overrightarrow{e_{2}}=\left[\begin{array}{l} 0 \\ 1 \end{array}\right]\right) (e1 =[10],e2 =[01])张成的空间，而所谓的向量 v ⃗ = [ v 1 v 2 ] \vec{v} =\left[\begin{array}{l} v_{1} \\ v_{2} \end{array}\right] v =[v1v2]其实就是线性组合 v ⃗ = v 1 e 1 → + v 2 e 2 → = v 1 [ 1 0 ] + v 2 [ 0 1 ] = [ v 1 v 2 ] \vec{v}=v_{1} \overrightarrow{e_{1}}+v_{2} \overrightarrow{e_{2}}=v_{1}\left[\begin{array}{l} 1 \\ 0 \end{array}\right]+v_{2}\left[\begin{array}{l} 0 \\ 1 \end{array}\right]=\left[\begin{array}{l} v_{1} \\ v_{2} \end{array}\right] v =v1e1 +v2e2 =v1[10]+v2[01]=[v1v2]。
矩阵乘法的意义，其实就是将一个向量，经过某个矩阵(函数)之后，输出为另外一个向量，就是说，变换就意味则，将原来的向量变换到另外一个地方。而线性变换，也就是在变换的基础上，再加一个条件，线性的，也就是原来的一条直线，在变换了之后还应该是直线。
下面我们来理解什么是线性变换。为了避免混淆，我们不用二维空间 X Y XY XY 的 x y xy xy 基底，而是选用一组新的基底 e 1 ⃗ = [ 1 1 ] e 2 ⃗ = [ − 1 1 ] \color{blue} \vec {e_{1}}=\left[\begin{array}{l} 1 \\ \\ 1 \end{array}\right]~~~~~~~~~~~ \vec{e_{2}}=\left[\begin{array}{c} -1 \\ \\ 1 \end{array}\right] e1 =⎣ ⎡11⎦ ⎤e2 =⎣ ⎡−11⎦ ⎤假设原向量为 v ⃗ = 1 e 1 ⃗ + 1 e 2 ⃗ = [ 0 2 ] \vec v=1\vec {e_{1}} +1\vec {e_2} = \left[\begin{array}{c} 0 \\ 2 \end{array}\right] v =1e1 +1e2 =[02], 而我们想要向量 v ⃗ \vec v v 经过矩阵 A A A 变换成另外同一个向量 v ′ ⃗ \vec {v'} v′ 。假设变换矩阵 A A A 如下： A = [ 0 − 1 1 0 ] ( 逆时针旋转 90 ° ) \color{blue} A=\left[\begin{array}{cc} 0 & -1 \\ \\ 1 & 0 \end{array}\right](逆时针旋转90°) A=⎣ ⎡01−10⎦ ⎤(逆时针旋转90°)按照之前的计算 v ′ ⃗ = A v ⃗ = [ 0 − 1 1 0 ] [ 0 2 ] = [ − 2 0 ] \color{blue} \vec{v'}=A \vec{v}=\left[\begin{array}{cc} 0 & -1 \\ \\ 1 & 0 \end{array}\right]\left[\begin{array}{l} 0 \\ \\ 2 \end{array}\right]=\left[\begin{array}{c} -2 \\ \\ 0 \end{array}\right] v′ =Av =⎣ ⎡01−10⎦ ⎤⎣ ⎡02⎦ ⎤=⎣ ⎡−20⎦ ⎤上面的结果比好理解，也比较直观。那我们再来看另外一种解释，矩阵 A A A 对向量的变换，其实是施加再其基底的变换，而新的向量 v ′ ⃗ \vec {v'} v′ 关于新的基底 ( e 1 ′ ⃗ , e 2 ′ ⃗ ) (\vec {e'_{1}},\vec {e'_2}) (e1′ ,e2′ ) 的线性组合。为什么呢?如下图
上图中表示 v ⃗ = 1 e 1 ⃗ + 1 e 2 ⃗ = [ 0 2 ] \vec v=1\vec {e_{1}} +1\vec {e_2} = \left[\begin{array}{c} 0 \\ 2 \end{array}\right] v =1e1 +1e2 =[02](注意系数为1)，也就是使用基底 e 1 ⃗ , e 2 ⃗ \vec {e_{1}} ,\vec {e_2} e1 ,e2 ,则 v ⃗ = [ 1 1 ] \vec v= \left[\begin{array}{c} 1 \\ 1 \end{array}\right] v =[11]。 v ⃗ \vec v v 经过线性变换 A A A 之后显示如下：
这次我们把变换作用在基底上，也就是基底 e 1 ⃗ = [ 1 1 ] ， e 2 ⃗ = [ − 1 1 ] \vec {e_{1}}=\left[\begin{array}{l} 1 \\ 1 \end{array}\right]，\vec{e_{2}}=\left[\begin{array}{c} -1 \\ 1 \end{array}\right] e1 =[11]，e2 =[−11] 经过线性变换 A A A 之后变成新的基底 e 1 ′ → = A e 1 → = [ − 1 1 ] , e 2 ′ → = A e 2 → = [ − 1 − 1 ] \color{blue} \overrightarrow{e'_{1}}=A \overrightarrow{e_{1}}=\left[\begin{array}{c} -1 \\ \\ 1 \end{array}\right], ~~~~~~~~~~~\overrightarrow{e'_{2}}=A \overrightarrow{e_{2}}=\left[\begin{array}{l} -1 \\ \\ -1 \end{array}\right] e1′ =Ae1 =⎣ ⎡−11⎦ ⎤,e2′ =Ae2 =⎣ ⎡−1−1⎦ ⎤那么 v ′ ⃗ \vec {v'} v′ 推导过程如下： v ′ → = 1 e 1 ′ → + 1 e 2 ′ → = 1 ( A e 1 → ) + 1 ( A e 2 → ) = 1 [ 1 − 1 ] + 1 [ − 1 1 ] = [ − 2 0 ] \color{blue} \overrightarrow{v^{\prime}}=1 \overrightarrow{e'_{1}}+1 \overrightarrow{e'_{2}}=1\left(A \overrightarrow{e_{1}}\right)+1\left(A \overrightarrow{e_{2}}\right)=1\left[\begin{array}{c} 1 \\ \\ -1 \end{array}\right]+1\left[\begin{array}{c} -1 \\ \\ 1 \end{array}\right]=\left[\begin{array}{c} -2 \\ \\ 0 \end{array}\right] v′ =1e1′ +1e2′ =1(Ae1 )+1(Ae2 )=1⎣ ⎡1−1⎦ ⎤+1⎣ ⎡−11⎦ ⎤=⎣ ⎡−20⎦ ⎤可以看到其关于 e 1 ′ ⃗ , e 2 ′ ⃗ \vec {e'_{1}} ,\vec {e'_2} e1′ ,e2′ 为基底的时候，其系数依旧为1，也就是 v ′ ⃗ \vec {v'} v′ 以 e 1 ′ ⃗ , e 2 ′ ⃗ \vec {e'_{1}} ,\vec {e'_2} e1′ ,e2′ 为基底可以表示为 v ′ ⃗ = [ 1 1 ] \vec {v'}= \left[\begin{array}{c} 1 \\ 1 \end{array}\right] v′ =[11]。
这样我们可以很明显的看到，变换作用在基底的时候，系数是不会发生改变的，也就是这个向量关于基的线性组合方式是没有变化的，改变的是基底。换句话说，对于一个线性变换，我们只需要跟踪其基在变换前后的变化，便可以掌握整个空间的变化。
二、旋转矩阵
通过上面的介绍，可以知道，一个矩阵作用于一个向量的线性变换，可以看作是对其基做了线性变换。这样能够保证该向量相对于原基与新基做变换是一致的(系数不会改变)。那么我们现来聊聊三维空间的旋转。单我们描述一个物体的时候，其除了位置信息还有姿态信息，这里拿小猫举例: 小猫在二楼的房间面对着窗户。在二楼是位置信息，面对着窗户是姿态信息。后续我们研究的对象多为相机，相机也是同样的道理，其在三维空间内存在位置信息，也存在次态信息。假设我们把手机从空中抛到地上，其在落地期间，姿态(翻转)与位置(移动)都在发生变换。那么应该如何取描述手机在空中的变化呢?这里把位置与姿态统称为位姿，也就是后续主要的研究对象。
假设在三维空间中，我们找到了该空间的一组基 ( e ⃗ 1 , e ⃗ 2 , e ⃗ 3 ) (\vec e_1, \vec e_2, \vec e_3) (e 1,e 2,e 3), 那么任意一个向量 a ⃗ \vec 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) \tag {1} \color{blue} \vec {a}=\left[\vec {e}_{1}, \vec{e}_{2}, \vec{e}_{3}\right]\left[\begin{array}{l} a_{1} \\ a_{2} \\ a_{3} \end{array}\right]=a_{1} \vec{e}_{1}+a_{2} \vec{e}_{2}+a_{3} \vec{e}_{3} a =[e 1,e 2,e 3]⎣ ⎡a1a2a3⎦ ⎤=a1e 1+a2e 2+a3e 3(1)其上面的 ( a 1 , a 2 , a 3 ) T (a_1,a_2,a_3)^T (a1,a2,a3)T 称为 a ⃗ \vec a a 在此基下的坐标。坐标的具体取值, 一是和向量本身有关, 二是和坐标 系（基）的选取有关。坐标系通常由 3 个正交的坐标轴组成 (尽管也可以有非正交的, 但实际中 很少见 )。例如, 当给定 x \boldsymbol{x} x 和 y \boldsymbol{y} y 轴时, z \boldsymbol{z} z 轴就可以通过右手 (或左手) 法则由 x × y \boldsymbol{x} \times \boldsymbol{y} x×y 定义出来。根 据定义方式的不同, 坐标系又分为左手系和右手系。左手系的第 3 个轴与右手系方向相反。大部 分 3D 程序库使用右手系 (如 OpenGL、3D Max 等), 也有部分库使用左手系 ( 如 Unity、Direct3D 等)。
我们经常在实际场景中定义各种各样的坐标系。在机器人学中, 你会给每一个连杆和关节定 义它们的坐标系; 在 3D 作图时, 我们也会定义每一个长方体、圆柱体的坐标系。如果考虑运动 的机器人, 那么常见的做法是设定一个惯性坐标系 (或者叫世界坐标系), 可以认为它是固定不 动的, 例如下图中的 x W , y W , z W x_{\mathrm{W}}, y_{\mathrm{W}}, z_{\mathrm{W}} xW,yW,zW 定义的坐标系。
同时, 相机或机器人是一个移动坐标系, 例如上图中 x C , y C , z C x_{\mathrm{C}}, y_{\mathrm{C}}, z_{\mathrm{C}} xC,yC,zC 定义的坐标系。读者可能会问: 相机视野中某个向量 p \boldsymbol{p} p , 它在相机坐标系下的坐标为 p c \boldsymbol p_{\mathrm{c}} pc , 而从世界坐标系下看, 它的坐标为 p w \boldsymbol{p}_{\mathrm{w}} pw , 那么, 这两个坐标之间是如何转换的呢? 这时, 就需 要先得到该点针对机器人坐标系的坐标值, 再根据机器人位姿变换到世界坐标系中。我们需要一 种数学手段来描述这个变换关系, 稍后我们会看到, 可以用一个矩阵 T \boldsymbol{T} T 来描述它。
两个坐标系之间的运动由一个旋转加上一个平移组成, 这种运动称为刚体运动。相机运动就是一个刚体运动。刚体运动过程中, 同一个向量在各个坐标系下的长度和夹角都不会发生变化。 想象你把手机抛到空中, 在它落地摔碎之前 , 只可能有空间位置和姿态的不同, 而它自己的长 度、各个面的角度等性质不会有任何变化。手机并不会像橡皮那样一会儿被挤扁,一会儿被拉长。 此时, 我们说手机坐标系到世界坐标之间, 相差了一个欧氏变换 (Euclidean Transform )。
欧氏变换由旋转和平移组成。我们首先考虑旋转。设某个单位正交基 ( e ⃗ 1 , e ⃗ 2 , e ⃗ 3 ) \left(\vec {e}_{1}, \vec{e}_{2}, \vec {e}_{3}\right) (e 1,e 2,e 3) 经过一次旋 转变成了 ( e ′ ⃗ , e ′ ⃗ , e ′ ⃗ 3 ) \left(\vec {e'}, \vec {e'}, \vec {e'}_{3}\right) (e′ ,e′ ,e′ 3) 。那么, 对于同一个向量 a ⃗ \vec {a} a (该向量并没有随着坐标系的旋转而发生运动), 它在两个坐标系下的坐标为 [ a 1 , a 2 , a 3 ] T \left[a_{1}, a_{2}, a_{3}\right]^{\mathrm{T}} [a1,a2,a3]T 和 [ a 1 ′ , a 2 ′ , a 3 ′ ] T \left[a_{1}^{\prime}, a_{2}^{\prime}, a_{3}^{\prime}\right]^{\mathrm{T}} [a1′,a2′,a3′]T 。因为向量本身没变, 所以根据坐标的定 义, 有 [ 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) \tag {2} \color{blue} \left[\vec {e}_{1}, \vec {e}_{2}, \vec {e}_{3}\right]\left[\begin{array}{l} a_{1} \\ a_{2} \\ a_{3} \end{array}\right]=\left[\vec {e'_1}, \vec {e'_2}, \vec {e'_3}\right]\left[\begin{array}{c} a_{1}^{\prime} \\ a_{2}^{\prime} \\ a_{3}^{\prime} \end{array}\right] [e 1,e 2,e 3]⎣ ⎡a1a2a3⎦ ⎤=[e1′ ,e2′ ,e3′ ]⎣ ⎡a1′a2′a3′⎦ ⎤(2)为了描述两个坐标之间的关系, 我们对上述等式的左右两边同时左乘 [ e 1 T e 2 T e 3 T ] \left[\begin{array}{l}\boldsymbol{e}_{1}^{\mathrm{T}} \\ \boldsymbol{e}_{2}^{\mathrm{T}} \\ \boldsymbol{e}_{3}^{\mathrm{T}}\end{array}\right] ⎣ ⎡e1Te2Te3T⎦ ⎤ , 那么左边的系数就变成了单位矩阵, 所以: [ 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 ′ ] = def R a ⃗ (3) \tag {3} \color{blue} \left[\begin{array}{l} a_{1} \\ a_{2} \\ a_{3} \end{array}\right]=\left[\begin{array}{ccc} \boldsymbol{e}_{1}^{\mathrm{T}} \boldsymbol{e}_{1}^{\prime} & \boldsymbol{e}_{1}^{\mathrm{T}} \boldsymbol{e}_{2}^{\prime} & \boldsymbol{e}_{1}^{\mathrm{T}} \boldsymbol{e}_{3}^{\prime} \\ \boldsymbol{e}_{2}^{\mathrm{T}} \boldsymbol{e}_{1}^{\prime} & \boldsymbol{e}_{2}^{\mathrm{T}} \boldsymbol{e}_{2}^{\prime} & \boldsymbol{e}_{2}^{\mathrm{T}} \boldsymbol{e}_{3}^{\prime} \\ \boldsymbol{e}_{3}^{\mathrm{T}} \boldsymbol{e}_{1}^{\prime} & \boldsymbol{e}_{3}^{\mathrm{T}} \boldsymbol{e}_{2}^{\prime} & \boldsymbol{e}_{3}^{\mathrm{T}} \boldsymbol{e}_{3}^{\prime} \end{array}\right]\left[\begin{array}{c} a_{1}^{\prime} \\ a_{2}^{\prime} \\ a_{3}^{\prime} \end{array}\right] \stackrel{\text { def }}{=} {R} \vec {a} ⎣ ⎡a1a2a3⎦ ⎤=⎣ ⎡e1Te1′e2Te1′e3Te1′e1Te2′e2Te2′e3Te2′e1Te3′e2Te3′e3Te3′⎦ ⎤⎣ ⎡a1′a2′a3′⎦ ⎤=defRa (3)我们把中间的矩阵拿出来, 定义成一个矩阵 R R R。这个矩阵由两组基之间的内积组成, 刻画了旋转前后同一个向量的坐标变换关系。只要旋转是一样的, 这个矩阵就是一样的。可以说, 矩阵 R R R 描述了旋转本身。因此, 称为旋转矩阵（Rotation Matrix)。同时, 该矩阵各分量是两个坐标系基的 内积, 由于基向量的长度为 1, 所以实际上是各基向量夹角的余弦值。所以这个矩阵也叫方向余 弦矩阵 (Direction Cosine Matrix )。统一称它为旋转矩阵。
旋转矩阵有一些特别的性质。事实上, 它是一个行列式为 1 的正交矩阵( 后面有证明过程 \color{red}{后面有证明过程} 后面有证明过程)。反之, 行列式 为 1 的正交矩阵也是一个旋转矩阵。所以, 可以将 n n n 维旋转矩阵的集合定义如下(下面的 E E E表示基本矩阵):
SO ( n ) = { R ∈ R n × n ∣ R R T = E , det ( R ) = 1 } (4) \tag {4} \color{blue} \operatorname{SO}(n)=\left\{{R} \in \mathbb{R}^{n \times n} \mid {R} {R}^{\mathrm{T}}={E}, \operatorname{det}({R})=1\right\} SO(n)={R∈Rn×n∣RRT=E,det(R)=1}(4)
S O ( n ) \mathrm{SO}(n) SO(n) 是特殊正交群 ( Special Orthogonal Group ） 的意思。我们把“群” 的内容留到下一讲。 这个集合由 n 维空间的旋转矩阵组成, 特别地, S O ( 3 ) \mathrm{SO}(3) SO(3) 就是指三维空间的旋转。通过旋转矩阵, 我们可以直接谈论两个坐标系之间的旋转变换，而不用再从基开始谈起。由于旋转矩阵为正交矩阵, 它的逆 (即转置) 描述了一个相反的旋转。按照上面的定义方式, 有
a ′ ⃗ = R − 1 a ⃗ = R T a ⃗ (5) \tag {5} \color{blue} \vec {a'}={R}^{-1} \vec{a}={R}^{\mathrm{T}} \vec {a} a′ =R−1a =RTa (5) 显然 R T R^T RT 刻画了一个相反的旋转。
三、平移
在欧氏变换中, 除了旋转还有平移。考虑世界坐标系中的向量 a ⃗ \vec {a} a , 经过一次旋转(用 R R R描述)和一次平移 t t t 后, 得到了 a ′ ⃗ \vec {a'} a′ , 那么把旋转和平移合到一起, 有 a ′ ⃗ = R a ⃗ + t (6) \tag {6} \color{blue} \vec {a'}={R \vec a}+{t} a′ =Ra +t(6)其中, t t t 称为平移向量。相比于旋转, 平移部分只需把平移向量加到旋转之后的坐标上, 非常简 单。通过上式, 我们用一个旋转矩阵 R {R} R 和一个平移向量 t {t} t 完整地描述了一个欧氏空间的坐标变换 关系。实际当中, 我们会定义坐标系 1、坐标系 2, 那么向量 a ⃗ \vec {a} a 在两个坐标系下的坐标为 a 1 {a}_{1} a1, a 2 {a}_{2} a2 , 它们之间的关系应该是: a 1 = R 12 a 2 + t 12 (7) \tag {7} \color{blue} {a}_{1}={R}_{12} {a}_{2}+{t}_{12} a1=R12a2+t12(7)这里的 R 12 {R}_{12} R12 是指 “把坐标系 2 的向量变换到坐标系 1" 中。由于向量乘在这个矩阵的右边, 它的 下标是从右读到左的。这也是本书的习惯写法。坐标变换很容易搞混, 特别是存在多个坐标系的 情况下。同理, 如果我们要表达 “从 1 到 2 的旋转矩阵” 时, 就写成 R 21 {R}_{21} R21 。请读者务必清楚本书 的记法, 因为不同书籍里写法不同，有的会记成左上/下标, 而本书写在右侧下标。关于平移 t 12 t_{12} t12 , 它实际对应的是坐标系 1 原点指向坐标系 2 原点的向量,在坐标系 1 下取的坐标。
坐标, 所以笔者建议读者把它记作 “从 1 到 2 的向量”。但是反过来的 t 21 t_{21} t21 , 即从 2 指向 1 的向 量在坐标系 2 下的坐标, 却并不等于 − t 12 -t_{12} −t12 , 而是和两个系的旋转还有关系 。所以, 当初学者问 “我的坐标在哪里” 这样的问题时, 我们需要清楚地说明这句话的含义。这里 “我的坐标” 实际上 指从世界坐标系指向自己坐标系原点的向量, 在世界坐标系下取到的坐标。对应到数学符号上, 应该是 t W C t_{\mathrm{WC}} tWC 的取值。同理, 它也不是 − t C W -t_{\mathrm{CW}} −tCW 。
四、欧式变换与齐次坐标
式 (6) 完整地表达了欧氏空间的旋转与平移, 不过还存在一个小问题：这里的变换关系不是一个线性关系。假设我们进行了两次变换: 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 (8) \tag {8} \color{blue} {\vec b}={R}_{1} \vec a+t_{1}, \quad \vec c={R}_{2} \vec b+t_{2} b =R1a +t1,c =R2b +t2(8)那么，从 a ⃗ \vec a a 到 c ⃗ \vec c c 的变换为 c ⃗ = R 2 ( R 1 a + t 1 ) + t 2 (9) \tag {9} \color{blue} \vec c={R}_{2}\left({R}_{1} a+{t}_{1}\right)+{t}_{2} c =R2(R1a+t1)+t2(9)这样的形式在变换多次之后会显得很啰嗦。因此, 我们引人齐次坐标和变换矩阵, 重写式(6): [ a ′ ⃗ 1 ] = [ R t 0 T 1 ] [ a ⃗ 1 ] = def T [ a ⃗ 1 ] (9) \tag {9} \color{blue} \left[\begin{array}{c} \vec {a'}\\ 1 \end{array}\right]=\left[\begin{array}{ll} {R} & {t} \\ {0}^{\mathrm{T}} & 1 \end{array}\right]\left[\begin{array}{l} {\vec a} \\ 1 \end{array}\right] \stackrel{\text { def }}{=} {T}\left[\begin{array}{l} {\vec a} \\ 1 \end{array}\right] [a′ 1]=[R0Tt1][a 1]=defT[a 1](9)这是一个数学技巧: 我们在一个三维向量的末尾添加 1, 将其变成了四维向量, 称为齐次坐 标。对于这个四维向量, 我们可以把旋转和平移写在一个矩阵里, 使得整个关系变成线性关系。 该式中, 矩阵 T {T} T 称为变换矩阵 (Transform Matrix)。
我们暂时用 a ~ \tilde{{a}} a~ 表示 a ⃗ {\vec a} a 的齐次坐标。那么依靠齐次坐标和变换矩阵, 两次变换的叠加就可以 有很好的形式： b ~ = T 1 a ~ , c ~ = T 2 b ~ ⇒ c ~ = T 2 T 1 a ~ (10) \tag {10} \color{blue} \tilde{{b}}={T}_{1} \tilde{{a}}, \tilde{{c}}={T}_{2} \tilde{{b}} \quad \Rightarrow \tilde{{c}}={T}_{2} {T}_{1} \tilde{{a}} b~=T1a~,c~=T2b~⇒c~=T2T1a~(10)但是区分齐次和非齐次坐标的符号令我们感到厌烦, 因为此处只需要在向量末尾添加 1 或者去掉1, 在不引起歧义的情况下, 以后我们就直接把它写成 b = T a b=Ta b=Ta 的样子, 默认其中进行了 齐次坐标的转换。关于变换矩阵 T {T} T , 它具有比较特别的结构: 左上角为旋转矩阵, 右侧为平移向量, 左下角为 0 向量, 右下角为 1。这种矩阵又称为特殊欧氏群 (Special Euclidean Group ):
S E ( 3 ) = { T = [ R t 0 T 1 ] ∈ R 4 × 4 ∣ R ∈ S O ( 3 ) , t ∈ R 3 } (11) \tag {11} \color{blue} {SE}(3)=\left\{{T}=\left[\begin{array}{ll} {R} & {t} \\ {0}^{\mathrm{T}} & 1 \end{array}\right] \in \mathbb{R}^{4 \times 4} \mid {R} \in \mathrm{SO}(3), {t} \in \mathbb{R}^{3}\right\} SE(3)={T=[R0Tt1]∈R4×4∣R∈SO(3),t∈R3}(11)
与 S O ( 3 ) {SO}(3) SO(3) 一样, 求解该矩阵的逆表示一个反向的变换: T − 1 = [ R T − R T t 0 T 1 ] \color{blue} {T}^{-1}=\left[\begin{array}{cc} {R}^{\mathrm{T}} & -{R}^{\mathrm{T}} {t} \\ {0}^{\mathrm{T}} & 1 \end{array}\right] T−1=[RT0T−RTt1]
同样, 我们用 T 12 T_{12} T12 这样的写法来表示从 2 到 1 的变换。并且, 为了保持符号的简洁, 在不引 起歧义的情况下, 以后不刻意区别齐次坐标与普通坐标的符号, 默认使用的是符合运算法则的那 一种。例如, 当我们写 T a {T a} Ta 时, 使用的是齐次坐标 (不然没法计算)。而写 R a {R a} Ra 时, 使用的是非 齐次坐标。如果写在一个等式中, 就假设齐次坐标到普通坐标的转换是已经做好了的一一因为齐 次坐标和非齐次坐标之间的转换事实上非常容易, 而在 C + + \mathrm{C}++ C++ 程序中你可以使用运算符重载来完 成这个功能, 保证在程序中看到的运算是统一的。
回顾: 首先, 我们介绍了向量及其坐标表示, 并介绍了向量间的运算; 然后, 坐标系之间的 运动由欧氏变换描述, 它由平移和旋转组成。旋转可以由旋转矩阵 S O ( 3 ) {SO}(3) SO(3) 描述, 而平移直接由一 个 R 3 \mathbb{R}^{3} R3 向量描述。最后, 如果将平移和旋转放在一个矩阵中, 就形成了变换矩阵 S E ( 3 ) {SE}(3) SE(3) 。
旋转矩阵性质证明
根据前面的(3)式，可以知道旋转矩阵就是两个基(正交矩阵)相乘。正交矩阵的定义比较简单:
A A T = E \color{blue} AA^T=E AAT=E只要满足以上公式，我们则认为 A A A 为正交矩阵，简单的理解，就是矩阵 A A A 每一行，各元的平方和为1，则为正交矩阵。通过如下推导(两边取行列式)我们可以知道正交矩阵 A A A 的行列式 为 ± 1 \pm 1 ±1。
∣ A A T ∣ = ∣ A ∣ ∣ A T ∣ = ∣ A ∣ 2 = ∣ E ∣ = 1 \color{blue} |AA^T|=|A||A^T|=|A|^2=|E|=1 ∣AAT∣=∣A∣∣AT∣=∣A∣2=∣E∣=1这里能取 ± 1 \pm 1 ±1 的原因，主要是因为左手法则与右手法则的区别，或者说坐标系选取时候 x y xy xy 轴的夹角说决定。另外 三阶行列式的几何意义是行向量或者列向量张成的有向体积。即 d e t ( r 1 , r 2 , r 3 ) = r 1 ⋅ ( r 1 × r 3 ) = 1 det(r_1,r_2,r_3)=r_1\cdot(r_1 \times r_3)=1 det(r1,r2,r3)=r1⋅(r1×r3)=1(注意点乘与叉乘的区别)。这里不做太深的讲解，有兴趣的朋友可以自行了解一下。
前面说到 旋转矩阵 的本质是两个 正交矩阵相乘，那么正交矩阵还有一个性质，那就是两个正交矩阵相乘，他们的结果依旧为正交矩阵，推导如下,设 A , B A,B A,B 都为正交矩阵：
A A T = B B T = E \color{blue} AA^T=BB^T=E AAT=BBT=E ( A B ) ( A B ) T = A B B T A T = A A T = E \color{blue} (AB)(AB)^T=ABB^TA^T=AA^T=E (AB)(AB)T=ABBTAT=AAT=E所以我们可以得到两个正交矩阵相乘仍为正交矩阵。那就这样就可以推导出来 旋转矩阵R为正交矩阵。
五、结语
该篇博客大部分来自于slam十四讲，有兴趣的朋友可以取阅读一下这本书。在 slam 中可以说是十分著名的一本书籍。
