标签：set 粒子 sample cluster AMCL pf samples 源码

 

上一讲介绍了粒子滤波器模型的相关理论以及pf.cpp中的几个关键函数，这一讲我们将对pf.cpp的代码进行详细分析。

先看pf.cpp引用的关键头文件，我们稍后再梳理这些头文件，现在先将pf.cpp的脉络梳理清楚。

#include "amcl/pf/pf.h"
#include "amcl/pf/pf_pdf.h"
#include "amcl/pf/pf_kdtree.h"  

在计算所需的粒子sample的数目的时候，这里使用KLD算法精髓里的直方图概念来表达，选用的是粒子sample集分布在直方图的k个bin里。

图1.直方图表示

//pf粒子滤波器是粒子sample集的来源，k是至少填充了一个粒子的直方图的位数
static int pf_resample_limit(pf_t *pf, int k);

1.创建一个粒子滤波器

创建一个粒子滤波器，输入为我们关心的粒子sample集的最小数目，最大数目， αslow ， αfast ，粒子滤波器初始化模型，随机生成数据的函数。

// Create a new filter
pf_t *pf_alloc(int min_samples, int max_samples,
               double alpha_slow, double alpha_fast,
               pf_init_model_fn_t random_pose_fn, void *random_pose_data)

在函数内部对粒子滤波器,粒子sample集，粒子sample进行定义声明初始化。

{  
  int i, j;
  pf_t *pf;
  //粒子sample集
  pf_sample_set_t *set;
  //粒子sample
  pf_sample_t *sample;
  
  srand48(time(NULL));
  //根据粒子滤波器的类型创建pf
  pf = calloc(1, sizeof(pf_t));
  //对粒子滤波器的随机位姿生成模型进行初始化
  pf->random_pose_fn = random_pose_fn;
  pf->random_pose_data = random_pose_data;
  //粒子sample集的最小数目
  pf->min_samples = min_samples;
  //粒子sample集的最大数目
  pf->max_samples = max_samples;
  ...
}

接下来用到的几个参数:pop_err,pop_z，dist_threshold均与KLD算法有关。其中pop_error代表两个概率分布差的最大测度，pop_z用于确定样本数，基于参数sigma,代表标准正态分布上的1-sigma分位点。对典型值sigma，pop_z（1-sigma）的值在标准统计表里是现成的。dist_threshold为阈值。

//pop_error代表两个概率分布差的最大测度（真实分布与估计分布）
//[err] is the max error between the true distribution and the estimated
distribution.
pf->pop_err = 0.01;

//pop_z代表标准正态分布上的1-sigma分位点
//[z] is the upper standard normal quantile for (1 - p), where p is
the probability that the error on the estimated distrubition will be 
less than [err].
pf->pop_z = 3;
pf->dist_threshold = 0.5; 

叶子节点的数目不能高过粒子样本集的最大数目：

  // 叶子节点的数目不能高过粒子样本集的最大数目
  pf->limit_cache = calloc(max_samples, sizeof(int));

关于粒子滤波器的两个粒子sample集：current_set和set

 //将pf->current_set设置为0
 pf->current_set = 0;

 //使用一个for循环，遍历两次：从而刷新两个set
 for (j = 0; j < 2; j++)
  {
    //set是指针，这里使用j依次读取pf->sets的元素set
    set = pf->sets + j;
    //设置粒子集set的sample集最大数目
    set->sample_count = max_samples;
    //给粒子sample集set以粒子sample集的最大数目分配内存
    set->samples = calloc(max_samples, sizeof(pf_sample_t));
    
    //这里使用一个for循环遍历该粒子sample集set,从而读取到每一个sample对象
    for (i = 0; i < set->sample_count; i++)
    {
      //sample是指针，这里使用i依次读取set->samples的元素sample
      sample = set->samples + i;
      //对粒子sample的位姿和权重进行初始化
      sample->pose.v[0] = 0.0;
      sample->pose.v[1] = 0.0;
      sample->pose.v[2] = 0.0;
      sample->weight = 1.0 / max_samples;
    }

    // HACK: is 3 times max_samples enough?
    //粒子sample集set的kdtree数据结构，用三倍于粒子sample集的最大数目来分配内存：
    //实际上这里需要计算复杂度，很显然代码的作者并没有计算，而是简单粗暴分配三倍内存
    set->kdtree = pf_kdtree_alloc(3 * max_samples);
    //对粒子sample集set的簇cluster进行初始化
    set->cluster_count = 0;
    set->cluster_max_count = max_samples;
    //给粒子sample集set的簇cluster分配内存
    set->clusters = calloc(set->cluster_max_count, sizeof(pf_cluster_t));
    //对粒子sample集set的均值和协方差进行初始化
    set->mean = pf_vector_zero();
    set->cov = pf_matrix_zero();
  }

