贝叶斯树增量式更新的细嚼慢咽

Title: 贝叶斯树增量式更新的细嚼慢咽 —— Notes for “The Bayes Tree: An Algorithmic Foundation for Probabilistic Robot Mapping”

---

---

关联博客:

因子图、边缘化与消元算法的抽丝剥茧 —— Notes for “Factor Graphs for Robot Perception”
基于 GTSAM 的因子图简单实例

GTSAM 中的鲁棒噪声模型与 M-估计 (GTSAM Robust Noise Model and M-Estimator)
贝叶斯树定义与构建的寻行数墨 —— Notes for “The Bayes Tree: An Algorithmic Foundation for Probabilistic Robot Mapping”

贝叶斯树增量式更新的细嚼慢咽 —— Notes for “The Bayes Tree: An Algorithmic Foundation for Probabilistic Robot Mapping”$\Leftarrow$ 本篇
iSAM2 部分状态更新算法 (I-原理解读)

---

I. 前言

Kaess 等人在论文 “The Bayes Tree: An Algorithmic Foundation for Probabilistic Robot Mapping”^[1] 中提出了一种新型的数据结构 —— 贝叶斯树 (Bayes Tree).

之前的博文对贝叶斯树的特性进行了介绍, 也介绍如何基于已获得的贝叶斯网络构建贝叶斯树.

本篇博文应用实例介绍在获得新的因子 (如测量因子、里程计因子、GPS因子等) 后贝叶斯树的增量更新算法.

增量式更新是实现基于贝叶斯树的实时推理的基础. 所谓增量式更新, 即只需要快速计算更新涉及变化的贝叶斯树, 而无需计算更新整个贝叶斯树. 这样大大降低计算资源消耗, 提高了算法实时性能.

而原作者在同一论文中讲到的贝叶斯树求解、流畅重线性化、增量重排序等此处不涉及.

---

II. 贝叶斯树的构建关系

根据上一博文的内容, 总结贝叶斯树的构建关系如下图.

Fig. 1 贝叶斯树的构建关系

可以说, 贝叶斯树存储了平方根信息矩阵 $\mathbf{R}$ 的结构信息, 或者平方根信息矩阵 $\mathbf{R}$ 以贝叶斯树的形式表现/存储.

---

III. 贝叶斯树增量式更新的基础

我们先借用因子图消元算法最后得到贝叶斯网络的过程实例, 以此来查看消元过程中的信息流.

顺序	因子图消元	信息流
1		消除 $l_1$, 信息流向 $x_1$ 和 $x_2$
2		消除 $l_2$, 信息流向 $x_3$
3		消除 $x_1$, 信息流向 $x_2$
4		消除 $x_2$, 信息流向 $x_3$
5		消除 $x_3$, 获得贝叶斯网络

由上面获得贝叶斯网络的因子图消元过程可知, 消元过程伴随着信息的流动过程.

也就是说 $l_1\to l_2\to x_1\to x_2\to x_3$ 既描述了因子图消元顺序, 也描述了伴随着的信息流转方向.

我们以根团 C r = { x 2 , x 3 } C_r =\{x_2, x_3\} Cr​={x2​,x3​} 视角来分析,

- 在消除子团 { l 1 , x 1 } : { x 2 } \{l_1, x_1\}:\{x_2\} {l1​,x1​}:{x2​} 中前端变量 { l 1 , x 1 } \{l_1, x_1\} {l1​,x1​}的过程中, 信息流向量了根团中的变量 { x 2 } \{x_2\} {x2​};

- 在消除子团 { l 2 } : { x 3 } \{l_2\}:\{x_3\} {l2​}:{x3​} 中前端变量 { l 2 } \{l_2\} {l2​} 的过程中, 信息流向了根团中变量 { x 3 } \{x_3\} {x3​}.

也就是说, 该消元实例过程中根团收集了被消除的各个子团的信息.

更一般化的描述为:

- 父团中的变量通过消除子团中变量来收集子团中包含的信息;

- 任何团中的信息都只会向上传播 (子团传播到父团) 到直到根团.

从上述消元过程实例中还可以看出,

变量消元的过程中关联的因子也被消除了, 因子会在其所含的第一个变量被消除时即进入消元通道, 即因子所含信息向上流入父团.

在消元过程中或信息流向根团的过程中, 形成了消元路径或信息流轨迹. 同时, 变量之间也形成了后继关系 (一种偏序关系), 如下所示.

消元路径/信息流轨迹	后继关系
	$l_1$ 的后继变量有 { x 1 , x 2 , x 3 } \{x_1, x_2,x_3\} {x1​,x2​,x3​} $x_1$ 的后继变量有 { x 2 , x 3 } \{ x_2,x_3\} {x2​,x3​} $x_2$ 的后继变量为 $x_3$ $l_2$ 的后继变量为 $x_3$

我们知道, 因子图中因子信息向根团汇聚的过程中, 可以选择不同路径进行消元, 这样获得贝叶斯网络上的不同依赖关系, 也获得不同后继关系.

综合上述内容和结论, 这样就得到了贝叶斯树增量更新的理论基础:

