AMCL代码详解（六）amcl中的重采样

1.重采样判断

上一章讲述了amcl中如何根据激光观测更新粒子权重，当粒子更新完后amcl会需要根据程序判断是否需要进行重采样。这个判断在粒子观测更新权重后进行判断，代码在amcl_node.cpp中：

    if(!(++resample_count_ % resample_interval_))
    { 
      pf_update_resample(pf_);
      resampled = true;
    }

这里通过两个参数判断是否进行重采样操作，resample_count_是个计数值，每次自加1，resample_interval_是一个常系数参数，可以在.launch文件中设置。这个值默认为1，所以如果不修改的话算法每一次运算都会进行重采样操作。

2.重采样原理

关于amcl中的重采样原理，这里一篇文章讲述的比较通俗易懂:《粒子滤波重采样的理解及MATLAB实现》。粒子滤波中重采样的过程其实就是相当于一个转转盘的过程，如下图所示：

这个转盘被分成了N分，即从w1到wn，和为1。每个区间就是一个粒子的权值，权值越大，区间的弧度就越大。每次都从w1左边的竖线开始转，显而易见，转到w1的概率为0-w1之间的随机数，转到w2的概率为w1到w1+w2之间的随机数，转到w3的概率为w1+w2到w1+w2+w3之间的随机数，后面的依次类推。当某个粒子的权值较大的时候，产生的随机数落在相应区间的概率就会增大，从而实现了对权值大粒子的多次复制，权值小的粒子的剔除。这样重采样的过程中不是全部复制大权值粒子，也有可能对小权值粒子进行了复制，因为区间虽小，但也有可能转到这个区间。不过这样在一定程度上也保证了粒子的多样性。

3.重采样代码实现

amcl中重采样的代码实际是在pf.c中：

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 r,c,U;
  //int m;
  //double count_inv;
  double* c;

  double w_diff;

  set_a = pf->sets + pf->current_set;//current，只有0和1，a和b交替指向sets的第一个和第二个位置，
                                     //每个周期由a生成b，但是a所指向的sets的位置不一样，a在本个周期指向的是上个周期中b的位置
  set_b = pf->sets + (pf->current_set + 1) % 2;//这里是粒子集指针的切换
  //set_a为上周期采样的粒子，set_b为本周期将要采样的粒子
  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.累积概率以进行重采样
  // TODO: Replace this with a more efficient procedure   这里应该有更高效的方法
  // (e.g., http://www.network-theory.co.uk/docs/gslref/GeneralDiscreteDistributions.html)
  // 然后对粒子的权重进行积分，获得分布函数，后面用于生成样本。
  c = (double*)malloc(sizeof(double)*(set_a->sample_count+1));//malloc函数用于分配内存
  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);//为自适应采样创建kd树
  
  // 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;
  // 此时和初始的分布差异较大，即w_idff变大，需要重采样；如果是大部分粒子有较大的权重，差异不大，不进行重采样
  if(w_diff < 0.0)
    w_diff = 0.0;
  //printf("w_diff: %9.6f\n", w_diff);

  // Can't (easily) combine low-variance sampler with KLD adaptive
  // sampling, so we'll take the more traditional route.
  /*
  //不能容易地将低方差采样器与KLD自适应组合采样，所以我们采取更传统的路线。
  // Low-variance resampler, taken from Probabilistic Robotics, p110
  count_inv = 1.0/set_a->sample_count;
  r = drand48() * count_inv;
  c = set_a->samples[0].weight;
  i = 0;
  m = 0;
  */
  while(set_b->sample_count < pf->max_samples)
  {
    //set_b的粒子指针指向set_b的采样数之后的位置，用于添加重采样的粒子
    sample_b = set_b->samples + set_b->sample_count++;
    //0-1之间均匀分布的随机数，按照概率增加，w_diff越大，增加粒子的可能性也越大
    if(drand48() < w_diff)
      sample_b->pose = (pf->random_pose_fn)(pf->random_pose_data);//增加随机分布粒子，重采样
    else
    {
      // Can't (easily) combine low-variance sampler with KLD adaptive
      // sampling, so we'll take the more traditional route.
      /*
      // Low-variance resampler, taken from Probabilistic Robotics, p110
      U = r + m * count_inv;
      while(U>c)
      {
        i++;
        // Handle wrap-around by resetting counters and picking a new random
        // number
        if(i >= set_a->sample_count)
        {
          r = drand48() * count_inv;
          c = set_a->samples[0].weight;
          i = 0;
          m = 0;
          U = r + m * count_inv;
          continue;
        }
        c += set_a->samples[i].weight;
      }
      m++;
      */

      // Naive discrete event sampler  离散采样器
      double r; //生成一个随机数
      r = drand48();
      //判断这个随机数处于哪个粒子周围，这里生成出一个随机数，当一个粒子权重越大，则其在0-1之间占用的比例越大，随机数取到这个粒子占用部分的概率越大
      for(i=0;i<set_a->sample_count;i++)
      {
        if((c[i] <= r) && (r < c[i+1]))//将随机数以权重为概率分配到某处
          break;
      }
      //assert其作用是如果它的条件返回错误,则终止程序执行
      assert(i<set_a->sample_count);

      sample_a = set_a->samples + i;

      assert(sample_a->weight > 0);

      // Add sample to list
      sample_b->pose = sample_a->pose;
      //在一个即生成随机数位置增加一个粒子，某个位置附近粒子数越多或者权重越大，这个位置生成重采样的粒子概率越大
    }

    sample_b->weight = 1.0;//每个粒子都是1，之后标准化
    total += sample_b->weight;

    // Add sample to histogram
    pf_kdtree_insert(set_b->kdtree, sample_b->pose, sample_b->weight);//将样本添加到直方图，关于位姿的二叉树

    // See if we have enough samples yet
    if (set_b->sample_count > pf_resample_limit(pf, set_b->kdtree->leaf_count))
      break;
  }
  
  // Reset averages, to avoid spiraling off into complete randomness.
  if(w_diff > 0.0)//增加粒子集后重置似然
    pf->w_slow = pf->w_fast = 0.0;

  //fprintf(stderr, "\n\n");

  // Normalize weights
  for (i = 0; i < set_b->sample_count; i++)
  {
    sample_b = set_b->samples + i;
    sample_b->weight /= total;//重采样以后每个粒子权重=1 / M
  }
  
  // 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;
}

