gmapping源码分析2——粒子滤波

第一： 粒子定义

/**所有粒子的滤波结构体数组*/
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;
        };

第二：粒子的初始化

//一.先定义了gmapping slam 算法指针变量
GMapping::GridSlamProcessor* gsp_; 

gsp_ = new GMapping::GridSlamProcessor();

/* 二.对gmapping slam 算法初始化
   参数:particles_默认是30,配置是50
  *xmin_, ymin_, xmax_, ymax_分别是-1,-1,1,1
  * delta_ 是地图分辨率，0.05
  * initialPose 是此时里程计坐标和方向
  */
gsp_->GridSlamProcessor::init(particles_, xmin_, ymin_, xmax_, ymax_,
                                delta_, initialPose);


/*
   三. GridFastSLAM初始化
  5.主要是用来初始化各个粒子的一些信息。,
    参数:size(particles_)默认是30,配置是50
  *xmin_, ymin_, xmax_, ymax_分别是-1,-1,1,1
  * delta_ 是地图分辨率，0.05
  * initialPose 是此时里程计坐标和方向
  */
  void GridSlamProcessor::init(unsigned int size, double xmin, double ymin, double xmax, double ymax, double delta, OrientedPoint initialPose)
{
  
  /*设置每个粒子的初始值*/
    m_particles.clear();
    //TNode 树父节点
    TNode* node=new TNode(initialPose, 0, 0, 0);
    //粒子对应的地图进行初始化 用两个地图来进行初始化 一个高分辨率地图 一个低分辨率地图
    //高分辨率地图由自己指定 低分辨率地图固定为0.1m
    ScanMatcherMap lmap(Point(xmin+xmax, ymin+ymax)*.5, xmax-xmin, ymax-ymin, delta);
    //每一个粒子保存一张地图？一开始粒子的位置都相同
    for (unsigned int i=0; i<size; i++){
      m_particles.push_back(Particle(lmap));
      m_particles.back().pose=initialPose;
      m_particles.back().previousPose=initialPose;
      m_particles.back().setWeight(0);
      m_particles.back().previousIndex=0;

		m_particles.back().node= node;
    }
    //此时m_neff 为50，粒子个数
    m_neff=(double)size;  
}  

第三：粒子经过里程计运动采样，位置发生差异性变化

   bool GridSlamProcessor::processScan(const RangeReading & reading, int adaptParticles)
{
    for (ParticleVector::iterator it=m_particles.begin(); it!=m_particles.end(); it++){

      OrientedPoint& pose(it->pose);
      //参数:粒子估计的最优位置(机器人上一个时刻的最优位置),表示里程计算出来的新的位置,表示里程计算出来的旧的位置(即上一个里程计的位置)
      /*it->pose 是粒子的位置
        relPose 为现在时刻粒子位置
        m_odoPose 为上一时刻粒子位置
      */
      pose=m_motionModel.drawFromMotion(it->pose, relPose, m_odoPose);
    }   
}

第四：计算粒子更精准的位置，和权重，累计权重等

inline void GridSlamProcessor::scanMatch(const double* plainReading)
{
    double sumScore=0;
  /*每个粒子都要进行scan-match*/
  //用openMP的方式来进行并行化
  //并行话之后会把里面的循环均匀的分到各个不同的核里面去
  for (ParticleVector::iterator it=m_particles.begin(); it!=m_particles.end(); it++){
    OrientedPoint corrected;
    double score, l, s;
    /*进行scan-match 计算粒子的最优位姿 调用scanmatcher.cpp里面的函数 --这是gmapping本来的做法*/
    score=m_matcher.optimize(corrected, it->map, it->pose, plainReading);

    /*矫正成功则更新位姿*/
    if (score>m_minimumScore){
      it->pose=corrected;
    }
    //粒子的最优位姿计算了之后，重新计算粒子的权重(相当于粒子滤波器中的观测步骤，计算p(z|x,m))，粒子的权重由粒子的似然来表示。
       /*
        * 计算粒子的得分和权重(似然)   注意粒子的权重经过ScanMatch之后已经更新了
        * 在论文中 例子的权重不是用最有位姿的似然值来表示的。
        * 是用所有的似然值的和来表示的。
        */
    m_matcher.likelihoodAndScore(s, l, it->map, it->pose, plainReading);
    sumScore+=score;
    it->weight+=l;
    it->weightSum+=l;

    /*计算出来最优的位姿之后，进行地图的扩充  这里不会进行内存分配
         *不进行内存分配的原因是这些粒子进行重采样之后有可能会消失掉，因此在后面进行冲采样的时候统一进行内存分配。
         *理论上来说，这里的操作是没有必要的，因为后面的重采样的时候还会进行一遍
     */
    m_matcher.invalidateActiveArea();
    m_matcher.computeActiveArea(it->map, it->pose, plainReading);
  }
}

第五：对粒子进行重采样，并更新粒子权重

