---

5 基于地标的 SLAM

5.1 基础

图10: 基于地标的 SLAM 的因子图

在基于地标的 SLAM中，我们通过观测到的地标位置显式地构建地图，这在因子图中引入了除了机器人位姿之外的第二种变量。图10展示了一个基于地标的 SLAM 示例的因子图，其中显示了典型的连通性：位姿通过里程计马尔可夫链连接，并且地标由多个位姿观测到，从而引入二元因子。此外，位姿 $x_1$ 上包含常见的先验信息。

图11: 优化结果及其协方差椭圆，包括位姿（绿色）和地标（蓝色）。还显示了轨迹（红色）和地标观测（青色）。

图10中的因子图可以使用列表5.1中的 MATLAB 代码创建。如前所述，第2行创建因子图，第8-18行创建先验/里程计链条，这是我们已经熟悉的部分。然而，第20-25行的代码是新的：它创建了三个测量因子，在这种情况下是从位姿到地标的“方位/距离”测量。

% 创建因子图容器，并向其中添加因子
graph = NonlinearFactorGraph;  % 创建一个空的非线性因子图
% 创建变量的键
i1 = symbol('x',1); i2 = symbol('x',2); i3 = symbol('x',3);  % 创建三个姿态变量的键
j1 = symbol('l',1); j2 = symbol('l',2);  % 创建两个地标变量的键
% 添加先验因子
priorMean = Pose2(0.0, 0.0, 0.0);  % 先验均值为(0, 0, 0)，表示在原点
priorNoise = noiseModel.Diagonal.Sigmas([0.3; 0.3; 0.1]);  % 先验噪声模型，标准差为[0.3, 0.3, 0.1]
% 直接添加到因子图
graph.add(PriorFactorPose2(i1, priorMean, priorNoise));  % 在姿态i1上添加先验因子

% 添加里程计因子
odometry = Pose2(2.0, 0.0, 0.0);  % 里程计测量值为(2, 0, 0)
odometryNoise = noiseModel.Diagonal.Sigmas([0.2; 0.2; 0.1]);  % 里程计噪声模型，标准差为[0.2, 0.2, 0.1]
graph.add(BetweenFactorPose2(i1, i2, odometry, odometryNoise));  % 添加从姿态i1到i2的里程计因子
graph.add(BetweenFactorPose2(i2, i3, odometry, odometryNoise));  % 添加从姿态i2到i3的里程计因子

% 添加方位/距离测量因子
degrees = pi/180;  % 将角度转换为弧度
brNoise = noiseModel.Diagonal.Sigmas([0.1; 0.2]);  % 方位/距离测量的噪声模型，标准差为[0.1, 0.2]
graph.add(BearingRangeFactor2D(i1, j1, Rot2(45*degrees), sqrt(8), brNoise));  % 添加从姿态i1到地标j1的方位/距离测量因子，方位为45度，距离为sqrt(8)
graph.add(BearingRangeFactor2D(i2, j1, Rot2(90*degrees), 2, brNoise));  % 添加从姿态i2到地标j1的方位/距离测量因子，方位为90度，距离为2
graph.add(BearingRangeFactor2D(i3, j2, Rot2(90*degrees), 2, brNoise));  % 添加从姿态i3到地标j2的方位/距离测量因子，方位为90度，距离为2

5.2 关于键和符号

唯一未解释的代码在第4-6行：这里我们使用 symbol 函数为位姿和地标创建整数键。在 GTSAM 中，我们使用 Key 类型来管理所有变量，这实际上是 size_t 的一个类型定义。键不需要连续编号，但在给定的因子图中必须唯一。对于包含不同类型变量的因子图，我们在 MATLAB 和 C++ 中提供了 Symbol 类型，帮助创建（大范围）整数键，这些键在可能键的空间中分布较远，因此无需从某个任意偏移开始编号点。要创建一个符号键，只需提供一个字符和一个整数索引。可以使用基数0或1，或者任意索引：这并不重要。在上述代码中，我们使用 x 表示位姿，l 表示地标。 列表5.1中创建的因子图的优化结果如图11所示，并且显然具有两次测量的地标 $l_1$ 定位效果更好。在 MATLAB 中，我们还可以检查实际数值，这样可以揭示更多 GTSAM 的“魔力”：

>> result Values with 5 values: l1: (2, 2) l2: (4, 2) x1: (-1.8e-16, 5.1e-17, -1.5e-17) x2: (2, -5.8e-16, -4.6e-16) x3: (4, -3.1e-15, -4.6e-16)

实际上，由符号生成的键可以通过 Values 类中的 print 方法自动检测，并以可读的形式呈现，例如“x1”、“l2”等，而不是作为大型、不便使用的整数。这种“魔力”扩展到使用 Key 类型的大多数因子和其他类。

5.3 更大的示例

图12: 一个更大的示例，包含大约100个位姿和约30个地标，由 gtsam_examples/PlanarSLAMExample_graph.m 生成。

