AMCL (2) --似然场模型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
    //self->laser_pose 进行pose选择和变换 ,使得激光点变换到世界坐标
    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;// 随机测量相关，1/雷达最大距离


    step = (data->range_count - 1) / (self->max_beams - 1);//间隔()默认30个点读取一个点的数量

    // 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];//距离
      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;
      //由世界坐标转换为栅格map坐标
      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))
        z = self->map->max_occ_dist;
      else
      // 求到障碍物的最小距离，MAP_INDEX：二维转一维序号
        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;
      //z 越大()距离越远，pz 越小，粒子权重越小
    }//一个粒子 
    sample->weight *= p;
    total_weight += sample->weight;
    // 每个粒子总权重
  }

  return(total_weight);
}