// Build up cumulative probability table for resampling. // TODO: Replace this with a more efficient procedure (e.g., GeneralDiscreteDistributions.html) // 对粒子的权重进行积分,获得分布函数,后面用于重采样 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; // c[1] = 第一个粒子权重 // c[2] = 第一和第二粒子权重之和,以此类推 c[i] 是粒子集的 前i个粒子的权重之和 // 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;
// 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; */ // 确保重采样生成的粒子集(set_b)的粒子数不超过规定的最大的粒子数 while(set_b->sample_count < pf->max_samples) { sample_b = set_b->samples + set_b->sample_count++; // 产生的随机数小于w_diff时,将往set_b中随机注入粒子 if(drand48() < w_diff) // pf->random_pose_fn 为一个函数指针,其返回一个随机位姿 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++; */