/*
由于scanMatch中对粒子的权重进行了更新，那么这个时候各个粒子的轨迹上的累计权重都需要重新计算
这个函数即更新各个粒子的轨迹上的累计权重是更新
GridSlamProcessor::updateTreeWeights(bool weightsAlreadyNormalized) 函数在gridslamprocessor_tree.cpp里面实现
*/
updateTreeWeights(false);


/*
* 粒子重采样  根据neff的大小来进行重采样  不但进行了重采样，也对地图进行更新
* GridSlamProcessor::resample 函数在gridslamprocessor.hxx里面实现
*/
 	resample(plainReading, adaptParticles, reading_copy);



void  GridSlamProcessor::updateTreeWeights(bool weightsAlreadyNormalized){

  if (!weightsAlreadyNormalized) {
    //这里应该是跳到gridslamprocessor.hxx 中进行粒子归一化计算
    normalize();
  }
  resetTree();
  propagateWeights();
}	

/*
@desc 把粒子的权重归一化
主要功能为归一化粒子的权重，同时计算出neff
std::vector<double> m_weights;
*/
inline void GridSlamProcessor::normalize(){

  double gain=1./(m_obsSigmaGain*m_particles.size());
  /*求所有粒子中的最大的权重*/
  double lmax= -std::numeric_limits<double>::max();
  for (ParticleVector::iterator it=m_particles.begin(); it!=m_particles.end(); it++){
    lmax=it->weight>lmax?it->weight:lmax;
  }

  /*权重以最大权重为中心的高斯分布 ???*/
  m_weights.clear();
  double wcum=0;
  m_neff=0;
  for (std::vector<Particle>::iterator it=m_particles.begin(); it!=m_particles.end(); it++){
    m_weights.push_back(exp(gain*(it->weight-lmax)));
    wcum+=m_weights.back();

  }
  /*
  计算有效粒子数 和 归一化权重
  权重=wi/w
  neff = 1/w*w
  */
  m_neff=0;
  for (std::vector<double>::iterator it=m_weights.begin(); it!=m_weights.end(); it++){
    *it=*it/wcum;
    double w=*it;
    m_neff+=w*w;
  }
  m_neff=1./m_neff;
}

/*把所有粒子的所有的轨迹中各个节点的accWeight清零*/
void GridSlamProcessor::resetTree(){
  // don't calls this function directly, use updateTreeWeights(..) !

	for (ParticleVector::iterator it=m_particles.begin(); it!=m_particles.end(); it++){
		TNode* n=it->node;
		while (n){
			n->accWeight=0;
			n->visitCounter=0;
			n=n->parent;
		}
	}
}


/*
这个函数被updateTreeWeights()调用
*/
double GridSlamProcessor::propagateWeights(){
  // don't calls this function directly, use updateTreeWeights(..) !

        // all nodes must be resetted to zero and weights normalized

        // the accumulated weight of the root
  // 求所有根节点的累计权重之和
	double lastNodeWeight=0;
	// sum of the weights in the leafs
  // 所有叶子节点的权重 也就是m_weights数组里面所有元素的和
	double aw=0;                   

	std::vector<double>::iterator w=m_weights.begin();
	for (ParticleVector::iterator it=m_particles.begin(); it!=m_particles.end(); it++){
    //求解所有叶子节点的累计权重
		double weight=*w;
		aw+=weight;
    //叶子节点的子节点累计权重就等于自己的权重 因为它没有子节点
    //每一个粒子的路径都是从叶子节点开始的，得到了叶子节点，就得到了路径
		TNode * n=it->node;
		n->accWeight=weight;
		lastNodeWeight+=propagateWeight(n->parent,n->accWeight);
		w++;
	}
	
	if (fabs(aw-1.0) > 0.0001 || fabs(lastNodeWeight-1.0) > 0.0001) {
	  cerr << "ERROR: ";
	  cerr << "root->accWeight=" << lastNodeWeight << "    sum_leaf_weights=" << aw << endl;
	  assert(0);         
	}
	return lastNodeWeight;
}	


/*
* 粒子重采样  根据neff的大小来进行重采样  不但进行了重采样，也对地图进行更新
* GridSlamProcessor::resample 函数在gridslamprocessor.hxx里面实现
*/
resample(plainReading, adaptParticles, reading_copy);
 	
template <class Particle, class Numeric>
struct uniform_resampler{
	std::vector<unsigned int> resampleIndexes(const std::vector<Particle> & particles, int nparticles=0) const;
	std::vector<Particle> resample(const std::vector<Particle> & particles, int nparticles=0) const;
	Numeric neff(const std::vector<Particle> & particles) const;
};

