GTSAM官方文档（一）

---

1 因子图

让我们以一个页面的因子图基础知识开始，这些内容不取代 Kschischang 等人（2001）和 Loeliger（2004）的优秀而详细的综述。

图1: 一个HMM，展开到三个时间步，由贝叶斯网络表示。

图1显示了一个隐藏马尔可夫模型（HMM）的贝叶斯网络，跨越三个步。在贝叶斯网络中，每个节点都与一个条件密度相关联：顶层的马尔可夫链编码了先验 $P(X_1)$ 和转移概率 $P(X_2|X_1)$ 以及 $P(X_3|X_2)$，而观测值 $Z_t$ 仅依赖于状态 $X_t$，由条件密度 $P(Z_t|X_t)$ 建模。 在给定观测值 $z_1$、$z_2$ 和 $z_3$ 的情况下，我们关注状态序列 $(X_1,X_2,X_3)$，以最大化后验概率 $P(X_1,X_2,X_3|Z_1=z_1,Z_2=z_2,Z_3=z_3)$。由于观测值 $Z_1$、$Z_2$ 和 $Z_3$ 是已知的，后验概率与六个因子的乘积成正比，其中三个来自马尔可夫链，另三个定义为 $L(X_t;z)\propto P(Z_t=z|X_t)$ 的似然因子：
$$P(X_1,X_2,X_3|Z_1,Z_2,Z_3)\propto P(X_1)P(X_2|X_1)P(X_3|X_2)L(X_1;z_1)L(X_2;z_2)L(X_3;z_3)$$

图2: 一个具有观测值的HMM，随时间展开，被表示为一个因子图。
这启发了一种不同的图形模型，即因子图，在其中我们仅表示未知变量 $X_1,X_2,X_3$，这些变量与表示其概率信息的因子相连，如图2所示。为了进行最大后验$MAP$推断，我们最大化以下乘积：

$$f(X_1,X_2,X_3)=\prod _i{f}_i({\mathcal{X}}_i)$$

即因子图的值。从图中可以清楚地看出因子图的连通性，每个因子 $f_i$ 编码了它所依赖的变量子集 ${\mathcal{X}}_i$。在下面的示例中，我们使用因子图来建模机器人领域中更复杂的MAP推断问题。

---

2 机器人运动建模

2.1 使用因子图建模

在进入一个SLAM示例之前，让我们考虑一个更简单的机器人运动建模问题。这可以通过连续的马尔可夫链来完成，并对GTSAM进行简要介绍。

图3: 用于机器人定位的因子图。
图3展示了一个简单示例的因子图。其中有三个变量 $x_1,x_2,x_3$，表示机器人随时间变化的位姿，在图中圆圈的变量节点表示。在此示例中，我们有一个一元因子 $f_0(x_1)$，作用于初始位姿 $x_1$，编码了关于 $x_1$ 的先验知识；以及两个二元因子，分别为 $f_1(x_1,x_2;o_1)$ 和 $f_2(x_2,x_3;o_2)$，它们连接了连续的位姿，其中 $o_1$ 和 $o_2$ 表示里程计测量。

---

2.2 创建因子图

以下C++代码作为示例，包含在GTSAM中，用于创建图3中的因子图：

// 创建一个空的非线性因子图
NonlinearFactorGraph graph;

// 在姿态x_1上添加一个高斯先验
Pose2 priorMean(0.0, 0.0, 0.0);  // 先验的均值为(0, 0, 0)
noiseModel::Diagonal::shared_ptr priorNoise =
  noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1));  // 先验的噪声模型，标准差为(0.3, 0.3, 0.1)
graph.add(PriorFactor<Pose2>(1, priorMean, priorNoise));  // 将先验因子添加到因子图中

// 添加两个里程计因子
Pose2 odometry(2.0, 0.0, 0.0);  // 里程计测量值为(2, 0, 0)
noiseModel::Diagonal::shared_ptr odometryNoise =
  noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));  // 里程计的噪声模型，标准差为(0.2, 0.2, 0.1)
graph.add(BetweenFactor<Pose2>(1, 2, odometry, odometryNoise));  // 添加从x_1到x_2的里程计因子
graph.add(BetweenFactor<Pose2>(2, 3, odometry, odometryNoise));  // 添加从x_2到x_3的里程计因子

