视觉SLAM十四讲(高翔版本)，ch3章节部分笔记

目标：理解slam的框架以及它的理论知识。供以后自己查阅。

第三章：主要讲一些基本的数学知识，包括点，向量，旋转矩阵三大类，高翔老师图文并用的方式讲的非常清晰。

第一部分

点
点需要在某个坐标系下的表达，一般在世界坐标系表达，一般情况是 e 0 = [ 1 0 0 ] e_0=\begin{bmatrix} 1 \\ 0 \\ 0 \end{bmatrix} e0=⎣⎡100⎦⎤, e 1 = [ 0 1 0 ] e_1=\begin{bmatrix} 0 \\ 1 \\ 0 \end{bmatrix} e1=⎣⎡010⎦⎤, e 2 = [ 0 0 1 ] e_2=\begin{bmatrix} 0 \\ 0 \\ 1 \end{bmatrix} e2=⎣⎡001⎦⎤。在日常表达中，这个这类基向量是省略的。
点存在于三维空间之中，因此在上述向量基空间中，由一个三维向量表示 p → = ( x , y , z ) \overrightarrow{p} =(x,y,z) p =(x,y,z)。

向量
因为有点的表达，但是点与点之间关系需要数学表达方式，它就也是由向量表示。也是由一个三维向量表示 A B = ( a , b , c ) AB=(a,b,c) AB=(a,b,c)，它只有方向和大小的概念。同时它在不同的坐标系下，一般和点在用一个坐标系下（也可以等同于同一个向量基）。

点可以向量都可以用向量表示。因此基于向量的操作，有以下几类。

点乘（内积）： a → ⋅ b → = a → T b → = ∑ i = 1 3 a i b i = ∣ a → ∣ b → ∣ c o s ( a , b ) \overrightarrow{a} \cdot \overrightarrow{b} = \overrightarrow{a}^T \overrightarrow{b} = \sum^{3}_{i=1}a_ib_i = |\overrightarrow{a}|\overrightarrow{b}|cos(a,b) a ⋅b =a Tb =i=1∑3aibi=∣a ∣b ∣cos(a,b)

