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” $\Leftarrow$ 本篇

---

I. 前言

之前博客写到 SLAM 问题的因子图建模以及因子图计算中通过消元获得贝叶斯网络的过程. 这里我们继续跟着原作者的研究一起看看并记录一下.

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

本篇博文中,

- 先简单翻译原作者对贝叶斯树的介绍, 以了解该数据结构的特性;

- 然后应用实例介绍如何基于已获得的贝叶斯网络构建贝叶斯树.

作为基础的因子图、噪声模型等内容可参见关联博文, 此处不再重复.

---

II. 贝叶斯树的定义

1. 贝叶斯树的背景

原论文 Introduction 部分介绍了贝叶斯树引入的背景^[1], 此处直接摘录并翻译.

Kaess 等人已提出了基于稀疏特性的平方根信息矩阵的增量更新算法 iSAM (Incremental Smoothing and Mapping). 该方法存在的问题是需要周期地批量计算变量重排序以及重线性化, 降低了计算效率及算法实时性.

概率推理视角下的图模型在 SLAM 中的应用已有一段时间, 也有一些较好的结果.

为了组合图模型以及稀疏线性代数的优点, 提出了一种新颖的数据结构 —— 贝叶斯树.

我们的方法建立在视矩阵分解为对因子图进行消元得到贝叶斯网络 (Bayes Net) 的过程. 其中贝叶斯网络等价于平方根信息矩阵 (Square Root Information Matrix) 的图模型.

在贝叶斯网络中执行边缘化和优化计算一般不容易. 然而, 由消元/分解等到的贝叶斯网络是弦的 (Chordal). 而弦的贝叶斯网络能够转换为树结构图模型. 在这种树结构图模型中就可以容易地执行边缘化和优化计算了.

最有名的树结构形式的数据结构是团树 (Clique Tree), 在 AI 文献中也称为联结树 (Junction Tree). 图树也已经在 SLAM (Simultaneous Localization and Mapping) 领域被研究探索过了.

然而, 我们提出的新的数据结构 —— 贝叶斯树, 是有向的也更自然地对应于 QR 分解所得的结果, 而且可以允许我们在该树中就条件概率密度进行分析. 我们进一步展示增量推理对应于贝叶斯树的简单编辑, 并进一步提出一种新颖的增量式变量排序策略.

弦的 (Chordal), 满足任意长度大于 3 的无向环都有一根弦

---

2. 贝叶斯树的特点

翻译原论文 Abstract 部分对贝叶斯树的特性描述^[1]如下.

贝叶斯树提供了一种算法基础, 能够更好地理解存在的图模型推理算法及其关联的稀疏矩阵因子分解方法.

类似于团树, 贝叶斯树能够编码因式化分解的概率密度. 但又与团树不同, 贝叶斯树是有向的, 并更自然地映射 SLAM 问题的平方根信息矩阵.

贝叶斯树能够对 SLAM 问题提供三项洞察:

- 就概率密度而言, 贝叶斯树能对批量矩阵分解产生更好地理解.

- 能够将相对抽象的矩阵分解的更新过程转化为简单的贝叶斯树编辑及其条件密度.

- 应用贝叶斯树可获得一个针对稀疏非线性增量优化的新颖算法, 该算法将增量更新与约简变量集合的流畅再线性化融合以获得更高的计算效率, 同时融合了快速收敛到精准解的特性.

---

3. 贝叶斯树的定义

同样摘录翻译原论文中关于贝叶斯树的定义^[1]如下.

通过因子图消元算法得到的贝叶斯网络是弦的. 这种贝叶斯网络可以转换为方便进行优化计算和边缘化计算的树结构图模型.

贝叶斯树是有向树, 它的节点代表对应的弦的贝叶斯网络中的团 (Clique $C_k$, 其中 $k$ 为节点指标).

团 (Clique), 图中的完全子图, 每对不同顶点之间恰有一条边相连.

从这方面看, 贝叶斯树类似于团树. 但是贝叶斯树是有向的, 并且在编码因式分解的概率密度的方式上更接近于贝叶斯网络.