还有几个参数需要初始化，它们是 wslow , wfast , αslow , αfast 。

  pf->w_slow = 0.0;
  pf->w_fast = 0.0;

  pf->alpha_slow = alpha_slow;
  pf->alpha_fast = alpha_fast;

最后，将粒子sample集set收敛于0。

//set converged to 0
 pf_init_converged(pf);

至此，就完成了粒子滤波器的创建。

2.销毁一个粒子滤波器

关于释放一个粒子滤波器的内存，这里用到一个for循环，遍历两次，其实就是把粒子滤波器的两个粒子sample集set给依次释放内存了。而在粒子sample集set内部，先后将粒子sample集set的簇cluster占据的内存，粒子sample集set所使用的数据结构kdtree占据的内存，粒子sample集set的sample对象占据的内存释放，这就完成了一个粒子滤波器的销毁。

// Free an existing filter
void pf_free(pf_t *pf)
{
  int i;

  free(pf->limit_cache);
 //因为有两个set
  for (i = 0; i < 2; i++)
  {
    free(pf->sets[i].clusters);
    pf_kdtree_free(pf->sets[i].kdtree);
    free(pf->sets[i].samples);
  }
  free(pf);
  return;
}

3.初始化粒子滤波器

这里用了两种方法来初始化粒子滤波器，第一种方法是使用高斯模型(均值，协方差)来初始化滤波器，还有一种方法是使用模型来初始化。

先看第1种方法：

 // 使用高斯模型（均值，协方差）初始化滤波器
void pf_init(pf_t *pf, pf_vector_t mean, pf_matrix_t cov)
{
  int i;
  //创建粒子sample集set,粒子sample对象，概率密度函数pdf
  pf_sample_set_t *set;
  pf_sample_t *sample;
  pf_pdf_gaussian_t *pdf;
  
  //初始化粒子sample集set
  set = pf->sets + pf->current_set;

  //创建kdtree为了选择性采样
  // Create the kd tree for adaptive sampling 
  pf_kdtree_clear(set->kdtree);

  //粒子sample集set的sample数目设定
  set->sample_count = pf->max_samples;
  
  //使用高斯模型(均值，协方差)创建pdf
  pdf = pf_pdf_gaussian_alloc(mean, cov);
  
  //计算新的粒子sample对象的位姿
  //使用for循环遍历粒子sample集set的每个sample对象
  for (i = 0; i < set->sample_count; i++)
  {
    sample = set->samples + i;
    //对粒子sample的权重进行初始化
    sample->weight = 1.0 / pf->max_samples;
    //对粒子sample的位姿使用pdf模型产生
    sample->pose = pf_pdf_gaussian_sample(pdf);
    
    //以kdtree数据结构的方式添加/存储粒子sample 或者说以kdtree数据结构的方式表达直方图
    // Add sample to histogram(kdtree)
    pf_kdtree_insert(set->kdtree, sample->pose, sample->weight);
  }
  //对 w_{slow}  , w_{fast}  进行设置
  pf->w_slow = pf->w_fast = 0.0;
  
  //销毁释放pdf占用的内存
  pf_pdf_gaussian_free(pdf);
  
  //再次计算粒子sample集set的簇cluster的统计特性，输入为粒子滤波器pf,和set  
  // Re-compute cluster statistics
  pf_cluster_stats(pf, set); 
  
  //粒子sample集set收敛到0
  //set converged to 0
  pf_init_converged(pf);

  return;
}

然后是第2种方法：

