SLAM_因子图优化
概率模型到因子图模型
定义一个简单的机器人问题（SLAM）
假设有一个机器人在往前运动，运动过程中能观测到两个路标点，定义了三个时间，在三个时间中有对路标点的观测量和机器人自身运动的估计量（里程计）
用贝叶斯网络（Bayes Net）来描述SLAM建模问题：
在贝叶斯网络中通过有向的边来连接一些变量。图中定义了两类四种变量。
蓝色、 黄色圆圈这两者都属于状态变量。
红色、绿色圆圈都属于第二类变量-观测变量。
这个贝叶斯网络实际上描述了状态变量和观测变量的联合概率模型。
概率模型的定义：假设知道系统的状态变量（机器人所在位置、路标点所在位置），可以推测出机器人得到的观测量（观测量是通过某些传感器得到的，而传感器有已知模型，可以通过数据手册得到），即已知机器人的状态量和传感器的模型，就可以推算出机器人的观测量。
所以得到了一个生成模型（generative model）：用一个贝叶斯网络来描述系统模型，在已知状态量的情况下，如何得到观测量。
X为机器人状态量，Z为观测量。
假设知道系统状态的概率分布和给定系统状态下的观测量的条件概率（通过传感器的概率模型），相乘得到的联合概率就是系统状态变量和观测变量的联合概率。
观测变量的条件概率的特点：路标点1的观测变量只跟时间1下的机器人的位置（状态变量）和路标点1的位置有关，跟路标点2无关。图中可以很清晰地看到，路标点1的观测变量通过两个边连接到机器人的位置变量和路标点1的位置变量。这就说明这个观测变量是由这两个边连接的机器人的位置变量和路标点1的位置变量单独决定的。极少的状态变量决定每一个单独的观测变量，这对后面构建的矩阵的稀疏特性至关重要。
所有观测变量的条件概率是以一个乘积的形式存在的。在示例中公式的红色部分，三个观测变量的条件概率相互独立，相乘起来。同样绿色部分是两个odometry的观测变量，也相乘起来。这些条件概率可以分解。
但现实生活中需要求解的问题是知道观测量，需要求解状态量，是一个状态估计的问题（SLAM问题）
因子图就是这样一类估计模型（reference model ）。应用贝叶斯定律（Bayes rule）：给定Z，求解X的概率（估计模型）正比于给定X,求解Z的概率(生成模型)。
分母的Z先验值是跟X无关的(相互独立)，可以省略。
对于求解的状态估计问题，就是给定系统观测量，求解系统状态量，使得这个条件概率最大。也就是求解MAP（最大后验分布 maximum posterior inference）， 公式中右边项的X是先验概率（prior），一般是上一帧的最优后验概率，P(Z|X)是似然概率（likelihood，由传感器模型给定） ，左边就是最大后验概率。
每一个观测变量在贝叶斯网络里都是单独求解的（相互独立），所以所有的条件概率都是乘积的形式，且可分解，在因子图里面，分解的每一个项就是一个因子，乘积乘在一起用图的形式来描述就是因子图。
因子图里面包括两类节点和边
节点：状态变量（圆圈），是估计求解的变量。
观测节点（就是因子，用方块标定），每一个因子表示每一个得到的观测量。
因子图里面还会包括一个先验因子（prior factor），就是公式中X的先验值(先验概率)，用来固定整个系统的解，避免数值多解，达到数值可解 。
整个因子图实际上就是每个因子单独的乘积。红色对应观测量因子，绿色对应状态量之间的因子（里程计），紫色是先验因子。求解因子图就是将这些因子乘起来，求一个最大值，得到的系统状态就是概率上最可能的系统状态。
根据中心极限定理，绝大多数传感器的噪音是符合高斯分布的，所以每个因子都是用指数函数来定义的，指数函数对应了error function，包括两个部分：系统状态量和观测量。error function实际上表示的是用状态量去推测的观测量与实际观测量的区别。
以odometry factor为例，它的error function是：后一个位置减去前一个位置（预估的量，因为后一个位置是预估的），再减去实际观测到的量，其实就是预估的量减去观测的量。
Landmark factor的error function也一样，是用Xi和Lj推测出来的观测点的位置量减去实际的观测点位置量。
希望error是越小越好，这就说明观测值和预测值是接近的。套到MAP里面来看，因子图的求解是要所有因子的乘积最大化。而对于负指数函数形式，每一个因子乘积最大化代表里面的fx最小化。这正好符合希望误差函数的误差最小化。即系统状态与观测值越吻合，error越小，那φ就越大（MAP最大），就是希望能找到这么一组状态变量能尽可能和观测量吻合。
未完
