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集setset = 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收敛到0pf_init_converged(pf);return;
}

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

pf_kdtree_clear(set->kdtree);

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

然后算法通过：

pdf = pf_pdf_gaussian_alloc(mean, cov);

调用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;
}

然后通过下面的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);}

在这里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);

这里：

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

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

这个：

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

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

后面两个：

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

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

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

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

2.其他模型

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

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

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