//使用pf_init_model_fn_t模型初始化
void pf_init_model(pf_t *pf, pf_init_model_fn_t init_fn, void *init_data)
{
  int i;
  //创建粒子sample集set,粒子sample对象
  pf_sample_set_t *set;
  pf_sample_t *sample;

  //初始化粒子sample集set
  set = pf->sets + pf->current_set;

  // Create the kd tree for adaptive sampling 创建kdtree为了选择性采样
  pf_kdtree_clear(set->kdtree);

  set->sample_count = pf->max_samples;

  //计算新的粒子sample对象的位姿
  //使用for循环遍历粒子sample集set的每个sample对象
  // Compute the new sample poses
  for (i = 0; i < set->sample_count; i++)
  {
    sample = set->samples + i;
    sample->weight = 1.0 / pf->max_samples;
    //看这里！生成粒子位姿的方式不一样，不一样！
    //对粒子sample的位姿使用pf_init_model_fn_t模型生成
    sample->pose = (*init_fn) (init_data);

    //以kdtree数据结构的方式添加/存储粒子sample 或者说以kdtree数据结构的方式表达直方图
    // Add sample to histogram(kdtree)
    pf_kdtree_insert(set->kdtree, sample->pose, sample->weight);
  }
  //下面的都跟上面第1种方法一样啦，一样啦
  pf->w_slow = pf->w_fast = 0.0;

  // Re-compute cluster statistics
  pf_cluster_stats(pf, set);
  
  //set converged to 0
  pf_init_converged(pf);

  return;
}

4.粒子滤波器收敛

首先是粒子滤波器初始化收敛：

void pf_init_converged(pf_t *pf){
  pf_sample_set_t *set;
  set = pf->sets + pf->current_set;
 //就是这么简单，将converged标志符改成0
  set->converged = 0; 
  pf->converged = 0; 
}

其次是粒子滤波器更新收敛： 这里的关键在于dist_threshold,粒子的位姿pose在x,y方向上的值分别减去平均值mean_x,mean_y,得到的差值与dist_threshold比较，如果差值大于dist_threshold那就显示没收敛，粒子还处于发散的状态中。

int pf_update_converged(pf_t *pf)
{
  int i;
  //创建粒子sample集set,粒子sample对象
  pf_sample_set_t *set;
  pf_sample_t *sample;

  //初始化粒子sample集set
  set = pf->sets + pf->current_set;
  //初始化men_x,mean_y
  double mean_x = 0, mean_y = 0;

  //计算新的粒子sample对象的位姿
  //使用for循环遍历粒子sample集set的每个sample对象
  for (i = 0; i < set->sample_count; i++){
    sample = set->samples + i;
    //计算粒子sample对象位姿pose的x,y方向上的和
    mean_x += sample->pose.v[0];
    mean_y += sample->pose.v[1];
  }
   //计算粒子sample对象位姿pose的x,y方向上的均值mean
  mean_x /= set->sample_count;
  mean_y /= set->sample_count;

  //使用for循环遍历粒子sample集set的每个sample对象
  for (i = 0; i < set->sample_count; i++){
    sample = set->samples + i;

 //这里的关键在于dist_threshold,粒子的位姿pose在x,y方向上的值分别减去平均值mean_x,
 //mean_y,得到的结果与dist_threshold比较，如果结果大于dist_threshold那就显示没收敛，
 //粒子还处于发散的状态中，至少dist_threshold表示不满意！
    if(fabs(sample->pose.v[0] - mean_x) > pf->dist_threshold || 
       fabs(sample->pose.v[1] - mean_y) > pf->dist_threshold){
      set->converged = 0; //发散情况下，converged设置为0
      pf->converged = 0; //发散情况下，converged设置为0
      return 0;
    }
  }
  set->converged = 1; //收敛情况下，converged设置为1
  pf->converged = 1; //收敛情况下，converged设置为1
  return 1; 
}

5.粒子滤波器随运动和观测更新

关于原理部分请跳转至上一讲的“2.粒子滤波器与蒙特卡罗定位”。

Churlaaaaaaa：5.AMCL包源码分析 | 粒子滤波器模型与pf文件夹(一)2 赞同 · 0 评论文章

首先是根据运动来更新粒子滤波器，其实是更新粒子的位姿：

