amcl 定位代码laserReceived函数学习记录

amcl代码中laserReceived函数是处理雷达接收数据的回调函数，因为amcl依靠雷达测量出来的距离数据进行定位，所以定位的代码主要在该函数中。

结构体pf_t是amcl储存样本点，依据雷达数据对点的权重进行处理以及根据权重确定当前位置所依赖的数据结构。

typedef struct _pf_t
{ // This min and max number of samples 
  int min_samples, max_samples;
  // Population size parameters
  double pop_err, pop_z;
  // Resample limit cache
  int *limit_cache;
  // The sample sets.  We keep two sets and use [current_set] to identify the active set.
  int current_set;
  pf_sample_set_t sets[2];
  // Running averages, slow and fast, of likelihood
  double w_slow, w_fast;
  // Decay rates for running averages
  double alpha_slow, alpha_fast;
  // Function used to draw random pose samples
  pf_init_model_fn_t random_pose_fn;
  void *random_pose_data;
  double dist_threshold; //distance threshold in each axis over which the pf is considered to not be converged
  int converged; 
  // boolean parameter to enamble/diable selective resampling
  int selective_resampling;
} pf_t;

min_samples, max_samples 是样本数量的最大最小值；
pop_err, pop_z 是用于确定所需样本数量的参数；
sets是两个储存样本点的结构体，由current_set标示当前使用的是哪一个样本；
w_slow, w_fast 记录了样本点未经规范化权重的平均值，会影响到重新生成样本时所加入的新的点的数量；分别受到alpha_slow, alpha_fast 的值的影响；
random_pose_fn 重新生成样本时会使用该函数去生成新加入的点；amcl中使用的函数会在整个地图随机生成新点；
random_pose_data 用于储存地图数据

结构体pf_sample_set_t是储存样本点的数据结构。

typedef struct _pf_sample_set_t
{// The samples
  int sample_count;
  pf_sample_t *samples;
  // A kdtree encoding the histogram
  pf_kdtree_t *kdtree;
  // Clusters
  int cluster_count, cluster_max_count;
  pf_cluster_t *clusters;
  // Filter statistics
  pf_vector_t mean;
  pf_matrix_t cov;
  int converged; 
  double n_effective;
} pf_sample_set_t;

samples 是样本点数组，pf_sample_t包含了一个三维向量代表点的位置（x，y坐标以及方向）以及一个double值代表权重；
sample_count 记录了当前的样本数量；
kdtree 是一颗二叉树，结点是三维的，用以根据位置划分样本点。kdtree的叶子代表着0.5x0.5范围每10度范围内的样本点。
clusters 数组中的每一个元素代表着位置相邻的一组点。amcl会选取其中权重最大的一组的平均位置作为定位的位置。

laserReceived函数接受到数据后，先判断当前移动（旋转）的位移；如果超过设定值就更新样本点的位置和权重信息。pose是当前雷达测量发生的位置，pf_odom_pose_是上次测量发生的位置。

// Compute change in pose
delta.v[0] = pose.v[0] - pf_odom_pose_.v[0];
delta.v[1] = pose.v[1] - pf_odom_pose_.v[1];
delta.v[2] = angle_diff(pose.v[2], pf_odom_pose_.v[2]);
// See if we should update the filter
bool update = fabs(delta.v[0]) > d_thresh_ || fabs(delta.v[1]) > d_thresh_ || fabs(delta.v[2]) > a_thresh_;