实际上, 我们在贝叶斯树的每个节点上定义条件概率密度
$$P(F_k|S_k)\text{\hspace{1em}(II-3-1)}$$
其中, 分离子 (Separator) $S_k$ 是该节点代表的团 $C_k$ 和该节点的父团 (Parent Clique) ${\Pi }_k$ 的交集, 即
$$S_k\triangleq C_k\cap {\Pi }_k\text{\hspace{1em}(II-3-2)}$$
前端变量 (Frontal Variables) $F_k$ 为剩余变量, 即
$$F_k\triangleq C_k\backslash S_k\text{\hspace{1em}(II-3-3)}$$
团 $C_k$ 可以写成
$$C_k=F_k:S_k\text{\hspace{1em}(II-3-4)}$$
这样就得出由贝叶斯树定义的关于全体状态变量 $\Theta$ 的联合概率密度
$$P(\Theta )=\prod _{k}P(F_k|S_k)\text{\hspace{1em}(II-3-5)}$$
需要说明, 对于根团 $C_r=F_r$, 其分离子为空, 即根团概率密度为一先验概率 $P(F_r)$.

基于以上贝叶斯树的定义, 团 $C_k$ 的分离子 $S_k$ 总是其父团 ${\Pi }_k$ 的子集 (式 (II-3-2)). 因此贝叶斯树上有向边 (Direct Edges) 具有和贝叶斯网络一样的语义信息 —— 条件作用 (Conditioning).

以上的内容都是概念性的, 也比较抽象, 之前没有接触过的话, 会被绕得云里雾里的.

以上仅做铺垫, 下面我们进入具体内容:

如何将一个已经由因子图通过消元/边缘化获得的贝叶斯网络转化为可以实现增量式推理的贝叶斯树？

---

III. 贝叶斯树的构建

1. 贝叶斯树的构建算法

        算法: 从由因子图消元得到的弦的贝叶斯网络构建产生贝叶斯树^[1]

[1]   For (以消元顺序的逆序, 遍历贝叶斯网络中的每一个条件概率密度 $P({\theta }_j|S_j)$):

[2]        if (父集为空, 即 S j = { } S_j=\{\} Sj​={}):

[3]                开始一个包含 ${\theta }_j$ 的新根团 $F_r$

[4]        else:

[5]                识别父团 $C_p$, 该父团需包含分离子 $S_j$ 中第一个被消元的变量, 作为父团的一个前端变量

[6]                if (父团 $C_p=F_p\cup S_p$ 等于条件概率中的分离子 $S_j$):

[7]                          把条件概率插入父团 $C_p$ 中去

[8]                else:

[9]                          作为父团 $C_p$ 的子团, 开始一个包含变量 ${\theta }_j$ 的新子团 $C^{\prime }$

---

2. 贝叶斯树的构建实例

沿用之前博文中的因子图实例如下. 在高斯噪声模型下, 因子图描述的概率系统的最大后验估计, 经过负对数 ($-\log (\cdot )$) 处理后, 可以得到非线性二乘问题的局部线性化 Jacobian 矩阵^[2]. 这样因子图和 Jacobian 矩阵之间形成了映射.

因子图 (Factor Graph)	雅可比矩阵 (Jacobian)

通过对因子图进行消元/边缘化后得到的贝叶斯网络如下. 因子图的消元过程又对应着最大后验估计负对数处理导出的非线性二乘问题的局部线性化系统的 QR 分解. 这个 QR 分解得到的矩阵 $\mathbf{R}$ 就称为平方根信息矩阵. 因子图消元后对贝叶斯网络进行边缘化计算对应着计算 QR 分解后局部线性化系统的解^[2]. 这样因子图消元得到的贝叶斯网络和平方根信息矩阵 $\mathbf{R}$ 存在关联关系.

贝叶斯网络 (Bayes Net)	平方根信息矩阵 (Square Root Information Matrix)

因子图的消元顺序为 $l_1\to l_2\to x_1\to x_2\to x_3$.

得到了贝叶斯网络后, 如何通过贝叶斯树的构建算法一步一步的构建贝叶斯树呢？

---

3. 贝叶斯树的构建过程

下面我们按照贝叶斯树的构建算法一步一步分解执行.

步骤	实现
确定消元顺序的逆	$x_3\to x_2\to x_1\to l_2\to l_1$
处理 $P(x_3)$	判断 “父集为空” 成立
	开始一个新的根团 C r = F r = { x 3 } C_r=F_r = \{x_3\} Cr​=Fr​={x3​} (注意根团没有分离子)
