00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028 #include <assert.h>
00029 #include <math.h>
00030 #include <stdlib.h>
00031 #include <time.h>
00032
00033 #include "nav2d_localizer/pf.h"
00034 #include "nav2d_localizer/pf_pdf.h"
00035 #include "nav2d_localizer/pf_kdtree.h"
00036
00037
00038
00039
00040 static int pf_resample_limit(pf_t *pf, int k);
00041
00042
00043 static void pf_cluster_stats(pf_t *pf, pf_sample_set_t *set);
00044
00045
00046
00047 pf_t *pf_alloc(int min_samples, int max_samples,
00048 double alpha_slow, double alpha_fast,
00049 pf_init_model_fn_t random_pose_fn, void *random_pose_data)
00050 {
00051 int i, j;
00052 pf_t *pf;
00053 pf_sample_set_t *set;
00054 pf_sample_t *sample;
00055
00056 srand48(time(NULL));
00057
00058 pf = calloc(1, sizeof(pf_t));
00059
00060 pf->random_pose_fn = random_pose_fn;
00061 pf->random_pose_data = random_pose_data;
00062
00063 pf->min_samples = min_samples;
00064 pf->max_samples = max_samples;
00065
00066
00067
00068
00069
00070
00071 pf->pop_err = 0.01;
00072 pf->pop_z = 3;
00073
00074 pf->current_set = 0;
00075 for (j = 0; j < 2; j++)
00076 {
00077 set = pf->sets + j;
00078
00079 set->sample_count = max_samples;
00080 set->samples = calloc(max_samples, sizeof(pf_sample_t));
00081
00082 for (i = 0; i < set->sample_count; i++)
00083 {
00084 sample = set->samples + i;
00085 sample->pose.v[0] = 0.0;
00086 sample->pose.v[1] = 0.0;
00087 sample->pose.v[2] = 0.0;
00088 sample->weight = 1.0 / max_samples;
00089 }
00090
00091
00092 set->kdtree = pf_kdtree_alloc(3 * max_samples);
00093
00094 set->cluster_count = 0;
00095 set->cluster_max_count = max_samples;
00096 set->clusters = calloc(set->cluster_max_count, sizeof(pf_cluster_t));
00097
00098 set->mean = pf_vector_zero();
00099 set->cov = pf_matrix_zero();
00100 }
00101
00102 pf->w_slow = 0.0;
00103 pf->w_fast = 0.0;
00104
00105 pf->alpha_slow = alpha_slow;
00106 pf->alpha_fast = alpha_fast;
00107
00108 return pf;
00109 }
00110
00111
00112
00113 void pf_free(pf_t *pf)
00114 {
00115 int i;
00116
00117 for (i = 0; i < 2; i++)
00118 {
00119 free(pf->sets[i].clusters);
00120 pf_kdtree_free(pf->sets[i].kdtree);
00121 free(pf->sets[i].samples);
00122 }
00123 free(pf);
00124
00125 return;
00126 }
00127
00128
00129
00130 void pf_init(pf_t *pf, pf_vector_t mean, pf_matrix_t cov)
00131 {
00132 int i;
00133 pf_sample_set_t *set;
00134 pf_sample_t *sample;
00135 pf_pdf_gaussian_t *pdf;
00136
00137 set = pf->sets + pf->current_set;
00138
00139
00140 pf_kdtree_clear(set->kdtree);
00141
00142 set->sample_count = pf->max_samples;
00143
00144 pdf = pf_pdf_gaussian_alloc(mean, cov);
00145
00146
00147 for (i = 0; i < set->sample_count; i++)
00148 {
00149 sample = set->samples + i;
00150 sample->weight = 1.0 / pf->max_samples;
00151 sample->pose = pf_pdf_gaussian_sample(pdf);
00152
00153
00154 pf_kdtree_insert(set->kdtree, sample->pose, sample->weight);
00155 }
00156
00157 pf->w_slow = pf->w_fast = 0.0;
00158
00159 pf_pdf_gaussian_free(pdf);
00160
00161
00162 pf_cluster_stats(pf, set);
00163
00164 return;
00165 }
00166
00167
00168
00169 void pf_init_model(pf_t *pf, pf_init_model_fn_t init_fn, void *init_data)
00170 {
00171 int i;
00172 pf_sample_set_t *set;
00173 pf_sample_t *sample;
00174
00175 set = pf->sets + pf->current_set;
00176
00177
00178 pf_kdtree_clear(set->kdtree);
00179
00180 set->sample_count = pf->max_samples;
00181
00182
00183 for (i = 0; i < set->sample_count; i++)
00184 {
00185 sample = set->samples + i;
00186 sample->weight = 1.0 / pf->max_samples;
00187 sample->pose = (*init_fn) (init_data);
00188
00189
00190 pf_kdtree_insert(set->kdtree, sample->pose, sample->weight);
00191 }
00192
00193 pf->w_slow = pf->w_fast = 0.0;
00194
00195
00196 pf_cluster_stats(pf, set);
00197
00198 return;
00199 }
00200
00201
00202
00203 void pf_update_action(pf_t *pf, pf_action_model_fn_t action_fn, void *action_data)
00204 {
00205 pf_sample_set_t *set;
00206
00207 set = pf->sets + pf->current_set;
00208
00209 (*action_fn) (action_data, set);
00210
00211 return;
00212 }
00213
00214
00215 #include <float.h>
00216
00217 void pf_update_sensor(pf_t *pf, pf_sensor_model_fn_t sensor_fn, void *sensor_data)
00218 {
00219 int i;
00220 pf_sample_set_t *set;
00221 pf_sample_t *sample;
00222 double total;
00223
00224 set = pf->sets + pf->current_set;
00225
00226
00227 total = (*sensor_fn) (sensor_data, set);
00228
00229 if (total > 0.0)
00230 {
00231
00232 double w_avg=0.0;
00233 for (i = 0; i < set->sample_count; i++)
00234 {
00235 sample = set->samples + i;
00236 w_avg += sample->weight;
00237 sample->weight /= total;
00238 }
00239
00240 w_avg /= set->sample_count;
00241 if(pf->w_slow == 0.0)
00242 pf->w_slow = w_avg;
00243 else
00244 pf->w_slow += pf->alpha_slow * (w_avg - pf->w_slow);
00245 if(pf->w_fast == 0.0)
00246 pf->w_fast = w_avg;
00247 else
00248 pf->w_fast += pf->alpha_fast * (w_avg - pf->w_fast);
00249
00250
00251 }
00252 else
00253 {
00254
00255
00256
00257 for (i = 0; i < set->sample_count; i++)
00258 {
00259 sample = set->samples + i;
00260 sample->weight = 1.0 / set->sample_count;
00261 }
00262 }
00263
00264 return;
00265 }
00266
00267
00268
00269 void pf_update_resample(pf_t *pf)
00270 {
00271 int i;
00272 double total;
00273 pf_sample_set_t *set_a, *set_b;
00274 pf_sample_t *sample_a, *sample_b;
00275
00276
00277
00278
00279 double* c;
00280
00281 double w_diff;
00282
00283 set_a = pf->sets + pf->current_set;
00284 set_b = pf->sets + (pf->current_set + 1) % 2;
00285
00286
00287
00288
00289 c = (double*)malloc(sizeof(double)*(set_a->sample_count+1));
00290 c[0] = 0.0;
00291 for(i=0;i<set_a->sample_count;i++)
00292 c[i+1] = c[i]+set_a->samples[i].weight;
00293
00294
00295 pf_kdtree_clear(set_b->kdtree);
00296
00297
00298 total = 0;
00299 set_b->sample_count = 0;
00300
00301 w_diff = 1.0 - pf->w_fast / pf->w_slow;
00302 if(w_diff < 0.0)
00303 w_diff = 0.0;
00304
00305
00306
00307
00308
00309
00310
00311
00312
00313
00314
00315
00316 while(set_b->sample_count < pf->max_samples)
00317 {
00318 sample_b = set_b->samples + set_b->sample_count++;
00319
00320 if(drand48() < w_diff)
00321 sample_b->pose = (pf->random_pose_fn)(pf->random_pose_data);
00322 else
00323 {
00324
00325
00326
00327
00328
00329
00330
00331
00332
00333
00334
00335
00336
00337
00338
00339
00340
00341
00342
00343
00344
00345
00346
00347
00348
00349 double r;
00350 r = drand48();
00351 for(i=0;i<set_a->sample_count;i++)
00352 {
00353 if((c[i] <= r) && (r < c[i+1]))
00354 break;
00355 }
00356 assert(i<set_a->sample_count);
00357
00358 sample_a = set_a->samples + i;
00359
00360 assert(sample_a->weight > 0);
00361
00362
00363 sample_b->pose = sample_a->pose;
00364 }
00365
00366 sample_b->weight = 1.0;
00367 total += sample_b->weight;
00368
00369
00370 pf_kdtree_insert(set_b->kdtree, sample_b->pose, sample_b->weight);
00371
00372
00373 if (set_b->sample_count > pf_resample_limit(pf, set_b->kdtree->leaf_count))
00374 break;
00375 }
00376
00377
00378 if(w_diff > 0.0)
00379 pf->w_slow = pf->w_fast = 0.0;
00380
00381
00382
00383
00384 for (i = 0; i < set_b->sample_count; i++)
00385 {
00386 sample_b = set_b->samples + i;
00387 sample_b->weight /= total;
00388 }
00389
00390
00391 pf_cluster_stats(pf, set_b);
00392
00393
00394 pf->current_set = (pf->current_set + 1) % 2;
00395
00396 free(c);
00397 return;
00398 }
00399
00400
00401
00402
00403 int pf_resample_limit(pf_t *pf, int k)
00404 {
00405 double a, b, c, x;
00406 int n;
00407
00408 if (k <= 1)
00409 return pf->max_samples;
00410
00411 a = 1;
00412 b = 2 / (9 * ((double) k - 1));
00413 c = sqrt(2 / (9 * ((double) k - 1))) * pf->pop_z;
00414 x = a - b + c;
00415
00416 n = (int) ceil((k - 1) / (2 * pf->pop_err) * x * x * x);
00417
00418 if (n < pf->min_samples)
00419 return pf->min_samples;
00420 if (n > pf->max_samples)
00421 return pf->max_samples;
00422
00423 return n;
00424 }
00425
00426
00427
00428 void pf_cluster_stats(pf_t *pf, pf_sample_set_t *set)
00429 {
00430 int i, j, k, cidx;
00431 pf_sample_t *sample;
00432 pf_cluster_t *cluster;
00433
00434
00435 double m[4], c[2][2];
00436 size_t count;
00437 double weight;
00438
00439
00440 pf_kdtree_cluster(set->kdtree);
00441
00442
00443 set->cluster_count = 0;
00444
00445 for (i = 0; i < set->cluster_max_count; i++)
00446 {
00447 cluster = set->clusters + i;
00448 cluster->count = 0;
00449 cluster->weight = 0;
00450 cluster->mean = pf_vector_zero();
00451 cluster->cov = pf_matrix_zero();
00452
00453 for (j = 0; j < 4; j++)
00454 cluster->m[j] = 0.0;
00455 for (j = 0; j < 2; j++)
00456 for (k = 0; k < 2; k++)
00457 cluster->c[j][k] = 0.0;
00458 }
00459
00460
00461 count = 0;
00462 weight = 0.0;
00463 set->mean = pf_vector_zero();
00464 set->cov = pf_matrix_zero();
00465 for (j = 0; j < 4; j++)
00466 m[j] = 0.0;
00467 for (j = 0; j < 2; j++)
00468 for (k = 0; k < 2; k++)
00469 c[j][k] = 0.0;
00470
00471
00472 for (i = 0; i < set->sample_count; i++)
00473 {
00474 sample = set->samples + i;
00475
00476
00477
00478
00479 cidx = pf_kdtree_get_cluster(set->kdtree, sample->pose);
00480 assert(cidx >= 0);
00481 if (cidx >= set->cluster_max_count)
00482 continue;
00483 if (cidx + 1 > set->cluster_count)
00484 set->cluster_count = cidx + 1;
00485
00486 cluster = set->clusters + cidx;
00487
00488 cluster->count += 1;
00489 cluster->weight += sample->weight;
00490
00491 count += 1;
00492 weight += sample->weight;
00493
00494
00495 cluster->m[0] += sample->weight * sample->pose.v[0];
00496 cluster->m[1] += sample->weight * sample->pose.v[1];
00497 cluster->m[2] += sample->weight * cos(sample->pose.v[2]);
00498 cluster->m[3] += sample->weight * sin(sample->pose.v[2]);
00499
00500 m[0] += sample->weight * sample->pose.v[0];
00501 m[1] += sample->weight * sample->pose.v[1];
00502 m[2] += sample->weight * cos(sample->pose.v[2]);
00503 m[3] += sample->weight * sin(sample->pose.v[2]);
00504
00505
00506 for (j = 0; j < 2; j++)
00507 for (k = 0; k < 2; k++)
00508 {
00509 cluster->c[j][k] += sample->weight * sample->pose.v[j] * sample->pose.v[k];
00510 c[j][k] += sample->weight * sample->pose.v[j] * sample->pose.v[k];
00511 }
00512 }
00513
00514
00515 for (i = 0; i < set->cluster_count; i++)
00516 {
00517 cluster = set->clusters + i;
00518
00519 cluster->mean.v[0] = cluster->m[0] / cluster->weight;
00520 cluster->mean.v[1] = cluster->m[1] / cluster->weight;
00521 cluster->mean.v[2] = atan2(cluster->m[3], cluster->m[2]);
00522
00523 cluster->cov = pf_matrix_zero();
00524
00525
00526 for (j = 0; j < 2; j++)
00527 for (k = 0; k < 2; k++)
00528 cluster->cov.m[j][k] = cluster->c[j][k] / cluster->weight -
00529 cluster->mean.v[j] * cluster->mean.v[k];
00530
00531
00532
00533 cluster->cov.m[2][2] = -2 * log(sqrt(cluster->m[2] * cluster->m[2] +
00534 cluster->m[3] * cluster->m[3]));
00535
00536
00537
00538
00539 }
00540
00541
00542 set->mean.v[0] = m[0] / weight;
00543 set->mean.v[1] = m[1] / weight;
00544 set->mean.v[2] = atan2(m[3], m[2]);
00545
00546
00547 for (j = 0; j < 2; j++)
00548 for (k = 0; k < 2; k++)
00549 set->cov.m[j][k] = c[j][k] / weight - set->mean.v[j] * set->mean.v[k];
00550
00551
00552
00553 set->cov.m[2][2] = -2 * log(sqrt(m[2] * m[2] + m[3] * m[3]));
00554
00555 return;
00556 }
00557
00558
00559
00560 void pf_get_cep_stats(pf_t *pf, pf_vector_t *mean, double *var)
00561 {
00562 int i;
00563 double mn, mx, my, mrr;
00564 pf_sample_set_t *set;
00565 pf_sample_t *sample;
00566
00567 set = pf->sets + pf->current_set;
00568
00569 mn = 0.0;
00570 mx = 0.0;
00571 my = 0.0;
00572 mrr = 0.0;
00573
00574 for (i = 0; i < set->sample_count; i++)
00575 {
00576 sample = set->samples + i;
00577
00578 mn += sample->weight;
00579 mx += sample->weight * sample->pose.v[0];
00580 my += sample->weight * sample->pose.v[1];
00581 mrr += sample->weight * sample->pose.v[0] * sample->pose.v[0];
00582 mrr += sample->weight * sample->pose.v[1] * sample->pose.v[1];
00583 }
00584
00585 mean->v[0] = mx / mn;
00586 mean->v[1] = my / mn;
00587 mean->v[2] = 0.0;
00588
00589 *var = mrr / mn - (mx * mx / (mn * mn) + my * my / (mn * mn));
00590
00591 return;
00592 }
00593
00594
00595
00596 int pf_get_cluster_stats(pf_t *pf, int clabel, double *weight,
00597 pf_vector_t *mean, pf_matrix_t *cov)
00598 {
00599 pf_sample_set_t *set;
00600 pf_cluster_t *cluster;
00601
00602 set = pf->sets + pf->current_set;
00603
00604 if (clabel >= set->cluster_count)
00605 return 0;
00606 cluster = set->clusters + clabel;
00607
00608 *weight = cluster->weight;
00609 *mean = cluster->mean;
00610 *cov = cluster->cov;
00611
00612 return 1;
00613 }
00614
00615