在上方代码中，第2行创建了一个空的因子图。然后，我们在第5-8行通过模板类 PriorFactor<T> 添加了因子 $f_0(x_1)$，该类在slam子文件夹中提供，其中 $T=Pose2$。它的构造函数接收一个变量键（在此为1），一个类型为 Pose2 的均值（在第5行创建），以及用于先验密度的噪声模型。我们通过指定三个标准差来提供对角高斯噪声模型 noiseModel::Diagonal，分别为机器人位置的30厘米以及机器人方向的0.1弧度。需要注意的是，Sigmas 构造函数返回一个共享指针，通常用于许多不同因子共享相同的噪声模型。同样，里程计测量在第11行被指定为 Pose2，其噪声模型在第12-13行定义。随后，我们在第14-15行添加了两个因子 $f_1(x_1,x_2;o_1)$ 和 $f_2(x_2,x_3;o_2)$，它们分别是另一个模板类 BetweenFactor<T> 的实例，同样使用 $T=Pose2$。

当运行示例（在命令提示符下运行 make OdometryExample.run）时，它将按以下方式打印出因子图：

2.3 因子图与值

在这一部分，有必要强调 GTSAM 的两个重要设计理念：

1. 因子图及其在代码中的实现指定了机器人整个轨迹 X = { x 1 , x 2 , x 3 } X=\{x_1, x_2, x_3\} X={x1​,x2​,x3​} 的联合概率分布 $P(X|Z)$，而不仅仅是最后的位姿。这种平滑smoothing的视角赋予了 GTSAM 名称：“平滑与映射（smoothing and mapping）”。在本文档的后续部分，将会讨论如何使用 GTSAM 进行过滤（这通常不是期望的操作）或增量推理（我们经常需要这样做）。

2. 在 GTSAM 中，因子图仅是概率密度 $P(X|Z)$ 的一种规范化表示，其对应的 FactorGraph 类及其派生类永远不包含“解”。相反，还有一个独立的 Values 类型，用于指定（在本例中）$x_1,x_2,x_3$ 的具体值，这些值可以用来评估与特定值相关联的概率（或更常见的误差）。

后一点常常令 GTSAM 的初学者感到困惑。需要记住的是，在设计 GTSAM 时，采用了一种函数式方法，类对应于数学对象，通常是不可变的。应将因子图视为应用于值的函数（如 $f(X)\propto P(X|Z)$ 所表示），而不是可以被修改的对象。

---

2.4 GTSAM 中的非线性优化

以下代码创建了一个 Values 实例，并将其作为初始估计，用于找到轨迹 $X$ 的最大后验（MAP）分配：

第2.4节中的第2-5行创建了初始估计值，而在第8行，我们创建了一个非线性 $Levenberg-Marquardt$ 优化器，并通过调用 optimize 使用默认参数设置。GTSAM 需要执行非线性优化的原因是里程计因子 $f_1(x_1,x_2;o_1)$ 和 $f_2(x_2,x_3;o_2)$ 是非线性的，因为它们涉及机器人的方向。这也解释了为什么我们在第2.2节中创建的因子图属于 NonlinearFactorGraph 类型。优化器类会多次将该图线性化，以最小化因子指定的非线性平方误差。

运行示例的相关输出如下：

可以看到，在非常小的容差范围内，真实解被恢复为 $x_1=(0,0,0)$，$x_2=(2,0,0)$，$x_3=(4,0,0)$。

2.5 完全后验推断

GTSAM 还可以在结合所有测量值 $Z$ 后，为每个位姿计算协方差矩阵。因子图编码了后验密度 $P(X|Z)$，均值 $\mu$ 和每个位姿 $x$ 的协方差 $\Sigma$ 一起近似了边际后验密度 $P(x|Z)$。需要注意的是，这只是一个近似值，因为即使在这个简单的例子中，里程计因子在其参数中实际上是非线性的，而 GTSAM 仅计算对真实后验的高斯近似。
以下C++代码将恢复后验边际分布：

运行该示例的相关输出如下：

我们可以看到，边际协方差 $P(x_1|Z)$ 关于 $x_1$ 仅仅是 $x_1$ 的先验知识。但是，随着机器人在所有维度上的不确定性无界增长，位姿的 $y$ 和 $\theta$ 分量变得（正相关）相关。

在解释这些数值时需要注意一个重要事实：协方差矩阵是以相对坐标而不是绝对坐标给出的。这是因为 GTSAM 在内部针对线性化点进行优化，就像所有非线性优化库一样。

---

3 机器人定位

3.1 一元测量因子

在本节中，我们为因子图添加测量值，这些测量值将帮助我们在整个时间内实际定位机器人。该示例还可以作为创建新因子类型的教程。