- 在因子图中新加入一个因子, 该因子只能影响其所含变量及其后继变量.

- 该因子无法影响任何其他的变量, 这些其他变量是指除了新加入因子所含变量及这些变量的后继变量以外的其他状态变量.

当一个新的因子 $f^{\prime }(x_j,x_j^{\prime })$ 加入到原来的因子图中时, 对应的贝叶斯树哪些节点会受到影响呢？

根据上述 “新因子只影响该因子所涉变量及其后继变量” 的理论基础, 我们可以知道新加入因子 $f^{\prime }(x_j,x_j^{\prime })$, 对应的在贝叶斯树中产生的影响涉及到包含变量 $x_j$ 或 $x_j^{\prime }$ 的团并向上传播到根团 (Root Clique).

这里所谓的 “包含 $x_j$ 的团” 应该是以 $x_j$ 为前端变量的团, 而仅以 $x_j$ 作为分离子 (Seperator) 中变量的团不是此处说的 “包含 $x_j$ 的团”. 因为在前端变量被消除的过程中, 分离子中变量接收前端变量传过来的信息, 分离子中变量作为前端变量的后继变量.

也就是说, 新加入因子只影响贝叶斯树的 “顶部”.

新加入因子的变量也称为受影响的变量.

---

IV. 贝叶斯树增量式更新的算法

为了实现增量式因式分解/推理, 需要将受影响的贝叶斯树的顶部单独考虑,

(1) 先将这部分 “顶部” 贝叶斯树重新解释为因子图,

(2) 然后将新增的因子 (如 SLAM 运行中的实时测量因子等) 加入到这个因子图中去, 得到一个更新的因子图,

(3) 接着在这个更新的因子图中进行消元运算得到部分更新的贝叶斯树,

(4) 最后把初始贝叶斯树中不受影响的部分重新连接到部分更新的贝叶斯树中.

这就完成了贝叶斯树的增量式更新, 得到完整的更新过的贝叶斯树.

对应算法如下.

        算法: 贝叶斯树的增量式更新算法^[1]

        输入: 贝叶斯树 $\mathcal{T}$, 新因子 ${\mathcal{F}}^{\prime }$

        输出: 更新的贝叶斯树 ${\mathcal{T}}^{\prime }$

[1]   移除贝叶斯树中受影响的顶部, 并将该顶部重新解释为因子图:

[2]        a. 对于每一个受影响的变量, 移除相应的团及所有父团直到根团为止.

[3]        b. 存储移除团后的孤立子树 ${\mathcal{T}}_{orph}$.

[4]   将新的因子 ${\mathcal{F}}^{\prime }$ 添加到重新解释的因子图中去.

[5]   对该因子图重新排序并执行消元运算得到贝叶斯网络 (因子图的变量消元算法), 再贝叶斯网络重新组合得到贝叶斯树 (贝叶斯树的构建算法).

[6]   将孤立子树 ${\mathcal{T}}_{orph}$ 重新接入到这个新得到的贝叶斯树中去.

增量式更新主要体现在孤立子树原封不动地保存以及重新接驳入新的贝叶斯树中去.

---

V. 贝叶斯树增量式更新的实例

沿用简单 SLAM 例子, 在变量 $x_1$ 和 $x_3$ 之间增加新的因子 ${\varphi }_{10}(x_1,x_3)$, 示意如下.

Fig. 2 问题实例

下面借用上述问题实例来一步一步了解贝叶斯树的增量式更新算法的执行细节.

