AMCL中似然场模型相关代码

首先，我们要先了解四类测量误差：小的测量噪声、意外对象引起的误差，以及由于未检测到对象引起的误差和随机意外噪声。

a.小的测量噪声
即使传感器正确测量了最近对象的距离，它返回的值也受到误差的影响。该误差由测距传感器的有限分辨率、大气 对测量信号的影响等引起，呈高斯分布

b.意外对象引起的误差这种情况下测量距离的概率用指数分布来描述，这里不多讲解，因为似然域未用到。
c.由于未检测到对象引起的误差（检测失败）。
声呐传感器遇到镜面反射/激光测距时检测到黑色吸光对象/激光系统在强光下测量时会发生检测失败，但最典型的是最大距离测量问题：传感器返回它的最大允许值Z_max。

d.随机意外噪声

LikelihoodFieldModel
该模型的作用是：遍历粒子更新概率，计算所有粒子概率的总和。

double AMCLLaser::LikelihoodFieldModel(AMCLLaserData *data, pf_sample_set_t* set)
{
  AMCLLaser *self;
  int i, j, step;
  double z, pz;
  double p;
  double obs_range, obs_bearing;
  double total_weight;
  pf_sample_t *sample;
  pf_vector_t pose;
  pf_vector_t hit;

  self = (AMCLLaser*) data->sensor;

  total_weight = 0.0;

  // 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);//激光雷达的位姿转换到世界坐标系 

    p = 1.0;

    // 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;//无法解释的随机测量的分母

    step = (data->range_count - 1) / (self->max_beams - 1);//计算步长 只取一组数据中的max_beams个点

    // Step size must be at least 1
    if(step < 1)
      step = 1;

    for (i = 0; i < data->range_count; i += step)
    {
      obs_range = data->ranges[i][0];//观测到的距离 Z(k,t）
      obs_bearing = data->ranges[i][1];//θ_k,sense

      // This model ignores max range readings
	  //如果obs_range > data->range_max 则结束跳过以下更新（即：如果测距传感器输出了最大值z(k,t)= Z_max，则这些坐标在物理世界没有任何意义
	  //似然域测量模型简单地将大于最大距离的读数丢弃）
      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)) //Test to see if the given map coords lie within the absolute map bounds.
        z = self->map->max_occ_dist;
      else
        z = self->map->cells[MAP_INDEX(self->map,mi,mj)].occ_dist;
      // Gaussian model
      // NOTE: this should have a normalization of 1/(sqrt(2pi)*sigma)
      pz += self->z_hit * exp(-(z * z) / z_hit_denom);
      // Part 2: random measurements
      pz += self->z_rand * z_rand_mult;

      // TODO: outlier rejection for short readings

      assert(pz <= 1.0);
      assert(pz >= 0.0);
      //      p *= pz;
      // here we have an ad-hoc weighting scheme for combining beam probs
      // works well, though...
      p += pz*pz*pz;
    } 

    sample->weight *= p;
    total_weight += sample->weight;
  }

  return(total_weight);
}

对于最后的几行代码，在程序6.3中第7行 q 即p

 p += pz*pz*pz;

 sample->weight *= p;
 total_weight += sample->weight;

也有人在ros wiki上提了问 并未得到回答 以及在开源项目下也有人提出对应的是否经过成熟测试的质疑
laser model
质疑

LikelihoodFieldModelProb

double AMCLLaser::LikelihoodFieldModelProb(AMCLLaserData *data, pf_sample_set_t* 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;

  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表示是否需要重新分配需要被跳过的临时数据。
  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;
    
    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;//用于判断这个方向的点是否用来计算概率和
	}
	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);

到此为止计算基本一样，只是加了部分的beamskip和reallocate。
其中reallocTempData函数用于：

