karto探秘之open_karto 第一章 --- 数据结构与类的初始化

目录

1 雷达硬件与雷达数据管理

2 扫描匹配相关

3 地图相关

4 图优化的图结构相关

5 初始化

REFERENCES

---

 

本节将简要介绍open_karto中重要的类及其成员变量。

 

1 雷达硬件与雷达数据管理

1.1 抽象类  Sensor 

这个类为抽象类，无具体实现，只有一个成员变量

Parameter* m_pOffsetPose;  // 这个变量定义了雷达坐标系与base_link间的偏差

1.2 class LaserRangeFinder : public Sensor

这个类继承Sensor，定义了激光雷达数据的各个参数，如 最大最小距离，最大最小角度，角度分辨率，一帧雷达的点的个数，以及Sensor中定义的雷达坐标系与base_link间的偏差

1.3 class SensorData : public Object

SensorData是所有传感器数据的基类，定义了如下变量

    /**
     * ID unique to individual sensor
     */
    kt_int32s m_StateId;

    /**
     * ID unique across all sensor data
     */
    kt_int32s m_UniqueId;

    /**
     * Sensor that created this sensor data
     */
    Name m_SensorName;

    /**
     * Time the sensor data was created
     */
    kt_double m_Time;

    CustomDataVector m_CustomData;

1.4 class LaserRangeScan : public SensorData   

存储了最原始的扫描深度数据

    kt_double* m_pRangeReadings;        // 一帧scan的所有距离值，指向数组的指针
    kt_int32u m_NumberOfRangeReadings;  // 一帧scan的点数，也就是数组的个数

1.5 class LocalizedRangeScan : public LaserRangeScan

LocalizedRangeScan包含来自激光测距传感器传感器的单次扫描的二维空间和位置信息中的范围数据。 里程表位置是记录范围数据时机器人报告的位置。 校正后的位置是由映射器（或定位器）计算出的位置

    /**
     * Average of all the point readings
     * 所有点读数的平均值
     */
    Pose2 m_BarycenterPose;

    /**
     * Vector of point readings 
     * 将laser的扫描数据转换为 在世界坐标系中的二维坐标结果，在Update()函数中实现
     */
    PointVectorDouble m_PointReadings;

    /**
     * Vector of unfiltered point readings
     * 去掉雷达数据中nan数值后前的集合
     */
    PointVectorDouble m_UnfilteredPointReadings;

    /**
     * Bounding box of localized range scan
     * 这帧雷达数据的bounding box
     */
    BoundingBox2 m_BoundingBox;

    /**
     * Internal flag used to update point readings, barycenter and bounding box
     * 
     */
    kt_bool m_IsDirty;

1.6 class LocalizedRangeScanWithPoints : public LocalizedRangeScan

LocalizedRangeScanWithPoints是LocalizedRangeScan的扩展，带有预先计算的点。

1.7 class ScanManager

管理设备的扫描数据

m_RunningScans 实时维护的局部激光数据链，首末两帧距离在一定距离范围内，且满足一定数据规模，否则需要删除末端数据帧。维护当前局部数据链

    LocalizedRangeScanVector m_Scans;

    // 短时间内存储的一系列雷达点
    // 存储依据为：1 最初的雷达数据与最新的雷达数据 的frame_link 不超过一定距离，2 数量不超过一定数量
    LocalizedRangeScanVector m_RunningScans;   

    LocalizedRangeScan* m_pLastScan;
    kt_int32u m_RunningBufferMaximumSize;
    kt_double m_RunningBufferMaximumDistance;

1.8 class MapperSensorManager

以不同的名字来管理不同的雷达设备，可以返回指定名字的雷达设备的上一帧的雷达数据

typedef std::map ScanManagerMap;

 

2 扫描匹配相关

2.1 class ScanMatcher

  /**
   * Scan matcher
   */
  class KARTO_EXPORT ScanMatcher
  {
  private:
    Mapper* m_pMapper;

    // 更多用来存储栅格，同时提供world2grid这个功能，在其内部有 GridIndex 方法似乎和 Grid::GridIndex 一样
    CorrelationGrid* m_pCorrelationGrid;   
    Grid* m_pSearchSpaceProbs;

    // 用来存储经过不同旋转之后的nPoints个扫描点应该落在的位置上面
    GridIndexLookup* m_pGridLookup; 
  };  // ScanMatcher

