从零开始搭二维激光SLAM --- 基于gtsam的后端优化的代码实现

上一篇文章我们分析了如何使用ceres进行位姿图的优化.

这篇文章来讲一下如何使用gtsam进行位姿图的优化.

1 gtsam简介

gtsam是最近几年火起来的一个优化库.

GTSAM（Georgia Tech Smoothing and Mapping）是基于因子图的C++库，它由佐治亚理工学院的教授和学生们创造。它可以解决slam和sfm的问题，当然它也可以解决简单或者更加复杂的估计问题。

1.1 怎么学gtsam

由于其使用了概率图中的因子图这种理论, 以及官方文档的不足, 导致很多人学这个库不知道从哪里入手.

在这里简要说一下gtsam怎么学.

如果是刚接触gtsam的同学, 可以看一下董靖之前的直播, 能够对gtsam先产生个比较直观的印象.

【泡泡机器人公开课】第五十六课：gtsam_tutorial-董靖
https://www.bilibili.com/video/BV1C4411772G/?spm_id_from=333.788.recommend_more_video.1

之后要弄清楚一件事情, 就是学gtsam库和学概率图与因子图是2个事情.

学gtsam库, 就是学习如何使用gtsam库 编写代码进行优化, 只需要学习gtsam的接口, 学习怎么使用gtsam的函数就够了.

就像你学ceres库, 只是学了怎么使用ceres, ceres的接口是什么, 没必要去学ceres是怎么进行自动微分的. 学gtsam库也是一样, 人家就是为了不让每个人都去重复的学这一块的理论, 重复写代码, 所以弄成库放了出来, 直接用就可以了.

而如果你想要学gtsam背后的理论, 概率图和因子图等等 , 才需要去弄清楚什么是概率图, 什么是因子图, 怎么根据因子图做推断等等.

1.2 如何学gtsam库的使用

首先推荐一篇文章, 是这人学gtsam的建议.

GTSAM 库的学习建议
https://zhuanlan.zhihu.com/p/356968742

gtsam.pdf

网上的诸多文章, 大都是翻译的官方教程, 所以这里推荐大家直接去看官方的教程
https://gtsam.org/tutorials/intro.html

这个网址如果打不开, 可以去看 gtsam源码doc文件夹中的 gtsam.pdf, 他们的内容是一样的.

这个文章是翻译的, 但是翻译的不全
gtsam：从入门到使用
https://blog.csdn.net/QLeelq/article/details/111368277#t24

gtsam/examples

然后推荐去看gtsam源码中examples文件夹中的各种代码的举例, 再去结合网上的文章, 学习gtsam的接口应该是很快的.

OdometryExample.cpp 和 Pose2SLAMExample.cpp 两个文件展示了如何使用gtsam进行二维位姿的优化, 后一个文件也是 董靖 在直播中讲的例子. 如果你只是做2维位姿的优化, 那么看懂这2个文件就应该够用了.

如果你想对gtsam有更多的了解, 可以将example中的例子全部看完.

gtsam/examples/README.md 对所有的例子进行了简单的分类, 但是有点和文件对不上. 在这里我对 4.0.2版本的gtsam 的examples中的所有文件进行了简单的分类, 如下所示

• basic
  - SimpleRotation.cpp : 只有一个先验旋转的姿态优化

• 2D pose
  - OdometryExample.cpp : 使用里程计进行位姿估计
  - Pose2SLAMExample.cpp : 使用里程计进行位姿估计
  - PlanarSLAMExample.cpp : 带二维激光观测的二维SLAM位姿估计
  - LocalizationExample.cpp : 自定义GPS观测的GPS与里程计的位姿估计
  - Pose2SLAMExampleExpressions.cpp : 使用自动微分的位姿估计
  - Pose2SLAMExample_g2o.cpp
  - Pose2SLAMExample_graph.cpp
  - Pose2SLAMExample_graphviz.cpp
  - Pose2SLAMExample_lago.cpp
  - Pose2SLAMStressTest.cpp
  - Pose2SLAMwSPCG.cpp
  - RangeISAMExample_plaza2.cpp
  - METISOrderingExample.cpp

• 3D pose
  - Pose3SLAMExample_g2o.cpp : 几乎与二维的是相同的代码
  - Pose3SLAMExample_changeKeys.cpp
  - Pose3SLAMExample_initializePose3Chordal.cpp
  - Pose3SLAMExample_initializePose3Gradient.cpp
  - Pose3SLAMExampleExpressions_BearingRangeWithTransform.cpp

