AMCL代码详解（三）创建粒子模型

AMCL中创建粒子模型的方式有两种：

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中包含了位姿权重，kd树，簇等多个因素
  pf_sample_set_t *set;

  //pf_sample_t包含了位姿以及权重两个因素
  pf_sample_t *sample;
  pf_pdf_gaussian_t *pdf;
  //初始化粒子sample集set
  set = pf->sets + pf->current_set;
  
  // Create the kd tree for adaptive sampling
  //创建kdtree为了选择性采样
  pf_kdtree_clear(set->kdtree);
  //粒子sample集set的sample数目设定
  set->sample_count = pf->max_samples;
  //使用高斯模型(均值，协方差)创建pdf
  //这里设置了一个高斯模型
  pdf = pf_pdf_gaussian_alloc(mean, cov);
    
  // Compute the new sample poses
  //计算新的粒子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);

    // Add sample to histogram
    //以kdtree数据结构的方式添加/存储粒子sample 或者说以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);
    
  // Re-compute cluster statistics
  //再次计算粒子sample集set的簇cluster的统计特性，输入为粒子滤波器pf,和set  
  pf_cluster_stats(pf, set); 

  //set converged to 0
  //粒子sample集set收敛到0
  pf_init_converged(pf);

  return;
}

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55

前面主要是建立了几个结构体，具体信息可以在.h文件中查看这些结构体的定义。在创建完结构体之后，首先使用：

pf_kdtree_clear(set->kdtree);
1

清空了一个kd树。详细代码在pf_kdtree.cc中，将set结构体中的树清空，为后面插入位姿做准备。

然后算法通过：

pdf = pf_pdf_gaussian_alloc(mean, cov);
1

调用pf_pdf.c中的代码创建了一个高斯模型：

// Create a gaussian pdf
//这里相当于建立了一个高斯概率密度模型
//包含了均值，方差以及还有一个方差特征值分解得到的特征值以及特征向量
pf_pdf_gaussian_t *pf_pdf_gaussian_alloc(pf_vector_t x, pf_matrix_t cx)
{
  //double m[3][3]矩阵
  pf_matrix_t cd;
  
  
  //pf_vector_t x;  //double v[3];
  //pf_matrix_t cx; //double m[3][3];
  //pf_matrix_t cxi;//double m[3][3];
  //double cxdet;
  // Decomposed covariance matrix (rotation * diagonal)
  //pf_matrix_t cr;
  //pf_vector_t cd;
  pf_pdf_gaussian_t *pdf;
  //在内存的动态存储区中分配num个长度为size的连续空间，函数返回一个指向分配起始地址的指针；如果分配不成功，返回NULL。
  //void* calloc(unsigned int num，unsigned int size);
  pdf = calloc(1, sizeof(pf_pdf_gaussian_t));
  //对结构体中的前两项赋值。
  pdf->x = x;
  pdf->cx = cx;
  //pdf->cxi = pf_matrix_inverse(cx, &pdf->cxdet);

  // Decompose the convariance matrix into a rotation
  // matrix and a diagonal matrix.
  // cx为输入，对其进行特征值分解，特征值存入cd中，特征向量存入cr中
  pf_matrix_unitary(&pdf->cr, &cd, pdf->cx);
  pdf->cd.v[0] = sqrt(cd.m[0][0]);
  pdf->cd.v[1] = sqrt(cd.m[1][1]);
  pdf->cd.v[2] = sqrt(cd.m[2][2]);

  // Initialize the random number generator
  //pdf->rng = gsl_rng_alloc(gsl_rng_taus);
  //gsl_rng_set(pdf->rng, ++pf_pdf_seed);
  //rand48用于产生0-1之间的均匀分布的随机数
  srand48(++pf_pdf_seed);

  return pdf;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41

然后通过下面的for循环更新出一系列位姿

  // Compute the new sample poses
  //计算新的粒子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);

    // Add sample to histogram
    //以kdtree数据结构的方式添加/存储粒子sample 或者说以kdtree数据结构的方式表达直方图
    pf_kdtree_insert(set->kdtree, sample->pose, sample->weight);
  }
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16

在这里sample_count为粒子的数量，初始时每个粒子的权重是一样的。然后在pf_pdf_gaussian_sample函数中使用初始位姿、方差以及一个随机数综合产生一个符合高斯分布的随机位姿。作为第i个粒子的位姿。

接着将该位姿插入到kd树中。kd树作为AMCL的核心数据结构，担负的重要职能便是能自如地插入新的位姿

    // Add sample to histogram
    //以kdtree数据结构的方式添加/存储粒子sample 或者说以kdtree数据结构的方式表达直方图
    pf_kdtree_insert(set->kdtree, sample->pose, sample->weight);
1
2
3

这里：

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

与参数alpha_slow和alpha_fast相关，这些字段用于触发recover机制，增加随机位姿， 用以削弱粒子滤波器的粒子匮乏问题的影响。这里初始化时设置为0

这个：

//销毁释放pdf占用的内存
  pf_pdf_gaussian_free(pdf);
1
2

基本不用看，就是释放了一下内存。

后面两个：

// Re-compute cluster statistics
  //再次计算粒子sample集set的簇cluster的统计特性，输入为粒子滤波器pf,和set  
  pf_cluster_stats(pf, set); 
1
2
3

为一个粒子sample集重新计算其簇的均值/协方差进而得出粒子集合的均值/协方差。粒子sample->粒子簇cluster->粒子集set的统计特性:均值和协方差 粒子的分数在pf_kdtree.c中

一个粒子集中有很多个簇，簇中包含了这个簇中粒子的数量、权重、均值方差等信息。这里的簇似乎跟kd树是对应的，差不多有多少个kd树就有多少个簇。不知道理解的对不对。

另外就是这个计算出来的簇的统计特性是干嘛用的？这里暂时还不是很清楚。需要再往下看一看。

2.其他模型

这里的过程与上面的高斯模型基本一致，主要区别再于这里：

sample->pose = (*init_fn) (init_data);
1

其生成的位姿根据传参的两个参数变化，而高斯分布中传入的是均值与方差，从而生成随机数。