slam 14讲笔记

中秋抽时间混囵吞枣看了slam 14讲的前一半,稍微记录下.

slam的框架

• 视觉里程计, 估计相邻两帧图像之间的位姿(特帧匹配)以及局部地图(又称为前端)
• 后端优化, 主要利用前端返回的一些稀疏点,相机初步位姿, 传感器等信息, 回环检测,使用非线性优化来估计全局的位姿和地图.

数学知识

• 向量的内积
  $\mathbf{a}\cdot \mathbf{b}={\mathbf{a}}^{\mathrm{T}}\mathbf{b}={\sum }_{i=1}^3{a}_i{b}_i=|\mathbf{a}||\mathbf{b}|\cos ⟨\mathbf{a},\mathbf{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 − 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 ≜ a ∧ b \boldsymbol{a} \times \boldsymbol{b}=\left[
  ijka1a2a3b1b2b3" role="presentation">ia1b1ja2b2ka3b3$$\begin{array}{ccc}\mathit{i}& j& \mathit{k}\\ a_1& a_2& a_3\\ b_1& b_2& b_3\end{array}$$
  \right]=\left[
  a2b3−a3b2a3b1−a1b3a1b2−a2b1" role="presentation" style="position: relative;">a2b3−a3b2a3b1−a1b3a1b2−a2b1$$\begin{array}{c}{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{array}$$
  \right]=\left[
  0−a3a2a30−a1−a2a10" role="presentation" style="position: relative;">0a3−a2−a30a1a2−a10$$\begin{array}{ccc}0& -a_3& a_2\\ a_3& 0& -a_1\\ -a_2& a_1& 0\end{array}$$
  \right] \boldsymbol{b} \triangleq \boldsymbol{a}^{\wedge} \boldsymbol{b} a×b=⎣ ⎡​ia1​b1​​ja2​b2​​ka3​b3​​⎦ ⎤​=⎣ ⎡​a2​b3​−a3​b2​a3​b1​−a1​b3​a1​b2​−a2​b1​​⎦ ⎤​=⎣ ⎡​0a3​−a2​​−a3​0a1​​a2​−a1​0​⎦ ⎤​b≜a∧b
  这里^符号是反对称矩阵的意思, 即，
  a ∧ = [ 0 − a 3 a 2 a 3 0 − a 1 − a 2 a 1 0 ] \boldsymbol{a}^{\wedge}=\left[
  0−a3a2a30−a1−a2a10" role="presentation" style="position: relative;">0a3−a2−a30a1a2−a10$$\begin{array}{ccc}0& -a_3& a_2\\ a_3& 0& -a_1\\ -a_2& a_1& 0\end{array}$$
  \right] a∧=⎣ ⎡​0a3​−a2​​−a3​0a1​​a2​−a1​0​⎦ ⎤​
  这样可以将叉乘运算转换成矩阵和向量的乘法运算。注意：当一个矩阵$A=-A^t$时， 我们称A为反对称矩阵。类似的，符号∨则是将一个反对称矩阵转换位向量。
  

坐标系转换

假设一个单位正交基$\left(e_1,e_2,e_3\right)$经过旋转变成了$\left(e_1^{\prime },e_2^{\prime },e_3^{\prime }\right)$, 那么对于一个向量a有如下关系。
[ e 1 , e 2 , e 3 ] [ a 1 a 2 a 3 ] = [ e 1 ′ , e 2 ′ , e 3 ′ ] [ a 1 ′ a 2 ′ a 3 ′ ] \left[\boldsymbol{e}_1, \boldsymbol{e}_2, \boldsymbol{e}_3\right]\left[

\right]=\left[\boldsymbol{e}_1^{\prime}, \boldsymbol{e}_2^{\prime}, \boldsymbol{e}_3^{\prime}\right]\left[\right] [e1​,e2​,e3​]⎣ ⎡​a1​a2​a3​​⎦ ⎤​=[e1′​,e2′​,e3′​]⎣ ⎡​a1′​a2′​a3′​​⎦ ⎤​
两边同时左乘 [ e 1 T e 2 T e 3 T ] \left[

e1Te2Te3T" role="presentation" style="position: relative;">eT1eT2eT3$$\begin{array}{l}{e}_1^{\mathrm{T}}\\ {\mathit{e}}_2^{\mathrm{T}}\\ {\mathit{e}}_3^{\mathrm{T}}\end{array}$$

\right] ⎣ ⎡​e1T​e2T​e3T​​⎦ ⎤​,
有：
[ 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 ′ ] ≜ R a ′ \left[

a1a2a3" role="presentation" style="position: relative;">a1a2a3$$\begin{array}{l}{a}_1\\ a_2\\ a_3\end{array}$$

\right]=\left[

e1Te1′e1Te2′e1Te3′e2Te1′e2Te2′e2Te3′e3Te1′e3Te2′e3Te3′" role="presentation" style="position: relative;">eT1e′1eT2e′1eT3e′1eT1e′2eT2e′2eT3e′2eT1e′3eT2e′3eT3e′3$$\begin{array}{ccc}{\mathit{e}}_1^{\mathrm{T}}{\mathit{e}}_1^{\mathrm{\prime }}& {\mathit{e}}_1^{\mathrm{T}}{\mathit{e}}_2^{\mathrm{\prime }}& {\mathit{e}}_1^{\mathrm{T}}{\mathit{e}}_3^{\mathrm{\prime }}\\ {\mathit{e}}_2^{\mathrm{T}}{\mathit{e}}_1^{\mathrm{\prime }}& {\mathit{e}}_2^{\mathrm{T}}{\mathit{e}}_2^{\mathrm{\prime }}& {\mathit{e}}_2^{\mathrm{T}}{\mathit{e}}_3^{\mathrm{\prime }}\\ {\mathit{e}}_3^{\mathrm{T}}{\mathit{e}}_1^{\mathrm{\prime }}& {\mathit{e}}_3^{\mathrm{T}}{\mathit{e}}_2^{\mathrm{\prime }}& {\mathit{e}}_3^{\mathrm{T}}{\mathit{e}}_3^{\mathrm{\prime }}\end{array}$$

\right]\left[

a1′a2′a3′" role="presentation" style="position: relative;">a′1a′2a′3$$\begin{array}{c}{a}_1^{\mathrm{\prime }}\\ a_2^{\mathrm{\prime }}\\ a_3^{\mathrm{\prime }}\end{array}$$

\right] \triangleq \boldsymbol{R} \boldsymbol{a}^{\prime} ⎣ ⎡​a1​a2​a3​​⎦ ⎤​=⎣ ⎡​e1T​e1′​e2T​e1′​e3T​e1′​​e1T​e2′​e2T​e2′​e3T​e2′​​e1T​e3′​e2T​e3′​e3T​e3′​​⎦ ⎤​⎣ ⎡​a1′​a2′​a3′​​⎦ ⎤​≜Ra′
这里就能得到坐标系的旋转矩阵R。

• 旋转矩阵
  旋转矩阵是一个行列式位1的正交矩阵， 用公式来描述就是：
  $\mathrm{SO}(n)=\left\{\mathbf{R}\in {\mathbb{R}}^{n\times n}\mid \mathbf{R}{\mathbf{R}}^{\mathrm{T}}=\mathbf{I},\det (\mathbf{R})=1\right\}$
  这里的SO(n)是特殊正交群(Special Orthogonal Group)的缩写, 对于二维平面上的旋转位SO(2), 三维空间中的旋转这是SO(3).

• 变换矩阵T
  类似的， 对于同时包含旋转和平移的矩阵我们成为变换矩阵， 三维空间的变换矩阵又称为特殊欧式群(SE(3)).
  S E ( 3 ) = { T = [ R t 0 T 1 ] ∈ R 4 × 4 ∣ R ∈ S O ( 3 ) , t ∈ R 3 } \mathrm{SE}(3)=\left\{\boldsymbol{T}=\left[

  Rt0T1" role="presentation" style="position: relative;">R0Tt1$$\begin{array}{cc}\mathit{R}& \mathit{t}\\ {\mathbf{0}}^{\mathrm{T}}& 1\end{array}$$
  \right] \in \mathbb{R}^{4 \times 4} \mid \boldsymbol{R} \in \mathrm{SO}(3), \boldsymbol{t} \in \mathbb{R}^3\right\} SE(3)={T=[R0T​t1​]∈R4×4∣R∈SO(3),t∈R3}

李群和李代数

• 群
  群(Group)是一种集合加上一种运算的代数结构， 它要满足“封结幺逆”这四个条件。旋转矩阵乘以一个旋转矩阵还是一个旋转矩阵， 变换矩阵乘以一个变换矩阵仍然是个变换矩阵，所以他们才能叫做特殊正交群和特殊欧式群。
• 李群
  李群是指具有连续（光滑）性质的群。像整数群Z那样离散的群没 有连续性质，所以不是李群。而SO(n)和SE(n)在实数空间上是连续的。 我们能够直观地想象一个刚体能够连续地在空间中运动，所以它们都是李群
• 李代数
  旋转矩阵对时间求导
  
  就是说， 如果想对R进行求导， 只要左乘一个$\varphi (t)$矩阵即可，可以看出$\varphi (t)$反应了R的导数性质。R(t)在t0处泰勒展开， 那么有：
  
  
  解上面的微分方程可以得到：
  
  也就是说， 任意时刻一个矩阵R(SO(3))都有一个李代数(so(3))$\varphi (t)$与之对应， 只要将李代数下的一个向量经过指数转换即可得到一个对应的旋转矩阵。
  每个李群都有一个李代数与之对应。
  SO(3) ----> so(3)
  SE(3) ----> se(3)

使用包含旋转矩阵的优化问题时， 直接求解旋转矩阵则是一个带约束的优化问题（满足求出来的矩阵时正交的，而且行列式位1）这是很难的， 但是用李代数表示时，则是一个无约束的优化问题， 更加的简单， 这也就是为啥slam要学习李代数。

非线性优化

如果使用一阶梯度近似(最速下降法)

二阶近似(牛顿法)有：
求右侧等式的导数为0处有：

• 高斯牛顿法
  
• 列文伯格—马夸尔特方法
  使用了信赖区域
  

矩阵求逆

• QR分解
• Cholesky分解