2.2 class CorrelationGrid : public Grid

用于扫描匹配的相关网格的实现

  /**
   * Implementation of a correlation grid used for scan matching
   */
  class CorrelationGrid : public Grid
  {

    /**
     * The point readings are smeared by this value in X and Y to create a smoother response.
     * Default value is 0.03 meters.
     */
    kt_double m_SmearDeviation;

    // Size of one side of the kernel
    kt_int32s m_KernelSize;

    // Cached kernel for smearing
    kt_int8u* m_pKernel;

    // region of interest
    Rectangle2 m_Roi;

  };  // CorrelationGrid

2.3 Grid

  /**
   * Defines a grid class
   */
  template
  class Grid
  {
  public:
    /**
     * Creates a grid of given size and resolution
     * @param width
     * @param height
     * @param resolution
     * @return grid pointer
     */
    static Grid* CreateGrid(kt_int32s width, kt_int32s height, kt_double resolution)
    {
      Grid* pGrid = new Grid(width, height);

      pGrid->GetCoordinateConverter()->SetScale(1.0 / resolution);

      return pGrid;
    }

  private:
    kt_int32s m_Width;       // width of grid
    kt_int32s m_Height;      // height of grid
    kt_int32s m_WidthStep;   // 8 bit aligned width of grid
    T* m_pData;              // grid data

    // coordinate converter to convert between world coordinates and grid coordinates
    CoordinateConverter* m_pCoordinateConverter;
  };  // Grid

2.4 查找表 class GridIndexLookup

/ **
    *创建查找表以在网格中以不同角度读取点。
    *对于每个角度，将为每个范围读数计算网格索引。
    *这是为了加快寻找最佳角度/位置以进行局部范围扫描的速度
    *
    *在Mapper和Localizer中大量使用。
    *
    *在定位器中，这极大地提高了计算可能的位置的速度。 对于每个粒子，计算概率。 范围扫描是相同的，但是计算了所有可能角度的所有栅格索引。 因此，在特定角度计算粒子概率时，将使用索引表在概率网格中查找概率！
    *
    * /
  /**
   * Create lookup tables for point readings at varying angles in grid.
   * For each angle, grid indexes are calculated for each range reading.
   * This is to speed up finding best angle/position for a localized range scan
   *
   * Used heavily in mapper and localizer.
   *
   * In the localizer, this is a huge speed up for calculating possible position.  For each particle,
   * a probability is calculated.  The range scan is the same, but all grid indexes at all possible angles are
   * calculated.  So when calculating the particle probability at a specific angle, the index table is used
   * to look up probability in probability grid!
   *
   */
  template
  class GridIndexLookup
  {
  private:
    Grid* m_pGrid;

    kt_int32u m_Capacity;
    kt_int32u m_Size;

    LookupArray **m_ppLookupArray;

    // for sanity check
    std::vector m_Angles;
  };  // class GridIndexLookup


  /**
   * An array that can be resized as long as the size
   * does not exceed the initial capacity
   */
  class LookupArray
  {
  private:
    kt_int32s* m_pArray;
    kt_int32u m_Capacity;
    kt_int32u m_Size;
  };  // LookupArray

 

3 地图相关

3.1 Mapper

  /**
   * Graph SLAM mapper. Creates a map given a set of LocalizedRangeScans
   * The current Karto implementation is a proprietary, high-performance scan-matching algorithm that constructs a map from individual range scans and corrects for errors in the range and odometry data.
   */

  class KARTO_EXPORT Mapper : public Module
  {
    friend class MapperGraph;
    friend class ScanMatcher;

  protected:
    kt_bool m_Initialized;

    ScanMatcher* m_pSequentialScanMatcher;

    MapperSensorManager* m_pMapperSensorManager;

    MapperGraph* m_pGraph;
    ScanSolver* m_pScanOptimizer;
    LocalizationScanVertices m_LocalizationScanVertices;


    std::vector m_Listeners;

    // 以及各个参数
  };


  struct LocalizationScanVertex
  {
    LocalizationScanVertex(){return;};
    LocalizationScanVertex(const LocalizationScanVertex& obj){scan = obj.scan; vertex = obj.vertex;};
    LocalizedRangeScan* scan;
    Vertex* vertex;
  };

  typedef std::queue LocalizationScanVertices;