处理 $P(x_2|x_3)$	判断 “父集为空” 不成立
	识别父团为 $F_r$ 满足分离子 S j = { x 3 } S_j = \{x_3\} Sj​={x3​} 中的第一个消除元素 $x_3$ 包含在父团的前端变量集合 $F_r$ 中
	判断 “父团 $F_r$ 与条件概率中的分离子 $S_j$ 相等” 成立
	把条件概率插入父团 $F_r$ 中去 父团变为 F r = { x 2 , x 3 } F_r=\{x_2, x_3\} Fr​={x2​,x3​}
处理 $P(x_1|x_2)$	判断 “父集为空” 不成立
	识别父团为 F r = { x 2 , x 3 } F_r=\{x_2,x_3\} Fr​={x2​,x3​} 满足分离子 S j = { x 2 } S_j = \{x_2\} Sj​={x2​} 中的第一个消除元素 $x_2$ 包含在父团的前端变量集合 $F_r$ 中
	判断 “父团 $F_r$ 与条件概率中的分离子 $S_j$ 相等” 不成立 因为 F r = { x 2 , x 3 } ‾ ≠ S j = { x 2 } ‾ \underline{F_r=\{x_2, x_3\}} \neq \underline{S_j=\{x_2\}} Fr​={x2​,x3​}​=Sj​={x2​}​
	开始新的团 C 1 ′ = { x 1 , x 2 } C'_1=\{x_1, x_2\} C1′​={x1​,x2​}, 作为 $F_r$ 的子团 新团的分离子 S 1 ′ = C 1 ′ ∩ C p = { x 1 , x 2 } ∩ { x 2 , x 3 } = { x 2 } S'_1= C'_1 \cap C_p = \{x_1, x_2\} \cap \{x_2, x_3\} = \{x_2\} S1′​=C1′​∩Cp​={x1​,x2​}∩{x2​,x3​}={x2​} 新团的前端变量集合 F 1 ′ = C 1 ′ \ S 1 ′ = { x 1 } F'_1 = C'_1 \backslash S'_1 = \{x_1\} F1′​=C1′​\S1′​={x1​} 新团记为 C 1 ′ = { x 1 } : { x 2 } C'_1 = \{x_1\} : \{x_2\} C1′​={x1​}:{x2​}
处理 $P(l_2|x_3)$	判断 “父集为空” 不成立
	识别父团为 F r = { x 2 , x 3 } F_r=\{x_2,x_3\} Fr​={x2​,x3​} 满足分离子 S j = { x 3 } S_j = \{x_3\} Sj​={x3​} 中的第一个消除元素 $x_3$ 包含在父团的前端变量集合 $F_r$ 中
	判断 “父团 $F_r$ 与条件概率中的分离子 $S_j$ 相等” 不成立 因为 F r = { x 2 , x 3 } ‾ ≠ S j = { x 3 } ‾ \underline{F_r=\{x_2, x_3\}} \neq \underline{S_j=\{x_3\}} Fr​={x2​,x3​}​=Sj​={x3​}​
	开始新的团 C 2 ′ = { l 2 , x 3 } C'_2=\{l_2, x_3\} C2′​={l2​,x3​}, 作为 $F_r$ 的子团 新团的分离子 S 2 ′ = C 2 ′ ∩ C p = { l 2 , x 3 } ∩ { x 2 , x 3 } = { x 3 } S'_2= C'_2 \cap C_p = \{l_2, x_3\} \cap \{x_2, x_3\} = \{x_3\} S2′​=C2′​∩Cp​={l2​,x3​}∩{x2​,x3​}={x3​} 新团的前端变量集合 F 2 ′ = C 2 ′ \ S 2 ′ = { l 2 } F'_2 = C'_2 \backslash S'_2 = \{l_2\} F2′​=C2′​\S2′​={l2​} 新团记为 C 2 ′ = { l 2 } : { x 3 } C'_2 = \{l_2\} : \{x_3\} C2′​={l2​}:{x3​}
