Build two-dimensional laser SLAM from scratch --- code implementation based on gtsam-based back-end optimization

In the previous article, we analyzed how to use ceres to optimize the pose graph.

This article will talk about how to use gtsam to optimize the pose graph.

1 Introduction to gtsam

GTSAM is an optimization library that has become popular in recent years.

GTSAM (Georgia Tech Smoothing and Mapping) is a factor graph-based C++ library created by professors and students at the Georgia Institute of Technology. It can solve the problems of slam and sfm, and of course it can also solve simple or more complex estimation problems.

1.1 How to learn gtsam

Due to the use of the theory of factor graphs in probability graphs and the lack of official documentation, many people learn this library and don't know where to start.

Here is a brief talk about how to learn gtsam.

If you are new to gtsam, you can watch Dong Jing's previous live broadcast, and you can have a more intuitive impression of gtsam first.

[Bubble Robot Open Class] Lesson 56: gtsam_tutorial-Dong Jing
https://www.bilibili.com/video/BV1C4411772G/?spm_id_from=333.788.recommend_more_video.1

After that, one thing to be clear is that learning the gtsam library and learning the probability map and factor map are two things.

To learn the gtsam library is to learn how to use the gtsam library to write codes for optimization. You only need to learn the gtsam interface and learn how to use the gtsam functions.

Just like you learn the ceres library, you just learned how to use ceres, what is the interface of ceres, there is no need to learn how ceres performs automatic differentiation. The same is true for learning the gtsam library, they just want to prevent everyone from repeating the learning This part of the theory requires repeated code writing, so it is made into a library and released, and it can be used directly.

And if you want to learn the theory behind gtsam, probability graph and factor graph, etc., you need to figure out what is a probability graph, what is a factor graph, how to make inferences based on a factor graph, etc.

1.2 How to learn how to use the gtsam library

First recommend an article, which is the suggestion of this person to learn gtsam.

Learning suggestions for GTSAM library
https://zhuanlan.zhihu.com/p/356968742

gtsam.pdf

Most of the articles on the Internet are official tutorials for translation, so I recommend you to go directly to the official tutorial
https://gtsam.org/tutorials/intro.html

If this URL cannot be opened, you can go to the gtsam.pdf in the gtsam source code doc folder, their contents are the same.

This article is translated, but the translation is incomplete
gtsam: from getting started to using
https://blog.csdn.net/QLeelq/article/details/111368277#t24

gtsam/examples

Then it is recommended to look at the various code examples in the examples folder in the gtsam source code, and then combine the online articles, learning the interface of gtsam should be very fast.

The two files OdometryExample.cpp and Pose2SLAMExample.cpp show how to use gtsam to optimize two-dimensional poses. The latter file is also an example that Dong Jing talked about in the live broadcast. If you only do two-dimensional pose optimization, then understand These 2 files should be enough.

If you want to know more about gtsam, you can read all the examples in the example.

gtsam/examples/README.md has a simple classification of all the examples, but it is a bit inconsistent with the files. Here I have a simple classification of all the files in the examples of the 4.0.2 version of gtsam , as shown below

• basic
  - SimpleRotation.cpp : Pose optimization with only one prior rotation

• 2D pose
  - OdometryExample.cpp : pose estimation using odometry
  - Pose2SLAMExample.cpp : pose estimation using odometer
  - PlanarSLAMExample.cpp : 2D SLAM pose estimation with 2D laser observation
  - LocalizationExample.cpp : custom Pose estimation with GPS and odometry from GPS observations
  - Pose2SLAMExampleExpressions.cpp : Pose estimation using automatic differentiation
  - Pose2SLAMExample_g2o.cpp
  - Pose2SLAMExample_graph.cpp
  -
  Pose2SLAMExample_graphviz.cpp
  - Pose2SLAMExample_lago.cpp
  - Pose2SLAMStressTest.cpp
  - Pose2SLAMexample_SPCG.lapp - cpp
  -METISOrderingExample.cpp

• 3D pose
  - Pose3SLAMExample_g2o.cpp : Almost the same code as 2D pose
  - Pose3SLAMExample_changeKeys.cpp
  - Pose3SLAMExample_initializePose3Chordal.cpp
  - Pose3SLAMExample_initializePose3Gradient.cpp
  - Pose3SLAMexampleExpressions_BearingRangeWithTransform.cpp

• Vision
  - CameraResectioning.cpp
  - StereoVOExample.cpp : Example of binocular vision odometry
  - StereoVOExample_large.cpp
  - SelfCalibrationExample.cpp

