视觉SLAM十四讲 读书编程笔记 Chapter10 后端1

概述

概率估计的概率解释

线性系统和KF

经典线性卡尔曼滤波的概率推导：

非线性系统和EKF

对于非线性系统，扩展卡尔曼滤波的推导：

EKF的讨论

• 假设了马尔科夫性。
• EKF由于在局部点处进行泰勒展开，非线性误差是不容忽视的问题。
• EKF需要在程序中存储和维护状态量的均值和方差，如果把路标也放进状态量中，那么计算复杂性会变得很大。

BA与图优化

投影模型与代价函数

• 相机模型投影计算流程：
  左侧的p是全局坐标系下的三维坐标点，右侧的u_s,v_s是该点在图像平面上的最终像素坐标。中间畸变模块中的r_c²=u_c²+v_c²
• 观测方程的最小二乘问题构建
  在视觉SLAM中，观测量z=[u_s,v_s]^T,那么观测误差可以写为：
  
  整体代价函数为：
  
  其中z_ij表示位于位姿i除观测到了第j个路标点。对整体代价函数进行最小二乘求解，相当于对位姿和路标同时做了调整，这就是所谓的BA。

BA的求解

• 把自变量定义成所有待优化的变量：
  
• 当给自变量一个增量的时候，目标函数变为：
  
  其中F_ij表示整个代价函数在当前状态下对相机姿态的偏导数，而E_ij表示该函数对路标点位置的偏导。
• 把相机位姿变量，空间点变量各自放在一起之后的目标函数：
  
  
  目标函数变为：
  
  其中，雅克比矩阵E和F必须是整体目标函数对整体变量的导数。
• 线性增量方程中的H矩阵形式
  线性增量方程：
  
  整体的雅克比矩阵：
  
  Gauss-Newton法的黑森矩阵：
  
  这个矩阵将是非常大的具有特殊结构的矩阵，利用这个特殊结构，可以加速求解过程。

稀疏性和边缘化

• H矩阵的稀疏性是由雅克比矩阵J(x)的稀疏性引起的。
  考虑代价函数中的一项e_ij,它所对应的的雅克比矩阵具有如下的形式：
  
  可以看到，对于e_ij的雅克比矩阵，除了位于第i处位姿和第j个路标点位置的导数不为0，其余地方都为零。这体现了误差项与其他路标和轨迹无关的特性。

• J引起H矩阵的稀疏性
  设J_ij只在i,j处有非零块，那么它对H的贡献为J_ij^TJ_ij，这个J_ij^TJ_ij也仅有四个非零块，位于(i,i),(i,j),(j,i),(j,j)位置。
  
  对于整体的H，由于：
  
  可以对H进行分块：
  

• H具有稀疏结构
  将H进行分块之后，H₁₁只与相机位姿有关，H₂₂只与路标点有关。当遍历i,j时，以下事实总是成立：

1. 不管I,j怎么变，H₁₁都是对角阵，只在H_i,i处有非零块.
2. 同理，H₂₂也是对角阵，只在H_j,j处有非零块.
3. 对于H₁₂和H₂₁，他们可能是稀疏的，也可能是稠密的，视具体的观测数据而定.

• 一个简单的H具有稀疏结构的说明例子
  假设一个场景内有2个相机位姿(C₁,C₂)和6个路标(P₁,P₂,P₃,P₄,P₅,P₆).相机C₁观测到路标P₁,P₂,P₃,P₄，相机C₂观测到路标P₃,P₄,P₅,P₆
  可以推出，该场景下BA目标函数为：
  
  其中e₁₁描述了C₁看到了P₁这件事情，与其他的相机位姿和路标无关。把所有的变量按照如下的顺序排放：
  
  那么，e₁₁的雅克比矩阵J₁₁可以表示为：
  
  形象地表示为：
  
  那么，整体雅克比矩阵以及黑森矩阵可以形象地表示为：
  
  对于H矩阵当中处于非对角线的矩阵块来说，如果该矩阵块非零，则其位置对应的变量之间在图中会存在一条边，如下图e₂₆所示：
  
• 一般情况下的H矩阵的稀疏性
  设有m个相机位姿，n个路标点，通常情况下n远远大于m，所以H矩阵左上角块显得非常小，而右下角的对角块占据了大量的地方，就像一个箭头，可以成之为箭头形矩阵，如下图所示：
  
• 边缘化或Schur消元
  我们把矩阵H进行如下的区域划分：
  
  对应的线性方程组由H delta_x = g变为如下形式：
  
  其中，B是对角块矩阵，每个对角块的维度和相机参数的维度相同，对角块的个数等于相机变量的个数。同理，C也是对角块矩阵，由于三维空间中的每个路标点为三维，所以每个块为3*3的矩阵，同样的，C的规模远远大于B。
  因为，对角块矩阵求逆的难度远小于对一般矩阵的求逆难度，因为我们只需要对那些对角线矩阵块分别求逆即可。考虑到这个特性，我们对线性方程组进行高斯消元，目标是消去右上角的非对角部分E：
  
  经过消元之后，方程组第一行变成和delta x_p无关的项。单独把它拿出来，得到关于位姿部分的增量方程：
  
  我们求解这个方程，得到delta x_c，然后将其带入到原方程求解出delta x_p，这个过程就是Schur消元或者边缘化。
• 边缘化或Schur的优势

1. 在消元过程中，由于C为对角块，C^-1容易解出
2. 求解了delta x_c之后，路标部分的增量方程由delta x_p = C^-1(w - E^Tdelta x_c)给出，这依然用到了C^-1易于求解的特性。

• delta x_c的求解
  
  上述方程没有可用的特殊结构，是一个普通的线性方程，记此方程的系数为S，它的稀疏性也具有特定的物理意义。S矩阵的非对角线上的非零矩阵块，表示该处对应的两个相机之间存在着共同观测的路标点。
• 概率角度解释边缘化
  之所以称为边缘化，是因为把求（delta x_c,delta x_p）的问题，转化成了先求delta x_c，再求delta x_p的过程，这相当于做了条件概率展开：
  P(x_c,x_p) = P(x_c)P(x_p|x_c)
  不考虑x_p的影响，求出x_c，这相当于求出了关于x_c的边缘分布，故称边缘化。

鲁棒核函数

将最小化误差项的二范数平方和作为目标函数，存在一个严重的问题：如果出于误匹配等原因，某个误差项给的数据是错误的，算法会认为这是一条误差很大的边，梯度也很大，不得已顺应这条边的无理要求，使调整失败。
出现这种问题的原因是，当误差很大时，二范数增长得太快。解决办法就是将增长得太快的二范数换成一个增长没那么快的函数，同时保证自己的光滑性质。因为它们使得整个优化结果更为稳健，所以又叫它们鲁棒核函数。
鲁棒核函数有很多种，比如Huber核：

实践：g2o

出现错误：

./g2o_customBundle: error while loading shared libraries: libg2o_core.so: cannot open shared object file: No such file or directory
解决办法：
执行如下命令，再重新运行即可

sudo ldconfig

运行结果：

实践：Ceres