• 视觉
  - CameraResectioning.cpp
  - StereoVOExample.cpp : 双目视觉里程计的例子
  - StereoVOExample_large.cpp
  - SelfCalibrationExample.cpp

• imu预积分
  - ImuFactorsExample.cpp : imu预积分与GPS观测的位姿优化
  - ImuFactorExample2.cpp : 使用isam2进行imu的位姿优化

• isam算法
  - VisualISAMExample.cpp
  - VisualISAM2Example.cpp
  - ISAM2_SmartFactorStereo_IMU.cpp
  - ISAM2Example_SmartFactor.cpp

• kalman filter
  - easyPoint2KalmanFilter.cpp
  - elaboratePoint2KalmanFilter.cpp

• SFM
  - SFMExample.cpp
  - SFMExample_bal.cpp
  - SFMExample_bal_COLAMD_METIS.cpp
  - SFMExampleExpressions.cpp
  - SFMExampleExpressions_bal.cpp
  - SFMExample_SmartFactor.cpp
  - SFMExample_SmartFactorPCG.cpp

• 概率图相关
  - DiscreteBayesNet_FG.cpp
  - UGM_chain.cpp
  - UGM_small.cpp

• 其他
  - InverseKinematicsExampleExpressions.cpp
  - SolverComparer.cpp
  - TimeTBB.cpp

从分类中可以发现gtsam和isam的关系. gtsam是一个基于因子图的库, isam2是可以更快进行矩阵计算和更新的算法. gtsam库包含了isam2算法.

如果想要看更多使用gtsam进行编程的例子, 可以看看董靖自己写的demo.
https://hub.fastgit.org/dongjing3309/gtsam-examples

1.3 如何学因子图

关于 概率图, 因子图, 隐式马尔科夫 等等的学习, 我推荐一个b站上的讲机器学习的视频

【机器学习】【白板推导系列】【合集 1～23】
https://www.bilibili.com/video/BV1aE411o7qd?p=46

之后也有几个文章推荐

干货：因子图优化的资源合集 里边放了学习gtsam理论的视频和论文.
https://zhuanlan.zhihu.com/p/128720019
一文搞懂HMM（隐马尔可夫模型）
https://www.cnblogs.com/skyme/p/4651331.html
从贝叶斯方法谈到贝叶斯网络
https://blog.csdn.net/v_july_v/article/details/40984699

2 基于gtsam的后端优化的代码讲解

头文件也是继承karto::ScanSolver即可

2.1 向因子图中添加变量节点

首先, 看一下构造函数

GTSAMSolver::GTSAMSolver()
{
  // add the prior on the first node which is known
  noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector3(1e-6, 1e-6, 1e-8));
  graph_.emplace_shared<PriorFactor<Pose2>>(0, Pose2(0, 0, 0), priorNoise);
}

向因子图中添加了初始节点以及其协方差矩阵.

之后, 看一下添加节点这个函数.

void GTSAMSolver::AddNode(karto::Vertex<karto::LocalizedRangeScan> *pVertex)
{
  karto::Pose2 odom = pVertex->GetObject()->GetCorrectedPose();
  initialGuess_.insert(pVertex->GetObject()->GetUniqueId(),
                       Pose2(odom.GetX(), odom.GetY(), odom.GetHeading()));
  graphNodes_.push_back(Eigen::Vector2d(odom.GetX(), odom.GetY()));
}

可以看到, 这里将节点的位姿通过 initialGuess_.insert() 添加到因子图中, 作为节点的初值.

并且将这个节点的 xy 坐标保存在 graphNodes_ 这个vector中.

2.2 向因子图中添加因子节点

向因子图中添加因子节点, 对应着向位姿图中添加约束.

如果看完了example中的例子就会发现, 这段代码和例子中的代码几乎一样.

只不过这里先将约束的协方差矩阵转成gtsam格式的协方差矩阵, 然后向因子图中添加 BetweenFactor 格式的因子.

