GTSAM 官方教程学习

0. 前言

年初完成了深蓝学院多传感器融合的课程，接触到因子图，作业中完成了基于ceres库的因子图构建，但存在cost不下降的问题。计划利用gtsam进行重写，开始学习记录。本文主要内容为gtsam官方教程的翻译，结合泡泡机器人公开课56，会在中间穿插自己的理解和应用。但为了记录自己的学习过程，有些理论性已经掌握的内容会稍微省略。
官方git：官方git（https://github.com/borglab/gtsam）
官方教程：官方教程（https://gtsam.org/tutorials/intro.html）

1. 因子图

1.1 序

因子图作为一种概率图模型，是将一个具有多变量的全局函数因子分解，得到几个局部函数的乘积，以此为基础得到的一个双向图叫做因子图。
在面试的过程中，有被问到因子图和位姿图的区别，当时有些懵，这里一起记录。

• 从BA开始说起，BA是SLAM和SFM定义出来的一个优化问题，以重投影误差为误差函数，以每个时刻位姿和所有路标点为优化变量，即将位姿和特征点一起优化。
  

• 位姿图优化在优化几次以后把特征点固定住不再优化，只当做位姿估计的约束，之后主要优化位姿。其节点是位姿，边为两个节点之间的相对运动。

• 因子图是一种概率图，是增量的处理后端优化。每加入一个点，对不需要重新计算的就直接用之前的中间结果，对需要重新计算的再去计算。其节点分为两种，位姿节点与因子节点。 总的来看，BA的计算量最大，需要将位姿与特征点同时优化。位姿图在其基础上减少了对特征点的优化。而因子图和二者区别在于“增量更新”这一点，在储存过去所有信息（会经过边缘化）的基础上并不需要对所有帧进行优化。
  

1.2 因子图

1.2.1 贝叶斯网络

将含有三个时间戳的隐马尔科夫模型的贝叶斯网络表示为例。

在该贝叶斯网络中，每个节点都和一个条件密度相关联。

• 顶层马尔科夫链展示了先验 $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_t$是已知的，因此后验概率可以拆分为六个因子的乘积（和一个系数），即三个先验与三个似然因子¹。这里定义$L(X_t;z)\propto P(Z_t=z|X_t)$。

$$P\left(X_1,X_2,X_3\mid Z_1,Z_2,Z_3\right)\propto P\left(X_1\right)P\left(X_2\mid X_1\right)P\left(X_3\mid X_2\right)L\left(X_1;z_1\right)L\left(X_2;z_2\right)L\left(X3;z_3\right)$$

1.2.2 因子图

将上面的隐马尔科夫模型以因子图的形式表示出来，可以得到下图，只需要表示未知的状态变量$X_1,X_2,X_3$以及与因子相连的概率信息。

为了最大化后验，需要使各因子乘积最大化，即
$$f\left(X_1,X_2,X_3\right)=\prod f_i\left({\mathcal{X}}_i\right)$$

从图中可以看出，因子图的连通性为每个因子编码了它所依赖的变量子集。 在下面的示例中，我们使用因子图来模拟机器人中更复杂的 MAP 推理问题。

2. 运动建模

2.1 因子图建模

简单的运动模型的例子。

三个变量表示了机器人的三个位姿。在这个例子中，有一个一元的因子$f_0(x_1)$，代表了先验信息（$x_1$）。两个表示连续姿势相关的二元因子 $f_1(x_1,x_2;o_1)$、$f_2(x_2,x3;o_2)$。其中，$o_1,o_2$表示里程计观测。

2.2 创建因子图

// 新建一个非线性因子图
NonlinearFactorGraph graph;

// 假设x1上添加服从高斯分布的先验
Pose2 priorMean(0.0, 0.0, 0.0);  //先验均值
noiseModel::Diagonal::shared_ptr priorNoise = //先验标准差， 30cm， 30cm， 0.1弧度
  noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1));
graph.add(PriorFactor<Pose2>(1, priorMean, priorNoise));

// 添加里程计因子
Pose2 odometry(2.0, 0.0, 0.0);
noiseModel::Diagonal::shared_ptr odometryNoise =
  noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));
graph.add(BetweenFactor<Pose2>(1, 2, odometry, odometryNoise));
graph.add(BetweenFactor<Pose2>(2, 3, odometry, odometryNoise));

输出结果

