概述
概率估计的概率解释
线性系统和KF
经典线性卡尔曼滤波的概率推导:
非线性系统和EKF
对于非线性系统,扩展卡尔曼滤波的推导:
EKF的讨论
- 假设了马尔科夫性。
- EKF由于在局部点处进行泰勒展开,非线性误差是不容忽视的问题。
- EKF需要在程序中存储和维护状态量的均值和方差,如果把路标也放进状态量中,那么计算复杂性会变得很大。
BA与图优化
投影模型与代价函数
- 相机模型投影计算流程:
左侧的p是全局坐标系下的三维坐标点,右侧的us,vs是该点在图像平面上的最终像素坐标。中间畸变模块中的rc2=uc2+vc2 - 观测方程的最小二乘问题构建
在视觉SLAM中,观测量z=[us,vs]T,那么观测误差可以写为:
整体代价函数为:
其中zij表示位于位姿i除观测到了第j个路标点。对整体代价函数进行最小二乘求解,相当于对位姿和路标同时做了调整,这就是所谓的BA。
BA的求解
- 把自变量定义成所有待优化的变量:
- 当给自变量一个增量的时候,目标函数变为:
其中Fij表示整个代价函数在当前状态下对相机姿态的偏导数,而Eij表示该函数对路标点位置的偏导。 - 把相机位姿变量,空间点变量各自放在一起之后的目标函数:
目标函数变为:
其中,雅克比矩阵E和F必须是整体目标函数对整体变量的导数。 - 线性增量方程中的H矩阵形式
线性增量方程:
整体的雅克比矩阵:
Gauss-Newton法的黑森矩阵:
这个矩阵将是非常大的具有特殊结构的矩阵,利用这个特殊结构,可以加速求解过程。
稀疏性和边缘化
-
H矩阵的稀疏性是由雅克比矩阵J(x)的稀疏性引起的。
考虑代价函数中的一项eij,它所对应的的雅克比矩阵具有如下的形式:
可以看到,对于eij的雅克比矩阵,除了位于第i处位姿和第j个路标点位置的导数不为0,其余地方都为零。这体现了误差项与其他路标和轨迹无关的特性。 -
J引起H矩阵的稀疏性
设Jij只在i,j处有非零块,那么它对H的贡献为JijTJij,这个JijTJij也仅有四个非零块,位于(i,i),(i,j),(j,i),(j,j)位置。
对于整体的H,由于:
可以对H进行分块:
-
H具有稀疏结构
将H进行分块之后,H11只与相机位姿有关,H22只与路标点有关。当遍历i,j时,以下事实总是成立:
- 不管I,j怎么变,H11都是对角阵,只在Hi,i处有非零块.
- 同理,H22也是对角阵,只在Hj,j处有非零块.
- 对于H12和H21,他们可能是稀疏的,也可能是稠密的,视具体的观测数据而定.
- 一个简单的H具有稀疏结构的说明例子
假设一个场景内有2个相机位姿(C1,C2)和6个路标(P1,P2,P3,P4,P5,P6).相机C1观测到路标P1,P2,P3,P4,相机C2观测到路标P3,P4,P5,P6
可以推出,该场景下BA目标函数为:
其中e11描述了C1看到了P1这件事情,与其他的相机位姿和路标无关。把所有的变量按照如下的顺序排放:
那么,e11的雅克比矩阵J11可以表示为:
形象地表示为:
那么,整体雅克比矩阵以及黑森矩阵可以形象地表示为:
对于H矩阵当中处于非对角线的矩阵块来说,如果该矩阵块非零,则其位置对应的变量之间在图中会存在一条边,如下图e26所示:
- 一般情况下的H矩阵的稀疏性
设有m个相机位姿,n个路标点,通常情况下n远远大于m,所以H矩阵左上角块显得非常小,而右下角的对角块占据了大量的地方,就像一个箭头,可以成之为箭头形矩阵,如下图所示:
- 边缘化或Schur消元
我们把矩阵H进行如下的区域划分:
对应的线性方程组由H delta_x = g变为如下形式:
其中,B是对角块矩阵,每个对角块的维度和相机参数的维度相同,对角块的个数等于相机变量的个数。同理,C也是对角块矩阵,由于三维空间中的每个路标点为三维,所以每个块为3*3的矩阵,同样的,C的规模远远大于B。
因为,对角块矩阵求逆的难度远小于对一般矩阵的求逆难度,因为我们只需要对那些对角线矩阵块分别求逆即可。考虑到这个特性,我们对线性方程组进行高斯消元,目标是消去右上角的非对角部分E:
经过消元之后,方程组第一行变成和delta xp无关的项。单独把它拿出来,得到关于位姿部分的增量方程:
我们求解这个方程,得到delta xc,然后将其带入到原方程求解出delta xp,这个过程就是Schur消元或者边缘化。 - 边缘化或Schur的优势
- 在消元过程中,由于C为对角块,C-1容易解出
- 求解了delta xc之后,路标部分的增量方程由delta xp = C-1(w - ETdelta xc)给出,这依然用到了C-1易于求解的特性。
- delta xc的求解
上述方程没有可用的特殊结构,是一个普通的线性方程,记此方程的系数为S,它的稀疏性也具有特定的物理意义。S矩阵的非对角线上的非零矩阵块,表示该处对应的两个相机之间存在着共同观测的路标点。 - 概率角度解释边缘化
之所以称为边缘化,是因为把求(delta xc,delta xp)的问题,转化成了先求delta xc,再求delta xp的过程,这相当于做了条件概率展开:
P(xc,xp) = P(xc)P(xp|xc)
不考虑xp的影响,求出xc,这相当于求出了关于xc的边缘分布,故称边缘化。
鲁棒核函数
将最小化误差项的二范数平方和作为目标函数,存在一个严重的问题:如果出于误匹配等原因,某个误差项给的数据是错误的,算法会认为这是一条误差很大的边,梯度也很大,不得已顺应这条边的无理要求,使调整失败。
出现这种问题的原因是,当误差很大时,二范数增长得太快。解决办法就是将增长得太快的二范数换成一个增长没那么快的函数,同时保证自己的光滑性质。因为它们使得整个优化结果更为稳健,所以又叫它们鲁棒核函数。
鲁棒核函数有很多种,比如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