处理 $P(l_1|x_1,x_2)$	判断 “父集为空” 不成立
	分离子 S j = { x 1 , x 2 } S_j = \{x_1, x_2\} Sj​={x1​,x2​} 中的第一个消除元素为 $x_1$ 前端变量集合包含 $x_1$ 的团为 C 1 ′ = { x 1 } : { x 2 } C'_1 = \{x_1\} : \{x_2\} C1′​={x1​}:{x2​} 故识别到父团为 C 1 ′ = { x 1 } : { x 2 } C'_1 = \{x_1\} : \{x_2\} C1′​={x1​}:{x2​}
	判断 “父团 C 1 ′ = { x 1 , x 2 } C'_1=\{x_1, x_2\} C1′​={x1​,x2​} 与条件概率中的分离子 S j = { x 1 , x 2 } S_j=\{x_1, x_2\} Sj​={x1​,x2​} 相等” 成立
	把条件概率插入父团 $C_1^{\prime }$ 中去 父团变为 C 1 ′ = { l 1 , x 1 , x 2 } C'_1=\{l_1, x_1, x_2\} C1′​={l1​,x1​,x2​} 更新分离子 S 1 ′ = C 1 ′ ∩ F r = { l 1 , x 1 , x 2 } ∩ { x 2 , x 3 } = { x 2 } S'_1= C'_1 \cap F_r = \{l_1, x_1, x_2\} \cap \{x_2, x_3\} = \{x_2\} S1′​=C1′​∩Fr​={l1​,x1​,x2​}∩{x2​,x3​}={x2​} 更新前端变量集合 F 1 ′ = C 1 ′ \ S 1 ′ = { l 1 , x 1 } F'_1 = C'_1 \backslash S'_1 = \{l_1, x_1\} F1′​=C1′​\S1′​={l1​,x1​} 该团记为 C 1 ′ = { l 1 , x 1 } : { x 2 } C'_1 = \{l_1,x_1\} : \{x_2\} C1′​={l1​,x1​}:{x2​}

得到根团 $F_r=x_2,x_3$, 子团 C 1 ′ = { l 1 , x 1 } : { x 2 } C'_1=\{l_1, x_1\}:\{x_2\} C1′​={l1​,x1​}:{x2​} 和子团 C 2 ′ = { l 2 } : { x 3 } C'_2=\{l_2\}:\{x_3\} C2′​={l2​}:{x3​}, 很自然地贝叶斯树就可以画出来了.

贝叶斯树 (Bayes Tree)
团 { x 2 , x 3 } \{x_2, x_3\} {x2​,x3​} 定义了概率密度 $P(x_2,x_3)$ 团 { l 1 , x 1 } : { x 2 } \{l_1, x_1\}:\{x_2\} {l1​,x1​}:{x2​} 定义了条件概率密度 $P(l_1,x_1|x_2)$ 团 { l 2 } : { x 3 } \{l_2\}:\{x_3\} {l2​}:{x3​} 定义了条件概率密度 $P(l_2|x_3)$ 团 { l 1 , x 1 } : { x 2 } \{l_1, x_1\}:\{x_2\} {l1​,x1​}:{x2​} 与团 { l 2 } : { x 3 } \{l_2\}:\{x_3\} {l2​}:{x3​} 之间的相对独立

比较分析贝叶斯树与平方根信息矩阵, 可以发现贝叶斯树以图形的形式较好地刻画/映射了平方更信息矩阵的结构信息.

平方根信息矩阵 (Square Root Information Matrix)
矩阵中同一颜色的行代表在贝叶斯树中处于同一团 矩阵中不同颜色的行代表在贝叶斯树中处于不同的团 虚线框则与分离子有关 根团始终在矩阵的最底行

这样本实例的贝叶斯树就构建完毕了.

---

IV. 总结

本篇博文根据参考文献,

对贝叶斯树这一为 SLAM 因子图处理而引入的新数据结构进行了介绍,

然后介绍了由贝叶斯网络建立贝叶斯树的算法, 并用实例进行了演示模拟.

图模型 (因子图、贝叶斯网络、贝叶斯树等) 与数值模型 (最大后验概率、最小二乘法、Jacobian 矩阵、平方根信息矩阵等) 相互关联与印证中, 可以发现好多值得细细品味的地方, 赞叹一步一步把这些框架搭起来的前赴后继的研究人员的创造力！

预告一下, 后面将介绍基于贝叶斯树的增量更新, 来体验贝叶斯树的功效.

---

参考文献

[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

[2] Frank Dellaert, Michael Kaess, Factor Graphs for Robot Perception, Foundations and Trends in Robotics, Vol. 6, No. 1-2 (2017) 1–139