AMCL (1) --似然场模型LikelihoodFieldModelProb (最详细)

LikelihoodFieldModelProb

函数说明:

该函数主要是针对动态障碍物概率去除,选通过判断每个粒子相同方向的激光点,在地图内与障碍物的最近距离,是否小于一定距离阈值,并且超过一定数量阈值, 就不进行计算权重,如果在符合范围内就权重叠加.

double AMCLLaser::LikelihoodFieldModelProb(AMCLLaserData *data, pf_sample_set_t* set)//指针set指向第一个地址的粒子
{
  AMCLLaser *self;//类对象
  int i, j, step;//点云序号,粒子序号,一个区域有多少个点
  double z, pz;//障碍物距离,概率值
  double log_p; //概率对数值
  double obs_range, obs_bearing;//数据距离,和角度
  double total_weight;//总权重
  pf_sample_t *sample;//单粒子
  pf_vector_t pose;//机器人位姿->雷达世界坐标
  pf_vector_t hit;//世界坐标
  // AMCLLaserData : public AMCLSensorData:public  AMCLSensor
// AMCLLaser : public AMCLSensor    
// 最后都是在创建对象AMCLSensor
  self = (AMCLLaser*) data->sensor;
// 初始化权重
  total_weight = 0.0;
//算要分成多少个区域
  step = ceil((data->range_count) / static_cast<double>(self->max_beams)); 
  // Step size must be at least 1
  //至少分一个区域
  if(step < 1)
    step = 1;
  // Pre-compute a couple of things
  // 预算噪声协方差和随机概率
  double z_hit_denom = 2 * self->sigma_hit * self->sigma_hit;
  double z_rand_mult = 1.0/data->range_max;
高斯分布模型,概率分布,取最大距离的概率
  double max_dist_prob = exp(-(self->map->max_occ_dist * self->map->max_occ_dist) / z_hit_denom);

  //Beam skipping - ignores beams for which a majoirty of particles do not agree with the map
  //prevents correct particles from getting down weighted because of unexpected obstacles 
  //such as humans 
//光束跳过-忽略大多数粒子与贴图不一致的光束
//防止正确的粒子由于意外的障碍物而降低权重
//比如人类
  bool do_beamskip = self->do_beamskip;//如果需要进行去除动态障碍运算
  double beam_skip_distance = self->beam_skip_distance;//障碍物激光点栅格坐标与地图障碍最近距离距离阈值
  double beam_skip_threshold = self->beam_skip_threshold;//光束跳过阈值
  
  //we only do beam skipping if the filter has converged 
  //如果粒子群收殓就,就不进行设置跳过光束,就不进行去除动态障碍物
  if(do_beamskip && !set->converged){
    do_beamskip = false;
  }

  //we need a count the no of particles for which the beam agreed with the map 我们需要计算出光束与地图一致的粒子数
  int *obs_count = new int[self->max_beams]();//区域内粒子数量

  //we also need a mask of which observations to integrate (to decide which beams to integrate to all particles) 
  // 我们还需要一个obs_mask来合并观测结果决定哪些光束要整合到所有粒子上
  bool *obs_mask = new bool[self->max_beams]();
  
  int beam_ind = 0;//一桢数据中采样 点云序号
  
  //realloc indicates if we need to reallocate the temp data structure needed to do beamskipping 
// realloc(重新分配)表示是否需要重新分配执行beamskipping所需的temp数据结构
  bool realloc = false; 

  if(do_beamskip){
    // 观测数量小于
    if(self->max_obs < self->max_beams){
      realloc = true;
    }
    //如果粒子群粒子粒子数量大于粒子群设定最大值,重新分配
    if(self->max_samples < set->sample_count){
      realloc = true;
    }
// 重新分配
    if(realloc){
      self->reallocTempData(set->sample_count, self->max_beams);     
      fprintf(stderr, "Reallocing temp weights %d - %d\n", self->max_samples, self->max_obs);
    }
  }

  // Compute the sample weights
  // 计算粒子权重
  for (j = 0; j < set->sample_count; j++)
  {
    sample = set->samples + j;//粒子序号
    pose = sample->pose;      //获取机器人位姿

    // Take account of the laser pose relative to the robot
    //结合机器人相对雷达的坐标,求出雷达在世界的坐标
    pose = pf_vector_coord_add(self->laser_pose, pose);

    log_p = 0;
    
    beam_ind = 0;
    // beam_ind 检测点序号,每个区域为step个粒子
    for (i = 0; i < data->range_count; i += step, beam_ind++)
    {
      obs_range = data->ranges[i][0];
      obs_bearing = data->ranges[i][1];

      // This model ignores max range readings,点云距离合理判断
      if(obs_range >= data->range_max){
        continue;
      }

      // Check for NaN角度数值合理判断
      if(obs_range != obs_range){
        continue;
      }

      pz = 0.0;

      // Compute the endpoint of the beam激光点云结合雷达坐标求出点云在世界的坐标
      hit.v[0] = pose.v[0] + obs_range * cos(pose.v[2] + obs_bearing);
      hit.v[1] = pose.v[1] + obs_range * sin(pose.v[2] + obs_bearing);

      // Convert to map grid coords.世界转栅格坐标
      int mi, mj;
      mi = MAP_GXWX(self->map, hit.v[0]);
      mj = MAP_GYWY(self->map, hit.v[1]);
      
      // Part 1: Get distance from the hit to closest obstacle.
      // Off-map penalized as max distance
      
      if(!MAP_VALID(self->map, mi, mj)){
      	  pz += self->z_hit * max_dist_prob;//如果超出地图范围,根据最大距离概率,计算概率值
        }
      else{
           z = self->map->cells[MAP_INDEX(self->map,mi,mj)].occ_dist;
           //否则求出该激光点离障碍物最短的距离.可以理解为当前束激光测的距离和地图匹配的距离做差值
          //  但是它实际是在激光点在附近每个方向都进行搜索最近的障碍的距离,取最小值

           if(z < beam_skip_distance ){//激光点与地图障碍距离小于阈值
	           obs_count[beam_ind] += 1;//一帧内第 beam_ind 个点云,与地图一致的粒子数量obs_count加一
	           }
	         pz += self->z_hit * exp(-(z * z) / z_hit_denom);//障碍权重乘以高斯概率模型
          }
       
      // Gaussian model
      // NOTE: this should have a normalization of 1/(sqrt(2pi)*sigma)
      
      // Part 2: random measurements
      pz += self->z_rand * z_rand_mult;//随机权重乘以随机概率模型

      assert(pz <= 1.0); 
      assert(pz >= 0.0);

      // TODO: outlier rejection for short readings
      // 如果不跳过,就算他的概率对数,并求和各个粒子的
      // 不做跳变进行独立处理，不做跳变时权重正常相加
      if(!do_beamskip){
    	log_p += log(pz);
      }
      else{
     	self->temp_obs[j][beam_ind] = pz; //第j个粒子的第beam_ind点云 的距离值
      }
    }
    if(!do_beamskip){
      sample->weight *= exp(log_p);//更新粒子权重
      total_weight += sample->weight;//权重求和
    }
  }
  // 做跳变时，如果跳变的个数占总个数一定权重时(即大于光束跳过阈值)，设定障碍物遮挡
  if(do_beamskip){
    int skipped_beam_count = 0; 
    for (beam_ind = 0; beam_ind < self->max_beams; beam_ind++){
      if((obs_count[beam_ind] / static_cast<double>(set->sample_count)) > beam_skip_threshold){
        	obs_mask[beam_ind] = true;
      }
      else{
        // 否则不进行遮挡
        	obs_mask[beam_ind] = false;
        	skipped_beam_count++; 
      }
    }

    //we check if there is at least a critical number of beams that agreed with the map 
    //otherwise it probably indicates that the filter converged to a wrong solution
    //if that's the case we integrate all the beams and hope the filter might converge to 
    //the right solution
    bool error = false; 
   //跳过的数量 大于 无效波束比率的阈值-则定位收敛出错
    //如果skipped_beam_count/beam_ind >= beam_skip_error_threshold 则收敛到一个错误的位姿
	//beam_ind为一桢数据中采样个数
    if(skipped_beam_count >= (beam_ind * self->beam_skip_error_threshold)){
      fprintf(stderr, "Over %f%% of the observations were not in the map - pf may have converged to wrong pose - integrating all observations\n", (100 * self->beam_skip_error_threshold));
      error = true; 
    }

    for (j = 0; j < set->sample_count; j++)
    {
	    sample = set->samples + j;//由set指向第一个粒子的地址开始
	    pose = sample->pose;

	    log_p = 0;

    	for (beam_ind = 0; beam_ind < self->max_beams; beam_ind++){
	      if(error || obs_mask[beam_ind]){//收敛错误或者第j个粒子的第beam_ind点云不是是遮挡光束则被用于计算概率和
	       log_p += log(self->temp_obs[j][beam_ind]);
	      }
	    } 
	
    	sample->weight *= exp(log_p);
	
    	total_weight += sample->weight;//权重总和
    }      
  }

  delete [] obs_count; 
  delete [] obs_mask;
  return(total_weight);
}