Title: 因子图、边缘化与消元算法的抽丝剥茧 —— Notes for “Factor Graphs for Robot Perception”
文章目录
- I. 前言
- II. 因子图的基本概念
- 1. 因子图的定义
- 2. SLAM 中的因子图
- A. 因子图的图示
- B. 因子图的因式
- C. 因子图的二分图形式
- III. 边缘化与消元运算的基本原理
- 1. 边缘化的定义
- 2. SLAM 中的边缘化
- 3. SLAM 中的变量消元算法
- A. 变量消元算法伪代码
- B. 消除第一个状态变量
- C. 消除第二个状态变量
- C. 消除第三个状态变量
- C. 消除第四个状态变量
- C. 消除第五个状态变量
- 4. 因子图与贝叶斯网络的简单说明
- IV. 总结
- 参考文献
关联博文:
因子图、边缘化与消元算法的抽丝剥茧 —— Notes for “Factor Graphs for Robot Perception”
基于 GTSAM 的因子图简单实例
I. 前言
第一次看 “Factor Graphs for Robot Perception”[1] 的时候直接被绕迷糊了, 又是概率图又是矩阵计算.
后来知道, 图是为了更加直观描述变量之间的关系与结构, 用于指导矩阵计算, 而矩阵计算给出实际推理计算的数值解.
有了曾经的困惑, 这里力求基于最基本的数学概念 (如工科大二概率论知识) 把 SLAM 中因子图的概念 (包括因子图、边缘化、变量消元算法) 啰啰嗦嗦地说明清楚.
这篇博文只涉及概念理解, 不涉及矩阵计算部分.
II. 因子图的基本概念
1. 因子图的定义
因子图是用来表示信息传递的一种图模型. 定义如下:
因子图定义[2]:
假设全局函数 (Global Function) g ( x 1 , x 2 , … , x n ) g(x_1, x_2, \ldots, x_n) g(x1,x2,…,xn) 因式分解为一些局部函数 (Local functions) f j ( X j ) f_j(X_j) fj(Xj) 的乘积.
g ( x 1 , x 2 , … , x n ) = ∏ j ∈ J f j ( X j ) (II-1-1) g(x_1, x_2, \ldots, x_n) = \prod_{j\in J} f_j(X_j) \tag{II-1-1} g(x1,x2,…,xn)=j∈J∏fj(Xj)(II-1-1)
式中
J J J —— 离散的指标集
X j X_j Xj —— { x 1 , x 2 , … , x n } \{x_1, x_2, \ldots, x_n\} {x1,x2,…,xn} 的子集
因子图是一个用来表示上述因子化的二分图 *.
因子图包含对应于每一个变量 x i x_i xi 的变量节点、对应于每个局部函数 f j f_j fj 的因子节点、连接变量节点 x i x_i xi 和因子节点 f j f_j fj (需以该变量 x i x_i xi 作为函数自变量) 的边.
* 二分图又称作二部图, 是图论中的一种特殊模型. 设 G = ( V , E ) G=(V,E) G=(V,E) 是一个无向图, 如果顶点 V V V 可分割为两个互不相交的子集 ( A , B ) (A,B) (A,B), 并且图中的每条边 ( i , j ) (i,j) (i,j) 所关联的两个顶点 i i i 和 j j j 分别属于这两个不同的顶点集 ( i ∈ A , j ∈ B ) (i \in A,j \in B) (i∈A,j∈B), 则称图 G G G 为一个二分图.
普通例子[2]: 令 g ( x 1 , x 2 , x 3 , x 4 , x 5 ) g(x_1,x_2,x_3,x_4,x_5) g(x1,x2,x3,x4,x5) 是五个变量的函数, 并且假设 g g g 可以表示为多个因子的乘积
g ( x 1 , x 2 , x 3 , x 4 , x 5 ) = f A ( x 1 ) f B ( x 2 ) f C ( x 1 , x 2 , x 3 ) f D ( x 3 , x 4 ) f E ( x 3 , x 5 ) (II-1-2) g(x_1,x_2,x_3,x_4,x_5) = f_A(x_1) f_B(x_2) f_C(x_1, x_2, x_3) f_D(x_3,x_4) f_E(x_3, x_5) \tag{II-1-2} g(x1,x2,x3,x4,x5)=fA(x1)fB(x2)fC(x1,x2,x3)fD(x3,x4)fE(x3,x5)(II-1-2)
所以指标集 J = { A , B , C , D , E } J=\{A, B, C, D, E\} J={A,B,C,D,E}, 变量子集 x A = { x 1 } x_A=\{x_1\} xA={x1}, x B = { x 2 } x_B=\{x_2\} xB={x2}, x C = { x 1 , x 2 , x 3 } x_C=\{x_1, x_2, x_3\} xC={x1,x2,x3}, x D = { x 3 , x 4 } x_D=\{x_3, x_4\} xD={x3,x4}, x E = { x 3 , x 5 } x_E=\{x_3, x_5\} xE={x3,x5}. 对应于上式的因子图如 Fig. 1 所示.
显然变量节点集合 { x 1 , x 2 , x 3 , x 4 , x 5 } \{x_1, x_2,x_3,x_4,x_5\} {x1,x2,x3,x4,x5} 与因子节点集合 { f A , f B , f C , f D , f E } \{f_A, f_B, f_C, f_D, f_E\} {fA,fB,fC,fD,fE} 被分割为了两个不相交的部分, 可以验证为二分图.
事实上, 只要各变量节点之间不直接相连及各因子节点之间不直接相连, 就一定是二分图.
Fig. 1 一个简单的因子图例子
2. SLAM 中的因子图
A. 因子图的图示
SLAM 例子[1]:
机器人先验的初始起点为 x 1 x_1 x1, 在该点上进行了一元测量 (Unary Measurement, 如 GPS、UWB 等绝对位姿测量) z 1 z_1 z1; 同时又探测到了路标 l 1 l_1 l1 并进行了二元测量 (Binary Measurement, 如激光扫描测量) z 2 z_2 z2.
然后机器人前进到了新的地点 x 2 x_2 x2, 并记录了从 x 1 x_1 x1 到 x 2 x_2 x2 的里程计信息; 在该点上也侦测到了路标点 l 1 l_1 l1 并进行了二元测量 z 3 z_3 z3.
机器人继续前进到 x 3 x_3 x3 并记录了这一段路程的里程计信息; 同时, 机器人在该点观察到了另一个路标点 l 2 l_2 l2 并记录了测量结果 z 4 z_4 z4.
对以上机器人运行过程进行概率计算为:
p ( X , Z ) = p ( x 1 ) ⏟ Prior on x 1 × p ( x 2 ∣ x 1 ) p ( x 3 ∣ x 2 ) ⏟ Odometries on x 1 → x 2 → x 3 × p ( l 1 ) p ( l 2 ) ⏟ Priors on l 1 and l 2 × p ( z 1 ∣ x 1 ) ⏟ Unary meas. at x 1 × p ( z 2 ∣ x 1 , l 1 ) ⏟ Binary Meas. btw. x 1 and l 1 × p ( z 3 ∣ x 2 , l 1 ) ⏟ Meas. btw. x 2 and l 1 × p ( z 4 ∣ x 3 , l 2 ) ⏟ Meas. btw. x 3 and l 2 (II-2-1) \begin{aligned} p(X, Z) = & \underbrace{p(x_1)}_{\color{darkgreen}\text{Prior on }x_1} \times \underbrace{p(x_2|x_1)\; p(x_3| x_2)}_{\color{blue}\text{Odometries on } x_1\rightarrow x_2 \rightarrow x_3} \\ & \times \underbrace{p(l_1) \; p(l_2)}_{\color{darkred}\text{Priors on } l_1 \text{ and } l_2} \times \underbrace{p(z_1|x_1)}_{\color{green}\text{Unary meas. at } x_1} \\ & \times \underbrace{p(z_2|x_1, l_1)}_{\text{Binary Meas. btw. } x_1\text{ and } l_1} \times \underbrace{p(z_3|x_2, l_1)}_{\color{darkblue}\text{Meas. btw. } x_2 \text{ and } l_1} \\ & \times \underbrace{p(z_4|x_3, l_2)}_{\color{darkred}\text{Meas. btw. } x_3\text{ and } l_2} \end{aligned} \tag{II-2-1} p(X,Z)=Prior on x1 p(x1)×Odometries on x1→x2→x3 p(x2∣x1)p(x3∣x2)×Priors on l1 and l2 p(l1)p(l2)×Unary meas. at x1 p(z1∣x1)×Binary Meas. btw. x1 and l1 p(z2∣x1,l1)×Meas. btw. x2 and l1 p(z3∣x2,l1)×Meas. btw. x3 and l2 p(z4∣x3,l2)(II-2-1)
此处状态变量 X = { x 1 , x 2 , x 3 , l 1 , l 2 } X = \{x_1, x_2, x_3, l_1, l_2\} X={x1,x2,x3,l1,l2}, 测量值 Z = { z 1 , z 2 , z 3 , z 4 } Z= \{z_1, z_2,z_3, z_4\} Z={z1,z2,z3,z4}.
根据以上机器人运行过程以及联合概率计算式 (II-2-1) 已经可以画出如下机器人运行对应的因子图.
状态变量以圆圈表示. 上式右手边的 9 个概率因式项就对应于 9 个因子, 以黑色小正方形表示. 而每个因子中包含的状态变量对应于图中的连接边.
Fig. 2 一个简单的 SLAM 中的因子图例子
B. 因子图的因式
为了将式 (II-2-1) 严格匹配因子图定义式 (II-1-1), 需要
- 将联合概率计算中的条件概率项表示成因子图中的局部函数形式, 此处直接转换以构造因子图.
- 将形如 p ( Z ∣ X ) p(Z|X) p(Z∣X) 的概率函数转变为以状态变量 X X X (而非测量值 Z Z Z) 作为自变量的因子图局部函数, 这就要引入似然 (Likelihood) 的概念.
似然函数定义[3]:
统计学中, 似然函数是一种关于统计模型参数的函数. 给定输出 z z z 时, 关于参数 θ \theta θ 的似然函数 L ( θ ∣ z ) L(\theta|z) L(θ∣z) (在数值上) 等于给定参数 θ \theta θ 后变量 X X X 的概率
L ( θ ∣ z ) = P ( X = z ∣ θ ) (II-2-2) L(\theta|z)=P(X=z|\theta) \tag{II-2-2} L(θ∣z)=P(X=z∣θ)(II-2-2)
“似然” 与 “概率” 意思相近, 都是指某种事件发生的可能性. 但是在统计学中, “似然” 和 “概率” 又有明确的区分. 概率用于在已知一些参数的情况下, 预测接下来的观测所得到的结果. 而似然则是用于在已知某些观测所得到的结果时, 对有关事物的性质的参数进行估计.
这里我们记给定测量值 Z Z Z 情况下状态变量 X X X 的似然为 l ( X ; Z ) l(X;Z) l(X;Z). 根据上述似然的定义, 可知
l ( X ; Z ) = p ( Z ∣ X ) (II-2-3) l(X; Z) = p(Z|X) \tag{II-2-3} l(X;Z)=p(Z∣X)(II-2-3)
那么联合概率式 (II-2-1) 就可以写成
p ( X , Z ) = p ( x 1 ) p ( x 2 ∣ x 1 ) p ( x 3 ∣ x 2 ) × p ( l 1 ) p ( l 2 ) × l ( x 1 ; z 1 ) × l ( x 1 , l 1 ; z 2 ) × l ( x 2 , l 1 ; z 3 ) × p ( x 3 , l 2 ; z 4 ) (II-2-4) \begin{aligned} p(X, Z) = &\; {p(x_1)} \; {p(x_2|x_1)\; p(x_3| x_2)} \\ & \times {p(l_1) \; p(l_2)} \times {l(x_1; z_1)} \\ & \times {l(x_1, l_1; z_2)} \times {l(x_2, l_1; z_3)} \\ & \times {p(x_3, l_2; z_4)} \end{aligned} \tag{II-2-4} p(X,Z)=p(x1)p(x2∣x1)p(x3∣x2)×p(l1)p(l2)×l(x1;z1)×l(x1,l1;z2)×l(x2,l1;z3)×p(x3,l2;z4)(II-2-4)
注意似然函数 l ( ⋅ ) l(\cdot) l(⋅) 不要和路标位置变量 { l 1 , l 2 } \{l_1, l_2\} {l1,l2} 搞混.
将上式中的概率因子都记成局部因子 ϕ i ( ⋅ ) \phi_i(\cdot) ϕi(⋅), 则上式形式上整理为
Φ ( X ) = ϕ 1 ( x 1 ) ϕ 2 ( x 2 , x 1 ) ϕ 3 ( x 3 , x 2 ) × ϕ 4 ( l 1 ) ϕ 5 ( l 2 ) × ϕ 6 ( x 1 ) × ϕ 7 ( x 1 , l 1 ) × ϕ 8 ( x 2 , l 1 ) × ϕ 9 ( x 3 , l 2 ) (II-2-5) \begin{aligned} \Phi(X)\; = \;& \;{\phi_1(x_1)} \; {\phi_2(x_2, x_1)\; \phi_3(x_3, x_2)} \\ & \times {\phi_4(l_1) \; \phi_5(l_2)} \times {\phi_6(x_1)} \\ & \times {\phi_7(x_1, l_1)} \times {\phi_8(x_2, l_1)} \\ & \times {\phi_9(x_3, l_2)} \end{aligned} \tag{II-2-5} Φ(X)=ϕ1(x1)ϕ2(x2,x1)ϕ3(x3,x2)×ϕ4(l1)ϕ5(l2)×ϕ6(x1)×ϕ7(x1,l1)×ϕ8(x2,l1)×ϕ9(x3,l2)(II-2-5)
其中 ϕ 1 ( x 1 ) = p ( x 1 ) \phi_1(x_1) = p(x_1) ϕ1(x1)=p(x1), ϕ 2 ( x 2 , x 1 ) = p ( x 2 ∣ x 1 ) \phi_2(x_2, x_1) = p(x_2|x_1) ϕ2(x2,x1)=p(x2∣x1), ϕ 3 ( x 3 , x 2 ) = p ( x 3 ∣ x 2 ) \phi_3(x_3, x_2) = p(x_3| x_2) ϕ3(x3,x2)=p(x3∣x2), ϕ 4 ( l 1 ) = p ( l 1 ) \phi_4(l_1) = p(l_1) ϕ4(l1)=p(l1), ϕ 5 ( l 2 ) = p ( l 2 ) \phi_5(l_2) = p(l_2) ϕ5(l2)=p(l2), ϕ 6 ( x 1 ) = l ( x 1 ; z 1 ) \phi_6(x_1) = l(x_1; z_1) ϕ6(x1)=l(x1;z1), ϕ 7 ( x 1 , l 1 ) = l ( x 1 , l 1 ; z 2 ) \phi_7(x_1, l_1) = l(x_1, l_1; z_2) ϕ7(x1,l1)=l(x1,l1;z2), ϕ 8 ( x 2 , l 1 ) = l ( x 2 , l 1 ; z 3 ) \phi_8(x_2, l_1) = l(x_2, l_1; z_3) ϕ8(x2,l1)=l(x2,l1;z3), ϕ 9 ( x 3 , l 2 ) = p ( x 3 , l 2 ; z 4 ) \phi_9(x_3, l_2) = p(x_3, l_2; z_4) ϕ9(x3,l2)=p(x3,l2;z4).
因为全局/局部因子代表的是关于状态变量的函数, 故不在全局/局部因子中显式地写出测量值.
式 (II-2-5) 转为图的形式, 就完全对应于 Fig. 2.
C. 因子图的二分图形式
因子图定义中有关于二分图的要求.
实际上, Fig. 2 很容易重新排布为 Fig. 3 所示的二分图形式.
不过结构上还是 Fig. 2 比起 Fig. 3 更清晰和更直观. Fig. 3 仅应用于验证二分图性质, 指导实际计算还是用 Fig. 2.
至此, 我们完全获得了一个 SLAM 场景下的因子图模型.
Fig. 3 一个简单的 SLAM 中的因子图例子转换为二分图形式
III. 边缘化与消元运算的基本原理
1. 边缘化的定义
因子图是用来表示信息传递的, 所谓信息传递其实就是边缘化的计算过程, 而边缘化又实现了消元以及图模型的简化.
边缘化/消元计算实现了从描述整个机器人 SLAM 过程的联合概率转换得到描述机器人运行中某一变量点发生的概率 (即边缘概率).
我们先把边缘分布的概念理解清晰.
边缘分布定义[4]:
边缘分布 (Marginal Distribution) 指在概率论和统计学的多维随机变量中, 只包含其中部分变量的概率分布.
边缘分布与边缘化例子[4]:
假设有两个随机变量 ( x , y ) (x,y) (x,y), 和两个变量相关的概率分布 P ( x , y ) P(x,y) P(x,y) 称为联合分布, 而关于其中一个特定变量的概率分布为边缘分布.
如果我们只考虑其中一个变量 x x x，另一个变量 y y y 可以取任何它可能取的值，则得到随机变量 x x x 的分布称为二维随机变量 ( x , y ) (x,y) (x,y) 关于 x x x 的边缘分布. 即
P ( x ) = ∑ y P ( x , y ) = ∑ y P ( x ∣ y ) P ( y ) (III-1-1) P(x) = \sum_{y} P(x,y) = \sum_{y} P(x|y)P(y) \tag{III-1-1} P(x)=y∑P(x,y)=y∑P(x∣y)P(y)(III-1-1)
在这个边缘分布中, 我们得到只关于一个变量 x x x 的概率分布, 而不再考虑另一变量 y y y 的影响. 上式进行了降维/消元操作, 就是边缘化.
在实际应用中, 例如人工神经网络的神经元互相关联, 在计算它们各自的参数的时候, 就会使用边缘分布计算得到某一特定神经元 (变量) 的值[4].
同理, 分量 y y y 的概率分布称为 ( x , y ) (x, y) (x,y) 的关于 y y y 的边缘分布.
普通例子:
边缘化计算推广到更多变量的情况, 从联合概率式 (II-1-2) 中获得边缘概率函数 g 1 ( x 1 ) g_1(x_1) g1(x1)
g 1 ( x 1 ) = ∑ { x 2 , x 3 , x 4 , x 5 } g ( x 1 , x 2 , x 3 , x 4 , x 5 ) = f A ( x 1 ) ⋅ { ∑ x 2 f B ( x 2 ) ⋅ [ ∑ x 3 f C ( x 1 , x 2 , x 3 ) ⋅ ( ∑ x 4 f D ( x 3 , x 4 ) ) ⋅ ( ∑ x 5 f E ( x 3 , x 5 ) ) ] } (III-1-2) \begin{aligned} g_1(x_1) &= \sum_{\{x_2, x_3, x_4, x_5\}} g(x_1,x_2,x_3,x_4,x_5) \\ &= f_A(x_1) \cdot \left\{\sum_{x_2} f_B(x_2)\cdot \left[ \sum_{x_3} f_C(x_1, x_2, x_3) \cdot \left(\sum_{x_4} f_D(x_3,x_4)\right)\cdot \left(\sum_{x_5} f_E(x_3, x_5)\right)\right]\right\} \end{aligned} \tag{III-1-2} g1(x1)={x2,x3,x4,x5}∑g(x1,x2,x3,x4,x5)=fA(x1)⋅{x2∑fB(x2)⋅[x3∑fC(x1,x2,x3)⋅(x4∑fD(x3,x4))⋅(x5∑fE(x3,x5))]}(III-1-2)
以上边缘化计算的顺序是 x 5 → x 4 → x 3 → x 2 x_5 \rightarrow x_4 \rightarrow x_3 \rightarrow x_2 x5→x4→x3→x2, 最后得到关于变量 x 1 x_1 x1 的边缘概率.
说明:
边缘化的顺序影响中间计算步骤, 尤其是矩阵计算的复杂度与稀疏性, 但不影响最终的计算结果;
联合概率 (联合分布) 决定边缘概率 (边缘分布), 边缘概率 (边缘分布) 一般不能决定联合概率 (联合分布).
2. SLAM 中的边缘化
我们先将式 (II-2-5) 或 Fig. 2 所示的简单 SLAM 例子参照式 (III-1-2) 进行按照以 l 1 → l 2 → x 1 → x 2 → x 3 l_1 \rightarrow l_2 \rightarrow x_1 \rightarrow x_2 \rightarrow x_3 l1→l2→x1→x2→x3 为顺序的边缘化计算.
Φ 5 ( x 3 ) = ( a ) ∑ { x 2 , x 1 , l 2 , l 1 } [ ϕ 1 ( x 1 ) ϕ 2 ( x 2 , x 1 ) ϕ 3 ( x 3 , x 2 ) ϕ 4 ( l 1 ) ‾ ϕ 5 ( l 2 ) ϕ 6 ( x 1 ) ϕ 7 ( x 1 , l 1 ) ‾ ϕ 8 ( x 2 , l 1 ) ‾ ϕ 9 ( x 3 , l 2 ) ] = ( b ) ∑ { x 2 , x 1 , l 2 } [ ϕ 1 ( x 1 ) ϕ 2 ( x 2 , x 1 ) ϕ 3 ( x 3 , x 2 ) ϕ 5 ( l 2 ) ‾ ϕ 6 ( x 1 ) ϕ 9 ( x 3 , l 2 ) ‾ ( ∑ l 1 ϕ 4 ( l 1 ) ϕ 7 ( x 1 , l 1 ) ϕ 8 ( x 2 , l 1 ) ) ⏟ ≜ τ 1 ( x 1 , x 2 ) ] = ( c ) ∑ { x 2 , x 1 } [ ϕ 1 ( x 1 ) ‾ ϕ 2 ( x 2 , x 1 ) ‾ ϕ 3 ( x 3 , x 2 ) ϕ 6 ( x 1 ) ‾ τ 1 ( x 1 , x 2 ) ‾ ( ∑ l 2 ϕ 5 ( l 2 ) ϕ 9 ( x 3 , l 2 ) ) ⏟ ≜ τ 2 ( x 3 ) ] = ( d ) ∑ x 2 [ ϕ 3 ( x 3 , x 2 ) τ 2 ( x 3 ) ( ∑ x 1 ϕ 6 ( x 1 ) τ 1 ( x 1 , x 2 ) ϕ 1 ( x 1 ) ϕ 2 ( x 2 , x 1 ) ) ⏟ ≜ τ 3 ( x 2 ) ] = ( e ) τ 2 ( x 3 ) ( ∑ x 2 ϕ 3 ( x 3 , x 2 ) τ 3 ( x 2 ) ) ⏟ ≜ τ 4 ( x 3 ) = ( f ) τ 2 ( x 3 ) τ 4 ( x 3 ) (III-2-1) \begin{aligned} {\Phi_{5}(x_3)} &\overset{(a)}{=} \sum_{\{x_2, x_1, l_2, l_1\}} \left[ {\phi_1(x_1)} {\phi_2(x_2, x_1) \phi_3(x_3, x_2)} \underline{{\phi_4(l_1)}} \phi_5(l_2) {\phi_6(x_1)} \underline{\phi_7(x_1, l_1)} \underline{\phi_8(x_2, l_1)} {\phi_9(x_3, l_2)}\right]\\ &\overset{(b)}{=} \sum_{\{x_2, x_1, l_2\}} \left[ {\phi_1(x_1)} {\phi_2(x_2, x_1) \phi_3(x_3, x_2)} \underline{\phi_5(l_2)} {\phi_6(x_1)} \underline{\phi_9(x_3, l_2)} \underbrace{\left( \sum_{l_1} {{\phi_4(l_1)}} {\phi_7(x_1, l_1)} {\phi_8(x_2, l_1)} \right)}_{\color{darkgreen} \triangleq \tau_{1}(x_1, x_2)}\right]\\ & \overset{(c)}{=} \sum_{\{x_2, x_1\}} \left[ \underline{\phi_1(x_1)} \underline{\phi_2(x_2, x_1)} \phi_3(x_3, x_2) \underline{\phi_6(x_1)} \underline{\color{darkgreen} \tau_1(x_1, x_2)} \underbrace{\left(\sum_{l_2} {\phi_5(l_2)} {\phi_9(x_3, l_2)}\right)}_{\color{darkred}\triangleq \tau_2(x_3)} \right] \\ & \overset{(d)}{=} \sum_{x_2} \left[ \phi_3(x_3, x_2) {\color{darkred}\tau_2(x_3)} \underbrace{\left( \sum_{x_1} {\phi_6(x_1)} {\color{darkgreen} \tau_1(x_1, x_2)} {\phi_1(x_1)} {\phi_2(x_2, x_1)}\right)}_{\color{blue}\triangleq \tau_3 (x_2)} \right] \\ & \overset{(e)}{=} {\color{darkred}\tau_2(x_3)} \underbrace{\left(\sum_{x_2} \phi_3(x_3, x_2) {\color{blue} \tau_3 (x_2)} \right)}_{\color{darkcyan}\triangleq \tau_{4}(x_3)} \\ & \overset{(f)}{=} {\color{darkred}\tau_2(x_3)} {\color{darkcyan}\tau_{4}(x_3)} \\ \end{aligned} \tag{III-2-1} Φ5(x3)=(a){x2,x1,l2,l1}∑[ϕ1(x1)ϕ2(x2,x1)ϕ3(x3,x2)ϕ4(l1)ϕ5(l2)ϕ6(x1)ϕ7(x1,l1)ϕ8(x2,l1)ϕ9(x3,l2)]=(b){x2,x1,l2}∑ ϕ1(x1)ϕ2(x2,x1)ϕ3(x3,x2)ϕ5(l2)ϕ6(x1)ϕ9(x3,l2)≜τ1(x1,x2) (l1∑ϕ4(l1)ϕ7(x1,l1)ϕ8(x2,l1)) =(c){x2,x1}∑ ϕ1(x1)ϕ2(x2,x1)ϕ3(x3,x2)ϕ6(x1)τ1(x1,x2)≜τ2(x3) (l2∑ϕ5(l2)ϕ9(x3,l2)) =(d)x2∑ ϕ3(x3,x2)τ2(x3)≜τ3(x2) (x1∑ϕ6(x1)τ1(x1,x2)ϕ1(x1)ϕ2(x2,x1)) =(e)τ2(x3)≜τ4(x3) (x2∑ϕ3(x3,x2)τ3(x2))=(f)τ2(x3)τ4(x3)(III-2-1)
以上是应用上一小节中最基本的边缘概率定义, 进行暴力计算的过程.
上式中步骤 (a) 是边缘化的基本定义. 参考上一小节可知, 将对联合概率 Φ ( X ) \Phi(X) Φ(X) 进行关于 l 1 l_1 l1、 l 2 l_2 l2、 x 1 x_1 x1 和 x 2 x_2 x2 的边缘化计算.
步骤 (b) 是对因子图执行关于变量 l 1 l_1 l1 的边缘化.
因为边缘化的计算只涉及含有 l 1 l_1 l1 作为自变量的这些因子 (局部函数), 所以先把以 l 1 l_1 l1 为函数自变量的因子全部找出. 在图上就是所有与变量节点 l 1 l_1 l1 之间有连接边的因子节点, 即所有与变量节点 l 1 l_1 l1 接邻的因子节点.
再进行关于变量 l 1 l_1 l1 的边缘化计算. 执行该步骤以后, 变量 l 1 l_1 l1 被消元了, 留下了 τ 1 ( x 1 , x 2 ) \tau_1(x_1, x_2) τ1(x1,x2) 加入余下的因子图中. 该步骤余下的因子图是关于变量 { x 3 , x 2 , x 1 , l 2 } \{x_3, x_2, x_1, l_2\} {x3,x2,x1,l2} 的简化过的因子图. 看起来 τ 1 ( x 1 , x 2 ) \tau_1 (x_1, x_2) τ1(x1,x2) 好像是 l 1 l_1 l1 被消元后留给剩余因子图的 “信息礼物”, 后文将继续讨论.
步骤 (c) 是关于变量 l 2 l_2 l2 的边缘化. 执行该步骤以后, 变量 l 2 l_2 l2 被消元了, 留下了 τ 2 ( x 3 ) \tau_2(x_3) τ2(x3) 作为新因子加入余下的因子图中. 该步骤余下的因子图是关于变量 { x 3 , x 2 , x 1 } \{x_3, x_2, x_1\} {x3,x2,x1} 的简化因子图.
步骤 (d) 是关于变量 x 1 x_1 x1 的边缘化. 执行该步骤以后, 变量 x 1 x_1 x1 被消元了, 留下了 τ 3 ( x 2 ) \tau_3(x_2) τ3(x2) 作为新因子加入余下的因子图中. 该步骤余下的因子图是关于变量 { x 3 , x 2 } \{x_3, x_2\} {x3,x2} 的简化因子图.
步骤 (e) 是关于变量 x 2 x_2 x2 的边缘化. 执行该步骤以后, 变量 x 2 x_2 x2 被消元了, 留下了 τ 4 ( x 3 ) \tau_4(x_3) τ4(x3) 作为新因子加入余下的简化的因子图中. 该步骤余下的因子图是关于变量 { x 3 } \{x_3\} {x3} 的简化因子图.
步骤 (f) 接手经过关于 l 1 → l 2 → x 1 → x 2 l_1 \rightarrow l_2 \rightarrow x_1 \rightarrow x_2 l1→l2→x1→x2 消元的因子图时, 只剩下了一个关于 x 3 x_3 x3 的因子 τ x ( x 3 ) τ 4 ( x 3 ) \tau_x(x_3)\,\tau_4(x_3) τx(x3)τ4(x3). 不再需要其他计算, 这样已经获得了关于变量 x 3 x_3 x3 的边缘概率 (严格说是正比于边缘概率, 相差一个常系数对 SLAM 算法中要进行的最优化计算并不会产生影响), 边缘化的目的达到.
说明:
此处边缘化的顺序 l 1 → l 2 → x 1 → x 2 → x 3 l_1 \rightarrow l_2 \rightarrow x_1 \rightarrow x_2 \rightarrow x_3 l1→l2→x1→x2→x3 是人为设定的, 有没有更优的顺序？
这是下一步基于因子图 SLAM 算法研究的关键点[1]. 本篇博文不涉及.
3. SLAM 中的变量消元算法
A. 变量消元算法伪代码
有了上一小节因子图边缘化计算的基础, 下面开始理解 “Factor Graphs for Robot Perception” 中的因子图的变量消元算法[1].
算法一: 变量消元算法 function ELIMINATE ( Φ 1 : n ) ⊳ 给定一个有 n 个变量的因子图 for j = 1 … n do ⊳ 对所有 n 个变量循环 p ( x j ∣ S j ) , Φ j + 1 : n ← EliminateOne ( Φ j : n , x j ) ⊳ 从因子图 Φ j : n 中消除变量 x j return p ( x 1 ∣ S 1 ) p ( x 2 ∣ S 2 ) … p ( x n ) ⊳ 返回一个贝叶斯网络 \begin{array}{lll} &\textbf{算法一: 变量消元算法} & \\ & \textbf{function} \; \;\text{ELIMINATE}(\Phi_{1:n}) & \rhd 给定一个有 \,n \,个变量的因子图\\ & \qquad \textbf{for}\;\; j=1 \ldots n \;\;\textbf{do} & \rhd 对所有\, n\, 个变量循环\\ & \qquad \qquad p(x_j|S_j), \Phi_{j+1:n}\; \leftarrow\; \text{EliminateOne}(\Phi_{j:n}, x_j) &\rhd 从因子图\, \Phi_{j:n} \, {中消除变量} \, x_j\\ & \qquad \textbf{return}\;\; p(x_1|S_1) p(x_2|S_2)\ldots p(x_n) &\rhd 返回一个贝叶斯网络 \end{array} 算法一: 变量消元算法functionELIMINATE(Φ1:n)forj=1…ndop(xj∣Sj),Φj+1:n←EliminateOne(Φj:n,xj)returnp(x1∣S1)p(x2∣S2)…p(xn)⊳给定一个有n个变量的因子图⊳对所有n个变量循环⊳从因子图Φj:n中消除变量xj⊳返回一个贝叶斯网络
算法二: 从因子图 Φ j : n 中消除变量 x j function EliminateOne ( Φ j : n , x j ) ⊳ 给定一个简化因子图 Φ j : n 移除所有与变量 x j 接邻的因子 ϕ i ( X i ) ⊳ 所有与变量节点 x i 有连接边的因子全部从简化因子图 Φ j : n 中移除 S ( x j ) ← 上一步中除了 x j 以外所有涉及的变量 ⊳ 上一步中所有移除的因子中涉及的变量集合 ( 除了 x j ) 定为分离子集合 ψ ( x j , S j ) ← ∏ i ϕ i ( X i ) ⊳ 产生乘积因子 ψ p ( x j ∣ S j ) τ ( S j ) ← ψ ( x j , S j ) ⊳ 因式分解这个乘积 ψ 增加新的因子 τ ( S j ) 到因子图中 ⊳ 新产生因子 τ ( S j ) 添加到余下的因子图中 , 得到新的简化因子图 Φ j + 1 : n return p ( x j ∣ S j ) , Φ j + 1 : n ⊳ 条件概率和新简化因子图 \begin{array}{lll} &\textbf{算法二: 从因子图}\, \Phi_{j:n} \, \textbf{中消除变量} \, x_j &\\ & \textbf{function} \; \;\text{EliminateOne}(\Phi_{j:n}, x_j) & \rhd \text{给定一个简化因子图} \, \Phi_{j:n}\\ & \qquad 移除所有与变量\, x_j \,接邻的因子\, \phi_i(X_i) &\rhd 所有与变量节点\, x_i\, 有连接边的因子全部从简化因子图 \, \Phi_{j:n} 中移除\\ & \qquad \mathcal{S}(x_j) \;\leftarrow \; 上一步中除了 \,x_j\,以外所有涉及的变量 &\rhd 上一步中所有移除的因子中涉及的变量集合\, (除了\, x_j)\, 定为分离子集合\\ & \qquad \psi(x_j,{S}_j) \;\leftarrow \; \prod_{i} \phi_i(X_i) &\rhd 产生乘积因子\, \psi\\ &\qquad p(x_j|S_j) \tau(S_j) \; \leftarrow \; \psi(x_j,{S}_j) & \rhd 因式分解这个乘积 \, \psi\\ &\qquad 增加新的因子 \,\tau(S_j)\, 到因子图中 & \rhd 新产生因子 \, \tau(S_j)\,添加到余下的因子图中, 得到新的简化因子图 \, \Phi_{j+1:n}\\ &\qquad \textbf{return} \; p(x_j|S_j), \Phi_{j+1:n} & \rhd 条件概率和新简化因子图 \end{array} 算法二: 从因子图Φj:n中消除变量xjfunctionEliminateOne(Φj:n,xj)移除所有与变量xj接邻的因子ϕi(Xi)S(xj)←上一步中除了xj以外所有涉及的变量ψ(xj,Sj)←∏iϕi(Xi)p(xj∣Sj)τ(Sj)←ψ(xj,Sj)增加新的因子τ(Sj)到因子图中returnp(xj∣Sj),Φj+1:n⊳给定一个简化因子图Φj:n⊳所有与变量节点xi有连接边的因子全部从简化因子图Φj:n中移除⊳上一步中所有移除的因子中涉及的变量集合(除了xj)定为分离子集合⊳产生乘积因子ψ⊳因式分解这个乘积ψ⊳新产生因子τ(Sj)添加到余下的因子图中,得到新的简化因子图Φj+1:n⊳条件概率和新简化因子图
以上两个算法中, 算法二是算法一的子算法. 算法一多次循环调用算法二构成因子图的变量消元算法.
下面结合上一节中式 (III-2-1) 的边缘化计算过程, 具体说明算法二中的原理和概念.
B. 消除第一个状态变量
我们先以消元 l 1 l_1 l1 为例一步步展开, 对应于边缘化计算式 (III-2-1) 中的 (b) 步骤.
序号 操作 0 此时调用 EliminateOne( Φ 1 : 5 , l 1 \Phi_{1:5}, l_1 Φ1:5,l1), 其中 Φ 1 : 5 \Phi_{1:5} Φ1:5 是初始因子图, 如 Fig. 2 所示, 公式为 Φ 1 : 5 ≜ ϕ 1 ( x 1 ) ϕ 2 ( x 2 , x 1 ) ϕ 3 ( x 3 , x 2 ) ϕ 4 ( l 1 ) ‾ ϕ 5 ( l 2 ) ϕ 6 ( x 1 ) ϕ 7 ( x 1 , l 1 ) ‾ ϕ 8 ( x 2 , l 1 ) ‾ ϕ 9 ( x 3 , l 2 ) \Phi_{1:5} \triangleq {\phi_1(x_1)} \; {\phi_2(x_2, x_1)\; \phi_3(x_3, x_2)}\; \underline{{\phi_4(l_1)}} \; \phi_5(l_2) \; {\phi_6(x_1)} \; \underline{\phi_7(x_1, l_1)}\; \underline{\phi_8(x_2, l_1)} \;{\phi_9(x_3, l_2)} Φ1:5≜ϕ1(x1)ϕ2(x2,x1)ϕ3(x3,x2)ϕ4(l1)ϕ5(l2)ϕ6(x1)ϕ7(x1,l1)ϕ8(x2,l1)ϕ9(x3,l2)
1 所有与 l 1 l_1 l1 接邻的因子有 { ϕ 4 ( l 1 ) , ϕ 7 ( x 1 , l 1 ) , ϕ 8 ( x 2 , l 1 ) } \{\phi_4(l_1), \phi_7(x_1, l_1), \phi_8(x_2, l_1)\} {ϕ4(l1),ϕ7(x1,l1),ϕ8(x2,l1)} 2 S 1 = S ( l 1 ) ≜ { x 1 , x 2 } S_1 =\mathcal{S}(l_1) \triangleq \{x_1, x_2\} S1=S(l1)≜{x1,x2} 3 ψ ( l 1 , S 1 ) = ϕ 4 ( l 1 ) ϕ 7 ( x 1 , l 1 ) ϕ 8 ( x 2 , l 1 ) \psi(l_1, S_1) = \phi_4(l_1)\, \phi_7(x_1, l_1)\, \phi_8(x_2, l_1) ψ(l1,S1)=ϕ4(l1)ϕ7(x1,l1)ϕ8(x2,l1) 4 因式分解 ψ ( l 1 , x 1 , x 2 ) = p ( l 1 ∣ x 1 , x 2 ) τ 1 ( x 1 , x 2 ) \psi (l_1, x_1, x_2) = p(l_1|x_1,x_2) \tau_1(x_1, x_2) ψ(l1,x1,x2)=p(l1∣x1,x2)τ1(x1,x2)
5 新的简化因子图, 如 Fig. 4 所示, 公式为 Φ 2 : 5 ≜ ϕ 1 ( x 1 ) ϕ 2 ( x 2 , x 1 ) ϕ 3 ( x 3 , x 2 ) ϕ 5 ( l 2 ) ϕ 6 ( x 1 ) ϕ 9 ( x 3 , l 2 ) τ 1 ( x 1 , x 2 ) \Phi_{2:5} \triangleq {\phi_1(x_1)} \; {\phi_2(x_2, x_1)\; \phi_3(x_3, x_2)}\; {\phi_5(l_2)} \; {\phi_6(x_1)} \; {\phi_9(x_3, l_2)} \;{\color{darkgreen} \tau_{1}(x_1, x_2)} Φ2:5≜ϕ1(x1)ϕ2(x2,x1)ϕ3(x3,x2)ϕ5(l2)ϕ6(x1)ϕ9(x3,l2)τ1(x1,x2)
直到上面算法第 4 步因子分解还比较容易理解. 然而,
(1) 算法中需要把 τ ( x 1 , x 2 ) \tau(x_1, x_2) τ(x1,x2) 添加到余下的因子图中, 这如何理解并且有何依据？
(2) 此处的变量消元算法与上一节中的边缘化计算式 (III-2-1) 有什么样的联系？
以上两个问题是真正理清边缘化和消元算法的关键.
第 4 步中由 ϕ 4 ( l 1 ) ϕ 7 ( x 1 , l 1 ) ϕ 8 ( x 2 , l 1 ) \phi_4(l_1)\, \phi_7(x_1, l_1)\, \phi_8(x_2, l_1) ϕ4(l1)ϕ7(x1,l1)ϕ8(x2,l1) 因式分解比较容易, 把包含 l 1 l_1 l1 的因式项给到 p ( l 1 ∣ x 1 , x 2 ) p(l_1|x_1,x_2) p(l1∣x1,x2), 而把不包含 l 1 l_1 l1 (仅包含 x 1 x_1 x1 和 x 2 x_2 x2) 的因式项给到 τ 1 ( x 1 , x 2 ) \tau_1(x_1, x_2) τ1(x1,x2).
如式 (III-2-1) 中 (b) 步骤对变量 l 1 l_1 l1 求和 (边缘化)
∑ l 1 ϕ 4 ( l 1 ) ϕ 7 ( x 1 , l 1 ) ϕ 8 ( x 2 , l 1 ) = factorize ∑ l 1 p ( l 1 ∣ x 1 , x 2 ) τ 1 ( x 1 , x 2 ) S 1 ≜ { x 1 , x 2 } = ∑ l 1 p ( l 1 ∣ S 1 ) τ 1 ( S 1 ) = ( ∑ l 1 p ( l 1 ∣ S 1 ) ) ⏟ = 1 τ 1 ( S 1 ) = τ 1 ( S 1 ) (III-3-1) \begin{aligned} \sum_{l_1} {{\phi_4(l_1)}} \; {\phi_7(x_1, l_1)}\; {\phi_8(x_2, l_1)} &\overset{\text{factorize}}{=} \sum_{l_1} p(l_1|x_1,x_2)\tau_1(x_1, x_2)\\ {\color{gray}S_1\triangleq \{x_1, x_2\}} \qquad & = \sum_{l_1} p(l_1|S_1)\tau_1(S_1)\\ &=\underbrace{\left(\sum_{l_1}p(l_1|S_1)\right)}_{= 1}\tau_1(S_1)\\ &=\tau_1(S_1) \\ \end{aligned} \tag{III-3-1} l1∑ϕ4(l1)ϕ7(x1,l1)ϕ8(x2,l1)S1≜{x1,x2}=factorizel1∑p(l1∣x1,x2)τ1(x1,x2)=l1∑p(l1∣S1)τ1(S1)==1 (l1∑p(l1∣S1))τ1(S1)=τ1(S1)(III-3-1)
由式 (III-3-1) 可知:
变量 l 1 l_1 l1 通过边缘化计算中被消元了, 留下新因子 τ 1 ( x 1 , x 2 ) \tau_1(x_1, x_2) τ1(x1,x2) 融合到移除了 { ϕ 4 ( l 1 ) , ϕ 7 ( x 1 , l 1 ) , ϕ 8 ( x 2 , l 1 ) } \{\phi_4(l_1), \phi_7(x_1, l_1), \phi_8(x_2, l_1)\} {ϕ4(l1),ϕ7(x1,l1),ϕ8(x2,l1)} 后的余下的因子图中, 形成新的简化因子图 Φ 2 : 5 \Phi_{2:5} Φ2:5.
新因子 τ 1 ( x 1 , x 2 ) \tau_1(x_1, x_2) τ1(x1,x2) 并不需要像式 (III-2-1) 那样进行暴力计算得到, 而只需像算法第 4 步那样对以该消元变量为自变量的所有因子的乘积作因式分解就可以.
而这个新因子 τ 1 ( S 1 ) \tau_1(S_1) τ1(S1) 就是消除变量 l 1 l_1 l1 过程中, 传递给 l 1 l_1 l1 的邻居们 S 1 S_1 S1 的信息因子. 这个新因子确保了在消除 l 1 l_1 l1 前的原因子图 Φ 1 : 5 \Phi_{1:5} Φ1:5 和消除 l 1 l_1 l1 后的新因子图 Φ 2 : 5 \Phi_{2:5} Φ2:5 各自所包含的信息不变.
消除 l 1 l_1 l1 后, 算法第 4 步中得到的条件概率 p ( l 1 ∣ S 1 ) p(l_1|S_1) p(l1∣S1) 变身为贝叶斯网络 (Bayesian Network) 节点 l 1 l_1 l1 的概率值 (条件概率值). 在具体推理计算状态变量 l 1 l_1 l1 的概率分布时, 就会用到这个由因式分解得到的藏于贝叶斯网络节点中的条件概率.
另外, 概率 p ( l 1 ∣ S 1 ) p(l_1|S_1) p(l1∣S1) 中事件 l 1 l_1 l1 的条件是事件 S 1 S_1 S1, 代表了因果关系, 故转化得到贝叶斯网络中 { x 1 , x 2 } \{x_1,x_2\} {x1,x2} 指向 l 1 l_1 l1.
以上过程得到第一个简化因子图 Φ 2 : 5 \Phi_{2:5} Φ2:5 如 Fig. 4 所示.
Fig. 4 消去第一个状态变量后得到简化的因子图
C. 消除第二个状态变量
接着, 我们消除第二个变量 l 2 l_2 l2, 对应于边缘化计算式 (III-2-1) 中的 (c) 步骤.
序号 操作 0 此时调用 EliminateOne( Φ 2 : 5 , l 2 \Phi_{2:5}, l_2 Φ2:5,l2) Φ 2 : 5 = ϕ 1 ( x 1 ) ϕ 2 ( x 2 , x 1 ) ϕ 3 ( x 3 , x 2 ) ϕ 5 ( l 2 ) ‾ ϕ 6 ( x 1 ) ϕ 9 ( x 3 , l 2 ) ‾ τ 1 ( x 1 , x 2 ) \Phi_{2:5} = {\phi_1(x_1)} \; {\phi_2(x_2, x_1)\; \phi_3(x_3, x_2)}\; \underline{\phi_5(l_2)} \; {\phi_6(x_1)} \; \underline{\phi_9(x_3, l_2)} \;{\color{darkgreen} \tau_{1}(x_1, x_2)} Φ2:5=ϕ1(x1)ϕ2(x2,x1)ϕ3(x3,x2)ϕ5(l2)ϕ6(x1)ϕ9(x3,l2)τ1(x1,x2)
1 所有与 l 2 l_2 l2 接邻的因子有 { ϕ 5 ( l 2 ) , ϕ 9 ( x 3 , l 2 ) } \{\phi_5(l_2), \phi_9(x_3, l_2)\} {ϕ5(l2),ϕ9(x3,l2)} 2 S 2 = S ( l 2 ) ≜ { x 3 } S_2 =\mathcal{S}(l_2) \triangleq \{x_3\} S2=S(l2)≜{x3} 3 ψ ( l 2 , S 2 ) = ϕ 5 ( l 2 ) ϕ 9 ( x 3 , l 2 ) \psi(l_2, S_2) = \phi_5(l_2)\; \phi_9(x_3, l_2) ψ(l2,S2)=ϕ5(l2)ϕ9(x3,l2) 4 因式分解 ψ ( l 2 , x 3 ) = p ( l 2 ∣ x 3 ) τ 2 ( x 3 ) \psi (l_2, x_3) = p(l_2|x_3)\tau_2(x_3) ψ(l2,x3)=p(l2∣x3)τ2(x3)
5 新的简化因子图, 如 Fig. 5 所示, 公式为 Φ 3 : 5 ≜ ϕ 1 ( x 1 ) ϕ 2 ( x 2 , x 1 ) ϕ 3 ( x 3 , x 2 ) ϕ 6 ( x 1 ) τ 1 ( x 1 , x 2 ) τ 2 ( x 3 ) \Phi_{3:5} \triangleq {\phi_1(x_1)} \; {\phi_2(x_2, x_1)\; \phi_3(x_3, x_2)}\; {\phi_6(x_1)} \;{\color{darkgreen} \tau_{1}(x_1, x_2)} \; {\color{darkred} \tau_2(x_3)} Φ3:5≜ϕ1(x1)ϕ2(x2,x1)ϕ3(x3,x2)ϕ6(x1)τ1(x1,x2)τ2(x3)
Fig. 5 消去第二个状态变量后得到简化的因子图
C. 消除第三个状态变量
然后, 我们消除第三个变量 x 1 x_1 x1, 对应于边缘化计算式 (III-2-1) 中的 (d) 步骤.
序号 操作 0 此时调用 EliminateOne( Φ 3 : 5 , x 1 \Phi_{3:5}, x_1 Φ3:5,x1) Φ 3 : 5 ≜ ϕ 1 ( x 1 ) ‾ ϕ 2 ( x 2 , x 1 ) ‾ ϕ 3 ( x 3 , x 2 ) ϕ 6 ( x 1 ) ‾ τ 1 ( x 1 , x 2 ) ‾ τ 2 ( x 3 ) \Phi_{3:5} \triangleq \underline{\phi_1(x_1)} \; \underline{\phi_2(x_2, x_1)}\; \phi_3(x_3, x_2)\; \underline{\phi_6(x_1)} \;\underline{\color{darkgreen} \tau_{1}(x_1, x_2)} \; {\color{darkred} \tau_2(x_3)} Φ3:5≜ϕ1(x1)ϕ2(x2,x1)ϕ3(x3,x2)ϕ6(x1)τ1(x1,x2)τ2(x3)
1 所有与 x 1 x_1 x1 接邻的因子有 { ϕ 1 ( x 1 ) , ϕ 2 ( x 2 , x 1 ) , ϕ 6 ( x 1 ) , τ 1 ( x 1 , x 2 ) } \{ {\phi_1(x_1)} , {\phi_2(x_2, x_1)}, {\phi_6(x_1)} , {\color{darkgreen} \tau_{1}(x_1, x_2)} \} {ϕ1(x1),ϕ2(x2,x1),ϕ6(x1),τ1(x1,x2)} 2 S 3 = S ( x 1 ) ≜ { x 2 } S_3 =\mathcal{S}(x_1) \triangleq \{x_2\} S3=S(x1)≜{x2} 3 ψ ( x 1 , S 3 ) = ϕ 1 ( x 1 ) ϕ 2 ( x 2 , x 1 ) ϕ 6 ( x 1 ) τ 1 ( x 1 , x 2 ) \psi(x_1, S_3) = {\phi_1(x_1)} \; {\phi_2(x_2, x_1)}\; {\phi_6(x_1)} \;{\color{darkgreen} \tau_{1}(x_1, x_2)} ψ(x1,S3)=ϕ1(x1)ϕ2(x2,x1)ϕ6(x1)τ1(x1,x2) 4 因式分解 ψ ( x 1 , x 2 ) = p ( x 1 ∣ x 2 ) τ 3 ( x 2 ) \psi(x_1, x_2) = p(x_1|x_2)\tau_3(x_2) ψ(x1,x2)=p(x1∣x2)τ3(x2)
5 新的简化因子图, 如 Fig. 6 所示, 公式为 Φ 4 : 5 ≜ ϕ 3 ( x 3 , x 2 ) τ 2 ( x 3 ) τ 3 ( x 2 ) \Phi_{4:5} \triangleq \phi_3(x_3, x_2)\; {\color{darkred} \tau_2(x_3)} \; {\color{blue}\tau_3(x_2)} Φ4:5≜ϕ3(x3,x2)τ2(x3)τ3(x2)
Fig. 6 消去第三个状态变量后得到简化的因子图
C. 消除第四个状态变量
再然后, 我们消除第四个变量 x 2 x_2 x2, 对应于边缘化计算式 (III-2-1) 中的 (e) 步骤.
序号 操作 0 此时调用 EliminateOne( Φ 4 : 5 , x 2 \Phi_{4:5}, x_2 Φ4:5,x2) Φ 4 : 5 ≜ ϕ 3 ( x 3 , x 2 ) ‾ τ 2 ( x 3 ) τ 3 ( x 2 ) ‾ \Phi_{4:5} \triangleq \underline{\phi_3(x_3, x_2)} \; {\color{darkred} \tau_2(x_3)} \; \underline{\color{blue}\tau_3(x_2)} Φ4:5≜ϕ3(x3,x2)τ2(x3)τ3(x2)
1 所有与 x 2 x_2 x2 接邻的因子有 { ϕ 3 ( x 3 , x 2 ) , τ 3 ( x 2 ) } \{\phi_3(x_3, x_2) , {\color{blue}\tau_3(x_2)} \} {ϕ3(x3,x2),τ3(x2)} 2 S 4 = S ( x 2 ) ≜ { x 3 } S_4 =\mathcal{S}(x_2) \triangleq \{x_3\} S4=S(x2)≜{x3} 3 ψ ( x 2 , S 4 ) = ϕ 3 ( x 3 , x 2 ) τ 3 ( x 2 ) \psi(x_2, S_4) = {\phi_3(x_3, x_2)} \; {\color{blue}\tau_3(x_2)} ψ(x2,S4)=ϕ3(x3,x2)τ3(x2) 4 因式分解 ψ ( x 2 , x 3 ) = p ( x 2 ∣ x 3 ) τ 4 ( x 3 ) \psi(x_2, x_3) = p(x_2|x_3)\tau_4(x_3) ψ(x2,x3)=p(x2∣x3)τ4(x3)
5 新的简化因子图, 如 Fig. 7 所示, 公式为 Φ 5 : 5 ≜ τ 2 ( x 3 ) τ 4 ( x 3 ) \Phi_{5:5} \triangleq {\color{darkred} \tau_2(x_3)} \; \color{DarkCyan} \tau_4(x_3) Φ5:5≜τ2(x3)τ4(x3)
Fig. 7 消去第四个状态变量后得到简化的因子图
C. 消除第五个状态变量
最后, 我们消除第五个变量 x 3 x_3 x3, 对应于边缘化计算式 (III-2-1) 中的 (f) 步骤.
序号 操作 0 此时调用 EliminateOne( Φ 5 : 5 , x 3 \Phi_{5:5}, x_3 Φ5:5,x3) Φ 5 : 5 ≜ τ 2 ( x 3 ) τ 4 ( x 3 ) \Phi_{5:5} \triangleq {\color{darkred} \tau_2(x_3)} \; \color{DarkCyan} \tau_4(x_3) Φ5:5≜τ2(x3)τ4(x3)
1 所有与 x 3 x_3 x3 接邻的因子有 { τ 2 ( x 3 ) , τ 4 ( x 3 ) } \{ {\color{darkred} \tau_2(x_3)}, \color{DarkCyan} \tau_4(x_3)\} {τ2(x3),τ4(x3)} 2 S 5 = S ( x 3 ) S_5 =\mathcal{S}(x_3) S5=S(x3) 为空集 3 ψ ( x 3 ) = τ 2 ( x 3 ) τ 4 ( x 3 ) \psi(x_3) = {\color{darkred} \tau_2(x_3)} \; \color{DarkCyan} \tau_4(x_3) ψ(x3)=τ2(x3)τ4(x3) 4 因式分解步骤无需处理 5 因子图中原有节点全部消除, 原因子图转化为贝叶斯网络, 如 Fig. 8 所示. 贝叶斯网络根节点 x 3 x_3 x3 所含概率为 p ( x 3 ) ≜ τ 2 ( x 3 ) τ 4 ( x 3 ) p(x_3) \triangleq {\color{darkred} \tau_2(x_3)} \; \color{DarkCyan} \tau_4(x_3) p(x3)≜τ2(x3)τ4(x3)
Fig. 8 消去第五个状态变量后得到简化的因子图
4. 因子图与贝叶斯网络的简单说明
上图所示贝叶斯网络中, 有了根节点 x 3 x_3 x3 概率及各个节点的条件概率, 就能够利用贝叶斯推理方法对状态变量 (节点) 进行概率计算.
哪为什么不一开始就建立贝叶斯网络, 而是通过先建立因子图再转换为贝叶斯网络呢？ 因为:
- 因子图的建立非常直观和简单, 通过计算自然就获得了因果依赖关系; 而贝叶斯网络要在建模时由人工考虑因果依赖关系, 增加了建模难度.
- 因子图能够适用于机器人多传感器融合中的各类场景下的建模. 适用的一元测量有 GPS、UWB、初始点设置等, 适用的二元测量有各类里程计、距离测量、角度测量等. 即使更复杂的状态估计场景无论是多源、异构、不同步、非线性等传感检测都能适用. 另外, 能够实现即插即用式的传感器网络配置; 而贝叶斯网络更适合事件推理的建模, 较难直观方便地对机器人运动、位姿状态、几何测量等建模.
因子图和贝叶斯网络的相互转换比较方便. 可先利用因子图进行机器人相关的运动与状态的建模, 再将已获得的因子图转换为贝叶斯网络, 将机器人与环境相关状态看作是概率事件, 最后利用贝叶斯网络推理计算强项进行机器人与环境状态的概率计算.
另外, 引入因子图建模的同时也引入了更多有效的数学工具, 比如稀疏计算等.
IV. 总结
本篇博文围绕因子图的定义与建模, 在 SLAM 中的应用展开, 重点关注因子图的边缘化和消元算法.
尤其消元算法部分的理解是重点和难点, 是下一步继续探究因子图算法的基础, 如 iSAM, iSAM2 等.
(如有问题, 请指出, 谢谢！)
参考文献
[1] Frank Dellaert, Michael Kaess, Factor Graphs for Robot Perception, Foundations and Trends in Robotics,
Vol. 6, No. 1-2 (2017) 1–139
[2] F. R. Kschischang, B. J. Frey and H. . -A. Loeliger, “Factor graphs and the sum-product algorithm,” in IEEE Transactions on Information Theory, vol. 47, no. 2, pp. 498-519, Feb 2001
[3] 百度百科, 似然函数, https://baike.baidu.com/item/%E4%BC%BC%E7%84%B6%E5%87%BD%E6%95%B0/6011241
[4] 百度百科, 边缘分布, https://baike.baidu.com/item/%E8%BE%B9%E7%BC%98%E5%88%86%E5%B8%83/15571865