Factor Graph:
size: 3
Factor 0: PriorFactor on 1
  prior mean: (0, 0, 0)
  noise model: diagonal sigmas [0.3; 0.3; 0.1];
Factor 1: BetweenFactor(1,2)
  measured: (2, 0, 0)
  noise model: diagonal sigmas [0.2; 0.2; 0.1];
Factor 2: BetweenFactor(2,3)
  measured: (2, 0, 0)
  noise model: diagonal sigmas [0.2; 0.2; 0.1];

2.3 因子图与变量

GTSAM中，有两个很重要的点

1. 因子图及其代码指的是在机器人整个轨迹上的联合概率分布$P(X|Z)$上，而不仅仅是最新的位姿。后续会讨论使用GTSAM进行滤除或增量推理（常用）。
2. GTSAM中一个因子图是对于概率密度$P(X|Z)$的体现，相应的FactorGraph类和他的派生类并不含有解。与之相反，有一个特定的类型Values制定了各个变量（本例中的$x_1,x_2,x_3$）的特定值，用于后续评估预期相关的概率（或误差）。

第二点常常是一个易混淆的点。在设计GTSAM时，采用了与不可变的数学对象所对应的函数类方法。我们在使用时应当将因子图视为一个应用于数值的函数而不是一个对象的模型化，即$f(X)\propto P(X\mid Z)$。

2.4 GTSAM中的非线性优化

这里创建了一个变量示例，将其作为初值以找到整个轨迹的最大后验（MAP）。

// 建立初始估计（不准确的）
Values initial;
initial.insert(1, Pose2(0.5, 0.0, 0.2));
initial.insert(2, Pose2(2.3, 0.1, -0.2));
initial.insert(3, Pose2(4.1, 0.1, 0.1));

// 使用LM优化
Values result = LevenbergMarquardtOptimizer(graph, initial).optimize();

结果如下，可以看出最终结果非常接近真值（0,0,0） (2,0,0) (4,0,0)

Initial Estimate:
Values with 3 values:
Value 1: (0.5, 0, 0.2)
Value 2: (2.3, 0.1, -0.2)
Value 3: (4.1, 0.1, 0.1)

Final Result:
Values with 3 values:
Value 1: (-1.8e-16, 8.7e-18, -9.1e-19)
Value 2: (2, 7.4e-18, -2.5e-18)
Value 3: (4, -1.8e-18, -3.1e-18)

2.5 完全后验推断

GTSAM可用于在合并所有观测信息$Z$后计算每个位姿的协方差矩阵。
因子图编码了后验概率$P(X|Z)$。每个位姿的$x$的均值$\mu$以及协方差$\Sigma$近似了边缘后验密度$P(x|Z)$。这只是一个近似值，因为即使在这种简单的情况下，里程计因子的参数实际上也是非线性的，并且 GTSAM 仅计算真实基础后验的高斯近似值。
下面的代码将复原后验边缘概率。

// 查询边缘
cout.precision(2);
Marginals marginals(graph, result);
cout << "x1 covariance:\n" << marginals.marginalCovariance(1) << endl;
cout << "x2 covariance:\n" << marginals.marginalCovariance(2) << endl;
cout << "x3 covariance:\n" << marginals.marginalCovariance(3) << endl;

相应输出

x1 covariance:
       0.09     1.1e-47     5.7e-33
    1.1e-47        0.09     1.9e-17
    5.7e-33     1.9e-17        0.01
x2 covariance:
       0.13     4.7e-18     2.4e-18
    4.7e-18        0.17        0.02
    2.4e-18        0.02        0.02
x3 covariance:
       0.17     2.7e-17     8.4e-18
    2.7e-17        0.37        0.06
    8.4e-18        0.06        0.03

我们看到的是 $x_1$ 上的边缘协方差 $P(x_1|Z)$ 只是 $x_1$ 上的先验知识，但是随着机器人移动，所有维度的不确定性都无限制地增长，并且 $y$ 和 $\theta$ 分量变为（正 ) 相关。

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

3. 机器人定位

3.1 一元测量因子

在本节中，我们将测量添加到因子图中，这将帮助我们随着时间的推移实际定位机器人。 该示例还可用作创建新因子类型的教程。

使用一元测量因子来处理外部观测。2.1中的因子图在实际中很少使用，因为它只含有里程计因子。这种外部会导致机器人位姿不确定度的快速增加。而上图添加的一元约束只与当前时刻的位姿有关系，可以由GPS、地图匹配位姿等方式得到。

3.2 定义自定义因子

