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;
142 pf_pdf_gaussian_t *pdf;
144 set = pf->sets + pf->current_set;
147 pf_kdtree_clear(set->kdtree);
149 set->sample_count = pf->max_samples;
151 pdf = pf_pdf_gaussian_alloc(mean, cov);
154 for (i = 0; i < set->sample_count; i++)
156 pf_sample_t *sample = set->samples + i;
157 sample->weight = 1.0 / pf->max_samples;
158 sample->pose = pf_pdf_gaussian_sample(pdf);
161 pf_kdtree_insert(set->kdtree, sample->pose, sample->weight);
164 pf->w_slow = pf->w_fast = 0.0;
166 pf_pdf_gaussian_free(pdf);
169 pf_cluster_stats(pf, set);
176 void pf_init_model(pf_t *pf, pf_init_model_fn_t init_fn,
void *init_data)
179 pf_sample_set_t *set;
182 set = pf->sets + pf->current_set;
185 pf_kdtree_clear(set->kdtree);
187 set->sample_count = pf->max_samples;
190 for (i = 0; i < set->sample_count; i++)
192 sample = set->samples + i;
193 sample->weight = 1.0 / pf->max_samples;
194 sample->pose = (*init_fn) (init_data);
197 pf_kdtree_insert(set->kdtree, sample->pose, sample->weight);
200 pf->w_slow = pf->w_fast = 0.0;
203 pf_cluster_stats(pf, set);
210 void pf_update_action(pf_t *pf, pf_action_model_fn_t action_fn,
void *action_data)
212 pf_sample_set_t *set;
214 set = pf->sets + pf->current_set;
216 (*action_fn) (action_data, set);
224 void pf_update_sensor(pf_t *pf, pf_sensor_model_fn_t sensor_fn,
void *sensor_data)
227 pf_sample_set_t *set;
231 set = pf->sets + pf->current_set;
234 total = (*sensor_fn) (sensor_data, set);
240 for (i = 0; i < set->sample_count; i++)
242 sample = set->samples + i;
243 w_avg += sample->weight;
244 sample->weight /= total;
247 w_avg /= set->sample_count;
248 if(pf->w_slow == 0.0)
251 pf->w_slow += pf->alpha_slow * (w_avg - pf->w_slow);
252 if(pf->w_fast == 0.0)
255 pf->w_fast += pf->alpha_fast * (w_avg - pf->w_fast);
264 for (i = 0; i < set->sample_count; i++)
266 sample = set->samples + i;
267 sample->weight = 1.0 / set->sample_count;
276 void pf_update_resample(pf_t *pf)
280 pf_sample_set_t *set_a, *set_b;
281 pf_sample_t *sample_a, *sample_b;
290 set_a = pf->sets + pf->current_set;
291 set_b = pf->sets + (pf->current_set + 1) % 2;
296 c = (
double*)malloc(
sizeof(
double)*(set_a->sample_count+1));
298 for(i=0;i<set_a->sample_count;i++)
299 c[i+1] = c[i]+set_a->samples[i].weight;
302 pf_kdtree_clear(set_b->kdtree);
306 set_b->sample_count = 0;
308 w_diff = 1.0 - pf->w_fast / pf->w_slow;
323 while(set_b->sample_count < pf->max_samples)
325 sample_b = set_b->samples + set_b->sample_count++;
327 if(drand48() < w_diff)
328 sample_b->pose = (pf->random_pose_fn)(pf->random_pose_data);
358 for(i=0;i<set_a->sample_count;i++)
360 if((c[i] <= r) && (r < c[i+1]))
363 assert(i<set_a->sample_count);
365 sample_a = set_a->samples + i;
367 assert(sample_a->weight > 0);
370 sample_b->pose = sample_a->pose;
373 sample_b->weight = 1.0;
374 total += sample_b->weight;
377 pf_kdtree_insert(set_b->kdtree, sample_b->pose, sample_b->weight);
380 if (set_b->sample_count > pf_resample_limit(pf, set_b->kdtree->leaf_count))
386 pf->w_slow = pf->w_fast = 0.0;
391 for (i = 0; i < set_b->sample_count; i++)
393 sample_b = set_b->samples + i;
394 sample_b->weight /= total;
398 pf_cluster_stats(pf, set_b);
401 pf->current_set = (pf->current_set + 1) % 2;
410 int pf_resample_limit(pf_t *pf,
int k)
416 return pf->max_samples;
419 b = 2 / (9 * ((double) k - 1));
420 c = sqrt(2 / (9 * ((
double) k - 1))) * pf->pop_z;
423 n = (int) ceil((k - 1) / (2 * pf->pop_err) * x * x * x);
425 if (n < pf->min_samples)
426 return pf->min_samples;
427 if (n > pf->max_samples)
428 return pf->max_samples;
435 void pf_cluster_stats(pf_t *pf, pf_sample_set_t *set)
438 pf_cluster_t *cluster;
441 double m[4], c[2][2];
446 pf_kdtree_cluster(set->kdtree);
449 set->cluster_count = 0;
451 for (i = 0; i < set->cluster_max_count; i++)
453 cluster = set->clusters + i;
456 cluster->mean = pf_vector_zero();
457 cluster->cov = pf_matrix_zero();
459 for (j = 0; j < 4; j++)
461 for (j = 0; j < 2; j++)
462 for (k = 0; k < 2; k++)
463 cluster->c[j][k] = 0.0;
469 set->mean = pf_vector_zero();
470 set->cov = pf_matrix_zero();
471 for (j = 0; j < 4; j++)
473 for (j = 0; j < 2; j++)
474 for (k = 0; k < 2; k++)
478 for (i = 0; i < set->sample_count; i++)
480 pf_sample_t *sample = set->samples + i;
485 int cidx = pf_kdtree_get_cluster(set->kdtree, sample->pose);
487 if (cidx >= set->cluster_max_count)
489 if (cidx + 1 > set->cluster_count)
490 set->cluster_count = cidx + 1;
492 cluster = set->clusters + cidx;
495 cluster->weight += sample->weight;
498 weight += sample->weight;
501 cluster->m[0] += sample->weight * sample->pose.v[0];
502 cluster->m[1] += sample->weight * sample->pose.v[1];
503 cluster->m[2] += sample->weight * cos(sample->pose.v[2]);
504 cluster->m[3] += sample->weight * sin(sample->pose.v[2]);
506 m[0] += sample->weight * sample->pose.v[0];
507 m[1] += sample->weight * sample->pose.v[1];
508 m[2] += sample->weight * cos(sample->pose.v[2]);
509 m[3] += sample->weight * sin(sample->pose.v[2]);
512 for (j = 0; j < 2; j++)
513 for (k = 0; k < 2; k++)
515 cluster->c[j][k] += sample->weight * sample->pose.v[j] * sample->pose.v[k];
516 c[j][k] += sample->weight * sample->pose.v[j] * sample->pose.v[k];
521 for (i = 0; i < set->cluster_count; i++)
523 cluster = set->clusters + i;
525 cluster->mean.v[0] = cluster->m[0] / cluster->weight;
526 cluster->mean.v[1] = cluster->m[1] / cluster->weight;
527 cluster->mean.v[2] = atan2(cluster->m[3], cluster->m[2]);
529 cluster->cov = pf_matrix_zero();
532 for (j = 0; j < 2; j++)
533 for (k = 0; k < 2; k++)
534 cluster->cov.m[j][k] = cluster->c[j][k] / cluster->weight -
535 cluster->mean.v[j] * cluster->mean.v[k];
539 cluster->cov.m[2][2] = -2 * log(sqrt(cluster->m[2] * cluster->m[2] +
540 cluster->m[3] * cluster->m[3]));
548 set->mean.v[0] = m[0] / weight;
549 set->mean.v[1] = m[1] / weight;
550 set->mean.v[2] = atan2(m[3], m[2]);
553 for (j = 0; j < 2; j++)
554 for (k = 0; k < 2; k++)
555 set->cov.m[j][k] = c[j][k] / weight - set->mean.v[j] * set->mean.v[k];
559 set->cov.m[2][2] = -2 * log(sqrt(m[2] * m[2] + m[3] * m[3]));
566 void pf_get_cep_stats(pf_t *pf, pf_vector_t *mean,
double *var)
569 double mn, mx, my, mrr;
570 pf_sample_set_t *set;
572 set = pf->sets + pf->current_set;
579 for (i = 0; i < set->sample_count; i++)
581 pf_sample_t *sample = set->samples + i;
583 mn += sample->weight;
584 mx += sample->weight * sample->pose.v[0];
585 my += sample->weight * sample->pose.v[1];
586 mrr += sample->weight * sample->pose.v[0] * sample->pose.v[0];
587 mrr += sample->weight * sample->pose.v[1] * sample->pose.v[1];
590 mean->v[0] = mx / mn;
591 mean->v[1] = my / mn;
594 *var = mrr / mn - (mx * mx / (mn * mn) + my * my / (mn * mn));
601 int pf_get_cluster_stats(pf_t *pf,
int clabel,
double *weight,
602 pf_vector_t *mean, pf_matrix_t *cov)
604 pf_sample_set_t *set;
605 pf_cluster_t *cluster;
607 set = pf->sets + pf->current_set;
609 if (clabel >= set->cluster_count)
611 cluster = set->clusters + clabel;
613 *weight = cluster->weight;
614 *mean = cluster->mean;