关于因子图在SLAM中的应用基础理论总结

参考文章：
1.张兴学. 基于因子图的多传感器信息融合导航算法研究[D].哈尔滨工业大学,2018.
2.SLAM 中的位姿图优化，BA优化，因子图优化，非线性优化之间的区别
3.因子图的理论基础
4.机器人感知：因子图在SLAM中的应用(读书笔记)第一至三章

SLAM中的定位问题，是根据先验信息（上一时刻的位姿和已知路标点信息）以及传感器的观测值（当前时刻测到的路标点信息），推断出该时刻的位姿以及未知路标点位置。对于这样一个概率推理问题，可通过因子图简化。

因子图是概率图模型的一种表示方式，表示了全局函数和局部函数之间的关系，同时也表示了各个变量与局部函数之间的关系。通过因子图可以将复杂系统进行简化，有利于处理复杂的概率问题。

例如对于一个函数$$f(x_1,x_2,x_3,x_4)=f_1(x_1,x_2)f_2(x_2,x_3,x_4)f_3(x_4)$$与其对应的因子图可以表示为：

如上图所示，$x$代表变量，$f$代表因子，当且仅当$f$是关于$x$的函数时，才会相连。并且每个因子对应唯一结点，每个变量对应唯一的边缘或半边缘（只与一个因子相连）。所以每个变量最多同时连接2个因子！

如果一个变量出现在多个因子中时，例如𝑋是一个实数随机变量，$Y_1$ 和$Y_2$是关于$X$的两个独立实数带噪声观测量。则这些变量的联合概率密度是:$$f(x_1,y_1,y_2)=f(x)f(y_1|x)f(y_2|x)$$与其对应的因子图可以表示为：

其联合概率密度是:$$f(x,x^{\prime },x^{\prime \prime },y_1,y_2)=f(x)f(y_1|x^{\prime })f(y_2|x^{\prime \prime })f_{=}(x,x^{\prime },x^{\prime \prime })$$这里的等式约束函数：$$f_{=}(x,x^{\prime },x^{\prime \prime })\triangleq \delta (x-x^{\prime })(x-x^{\prime \prime })$$等式约束因子可以看做是一个分支，它允许超过两个以上的因子去使用同一个变量。

---

消息传递算法
和—积算法规则：沿着边缘𝑥从结点（因子）𝑔传递出的信息是𝑔和沿着除𝑥以外其余所有边缘传入的信息的乘积，然后对除𝑥以外其余所有相关变量进行求和的结果。

例如上图，对于输出${\mu }_3$，等于除$x$以外的边缘传入信息+相关变量求和：$${\mu }_3\triangleq f_3(x_1,x_2,x_3,x_4){\mu }_1{\mu }_2\sum _{x_1,x_2,x_3}$$

---

以下为知乎上一些回答的总结：
因子图优化，可以在问题的拓扑结构上进行一些顶层抽象和简化，而不是直接硬解，相当于多了一步简化过程。如果只有路标和位姿之间的因子，和BA优化完全一样。因子图优化的优化落脚点不再是最小二乘理论了，而是最大后验概率理论了。相比较卡尔曼滤波，因子图的优势是，无论是数据压入多少维度，解法是一样的。

综上，我理解因为因子图的消息传递算法，在接收到新的观测信息后，根据传感器的观测方程以及相应的代价函数进行变量边缘的状态更新，实现多传感器的数据融合。所以无论有多少传感器数据，计算方法都是一样的。