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

概述

概率估计的概率解释

在这里插入图片描述

线性系统和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时,以下事实总是成立:

  1. 不管I,j怎么变,H11都是对角阵,只在Hi,i处有非零块.
  2. 同理,H22也是对角阵,只在Hj,j处有非零块.
  3. 对于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的优势
  1. 在消元过程中,由于C为对角块,C-1容易解出
  2. 求解了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

在这里插入图片描述
在这里插入图片描述

发布了83 篇原创文章 · 获赞 60 · 访问量 3万+
展开阅读全文

没有更多推荐了,返回首页

©️2019 CSDN 皮肤主题: 大白 设计师: CSDN官方博客

分享到微信朋友圈

×

扫一扫,手机浏览