gtsam：从入门到使用

一. 总览

GTSAM（Georgia Tech Smoothing and Mapping）是基于因子图的C++库，它由佐治亚理工学院的教授和学生们创造。它可以解决slam和sfm的问题，当然它也可以解决简单或者更加复杂的估计问题。
因子图是一种图模型，非常适合于复杂的估计问题的建模，比如SLAM或者是SFM( Structure from Motion)。贝叶斯网络属于一种无环图模型，如果对贝叶斯网络感兴趣可以看本人写过的一篇文章。

参考网站：
官网教程：https://gtsam.org/tutorials/intro.html
github: https://github.com/borglab/gtsam
iSAM1:http://www.cs.cmu.edu/~kaess/pub/Kaess08tro.pdf
iSAM2：https://www.cs.cmu.edu/~kaess/pub/Kaess12ijrr.pdf

因子图的三个基本组成部分：

• 因子图（factor graph）：它属于一个二分图，由因子和变量连接而成。
• 变量（variables）：估计问题中的未知随机变量。
• 因子（factors）：非线性因子表示变量之间的约束，在slam中，可能为landmark或者odometry的读数。

二. 贝叶斯网络和因子图

• 贝叶斯网络

我们下面先来讲解一个贝叶斯网络例子：

上图的贝叶斯网络由三步的隐马尔可夫模型组成。上面的马尔科夫链由一个先验概率和两个转移概率组成：$P(X_1),P(X_2|X_1),P(X_3|X_2)$。给定一组测量值$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)$。由贝叶斯法则可知，最大化后验概率正比于似然估计与先验概率的乘积，即：
$$P(X|Z)=\frac{P(Z|X)(PX)}{P(Z)}\propto P(Z|X)P(X)$$
其中P(Z|X)表示似然估计，P(X)表示先验概率。
在本例中，我们定义似然估计$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(X3;z_3)$$

• 因子图
  
  上图中的因子图由三步的隐马尔可夫模型组成。其中未知变量(variables)为:$X_1,X_2,X_3$。后验概率最大化模型：
  $$f(X_1,X_2,X_3)=\prod f_i({\chi }_i)$$
  上图的因子图中黑块表示因子（factors），对于每一个因子$f_i$，都依赖于变量。

三. 机器人运动建模

3.1 使用因子图建模

在深入slam之前，我们先来考虑以下一个马尔科夫链的机器人运动模型。

$x_1,x_2,x_3$：三个变量，表示机器人随时间变化的位姿。
一元因子（unary factor）：$f_0(x_1)$，组成了我们的$x_1$的先验知识。
二元因子（binary factor）：$f_1(x_1,x_2,o_1),f_2(x_2,x_3,o_2)$，表示连续位姿的相关性。其中$o_1,o_2$表示里程计的测量值。

3.2 建立因子图

上图中的因子图用GTSAM中的C++语言表示如下：

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

// 添加一个高斯先验x_1
Pose2 priorMean(0.0, 0.0, 0.0);
noiseModel::Diagonal::shared_ptr priorNoise =
  noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1));//对角线高斯模型0.3m，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));

当运行这个例子时候(make OdometryExample.run)，将会输出：

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];//第三个

3.3 因子图与变量

GTSAM中的两个重要思想：

1. 因子图体现在联合概率P(X|Z)中，而不仅仅是最新的位姿，其中X表示整个轨迹$\{x_1,x_2,x_3\}$
2. 在GTSAM中仅仅指明了概率密度P(X|Z)，和派生出来的概念。
   应该把因子图当作一个函数：f(X) ∝ P(X|Z).

3.4 GTSAM中的非线性优化

下面创建了一些变量（values），并且进行了初始估计来进行最大后验估计估计X。

// create (deliberately inaccurate) initial estimate
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));

// optimize using Levenberg-Marquardt optimization
Values result = LevenbergMarquardtOptimizer(graph, initial).optimize();

相应的输出为：

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)

3.5 全后验推论

GTSAM也可以用于计算每一个位姿的协方差矩阵，认识到因子图编码了后验概率P(X|Z)，对于每个$x$的均值$u$和协方差$\Sigma$，都有一个边缘后验概率弥补$P(x|Z)$。这仅仅是一个近似，因为最简单的里程计因子也是非线性的，GTSAM只计算高斯近似下的后验。
下面的代码将复原后验边缘概率。

// Query the marginals
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

需要注意的是，这里的协方差矩阵是由相对坐标给出的，而不是绝对值。

四. 机器人定位

4.1 一元测量因子

我们增加一个测量因子(unary measurement factors)帮助我们实时的进行机器人定位。

在第三部分的举例中，对实际的机器人并不实用，因为它只有里程计的测量因子。而在上图中，我们使用了一元测量因子来处理外部测量（省略了先验$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、激光测距仪等。

4.2 自定义因子

