pf_pdf.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: Useful pdf functions
23  * Author: Andrew Howard
24  * Date: 10 Dec 2002
25  * CVS: $Id: pf_pdf.c 6348 2008-04-17 02:53:17Z gerkey $
26  *************************************************************************/
27 
28 #include <assert.h>
29 #include <math.h>
30 #include <stdlib.h>
31 #include <string.h>
32 //#include <gsl/gsl_rng.h>
33 //#include <gsl/gsl_randist.h>
34 
35 #include "nav2d_localizer/pf_pdf.h"
36 
37 // Random number generator seed value
38 static unsigned int pf_pdf_seed;
39 
40 
41 /**************************************************************************
42  * Gaussian
43  *************************************************************************/
44 
45 // Create a gaussian pdf
47 {
48  pf_matrix_t cd;
49  pf_pdf_gaussian_t *pdf;
50 
51  pdf = calloc(1, sizeof(pf_pdf_gaussian_t));
52 
53  pdf->x = x;
54  pdf->cx = cx;
55  //pdf->cxi = pf_matrix_inverse(cx, &pdf->cxdet);
56 
57  // Decompose the convariance matrix into a rotation
58  // matrix and a diagonal matrix.
59  pf_matrix_unitary(&pdf->cr, &cd, pdf->cx);
60  pdf->cd.v[0] = sqrt(cd.m[0][0]);
61  pdf->cd.v[1] = sqrt(cd.m[1][1]);
62  pdf->cd.v[2] = sqrt(cd.m[2][2]);
63 
64  // Initialize the random number generator
65  //pdf->rng = gsl_rng_alloc(gsl_rng_taus);
66  //gsl_rng_set(pdf->rng, ++pf_pdf_seed);
67  srand48(++pf_pdf_seed);
68 
69  return pdf;
70 }
71 
72 
73 // Destroy the pdf
75 {
76  //gsl_rng_free(pdf->rng);
77  free(pdf);
78  return;
79 }
80 
81 
82 /*
83 // Compute the value of the pdf at some point [x].
84 double pf_pdf_gaussian_value(pf_pdf_gaussian_t *pdf, pf_vector_t x)
85 {
86  int i, j;
87  pf_vector_t z;
88  double zz, p;
89 
90  z = pf_vector_sub(x, pdf->x);
91 
92  zz = 0;
93  for (i = 0; i < 3; i++)
94  for (j = 0; j < 3; j++)
95  zz += z.v[i] * pdf->cxi.m[i][j] * z.v[j];
96 
97  p = 1 / (2 * M_PI * pdf->cxdet) * exp(-zz / 2);
98 
99  return p;
100 }
101 */
102 
103 
104 // Generate a sample from the the pdf.
106 {
107  int i, j;
108  pf_vector_t r;
109  pf_vector_t x;
110 
111  // Generate a random vector
112  for (i = 0; i < 3; i++)
113  {
114  //r.v[i] = gsl_ran_gaussian(pdf->rng, pdf->cd.v[i]);
115  r.v[i] = pf_ran_gaussian(pdf->cd.v[i]);
116  }
117 
118  for (i = 0; i < 3; i++)
119  {
120  x.v[i] = pdf->x.v[i];
121  for (j = 0; j < 3; j++)
122  x.v[i] += pdf->cr.m[i][j] * r.v[j];
123  }
124 
125  return x;
126 }
127 
128 // Draw randomly from a zero-mean Gaussian distribution, with standard
129 // deviation sigma.
130 // We use the polar form of the Box-Muller transformation, explained here:
131 // http://www.taygeta.com/random/gaussian.html
132 double pf_ran_gaussian(double sigma)
133 {
134  double x1, x2, w, r;
135 
136  do
137  {
138  do { r = drand48(); } while (r==0.0);
139  x1 = 2.0 * r - 1.0;
140  do { r = drand48(); } while (r==0.0);
141  x2 = 2.0 * r - 1.0;
142  w = x1*x1 + x2*x2;
143  } while(w > 1.0 || w==0.0);
144 
145  return(sigma * x2 * sqrt(-2.0*log(w)/w));
146 }
147 
148 #if 0
149 
150 /**************************************************************************
151  * Discrete
152  * Note that GSL v1.3 and earlier contains a bug in the discrete
153  * random generator. A patched version of the the generator is included
154  * in gsl_discrete.c.
155  *************************************************************************/
156 
157 
158 // Create a discrete pdf
159 pf_pdf_discrete_t *pf_pdf_discrete_alloc(int count, double *probs)
160 {
161  pf_pdf_discrete_t *pdf;
162 
163  pdf = calloc(1, sizeof(pf_pdf_discrete_t));
164 
165  pdf->prob_count = count;
166  pdf->probs = malloc(count * sizeof(double));
167  memcpy(pdf->probs, probs, count * sizeof(double));
168 
169  // Initialize the random number generator
170  pdf->rng = gsl_rng_alloc(gsl_rng_taus);
171  gsl_rng_set(pdf->rng, ++pf_pdf_seed);
172 
173  // Initialize the discrete distribution generator
174  pdf->ran = gsl_ran_discrete_preproc(count, probs);
175 
176  return pdf;
177 }
178 
179 
180 // Destroy the pdf
181 void pf_pdf_discrete_free(pf_pdf_discrete_t *pdf)
182 {
183  gsl_ran_discrete_free(pdf->ran);
184  gsl_rng_free(pdf->rng);
185  free(pdf->probs);
186  free(pdf);
187  return;
188 }
189 
190 
191 // Compute the value of the probability of some element [i]
192 double pf_pdf_discrete_value(pf_pdf_discrete_t *pdf, int i)
193 {
194  return pdf->probs[i];
195 }
196 
197 
198 // Generate a sample from the the pdf.
199 int pf_pdf_discrete_sample(pf_pdf_discrete_t *pdf)
200 {
201  int i;
202 
203  i = gsl_ran_discrete(pdf->rng, pdf->ran);
204  assert(i >= 0 && i < pdf->prob_count);
205 
206  return i;
207 }
208 
209 #endif
void pf_pdf_gaussian_free(pf_pdf_gaussian_t *pdf)
Definition: pf_pdf.c:74
double pf_ran_gaussian(double sigma)
Definition: pf_pdf.c:132
double v[3]
Definition: pf_vector.h:40
pf_vector_t x
Definition: pf_pdf.h:48
pf_matrix_t cr
Definition: pf_pdf.h:54
pf_vector_t pf_pdf_gaussian_sample(pf_pdf_gaussian_t *pdf)
Definition: pf_pdf.c:105
pf_pdf_gaussian_t * pf_pdf_gaussian_alloc(pf_vector_t x, pf_matrix_t cx)
Definition: pf_pdf.c:46
static unsigned int pf_pdf_seed
Definition: pf_pdf.c:38
pf_matrix_t cx
Definition: pf_pdf.h:49
double m[3][3]
Definition: pf_vector.h:47
TFSIMD_FORCE_INLINE const tfScalar & x() const
pf_vector_t cd
Definition: pf_pdf.h:55
TFSIMD_FORCE_INLINE const tfScalar & w() const
void pf_matrix_unitary(pf_matrix_t *r, pf_matrix_t *d, pf_matrix_t a)
Definition: pf_vector.c:222


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