首先是UpdateAction函数更新点的位置，有四个更新的模型，其中ODOM_MODEL_DIFF_CORRECTED的代码如下。
其中delta是当前位置和上一个位置的差值。
假设上一个位置是(x1,y1)，方向是α1；现位置是(x2,y2)，方向是α2；两位置之间的位移距离为s，方向是α3。
则delta.v[2]=α2-α1；delta_rot1=α3-α1；delta_trans=s；delta_rot2=α2-α3。
pf_ran_gaussian是一个生成随机数的函数，这些随机数服从以0为平均值，传入参数为方差的正态分布。
delta_rot1_hat，delta_trans_hat，delta_rot2_hat分别是delta_rot1，delta_trans，delta_rot2加上这些随机数之后的值。
最后将delta_rot1_hat，delta_trans_hat，delta_rot2_hat加到样本点的位置中得到新的位置，模拟里程计存在的误差情况。

 case ODOM_MODEL_DIFF_CORRECTED:
  {// Implement sample_motion_odometry (Prob Rob p 136)
    double delta_rot1, delta_trans, delta_rot2;
    double delta_rot1_hat, delta_trans_hat, delta_rot2_hat;
    double delta_rot1_noise, delta_rot2_noise;
    // Avoid computing a bearing from two poses that are extremely near each other (happens on in-place rotation).
    if(sqrt(ndata->delta.v[1]*ndata->delta.v[1] + ndata->delta.v[0]*ndata->delta.v[0]) < 0.01)
      delta_rot1 = 0.0;
    else  
      delta_rot1 = angle_diff(atan2(ndata->delta.v[1], ndata->delta.v[0]), old_pose.v[2]);//前一位置方向和坐标位移方向的夹角α3-α1
    delta_trans = sqrt(ndata->delta.v[0]*ndata->delta.v[0] + ndata->delta.v[1]*ndata->delta.v[1]);//坐标位移距离s
    delta_rot2 = angle_diff(ndata->delta.v[2], delta_rot1);//现位置方向和坐标位移方向的夹角(α2-α1)-(α3-α1)=α2-α3
    delta_rot1_noise = std::min(fabs(angle_diff(delta_rot1,0.0)), fabs(angle_diff(delta_rot1,M_PI)));
    delta_rot2_noise = std::min(fabs(angle_diff(delta_rot2,0.0)), fabs(angle_diff(delta_rot2,M_PI)));
    for (int i = 0; i < set->sample_count; i++)
    { pf_sample_t* sample = set->samples + i;
      // Sample pose differences
      delta_rot1_hat = angle_diff(delta_rot1,
                                  pf_ran_gaussian(sqrt(this->alpha1*delta_rot1_noise*delta_rot1_noise +
                                                       this->alpha2*delta_trans*delta_trans)));
      delta_trans_hat = delta_trans - 
              pf_ran_gaussian(sqrt(this->alpha3*delta_trans*delta_trans +
                                   this->alpha4*delta_rot1_noise*delta_rot1_noise +
                                   this->alpha4*delta_rot2_noise*delta_rot2_noise));
      delta_rot2_hat = angle_diff(delta_rot2,
                                  pf_ran_gaussian(sqrt(this->alpha1*delta_rot2_noise*delta_rot2_noise +
                                                       this->alpha2*delta_trans*delta_trans)));
      // Apply sampled update to particle pose
      sample->pose.v[0] += delta_trans_hat * cos(sample->pose.v[2] + delta_rot1_hat);//x1'+s'*cos(α1'+(α3-α1)')
      sample->pose.v[1] += delta_trans_hat * sin(sample->pose.v[2] + delta_rot1_hat);//y1'+s'*sin(α1'+(α3-α1)')
      sample->pose.v[2] += delta_rot1_hat + delta_rot2_hat;//α1' + (α3-α1)' + (α2-α3)'
    }
  }

然后是UpdateSensor函数根据雷达数据更新点的权重。权重是点根据之前所有雷达数据确定的一个概率。
有三个更新的模型，其中LikelihoodFieldModel的代码如下。
该模型根据雷达数据分两部分计算样本点是机器人位置的可能性。
一部分是雷达观测的误差模型，雷达测得的距离与实际的距离的差符合平均数为0的正态分布。根据样本点位置与测量数据的距离和方向计算出测量终点的位置，如果位置未超出地图范围则取终点与最近的障碍物的距离根据正态分布计算概率，否则取最小的可能的概率。
另一部分是由于其他未知情况导致测量值可能是最大测量距离内随机值的概率，与最大测量距离成反比。
雷达能一次测出一个角度范围内的数据，因此每一个样本点都会计算测量范围内各个角度的概率并相加得到点的总的概率并叠加到之前的权重之上得到当前雷达数据下的权重。（代码将计算出的概率的立方作为该方向的概率；并且从1起相加；最后乘到之前权重之上。暂未学懂为什么要这样处理）
函数会记录并返回一个总的权重，用以后续处理每个点的权重，使得点的权重总和为1。

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);
    // 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;
      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
        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;
      assert(pz <= 1.0);
      assert(pz >= 0.0);
      p += pz*pz*pz;
    }
    sample->weight *= p;
    total_weight += sample->weight;
  }
  return(total_weight);
}

利用雷达数据更新样本2次后，laserReceived函数会重新生成样本。
首先会根据w_fast和w_slow确定一个0和1之间的值w_diff。然后生成0到1之间的随机数，小于w_diff就使用random_pose_fn重新生成样本点，否则就从原来的样本点中取点，点的权重越大被选取的可能性也就越大。
从旧样本中取点具体是通过数组c按顺序累加每个点的权重（已经标准化使和为1），c[i]记录前i个点的权重和，则c[i]-c[i-1]为第i个点的权重。随机生成0到1的数，数落在c[j-1]到c[j]之间则取第j个点，j的权重越大则生成的随机数落在c[j-1]到c[j]的概率越大，j被选取的概率越大。从而权重较大的点的比例会不断提高。

