Karto后端揭秘：2D激光SLAM世界里的位姿图解析与提升路径

从这篇文章开始, 学习如何使用g2o, ceres, gtsam三个优化库进行后端优化.

在开始使用g2o进行优化的代码讲解之前, 先分析一下karto的后端的位姿图相关的数据结构.

karto 在 Mapper.h 中定义了后端优化的接口 ScanSolver, 在实现自己的优化的代码时只有对这个接口进行继承并重写就可以了.

ScanSolver的定义如下

/**
   * Graph optimization algorithm
   */classScanSolver{
   
   public:/**
     * Vector of id-pose pairs
     */typedef std::vector<std::pair<kt_int32s, Pose2>> IdPoseVector;public:/**
     * Solve!
     */virtualvoidCompute()=0;/**
     * Adds a node to the solver
     */virtualvoidAddNode(Vertex<LocalizedRangeScan>*/*pVertex*/){
   
   }/**
     * Adds a constraint to the solver
     */virtualvoidAddConstraint(Edge<LocalizedRangeScan>*/*pEdge*/){
   
   }};// ScanSolver

1 节点的数据结构

首先看AddNode函数

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

可知, 这个函数传入的是 karto::Vertex<karto::LocalizedRangeScan> *pVertex 格式的节点类型.

karto::Vertex

接下来, 看一下karto中对Vertex的定义的代码. 如下所示

/**
   * Represents an object in a graph
   */template<typenameT>classVertex{
   
   friendclassEdge<T>;inlineconst std::vector<Edge<T>*>&GetEdges()const{
   
   return m_Edges;}inline T *GetObject()