// Update the filter with some new action
void pf_update_action(pf_t *pf, pf_action_model_fn_t action_fn, void *action_data)
{
 
  //创建粒子sample集set
  pf_sample_set_t *set;

  //初始化粒子sample集set
  set = pf->sets + pf->current_set;
  //引入运动模型action_fn，举个例子，这里我选择差分模型：odom_diff!
  (*action_fn) (action_data, set);
  
  return;
}

然后是根据观测来更新粒子滤波器,其实是更新粒子的权重：

// Update the filter with some new sensor observation
void pf_update_sensor(pf_t *pf, pf_sensor_model_fn_t sensor_fn, void *sensor_data)
{
  int i;
  //创建粒子sample集set,粒子sample对象
  pf_sample_set_t *set;
  pf_sample_t *sample;

  //想想看这个total是干什么的呢？
  double total;

  //初始化粒子sample集set
  set = pf->sets + pf->current_set;

  //引入sensor_fn，这里我选择似然域模型:likelihood_filed 来计算粒子sample集set的权重
  total = (*sensor_fn) (sensor_data, set);
  
  //又一个重要参数n_eff
  set->n_effective = 0;
  
  //如果呢，观测模型计算出的粒子sample集set的权值(或者说概率probability)大于0，那么：
  if (total > 0.0)
  {
    //粒子sample集sample对象的权重归一化，设置w_avg
    // Normalize weights
    double w_avg=0.0;

    //使用for循环遍历粒子sample集set的每个sample对象
    for (i = 0; i < set->sample_count; i++)
    {
     
      sample = set->samples + i;
     //利用粒子sample集sample对象的权重对w_avg赋值
      w_avg += sample->weight;
     //粒子集sample对象的权重归一化
      sample->weight /= total;
     //利用粒子sample集sample对象的权重的平方对粒子集set的n_eff参数赋值
      set->n_effective += sample->weight*sample->weight;
    }
    // Update running averages of likelihood of samples (Prob Rob p258)
    //将w_avg除以粒子集sample数目，得到新的w_avg
    w_avg /= set->sample_count;
    
    //重新刷新w_slow的值，怎么刷新呢？
    //如果w_slow等于0，那么将w_avg的值赋给它
    if(pf->w_slow == 0.0)
      pf->w_slow = w_avg;
    //如果w_slow不等于0，那么它将获得alpha_slow*(w_avg-w_slow)
    else
      pf->w_slow += pf->alpha_slow * (w_avg - pf->w_slow);

    //重新刷新w_fast的值，怎么刷新呢？
    if(pf->w_fast == 0.0)
     //如果w_fast等于0，那么将w_avg的值赋给它
      pf->w_fast = w_avg;
    //如果w_fast不等于0，那么它将获得alpha_fast*(w_avg-w_fast)
    else
      pf->w_fast += pf->alpha_fast * (w_avg - pf->w_fast);
  }
 //如果呢，观测模型计算出的粒子sample集set的权值(或者说概率probability)不大于0，那该怎么办呢？
  else
  {
    // Handle zero total
    //使用for循环遍历粒子sample集set的每个sample对象，并将每个sample的权重设置得一模一样的
    //都是weight = 1.0 / sample_count;
    for (i = 0; i < set->sample_count; i++)
    {
      sample = set->samples + i;
      sample->weight = 1.0 / set->sample_count;
    }
  }
  最后，将粒子sample集set的n_eff参数设置为：1/n_eff
  set->n_effective = 1.0/set->n_effective;
  return;

}

6.复制粒子sample集set