template <class Particle, class Numeric>
std::vector<unsigned int> uniform_resampler<Particle, Numeric>:: resampleIndexes(const std::vector<Particle>& particles, int nparticles) const{
	Numeric cweight=0;

  //particles 是以权重最大粒子为中心的新的高斯分布，计算累积权值
	unsigned int n=0;
	for (typename std::vector<Particle>::const_iterator it=particles.begin(); it!=particles.end(); ++it){
		cweight+=(Numeric)*it;
		n++;
	}
  //nparticles 是0
	if (nparticles>0)
		n=nparticles;
	
	//compute the interval
	Numeric interval=cweight/n;

	//compute the initial target weight
	Numeric target=interval*::drand48();
	//compute the resampled indexes

     /*根据权值进行采样*/
  //采用度盘轮转算法，target每次加interval。如果某个粒子的权重比较大的话，那么他肯定会被采样到很多次。
  //比如说如果某个粒子的区间为4*interval。那么它至少被采样3次
	cweight=0;
	std::vector<unsigned int> indexes(n);
	n=0;
	unsigned int i=0;
	for (typename std::vector<Particle>::const_iterator it=particles.begin(); it!=particles.end(); ++it, ++i){
		cweight+=(Numeric)* it;
		while(cweight>target){
			indexes[n++]=i;
			target+=interval;
		}
	}
	return indexes;
}

第五：最后是取累计权重值最大的粒子来描述地图，取该粒子在各个时刻保存的地图，拼接起来
 

 /*
  得到累计权重最大的粒子的下标
  注意不是当前的最大权重的粒子，是累计权重最大的粒子
  累计权重即该粒子在各个时刻的权重之和(轨迹上的各个节点的权重之和)
  */
  int GridSlamProcessor::getBestParticleIndex() const{
    unsigned int bi=0;
    double bw=-std::numeric_limits<double>::max();
    for (unsigned int i=0; i<m_particles.size(); i++)
      if (bw<m_particles[i].weightSum){
    bw=m_particles[i].weightSum;
    bi=i;
      }
    return (int) bi;
  }
 
/*
更新地图:这里的更新地图，只是为了可视化。因为真正的地图都存储在粒子里面。
这里会拿一个权值最大的粒子的地图发布出来.

得到权值最大的粒子，然后遍历这个粒子的整个轨迹，根据轨迹上记录的信息来进行建图
然后把得到的地图发布出去
这个函数被laserCallback()调用，每次addScan()成功了，就会调用这个函数来生成地图，并发布出去

*/
void
SlamGMapping::updateMap(const sensor_msgs::LaserScan& scan)
{
  boost::mutex::scoped_lock map_lock (map_mutex_);
  GMapping::ScanMatcher matcher;
  /*设置scanmatcher的各个参数*/
  matcher.setLaserParameters(scan.ranges.size(), &(laser_angles_[0]),
                             gsp_laser_->getPose());

  matcher.setlaserMaxRange(maxRange_);
  matcher.setusableRange(maxUrange_);
  matcher.setgenerateMap(true);

  /*得到权值最高的粒子*/
  GMapping::GridSlamProcessor::Particle best =
          gsp_->getParticles()[gsp_->getBestParticleIndex()];

  /*地图的中点*/
  GMapping::Point center;
  center.x=(xmin_ + xmax_) / 2.0;
  center.y=(ymin_ + ymax_) / 2.0;
  /*初始化一个scanmatcherMap 创建一个地图*/
  GMapping::ScanMatcherMap smap(center, xmin_, ymin_, xmax_, ymax_,
                                delta_);
  /*更新地图*/
  //遍历粒子的整条轨迹 按照轨迹上各个节点存储的信息来重新计算一个地图
  for(GMapping::GridSlamProcessor::TNode* n = best.node;
      n;
      n = n->parent)
  {
    //进行地图更新
    matcher.invalidateActiveArea();
    matcher.computeActiveArea(smap, n->pose, &((*n->reading)[0]));
    matcher.registerScan(smap, n->pose, &((*n->reading)[0]));
  }
 
//根据地图的信息计算出来各个点的情况:occ、free、noinformation
//这样对地图进行标记主要是方便用RVIZ显示出来
  for(int x=0; x < smap.getMapSizeX(); x++)
  {
    for(int y=0; y < smap.getMapSizeY(); y++)
    {
      // 得到.xy被占用的概率
      GMapping::IntPoint p(x, y);
      double occ=smap.cell(p);
      assert(occ <= 1.0);
      //unknown
      if(occ < 0)
        map_.map.data[MAP_IDX(map_.map.info.width, x, y)] = -1;
      //占用
      else if(occ > occ_thresh_)
      {
        map_.map.data[MAP_IDX(map_.map.info.width, x, y)] = 100;
      }
      //freespace
      else
        map_.map.data[MAP_IDX(map_.map.info.width, x, y)] = 0;
    }
  }
  //把计算出来的地图发布出去
  map_.map.header.stamp = ros::Time::now();
  map_.map.header.frame_id = tf_.resolve( map_frame_ );

  sst_.publish(map_.map);
  sstm_.publish(map_.map.info);
}