3.2 class MapperGraph : public Graph

  /**
   * Graph
   */
  template
  class Graph
  {

  protected:
    /**
     * Map of names to vector of vertices
     */
    VertexMap m_Vertices;

    /**
     * Edges of this graph
     */
    std::vector*> m_Edges;
  };  // Graph


  /**
   * Graph for graph SLAM algorithm
   */
  class KARTO_EXPORT MapperGraph : public Graph
  {
  private:
    /**
     * Mapper of this graph
     */
    Mapper* m_pMapper;

    /**
     * Scan matcher for loop closures
     */
    ScanMatcher* m_pLoopScanMatcher;

    /**
     * Traversal algorithm to find near linked scans
     */
    GraphTraversal* m_pTraversal;
  };  // MapperGraph


  /**
   * Graph traversal algorithm
   */
  template
  class GraphTraversal
  {
    Graph* m_pGraph;
  };  // GraphTraversal


  // 深度优先（DFS）广度优先（BFS）算法可以参考这篇文章（http://www.cnblogs.com/skywang12345/p/3711483.html）
  template   
  class BreadthFirstTraversal : public GraphTraversal
  {
    /**
     * Traverse the graph starting with the given vertex; applies the visitor to visited nodes
     * @param pStartVertex
     * @param pVisitor
     * @return visited vertices
     */
    virtual std::vector Traverse(Vertex* pStartVertex, Visitor* pVisitor)
  }

 

3.3 占用网格

  /**
   * Occupancy grid definition. See GridStates for possible grid values.
   */
  class OccupancyGrid : public Grid
  {
    friend class CellUpdater;
    friend class IncrementalOccupancyGrid;
  private:
    CellUpdater* m_pCellUpdater;

    
    // NOTE: These two values are dependent on the resolution.  If the resolution is too small,
    // then not many beams will hit the cell!

    // Number of beams that must pass through a cell before it will be considered to be occupied
    // or unoccupied.  This prevents stray beams from messing up the map.
    Parameter* m_pMinPassThrough;

    // Minimum ratio of beams hitting cell to beams passing through cell for cell to be marked as occupied
    Parameter* m_pOccupancyThreshold;
  };  // OccupancyGrid


  class KARTO_EXPORT CellUpdater : public Functor
  {
  public:
    CellUpdater(OccupancyGrid* pGrid)
      : m_pOccupancyGrid(pGrid)
    {
    }

    /**
     * Updates the cell at the given index based on the grid's hits and pass counters
     * @param index
     */
    virtual void operator() (kt_int32u index);

  private:
    OccupancyGrid* m_pOccupancyGrid;
  };  // CellUpdater

 

4 图优化的图结构相关

4.1 Vertex

  /**
   * Represents an object in a graph
   */
  template
  class Vertex
  {
    friend class Edge;
  public:
    /**
     * Constructs a vertex representing the given object
     * @param pObject
     */
    Vertex()
      : m_pObject(NULL), m_Score(1.0)
    {
    }
    Vertex(T* pObject)
      : m_pObject(pObject), m_Score(1.0)
    {
    }
  // ...
    /**
     * Adds the given edge to this vertex's edge list
     * @param pEdge edge to add
     */
    inline void AddEdge(Edge* pEdge)
    {
      m_Edges.push_back(pEdge);
    }

    T* m_pObject;
    std::vector*> m_Edges;
    kt_double m_Score;

    friend class boost::serialization::access;
    template
    void serialize(Archive &ar, const unsigned int version)
    {
      ar & BOOST_SERIALIZATION_NVP(m_pObject);
      ar & BOOST_SERIALIZATION_NVP(m_Edges);
      ar & BOOST_SERIALIZATION_NVP(m_Score);
    }
  };  // Vertex

