1. 构建简单因子图模型

任务目标

使用因子图（Factor Graph）表示一个最简单的SLAM前端问题：两个机器人位姿节点 \(X_1, X_2\)，以及连接二者的一个里程计约束因子 \(F_{12}\)。最终输出该因子图的结构化描述，并生成可视化图形。

核心概念

因子图是一种二分图，包含两类节点：变量节点（圆圈表示，代表待估计状态）和因子节点（方块表示，代表约束或观测）。本例中，位姿 \(X_1, X_2\) 为变量节点，里程计约束 \(F_{12}\) 为因子节点，连接 \(X_1\) 与 \(X_2\)。

实现代码

以下Python代码使用matplotlib构建并可视化该因子图，同时输出图的拓扑描述：

import matplotlib.pyplot as plt
import matplotlib.patches as mpatches

# 定义节点坐标（仅用于可视化布局）
var_pos = {'X1': (0, 0), 'X2': (2, 0)}   # 变量节点位置
factor_pos = {'F12': (1, 0)}             # 因子节点位置

# 定义图的连接关系：因子 -> 连接的变量
edges = {'F12': ['X1', 'X2']}

# 绘制
fig, ax = plt.subplots(figsize=(6, 2))
ax.set_aspect('equal')
ax.axis('off')

# 1. 绘制变量节点（圆圈）
for name, (x, y) in var_pos.items():
    circle = mpatches.Circle((x, y), 0.15, facecolor='lightblue', edgecolor='black', linewidth=2)
    ax.add_patch(circle)
    ax.text(x, y, name, ha='center', va='center', fontsize=12, fontweight='bold')

# 2. 绘制因子节点（方块）
for name, (x, y) in factor_pos.items():
    rect = mpatches.FancyBboxPatch((x-0.15, y-0.15), 0.3, 0.3,
                                   boxstyle="round,pad=0.02",
                                   facecolor='lightcoral', edgecolor='black', linewidth=2)
    ax.add_patch(rect)
    ax.text(x, y, name, ha='center', va='center', fontsize=12, fontweight='bold')

# 3. 绘制边（因子与变量之间的连线）
for factor, vars_list in edges.items():
    fx, fy = factor_pos[factor]
    for var in vars_list:
        vx, vy = var_pos[var]
        ax.plot([fx, vx], [fy, vy], 'k-', linewidth=2)

# 4. 输出图的拓扑描述（文本验证）
print("因子图拓扑结构：")
print("变量节点:", list(var_pos.keys()))
print("因子节点:", list(factor_pos.keys()))
for f, vs in edges.items():
    print(f"因子 {f} 连接: {vs}")

plt.tight_layout()
plt.show()

运行与验证

环境依赖：Python 3.6+，需安装matplotlib（pip install matplotlib）。

运行方式：将代码保存为simple_factor_graph.py并执行：

python simple_factor_graph.py

验证方法：

1. 控制台输出应显示：变量节点为['X1', 'X2']，因子节点为['F12']，且F12连接['X1', 'X2']——这确认了图结构的正确性。
2. 图形窗口应弹出可视化结果：两个浅蓝色圆圈（X1、X2）被一个浅红色圆角方块（F12）通过黑色实线连接，直观展示因子图结构。

预期输出

控制台输出（确定性结果）：

因子图拓扑结构：
变量节点: ['X1', 'X2']
因子节点: ['F12']
因子 F12 连接: ['X1', 'X2']

可视化图形如下：

图中圆圈代表位姿变量节点，方块代表里程计因子，连线表示因子与变量的关联关系。该模型是SLAM因子图的最基本单元，后续可在此基础上扩展更多位姿与观测因子。