Fawkes API  Fawkes Development Version
pf_pdf.c
1 
2 /***************************************************************************
3  * pf_pdf.c: Useful pdf functions
4  *
5  * Created: Thu May 24 18:41:18 2012
6  * Copyright 2000 Brian Gerkey
7  * 2000 Kasper Stoy
8  * 2012 Tim Niemueller [www.niemueller.de]
9  ****************************************************************************/
10 
11 /* This program is free software; you can redistribute it and/or modify
12  * it under the terms of the GNU General Public License as published by
13  * the Free Software Foundation; either version 2 of the License, or
14  * (at your option) any later version.
15  *
16  * This program is distributed in the hope that it will be useful,
17  * but WITHOUT ANY WARRANTY; without even the implied warranty of
18  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
19  * GNU Library General Public License for more details.
20  *
21  * Read the full text in the LICENSE.GPL file in the doc directory.
22  */
23 
24 /* From:
25  * Player - One Hell of a Robot Server (LGPL)
26  * Copyright (C) 2000 Brian Gerkey & Kasper Stoy
27  * gerkey@usc.edu kaspers@robotics.usc.edu
28  */
29 /**************************************************************************
30  * Desc: Useful pdf functions
31  * Author: Andrew Howard
32  * Date: 10 Dec 2002
33  *************************************************************************/
34 
35 #include <assert.h>
36 #include <math.h>
37 #include <stdlib.h>
38 #include <string.h>
39 //#include <gsl/gsl_rng.h>
40 //#include <gsl/gsl_randist.h>
41 
42 #include "pf_pdf.h"
43 
44 /// @cond EXTERNAL
45 
46 // Random number generator seed value
47 static unsigned int pf_pdf_seed;
48 
49 
50 /**************************************************************************
51  * Gaussian
52  *************************************************************************/
53 
54 // Create a gaussian pdf
55 pf_pdf_gaussian_t *pf_pdf_gaussian_alloc(pf_vector_t x, pf_matrix_t cx)
56 {
57  pf_matrix_t cd;
58  pf_pdf_gaussian_t *pdf;
59 
60  pdf = calloc(1, sizeof(pf_pdf_gaussian_t));
61 
62  pdf->x = x;
63  pdf->cx = cx;
64  //pdf->cxi = pf_matrix_inverse(cx, &pdf->cxdet);
65 
66  // Decompose the convariance matrix into a rotation
67  // matrix and a diagonal matrix.
68  pf_matrix_unitary(&pdf->cr, &cd, pdf->cx);
69  pdf->cd.v[0] = sqrt(cd.m[0][0]);
70  pdf->cd.v[1] = sqrt(cd.m[1][1]);
71  pdf->cd.v[2] = sqrt(cd.m[2][2]);
72 
73  // Initialize the random number generator
74  //pdf->rng = gsl_rng_alloc(gsl_rng_taus);
75  //gsl_rng_set(pdf->rng, ++pf_pdf_seed);
76  srand48(++pf_pdf_seed);
77 
78  return pdf;
79 }
80 
81 
82 // Destroy the pdf
83 void pf_pdf_gaussian_free(pf_pdf_gaussian_t *pdf)
84 {
85  //gsl_rng_free(pdf->rng);
86  free(pdf);
87  return;
88 }
89 
90 
91 /*
92 // Compute the value of the pdf at some point [x].
93 double pf_pdf_gaussian_value(pf_pdf_gaussian_t *pdf, pf_vector_t x)
94 {
95  int i, j;
96  pf_vector_t z;
97  double zz, p;
98 
99  z = pf_vector_sub(x, pdf->x);
100 
101  zz = 0;
102  for (i = 0; i < 3; i++)
103  for (j = 0; j < 3; j++)
104  zz += z.v[i] * pdf->cxi.m[i][j] * z.v[j];
105 
106  p = 1 / (2 * M_PI * pdf->cxdet) * exp(-zz / 2);
107 
108  return p;
109 }
110 */
111 
112 
113 // Generate a sample from the the pdf.
114 pf_vector_t pf_pdf_gaussian_sample(pf_pdf_gaussian_t *pdf)
115 {
116  int i, j;
117  pf_vector_t r;
118  pf_vector_t x;
119 
120  // Generate a random vector
121  for (i = 0; i < 3; i++)
122  {
123  //r.v[i] = gsl_ran_gaussian(pdf->rng, pdf->cd.v[i]);
124  r.v[i] = pf_ran_gaussian(pdf->cd.v[i]);
125  }
126 
127  for (i = 0; i < 3; i++)
128  {
129  x.v[i] = pdf->x.v[i];
130  for (j = 0; j < 3; j++)
131  x.v[i] += pdf->cr.m[i][j] * r.v[j];
132  }
133 
134  return x;
135 }
136 
137 // Draw randomly from a zero-mean Gaussian distribution, with standard
138 // deviation sigma.
139 // We use the polar form of the Box-Muller transformation, explained here:
140 // http://www.taygeta.com/random/gaussian.html
141 double pf_ran_gaussian(double sigma)
142 {
143  double x1, x2, w, r;
144 
145  do
146  {
147  do { r = drand48(); } while (r==0.0);
148  x1 = 2.0 * r - 1.0;
149  do { r = drand48(); } while (r==0.0);
150  x2 = 2.0 * r - 1.0;
151  w = x1*x1 + x2*x2;
152  } while(w > 1.0 || w==0.0);
153 
154  return(sigma * x2 * sqrt(-2.0*log(w)/w));
155 }
156 
157 #if 0
158 
159 /**************************************************************************
160  * Discrete
161  * Note that GSL v1.3 and earlier contains a bug in the discrete
162  * random generator. A patched version of the the generator is included
163  * in gsl_discrete.c.
164  *************************************************************************/
165 
166 
167 // Create a discrete pdf
168 pf_pdf_discrete_t *pf_pdf_discrete_alloc(int count, double *probs)
169 {
170  pf_pdf_discrete_t *pdf;
171 
172  pdf = calloc(1, sizeof(pf_pdf_discrete_t));
173 
174  pdf->prob_count = count;
175  pdf->probs = malloc(count * sizeof(double));
176  memcpy(pdf->probs, probs, count * sizeof(double));
177 
178  // Initialize the random number generator
179  pdf->rng = gsl_rng_alloc(gsl_rng_taus);
180  gsl_rng_set(pdf->rng, ++pf_pdf_seed);
181 
182  // Initialize the discrete distribution generator
183  pdf->ran = gsl_ran_discrete_preproc(count, probs);
184 
185  return pdf;
186 }
187 
188 
189 // Destroy the pdf
190 void pf_pdf_discrete_free(pf_pdf_discrete_t *pdf)
191 {
192  gsl_ran_discrete_free(pdf->ran);
193  gsl_rng_free(pdf->rng);
194  free(pdf->probs);
195  free(pdf);
196  return;
197 }
198 
199 
200 // Compute the value of the probability of some element [i]
201 double pf_pdf_discrete_value(pf_pdf_discrete_t *pdf, int i)
202 {
203  return pdf->probs[i];
204 }
205 
206 
207 // Generate a sample from the the pdf.
208 int pf_pdf_discrete_sample(pf_pdf_discrete_t *pdf)
209 {
210  int i;
211 
212  i = gsl_ran_discrete(pdf->rng, pdf->ran);
213  assert(i >= 0 && i < pdf->prob_count);
214 
215  return i;
216 }
217 
218 
219 #endif
220 
221 /// @endcond
222