4.2 Edge

   /**
   * Represents an edge in a graph
   */
  template
  class Edge
  {
  private:
    Vertex* m_pSource;
    Vertex* m_pTarget;
    EdgeLabel* m_pLabel;
  };  // class Edge


 /**
   * Class for edge labels
   */
  class EdgeLabel
  {
  public:
    /**
     * Default constructor
     */
    EdgeLabel()
    {
    }

    /**
     * Destructor
     */
    virtual ~EdgeLabel()
    {
    }
  };  // EdgeLabel

  
  // A LinkInfo object contains the requisite information for the "spring"
  // that links two scans together--the pose difference and the uncertainty
  // (represented by a covariance matrix).
  class LinkInfo : public EdgeLabel
  {

  private:
    Pose2 m_Pose1;
    Pose2 m_Pose2;
    Pose2 m_PoseDifference;
    Matrix3 m_Covariance;
  };  // LinkInfo

4.3 SPA优化方法

  /**
   * Graph optimization algorithm
   */
  class ScanSolver
  {
  public:
    /**
     * Vector of id-pose pairs
     */
    typedef std::vector > IdPoseVector;
  }

class SpaSolver : public karto::ScanSolver
{

private:
  karto::ScanSolver::IdPoseVector corrections;

  sba::SysSPA2d m_Spa;
};

 

其他

  /**
   * Represents a vector (x, y) in 2-dimensional real space.
   */
  template
  class Vector2
  {
  private:
    T m_Values[2];
  };  // Vector2

  /**
   * Type declaration of Vector2 vector
   */
  typedef std::vector< Vector2 > PointVectorDouble;


  /**
   * Defines a position (x, y) in 2-dimensional space and heading.
   */
  class Pose2
  {
  private:
    Vector2 m_Position;

    kt_double m_Heading;
  };  // Pose2

  /**
   * Type declaration of Pose2 vector
   */
  typedef std::vector< Pose2 > Pose2Vector;

 

5 初始化

// slam_karto.cpp
int
main(int argc, char** argv)
{
  ros::init(argc, argv, "slam_karto");

  SlamKarto kn;

  ros::spin();

  return 0;
}

class SlamKarto
{
    // Karto bookkeeping
    karto::Mapper* mapper_;
    karto::Dataset* dataset_;
    SpaSolver* solver_;
};

SlamKarto::SlamKarto() :
        got_map_(false),
        laser_count_(0),
        transform_thread_(NULL),
        marker_count_(0)
{

  // Initialize Karto structures
  mapper_ = new karto::Mapper();
  dataset_ = new karto::Dataset();
  solver_ = new SpaSolver();
  mapper_->SetScanSolver(solver_);
}

5.1 Mapper的初始化

  /**
   * Default constructor
   */
  Mapper::Mapper()
    : Module("Mapper")
    , m_Initialized(false)
    , m_pSequentialScanMatcher(NULL)
    , m_pMapperSensorManager(NULL)
    , m_pGraph(NULL)
    , m_pScanOptimizer(NULL)
  {
    InitializeParameters();
  }


void Mapper::Initialize(kt_double rangeThreshold)
  {
    if (m_Initialized == false)
    {
      // create sequential scan and loop matcher
      m_pSequentialScanMatcher = ScanMatcher::Create(this,
                                                    m_pCorrelationSearchSpaceDimension->GetValue(), // 0.3
                                                    m_pCorrelationSearchSpaceResolution->GetValue(), // 0.01
                                                    m_pCorrelationSearchSpaceSmearDeviation->GetValue(),  // 0.03
                                                    rangeThreshold);
      assert(m_pSequentialScanMatcher);
      //m_pScanBufferSize   running_scan 数量长度，单位：个
      //m_pScanBufferMaximumScanDistance  running_scan 地图上的长度，单位：m
      m_pMapperSensorManager = new MapperSensorManager(m_pScanBufferSize->GetValue(),
                                                       m_pScanBufferMaximumScanDistance->GetValue());

      m_pGraph = new MapperGraph(this, rangeThreshold);

      m_Initialized = true;
    }
  }