序号	图示	说明
1		加入新因子 ${\varphi }_{10}(x_1,x_3)$ 后, 受影响的团如虚线标注 团 { l 1 , x 1 : x 2 } \{l_1, x_1:x_2\} {l1​,x1​:x2​} 受影响是因为前端变量中含 $x_1$ 团 { x 2 , x 3 } \{x_2, x_3\} {x2​,x3​} 是根团, 因为向上传播的缘故必然受影响 团 { l 2 : x 3 } \{l_2:x_3\} {l2​:x3​} 不受影响, 是因为其前端变量中不含 $x_1$ 或 $x_3$, 且不在受影响团的向上传播路径中 孤立子树 T o r p h = { l 2 : x 3 } \mathcal{T}_{orph}=\{l_2:x_3\} Torph​={l2​:x3​}
2		将受影响的团单独拿出来后, 重新转换成因子图 并把新的因子 ${\varphi }_{10}$ 加入该因子图中
3		开始基于 “因子图的变量消元算法” 对受影响部分因子图进行重新消元 消元顺序选择为 $l_1\to x_1\to x_2\to x_3$ (a) 消元 $l_1$
4		(b) 消元 $x_1$
5		(c) 消元 $x_2$
6		(d) 消元 $x_3$, 获得贝叶斯网络 获得贝叶斯网络构成两个团, { x 1 , x 2 , x 3 } \{x_1, x_2, x_3\} {x1​,x2​,x3​} 和 { l 1 , x 1 , x 2 } \{l_1, x_1, x_2\} {l1​,x1​,x2​}
7		基于 “贝叶斯树的构建算法” 重新构建贝叶斯树 消元顺序的逆序为 $x_3\to x_2\to x_1\to l_1$ (a) 处理 $p(x_3)$: 判断 “父集为空” 成立, 开始新的根团 C r = F r = { x 3 } C_r= F_r=\{x_3\} Cr​=Fr​={x3​} (b) 处理 $p(x_2|x_3)$: 识别父团就是根团 { x 3 } \{x_3\} {x3​}, 条件概率分离子与父团元素相同, 将条件概率插入父团 (根团) 得到 { x 2 , x 3 } \{x_2, x_3\} {x2​,x3​} (c) 处理 $p(x_1|x_2,x_3)$: 识别父团就是根团 { x 2 , x 3 } \{x_2, x_3\} {x2​,x3​}, 条件概率分离子与父团元素相同, 讲条件概率插入父团 (根团) 得到 C r = F r = { x 1 , x 2 , x 3 } C_r=F_r=\{x_1, x_2, x_3\} Cr​=Fr​={x1​,x2​,x3​} (d) 处理 $p(l_1|x_1,x_2)$: 变量 $x_1$ 是分离子 { x 1 , x 2 } \{x_1, x_2\} {x1​,x2​} 中第一个被消元的元素, $x_1$ 也包含在根团的前端变量集合 { x 1 , x 2 , x 3 } \{x_1, x_2, x_3\} {x1​,x2​,x3​} 中, 识别出父团就是根团 { x 1 , x 2 , x 3 } \{x_1, x_2, x_3\} {x1​,x2​,x3​}; 父团与条件概率的分离子不完全相同, 开始一个新团 C ′ = { l 1 , x 1 , x 2 } C'=\{l_1, x_1, x_2\} C′={l1​,x1​,x2​} 作为根团的子团; 新团的分离子为 S ′ = C ′ ∩ C r = { l 1 , x 1 , x 2 } ∩ { x 1 , x 2 , x 3 } = { x 1 , x 2 } S'= C' \cap C_r= \{l_1, x_1, x_2\} \cap \{x_1, x_2, x_3\}=\{x_1, x_2\} S′=C′∩Cr​={l1​,x1​,x2​}∩{x1​,x2​,x3​}={x1​,x2​}, 新团的前端变量为 F ′ = C ′ \ S ′ = { l 1 } F'=C'\backslash S'=\{l_1\} F′=C′\S′={l1​}, 新团记作 C ′ = { l 1 } : { x 1 , x 2 } C'=\{l_1\}:\{x_1, x_2\} C′={l1​}:{x1​,x2​}
8		基于 “贝叶斯树的构建算法” 中的父团识别方法, 识别孤立子树 T o r p h = { l 2 : x 3 } \mathcal{T}_{orph}=\{l_2:x_3\} Torph​={l2​:x3​} 在新贝叶斯树中的父团 孤立子树的分离子元素为 $x_3$, 包含在根团 { x 1 , x 2 , x 3 } \{x_1, x_2, x_3\} {x1​,x2​,x3​} 的前端变量集合中, 故孤立子树的父团就是根团 以根团为其父团, 将孤立子树 T o r p h = { l 2 : x 3 } \mathcal{T}_{orph}=\{l_2:x_3\} Torph​={l2​:x3​} 重新接驳入到这个新贝叶斯树中去 最终获得增量式更新的贝叶斯树

可以看出, 贝叶斯树的更新是树结构的更新, 也是其树结构背后的数据结构的更新.
受影响部分贝叶斯树重解释获得部分新的因子图, 对这部分因子图的重新消元的顺序也和完整因子图消元顺序一样采用经验式/启发式确定.
但在 SLAM 因子图中一条明确的准则是: 新加入的因子时最近 (最新) 访问的变量尽量排到最后消元, 这样能够让最新变量位于贝叶斯树的根部, 当又有新因子加入时对贝叶斯树的影响区域减少. 这是因为 SLAM 应用中, 新因子更大概率和新变量有关联, 而不是新因子与时间上久远的变量有关联, 除非发生回环.

---

VI. 总结

本篇博文中,

先利用因子图消元算法的示例, 分析了贝叶斯树中信息的流转路径, 即信息只能由下 (子团) 向上 (父团) 流转直至根团.

这样就可以知道, 贝叶斯树的更新只需要重新计算该树的 “顶部”, 而无需处理不受影响的其他部分子树.

然后呈现了贝叶斯树的增量式更新算法, 并利用示例一步一步地详细解读贝叶斯树增量式更新的细节.

变量之间的依赖关系以及增量更新原理, 只有在图模型中才能够如此直观地呈现, 计算模型缺少这种洞见.

(如有问题, 请指出, 谢谢！)

---

参考文献

[1] Kaess, M., Ila, V., Roberts, R., Dellaert, F. (2010). The Bayes Tree: An Algorithmic Foundation for Probabilistic Robot Mapping. In: Hsu, D., Isler, V., Latombe, JC., Lin, M.C. (eds) Algorithmic Foundations of Robotics IX. Springer Tracts in Advanced Robotics, vol 68. Springer, Berlin, Heidelberg.