我们可以创建自定义的一元约束。这可以通过继承类NoiseModelFactor1实现，其噪声模型为高斯模型。
$$L(q;m)=\exp \left\{-\frac{1}{2}\Vert h(q)-m{\Vert }_{\Sigma }^2\right\}=f(q)$$
其中，$m$为观测值，认为是已知量；$q$为未知变量；$h(q)$是观测方程（可能为非线性）；$\Sigma$是噪声方差。需要注意的是，$似然L(q;m)$是与条件概率$P(m|q)$成正比的任意函数，仅作为q的一个函数来计算，即可以表示为$f(q)$。
一元约束的定义，可以用来实现GNSS、map-matching等位姿的约束。
每个因子最重要的就是实现函数evaluateError()。因为我们需要将其用于非线性优化，所以应当显式的提供观测方程$h(q)$的雅克比矩阵H。

class UnaryFactor: public NoiseModelFactor1<Pose2> { //Pose2作为默认类型
  double mx_, my_; /// 位置的观测量

public:
  UnaryFactor(Key j, double x, double y, const SharedNoiseModel& model):
    NoiseModelFactor1<Pose2>(model, j), mx_(x), my_(y) {} //传入变量索引j，噪声模型，观测位置

//最重要的实现
  Vector evaluateError(const Pose2& q,
                       boost::optional<Matrix&> H = boost::none) const
  {
    if (H) (*H) = (Matrix(2,3)<< 1.0,0.0,0.0, 0.0,1.0,0.0).finished();
    return (Vector(2) << q.x() - mx_, q.y() - my_).finished();
  }
};

在本例中，观测方程$h\left(\begin{array}{l}q\end{array}\right)=\left[\begin{array}{l}{q}_x\\ q_y\end{array}\right]$，但最终位姿为$q=\left(q_x,q_y,q_{\theta }\right)$，因此矩阵H为
$$H=\left[\begin{array}{lll}1& 0& 0\\ 0& 1& 0\end{array}\right]$$

3.3 使用自定义因子

在3.2中完成了因子的定义，接下来进行使用。

// 为所有位姿添加一元约束
	//定义一元约束噪声
noiseModel::Diagonal::shared_ptr unaryNoise =
	noiseModel::Diagonal::Sigmas(Vector2(0.1, 0.1)); // 10cm std on x,y
	//添加约束
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.4 完全后验推断

这三个GPS因素足以完全约束所有未知姿势，并将它们绑定到全局坐标系。否则，GTSAM将以奇异矩阵异常退出。可完全按照第2.5节的要求恢复边缘值，现在通过以下公式给出解和边缘协方差。误差最大值与2.5中相近，但不再逐步增大。这说明全局约束限制了姿态。

Final Result:
Values with 3 values:
Value 1: (-1.5e-14, 1.3e-15, -1.4e-16)
Value 2: (2, 3.1e-16, -8.5e-17)
Value 3: (4, -6e-16, -8.2e-17)

x1 covariance:
      0.0083      4.3e-19     -1.1e-18
     4.3e-19       0.0094      -0.0031
    -1.1e-18      -0.0031       0.0082
x2 covariance:
      0.0071      2.5e-19     -3.4e-19
     2.5e-19       0.0078      -0.0011
    -3.4e-19      -0.0011       0.0082
x3 covariance:
     0.0083     4.4e-19     1.2e-18
    4.4e-19      0.0094      0.0031
    1.2e-18      0.0031       0.018

以下是增加全局约束前后误差分布对比。

4. pose slam

4.1 回环闭合约束

SLAM问题最简单的实例是PoseSLAM，避免了显式构建环境。SLAM的目标是在给定传感器测量值的情况下，同时定位机器人并绘制环境地图。除了轮式里程计，最常用的传感器之一是2D激光雷达，它提供连续位姿之间的里程计约束，以及机器人的回环约束。
新的因子图如下

代码实现如下

NonlinearFactorGraph graph;
noiseModel::Diagonal::shared_ptr priorNoise =
  noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1));
// 添加先验
graph.add(PriorFactor<Pose2>(1, Pose2(0, 0, 0), priorNoise));

// 添加里程计因子
noiseModel::Diagonal::shared_ptr model =  //里程计噪声
  noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));
