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

1 g2o简介

g2o(General Graphic Optimization)是基于图优化的库。

图优化是把优化问题表现成图的一种方式。一个图由若干个顶点(Vertex)，以及连接这些顶点的边(Edge)组成。用顶点表示优化变量，用边表示误差项。

更具体的介绍网上有很多, 这里就不多说了.

在这里推荐几篇g2o的讲解文章, 讲的很好.

从零开始一起学习SLAM | 理解图优化，一步步带你看懂g2o代码
https://blog.csdn.net/electech6/article/details/86534426
从零开始一起学习SLAM | 掌握g2o顶点编程套路
https://blog.csdn.net/electech6/article/details/88018481
从零开始一起学习SLAM | 掌握g2o边的代码套路
https://zhuanlan.zhihu.com/p/58521241
SLAM本质剖析-G2O
https://www.guyuehome.com/34679

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

关于karto中位姿图的节点及边的讲解, 在上一篇文章中已经介绍完了, 感兴趣的同学可以看看.

从零开始搭二维激光SLAM — Karto后端的位姿图相关的数据结构分析
https://blog.csdn.net/tiancailx/article/details/121317039

2.1 头文件

首先, 声明一个G2oSolver类, 继承karto中的优化接口ScanSolver.

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 优化器的初始化

g2o的初始化基本就这4个步骤, 上边的第一篇文章中已经讲的很清楚了, 我就不多说了.
将优化器的初始化放在构造函数中了, 使用LinearSolverCSparse线性求解器, 使用了LM下降算法.

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 向位姿图中添加节点

karto中的顶点的数据结构是 karto::Vertexkarto::LocalizedRangeScan , 在这里要转成g2o定义好的数据结构 VertexSE2.

通过源码可知,
VertexSE2 : public BaseVertex<3, SE2> //2D pose Vertex, (x,y,theta)

VertexSE2代表了 2D 情况下的 pose 的顶点, 分别是 (x,y,theta).

这里的代码十分清晰, 就是获取到节点匹配后的位姿, 转成 g2o::SE2 格式传入到 poseVertex->setEstimate() 函数中.

并且设置一下这个节点的ID就可以了.

设置完节点再将这个节点放入优化器中.

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 向位姿图中添加边(约束)

首先, 先声明一个 EdgeSE2 格式的边, 然后向这个边中添加数据.

首先向边中添加2个节点对应的id号.

然后将这两个位姿间的位姿变换作为观测添加到 g2o::SE2 measurement() 函数中.

正常情况下 通过2个节点的pose与观测是一样的, 不会对优化产生作用. 但是在检测出回环后, 节点的位姿将会改变, 这时再通过2个节点的pose计算出的位姿变换与这个观测将不一样, 就会对优化产生作用.

再将kartoz中的协方差矩阵, 求逆, 添加到边的信息矩阵中.

最后将设置好的边添加到位姿图中.

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 进行求解

进行求解, 并将优化后的位姿进行保存.

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 运行

3.1 依赖

这篇文章的代码是需要依赖 kinetic 版本的 g2o的, 需要通过如下指令进行安装.
sudo apt-get install ros-kinetic-libg2o

没有测试过 ubuntu1804的g2o, 不知道能不能编译通过.

3.2 运行

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

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

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

3.3 结果分析

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

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

在运行前期, 由于没有找到回环, 所以一直没有进行优化.

进行优化的时候, 会在终端中打印 [g2o] 字样的log.

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

之前 Karto的后端优化与回环检测功能对比测试与分析 这篇文章分析过, 如果不加回环和优化功能, 会在最后产生十分明显的叠图.

优化前

优化后

最终的地图

可以看到, 优化还是起了作用的.

4 总结

这篇文章简要说明了如何使用g2o进行karto的优化的计算, 只是定性的分析了一下g2o优化确实产生了效果, 如果有同学想要自己定量的分析可以自己更改下代码做一下分析.

下篇文章将使用ceres来进行后端优化的计算.