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

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

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

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

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

typedef std::map<Name, ScanManager*> ScanManagerMap;

  /*** 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

  /*** 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 kernelkt_int32s m_KernelSize;// Cached kernel for smearingkt_int8u* m_pKernel;// region of interestRectangle2<kt_int32s> m_Roi;};  // CorrelationGrid

  /*** 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 gridkt_int32s m_Height;      // height of gridkt_int32s m_WidthStep;   // 8 bit aligned width of gridT* m_pData;              // grid data// coordinate converter to convert between world coordinates and grid coordinatesCoordinateConverter* m_pCoordinateConverter;};  // Grid

/ ***创建查找表以在网格中以不同角度读取点。*对于每个角度，将为每个范围读数计算网格索引。*这是为了加快寻找最佳角度/位置以进行局部范围扫描的速度**在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 checkstd::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

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

  /*** 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)}

  /*** 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 occupiedParameter<kt_double>* m_pOccupancyThreshold;};  // OccupancyGridclass 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

  /*** 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>

   /*** 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

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

// slam_karto.cpp
int
main(int argc, char** argv)
{ros::init(argc, argv, "slam_karto");SlamKarto kn;ros::spin();return 0;
}class SlamKarto
{// Karto bookkeepingkarto::Mapper* mapper_;karto::Dataset* dataset_;SpaSolver* solver_;
};SlamKarto::SlamKarto() :got_map_(false),laser_count_(0),transform_thread_(NULL),marker_count_(0)
{// Initialize Karto structuresmapper_ = new karto::Mapper();dataset_ = new karto::Dataset();solver_ = new SpaSolver();mapper_->SetScanSolver(solver_);
}

  /*** 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 matcherm_pSequentialScanMatcher = ScanMatcher::Create(this,m_pCorrelationSearchSpaceDimension->GetValue(), // 0.3m_pCorrelationSearchSpaceResolution->GetValue(), // 0.01m_pCorrelationSearchSpaceSmearDeviation->GetValue(),  // 0.03rangeThreshold);assert(m_pSequentialScanMatcher);//m_pScanBufferSize   running_scan 数量长度，单位：个//m_pScanBufferMaximumScanDistance  running_scan 地图上的长度，单位：mm_pMapperSensorManager = new MapperSensorManager(m_pScanBufferSize->GetValue(),m_pScanBufferMaximumScanDistance->GetValue());m_pGraph = new MapperGraph(this, rangeThreshold);m_Initialized = true;}}

  ScanMatcher* ScanMatcher::Create(Mapper* pMapper, kt_double searchSize, kt_double resolution,kt_double smearDeviation, kt_double rangeThreshold){// invalid parametersif (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// 31kt_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) // 1200kt_int32u pointReadingMargin = static_cast<kt_int32u>(ceil(rangeThreshold / resolution));//这里写出了应该搜索的范围的grid = 2431kt_int32s gridSize = searchSpaceSideSize + 2 * pointReadingMargin;// create correlation gridassert(gridSize % 2 == 1);//CorrelationGrid 和 Grid<kt_double> 都是 Grid<T>的形式，但是这里面有borderSize//作为x,y的坐标，实在是不能理解CorrelationGrid* pCorrelationGrid = CorrelationGrid::CreateGrid(gridSize, gridSize, resolution, smearDeviation);// create search space probabilitiesGrid<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;}

    /*** 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 interestm_Roi = Rectangle2<kt_int32s>(borderSize, borderSize, width, height);// calculate kernelCalculateKernel();}/*** 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 resolutionconst 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 bigif (!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 // = 13m_KernelSize = 2 * GetHalfKernelSize(m_SmearDeviation, resolution) + 1;// allocate kernelm_pKernel = new kt_int8u[m_KernelSize * m_KernelSize];if (m_pKernel == NULL){throw std::runtime_error("Unable to allocate memory for kernel!");}// calculate kernelkt_int32s halfKernel = m_KernelSize / 2;for (kt_int32s i = -halfKernel; i <= halfKernel; i++){for (kt_int32s j = -halfKernel; j <= halfKernel; j++){#ifdef WIN32kt_double distanceFromMean = _hypot(i * resolution, j * resolution);#elsekt_double distanceFromMean = hypot(i * resolution, j * resolution);#endifkt_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);}}}

    /*** 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;}

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

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