void pf_update_resample(pf_t *pf)
{ int i;
  double total;
  pf_sample_set_t *set_a, *set_b;
  pf_sample_t *sample_a, *sample_b;
  double* c;
  double w_diff;
  set_a = pf->sets + pf->current_set;
  set_b = pf->sets + (pf->current_set + 1) % 2;
  if (pf->selective_resampling != 0)
  { if (set_a->n_effective > 0.5*(set_a->sample_count))
    { // copy set a to b
      copy_set(set_a,set_b);
      // Re-compute cluster statistics
      pf_cluster_stats(pf, set_b);
      // Use the newly created sample set
      pf->current_set = (pf->current_set + 1) % 2;
      return;
    }
  }
  // Build up cumulative probability table for resampling.
  c = (double*)malloc(sizeof(double)*(set_a->sample_count+1));
  c[0] = 0.0;
  for(i=0;i<set_a->sample_count;i++)
    c[i+1] = c[i]+set_a->samples[i].weight;
  // Create the kd tree for adaptive sampling
  pf_kdtree_clear(set_b->kdtree);  
  // Draw samples from set a to create set b.
  total = 0;
  set_b->sample_count = 0;
  w_diff = 1.0 - pf->w_fast / pf->w_slow;
  if(w_diff < 0.0)
    w_diff = 0.0;
  while(set_b->sample_count < pf->max_samples)
  { sample_b = set_b->samples + set_b->sample_count++;
    if(drand48() < w_diff)
      sample_b->pose = (pf->random_pose_fn)(pf->random_pose_data);
    else
    { // Naive discrete event sampler
      double r;
      r = drand48();
      for(i=0;i<set_a->sample_count;i++)
      { if((c[i] <= r) && (r < c[i+1]))
          break;
      }
      sample_a = set_a->samples + i;
      // Add sample to list
      sample_b->pose = sample_a->pose;
    }
    sample_b->weight = 1.0;
    total += sample_b->weight;
    // Add sample to histogram
    pf_kdtree_insert(set_b->kdtree, sample_b->pose, sample_b->weight);//构造kdtree
    // See if we have enough samples yet
    if (set_b->sample_count > pf_resample_limit(pf, set_b->kdtree->leaf_count))//根据kdtree的叶子数确定所需的样本点数量
      break;
  }  
  // Reset averages, to avoid spiraling off into complete randomness.
  if(w_diff > 0.0)
    pf->w_slow = pf->w_fast = 0.0;
  // Normalize weights
  for (i = 0; i < set_b->sample_count; i++)
  { sample_b = set_b->samples + i;
    sample_b->weight /= total;
  } 
  // Re-compute cluster statistics
  pf_cluster_stats(pf, set_b);
  // Use the newly created sample set
  pf->current_set = (pf->current_set + 1) % 2; 
  pf_update_converged(pf);
  free(c);
  return;
}

生成点的同时会将其插入到样本集的kdtree进行重新构造（一开始已经用pf_kdtree_clear函数将树清空）。
pf_kdtree_insert中根据位置和方向划分样本点，一定范围内一定角度内的点被划分作为树的一个叶子，叶子的权重是所划分的点的总权重，从而根据kdtree的叶子数量可判断样本的聚集程度。函数会根据旧样本的kdtree的叶子数量确定新样本所需的点数量，从而在样本点较为聚集时压缩点的数量，提高计算效率。

void pf_kdtree_insert(pf_kdtree_t *self, pf_vector_t pose, double value)
{ int key[3];
  key[0] = floor(pose.v[0] / self->size[0]);//self->size[0] = 0.50;
  key[1] = floor(pose.v[1] / self->size[1]);//self->size[1] = 0.50;
  key[2] = floor(pose.v[2] / self->size[2]);//self->size[2] = (10 * M_PI / 180);
  //根据上面划分的key插入到kdtree中。
  //与叶节点比较时，如果key相等则将权重value加到该叶节点；
  //否则，将相差最大的维度及该维度下两个节点的平均值记录到该叶节点中，
  //然后根据该维度下两节点key值大小将两节点插入为该叶节点的左右子树；
  //与非叶节点比较时，取该节点所记录维度的插入节点的key值与该节点记录的平均值进行比较，
  //从而确定插入节点应递归插入左子树还是右子树
  self->root = pf_kdtree_insert_node(self, NULL, self->root, key, value);//递归插入
  return;
}

