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 x2, w;
144 
145  do
146  {
147  double r;
148  do { r = drand48(); } while (r==0.0);
149  double x1 = 2.0 * r - 1.0;
150  do { r = drand48(); } while (r==0.0);
151  x2 = 2.0 * r - 1.0;
152  w = x1*x1 + x2*x2;
153  } while(w > 1.0 || w==0.0);
154 
155  return(sigma * x2 * sqrt(-2.0*log(w)/w));
156 }
157 
158 #if 0
159 
160 /**************************************************************************
161  * Discrete
162  * Note that GSL v1.3 and earlier contains a bug in the discrete
163  * random generator. A patched version of the the generator is included
164  * in gsl_discrete.c.
165  *************************************************************************/
166 
167 
168 // Create a discrete pdf
169 pf_pdf_discrete_t *pf_pdf_discrete_alloc(int count, double *probs)
170 {
171  pf_pdf_discrete_t *pdf;
172 
173  pdf = calloc(1, sizeof(pf_pdf_discrete_t));
174 
175  pdf->prob_count = count;
176  pdf->probs = malloc(count * sizeof(double));
177  memcpy(pdf->probs, probs, count * sizeof(double));
178 
179  // Initialize the random number generator
180  pdf->rng = gsl_rng_alloc(gsl_rng_taus);
181  gsl_rng_set(pdf->rng, ++pf_pdf_seed);
182 
183  // Initialize the discrete distribution generator
184  pdf->ran = gsl_ran_discrete_preproc(count, probs);
185 
186  return pdf;
187 }
188 
189 
190 // Destroy the pdf
191 void pf_pdf_discrete_free(pf_pdf_discrete_t *pdf)
192 {
193  gsl_ran_discrete_free(pdf->ran);
194  gsl_rng_free(pdf->rng);
195  free(pdf->probs);
196  free(pdf);
197  return;
198 }
199 
200 
201 // Compute the value of the probability of some element [i]
202 double pf_pdf_discrete_value(pf_pdf_discrete_t *pdf, int i)
203 {
204  return pdf->probs[i];
205 }
206 
207 
208 // Generate a sample from the the pdf.
209 int pf_pdf_discrete_sample(pf_pdf_discrete_t *pdf)
210 {
211  int i;
212 
213  i = gsl_ran_discrete(pdf->rng, pdf->ran);
214  assert(i >= 0 && i < pdf->prob_count);
215 
216  return i;
217 }
218 
219 
220 #endif
221 
222 /// @endcond
223