5.2 scan_match初始化

  ScanMatcher* ScanMatcher::Create(Mapper* pMapper, kt_double searchSize, kt_double resolution,
                                   kt_double smearDeviation, kt_double rangeThreshold)
  {
    // invalid parameters
    if (resolution <= 0)
    {
      return NULL;
    }
    if (searchSize <= 0)
    {
      return NULL;
    }
    if (smearDeviation < 0)
    {
      return NULL;
    }
    if (rangeThreshold <= 0)
    {
      return NULL;
    }

    assert(math::DoubleEqual(math::Round(searchSize / resolution), (searchSize / resolution)));

    // calculate search space in grid coordinates   //searchSize大小是0.3，既然和rangeThreshold有相同单位，就是说匹配范围扩大0.3m，类似于卷积核大小是0.3m
    // 31
    kt_int32u searchSpaceSideSize = static_cast(math::Round(searchSize / resolution) + 1);

    // compute requisite size of correlation grid (pad grid so that scan points can't fall off the grid
    // if a scan is on the border of the search space) 
    // 1200
    kt_int32u pointReadingMargin = static_cast(ceil(rangeThreshold / resolution));
    //这里写出了应该搜索的范围的grid = 2431
    kt_int32s gridSize = searchSpaceSideSize + 2 * pointReadingMargin;

    // create correlation grid
    assert(gridSize % 2 == 1);
    //CorrelationGrid 和 Grid 都是 Grid的形式，但是这里面有borderSize
    //作为x,y的坐标，实在是不能理解
    CorrelationGrid* pCorrelationGrid = CorrelationGrid::CreateGrid(gridSize, gridSize, resolution, smearDeviation);

    // create search space probabilities
    Grid* pSearchSpaceProbs = Grid::CreateGrid(searchSpaceSideSize,
                                                                     searchSpaceSideSize, resolution);

    ScanMatcher* pScanMatcher = new ScanMatcher(pMapper);
    pScanMatcher->m_pCorrelationGrid = pCorrelationGrid;
    pScanMatcher->m_pSearchSpaceProbs = pSearchSpaceProbs;
    pScanMatcher->m_pGridLookup = new GridIndexLookup(pCorrelationGrid);

    return pScanMatcher;
  }

 5.2.1 CorrelationGrid初始化

    /**
     * Create a correlation grid of given size and parameters
     * @param width
     * @param height
     * @param resolution
     * @param smearDeviation
     * @return correlation grid
     */
    static CorrelationGrid* CreateGrid(kt_int32s width,
                                       kt_int32s height,
                                       kt_double resolution,
                                       kt_double smearDeviation)
    {
      assert(resolution != 0.0);

      // +1 in case of roundoff
      // borderSize 的值是7，作用是什么呢
      kt_int32u borderSize = GetHalfKernelSize(smearDeviation, resolution) + 1;  

      CorrelationGrid* pGrid = new CorrelationGrid(width, height, borderSize, resolution, smearDeviation);

      return pGrid;
    }


    /**
     * Computes the kernel half-size based on the smear distance and the grid resolution.
     * Computes to two standard deviations to get 95% region and to reduce aliasing.
     * @param smearDeviation
     * @param resolution
     * @return kernel half-size based on the parameters
     */
    static kt_int32s GetHalfKernelSize(kt_double smearDeviation, kt_double resolution)
    {
      assert(resolution != 0.0);

      return static_cast(math::Round(2.0 * smearDeviation / resolution));
    }


    /**
     * Constructs a correlation grid of given size and parameters
     * @param width
     * @param height
     * @param borderSize
     * @param resolution
     * @param smearDeviation
     */
    CorrelationGrid(kt_int32u width, kt_int32u height, kt_int32u borderSize,
                    kt_double resolution, kt_double smearDeviation)
      : Grid(width + borderSize * 2, height + borderSize * 2)
      , m_SmearDeviation(smearDeviation)
      , m_pKernel(NULL)
    {
      GetCoordinateConverter()->SetScale(1.0 / resolution);

      // setup region of interest
      m_Roi = Rectangle2(borderSize, borderSize, width, height);

      // calculate kernel
      CalculateKernel();
    }


    /**
     * Constructor initializing rectangle parameters
     * @param x x-coordinate of left edge of rectangle
     * @param y y-coordinate of bottom edge of rectangle
     * @param width width of rectangle
     * @param height height of rectangle
     */
    Rectangle2(T x, T y, T width, T height)
      : m_Position(x, y)
      , m_Size(width, height)
    {
    }


    /**
     * Sets up the kernel for grid smearing.
     */
    virtual void CalculateKernel()
    {
      kt_double resolution = GetResolution();

      assert(resolution != 0.0);
      assert(m_SmearDeviation != 0.0);

      // min and max distance deviation for smearing;
      // will smear for two standard deviations, so deviation must be at least 1/2 of the resolution
      const kt_double MIN_SMEAR_DISTANCE_DEVIATION = 0.5 * resolution;
      const kt_double MAX_SMEAR_DISTANCE_DEVIATION = 10 * resolution;

      // check if given value too small or too big
      if (!math::InRange(m_SmearDeviation, MIN_SMEAR_DISTANCE_DEVIATION, MAX_SMEAR_DISTANCE_DEVIATION))
      {
        std::stringstream error;
        error << "Mapper Error:  Smear deviation too small:  Must be between "
              << MIN_SMEAR_DISTANCE_DEVIATION
              << " and "
              << MAX_SMEAR_DISTANCE_DEVIATION;
        throw std::runtime_error(error.str());
      }

      // NOTE:  Currently assumes a two-dimensional kernel

      // +1 for center // = 13
      m_KernelSize = 2 * GetHalfKernelSize(m_SmearDeviation, resolution) + 1;

      // allocate kernel
      m_pKernel = new kt_int8u[m_KernelSize * m_KernelSize];
      if (m_pKernel == NULL)
      {
        throw std::runtime_error("Unable to allocate memory for kernel!");
      }

      // calculate kernel
      kt_int32s halfKernel = m_KernelSize / 2;
      for (kt_int32s i = -halfKernel; i <= halfKernel; i++)
      {
        for (kt_int32s j = -halfKernel; j <= halfKernel; j++)
        {
          #ifdef WIN32
          kt_double distanceFromMean = _hypot(i * resolution, j * resolution);
          #else
          kt_double distanceFromMean = hypot(i * resolution, j * resolution);
          #endif
          kt_double z = exp(-0.5 * pow(distanceFromMean / m_SmearDeviation, 2));

          kt_int32u kernelValue = static_cast(math::Round(z * GridStates_Occupied));
          assert(math::IsUpTo(kernelValue, static_cast(255)));

          int kernelArrayIndex = (i + halfKernel) + m_KernelSize * (j + halfKernel);
          m_pKernel[kernelArrayIndex] = static_cast(kernelValue);
        }
      }
    }

 5.2.2 Grid初始化

    /**
     * Creates a grid of given size and resolution
     * @param width
     * @param height
     * @param resolution
     * @return grid pointer
     */
    static Grid* CreateGrid(kt_int32s width, kt_int32s height, kt_double resolution)
    {
      Grid* pGrid = new Grid(width, height);

      pGrid->GetCoordinateConverter()->SetScale(1.0 / resolution);

      return pGrid;
    }