// copy set a to set b
void copy_set(pf_sample_set_t* set_a, pf_sample_set_t* set_b)
{
  int i;
  double total;

  //创建粒子sample对象sample_a,sample_b
  pf_sample_t *sample_a, *sample_b;
  
  //清除粒子sample集set_b的kdtree
  // Clean set b's kdtree
  pf_kdtree_clear(set_b->kdtree);

  //从set_a那里复制sample来创建set_b
  // Copy samples from set a to create set b
  total = 0;
  //将set_b的sample数目初始化为0
  set_b->sample_count = 0;

  //使用for循环遍历粒子sample集set_a的每个sample对象,再copy到set_b里
  for(i = 0; i < set_a->sample_count; i++)
  {
    sample_b = set_b->samples + set_b->sample_count++;

    sample_a = set_a->samples + i;

    assert(sample_a->weight > 0);

    // Copy sample a to sample b：不仅仅是sample的位姿，权重也要一并复制
    sample_b->pose = sample_a->pose;
    sample_b->weight = sample_a->weight;
    
    //一直在累积粒子sample_b的权重
    total += sample_b->weight;
    
    //将粒子sample_b加入到直方图里，这里用kdtree的数据结构表示，结果就是插入到粒子sample集set_b里
    // Add sample to histogram(kdtree)
    pf_kdtree_insert(set_b->kdtree, sample_b->pose, sample_b->weight);
  }
   
  //将粒子sample集set_b的权重归一化
  // Normalize weights
  for (i = 0; i < set_b->sample_count; i++)
  {
    sample_b = set_b->samples + i;
    sample_b->weight /= total;
  }
  //将粒子sample集set_b的收敛状态设置得跟粒子sample集set_a一致
  set_b->converged = set_a->converged;
}

7.对分布进行重采样