图4: 机器人定位因子图，每个时间步都有一元测量因子。
特别地，我们使用一元测量因子来处理外部测量值。第2节的示例在真实机器人上并不实用，因为它仅包含与里程计测量相关的因子。这些测量值并不完美，会导致最后一个机器人位姿的不确定性迅速累积，至少在没有任何外部测量的情况下（参见第2.5节）。图4显示了一个新的因子图，其中省略了先验因子 $f_0(x_1)$，取而代之的是添加了三个一元因子 $f_1(x_1;z_1)$、$f_2(x_2;z_2)$ 和 $f_3(x_3;z_3)$，每个因子对应一个定位测量 $z_t$。这样的因子适用于仅依赖于当前机器人位姿的测量值，例如 GPS 读数、激光测距仪与预定义地图的相关性，或者天花板灯光的有无（参见 Dellaert 等人，1999 年的有趣示例）。

---

3.2 定义自定义因子

在 GTSAM 中，可以通过继承内置类 NoiseModelFactor1<T> 创建自定义的一元因子，该类实现了一个与测量似然性相关的一元因子，使用高斯噪声模型：

$$L(q;m)=\exp \left(-\frac{1}{2}||h(q)-m|{|}_{\Sigma }^2\right)=f(q)$$

其中，$m$ 是测量值，$q$ 是未知变量，$h(q)$ 是（可能是非线性的）测量函数，$\Sigma$ 是噪声协方差。注意，上述 $m$ 被认为是已知的，而似然性 $L(q;m)$ 将始终作为 $q$ 的函数进行评估，这解释了为什么它是一元因子 $f(q)$。$q$ 始终是未知变量，其值在测量的情况下可能或不可能出现。

注意: 许多人对这一点感到困惑，通常被条件密度符号 $P(m|q)$ 所误导。实际上，似然性 $L(q;m)$ 被定义为 $q$ 的任何函数，与 $P(m|q)$ 成比例。

示例: 列表 3.2 显示了如何定义自定义因子类 UnaryFactor，实现“类似 GPS”的测量似然性：

class UnaryFactor: public NoiseModelFactor1<Pose2> {
  double mx_, my_; ///< X和Y的测量值

public:
  // 构造函数
  UnaryFactor(Key j, double x, double y, const SharedNoiseModel& model):
    NoiseModelFactor1<Pose2>(model, j), mx_(x), my_(y) {}

  // 计算误差向量
  Vector evaluateError(const Pose2& q,
                       boost::optional<Matrix&> H = boost::none) const
  {
    const Rot2& R = q.rotation();  // 获取姿态的旋转部分
    if (H) {  // 如果需要计算雅可比矩阵
      (*H) = (gtsam::Matrix(2, 3) <<  // 构建雅可比矩阵
              R.c(), -R.s(), 0.0,  // 对x的偏导
              R.s(), R.c(), 0.0).finished();  // 对y的偏导
    }
    // 计算误差向量
    return (Vector(2) << q.x() - mx_, q.y() - my_).finished();
  }
};

在第1行中，定义派生类时，我们提供了模板参数 Pose2，用于指示变量 $q$ 的类型，而测量值被存储为实例变量 mx_ 和 my_（在第2行定义）。第5-6行的构造函数只是将变量键 $j$ 和噪声模型传递给超类，并存储提供的测量值。

每个因子类中最重要的函数是 evaluateError，它应返回 $E(q)=h(q)-m$，这在第11行完成。因为我们希望将该因子用于非线性优化（详见 Dellaert 和 Kaess 2006），当提供可选参数 $H$（一个矩阵引用）时，函数应为其赋值为 $h(q)$ 的雅可比矩阵，并在提供的 $q$ 值处进行评估。对于本例，这在第11行完成。在这种情况下，二维函数 $h$ 的雅可比矩阵 $H$ 对机器人位姿 $q$ 求导，其中 $h(q)$ 返回机器人的位置：

$$h(q)=\left[\begin{array}{c}{q}_x\\ q_y\end{array}\right]$$

对于三维位姿 $q=(q_x,q_y,q_{\theta })$，其雅可比矩阵为：

$$H=\left[\begin{array}{ccc}\cos (q_{\theta })& -\sin (q_{\theta })& 0\\ \sin (q_{\theta })& \cos (q_{\theta })& 0\end{array}\right]$$

