42 #include "pf_kdtree.h" 48 static int pf_resample_limit(pf_t *pf,
int k);
51 static void pf_cluster_stats(pf_t *pf, pf_sample_set_t *
set);
55 pf_t *pf_alloc(
int min_samples,
int max_samples,
56 double alpha_slow,
double alpha_fast,
57 pf_init_model_fn_t random_pose_fn,
void *random_pose_data)
66 pf = calloc(1,
sizeof(pf_t));
68 pf->random_pose_fn = random_pose_fn;
69 pf->random_pose_data = random_pose_data;
71 pf->min_samples = min_samples;
72 pf->max_samples = max_samples;
83 for (j = 0; j < 2; j++)
87 set->sample_count = max_samples;
88 set->samples = calloc(max_samples,
sizeof(pf_sample_t));
90 for (i = 0; i <
set->sample_count; i++)
92 sample =
set->samples + i;
93 sample->pose.v[0] = 0.0;
94 sample->pose.v[1] = 0.0;
95 sample->pose.v[2] = 0.0;
96 sample->weight = 1.0 / max_samples;
100 set->kdtree = pf_kdtree_alloc(3 * max_samples);
102 set->cluster_count = 0;
103 set->cluster_max_count = max_samples;
104 set->clusters = calloc(set->cluster_max_count,
sizeof(pf_cluster_t));
106 set->mean = pf_vector_zero();
107 set->cov = pf_matrix_zero();
113 pf->alpha_slow = alpha_slow;
114 pf->alpha_fast = alpha_fast;
121 void pf_free(pf_t *pf)
125 for (i = 0; i < 2; i++)
127 free(pf->sets[i].clusters);
128 pf_kdtree_free(pf->sets[i].kdtree);
129 free(pf->sets[i].samples);
138 void pf_init(pf_t *pf, pf_vector_t mean, pf_matrix_t cov)
141 pf_sample_set_t *
set;
143 pf_pdf_gaussian_t *pdf;
145 set = pf->sets + pf->current_set;
148 pf_kdtree_clear(set->kdtree);
150 set->sample_count = pf->max_samples;
152 pdf = pf_pdf_gaussian_alloc(mean, cov);
155 for (i = 0; i <
set->sample_count; i++)
157 sample =
set->samples + i;
158 sample->weight = 1.0 / pf->max_samples;
159 sample->pose = pf_pdf_gaussian_sample(pdf);
162 pf_kdtree_insert(set->kdtree, sample->pose, sample->weight);
165 pf->w_slow = pf->w_fast = 0.0;
167 pf_pdf_gaussian_free(pdf);
170 pf_cluster_stats(pf,
set);
177 void pf_init_model(pf_t *pf, pf_init_model_fn_t init_fn,
void *init_data)
180 pf_sample_set_t *
set;
183 set = pf->sets + pf->current_set;
186 pf_kdtree_clear(set->kdtree);
188 set->sample_count = pf->max_samples;
191 for (i = 0; i <
set->sample_count; i++)
193 sample =
set->samples + i;
194 sample->weight = 1.0 / pf->max_samples;
195 sample->pose = (*init_fn) (init_data);
198 pf_kdtree_insert(set->kdtree, sample->pose, sample->weight);
201 pf->w_slow = pf->w_fast = 0.0;
204 pf_cluster_stats(pf,
set);
211 void pf_update_action(pf_t *pf, pf_action_model_fn_t action_fn,
void *action_data)
213 pf_sample_set_t *
set;
215 set = pf->sets + pf->current_set;
217 (*action_fn) (action_data,
set);
225 void pf_update_sensor(pf_t *pf, pf_sensor_model_fn_t sensor_fn,
void *sensor_data)
228 pf_sample_set_t *
set;
232 set = pf->sets + pf->current_set;
235 total = (*sensor_fn) (sensor_data,
set);
241 for (i = 0; i <
set->sample_count; i++)
243 sample =
set->samples + i;
244 w_avg += sample->weight;
245 sample->weight /= total;
248 w_avg /=
set->sample_count;
249 if(pf->w_slow == 0.0)
252 pf->w_slow += pf->alpha_slow * (w_avg - pf->w_slow);
253 if(pf->w_fast == 0.0)
256 pf->w_fast += pf->alpha_fast * (w_avg - pf->w_fast);
265 for (i = 0; i <
set->sample_count; i++)
267 sample =
set->samples + i;
268 sample->weight = 1.0 /
set->sample_count;
277 void pf_update_resample(pf_t *pf)
281 pf_sample_set_t *set_a, *set_b;
282 pf_sample_t *sample_a, *sample_b;
291 set_a = pf->sets + pf->current_set;
292 set_b = pf->sets + (pf->current_set + 1) % 2;
297 c = (
double*)malloc(
sizeof(
double)*(set_a->sample_count+1));
299 for(i=0;i<set_a->sample_count;i++)
300 c[i+1] = c[i]+set_a->samples[i].weight;
303 pf_kdtree_clear(set_b->kdtree);
307 set_b->sample_count = 0;
309 w_diff = 1.0 - pf->w_fast / pf->w_slow;
324 while(set_b->sample_count < pf->max_samples)
326 sample_b = set_b->samples + set_b->sample_count++;
328 if(drand48() < w_diff)
329 sample_b->pose = (pf->random_pose_fn)(pf->random_pose_data);
359 for(i=0;i<set_a->sample_count;i++)
361 if((c[i] <= r) && (r < c[i+1]))
364 assert(i<set_a->sample_count);
366 sample_a = set_a->samples + i;
368 assert(sample_a->weight > 0);
371 sample_b->pose = sample_a->pose;
374 sample_b->weight = 1.0;
375 total += sample_b->weight;
378 pf_kdtree_insert(set_b->kdtree, sample_b->pose, sample_b->weight);
381 if (set_b->sample_count > pf_resample_limit(pf, set_b->kdtree->leaf_count))
387 pf->w_slow = pf->w_fast = 0.0;
392 for (i = 0; i < set_b->sample_count; i++)
394 sample_b = set_b->samples + i;
395 sample_b->weight /= total;
399 pf_cluster_stats(pf, set_b);
402 pf->current_set = (pf->current_set + 1) % 2;
411 int pf_resample_limit(pf_t *pf,
int k)
417 return pf->max_samples;
420 b = 2 / (9 * ((double) k - 1));
421 c = sqrt(2 / (9 * ((
double) k - 1))) * pf->pop_z;
424 n = (int) ceil((k - 1) / (2 * pf->pop_err) * x * x * x);
426 if (n < pf->min_samples)
427 return pf->min_samples;
428 if (n > pf->max_samples)
429 return pf->max_samples;
436 void pf_cluster_stats(pf_t *pf, pf_sample_set_t *
set)
440 pf_cluster_t *cluster;
443 double m[4], c[2][2];
448 pf_kdtree_cluster(set->kdtree);
451 set->cluster_count = 0;
453 for (i = 0; i <
set->cluster_max_count; i++)
455 cluster =
set->clusters + i;
458 cluster->mean = pf_vector_zero();
459 cluster->cov = pf_matrix_zero();
461 for (j = 0; j < 4; j++)
463 for (j = 0; j < 2; j++)
464 for (k = 0; k < 2; k++)
465 cluster->c[j][k] = 0.0;
471 set->mean = pf_vector_zero();
472 set->cov = pf_matrix_zero();
473 for (j = 0; j < 4; j++)
475 for (j = 0; j < 2; j++)
476 for (k = 0; k < 2; k++)
480 for (i = 0; i <
set->sample_count; i++)
482 sample =
set->samples + i;
487 cidx = pf_kdtree_get_cluster(set->kdtree, sample->pose);
489 if (cidx >= set->cluster_max_count)
491 if (cidx + 1 > set->cluster_count)
492 set->cluster_count = cidx + 1;
494 cluster =
set->clusters + cidx;
497 cluster->weight += sample->weight;
500 weight += sample->weight;
503 cluster->m[0] += sample->weight * sample->pose.v[0];
504 cluster->m[1] += sample->weight * sample->pose.v[1];
505 cluster->m[2] += sample->weight * cos(sample->pose.v[2]);
506 cluster->m[3] += sample->weight * sin(sample->pose.v[2]);
508 m[0] += sample->weight * sample->pose.v[0];
509 m[1] += sample->weight * sample->pose.v[1];
510 m[2] += sample->weight * cos(sample->pose.v[2]);
511 m[3] += sample->weight * sin(sample->pose.v[2]);
514 for (j = 0; j < 2; j++)
515 for (k = 0; k < 2; k++)
517 cluster->c[j][k] += sample->weight * sample->pose.v[j] * sample->pose.v[k];
518 c[j][k] += sample->weight * sample->pose.v[j] * sample->pose.v[k];
523 for (i = 0; i <
set->cluster_count; i++)
525 cluster =
set->clusters + i;
527 cluster->mean.v[0] = cluster->m[0] / cluster->weight;
528 cluster->mean.v[1] = cluster->m[1] / cluster->weight;
529 cluster->mean.v[2] = atan2(cluster->m[3], cluster->m[2]);
531 cluster->cov = pf_matrix_zero();
534 for (j = 0; j < 2; j++)
535 for (k = 0; k < 2; k++)
536 cluster->cov.m[j][k] = cluster->c[j][k] / cluster->weight -
537 cluster->mean.v[j] * cluster->mean.v[k];
541 cluster->cov.m[2][2] = -2 * log(sqrt(cluster->m[2] * cluster->m[2] +
542 cluster->m[3] * cluster->m[3]));
550 set->mean.v[0] = m[0] / weight;
551 set->mean.v[1] = m[1] / weight;
552 set->mean.v[2] = atan2(m[3], m[2]);
555 for (j = 0; j < 2; j++)
556 for (k = 0; k < 2; k++)
557 set->cov.m[j][k] = c[j][k] / weight - set->mean.v[j] * set->mean.v[k];
561 set->cov.m[2][2] = -2 * log(sqrt(m[2] * m[2] + m[3] * m[3]));
568 void pf_get_cep_stats(pf_t *pf, pf_vector_t *mean,
double *var)
571 double mn, mx, my, mrr;
572 pf_sample_set_t *
set;
575 set = pf->sets + pf->current_set;
582 for (i = 0; i <
set->sample_count; i++)
584 sample =
set->samples + i;
586 mn += sample->weight;
587 mx += sample->weight * sample->pose.v[0];
588 my += sample->weight * sample->pose.v[1];
589 mrr += sample->weight * sample->pose.v[0] * sample->pose.v[0];
590 mrr += sample->weight * sample->pose.v[1] * sample->pose.v[1];
593 mean->v[0] = mx / mn;
594 mean->v[1] = my / mn;
597 *var = mrr / mn - (mx * mx / (mn * mn) + my * my / (mn * mn));
604 int pf_get_cluster_stats(pf_t *pf,
int clabel,
double *weight,
605 pf_vector_t *mean, pf_matrix_t *cov)
607 pf_sample_set_t *
set;
608 pf_cluster_t *cluster;
610 set = pf->sets + pf->current_set;
612 if (clabel >= set->cluster_count)
614 cluster =
set->clusters + clabel;
616 *weight = cluster->weight;
617 *mean = cluster->mean;