// Resample the distribution
void pf_update_resample(pf_t *pf)
{
  int i;
  double total;

 //创建粒子sample集set_a,set_b
 //创建粒子sample对象sample_a,sample_b
  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;

 //Attention:新的参数:w_diff!
  double w_diff;

 //初始化粒子sample集set_a,set_b，但是两个有点细微的区别
  set_a = pf->sets + pf->current_set;
  set_b = pf->sets + (pf->current_set + 1) % 2;

 //如果选择性采样标志不等于0，那么：
  if (pf->selective_resampling != 0)
  {
    //如果粒子sample集set_a的n_eff > 0.5 * (set_a的粒子sample数目)，那么：
    if (set_a->n_effective > 0.5*(set_a->sample_count))
    {
      //将粒子集set_a复制到set_b
      // copy set a to b
      copy_set(set_a,set_b);

      //再次计算簇的统计特性，输入为pf和粒子sample集set_b
      // Re-compute cluster statistics
      pf_cluster_stats(pf, set_b);
      
      //使用新创建的粒子sample集，其实就是对current_set的值刷新一下
      // 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)
  //用粒子sample集set_a的粒子sample数目来创建c的内存
  c = (double*)malloc(sizeof(double)*(set_a->sample_count+1));
  c[0] = 0.0;
  //根据粒子sample集set_a的粒子sample数目，遍历，遍历，
  //从而将粒子sample集set_a的粒子sample对象的权重依次取出来，存在c里
  for(i=0;i<set_a->sample_count;i++)
    c[i+1] = c[i]+set_a->samples[i].weight;
  //创建kdtree为选择性采样做准备
  // Create the kd tree for adaptive sampling
  pf_kdtree_clear(set_b->kdtree);
  
  //从粒子sample集set_a中draw samples去创造粒子sample集set_b
  // Draw samples from set a to create set b.
  total = 0;
  //将粒子sample集set_b的sample数目初始化为0
  set_b->sample_count = 0;
  
  //w_diff = 1.0 - w_fast/w_slow;
  w_diff = 1.0 - pf->w_fast / pf->w_slow;
  if(w_diff < 0.0)
    w_diff = 0.0;//如果w_diff小于0，那就将w_diff置为0咯！
  //printf("w_diff: %9.6f\n", w_diff);
 
  //由于呢，KLD适应性采样不是那么容易用低方差采样实现，所以这里采用传统方法实现
  // 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
  count_inv = 1.0/set_a->sample_count;
  r = drand48() * count_inv;
  c = set_a->samples[0].weight;
  i = 0;
  m = 0;
  */
  
  //当粒子sample集set_b的sample数目小于粒子滤波器的sample的最大数目
  while(set_b->sample_count < pf->max_samples)
  {
    //逐步从粒子sample集的set_b中取出sample对象
    sample_b = set_b->samples + set_b->sample_count++;
 
    //当随机数小于w_diff;w_diff的赋值在上面实现过了
    if(drand48() < w_diff)
     //使用随机生成位姿的模型生成sample_b的位姿；真够传统的方法呀，拍拍手。
      sample_b->pose = (pf->random_pose_fn)(pf->random_pose_data);

    //当随机数大于w_diff;那就复杂了，怎么生成sample_b的位姿呢？进入else吗？这里注释掉了
    低方差重采样的代码实现，谁也没跑过，摊摊我的小手，我也不知道。
    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();
      //遍历粒子sample集set_a，把c拉出来跟r做比较；
      for(i=0;i<set_a->sample_count;i++)
      {
        if((c[i] <= r) && (r < c[i+1]))
          break;
      }
      assert(i<set_a->sample_count);

      //遍历粒子sample集set_a，将sample_a取出来
      sample_a = set_a->samples + i;

      assert(sample_a->weight > 0);
      
      //把sample_a的位姿赋给sample_b
      // Add sample to list
      sample_b->pose = sample_a->pose;
    }
   //将sample_b的权重初始化为1
    sample_b->weight = 1.0;
   //计算sample_b所在set的权重和
    total += sample_b->weight;
   //把粒子sample_b添加到kdtree里，其实也就是添加到粒子sample集set_b里。
    // Add sample to histogram(kdtree)
    pf_kdtree_insert(set_b->kdtree, sample_b->pose, sample_b->weight);
    
   //检查看是否有足够的粒子sample了呢？
  //这里使用pf_resample_limit函数，输入为粒子sample集set_b的kdtree的叶子节点个数，这也代表
  //着直方图的k个bin。
    // See if we have enough samples yet
    if (set_b->sample_count > pf_resample_limit(pf, set_b->kdtree->leaf_count))
      break;
  }
  //重设w_slow和w_fast
  // 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");

  // 使用for循环，遍历粒子sample集set_b,将sample_b权重归一化
  for (i = 0; i < set_b->sample_count; i++)
  {
    sample_b = set_b->samples + i;
    sample_b->weight /= total;
  }
  
  // 重新计算粒子sample集set_b的簇的统计特性
  // Re-compute cluster statistics
  pf_cluster_stats(pf, set_b);

 //重新设置current_set的值 
 // Use the newly created sample set
  pf->current_set = (pf->current_set + 1) % 2; 
  //粒子滤波器更新收敛
  pf_update_converged(pf);
 //释放c的内存
  free(c);
  return;
}

8.计算所需的粒子sample数目

给定直方图的bins的数目k，计算所需的粒子sample的数目，这个可真难呀！

// Compute the required number of samples, given that there are k bins
// with samples in them. (跟historgram有关) 
//This is taken directly from Fox et al.
int pf_resample_limit(pf_t *pf, int k)
{
  double a, b, c, x;
  int n;
  
 //如果k超出边界，那就返回粒子sample的最大数目
  // Return max_samples in case k is outside expected range, this shouldn't
  // happen, but is added to prevent any runtime errors 约束 && k
  if (k < 1 || k > pf->max_samples)
      return pf->max_samples;

  //pf粒子滤波器 && limit_cache ,pop_err,pop_z;
  // Return value if cache is valid, which means value is non-zero positive 
  if (pf->limit_cache[k-1] > 0)
    return pf->limit_cache[k-1];
  //如果k等于1呢，那也是返回粒子sample的最大数目
  if (k == 1)
  {
    pf->limit_cache[k-1] = pf->max_samples;
    return pf->max_samples;
  }
  
  //这里用到直方图的k,粒子滤波器的pop_err,pop_z;
  a = 1;
  b = 2 / (9 * ((double) k - 1));
  c = sqrt(2 / (9 * ((double) k - 1))) * pf->pop_z;
  x = a - b + c;
  //这里计算得出n
  n = (int) ceil((k - 1) / (2 * pf->pop_err) * x * x * x);
  //如果n小于粒子sample的最小数目，那么返回粒子sample的最小数目
  if (n < pf->min_samples)
  {
    pf->limit_cache[k-1] = pf->min_samples;
    return pf->min_samples;
  }
  //如果n大于粒子sample的最小数目，那么返回粒子sample的最大数目
  if (n > pf->max_samples)
  {
    pf->limit_cache[k-1] = pf->max_samples;
    return pf->max_samples;
  }
  //如果n很听话呢，那就好好利用它吧！
  pf->limit_cache[k-1] = n;
  return n;
}

9.为一个粒子sample集重新计算其簇的均值/协方差进而得出粒子集合的均值/协方差

// Re-compute the cluster statistics for a sample set
void pf_cluster_stats(pf_t *pf, pf_sample_set_t *set)
{
  int i, j, k, cidx;
  pf_sample_t *sample;
  pf_cluster_t *cluster;
  
  // Workspace
  double m[4], c[2][2];
  size_t count;
  double weight;

  // Cluster the samples：聚集粒子sample成簇
  pf_kdtree_cluster(set->kdtree);
  
  // Initialize cluster stats:初始化簇的数目
  set->cluster_count = 0;
  //根据粒子sample集set的簇的最大数目，逐步遍历每个簇
  for (i = 0; i < set->cluster_max_count; i++)
  {
    cluster = set->clusters + i;
    cluster->count = 0;
    cluster->weight = 0;
    cluster->mean = pf_vector_zero();
    cluster->cov = pf_matrix_zero();

    for (j = 0; j < 4; j++)
      cluster->m[j] = 0.0;
    for (j = 0; j < 2; j++)
      for (k = 0; k < 2; k++)
        cluster->c[j][k] = 0.0;
  }
  //初始化滤波器的统计特性
  // Initialize overall filter stats
  count = 0;
  weight = 0.0;
  //粒子sample集的均值和协方差初始化
  set->mean = pf_vector_zero();
  set->cov = pf_matrix_zero();
  for (j = 0; j < 4; j++)
    m[j] = 0.0;
  for (j = 0; j < 2; j++)
    for (k = 0; k < 2; k++)
      c[j][k] = 0.0;
  //计算簇的统计特性
  // Compute cluster stats
  //遍历set的所有sample对象
  for (i = 0; i < set->sample_count; i++)
  {
    sample = set->samples + i;

    //printf("%d %f %f %f\n", i, sample->pose.v[0], sample->pose.v[1], sample->pose.v[2]);
    //获得这个sample对象的簇标签
    // Get the cluster label for this sample
    cidx = pf_kdtree_get_cluster(set->kdtree, sample->pose);
    assert(cidx >= 0);
    //根据簇的最大数目来判断：
    if (cidx >= set->cluster_max_count)
      continue;
    if (cidx + 1 > set->cluster_count)
      set->cluster_count = cidx + 1;
    //这里取出了簇对象
    cluster = set->clusters + cidx;
    //簇的数目加1，权重也接收了来自粒子sample对象的赋值
    cluster->count += 1;
    cluster->weight += sample->weight;
    //粒子sample对象的权重也在统计着
    count += 1;
    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]);

    //计算簇的均值的和:m[0],m[1],m[2],m[3]
    m[0] += sample->weight * sample->pose.v[0];
    m[1] += sample->weight * sample->pose.v[1];
    m[2] += sample->weight * cos(sample->pose.v[2]);
    m[3] += sample->weight * sin(sample->pose.v[2]);
    
    //计算簇的协方差，先用双重for循环实现一个遍历
    // Compute covariance in linear components
    for (j = 0; j < 2; j++)
      for (k = 0; k < 2; k++)
      {
        //对簇的协方差进行赋值
        cluster->c[j][k] += sample->weight * sample->pose.v[j] * sample->pose.v[k];
        //计算簇的协方差的和:c[j][k]
        c[j][k] += sample->weight * sample->pose.v[j] * sample->pose.v[k];
      }
  }

  // Normalize:归一化；使用for循环遍历粒子sample集的簇，将均值和协方差归一化
  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]);

    cluster->cov = pf_matrix_zero();

    // Covariance in linear components：线性部分的协方差
    for (j = 0; j < 2; j++)
      for (k = 0; k < 2; k++)
        cluster->cov.m[j][k] = cluster->c[j][k] / cluster->weight -
          cluster->mean.v[j] * cluster->mean.v[k];

    // Covariance in angular components; I think this is the correct
    // formula for circular statistics.：角度部分的协方差
    cluster->cov.m[2][2] = -2 * log(sqrt(cluster->m[2] * cluster->m[2] +
                                         cluster->m[3] * cluster->m[3]));

  }
  //计算粒子sample集set的均值
  // Compute overall filter stats
  set->mean.v[0] = m[0] / weight;
  set->mean.v[1] = m[1] / weight;
  set->mean.v[2] = atan2(m[3], m[2]);
  //计算粒子sample集set的线性方向的协方差
  // Covariance in linear components
  for (j = 0; j < 2; j++)
    for (k = 0; k < 2; k++)
      set->cov.m[j][k] = c[j][k] / weight - set->mean.v[j] * set->mean.v[k];
  //计算粒子sample集set的角度方向的协方差
  // Covariance in angular components; I think this is the correct
  // formula for circular statistics.
  set->cov.m[2][2] = -2 * log(sqrt(m[2] * m[2] + m[3] * m[3]));

  return;
}