重要说明
许多用户在尝试创建自定义因子时，最初会对雅可比矩阵与直觉不符感到惊讶。例如，上述示例中，您可能期望一个 $2\times 3$ 的对角矩阵。这对于属于向量空间的变量是正确的。然而，在 GTSAM 中，我们更普遍地定义雅可比矩阵为矩阵 $H$，使其满足特定条件。

$$h(q{e}^{\hat{\xi }})\approx h(q)+H\xi$$

其中，$\xi =(\delta x,\delta y,\delta \theta )$ 是一个增量更新，$e^{\hat{\xi }}$ 是我们要更新的变量的指数映射。在此情况下，$q\in SE(2)$，其中 $SE(2)$ 是2D刚体变换群，由 Pose2 实现。对于 $SE(2)$ 的指数映射，其一阶近似为：

$$e^{\hat{\xi }}\approx \left[\begin{array}{ccc}1& -\delta \theta & \delta x\\ \delta \theta & 1& \delta y\\ 0& 0& 1\end{array}\right]$$

在使用2D位姿的 $3\times 3$ 矩阵表示时，因此：

这解释了雅可比矩阵 $H$ 的来源。

3.3 使用自定义因子

以下 C++ 代码片段展示了如何创建并将自定义因子添加到因子图中：

// 添加一元测量因子，类似于GPS，对所有三个姿态进行约束
noiseModel::Diagonal::shared_ptr unaryNoise =
  noiseModel::Diagonal::Sigmas(Vector2(0.1, 0.1)); // x和y的标准差为10厘米
graph.add(boost::make_shared<UnaryFactor>(1, 0.0, 0.0, unaryNoise));
graph.add(boost::make_shared<UnaryFactor>(2, 2.0, 0.0, unaryNoise));
graph.add(boost::make_shared<UnaryFactor>(3, 4.0, 0.0, unaryNoise));

在列表 3.3 中，第2-3行创建了噪声模型，其中现在为测量值 $m_x$ 和 $m_y$ 指定了两个标准差。在第4-6行中，创建了三个新的一元因子的 shared_ptr 版本，并将它们添加到因子图中。GTSAM 使用共享指针来引用因子图中的因子，而 boost::make_shared 是一个便捷函数，用于同时构造类并创建其 shared_ptr。由此，我们得到了图4中的因子图。

3.4 完全后验推断

三个 GPS 因子足以完全约束所有未知位姿，并将它们绑定到“全局”参考框架，包括三个未知方向。如果不这样，GTSAM 将因奇异矩阵异常而退出。边际分布可以如第2.5节中那样精确恢复，解和边际协方差现在如下所示：

与第2.5节中的协方差矩阵相比，可以看出，随着测量不确定性的积累，不确定性不再无限增长。相反，“GPS”测量在很大程度上均匀地约束了位姿，符合预期。

子图 a: 里程计边际分布

图5: 比较图3中的“里程计”因子图和图4中的“定位”因子图所产生的边际分布。

子图 b: 定位边际分布

当以图形方式查看时（如图5所示），这非常有帮助。在图中，显示了位置的边际分布作为包含99.99996%概率质量的5西格玛协方差椭圆。对于里程计边际分布，图中显然可以看出：(1) 位姿的不确定性持续增长；(2) 角度里程计上的不确定性转化为 $y$ 上的不确定性增长。而定位边际分布则通过一元因子进行约束，其值更小。此外，虽然不太明显，但由于受两侧里程计约束，中间位姿的不确定性实际上较小。

您可能想知道这些图是如何生成的。答案是通过 GTSAM 的 MATLAB 接口完成的，我们将在下一节中展示。

4 PoseSLAM

4.1 回环闭合约束

SLAM 问题最简单的实例是 PoseSLAM，它避免了显式构建环境地图。SLAM 的目标是在给定传感器测量的情况下同时对机器人进行定位并绘制环境地图（Durrant-Whyte 和 Bailey, 2006）。除了轮式里程计外，机器人在平面上移动最常用的传感器之一是2D激光雷达，它为连续位姿之间提供里程计约束，并在机器人重新访问环境中已探索部分时提供回环闭合约束。

图6: PoseSLAM 的因子图

图6展示了一个用于 PoseSLAM 的因子图示例。以下 C++ 代码（包含在 GTSAM 中的示例）创建了该因子图：

NonlinearFactorGraph graph;  // 创建一个空的非线性因子图

// 添加先验因子
noiseModel::Diagonal::shared_ptr priorNoise =
  noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1));  // 先验噪声模型，标准差为(0.3, 0.3, 0.1)
