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

1 Introduction to g2o

g2o (General Graphic Optimization) is a library based on graph optimization.

Graph optimization is a way to represent optimization problems as graphs. A graph consists of several vertices (Vertex) and edges (Edge) connecting these vertices. The optimization variables are represented by vertices, and the error terms are represented by edges.

There are many more specific introductions on the Internet, so I won’t go into details here.

Here are a few explanation articles about g2o, which are very good.

Learn SLAM together from scratch | Understand graph optimization, take you to understand g2o code step by step
https://blog.csdn.net/electech6/article/details/86534426
Learn SLAM together from scratch | Master g2o vertex programming routines
https: //blog.csdn.net/electech6/article/details/88018481
Learn SLAM together from scratch | Master the code routines of g2o
https://zhuanlan.zhihu.com/p/58521241
Analysis of the essence of SLAM-G2O
https:// www.guyuehome.com/34679

2 Code explanation of backend optimization based on g2o

The explanation of the nodes and edges of the pose graph in karto has been introduced in the previous article, interested students can take a look.

Build two-dimensional laser SLAM from scratch — data structure analysis related to the pose graph of the Karto backend
https://blog.csdn.net/tiancailx/article/details/121317039

2.1 Header file

First, declare a G2oSolver class that inherits the optimization interface ScanSolver in karto.

class G2oSolver : public karto::ScanSolver
{
    
    
public:
  G2oSolver();
  virtual ~G2oSolver();

public:
  virtual void Clear();
  virtual void Compute();
  virtual const karto::ScanSolver::IdPoseVector &GetCorrections() const;

  virtual void AddNode(karto::Vertex<karto::LocalizedRangeScan> *pVertex);
  virtual void AddConstraint(karto::Edge<karto::LocalizedRangeScan> *pEdge);

  void publishGraphVisualization(visualization_msgs::MarkerArray &marray);

private:
  karto::ScanSolver::IdPoseVector mCorrections;
  g2o::SparseOptimizer mOptimizer;
};

2.2 Initialization of the optimizer

The initialization of g2o basically consists of these 4 steps. The first article above has already explained it very clearly, so I won’t say more. Put the initialization of the optimizer in the constructor,
use the LinearSolverCSparse linear solver, use LM descent algorithm.

typedef g2o::BlockSolver<g2o::BlockSolverTraits<-1, -1>> SlamBlockSolver;
typedef g2o::LinearSolverCSparse<SlamBlockSolver::PoseMatrixType> SlamLinearSolver;

G2oSolver::G2oSolver()
{
    
    
  // 第1步：创建一个线性求解器LinearSolver
  SlamLinearSolver *linearSolver = new SlamLinearSolver();
  linearSolver->setBlockOrdering(false);
  // 第2步：创建BlockSolver。并用上面定义的线性求解器初始化
  SlamBlockSolver *blockSolver = new SlamBlockSolver(linearSolver);
  // 第3步：创建总求解器solver。并从GN, LM, DogLeg 中选一个，再用上述块求解器BlockSolver初始化
  g2o::OptimizationAlgorithmLevenberg *solver =
      new g2o::OptimizationAlgorithmLevenberg(blockSolver);
  // 第4步：创建稀疏优化器（SparseOptimizer）
  mOptimizer.setAlgorithm(solver);
}

2.3 Adding nodes to the pose graph

The data structure of the vertices in karto is karto::Vertexkarto::LocalizedRangeScan, which needs to be converted into the data structure VertexSE2 defined by g2o.

According to the source code,
VertexSE2 : public BaseVertex<3, SE2> //2D pose Vertex, (x,y,theta)

VertexSE2 represents the vertices of the pose in 2D, which are (x, y, theta).

The code here is very clear, that is, the pose after the node matching is obtained, converted into g2o::SE2 format and passed to the poseVertex->setEstimate() function.

And set the ID of this node.

After setting up the node, put this node into the optimizer.

void G2oSolver::AddNode(karto::Vertex<karto::LocalizedRangeScan> *pVertex)
{
    
    
  karto::Pose2 odom = pVertex->GetObject()->GetCorrectedPose();
  g2o::VertexSE2 *poseVertex = new g2o::VertexSE2;
  poseVertex->setEstimate(g2o::SE2(odom.GetX(), odom.GetY(), odom.GetHeading()));
  poseVertex->setId(pVertex->GetObject()->GetUniqueId());
  mOptimizer.addVertex(poseVertex);
  ROS_DEBUG("[g2o] Adding node %d.", pVertex->GetObject()->GetUniqueId());
}

2.4 Adding edges (constraints) to the pose graph

First, declare an edge in EdgeSE2 format, and then add data to this edge.

First add the id numbers corresponding to the two nodes to the edge.