graph.add(BetweenFactor<Pose2>(1, 2, Pose2(2, 0, 0     ), model));
graph.add(BetweenFactor<Pose2>(2, 3, Pose2(2, 0, M_PI_2), model));
graph.add(BetweenFactor<Pose2>(3, 4, Pose2(2, 0, M_PI_2), model));
graph.add(BetweenFactor<Pose2>(4, 5, Pose2(2, 0, M_PI_2), model));

// 添加回环约束
graph.add(BetweenFactor<Pose2>(5, 2, Pose2(2, 0, M_PI_2), model));

4.2 使用 MATLAB 接口

这边的话之后不太用matlab，所以跳过。

4.3 读取和优化位姿图

Matlab代码，生成100个位姿，运行结果如像下图所示。绿色位姿为初始位姿，蓝色为优化后位姿。

%% Initialize graph, initial estimate, and odometry noise
datafile = findExampleDataFile('w100.graph');
model = noiseModel.Diagonal.Sigmas([0.05; 0.05; 5*pi/180]);
[graph,initial] = load2D(datafile, model);

%% Add a Gaussian prior on pose x_0
priorMean = Pose2(0, 0, 0);
priorNoise = noiseModel.Diagonal.Sigmas([0.01; 0.01; 0.01]);
graph.add(PriorFactorPose2(0, priorMean, priorNoise));

%% Optimize using Levenberg-Marquardt optimization and get marginals
optimizer = LevenbergMarquardtOptimizer(graph, initial);
result = optimizer.optimizeSafely;
marginals = Marginals(graph, result);

4.4 3D 的 PoseSLAM

PoseSlam可以很容易的扩展为三维位姿，但是需要注意更新旋转表示（四元数或旋转矩阵）。这可以使用GTSAM_USE_QUATERNIONS选择。
通过以下MATLAB代码可以生成下图，绿色为优化前轨迹，红色为优化后轨迹。

%% Initialize graph, initial estimate, and odometry noise
datafile = findExampleDataFile('sphere2500.txt');
model = noiseModel.Diagonal.Sigmas([5*pi/180; 5*pi/180; 5*pi/180; 0.05; 0.05; 0.05]);
[graph,initial] = load3D(datafile, model, true, 2500);
plot3DTrajectory(initial, 'g-', false); % Plot Initial Estimate

%% Read again, now with all constraints, and optimize
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;

5. 基于landmark的slam

5.1 基础知识

在基于地标的SLAM中，通过机器人位姿和地标的位置来显示构建地图。一个典型的链接关系如下图：位姿以里程计马尔科夫链链接，地标可以从不同的位姿被观测到，包括一元约束和二元约束；同时，x1存在先验因子。

该因子图的优化结果如下：绿线和蓝线分别表示了位姿和地标的协方差椭圆表示，轨迹和地标使用红色和青色表示。

其MATLAB代码如下：

% 创建图
graph = NonlinearFactorGraph;

% 创建变量索引（key）
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); % prior at origin
priorNoise = noiseModel.Diagonal.Sigmas([0.3; 0.3; 0.1]);
% add directly to graph
graph.add(PriorFactorPose2(i1, priorMean, priorNoise));

% 添加里程计因子
odometry = Pose2(2.0, 0.0, 0.0);
odometryNoise = noiseModel.Diagonal.Sigmas([0.2; 0.2; 0.1]);
graph.add(BetweenFactorPose2(i1, i2, odometry, odometryNoise));
graph.add(BetweenFactorPose2(i2, i3, odometry, odometryNoise));

% 添加距离观测因子，以极坐标形式。
degrees = pi/180;
brNoise = noiseModel.Diagonal.Sigmas([0.1; 0.2]);
graph.add(BearingRangeFactor2D(i1, j1, Rot2(45*degrees), sqrt(8), brNoise));
graph.add(BearingRangeFactor2D(i2, j1, Rot2(90*degrees), 2, brNoise));
graph.add(BearingRangeFactor2D(i3, j2, Rot2(90*degrees), 2, brNoise));

5.2 键和符号

在上面代码中，4~6行创建变量索引（key）在之前并没有强调。这里是使用符号函数为位姿和地标创建整数索引（key）。在GTSAM中，使用Key类型来处理所有变量，其类型为$size\_t$（32或64位整形）。Key并不需要连续，但是必须唯一。对于不同类型的变量，MATLAB中提供了符号函数，c++中提供了符号类型，用于帮助创建可能距离较远的Key。因此可以开始对任意点开始编号而不用考虑偏移量。
创建Key只需要提供一个字符和一个整数索引。实际上，由符号生成Key是由Values类中的print函数自动生成的。