叉乘（外积）： a → × b → = [ i j k a 1 a 2 a 3 b 1 b 2 b 3 ] = [ a 2 b 3 − a 3 b 2 a 3 b 1 − b 1 a 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 → = s k e w ( a → ) b → \overrightarrow{a} \times \overrightarrow{b} = \begin{bmatrix} i & j & k \\ a_1 & a_2 & a_3 \\ b_1 & b_2 & b_3 \end{bmatrix} = \begin{bmatrix} a_2b_3-a_3b_2 \\ a_3b_1-b_1a_3 \\ a_1b_2-a_2b1 \end{bmatrix} = \begin{bmatrix} 0& -a_3 & a_2 \\ a_3 & 0 & -a_1 \\ -a_2 & a_1 & 0 \end{bmatrix} \overrightarrow{b} = skew( \overrightarrow{a}) \overrightarrow{b} a ×b =⎣⎡ia1b1ja2b2ka3b3⎦⎤=⎣⎡a2b3−a3b2a3b1−b1a3a1b2−a2b1⎦⎤=⎣⎡0a3−a2−a30a1a2−a10⎦⎤b =skew(a )b

其中 s k e w ( a → ) skew(\overrightarrow{a}) skew(a )为反对称矩阵。

第二部分

三维刚体运动
上述介绍了一些向量和点等等的基本运算。进一步需要知道在空间中，坐标系是怎么变化的。以及怎么样计算不同坐标系之间，同一个点的坐标表达。

高翔老师介绍的怎么样从一个坐标系转化为另一个坐标系。可以看到，它在自己的坐标系下，先进行旋转，使得它和目标坐标系方向一致，轴对应。然后用平移向量，移动到目标坐标系即可。它包括这两个过程，使用表达方式为：旋转，平移。

１）旋转：

向量基：它是一组相互垂直的单位向量组成的。一般坐标系都是一组向量基组成，
假设原始向量基为： e 0 → \overrightarrow{e_0} e0 , e １ → \overrightarrow{e_１} e１ , e ２ → \overrightarrow{e_２} e２。目标向量基为： e 0 → ′ \overrightarrow{e_0}' e0 ′, e １ → ′ \overrightarrow{e_１}' e１ ′, e ２ → ′ \overrightarrow{e_２}' e２ ′。假设一个向量 a → \overrightarrow{a} a 是固定向量，它不随着坐标系变化而移动，因此还是 a → \overrightarrow{a} a ，但是它不同坐标系下，它的坐标发生了变化。可以表达为：
[ e 0 → e 1 → e 2 → ] [ a 0 a 1 a 2 ] = [ e 0 → ′ e 1 → ′ e 2 → ′ ] [ a 0 ′ a 1 ′ a 2 ′ ] ( 1 ) \begin{bmatrix} \overrightarrow{e_0} & \overrightarrow{e_1}&\overrightarrow{e_2} \end{bmatrix} \begin{bmatrix} a_0 \\ a_1 \\ a_2 \end{bmatrix} = \begin{bmatrix} \overrightarrow{e_0}' & \overrightarrow{e_1}'&\overrightarrow{e_2}' \end{bmatrix} \begin{bmatrix} a_0' \\ a_1' \\ a_2' \end{bmatrix}　\space \space \space \space (1) [e0 e1 e2 ]⎣⎡a0a1a2⎦⎤=[e0 ′e1 ′e2 ′]⎣⎡a0′a1′a2′⎦⎤　 (1)

其中 a → \overrightarrow{a} a 在 E = [ e 0 → e 1 → e 2 → ] E=\begin{bmatrix} \overrightarrow{e_0} & \overrightarrow{e_1}&\overrightarrow{e_2} \end{bmatrix} E=[e0 e1 e2 ]坐标系下的坐标； a → ′ \overrightarrow{a}' a ′在 E ′ = [ e 0 → ′ e 1 → ′ e 2 → ′ ] E'=\begin{bmatrix} \overrightarrow{e_0}' & \overrightarrow{e_1}'&\overrightarrow{e_2}' \end{bmatrix} E′=[e0 ′e1 ′e2 ′] 坐标系下的坐标。

对 1 1 1公式左乘 E E E：
１）因为 E E E是一组向量基组成的向量，因此 E T E = I E^TE=I ETE=I为单位矩阵。因此左边的为 [ a 0 a 1 a 2 ] \begin{bmatrix} a_0 \\ a_1 \\ a_2 \end{bmatrix} ⎣⎡a0a1a2⎦⎤（因为 e 0 → T e 1 → \overrightarrow{e_0}^T\overrightarrow{e_1} e0 Te1 =0）,通过计算容易得到。
２）因为１）属性，得到如下：
[ a 0 a 1 a 2 ] = [ e 0 → T e 0 → ′ e 0 → T e 1 → ′ e 0 → T e 2 → ′ e 1 → T e 0 → ′ e 1 → T e 1 → ′ e 1 → T e 2 → ′ e 2 → T e 0 → ′ e 2 → T e 1 → ′ e 2 → T e 2 → ′ ] [ a 0 ′ a 1 ′ a 2 ′ ] = R [ a 0 ′ a 1 ′ a 2 ′ ] ( 2 ) \begin{bmatrix} a_0 \\ a_1 \\ a_2 \end{bmatrix} =\begin{bmatrix} \overrightarrow{e_0}^T\overrightarrow{e_0}' &\overrightarrow{e_0}^T\overrightarrow{e_1}' & \overrightarrow{e_0}^T\overrightarrow{e_2}' \\ \overrightarrow{e_1}^T\overrightarrow{e_0}' &\overrightarrow{e_1}^T\overrightarrow{e_1}' & \overrightarrow{e_1}^T\overrightarrow{e_2}' \\ \overrightarrow{e_2}^T\overrightarrow{e_0}' &\overrightarrow{e_2}^T\overrightarrow{e_1}' & \overrightarrow{e_2}^T\overrightarrow{e_2}' \end{bmatrix} \begin{bmatrix} a_0' \\ a_1' \\ a_2' \end{bmatrix} = \bold{R} \begin{bmatrix} a_0' \\ a_1' \\ a_2' \end{bmatrix} \space \space \space \space (2) ⎣⎡a0a1a2⎦⎤=⎣⎢⎡e0 Te0 ′e1 Te0 ′e2 Te0 ′e0 Te1 ′e1 Te1 ′e2 Te1 ′e0 Te2 ′e1 Te2 ′e2 Te2 ′⎦⎥⎤⎣⎡a0′a1′a2′⎦⎤=R⎣⎡a0′a1′a2′⎦⎤ (2)
因此得到 R = [ e 0 → T e 0 → ′ e 0 → T e 1 → ′ e 0 → T e 2 → ′ e 1 → T e 0 → ′ e 1 → T e 1 → ′ e 1 → T e 2 → ′ e 2 → T e 0 → ′ e 2 → T e 1 → ′ e 2 → T e 2 → ′ ] \bold{R} =\begin{bmatrix} \overrightarrow{e_0}^T\overrightarrow{e_0}' &\overrightarrow{e_0}^T\overrightarrow{e_1}' & \overrightarrow{e_0}^T\overrightarrow{e_2}' \\ \overrightarrow{e_1}^T\overrightarrow{e_0}' &\overrightarrow{e_1}^T\overrightarrow{e_1}' & \overrightarrow{e_1}^T\overrightarrow{e_2}' \\ \overrightarrow{e_2}^T\overrightarrow{e_0}' &\overrightarrow{e_2}^T\overrightarrow{e_1}' & \overrightarrow{e_2}^T\overrightarrow{e_2}' \end{bmatrix} R=⎣⎢⎡e0 Te0 ′e1 Te0 ′e2 Te0 ′e0 Te1 ′e1 Te1 ′e2 Te1 ′e0 Te2 ′e1 Te2 ′e2 Te2 ′⎦⎥⎤

１： R \bold{R} R它是一个单位正交矩阵，因为 E , E ′ E,E' E,E′都是单位正交矩阵，可以自己计算，两两向量相乘，非常容易得到。这类的矩阵也是逆等于它的转置。
２： R \bold{R} R的秩为１１１

这类可以表达为数学形式为：
S O ( n ) = R ∈ R n × n ∣ R R T = I , d e t ( R ) = 1 ( 3 ) SO(n)={\bold{R} \in R^{n\times n} | \bold{R} \bold{R} ^T=I,det(\bold{R} )=1} \space \space \space \space (3) SO(n)=R∈Rn×n∣RRT=I,det(R)=1 (3)

公式 ( 3 ) (3) (3)称这类旋转矩阵为特殊正交群（群的概念第 4 4 4）章将介绍。

因为在空间坐标系下 n = 3 n=3 n=3。满足以上公式的 3 × 3 3\times3 3×3为空间坐标系下的旋转矩阵。 n = 3 n=3 n=3代入上述公式 ( 3 ) (3) (3)得到三维空间的旋转矩阵，也是slam中的基本概念 S O ( 3 ) SO(3) SO(3)：
S O ( 3 ) = R ∈ R 3 × 3 ∣ R R T = I , d e t ( R ) = 1 ( 3 ) SO(3)={\bold{R} \in R^{3\times 3} | \bold{R} \bold{R} ^T=I,det(\bold{R} )=1} \space \space \space \space (3) SO(3)=R∈R3×3∣RRT=I,det(R)=1 (3)

它有如下几种性质：
假设： a 1 = R 12 a 2 a_1=\bold{R}_{12}a_2 a1=R12a2; a 2 = R 21 a 1 a_2=\bold{R}_{21}a_1 a2=R21a1,其中 R 12 , R 21 \bold{R}_{12},\bold{R}_{21} R12,R21在两个坐标系的相互转化的旋转矩阵。它们的关系为：
R 12 = R 21 − 1 = R 21 T ( ４ ) \bold{R}_{12}=\bold{R}_{21}^{-1}=\bold{R}_{21}^{T}　\space \space \space \space (４) R12=R21−1=R21T　 (４)

公式 4 4 4非常容易得到，因为它是它是一个正交矩阵，通过公式可以得到，以后证明,很简单，直接相乘就可以了，相乘后除了对角线为１，其它都为０。

２）平移：

这个简单，暂且省略。它表达为 t t t

３）旋转加平移（后续 n = 3 n=3 n=3）：

基于上述的先旋转后平移得到公式如下：
a → ′ = R a → + t ( 5 ) \overrightarrow{a}'= \bold{R}\overrightarrow{a} +t \space \space \space \space (5) a ′=Ra +t (5)

对于多次坐标系转化，上述方式写法，不容易写，因此它们统一到矩阵表达上，采用了齐次坐标的表达方式。如下：
[ a → ′ 1 ] = [ R t 0 T 1 ] [ a → 1 ] = T [ a → 1 ] ( 6 ) \begin{bmatrix} \overrightarrow{a}' \\ 1 \end{bmatrix} = \begin{bmatrix} \bold{R} & t \\ 0^T & 1 \end{bmatrix} \begin{bmatrix} \overrightarrow{a} \\ 1 \end{bmatrix} = \bold{T}\begin{bmatrix} \overrightarrow{a} \\ 1 \end{bmatrix} \space \space \space \space (6) [a ′1]=[R0Tt1][a 1]=T[a 1] (6)

因此多次坐标系的转化可以表达为：
b → = T 1 a → , c → = T 2 b → = > c → = T 2 T 1 a → ( 7 ) \overrightarrow{b}= \bold{T}_{1}\overrightarrow{a}, \overrightarrow{c}= \bold{T}_{2}\overrightarrow{b} => \overrightarrow{c}=\bold{T}_{2}\bold{T}_{1} \overrightarrow{a} \space \space \space \space (7) b =T1a ,c =T2b =>c =T2T1a (7)

这类引入齐次坐标系的旋转平移向量，用统一代数表达为：
S E ( 3 ) = { T = [ R t 0 T 1 ] ∈ R 4 × 4 ∣ R ∈ S O ( 3 ) , T ∈ R 3 } ( 8 ) SE(3) = \begin{Bmatrix} \bold{T} = \begin{bmatrix} \bold{R} & t \\ 0^T & 1 \end{bmatrix} \in R^{4 \times 4} | \bold{R} \in SO(3), T \in R^3 \end{Bmatrix} \space \space \space \space (8) SE(3)={T=[R0Tt1]∈R4×4∣R∈SO(3),T∈R3} (8)

公式 8 8 8可以得到 T \bold{T} T，这类矩阵也被称为特殊欧式群。它的逆变化为：
T − 1 = [ R T − R T t 0 T 1 ] ( 9 ) \bold{T}^{-1} = \begin{bmatrix} \bold{R}^T & -\bold{R}^T t \\ 0^T & 1 \end{bmatrix} \space \space \space \space (9) T−1=[RT0T−RTt1] (9)

４）角轴

上面介绍的理论知识非常重要。但是它都是基于矩阵的表达形式，它和矩阵表达的意义等价的。下面介绍另一种表达形式（角轴）。
在真实的世界中，向量也可以表示旋转矩阵，它比上面的矩阵表示更为紧凑。如下图，给出了物理意义上的向量表示旋转。

从上图可以看到旋转可以表示为绕着一根轴旋转一定的角度。使用 W = θ n → W=\theta \overrightarrow{n} W=θn 表示。其中 θ \theta θ为角度， n → \overrightarrow{n} n 表示旋转轴。可以见我之前写的blog，(我使用的是 u → \overrightarrow{u} u 表示，这里使用 n → \overrightarrow{n} n 表示，其实意思是一样的)。
https://blog.csdn.net/weixin_43851636/article/details/126004987?spm=1001.2014.3001.5502

它计算罗德里公式：
R = I c o s θ + ( 1 − c o s θ ) n → n → T + n → × s i n θ ( 10 ) \bold{R}=Icos\theta+(1-cos\theta)\overrightarrow{n}\overrightarrow{n}^T+\overrightarrow{n} \times sin\theta \space \space \space \space (10) R=Icosθ+(1−cosθ)n n T+n ×sinθ (10)

它的逆变化，从 R \bold{R} R到旋转角，和角轴 θ , n → \theta, \overrightarrow{n} θ,n 为（那篇blog后续补上这个推导）：
角度：
θ = a r c c o s ( t r ( R ) − 1 2 ) ( 11 ) \theta = arccos(\cfrac{tr(\bold{R})-1}{2}) \space \space \space \space (11) θ=arccos(2tr(R)−1) (11)
轴：
R n → = n → ( 12 ) \bold{R}\overrightarrow{n}=\overrightarrow{n} \space \space \space \space (12) Rn =n (12)

上述的表达，可以看到旋转，可以按照某种顺序旋转。如果按照 Z − Y − X Z-Y-X Z−Y−X的顺序旋转物体。
１）绕着 Z Z Z轴旋转，得到的是偏航角yaw。
２）绕着 Y Y Y轴旋转，得到是俯仰角pitch。
３）绕着 x x x轴旋转，得到是翻滚角roll。
它符合人的视角，但是在计算不好。还有一些缺陷。比如旋转顺序，还有万向锁的问题等等，用的不多，所以简略记录。

四元素

上述的欧拉角具有的缺点，四元素不具有，它具有广泛性，且紧凑性，在工程中常常遇到。它也是可以表达旋转矩阵。

四元素定义：一种扩展的复数，
（二维的）可以表达平面旋转。
表达为：
q = q 0 + q 1 i → + q 2 j → + q 3 k → ( 13 ) { i → 2 = j → 2 = k → 2 = − 1 i → j → = k → , j → i → = − k → j → k → = i → , k → j → = − i → k → i → = j → , i → k → = − j → \bold{q}=q_0 + q_1 \overrightarrow{i} + q_2 \overrightarrow{j}+ q_3 \overrightarrow{k} \space \space \space (13) \\ \begin{cases} \overrightarrow{i}^2 = \overrightarrow{j}^2 = \overrightarrow{k}^2 = -1 \\ \overrightarrow{i} \overrightarrow{j}= \overrightarrow{k}, \overrightarrow{j} \overrightarrow{i} = -\overrightarrow{k} \\ \overrightarrow{j} \overrightarrow{k}= \overrightarrow{i}, \overrightarrow{k} \overrightarrow{j} = -\overrightarrow{i} \\ \overrightarrow{k} \overrightarrow{i}= \overrightarrow{j}, \overrightarrow{i} \overrightarrow{k} = -\overrightarrow{j} \end{cases} q=q0+q1i +q2j +q3k (13)⎩⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎧i 2=j 2=k 2=−1i j =k ,j i =−k j k =i ,k j =−i k i =j ,i k =−j
它表达形式也可以为：
q = [ s , v → ] , s = q 0 ∈ R , v → = [ q 1 , q 2 , q 3 ] T ∈ R 3 ( 14 ) { i → 2 = j → 2 = k → 2 = − 1 i → j → = k → , j → i → = − k → j → k → = i → , k → j → = − i → k → i → = j → , i → k → = − j → \bold{q}=[s, \overrightarrow{v}], s=q_0 \in R, \overrightarrow{v} = [q_1, q_2, q_3 ]^T \in R^3 \space \space \space (14) \\ \begin{cases} \overrightarrow{i}^2 = \overrightarrow{j}^2 = \overrightarrow{k}^2 = -1 \\ \overrightarrow{i} \overrightarrow{j}= \overrightarrow{k}, \overrightarrow{j} \overrightarrow{i} = -\overrightarrow{k} \\ \overrightarrow{j} \overrightarrow{k}= \overrightarrow{i}, \overrightarrow{k} \overrightarrow{j} = -\overrightarrow{i} \\ \overrightarrow{k} \overrightarrow{i}= \overrightarrow{j}, \overrightarrow{i} \overrightarrow{k} = -\overrightarrow{j} \end{cases} q=[s,v ],s=q0∈R,v =[q1,q2,q3]T∈R3 (14)⎩⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎧i 2=j 2=k 2=−1i j =k ,j i =−k j k =i ,k j =−i k i =j ,i k =−j

它的虚部的向量基和相机坐标系中的右手坐标系一样。
这些四元素的基本运算如下，它是基本的运算规则：

这些可以手动运算一下验证，这些都是基本的代数运算规则。
基于四元素的定义，它怎么表示三维旋转？
答：
直观上可以得到从角轴到四元素（形式转换）：
q = [ c o s θ 2 n x s i n θ 2 n y s i n θ 2 n z s i n θ 2 ] \bold{q}=\begin{bmatrix} cos \cfrac{\theta}{2} & n_x sin \cfrac{\theta}{2} & n_y sin \cfrac{\theta}{2} & n_z sin \cfrac{\theta}{2} \end{bmatrix} q=[cos2θnxsin2θnysin2θnzsin2θ]
四元素到角轴：

{ θ = 2 a r c c o s q 0 [ n x n y n z ] T = [ q 1 q 2 q 3 ] T / s i n θ 2 \begin{cases} \theta=2arccos q_0 \\ \begin{bmatrix} n_x & n_y & n_z \end{bmatrix} ^T= \begin{bmatrix} q_1 & q_2 & q_3 \end{bmatrix} ^T / sin \cfrac{\theta}{2} \end{cases} ⎩⎨⎧θ=2arccosq0[nxnynz]T=[q1q2q3]T/sin2θ
以上是和欧拉角进行互算，但是基于物理意义上，怎么利用四元素对空间的一个点进行旋转？
假设一个顶点 p = [ 0 x y z ] p=\begin{bmatrix} 0 & x & y & z \end{bmatrix} p=[0xyz] 经过 q \bold{q} q旋转后得到 p ′ p' p′，这类关系怎么用代数表达？
旋转后得到如下： p ′ = q p q − 1 p'=\bold{q}p\bold{q}^{-1} p′=qpq−1
它是先转到四维空间（第一维不为０），然后再转回三维空间。
实践部分，后面补充。