SLAM算法-因子图建模

1 整体介绍

SLAM或者计算机视觉的问题目标是利用观察到的数据(图像,点云)来推理现实世界中的某些事情(全局状态),比如通过连续的视频帧推理相机的运动，比如通过3D点云数据推理机器人的移动状态(位置和姿态)。SLAM和计算机视觉都尝试描述一个用来解决此类问题的数学框架模型。解决此类问题需要三个要素:
模型: 在数学上将数据和全局状态关联起来
推理:根据数据，利用模型计算全局状态的后验概率
学习:利用成对样本和状态，来学习模型参数

1.0 概率图模型

因子图属于概率图模型(PGM)中的一种，先介绍PGM知识。主要包括三部分：表示，推理，学习。

1.0.1 表示

图是由节点和边构成的抽象网络图，其中包括几个元素：
(1) 点：表示随机变量
(2) 边：表示节点之间的关系,边具有方向
(3) 图分为:有向图和无向图:

在概率论部分,其中条件独立性是概率推断的基础，基本公式：
(1) 独立性:P(AB) =P(A)P(B)
(2) 条件独立性：P(AB|C) = P(A|C)P(B|C)
(2) 贝叶斯公式： f(A|B)=f(B|A)*f(A)/f(B)
(3) 乘法公式 联合概率 = 条件概率 * 边缘概率： f(A,B) = f(A|B) * f(B)
(4) 条件概率分布变换规则: f(a,b|c) = f(a|b,c) * f(b|c)
(5) *联合分布链式法则：f(a,b,c) = f(a)f(b|c)f(c|a,b)

按照具体分类包括三类：贝叶斯网络，马尔科夫随机场，因子图。

1.0.2 贝叶斯网络

有向图
贝叶斯网络是个有向无圈图，由变量节点和相邻节点的有向边构成,其中边表示父节点指向子节点，用条件概率表达依赖关系，用先验概率表达没有父节点的节点。联合概率分布可以表示为各个节点的条件概率分布的乘积：

按照贝叶斯乘积公式，看具体分解：

贝叶斯网络联合概率可以分解为乘积的形式主要是条件独立性,顺着箭头方向表示因果推理，逆着箭头方向表示证明推理。

1.0.3 马尔科夫随机场

无向图
利用势函数的乘积表示变量的概率分布：

势函数是关于随机变量的函数，随机变量取不同的值，势函数的大小不一样。当所有的势函数值都最大时，联合概率的取值最大。为了将势函数取值从正值扩充到任意实数值，将联合概率分布改写为吉布斯分布：

1.0.4 因子图

因此图属于无向图，因子指的是势函数，我们知道变量的联合概率可以表示为势函数的连乘积形式，将势函数表示乘图模型，势函数是关于变量的函数，变量用圆圈表示，势函数用方块表示，将之间的关联关系用线段连接形成的图为因子图：

定义在因子图上的概率分布可以表示为各个因子的联乘积形式。可以理解为因子图是通过局部变量(圆圈节点)的
依赖关系来表示整体的联合概率分布。

1.0.5 典型图模型

SLAM中或者计算机视觉中常用的模型有下面两类：

我们知道模型包含两类：
(1) 判别模型：
建立在数据p(w|x)上的全局状态可能性模型
(2) 生成模型：
建立在全局状态p(x|w)上的数据可能性模型
以上图模型有个共同特点：含有从状态量w到测量数据x的有向边，该边表明w与x的关系：p(x|w)，在给定的数据上构建了生成模型的概率分布。上述模型的内部是非常稀疏的：每个数据量x只与一个状态量w连接，每个状态量与另外少量的几个状态量连接，结果是模型中蕴含大量的条件独立性关系，利用这种冗余性构建用于学习和推理的高效算法。当存在大规模的未知状态量时，如何进行模型推理？有向图与无向图模型之间的性能比较如何？

1.1 推理问题

概率图模型的推理是求解模型，推理复杂度高，而精确推理是NP难得问题，推理是和具体应用相关的需要先掌握基本算法，再看是如何在SLAM中解决问题。主要掌握几个推理算法，如变量消元算法。

1.1.1 MAP推理

在链式和树模型上可以动态规划进行：

这里不详细介绍动态规划，可以去看相关书籍。

1.1.2 消元算法

消元法计算边缘概率
基本思想是从联合概率分布中消除变量，求解边缘分布。边缘概率原始定义如下：

例1下表描述ABC的联合概率，求A的边缘概率：

第一步，去掉B变量后，剩余的A和C的相同的概率做累加，得到AC概率表
第二步，去掉C后，相同的A做累加，得到关于A的表
第三步，归一化处理
主要采用消除变量，然后求和边缘化。
例2 链式模型，求边缘

1.1.3 时间复杂度

消元的时间复杂度影响因素：中间产生的变量因子的数量。

2 SLAM中的因子图

以上大量篇幅介绍了图模型和简单的推理算法，是通用的理论框架，那么怎么应用在SLAM领域了，可以带着这几个问题：
(1) SLAM中包含怎么样的图模型，贝叶斯有向图？因子图？MRF马尔科夫随机场？
(2) SLAM中的推理问题包含那些了，MAP推理？边缘化？
(3) SLAM推理的算法和消元算法有什么关系？时间复杂度是什么？

2.1 问题模型和表示

SLAM是在贝叶斯框架下的概率推断，主要目标是利用传感器的观测数据(z)对机器人自身的位姿(变量x)和已知的控制输入进行推断同时建立环境地图(变量l)。
图1 描述贝叶斯框架

按照图的概率定义，联合概率密度函数表示为：

图模型的结构说明了数据之间关联，在具体应用中，可指定概率密度函数的具体形式，如高斯分布。这样可得到观测概率模型和运动概率模型，得到联合概率分布，而求解的方式一般采用模拟采样。实际我们并不关心联合概率分布和其分解，在SLAM领域最关心的是给定观测数据Z的条件下，最大化未知变量x和l，下面用大写X表示(x,l) ，转化为数学问题就是求未知状态的MAP估计，

求解的方法是贝叶斯转化。因为最终生成的观测似然函数不一定服从高斯分布，推断方法会无效。
因子图可以解决以上的问题，观测量Z不是最终要求的对象，可以使用因子节点去表示每一个后验概率密度P(X|Z),每一个贝叶斯网络节点分解为相应的因子图中的变量节点和因子节点，因子与变量节点相连。

下面进入最关键的一步，如何对大规模的变量节点进行问题建模和推断。需要理解两个问题：
(1) 因子图中的最大后验概率如何表示？
(2) 因子图中的势函数(或成本函数)具体形式是什么？

2.2 模型推断与简化

2.2.1 非线性模型

采用更一般的形式表示SLAM中的因子图推断，将变量节点扩大，更一般的形式如下：

其实从开始介绍PGM到SLAM中的图，目标就是一步步的得出非线性最小二乘模型，转化为一个数学上的模型求解问题。

2.2.2 线性化

对上面的非线性最小二乘问题一般处理是先所有的观测函数线性化。

具体线性化采用泰勒一阶展开，具体可参考《Factor Graphs for Robot Perception》

下一篇《SLAM算法-因子图优化》会就如何高效求解模型，具体展开。

参考文献

[1] 《计算机视觉模型、学习和推理》
[2] 《机器人感知:因子图在SLAM中的应用》