GTSAM 提供了一个稍大的示例，从 PlanarSLAMExample_graph.m 文件读取并如图12所示。为了使图形更清晰，仅显示边际分布，而不显示视线。此示例包含119个（多变量）变量和517个因子，优化时间少于10毫秒。

5.4 一个真实世界的示例

图13: 优化轨迹和地标（激光测距仪扫描中检测到的树木）的一个小部分，数据来源于悉尼维多利亚公园的数据集（由悉尼大学的 Jose Guivant 提供）。

一个真实世界的示例如图13所示，使用了悉尼维多利亚公园的一个著名数据集，该数据集是使用配备激光测距仪的卡车采集的。图中的协方差矩阵计算非常高效，详细说明见 [Kaess 和 Dellaert, 2009]。通过快速算法获得的精确协方差（蓝色，小椭圆）与基于完整求逆的精确协方差（橙色，主要被蓝色覆盖）一致。更大且保守的协方差估计（绿色，大椭圆）基于我们早期的研究 [Kaess 等, 2008]。

6 从运动中恢复结构（Structure from Motion）

图14: 优化后的“从运动中恢复结构（Structure from Motion）”示例，包含10个摄像机按圆形排列，观测中心在原点周围的一个 $20\times 20\times 20$ 立方体的8个顶点。摄像机使用颜色编码的坐标轴渲染（RGB 对应于 XYZ），观测方向沿正 Z 轴。图中还显示了摄像机和点的3D误差协方差椭圆。

从运动中恢复结构（Structure from Motion, SFM） 是一种从一组无序图像中的对应视觉特征恢复环境的3D重建的技术，见图14。在 GTSAM 中，这是通过完全相同的因子图框架实现的，只需使用特定于 SFM 的测量因子。具体来说，存在一个投影因子（projection factor），用于计算给定摄像机位姿 $x_i$（类型为 Pose3）和点 $p_j$（类型为 Point3）的重投影误差 $f(x_i,p_j;z_{ij},K)$。该因子由二维测量值 $z_{ij}$（类型为 Point2）以及已知的相机标定参数 $K$（类型为 Cal3_S2）参数化。以下代码展示了如何创建该因子图：

%% 为所有测量添加因子
noise = noiseModel.Isotropic.Sigma(2, measurementNoiseSigma);  % 创建一个各向同性噪声模型，标准差为 measurementNoiseSigma
for i = 1:length(Z),  % 遍历每个测量数据
    for k = 1:length(Z{i})  % 遍历每个测量数据中的每个测量
        j = J{i}{k};  % 获取对应的地标索引
        G.add(GenericProjectionFactorCal3_S2(  % 添加通用投影因子
              Z{i}{k}, noise, symbol('x', i), symbol('p', j), K));
    end
end

在列表6中，假设因子图已经创建完成，我们在双循环中添加测量因子。我们遍历图像索引 $i$，在该示例中，数据以两个单元数组提供： Z { i } Z\{i\} Z{i} 指定图像 $i$ 中的测量值集合 $z_k$， J { i } J\{i\} J{i} 指定对应的点索引。我们使用的特定因子类型是 GenericProjectionFactorCal3_S2，它是 C++ 类 GenericProjectionFactor<Cal3_S2> 的 MATLAB 等价类，其中 Cal3_S2 是我们选择使用的摄像机标定类型（标准，无径向畸变的5参数标定矩阵）。与基于地标的 SLAM（第5节）类似，这里我们使用符号键，但我们现在使用字符 p 表示点，而不是用 l 表示地标。

重要提示: 使 SFM 工作的一个非常棘手且困难的部分是 (a) 数据关联 和 (b) 初始化。GTSAM 不负责完成这些任务：它仅提供“束调整”优化。在示例中，我们简单地假设数据关联是已知的（它已编码在 $J$ 集中），并使用真实值进行初始化，因为示例的目的是向您展示如何设置优化问题。

---

7 iSAM：增量平滑与映射

GTSAM 提供了一种基于更高级图模型（Bayes 树）的增量推理算法，即 iSAM（增量平滑与映射）算法，参见 [Kaess 等, 2008]；[Kaess 等, 2012] 以获取详细说明。对于实时操作的移动机器人，重要的是在新的传感器测量到来时，能够访问最新的地图。iSAM 以高效的方式保持地图的最新状态。
列表7展示了如何在一个简单的视觉 SLAM 示例中使用 iSAM。在第1-2行中，我们创建了一个 NonlinearISAM 对象，该对象将每隔3步重新线性化并重新排序变量。此参数的正确值取决于问题的非线性程度以及每一步接近黄金标准解的需求。在 iSAM 2.0 中，不再需要此参数，因为 iSAM2 会自动确定何时以及对哪些变量进行线性化。

该示例涉及8个3D点，这些点可从8个连续的摄像机位姿中观测到。因此，在第一步中（此处省略）所有8个地标和第一个位姿均已正确初始化。在代码中，这是通过扰动已知的真实值来完成的，但在实际应用中，特别是单目序列中，需要特别小心地正确初始化位姿和地标。

int relinearizeInterval = 3;  // 设置重线性化间隔为3
NonlinearISAM isam(relinearizeInterval);  // 创建一个NonlinearISAM对象