graph.add(PriorFactor<Pose2>(1, Pose2(0, 0, 0), priorNoise));  // 在姿态1上添加先验，初始位置为(0, 0, 0)

// 添加里程计因子
noiseModel::Diagonal::shared_ptr model =
  noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));  // 里程计噪声模型，标准差为(0.2, 0.2, 0.1)
graph.add(BetweenFactor<Pose2>(1, 2, Pose2(2, 0, 0     ), model));  // 从姿态1到姿态2的里程计测量，移动2米，无旋转
graph.add(BetweenFactor<Pose2>(2, 3, Pose2(2, 0, M_PI_2), model));  // 从姿态2到姿态3的里程计测量，移动2米，旋转90度
graph.add(BetweenFactor<Pose2>(3, 4, Pose2(2, 0, M_PI_2), model));  // 从姿态3到姿态4的里程计测量，移动2米，旋转90度
graph.add(BetweenFactor<Pose2>(4, 5, Pose2(2, 0, M_PI_2), model));  // 从姿态4到姿态5的里程计测量，移动2米，旋转90度

// 添加闭环约束
graph.add(BetweenFactor<Pose2>(5, 2, Pose2(2, 0, M_PI_2), model));  // 从姿态5回到姿态2的闭环约束，移动2米，旋转90度

如前所述，第1-4行创建了一个非线性因子图，并添加了一元因子 $f_0(x_1)$。当机器人在环境中移动时，会为里程计对应的二元因子 $f_t(x_t,x_{t+1})$（添加在第6-12行）生成这些因子（注意：M_PI_2 表示 $\pi /2$）。但第15行模拟了一个不同的事件：回环闭合。例如，机器人可能通过视觉或激光测距仪识别相同的位置，并计算其几何位姿约束，回到首次访问该位置的时间点。这在位姿 $x_5$ 和 $x_2$ 上表现为一个（红色的）回环闭合因子 $f_5(x_5,x_2)$。

图7: 在图6中的因子图上运行优化的结果

我们可以像之前一样优化该因子图，通过创建类型为 Values 的初始估计值，并创建并运行优化器。结果如图7所示，并以绿色显示协方差椭圆。这些二维的5西格玛协方差椭圆表示位置的边际分布，覆盖所有可能的方向，并显示包含99.99996%概率质量的区域。图中清晰地表明，与仅有里程计测量相比，位姿 $x_5$ 的不确定性现在大大减少。而具有最高不确定性的位姿 $x_4$ 是距离一元约束 $f_0(x_1)$ 最远的位姿，$f_0(x_1)$ 是将因子图绑定到全局坐标系的唯一因子。

上述图是通过一个接口创建的，该接口允许在 MATLAB 中使用 GTSAM，提供可视化和快速开发能力。我们将在接下来讨论。

4.2 使用 MATLAB 接口

GTSAM 的大部分功能可以通过 MATLAB 中的封装类访问。以下代码摘录是列表4.1中的C++代码的 MATLAB 等价代码：

graph = NonlinearFactorGraph;  % 创建一个空的非线性因子图

% 添加先验因子
priorNoise = noiseModel.Diagonal.Sigmas([0.3; 0.3; 0.1]);  % 先验噪声模型，标准差为[0.3, 0.3, 0.1]
graph.add(PriorFactorPose2(1, Pose2(0, 0, 0), priorNoise));  % 在姿态1上添加先验，初始位置为(0, 0, 0)

%% 添加里程计因子
model = noiseModel.Diagonal.Sigmas([0.2; 0.2; 0.1]);  % 里程计噪声模型，标准差为[0.2, 0.2, 0.1]
graph.add(BetweenFactorPose2(1, 2, Pose2(2, 0, 0   ), model));  % 从姿态1到姿态2的里程计测量，移动2米，无旋转
graph.add(BetweenFactorPose2(2, 3, Pose2(2, 0, pi/2), model));  % 从姿态2到姿态3的里程计测量，移动2米，旋转90度
graph.add(BetweenFactorPose2(3, 4, Pose2(2, 0, pi/2), model));  % 从姿态3到姿态4的里程计测量，移动2米，旋转90度
graph.add(BetweenFactorPose2(4, 5, Pose2(2, 0, pi/2), model));  % 从姿态4到姿态5的里程计测量，移动2米，旋转90度

%% 添加闭环约束
graph.add(BetweenFactorPose2(5, 2, Pose2(2, 0, pi/2), model));  % 从姿态5回到姿态2的闭环约束，移动2米，旋转90度