The pose transformation between these two poses is then added as an observation to the g2o::SE2 measurement() function.

Under normal circumstances, the pose of the two nodes is the same as the observation, and will not affect the optimization. But after the loop is detected, the pose of the node will change, and then the position calculated by the pose of the two nodes The attitude transformation will be different from this observation, and it will have an effect on the optimization.

Then add the covariance matrix in kartoz to the side information matrix.

Finally, the set edges are added to the pose graph.

void G2oSolver::AddConstraint(karto::Edge<karto::LocalizedRangeScan> *pEdge)
{
    
    
  // Create a new edge
  g2o::EdgeSE2 *odometry = new g2o::EdgeSE2;

  // Set source and target
  int sourceID = pEdge->GetSource()->GetObject()->GetUniqueId();
  int targetID = pEdge->GetTarget()->GetObject()->GetUniqueId();
  odometry->vertices()[0] = mOptimizer.vertex(sourceID);
  odometry->vertices()[1] = mOptimizer.vertex(targetID);

  // Set the measurement (odometry distance between vertices)
  karto::LinkInfo *pLinkInfo = (karto::LinkInfo *)(pEdge->GetLabel());
  karto::Pose2 diff = pLinkInfo->GetPoseDifference();
  g2o::SE2 measurement(diff.GetX(), diff.GetY(), diff.GetHeading());
  odometry->setMeasurement(measurement);

  // Set the covariance of the measurement
  karto::Matrix3 precisionMatrix = pLinkInfo->GetCovariance().Inverse();
  Eigen::Matrix<double, 3, 3> info;
  info(0, 0) = precisionMatrix(0, 0);
  info(0, 1) = info(1, 0) = precisionMatrix(0, 1);
  info(0, 2) = info(2, 0) = precisionMatrix(0, 2);
  info(1, 1) = precisionMatrix(1, 1);
  info(1, 2) = info(2, 1) = precisionMatrix(1, 2);
  info(2, 2) = precisionMatrix(2, 2);
  odometry->setInformation(info);

  // Add the constraint to the optimizer
  ROS_DEBUG("[g2o] Adding Edge from node %d to node %d.", sourceID, targetID);
  mOptimizer.addEdge(odometry);
}

2.5 Solving

Solve and save the optimized pose.

void G2oSolver::Compute()
{
    
    
  mCorrections.clear();

  // Fix the first node in the graph to hold the map in place
  g2o::OptimizableGraph::Vertex *first = mOptimizer.vertex(0);
  if (!first)
  {
    
    
    ROS_ERROR("[g2o] No Node with ID 0 found!");
    return;
  }
  first->setFixed(true);

  // Do the graph optimization
  mOptimizer.initializeOptimization();
  int iter = mOptimizer.optimize(40);

  // Write the result so it can be used by the mapper
  g2o::SparseOptimizer::VertexContainer nodes = mOptimizer.activeVertices();
  for (g2o::SparseOptimizer::VertexContainer::const_iterator n = nodes.begin(); n != nodes.end(); n++)
  {
    
    
    double estimate[3];
    if ((*n)->getEstimateData(estimate))
    {
    
    
      karto::Pose2 pose(estimate[0], estimate[1], estimate[2]);
      mCorrections.push_back(std::make_pair((*n)->id(), pose));
    }
    else
    {
    
    
      ROS_ERROR("[g2o] Could not get estimated pose from Optimizer!");
    }
  }
}

3 run

3.1 Dependency

The code of this article needs to rely on the kinetic version of g2o, which needs to be installed through the following instructions.
sudo apt-get install ros-kinetic-libg2o

I haven't tested g2o of ubuntu1804, so I don't know if it can be compiled.

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:=g2o_solver

3.3 Analysis of results

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

[ INFO] [1636946951.395714883]: ----> Karto SLAM started.
[ INFO] [1636946951.445798371]: Use back end.
[ INFO] [1636946951.445875510]: solver type is G2OSolver.

In the early stage of operation, because no loopback was found, it has not been optimized.

When optimizing, the log of [g2o] will be printed in the terminal.

[ INFO] [1636947150.294108945, 1606808847.990224492]: [g2o] Optimization finished after 31 iterations.
[ INFO] [1636947150.332113885, 1606808848.030497104]: [g2o] Clearing Optimizer..

Previously, Karto’s back-end optimization and loopback detection function comparison test and analysis have been analyzed in this article. If the loopback and optimization functions are not added, a very obvious overlay will be generated at the end.

before optimization

Optimized

final map

It can be seen that the optimization still works.

4 Summary

This article briefly explains how to use g2o to calculate the optimization of karto. It is just a qualitative analysis that the g2o optimization has indeed produced an effect. If some students want to do their own quantitative analysis, they can change the code to do the analysis.

The next article will use ceres to calculate the back-end optimization.