gmapping 中一些关键定义

/**所有粒子的滤波结构体数组*/

typedef std::vector<Particle> ParticleVector;   
ParticleVector m_particles;

/*
* 粒子滤波器中的粒子结构体  每个粒子有自己的地图、位姿、权重、轨迹
* 轨迹是按照时间顺序排列的，叶子节点表示最近的节点
*/
  

struct Particle{
      //给定两个地图 一个高分辨率地图 一个低分辨率地图
      Particle(const ScanMatcherMap& map);

      inline operator double() const {return weight;}

      inline operator OrientedPoint() const {return pose;}

      inline void setWeight(double w) {weight=w;}
 
      /* The map  地图 高分辨率地图*/
      ScanMatcherMap map;
      /** The pose of the robot 机器人位姿*/
      OrientedPoint pose;

      /*机器人上一帧的位姿 这个位姿是用来计算里程计的位移的*/
      OrientedPoint previousPose;

      /*粒子的权重*/
      double weight;

      /*粒子的累计权重*/
      double weightSum;

      double gweight;

      /*上一个粒子的下标*/
      int previousIndex;

      // 该粒子对应的整条轨迹 记录的是粒子的最近的一个节点
      TNode* node;
    };

       
    TNode 树的定义，在gridslamprocessor.h 中
 /**该类定义存储轨迹的反向树的节点。
树的每个节点都有一个指向父节点的指针和一个指示子节点数目的计数器。
更新树的方式与对粒子执行的操作一致
     //树的节点，一个树储存了一整条轨迹，一个节点表示这条轨迹中的其中一个点。
     //因为FastSLAM是一个full SLAM的方法，因此它需要存储机器人的整条轨迹。
     //轨迹上的一个节点(相当于一帧)存储了一下几个信息：
       机器人的位姿
         该节点粒子的权重
         轨迹前面所有的节点的粒子的权重之和
     指向父节点的指针
     子节点的数量
*/
  

  struct TNode{
      /*作用: 构造轨迹树的一个节点
        参数pose 机器人在轨迹上的姿态
        weight：粒子在轨迹上这一点的重量
        parent: 该点的父节点
        childs: 该点的子节点
      */
      TNode(const OrientedPoint& pose, double weight, TNode* parent=0, unsigned int childs=0);

      /**
      销毁树节点，并始终更新树。如果一个节点的父节点只有一个子节点被删除，那么父节点也会被删除。
      这是因为在轨迹树中父节点不再是可访问的了，它会构造轨迹树的一个节点。
      */
      ~TNode();

      //该节点机器人的位姿
      OrientedPoint pose;
      
      //该节点粒子的权重
      double weight;

      // 轨迹前面所有节点的粒子的权重之和 该节点的子节点之和
      double accWeight;

      double gweight;

      //指向父节点的指针 这里没有指向子节点的指针
      //因此每个粒子的路径都是记录最近的一个点，然后通过parent指针遍历整条路径
      TNode* parent;

      /**The range reading to which this node is associated*/
      // 该节点激光雷达的读数
      const RangeReading* reading;

      // 该节点的子节点的数量
      unsigned int childs;

      /**访问节点的计数器(内部使用)*/
      mutable unsigned int visitCounter;

      /**参观标志(内部使用)*/
      mutable bool flag;
    };

OrientedPoint 的定义在utils/point.h 中
/*带方向的点  即除了位置之外，还有角度，可以理解为机器人的位姿*/
template <class T, class A>
struct orientedpoint: public point<T>{
  //构造函数，初始话点的坐标和位姿角度
  inline orientedpoint() : point<T>(0,0), theta(0) {};

    inline orientedpoint(const point<T>& p);
    
    inline orientedpoint(T x, T y, A _theta): point<T>(x,y), theta(_theta){}
    
        inline void normalize();
        
    inline orientedpoint<T,A> rotate(A alpha){
        T s=sin(alpha), c=cos(alpha);
        A a=alpha+theta;
        a=atan2(sin(a),cos(a));
        return orientedpoint(
            c*this->x-s*this->y,
            s*this->x+c*this->y,
            a);
    }
    A theta;

};

map地图的定义：(在smmap.h中)
//定义最终使用的地图数据类型 cell类型为PointAccumulator
//存储数据类型为HierarchicalArray2D<PointAccumulator>，在grid/harray2d.h中
typedef Map<PointAccumulator,HierarchicalArray2D<PointAccumulator> > ScanMatcherMap;

cell栅格的定义:
/*表示地图中的一个cell*/
struct PointAccumulator{
    typedef point<float> FloatPoint;

    PointAccumulator(): acc(0,0), n(0), visits(0){}
    PointAccumulator(int i): acc(0,0), n(0), visits(0){assert(i==-1);}

        inline void update(bool value, const Point& p=Point(0,0));
    inline Point mean() const {return 1./n*Point(acc.x, acc.y);}
  /*返回被占用的概率*/
    inline operator double() const { return visits?(double)n*SIGHT_INC/(double)visits:-1; }
    inline void add(const PointAccumulator& p) {acc=acc+p.acc; n+=p.n; visits+=p.visits; }
    static const PointAccumulator& Unknown();
    static PointAccumulator* unknown_ptr;
  /*
       * cell的位置的累计值 这里认为每个cell算被击中的时候，有可能不是在同一个位置被击中的
       * 毕竟在查找对应点的时候是在一个kernelSize的窗口里面进行查找的，所以这里累计被击中的坐标值。
       * 而这个cell的真实坐标由这些累计值的均值表示
       */
    FloatPoint acc;
  /*n表示被hit中的次数  visits表示访问的次数*/
    int n, visits;
    inline double entropy() const;
};