需要注意的是，代码几乎是相同的，但存在一些语法和命名上的差异：

• 对象通过调用构造函数创建，而不是在堆上分配它们。
• 命名空间使用点符号，例如，noiseModel::Diagonal::SigmasClasses 在 MATLAB 中变为 noiseModel.Diagonal.Sigmas。
• C++ 中的 Vector 和 Matrix 类在 MATLAB 中只是向量或矩阵。
• 由于 MATLAB 中不存在模板类，这些类在 GTSAM 接口中被硬编码，例如，PriorFactorPose2 对应于 C++ 类 PriorFactor<Pose2> 等。

运行代码后，可以在 MATLAB 命令提示符中调用 whos 来查看已创建的对象。需要注意的是，Class 表示所包装的 C++ 类：

此外，任何 GTSAM 对象都可以被详细检查，其输出与 C++ 完全相同：

而且这不仅限于此：我们还可以调用一些为因子图定义的函数。例如：

计算优化前后总平方误差：

$$\frac{1}{2}\sum _i||h_i(X_i)-z_i|{|}_{\Sigma }^2$$

4.3 读取与优化位姿图

图8: 在 MATLAB 中绘制的小型曼哈顿世界示例，包含100个位姿（由 Ed Olson 提供）。初始估计以绿色显示，优化后的轨迹及其协方差椭圆以蓝色显示。

在 MATLAB 中工作能够显著加快开发周期，并轻松生成图形输出。图8中的优化轨迹由以下代码生成，其中 load2D 用于读取 TORO 文件。关于如何绘图，请参考完整源代码。

%% 初始化因子图、初始估计和里程计噪声
datafile = findExampleDataFile('w100.graph');  % 查找示例数据文件
model = noiseModel.Diagonal.Sigmas([0.05; 0.05; 5*pi/180]);  % 里程计噪声模型，标准差为[0.05米, 0.05米, 5度]
[graph, initial] = load2D(datafile, model);  % 加载二维因子图数据和初始估计

%% 在姿态x_0上添加一个高斯先验
priorMean = Pose2(0, 0, 0);  % 先验均值为(0, 0, 0)
priorNoise = noiseModel.Diagonal.Sigmas([0.01; 0.01; 0.01]);  % 先验噪声模型，标准差为[0.01米, 0.01米, 0.01弧度]
graph.add(PriorFactorPose2(0, priorMean, priorNoise));  % 添加先验因子

%% 使用Levenberg-Marquardt优化算法进行优化，并获取边际分布
optimizer = LevenbergMarquardtOptimizer(graph, initial);  % 创建优化器
result = optimizer.optimizeSafely;  % 安全地进行优化
marginals = Marginals(graph, result);  % 计算边际分布

4.4 3D 中的 PoseSLAM

PoseSLAM 可以轻松扩展到3D位姿，但在更新3D旋转时需要特别注意。GTSAM 支持使用**四元数（quaternions）和 $3\times 3$ 的旋转矩阵（rotation matrices）**来表示3D旋转。选择方式通过编译标志 GTSAM_USE_QUATERNIONS 指定。

图9: 3D 球体示例的绘图（由 Michael Kaess 提供）。从里程计推导出的初始估计（非常错误）以绿色显示，优化后的轨迹以红色显示。代码如下：

%% 初始化因子图、初始估计和里程计噪声
datafile = findExampleDataFile('sphere2500.txt');  % 查找三维因子图数据文件
model = noiseModel.Diagonal.Sigmas([5*pi/180; 5*pi/180; 5*pi/180; 0.05; 0.05; 0.05]);  % 里程计噪声模型，标准差为[5度, 5度, 5度, 0.05米, 0.05米, 0.05米]
[graph, initial] = load3D(datafile, model, true, 2500);  % 加载三维因子图数据和初始估计，使用部分约束
plot3DTrajectory(initial, 'g-', false); % 绘制初始估计轨迹，使用绿色线条

%% 重新加载数据，这次使用所有约束，并进行优化
graph = load3D(datafile, model, false, 2500);  % 加载三维因子图数据，使用所有约束
graph.add(NonlinearEqualityPose3(0, initial.atPose3(0)));  % 添加一个非线性等式约束，约束第一个姿态与初始估计相同
optimizer = LevenbergMarquardtOptimizer(graph, initial);  % 创建优化器
result = optimizer.optimizeSafely();  % 安全地进行优化
plot3DTrajectory(result, 'r-', false); axis equal; % 绘制优化后的轨迹，使用红色线条，并设置坐标轴等比例