5.3 更大的场景示例

本例大约含有100个位姿与30个左右的地标，包含119个变量和517个因子，优化时间在10ms之内。通过文件PlanarSLAMExample_graph.m运行而来。

5.4 一个真实世界的例子

下面是悉尼维多利亚公园的数据。蓝色的小椭圆是通过我们的快速算法获得的精确协方差与基于反演的精确协方差（橙色，被蓝色小椭圆所遮挡）一致。绿色椭圆则是基于我们早期工作的协方差。

6. SFM

SFM问题，本例在20X20X20的立方体周围摆放10个相机，用于捕获立方体顶点。
该问题可用GTSAM中完全相同的因子图框架，只是使用的是SFM观测因子。另外，增加了投影因子来计算给定相机位姿和顶点的重投影误差。因子由2D观测和已知的标定参数K来实例化。代码如下

%% Add factors for all measurements
noise = noiseModel.Isotropic.Sigma(2, 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

注：SFM的一个重要问题是数据关联和初始化，这是GTSAM没有完成的。GTSAM只是提供了BA优化。

7. iSAM

GTSAM提供了一种基于贝叶斯树的增量推理算法，即iSAM。对于移动机器人，在有新的传感器数据时能够获取最新的地图是十分重要的。iSAM的目的就是这个。

下面的代码展示了如何在简单的视觉SLAM中使用iSAM。该示例涉及从八个连续相机位姿中看到的八个3D点。因此，在这里省略的第一步中，所有八个地标和第一个位姿都需要被恰当地初始化。在代码中，这是通过在真值附近增加扰动来完成的，但在实际应用中，需要非常小心地进行初始化位姿和地标，尤其是在单目相机中。

//创建非线性iSAM对象。每隔3步进行重新线性化和排序。
//该值取决于问题的非线性程度以及每一步期望的准确程度。
//ISAM2中不需要这个参数，进行了自动化判断。
int relinearizeInterval = 3;  
NonlinearISAM isam(relinearizeInterval);

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

// 在不同的姿势上循环，将观察结果添加到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. 基于新的观测量创建因子。
2. 对新引入的变量创建初始估计。在本例中，第1帧观察到了所有的地标，因此对后续每个时间戳需要初始化的只有新的位姿。
3. 调用isam.update()。

8. 其他应用

8.1 共轭梯度优化

GTSAM还包括用于解决大规模SLAM问题的有效预处理共轭梯度（preconditioned conjugate gradients，PCG）方法。虽然直接法具有二次收敛性，并且对于稀疏问题非常有效，但它们通常需要大量的存储和有效的消除顺序。迭代优化方法只需要访问梯度，内存占用小，但收敛性差。GTSAM的子图预处理法结合了直接法和迭代法的优点，通过确定可以使用直接法轻松解决的子问题，并使用PCG解决剩余部分。

生成树显示为黑色，其余闭环约束显示为红色。GTSAM可以使用生成树作为预条件。

8.2 视觉里程计

视觉里程计。通过跟踪或关联由刚性安装在机器人上的摄像机拍摄的连续图像中的视觉特征，提供连续机器人位姿之间的约束。GTSAM包括C++和MATLAB两个示例代码，以及VO特定的因子。

8.3 视觉SLAM

视觉SLAM是一种SLAM变体，当摄像机在空间中移动时，摄像机可以观察3D点，可以安装在机器人上，也可以用手移动。GTSAM，尤其是iSAM（见下文），可以很容易地调整为在这种情况下的后端优化器。

8.4 固定滞后平滑和过滤

GTSAM可以轻松地执行递归估计，其中因子图中只保留位姿的子集，而其余位姿则被边缘化。在上面的所有示例中，使用所有可用的观测量显式优化所有变量，这称为平滑，因为轨迹是“平滑”的，这就是GTSAM的名称。相反，当图形中只保留最后n个姿势时，可以称为固定滞后平滑。最后，当只保留单个最新姿势时，可以说是滤波，事实上，SLAM的原始公式是基于滤波的。

8.5 离散变量和 HMM

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

后记

本文主要是对官方教程的翻译，代码较为简单，后续会进行代码方面的进一步学习。

---

1. 贝叶斯公式，最大后验概率与似然乘先验成正比。即
   $$P(X\mid Z)=\frac{P(Z\mid X)(PX)}{P(Z)}\propto P(Z\mid X)P(X)$$ ↩︎