void GTSAMSolver::AddConstraint(karto::Edge<karto::LocalizedRangeScan> *pEdge)
{
  // Set source and target
  int sourceID = pEdge->GetSource()->GetObject()->GetUniqueId();
  int targetID = pEdge->GetTarget()->GetObject()->GetUniqueId();

  // Set the measurement (poseGraphEdge distance between vertices)
  karto::LinkInfo *pLinkInfo = (karto::LinkInfo *)(pEdge->GetLabel());
  karto::Pose2 diff = pLinkInfo->GetPoseDifference();

  // Set the covariance of the measurement
  karto::Matrix3 precisionMatrix = pLinkInfo->GetCovariance();

  Eigen::Matrix<double, 3, 3> cov;
  cov(0, 0) = precisionMatrix(0, 0);
  cov(0, 1) = cov(1, 0) = precisionMatrix(0, 1);
  cov(0, 2) = cov(2, 0) = precisionMatrix(0, 2);
  cov(1, 1) = precisionMatrix(1, 1);
  cov(1, 2) = cov(2, 1) = precisionMatrix(1, 2);
  cov(2, 2) = precisionMatrix(2, 2);
  noiseModel::Gaussian::shared_ptr model = noiseModel::Diagonal::Covariance(cov);
  graph_.emplace_shared<BetweenFactor<Pose2>>(sourceID, targetID, Pose2(diff.GetX(), diff.GetY(), diff.GetHeading()), model);
}

2.3 优化求解

这段代码说明了如何使用gtsam进行优化的求解.

上边的2个函数将因子图已经构建好了, 这里就可以直接求解了.

首先, 设置 LM算法的各种参数.

然后生成一个 LevenbergMarquardtOptimizer 格式的LM优化器, 通过调用优化器的 optimize() 函数进行优化.

最后, 将优化完的节点进行保存.

void GTSAMSolver::Compute()
{
  corrections_.clear();
  graphNodes_.clear();

  LevenbergMarquardtParams parameters;
  parameters.relativeErrorTol = 1e-5;
  parameters.maxIterations = 500;
  LevenbergMarquardtOptimizer optimizer(graph_, initialGuess_, parameters);
  
  // optimize
  Values result = optimizer.optimize();
  Values::ConstFiltered<Pose2> viewPose2 = result.filter<Pose2>();

  // put values into corrections container
  for (const Values::ConstFiltered<Pose2>::KeyValuePair &key_value : viewPose2)
  {
    karto::Pose2 pose(key_value.value.x(), key_value.value.y(), key_value.value.theta());
    corrections_.push_back(std::make_pair(key_value.key, pose));
    graphNodes_.push_back(Eigen::Vector2d(key_value.value.x(), key_value.value.y()));
  }
}

3 运行

3.1 依赖

这篇文章的代码是需要依赖 4.0.2 版本的gtsam库, 如果没装gtsam的需要先安装一下.
我将gtsam库的安装包放在了 工程Creating-2D-laser-slam-from-scratch/TrirdParty文件加内, 可以直接解压安装.

安装的方法可以看一下 install_dependence.sh 脚本中的安装指令, 也可以直接执行这个脚本进行所有依赖项的安装.

安装完了gtsam之后, 编译代码, 如果一切顺利的话是可以编译通过的.

这里要说一下, 我一直用的是 1604版本的ubuntu, 没试过其他版本的ubuntu能不能编译通过. 如果实在编译不过就看看文章或者看看源码吧.

3.2 运行

本篇文章对应的数据包, 请在我的公众号中回复 lesson6 获得，并将launch中的bag_filename更改成您实际的目录名。

我将之前使用过的数据包的链接都放在腾讯文档里了, 腾讯文档的地址如下:
https://docs.qq.com/sheet/DVElRQVNlY0tHU01I?tab=BB08J2

通过如下命令运行本篇文章对应的程序
roslaunch lesson6 karto_slam_outdoor.launch solver_type:=gtsam_solver

3.3 结果分析

启动之后, 会显示出使用的优化器的具体类型.

[ INFO] [1637561045.388129070]: ----> Karto SLAM started.
[ INFO] [1637561045.420115628]: Use back end.
[ INFO] [1637561045.420160652]: solver type is GtsamSolver.

在运行前期, 由于没有找到回环, 所以一直没有进行优化. 在最后阶段产生回环时, 会调用基于ceres的优化, 并打印处如下的log.

[ INFO] [1637563465.472380312, 1606808846.502199766]: [gtsam] Calling gtsam for Optimization
[ INFO] [1637563465.841370726, 1606808846.864457794]: [gtsam] Calling gtsam for Optimization

优化前

优化后

前几次优化之后, 可以看到, 地图的方向发生了变化. 但是误差依然存在
在最后一次的优化完成之后, 地图和雷达数据才完全匹配上.

可以感受到, gtsam的优化在效果上没有g2o与ceres的优化的效果好. 而且地图的方向还会发生变化.

最终的地图

4 总结

通过这篇文章, 我们知道了如何使用gtsam进行后端优化, 体验了gtsam进行优化的效果.

下篇文章将进行从零开始搭二维激光SLAM系列的总结, 与下个系列的期望.