在重新生成样本点之后，pf_cluster_stats函数会将样本中相邻的点划分为一个cluster，并计算出点的总权重及加权的位置平均值记录到cluster中。
函数先使用pf_kdtree_cluster对ketree中的叶子进行标记，其中的pf_kdtree_cluster_node函数会递归地将位置相邻的叶子标记为同一个值。

// Recursively label nodes in this cluster
void pf_kdtree_cluster_node(pf_kdtree_t *self, pf_kdtree_node_t *node, int depth)
{ int i;
  int nkey[3];
  pf_kdtree_node_t *nnode;
  for (i = 0; i < 3 * 3 * 3; i++)
  {nkey[0] = node->key[0] + (i / 9) - 1;
    nkey[1] = node->key[1] + ((i % 9) / 3) - 1;
    nkey[2] = node->key[2] + ((i % 9) % 3) - 1;//与当前key三个值相差1的所有key
    nnode = pf_kdtree_find_node(self, self->root, nkey);//检索是否有相应key值的叶子；构造kdtree可以提高检索的效率
    if (nnode == NULL) continue;
    if (nnode->cluster >= 0) continue;
    // Label this node and recurse
    nnode->cluster = node->cluster;
    pf_kdtree_cluster_node(self, nnode, depth + 1);//递归标记
  }
  return;
}

标记完kdtree之后，遍历所有点，查询点对应叶子的标记，累计权重和加权位置到相应的cluster中，并在最后算出点的平均位置。

void pf_cluster_stats(pf_t *pf, pf_sample_set_t *set)
{ 
  ......
  // Cluster the samples
  pf_kdtree_cluster(set->kdtree);
  ......
  // Compute cluster stats
  for (i = 0; i < set->sample_count; i++)
  { sample = set->samples + i;
    cidx = pf_kdtree_get_cluster(set->kdtree, sample->pose);//获取点所对应叶子的标记
    if (cidx >= set->cluster_max_count) continue;
    if (cidx + 1 > set->cluster_count) set->cluster_count = cidx + 1;
    cluster = set->clusters + cidx;//标记相同的叶子所对应的cluster
    cluster->count += 1;
    cluster->weight += sample->weight;//累计点的权重
    // Compute mean
    cluster->m[0] += sample->weight * sample->pose.v[0];
    cluster->m[1] += sample->weight * sample->pose.v[1];
    cluster->m[2] += sample->weight * cos(sample->pose.v[2]);
    cluster->m[3] += sample->weight * sin(sample->pose.v[2]);//累计点的加权位置
  ......
  }
  // Normalize
  for (i = 0; i < set->cluster_count; i++)
  { cluster = set->clusters + i;
    cluster->mean.v[0] = cluster->m[0] / cluster->weight;
    cluster->mean.v[1] = cluster->m[1] / cluster->weight;
    cluster->mean.v[2] = atan2(cluster->m[3], cluster->m[2]);//计算出平均位置
    ......
 }
 ......
  return;
}

在重新生成样本之后，laserReceived会选出权重最大的cluster的位置平均值作为定位的位置进行发布。

if(resampled || force_publication)
{ // Read out the current hypotheses
  double max_weight = 0.0;
  int max_weight_hyp = -1;
  std::vector<amcl_hyp_t> hyps;
  hyps.resize(pf_->sets[pf_->current_set].cluster_count);
  for(int hyp_count = 0; hyp_count < pf_->sets[pf_->current_set].cluster_count; hyp_count++)
  { double weight;
    pf_vector_t pose_mean;
    pf_matrix_t pose_cov;
    if (!pf_get_cluster_stats(pf_, hyp_count, &weight, &pose_mean, &pose_cov)) break;
    hyps[hyp_count].weight = weight;
    hyps[hyp_count].pf_pose_mean = pose_mean;
    hyps[hyp_count].pf_pose_cov = pose_cov;
    if(hyps[hyp_count].weight > max_weight)
    {
      max_weight = hyps[hyp_count].weight;
      max_weight_hyp = hyp_count;//记录权重最大的cluster
    }
  }
  if(max_weight > 0.0)
  { ROS_DEBUG("Max weight pose: %.3f %.3f %.3f",
                hyps[max_weight_hyp].pf_pose_mean.v[0],
                hyps[max_weight_hyp].pf_pose_mean.v[1],
                hyps[max_weight_hyp].pf_pose_mean.v[2]);

amcl初始化样本时是根据初始位置利用正态分布生成点的，样本点会分布到初始位置的一定范围内，当初始位置与实际位置相差较远时，会导致定位不到实际的位置。这时需要请求amcl的global_localization服务，该服务会将样本点随机分布在地图范围内，从而可以快速地定位到实际的位置。