pf.c
Go to the documentation of this file.
1 /*
2  * Player - One Hell of a Robot Server
3  * Copyright (C) 2000 Brian Gerkey & Kasper Stoy
4  * gerkey@usc.edu kaspers@robotics.usc.edu
5  *
6  * This library is free software; you can redistribute it and/or
7  * modify it under the terms of the GNU Lesser General Public
8  * License as published by the Free Software Foundation; either
9  * version 2.1 of the License, or (at your option) any later version.
10  *
11  * This library is distributed in the hope that it will be useful,
12  * but WITHOUT ANY WARRANTY; without even the implied warranty of
13  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
14  * Lesser General Public License for more details.
15  *
16  * You should have received a copy of the GNU Lesser General Public
17  * License along with this library; if not, write to the Free Software
18  * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
19  *
20  */
21 /**************************************************************************
22  * Desc: Simple particle filter for localization.
23  * Author: Andrew Howard
24  * Date: 10 Dec 2002
25  * CVS: $Id: pf.c 6345 2008-04-17 01:36:39Z gerkey $
26  *************************************************************************/
27 
28 #include <assert.h>
29 #include <math.h>
30 #include <stdlib.h>
31 #include <time.h>
32 
33 #include "nav2d_localizer/pf.h"
34 #include "nav2d_localizer/pf_pdf.h"
36 
37 
38 // Compute the required number of samples, given that there are k bins
39 // with samples in them.
40 static int pf_resample_limit(pf_t *pf, int k);
41 
42 // Re-compute the cluster statistics for a sample set
43 static void pf_cluster_stats(pf_t *pf, pf_sample_set_t *set);
44 
45 
46 // Create a new filter
47 pf_t *pf_alloc(int min_samples, int max_samples,
48  double alpha_slow, double alpha_fast,
49  pf_init_model_fn_t random_pose_fn, void *random_pose_data)
50 {
51  int i, j;
52  pf_t *pf;
53  pf_sample_set_t *set;
54  pf_sample_t *sample;
55 
56  srand48(time(NULL));
57 
58  pf = calloc(1, sizeof(pf_t));
59 
60  pf->random_pose_fn = random_pose_fn;
61  pf->random_pose_data = random_pose_data;
62 
63  pf->min_samples = min_samples;
64  pf->max_samples = max_samples;
65 
66  // Control parameters for the population size calculation. [err] is
67  // the max error between the true distribution and the estimated
68  // distribution. [z] is the upper standard normal quantile for (1 -
69  // p), where p is the probability that the error on the estimated
70  // distrubition will be less than [err].
71  pf->pop_err = 0.01;
72  pf->pop_z = 3;
73 
74  pf->current_set = 0;
75  for (j = 0; j < 2; j++)
76  {
77  set = pf->sets + j;
78 
79  set->sample_count = max_samples;
80  set->samples = calloc(max_samples, sizeof(pf_sample_t));
81 
82  for (i = 0; i < set->sample_count; i++)
83  {
84  sample = set->samples + i;
85  sample->pose.v[0] = 0.0;
86  sample->pose.v[1] = 0.0;
87  sample->pose.v[2] = 0.0;
88  sample->weight = 1.0 / max_samples;
89  }
90 
91  // HACK: is 3 times max_samples enough?
92  set->kdtree = pf_kdtree_alloc(3 * max_samples);
93 
94  set->cluster_count = 0;
95  set->cluster_max_count = max_samples;
96  set->clusters = calloc(set->cluster_max_count, sizeof(pf_cluster_t));
97 
98  set->mean = pf_vector_zero();
99  set->cov = pf_matrix_zero();
100  }
101 
102  pf->w_slow = 0.0;
103  pf->w_fast = 0.0;
104 
105  pf->alpha_slow = alpha_slow;
106  pf->alpha_fast = alpha_fast;
107 
108  return pf;
109 }
110 
111 
112 // Free an existing filter
113 void pf_free(pf_t *pf)
114 {
115  int i;
116 
117  for (i = 0; i < 2; i++)
118  {
119  free(pf->sets[i].clusters);
120  pf_kdtree_free(pf->sets[i].kdtree);
121  free(pf->sets[i].samples);
122  }
123  free(pf);
124 
125  return;
126 }
127 
128 
129 // Initialize the filter using a guassian
130 void pf_init(pf_t *pf, pf_vector_t mean, pf_matrix_t cov)
131 {
132  int i;
133  pf_sample_set_t *set;
134  pf_sample_t *sample;
135  pf_pdf_gaussian_t *pdf;
136 
137  set = pf->sets + pf->current_set;
138 
139  // Create the kd tree for adaptive sampling
140  pf_kdtree_clear(set->kdtree);
141 
142  set->sample_count = pf->max_samples;
143 
144  pdf = pf_pdf_gaussian_alloc(mean, cov);
145 
146  // Compute the new sample poses
147  for (i = 0; i < set->sample_count; i++)
148  {
149  sample = set->samples + i;
150  sample->weight = 1.0 / pf->max_samples;
151  sample->pose = pf_pdf_gaussian_sample(pdf);
152 
153  // Add sample to histogram
154  pf_kdtree_insert(set->kdtree, sample->pose, sample->weight);
155  }
156 
157  pf->w_slow = pf->w_fast = 0.0;
158 
160 
161  // Re-compute cluster statistics
162  pf_cluster_stats(pf, set);
163 
164  return;
165 }
166 
167 
168 // Initialize the filter using some model
169 void pf_init_model(pf_t *pf, pf_init_model_fn_t init_fn, void *init_data)
170 {
171  int i;
172  pf_sample_set_t *set;
173  pf_sample_t *sample;
174 
175  set = pf->sets + pf->current_set;
176 
177  // Create the kd tree for adaptive sampling
178  pf_kdtree_clear(set->kdtree);
179 
180  set->sample_count = pf->max_samples;
181 
182  // Compute the new sample poses
183  for (i = 0; i < set->sample_count; i++)
184  {
185  sample = set->samples + i;
186  sample->weight = 1.0 / pf->max_samples;
187  sample->pose = (*init_fn) (init_data);
188 
189  // Add sample to histogram
190  pf_kdtree_insert(set->kdtree, sample->pose, sample->weight);
191  }
192 
193  pf->w_slow = pf->w_fast = 0.0;
194 
195  // Re-compute cluster statistics
196  pf_cluster_stats(pf, set);
197 
198  return;
199 }
200 
201 
202 // Update the filter with some new action
203 void pf_update_action(pf_t *pf, pf_action_model_fn_t action_fn, void *action_data)
204 {
205  pf_sample_set_t *set;
206 
207  set = pf->sets + pf->current_set;
208 
209  (*action_fn) (action_data, set);
210 
211  return;
212 }
213 
214 
215 #include <float.h>
216 // Update the filter with some new sensor observation
217 void pf_update_sensor(pf_t *pf, pf_sensor_model_fn_t sensor_fn, void *sensor_data)
218 {
219  int i;
220  pf_sample_set_t *set;
221  pf_sample_t *sample;
222  double total;
223 
224  set = pf->sets + pf->current_set;
225 
226  // Compute the sample weights
227  total = (*sensor_fn) (sensor_data, set);
228 
229  if (total > 0.0)
230  {
231  // Normalize weights
232  double w_avg=0.0;
233  for (i = 0; i < set->sample_count; i++)
234  {
235  sample = set->samples + i;
236  w_avg += sample->weight;
237  sample->weight /= total;
238  }
239  // Update running averages of likelihood of samples (Prob Rob p258)
240  w_avg /= set->sample_count;
241  if(pf->w_slow == 0.0)
242  pf->w_slow = w_avg;
243  else
244  pf->w_slow += pf->alpha_slow * (w_avg - pf->w_slow);
245  if(pf->w_fast == 0.0)
246  pf->w_fast = w_avg;
247  else
248  pf->w_fast += pf->alpha_fast * (w_avg - pf->w_fast);
249  //printf("w_avg: %e slow: %e fast: %e\n",
250  //w_avg, pf->w_slow, pf->w_fast);
251  }
252  else
253  {
254  //PLAYER_WARN("pdf has zero probability");
255 
256  // Handle zero total
257  for (i = 0; i < set->sample_count; i++)
258  {
259  sample = set->samples + i;
260  sample->weight = 1.0 / set->sample_count;
261  }
262  }
263 
264  return;
265 }
266 
267 
268 // Resample the distribution
270 {
271  int i;
272  double total;
273  pf_sample_set_t *set_a, *set_b;
274  pf_sample_t *sample_a, *sample_b;
275 
276  //double r,c,U;
277  //int m;
278  //double count_inv;
279  double* c;
280 
281  double w_diff;
282 
283  set_a = pf->sets + pf->current_set;
284  set_b = pf->sets + (pf->current_set + 1) % 2;
285 
286  // Build up cumulative probability table for resampling.
287  // TODO: Replace this with a more efficient procedure
288  // (e.g., http://www.network-theory.co.uk/docs/gslref/GeneralDiscreteDistributions.html)
289  c = (double*)malloc(sizeof(double)*(set_a->sample_count+1));
290  c[0] = 0.0;
291  for(i=0;i<set_a->sample_count;i++)
292  c[i+1] = c[i]+set_a->samples[i].weight;
293 
294  // Create the kd tree for adaptive sampling
295  pf_kdtree_clear(set_b->kdtree);
296 
297  // Draw samples from set a to create set b.
298  total = 0;
299  set_b->sample_count = 0;
300 
301  w_diff = 1.0 - pf->w_fast / pf->w_slow;
302  if(w_diff < 0.0)
303  w_diff = 0.0;
304  //printf("w_diff: %9.6f\n", w_diff);
305 
306  // Can't (easily) combine low-variance sampler with KLD adaptive
307  // sampling, so we'll take the more traditional route.
308  /*
309  // Low-variance resampler, taken from Probabilistic Robotics, p110
310  count_inv = 1.0/set_a->sample_count;
311  r = drand48() * count_inv;
312  c = set_a->samples[0].weight;
313  i = 0;
314  m = 0;
315  */
316  while(set_b->sample_count < pf->max_samples)
317  {
318  sample_b = set_b->samples + set_b->sample_count++;
319 
320  if(drand48() < w_diff)
321  sample_b->pose = (pf->random_pose_fn)(pf->random_pose_data);
322  else
323  {
324  // Can't (easily) combine low-variance sampler with KLD adaptive
325  // sampling, so we'll take the more traditional route.
326  /*
327  // Low-variance resampler, taken from Probabilistic Robotics, p110
328  U = r + m * count_inv;
329  while(U>c)
330  {
331  i++;
332  // Handle wrap-around by resetting counters and picking a new random
333  // number
334  if(i >= set_a->sample_count)
335  {
336  r = drand48() * count_inv;
337  c = set_a->samples[0].weight;
338  i = 0;
339  m = 0;
340  U = r + m * count_inv;
341  continue;
342  }
343  c += set_a->samples[i].weight;
344  }
345  m++;
346  */
347 
348  // Naive discrete event sampler
349  double r;
350  r = drand48();
351  for(i=0;i<set_a->sample_count;i++)
352  {
353  if((c[i] <= r) && (r < c[i+1]))
354  break;
355  }
356  assert(i<set_a->sample_count);
357 
358  sample_a = set_a->samples + i;
359 
360  assert(sample_a->weight > 0);
361 
362  // Add sample to list
363  sample_b->pose = sample_a->pose;
364  }
365 
366  sample_b->weight = 1.0;
367  total += sample_b->weight;
368 
369  // Add sample to histogram
370  pf_kdtree_insert(set_b->kdtree, sample_b->pose, sample_b->weight);
371 
372  // See if we have enough samples yet
373  if (set_b->sample_count > pf_resample_limit(pf, set_b->kdtree->leaf_count))
374  break;
375  }
376 
377  // Reset averages, to avoid spiraling off into complete randomness.
378  if(w_diff > 0.0)
379  pf->w_slow = pf->w_fast = 0.0;
380 
381  //fprintf(stderr, "\n\n");
382 
383  // Normalize weights
384  for (i = 0; i < set_b->sample_count; i++)
385  {
386  sample_b = set_b->samples + i;
387  sample_b->weight /= total;
388  }
389 
390  // Re-compute cluster statistics
391  pf_cluster_stats(pf, set_b);
392 
393  // Use the newly created sample set
394  pf->current_set = (pf->current_set + 1) % 2;
395 
396  free(c);
397  return;
398 }
399 
400 
401 // Compute the required number of samples, given that there are k bins
402 // with samples in them. This is taken directly from Fox et al.
403 int pf_resample_limit(pf_t *pf, int k)
404 {
405  double a, b, c, x;
406  int n;
407 
408  if (k <= 1)
409  return pf->max_samples;
410 
411  a = 1;
412  b = 2 / (9 * ((double) k - 1));
413  c = sqrt(2 / (9 * ((double) k - 1))) * pf->pop_z;
414  x = a - b + c;
415 
416  n = (int) ceil((k - 1) / (2 * pf->pop_err) * x * x * x);
417 
418  if (n < pf->min_samples)
419  return pf->min_samples;
420  if (n > pf->max_samples)
421  return pf->max_samples;
422 
423  return n;
424 }
425 
426 
427 // Re-compute the cluster statistics for a sample set
429 {
430  int i, j, k, cidx;
431  pf_sample_t *sample;
432  pf_cluster_t *cluster;
433 
434  // Workspace
435  double m[4], c[2][2];
436  size_t count;
437  double weight;
438 
439  // Cluster the samples
440  pf_kdtree_cluster(set->kdtree);
441 
442  // Initialize cluster stats
443  set->cluster_count = 0;
444 
445  for (i = 0; i < set->cluster_max_count; i++)
446  {
447  cluster = set->clusters + i;
448  cluster->count = 0;
449  cluster->weight = 0;
450  cluster->mean = pf_vector_zero();
451  cluster->cov = pf_matrix_zero();
452 
453  for (j = 0; j < 4; j++)
454  cluster->m[j] = 0.0;
455  for (j = 0; j < 2; j++)
456  for (k = 0; k < 2; k++)
457  cluster->c[j][k] = 0.0;
458  }
459 
460  // Initialize overall filter stats
461  count = 0;
462  weight = 0.0;
463  set->mean = pf_vector_zero();
464  set->cov = pf_matrix_zero();
465  for (j = 0; j < 4; j++)
466  m[j] = 0.0;
467  for (j = 0; j < 2; j++)
468  for (k = 0; k < 2; k++)
469  c[j][k] = 0.0;
470 
471  // Compute cluster stats
472  for (i = 0; i < set->sample_count; i++)
473  {
474  sample = set->samples + i;
475 
476  //printf("%d %f %f %f\n", i, sample->pose.v[0], sample->pose.v[1], sample->pose.v[2]);
477 
478  // Get the cluster label for this sample
479  cidx = pf_kdtree_get_cluster(set->kdtree, sample->pose);
480  assert(cidx >= 0);
481  if (cidx >= set->cluster_max_count)
482  continue;
483  if (cidx + 1 > set->cluster_count)
484  set->cluster_count = cidx + 1;
485 
486  cluster = set->clusters + cidx;
487 
488  cluster->count += 1;
489  cluster->weight += sample->weight;
490 
491  count += 1;
492  weight += sample->weight;
493 
494  // Compute mean
495  cluster->m[0] += sample->weight * sample->pose.v[0];
496  cluster->m[1] += sample->weight * sample->pose.v[1];
497  cluster->m[2] += sample->weight * cos(sample->pose.v[2]);
498  cluster->m[3] += sample->weight * sin(sample->pose.v[2]);
499 
500  m[0] += sample->weight * sample->pose.v[0];
501  m[1] += sample->weight * sample->pose.v[1];
502  m[2] += sample->weight * cos(sample->pose.v[2]);
503  m[3] += sample->weight * sin(sample->pose.v[2]);
504 
505  // Compute covariance in linear components
506  for (j = 0; j < 2; j++)
507  for (k = 0; k < 2; k++)
508  {
509  cluster->c[j][k] += sample->weight * sample->pose.v[j] * sample->pose.v[k];
510  c[j][k] += sample->weight * sample->pose.v[j] * sample->pose.v[k];
511  }
512  }
513 
514  // Normalize
515  for (i = 0; i < set->cluster_count; i++)
516  {
517  cluster = set->clusters + i;
518 
519  cluster->mean.v[0] = cluster->m[0] / cluster->weight;
520  cluster->mean.v[1] = cluster->m[1] / cluster->weight;
521  cluster->mean.v[2] = atan2(cluster->m[3], cluster->m[2]);
522 
523  cluster->cov = pf_matrix_zero();
524 
525  // Covariance in linear components
526  for (j = 0; j < 2; j++)
527  for (k = 0; k < 2; k++)
528  cluster->cov.m[j][k] = cluster->c[j][k] / cluster->weight -
529  cluster->mean.v[j] * cluster->mean.v[k];
530 
531  // Covariance in angular components; I think this is the correct
532  // formula for circular statistics.
533  cluster->cov.m[2][2] = -2 * log(sqrt(cluster->m[2] * cluster->m[2] +
534  cluster->m[3] * cluster->m[3]));
535 
536  //printf("cluster %d %d %f (%f %f %f)\n", i, cluster->count, cluster->weight,
537  //cluster->mean.v[0], cluster->mean.v[1], cluster->mean.v[2]);
538  //pf_matrix_fprintf(cluster->cov, stdout, "%e");
539  }
540 
541  // Compute overall filter stats
542  set->mean.v[0] = m[0] / weight;
543  set->mean.v[1] = m[1] / weight;
544  set->mean.v[2] = atan2(m[3], m[2]);
545 
546  // Covariance in linear components
547  for (j = 0; j < 2; j++)
548  for (k = 0; k < 2; k++)
549  set->cov.m[j][k] = c[j][k] / weight - set->mean.v[j] * set->mean.v[k];
550 
551  // Covariance in angular components; I think this is the correct
552  // formula for circular statistics.
553  set->cov.m[2][2] = -2 * log(sqrt(m[2] * m[2] + m[3] * m[3]));
554 
555  return;
556 }
557 
558 
559 // Compute the CEP statistics (mean and variance).
560 void pf_get_cep_stats(pf_t *pf, pf_vector_t *mean, double *var)
561 {
562  int i;
563  double mn, mx, my, mrr;
564  pf_sample_set_t *set;
565  pf_sample_t *sample;
566 
567  set = pf->sets + pf->current_set;
568 
569  mn = 0.0;
570  mx = 0.0;
571  my = 0.0;
572  mrr = 0.0;
573 
574  for (i = 0; i < set->sample_count; i++)
575  {
576  sample = set->samples + i;
577 
578  mn += sample->weight;
579  mx += sample->weight * sample->pose.v[0];
580  my += sample->weight * sample->pose.v[1];
581  mrr += sample->weight * sample->pose.v[0] * sample->pose.v[0];
582  mrr += sample->weight * sample->pose.v[1] * sample->pose.v[1];
583  }
584 
585  mean->v[0] = mx / mn;
586  mean->v[1] = my / mn;
587  mean->v[2] = 0.0;
588 
589  *var = mrr / mn - (mx * mx / (mn * mn) + my * my / (mn * mn));
590 
591  return;
592 }
593 
594 
595 // Get the statistics for a particular cluster.
596 int pf_get_cluster_stats(pf_t *pf, int clabel, double *weight,
597  pf_vector_t *mean, pf_matrix_t *cov)
598 {
599  pf_sample_set_t *set;
600  pf_cluster_t *cluster;
601 
602  set = pf->sets + pf->current_set;
603 
604  if (clabel >= set->cluster_count)
605  return 0;
606  cluster = set->clusters + clabel;
607 
608  *weight = cluster->weight;
609  *mean = cluster->mean;
610  *cov = cluster->cov;
611 
612  return 1;
613 }
614 
615 
double w_slow
Definition: pf.h:125
pf_init_model_fn_t random_pose_fn
Definition: pf.h:131
double(* pf_sensor_model_fn_t)(void *sensor_data, struct _pf_sample_set_t *set)
Definition: pf.h:54
static int pf_resample_limit(pf_t *pf, int k)
Definition: pf.c:403
void pf_update_sensor(pf_t *pf, pf_sensor_model_fn_t sensor_fn, void *sensor_data)
Definition: pf.c:217
double pop_err
Definition: pf.h:117
void pf_get_cep_stats(pf_t *pf, pf_vector_t *mean, double *var)
Definition: pf.c:560
double v[3]
Definition: pf_vector.h:40
void pf_kdtree_clear(pf_kdtree_t *self)
Definition: pf_kdtree.c:100
void pf_kdtree_insert(pf_kdtree_t *self, pf_vector_t pose, double value)
Definition: pf_kdtree.c:112
Definition: pf.h:111
pf_vector_t(* pf_init_model_fn_t)(void *init_data)
Definition: pf.h:45
pf_matrix_t cov
Definition: pf.h:81
pf_vector_t pf_vector_zero()
Definition: pf_vector.c:38
pf_kdtree_t * kdtree
Definition: pf.h:97
static int n
Definition: eig3.c:12
void pf_update_resample(pf_t *pf)
Definition: pf.c:269
pf_t * pf_alloc(int min_samples, int max_samples, double alpha_slow, double alpha_fast, pf_init_model_fn_t random_pose_fn, void *random_pose_data)
Definition: pf.c:47
void pf_init_model(pf_t *pf, pf_init_model_fn_t init_fn, void *init_data)
Definition: pf.c:169
void pf_kdtree_free(pf_kdtree_t *self)
Definition: pf_kdtree.c:90
ROSCPP_DECL void set(const std::string &key, const XmlRpc::XmlRpcValue &v)
int max_samples
Definition: pf.h:114
double weight
Definition: pf.h:77
double w_fast
Definition: pf.h:125
int min_samples
Definition: pf.h:114
double pop_z
Definition: pf.h:117
void pf_pdf_gaussian_free(pf_pdf_gaussian_t *pdf)
Definition: pf_pdf.c:74
int current_set
Definition: pf.h:121
Definition: pf.h:59
void pf_update_action(pf_t *pf, pf_action_model_fn_t action_fn, void *action_data)
Definition: pf.c:203
void * random_pose_data
Definition: pf.h:132
double m[4]
Definition: pf.h:84
double m[3][3]
Definition: pf_vector.h:47
pf_sample_t * samples
Definition: pf.h:94
pf_matrix_t pf_matrix_zero()
Definition: pf_vector.c:134
TFSIMD_FORCE_INLINE const tfScalar & x() const
int leaf_count
Definition: pf_kdtree.h:75
pf_pdf_gaussian_t * pf_pdf_gaussian_alloc(pf_vector_t x, pf_matrix_t cx)
Definition: pf_pdf.c:46
static void pf_cluster_stats(pf_t *pf, pf_sample_set_t *set)
Definition: pf.c:428
void pf_free(pf_t *pf)
Definition: pf.c:113
double alpha_fast
Definition: pf.h:128
pf_vector_t pose
Definition: pf.h:62
void(* pf_action_model_fn_t)(void *action_data, struct _pf_sample_set_t *set)
Definition: pf.h:49
int pf_get_cluster_stats(pf_t *pf, int clabel, double *weight, pf_vector_t *mean, pf_matrix_t *cov)
Definition: pf.c:596
int sample_count
Definition: pf.h:93
double weight
Definition: pf.h:65
void pf_init(pf_t *pf, pf_vector_t mean, pf_matrix_t cov)
Definition: pf.c:130
pf_vector_t mean
Definition: pf.h:80
pf_kdtree_t * pf_kdtree_alloc(int max_size)
Definition: pf_kdtree.c:66
pf_cluster_t * clusters
Definition: pf.h:101
pf_sample_set_t sets[2]
Definition: pf.h:122
int pf_kdtree_get_cluster(pf_kdtree_t *self, pf_vector_t pose)
Definition: pf_kdtree.c:168
double alpha_slow
Definition: pf.h:128
double c[2][2]
Definition: pf.h:84
void pf_kdtree_cluster(pf_kdtree_t *self)
Definition: pf_kdtree.c:358
pf_vector_t pf_pdf_gaussian_sample(pf_pdf_gaussian_t *pdf)
Definition: pf_pdf.c:105
int count
Definition: pf.h:74


nav2d_localizer
Author(s): Sebastian Kasperski
autogenerated on Tue Nov 7 2017 06:02:33