void AMCLLaser::reallocTempData(int new_max_samples, int new_max_obs){
  if(temp_obs){
    for(int k=0; k < max_samples; k++){
      delete [] temp_obs[k];
    }
    delete []temp_obs; 
  }
  max_obs = new_max_obs; 
  max_samples = fmax(max_samples, new_max_samples); 

  temp_obs = new double*[max_samples]();
  for(int k=0; k < max_samples; k++){
    temp_obs[k] = new double[max_obs]();
  }
}

如果do_beamskip的话，用self->temp_obs[j][beam_ind] 保存概率。

      // TODO: outlier rejection for short readings
            
      if(!do_beamskip){
	log_p += log(pz);
      }
      else{
	self->temp_obs[j][beam_ind] = pz; 
      }
    }
    if(!do_beamskip){
      sample->weight *= exp(log_p);
      total_weight += sample->weight;
    }
  }
  

if(!do_beamskip) 则计算的 sample->weight total_weight 和书上一样 注意此部分计算还是在j < set->sample_count的循环中。而do_beamskip的话，概率的计算如下：

  if(do_beamskip)
  {
    int skipped_beam_count = 0; 
 //某个角度下z < beam_skip_distance的点的个数/帧数>beam_skip_threshold时obs_mask[beam_ind] = true;
 //某个角度下z < beam_skip_distance的点的个数/帧数<beam_skip_threshold时obs_mask[beam_ind] = false，该点不用于计算概率和，但是如果这样的点多了，说明收敛到了一个错误的位姿，则所有的点都用来计算概率和
    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;
	pose = sample->pose;

	log_p = 0;

   //如果z < beam_skip_distance或者收敛到一个错误位姿 则被用于计算概率和
	for (beam_ind = 0; beam_ind < self->max_beams; beam_ind++){
	  if(error || obs_mask[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);
} 

待解决的问题：
prob模型是检查某一个的角度的异常值，那么速度快/转弯时的检测应该是有问题的吧
另外对以下代码也有疑惑

// Update the cspace distance values
void map_update_cspace(map_t *map, double max_occ_dist)
{
  unsigned char* marked;
  std::priority_queue<CellData> Q;

  marked = new unsigned char[map->size_x*map->size_y];
  memset(marked, 0, sizeof(unsigned char) * map->size_x*map->size_y);

  map->max_occ_dist = max_occ_dist;

  CachedDistanceMap* cdm = get_distance_map(map->scale, map->max_occ_dist);

  // Enqueue all the obstacle cells
  CellData cell;
  cell.map_ = map;
  for(int i=0; i<map->size_x; i++)
  {
    cell.src_i_ = cell.i_ = i;
    for(int j=0; j<map->size_y; j++)
    {
      if(map->cells[MAP_INDEX(map, i, j)].occ_state == +1)
      {
	map->cells[MAP_INDEX(map, i, j)].occ_dist = 0.0;
	cell.src_j_ = cell.j_ = j;
	marked[MAP_INDEX(map, i, j)] = 1;
	Q.push(cell);
      }
      else
	map->cells[MAP_INDEX(map, i, j)].occ_dist = max_occ_dist;
    }
  }

  while(!Q.empty())
  {
    CellData current_cell = Q.top();
    if(current_cell.i_ > 0)
      enqueue(map, current_cell.i_-1, current_cell.j_, 
	      current_cell.src_i_, current_cell.src_j_,
	      Q, cdm, marked);
    if(current_cell.j_ > 0)
      enqueue(map, current_cell.i_, current_cell.j_-1, 
	      current_cell.src_i_, current_cell.src_j_,
	      Q, cdm, marked);
    if((int)current_cell.i_ < map->size_x - 1)
      enqueue(map, current_cell.i_+1, current_cell.j_, 
	      current_cell.src_i_, current_cell.src_j_,
	      Q, cdm, marked);
    if((int)current_cell.j_ < map->size_y - 1)
      enqueue(map, current_cell.i_, current_cell.j_+1, 
	      current_cell.src_i_, current_cell.src_j_,
	      Q, cdm, marked);

    Q.pop();
  }

  delete[] marked;
}