在GTSAM中，我们使用NoiseModelFactor1定义一元因子，它利用了高斯噪声模型：
$$L(q;m)=exp\{-\frac{1}{2}||h(q)-m|{|}_{\Sigma }^2\}=f(q)$$
其中：
m：测量值；q未知变量；h(q)表示测量函数。Σ表示噪声协方差。由上面的概念$L(q;m)$表示似然估计，它仅仅是q的估计，所以可以表示为$f(q)$，实际上它就是$P(m|q)$。
下面举例怎么自定义一元因子（UnaryFactor），这里用到了GPS-like测量似然。

class UnaryFactor: public NoiseModelFactor1<Pose2> {
     
  double mx_, my_; ///< X and Y measurements

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
  {
     
    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();
  }
};

上式中evaluateError表示评价误差：$E(q)=h(q)-m$,H表示$h(q)$的雅可比矩阵。在这个例子中$h$表示二维函数：
$$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}1& 0& 0\\ 0& 1& 0\end{array}\right)$$

4.3 使用自定义因子

下列代码片段描述了怎么创建和增加自定义因子到因子图中。

// add unary measurement factors, like GPS, on all three poses
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));

上面的代码中首先创建了一个噪声模型，指定了两个标准偏差$m_x$和$m_y$。第4-6行中使用了自定义一元因子模型UnaryFactor，并且加入因子图中。

4.4 全后验推论

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

它与3.5中的全后验相比，这三个值并没有很大的突变。

五. PoseSLAM

5.1 闭环约束

除了轮子里程计之外，常用的测距仪由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));

// Add odometry factors
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));

// Add the loop closure constraint
graph.add(BetweenFactor<Pose2>(5, 2, Pose2(2, 0, M_PI_2), model));

代码中，创建了先验的一元因子$f_0(x_1)$，和四个随里程运动的二元因子$f_t(x_t,x_{t+1})$。最后还创建了一个闭环约束$f_5(x_5,x_2)$。

5.2 使用Matlab接口

GTSAM中提供了matlab接口。

5.3 读取和优化姿态图

上图展示了曼哈顿的100个位姿图，初始位姿为绿色的，利用协方差优化后的为蓝色的。
代码为：

5.4 3D中的poseSLAM

GTSAM支持四元数(quaternions )和3*3旋转矩阵的形式。通过GTSAM_USE_QUATERNIONS进行选择。

3D中地图中绿色为里程计的初始估计，优化后的路径为红色。
以下也是matlab的代码：

%% 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);

%% 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;

六. 基于Landmark的slam

6.1 基础

上图基于landmark的slam中，位姿是基于马尔可夫链，landmarks则是多个位姿下的观测值。

上图优化的结构中，绿色的椭圆为位姿，路标表示蓝色的协方差。matlab代码如下；

% Create graph container and add factors to it
graph = NonlinearFactorGraph;

% Create keys for variables
i1 = symbol('x',1); i2 = symbol('x',2); i3 = symbol('x',3);
j1 = symbol('l',1); j2 = symbol('l',2);

% Add prior
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));

% Add odometry
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));

% Add bearing/range measurement factors
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));

6.2 Keys和Symbols

在GTSAM中我们使用Key类型表示变量（32或64位的整形）。symbol函数则可以创建一个大的keys。在上面的代码中我们使用’x’表示位姿，‘I’表示landmarks。

6.3 A Larger Example

上图中表示100个位姿和差不多30个landmarks（这个例子在PlanarSLAMExample_graph.m中）

6.4 A Real-World Example

上图是悉尼的维多利亚公园的数据集。蓝色的表示协方差(Kaess and Dellaert, 2009).，绿色是更早时候的算法 (Kaess et al., 2008).。

七. iSAM: Incremental Smoothing and Mapping

GTSAM中提供了iSAM和iSAM2算法，具体算法可以查看相关的论文(Kaess et al. (2008); Kaess et al. (2012) )。iSAM提供了高效的更新地图的方法。
下列代码位iSAM实例：

int relinearizeInterval = 3;
NonlinearISAM isam(relinearizeInterval);

// ... first frame initialization omitted ...

// Loop over the different poses, adding the observations to iSAM
for (size_t i = 1; i < poses.size(); ++i) {
     

  // Add factors for each landmark observation
  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)
    );
  }

  // Add an initial guess for the current pose
  Values initialEstimate;
  initialEstimate.insert(Symbol('x', i), initial_x[i]);

  // Update iSAM with the new factors
  isam.update(graph, initialEstimate);
 }

这段代码是一个典型iSAM循环：

1. 给新测量创建新的因子。GenericProjectionFactor。
2. 给所有的新引进变量创建新的估计。initialEstimate.insert(Symbol(‘x’, i), initial_x[i]);
3. 更新iSAM。isam.update(graph, initialEstimate);