10.设置选择性采样

void pf_set_selective_resampling(pf_t *pf, int selective_resampling)
{
  //这里将selective_resampling标识符简单设置一下即可
  pf->selective_resampling = selective_resampling;
}

11.计算均值和协方差

这里相比前面简直太简单了，就不加注释了。

// Compute the CEP statistics (mean and variance).
void pf_get_cep_stats(pf_t *pf, pf_vector_t *mean, double *var)
{
  int i;
  double mn, mx, my, mrr;
  pf_sample_set_t *set;
  pf_sample_t *sample;
  
  set = pf->sets + pf->current_set;

  mn = 0.0;
  mx = 0.0;
  my = 0.0;
  mrr = 0.0;
  
  for (i = 0; i < set->sample_count; i++)
  {
    sample = set->samples + i;

    mn += sample->weight;
    mx += sample->weight * sample->pose.v[0];
    my += sample->weight * sample->pose.v[1];
    mrr += sample->weight * sample->pose.v[0] * sample->pose.v[0];
    mrr += sample->weight * sample->pose.v[1] * sample->pose.v[1];
  }

  mean->v[0] = mx / mn;
  mean->v[1] = my / mn;
  mean->v[2] = 0.0;

  *var = mrr / mn - (mx * mx / (mn * mn) + my * my / (mn * mn));

  return;
}

