karto探祕之open_karto 第一章 --- 數據結構與類的初始化

目錄

1 雷達硬件與雷達數據管理

2 掃描匹配相關

3 地圖相關

4 圖優化的圖結構相關

5 初始化

REFERENCES

---

 

本節將簡要介紹open_karto中重要的類及其成員變量。

 

1 雷達硬件與雷達數據管理

1.1 抽象類  Sensor 

這個類爲抽象類，無具體實現，只有一個成員變量

Parameter<Pose2>* 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<Name, ScanManager*> ScanManagerMap;

 

2 掃描匹配相關

2.1 class ScanMatcher

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

    // 更多用來存儲柵格，同時提供world2grid這個功能，在其內部有 GridIndex 方法似乎和 Grid<T>::GridIndex 一樣
    CorrelationGrid* m_pCorrelationGrid;   
    Grid<kt_double>* m_pSearchSpaceProbs;

    // 用來存儲經過不同旋轉之後的nPoints個掃描點應該落在的位置上面
    GridIndexLookup<kt_int8u>* m_pGridLookup; 
  };  // ScanMatcher

2.2 class CorrelationGrid : public Grid<kt_int8u>

用於掃描匹配的相關網格的實現

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

    /**
     * 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<kt_int32s> m_Roi;

  };  // CorrelationGrid

2.3 Grid

  /**
   * Defines a grid class
   */
  template<typename T>
  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<typename T>
  class GridIndexLookup
  {
  private:
    Grid<T>* m_pGrid;

    kt_int32u m_Capacity;
    kt_int32u m_Size;

    LookupArray **m_ppLookupArray;

    // for sanity check
    std::vector<kt_double> 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<MapperListener*> m_Listeners;

    // 以及各個參數
  };


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

  typedef std::queue<LocalizationScanVertex> LocalizationScanVertices;

3.2 class MapperGraph : public Graph

  /**
   * Graph
   */
  template<typename T>
  class Graph
  {

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

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


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

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

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


  /**
   * Graph traversal algorithm
   */
  template<typename T>
  class GraphTraversal
  {
    Graph<T>* m_pGraph;
  };  // GraphTraversal<T>


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

 

3.3 佔用網格

  /**
   * Occupancy grid definition. See GridStates for possible grid values.
   */
  class OccupancyGrid : public Grid<kt_int8u>
  {
    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<kt_int32u>* m_pMinPassThrough;

    // Minimum ratio of beams hitting cell to beams passing through cell for cell to be marked as occupied
    Parameter<kt_double>* 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<typename T>
  class Vertex
  {
    friend class Edge<T>;
  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<T>* pEdge)
    {
      m_Edges.push_back(pEdge);
    }

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

    friend class boost::serialization::access;
    template<class Archive>
    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<T>

4.2 Edge

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


 /**
   * 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<std::pair<kt_int32s, Pose2> > 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<typename T>
  class Vector2
  {
  private:
    T m_Values[2];
  };  // Vector2<T>

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


  /**
   * Defines a position (x, y) in 2-dimensional space and heading.
   */
  class Pose2
  {
  private:
    Vector2<kt_double> 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<kt_int32u>(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<kt_int32u>(ceil(rangeThreshold / resolution));
    //這裏寫出了應該搜索的範圍的grid = 2431
    kt_int32s gridSize = searchSpaceSideSize + 2 * pointReadingMargin;

    // create correlation grid
    assert(gridSize % 2 == 1);
    //CorrelationGrid 和 Grid<kt_double> 都是 Grid<T>的形式，但是這裏面有borderSize
    //作爲x,y的座標，實在是不能理解
    CorrelationGrid* pCorrelationGrid = CorrelationGrid::CreateGrid(gridSize, gridSize, resolution, smearDeviation);

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

    ScanMatcher* pScanMatcher = new ScanMatcher(pMapper);
    pScanMatcher->m_pCorrelationGrid = pCorrelationGrid;
    pScanMatcher->m_pSearchSpaceProbs = pSearchSpaceProbs;
    pScanMatcher->m_pGridLookup = new GridIndexLookup<kt_int8u>(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<kt_int32s>(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<kt_int8u>(width + borderSize * 2, height + borderSize * 2)
      , m_SmearDeviation(smearDeviation)
      , m_pKernel(NULL)
    {
      GetCoordinateConverter()->SetScale(1.0 / resolution);

      // setup region of interest
      m_Roi = Rectangle2<kt_int32s>(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<kt_int32u>(math::Round(z * GridStates_Occupied));
          assert(math::IsUpTo(kernelValue, static_cast<kt_int32u>(255)));

          int kernelArrayIndex = (i + halfKernel) + m_KernelSize * (j + halfKernel);
          m_pKernel[kernelArrayIndex] = static_cast<kt_int8u>(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<LocalizedRangeScan>(this);
  }


  template<typename T>   
  class BreadthFirstTraversal : public GraphTraversal<T>
  {
  public:
    /**
     * Constructs a breadth-first traverser for the given graph
     */
    BreadthFirstTraversal(Graph<T>* pGraph)
    : GraphTraversal<T>(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