代码中的：

  c = (double*)malloc(sizeof(double)*(set_a->sample_count+1));//malloc函数用于分配内存
  c[0] = 0.0;
  for(i=0;i<set_a->sample_count;i++)//权重累积
    c[i+1] = c[i]+set_a->samples[i].weight;

代表的就是标题2中的圆盘的产生过程，将每一个点的权重值放到一个对应的数组内，根据数组的值就可以知道当前处于哪个粒子。

然后下面的while循环实际上实现了粒子的重采样过程：

while(set_b->sample_count < pf->max_samples)
  {
    //set_b的粒子指针指向set_b的采样数之后的位置，用于添加重采样的粒子
    sample_b = set_b->samples + set_b->sample_count++;
    //0-1之间均匀分布的随机数，按照概率增加，w_diff越大，增加粒子的可能性也越大
    if(drand48() < w_diff)
      sample_b->pose = (pf->random_pose_fn)(pf->random_pose_data);//增加随机分布粒子，重采样
    else
    {
      // Naive discrete event sampler  离散采样器
      double r; //生成一个随机数
      r = drand48();
      //判断这个随机数处于哪个粒子周围，这里生成出一个随机数，当一个粒子权重越大，则其在0-1之间占用的比例越大，随机数取到这个粒子占用部分的概率越大
      for(i=0;i<set_a->sample_count;i++)
      {
        if((c[i] <= r) && (r < c[i+1]))//将随机数以权重为概率分配到某处
          break;
      }
      //assert其作用是如果它的条件返回错误,则终止程序执行
      assert(i<set_a->sample_count);

      sample_a = set_a->samples + i;

      assert(sample_a->weight > 0);

      // Add sample to list
      sample_b->pose = sample_a->pose;
      //在一个即生成随机数位置增加一个粒子，某个位置附近粒子数越多或者权重越大，这个位置生成重采样的粒子概率越大
    }

    sample_b->weight = 1.0;//每个粒子都是1，之后标准化
    total += sample_b->weight;

    // Add sample to histogram
    pf_kdtree_insert(set_b->kdtree, sample_b->pose, sample_b->weight);//将样本添加到直方图，关于位姿的二叉树

    // See if we have enough samples yet
    if (set_b->sample_count > pf_resample_limit(pf, set_b->kdtree->leaf_count))
      break;
  }
  

首先while循环中的第一个drand48()用于判断是否重采样粒子，这里用到了两个参数：pf->w_fast 与pf->w_slow。这两个参数代表了粒子的衰减率，当：

w_diff = 1.0 - pf->w_fast / pf->w_slow;

得到的值越大，说明粒子的权重越小，当前粒子的可信度比较小，需要重采样粒子。而w_diff的值越小，说明大部分粒子有较大的权重，差异不大，不进行重采样。

首先算法随机生成一个随机数，然后根据该随机数判断是否进行随机粒子生成。对于需要重新生成粒子的情况：

if(drand48() < w_diff)
      sample_b->pose = (pf->random_pose_fn)(pf->random_pose_data);//增加随机分布粒子，重采样

这里的位姿生成使用的是随机位姿生成的方式得到一个之前不存在的位姿，通过这种方式提高粒子的多样性。同时我们也可以看到，w_diff越小，重新生成随机粒子的概率也越小。

而对于不重新生成粒子的情况，这里用到了第二个drand48()，这个drand48()的作用就是前面提到的轮盘，它随机转到某一个权重范围下，然后根据这个权重范围获取对应的位姿。这样子权重大的粒子被复制的概率会高于权重低的粒子。当然这里虽然复制的位姿可能会是一样的，但是没有关系，因为里程计更新时携带了噪声系数，所以一次更新后这些一样的位姿就产生了变化。

最后当粒子的数量满足了一定要求后，整个重采样的过程就结束了。可以看出，在整个重采样的过程中，粒子进行了两次种情况的刷新，一种是随机生成的新的粒子，一种是对原来的粒子的复制。当粒子的权重越低，生成新的粒子的可能性就越大，粒子的权重越高，被复制的可能性也就越大。