视觉slam14讲——第10讲后端1

本系列文章是记录学习高翔所著《视觉slam14讲》的内容总结，文中的主要文字和代码、图片都是引用自课本和高翔博士的博客。代码运行效果是在自己电脑上实际运行得出。手动记录主要是为了深入理解
涉及到的主要内容如下

• 理解后端的概念
• 理解以EKF为代表的滤波器后端工作原理
• 理解非线性优化的后端，明白稀疏性是如何利用的
• 使用g2o和Ceres实际操作后端优化

前端视觉里程计能给出一个短时间内的轨迹和地图，但是会进行误差积累，长时间就不准确了。后端就是为了更长时间的最优轨迹和地图。

1 概述

1.1 状态估计的概率解释

在后端优化中，我们通常考虑更长的一段时间内的状态估计问题，而且不仅使用过去的信息更新自己的状态，也会用未来的信息更新自己，这种处理方式称为批量的（Batch）。如果当前状态只由过去决定，甚至只由前一个时刻决定，称为渐进的(Incremental)。

对于观测方程
xk=f(xk−1,μk)+ωk
zk,j=h(yj,xk)+vk,j
注意两点

• 只有当 xk 观测到了 yi ，才会产生观测数据。事实上在一个位置通常只能看到一小部分路标。
• 在没有运动方程的情况下，整个优化问题就只由许多个观测方程组成，类似于SfM（Structure from Motion）问题，相当于通过一组图像来恢复运动和结构。

由于每个方程都是受噪声影响的，所以把位姿 x 和路标 y 看做服从某种概率分布的随机变量，而不是单独的一个数。问题转换为，当存在一些运动和观测数据时，我们应该如何去估计状态量的高斯分布。

使用过去0-k中数据估计现在的状态分布，按照 xk−1 时刻为条件概率展开得到

P(xk|x0,μ1:k,z1:k−1)=∫P(xk|xk−1,x0,μ1:k,z1:k−1)P(xk−1|x0,μ1:k,z1:k−1)dxk−1

对状态如何估计有两种选择：
（1）假设马尔科夫性，使用EKF方法
（2）考虑k时刻的状态与之前说有的状态关系，使用非线性优化方法

1.2 线性系统和KF

考虑滤波器模型，上式的右侧第一项可以简化

P(xk|xk−1,x0,μ1:k,z1:k−1)=P(xk|xk−1,μk)

第二部分简化为

P(xk−1|x0,μ1:k,z1:k−1)dxk−1=P(xk−1|x0,μ1:k−1,z1:k−1)

最终通过复杂的公式推导可以归纳为预测和更新两个步骤

1.3 非线性系统和EKF

重要的思想

SLAM中运动方程和观测方程通常是非线性函数，尤其是SLAM中的相机模型，需要使用相机内参模型和李代数表示的位姿，更不可能是线性系统。一个高斯分布，经过非线性变换后，也往往不再是高斯分布，所以在非线性系统中，必须取一定的近似，将一个非高斯分布近似为高斯分布。

把KF扩展到非线性系统中就成为扩展卡尔曼滤波器EKF。通常的做法是在某个点附近考虑运动方程及观测方程的泰勒一阶展开，只保留一阶项，即线性部分，然后按照线性系统进行推导。最终得到EKF的推导公式。

卡尔曼滤波器给出了线性化以后的状态变量分布的变化过程。在非线性系统和高斯噪声下，卡尔曼滤波器给出了无偏最优估计。而在SLAM这种非线性情况下，给出了单词线性近似下的最大后验估计(MAP)

2 BA与图优化

2.1 投影模型和BA代价函数

上图是投影过程详细的参数化过程。整体的代价函数(Cost Function)为

12∑i=1m∑j=1n||eij||2=12∑i=1m∑j=1n||zij−h(ξi,pj)||2

2.2 BA的求解

容易发现 h(ξi,pj) 不是线性函数，使用非线性优化手段来优化它。
目标函数为

12||f(x+△x)||2=12||e+F△xc+E△xp||2

其中雅儿比矩阵E，F必须是整体目标函数对整体变量的倒数，它将是一个很大块的矩阵。
使用高斯牛顿法为例，

J=[FE]

H=JTJ=[FTFETFFTEETE]

幸运的是这里的H矩阵有一定的特殊结构，可以加速求解过程。

2.3 稀疏性和边缘化

通过下面的几张图对H矩阵的稀疏性有个基本的了解。

经过消元之后，得到关于位姿部分的增量方程

[B−EC−1ET]△xc=v−EC−1ω

求解这个方程的过程称为 Marginalization 或者 Schur消元 方法。

H矩阵的非对角块处的非零元素对应着相机和路标的关联，进行Schur消元后的S的稀疏性具有物理意义，表明该处对应两个相机变量之间存在着共同观测的路标点，称为共视（Co-visibility）。

S矩阵的稀疏性结构取决于实际的观测结果，无法提前预知。