12.获得一个粒子簇的统计特性：均值/协方差/权重

// Get the statistics for a particular cluster.
int pf_get_cluster_stats(pf_t *pf, int clabel, double *weight,
                         pf_vector_t *mean, pf_matrix_t *cov)
{
  pf_sample_set_t *set;
  pf_cluster_t *cluster;

  set = pf->sets + pf->current_set;

  if (clabel >= set->cluster_count)
    return 0;
 //注意这里的clabel哦！
  cluster = set->clusters + clabel;
 //这里取出簇的权重，均值和协方差
  *weight = cluster->weight;
  *mean = cluster->mean;
  *cov = cluster->cov;

  return 1;

好了，艰难地分析完了粒子滤波器所在的核心--pf.cpp，除了粒子滤波器这个磨人的小妖精，还有什么需要我们关心的呢？哎呀，搞了半天，kdtree是个啥呀？概率密度函数pdf咋生成的啊？这个协方差矩阵咋求解啊？怎么看到了eigen-descomposition(特征值分解)呢？那又怎么分解呢？

 

转自：6.AMCL包源码分析 | 粒子滤波器模型与pf文件夹(二) - 知乎 (zhihu.com)

 

标签：set,粒子,sample,cluster,AMCL,pf,samples,源码
来源： https://www.cnblogs.com/tdyizhen1314/p/16692805.html

本站声明： 1. iCode9 技术分享网（下文简称本站）提供的所有内容，仅供技术学习、探讨和分享；
2. 关于本站的所有留言、评论、转载及引用，纯属内容发起人的个人观点，与本站观点和立场无关；
3. 关于本站的所有言论和文字，纯属内容发起人的个人观点，与本站观点和立场无关；
4. 本站文章均是网友提供，不完全保证技术分享内容的完整性、准确性、时效性、风险性和版权归属；如您发现该文章侵犯了您的权益，可联系我们第一时间进行删除；
5. 本站为非盈利性的个人网站，所有内容不会用来进行牟利，也不会利用任何形式的广告来间接获益，纯粹是为了广大技术爱好者提供技术内容和技术思想的分享性交流网站。