5.3 MapperSensorManager初始化

  //集中了所有的device的scans，以名字为分隔
  /**
   * Manages the devices for the mapper
   */
    MapperSensorManager(kt_int32u runningBufferMaximumSize, kt_double runningBufferMaximumDistance)
      : m_RunningBufferMaximumSize(runningBufferMaximumSize)
      , m_RunningBufferMaximumDistance(runningBufferMaximumDistance)
      , m_NextScanId(0)
    {
    }

5.4 MapperGraph初始化

  MapperGraph::MapperGraph(Mapper* pMapper, kt_double rangeThreshold)
    : m_pMapper(pMapper)
  {
    m_pLoopScanMatcher = ScanMatcher::Create(pMapper, m_pMapper->m_pLoopSearchSpaceDimension->GetValue(), // 8.0
                                             m_pMapper->m_pLoopSearchSpaceResolution->GetValue(),  // 0.05
                                             m_pMapper->m_pLoopSearchSpaceSmearDeviation->GetValue(), rangeThreshold); // 0.03, 12
    assert(m_pLoopScanMatcher);

    m_pTraversal = new BreadthFirstTraversal(this);
  }


  template   
  class BreadthFirstTraversal : public GraphTraversal
  {
  public:
    /**
     * Constructs a breadth-first traverser for the given graph
     */
    BreadthFirstTraversal(Graph* pGraph)
    : GraphTraversal(pGraph)
    {
    }
  }

 

 

 

REFERENCES

Karto_slam框架与代码解析  https://blog.csdn.net/qq_24893115/article/details/52965410

Karto SLAM之open_karto代码学习笔记（一）  https://blog.csdn.net/wphkadn/article/details/85144186