// ... 第一帧初始化省略 ...

// 遍历不同的姿态，将观测数据添加到iSAM中
for (size_t i = 1; i < poses.size(); ++i) {

  // 为每个地标观测添加因子
  NonlinearFactorGraph graph;  // 创建一个空的非线性因子图
  for (size_t j = 0; j < points.size(); ++j) {
    graph.add(
      GenericProjectionFactor<Pose3, Point3, Cal3_S2>
        (z[i][j], noise, Symbol('x', i), Symbol('l', j), K)
    );  // 添加通用投影因子
  }

  // 为当前姿态添加初始估计
  Values initialEstimate;
  initialEstimate.insert(Symbol('x', i), initial_x[i]);  // 插入当前姿态的初始估计

  // 使用新的因子更新iSAM
  isam.update(graph, initialEstimate);
}

代码的其余部分展示了一个典型的 iSAM 循环：

1. 为新测量创建因子。在第9-18行中，创建了一个小型 NonlinearFactorGraph，用于保存类型为 GenericProjectionFactor<Pose3, Point3, Cal3_S2> 的新因子。
2. 为所有新引入的变量创建初始估计。在这个小示例中，所有地标都已经在第1帧中被观测到，因此每次需要初始化的唯一新变量是新的位姿。这在第20-22行中完成。注意，我们假设已有一个良好的初始估计，记为 initial_x[i]。
3. 最后，调用 isam.update() 方法，该方法获取因子和初始估计，并增量更新解。如果需要，可以通过 isam.estimate() 方法获取解。

---

8 更多应用

虽然对 GTSAM 功能的详细讨论会偏离主题，但以下是可以预期的应用范围，以及我们使用 GTSAM 实现的内容的一些简单概述。

8.1 共轭梯度优化

图15: 一张北京的地图，其中生成树以黑色显示，其余的回环约束以红色显示。生成树可作为 GTSAM 的一个预条件器。GTSAM 还包括高效的预条件共轭梯度（PCG）方法，用于解决大规模 SLAM 问题。尽管文献中流行的直接方法表现出二次收敛性，并且对稀疏问题相当高效，但它们通常需要大量的存储以及高效的消元顺序。而迭代优化方法仅需要访问梯度，并占用较小的内存，但可能面临较差的收敛性。我们的方法（子图预条件化，详见 [Dellaert 等, 2010] 和 [Jian 等, 2011]），结合了直接方法和迭代方法的优点，通过识别一个可以使用直接方法轻松求解的子问题，并使用 PCG 求解剩余部分。简单子问题对应于生成树、平面子图或其他可以高效解决的子结构。图15展示了此类子图的一个示例。

---

8.2 视觉里程计

视觉传感的一个简单介绍是视觉里程计（Visual Odometry, VO）（参见 Nistér 等, 2004），通过跟踪或关联连续图像中的视觉特征，提供连续机器人位姿之间的约束。图像由安装在机器人上的相机捕获。GTSAM 提供了 C++ 和 MATLAB 示例代码，以及特定于 VO 的因子来帮助实现目标。

---

8.3 视觉 SLAM

视觉 SLAM（Visual SLAM）（参见 Davison, 2003）是一种 SLAM 变体，其中当相机在空间中移动时（无论是安装在机器人上还是手动移动），相机观测到3D点。GTSAM，特别是 iSAM（见下文），可以很容易地用作这种场景的后端优化器。

8.4 固定延迟平滑与滤波

GTSAM 可以轻松执行递归估计，在这种情况下，只有一部分位姿保留在因子图中，而其余的位姿被边缘化处理。在上述所有示例中，我们显式优化了所有变量，利用了所有可用的测量数据，这被称为平滑（Smoothing），因为轨迹被“平滑”处理，这也是 GTSAM 名称的由来（GT Smoothing and Mapping）。当仅保留最后几个位姿时，这被称为固定延迟平滑（Fixed-lag Smoothing）。最终，当只保留最新的单个位姿时，这被称为滤波（Filtering）。实际上，SLAM 的最初形式是基于滤波的（参见 Smith 等，1988）。

8.5 离散变量与隐马尔可夫模型（HMMs）

因子图不仅限于连续变量：GTSAM 还可以用于建模和解决离散优化问题。例如，隐马尔可夫模型（HMM）与第2节中的机器人定位问题具有相同的图模型结构，只是 HMM 中的变量是离散的。GTSAM 可以优化并推理离散模型。

致谢

GTSAM 的开发得益于许多在佐治亚理工学院和其他地方的合作者的努力，包括但不限于 Doru Balcan、Chris Beall、Alex Cunningham、Alireza Fathi、Eohan George、Viorela Ila、Yong-Dian Jian、Michael Kaess、Kai Ni、Carlos Nieto、Duy-Nguyen Ta、Manohar Paluri、Christian Potthast、Richard Roberts、Grant Schindler 和 Stephen Williams。此外，Paritosh Mohan 为本手册提供了帮助。衷心感谢你们的辛勤付出！