• imu pre-integration
  - ImuFactorsExample.cpp : pose optimization of imu pre-integration and GPS observation
  - ImuFactorExample2.cpp : use isam2 for imu pose optimization

• 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

• Probability graph correlation
  - DiscreteBayesNet_FG.cpp
  - UGM_chain.cpp
  - UGM_small.cpp

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

The relationship between gtsam and isam can be found from the classification. gtsam is a library based on factor graphs, and isam2 is an algorithm that can perform matrix calculations and updates faster. The gtsam library contains the isam2 algorithm.

If you want to see more programming examples using gtsam, you can check out the demo written by Dong Jing himself.
https://hub.fastgit.org/dongjing3309/gtsam-examples

1.3 How to learn factor graph

Regarding the learning of probability graphs, factor graphs, hidden Markov, etc., I recommend a video on machine learning on station b

[Machine Learning] [Whiteboard Derivation Series] [Collection 1~23]
https://www.bilibili.com/video/BV1aE411o7qd?p=46

After that, there are several articles recommended

Dry goods: There are videos and papers on learning gtsam theory in the resource collection of factor graph optimization.
https://zhuanlan.zhihu.com/p/128720019
An article to understand HMM (Hidden Markov Model)
https://www.cnblogs .com/skyme/p/4651331.htmlFrom
the Bayesian method to the Bayesian network
https://blog.csdn.net/v_july_v/article/details/40984699

2 Code explanation of back-end optimization based on gtsam

The header file can also inherit karto::ScanSolver

2.1 Add variable nodes to the factor graph

First, look at the constructor

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

An initial node is added to the factor graph along with its covariance matrix.

After that, take a look at the add node function.

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

It can be seen that the pose of the node is added to the factor graph through initialGuess_.insert() as the initial value of the node.

And save the xy coordinates of this node in the graphNodes_ vector.

2.2 Adding factor nodes to the factor graph

Adding factor nodes to the factor graph corresponds to adding constraints to the pose graph.

If you read the example in example, you will find that this code is almost the same as the code in the example.

It's just that here first convert the constrained covariance matrix into a covariance matrix in gtsam format, and then add factors in the BetweenFactor format to the factor graph.

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 Optimization solution

This code shows how to use gtsam to solve the optimization.

The above two functions have constructed the factor graph, and here it can be solved directly.

First, set various parameters of the LM algorithm.

Then generate an LM optimizer in LevenbergMarquardtOptimizer format, and optimize by calling the optimize() function of the optimizer.

Finally, save the optimized nodes.

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 run

3.1 Dependency

The code of this article needs to rely on the gtsam library of version 4.0.2. If you don’t have gtsam installed, you need to install it first.
I put the installation package of the gtsam library in the project Creating-2D-laser-slam-from-scratch/TrirdParty The file is added, and it can be directly decompressed and installed.

For the installation method, please refer to the installation instructions in the install_dependence.sh script, or you can directly execute this script to install all dependencies.

After installing gtsam, compile the code, if everything goes well, it can be compiled and passed.

Here I want to say that I have been using the 1604 version of ubuntu, and I have not tried whether other versions of ubuntu can be compiled. If it does not compile, please read the article or look at the source code.

3.2 run

For the data package corresponding to this article, please reply to lesson6 in my official account to get it, and change the bag_filename in launch to your actual directory name.

I put the links of the previously used data packages in the Tencent document, the address of the Tencent document is as follows:
https://docs.qq.com/sheet/DVElRQVNlY0tHU01I?tab=BB08J2

Run the program corresponding to this article through the following command
roslaunch lesson6 karto_slam_outdoor.launch solver_type:=gtsam_solver

3.3 Analysis of results

After startup, the specific type of optimizer used will be displayed.

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

In the early stage of operation, because no loopback was found, no optimization has been performed. When a loopback is generated in the final stage, the optimization based on ceres will be called, and the following log will be printed.

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

before optimization

Optimized

After the first few optimizations, it can be seen that the direction of the map has changed. But the error still exists.
After the last optimization is completed, the map and radar data are completely matched.

It can be felt that the optimization effect of gtsam is not as good as that of g2o and ceres. Moreover, the direction of the map will change.

final map

4 Summary

Through this article, we know how to use gtsam for backend optimization and experience the effect of gtsam optimization.

The next article will summarize the two-dimensional laser SLAM series from scratch, and the expectations of the next series.