Fawkes API  Fawkes Development Version
pf_pdf.c
00001 
00002 /***************************************************************************
00003  *  pf_pdf.c: Useful pdf functions
00004  *
00005  *  Created: Thu May 24 18:41:18 2012
00006  *  Copyright  2000  Brian Gerkey
00007  *             2000  Kasper Stoy
00008  *             2012  Tim Niemueller [www.niemueller.de]
00009  ****************************************************************************/
00010 
00011 /*  This program is free software; you can redistribute it and/or modify
00012  *  it under the terms of the GNU General Public License as published by
00013  *  the Free Software Foundation; either version 2 of the License, or
00014  *  (at your option) any later version.
00015  *
00016  *  This program is distributed in the hope that it will be useful,
00017  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
00018  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00019  *  GNU Library General Public License for more details.
00020  *
00021  *  Read the full text in the LICENSE.GPL file in the doc directory.
00022  */
00023 
00024 /*  From:
00025  *  Player - One Hell of a Robot Server (LGPL)
00026  *  Copyright (C) 2000  Brian Gerkey   &  Kasper Stoy
00027  *                      gerkey@usc.edu    kaspers@robotics.usc.edu
00028  */
00029 /**************************************************************************
00030  * Desc: Useful pdf functions
00031  * Author: Andrew Howard
00032  * Date: 10 Dec 2002
00033  *************************************************************************/
00034 
00035 #include <assert.h>
00036 #include <math.h>
00037 #include <stdlib.h>
00038 #include <string.h>
00039 //#include <gsl/gsl_rng.h>
00040 //#include <gsl/gsl_randist.h>
00041 
00042 #include "pf_pdf.h"
00043 
00044 /// @cond EXTERNAL
00045 
00046 // Random number generator seed value
00047 static unsigned int pf_pdf_seed;
00048 
00049 
00050 /**************************************************************************
00051  * Gaussian
00052  *************************************************************************/
00053 
00054 // Create a gaussian pdf
00055 pf_pdf_gaussian_t *pf_pdf_gaussian_alloc(pf_vector_t x, pf_matrix_t cx)
00056 {
00057   pf_matrix_t cd;
00058   pf_pdf_gaussian_t *pdf;
00059 
00060   pdf = calloc(1, sizeof(pf_pdf_gaussian_t));
00061 
00062   pdf->x = x;
00063   pdf->cx = cx;
00064   //pdf->cxi = pf_matrix_inverse(cx, &pdf->cxdet);
00065 
00066   // Decompose the convariance matrix into a rotation
00067   // matrix and a diagonal matrix.
00068   pf_matrix_unitary(&pdf->cr, &cd, pdf->cx);
00069   pdf->cd.v[0] = sqrt(cd.m[0][0]);
00070   pdf->cd.v[1] = sqrt(cd.m[1][1]);
00071   pdf->cd.v[2] = sqrt(cd.m[2][2]);
00072 
00073   // Initialize the random number generator
00074   //pdf->rng = gsl_rng_alloc(gsl_rng_taus);
00075   //gsl_rng_set(pdf->rng, ++pf_pdf_seed);
00076   srand48(++pf_pdf_seed);
00077 
00078   return pdf;
00079 }
00080 
00081 
00082 // Destroy the pdf
00083 void pf_pdf_gaussian_free(pf_pdf_gaussian_t *pdf)
00084 {
00085   //gsl_rng_free(pdf->rng);
00086   free(pdf);
00087   return;
00088 }
00089 
00090 
00091 /*
00092 // Compute the value of the pdf at some point [x].
00093 double pf_pdf_gaussian_value(pf_pdf_gaussian_t *pdf, pf_vector_t x)
00094 {
00095   int i, j;
00096   pf_vector_t z;
00097   double zz, p;
00098   
00099   z = pf_vector_sub(x, pdf->x);
00100 
00101   zz = 0;
00102   for (i = 0; i < 3; i++)
00103     for (j = 0; j < 3; j++)
00104       zz += z.v[i] * pdf->cxi.m[i][j] * z.v[j];
00105 
00106   p =  1 / (2 * M_PI * pdf->cxdet) * exp(-zz / 2);
00107           
00108   return p;
00109 }
00110 */
00111 
00112 
00113 // Generate a sample from the the pdf.
00114 pf_vector_t pf_pdf_gaussian_sample(pf_pdf_gaussian_t *pdf)
00115 {
00116   int i, j;
00117   pf_vector_t r;
00118   pf_vector_t x;
00119 
00120   // Generate a random vector
00121   for (i = 0; i < 3; i++)
00122   {
00123     //r.v[i] = gsl_ran_gaussian(pdf->rng, pdf->cd.v[i]);
00124     r.v[i] = pf_ran_gaussian(pdf->cd.v[i]);
00125   }
00126 
00127   for (i = 0; i < 3; i++)
00128   {
00129     x.v[i] = pdf->x.v[i];
00130     for (j = 0; j < 3; j++)
00131       x.v[i] += pdf->cr.m[i][j] * r.v[j];
00132   } 
00133   
00134   return x;
00135 }
00136 
00137 // Draw randomly from a zero-mean Gaussian distribution, with standard
00138 // deviation sigma.
00139 // We use the polar form of the Box-Muller transformation, explained here:
00140 //   http://www.taygeta.com/random/gaussian.html
00141 double pf_ran_gaussian(double sigma)
00142 {
00143   double x1, x2, w, r;
00144 
00145   do
00146   {
00147     do { r = drand48(); } while (r==0.0);
00148     x1 = 2.0 * r - 1.0;
00149     do { r = drand48(); } while (r==0.0);
00150     x2 = 2.0 * r - 1.0;
00151     w = x1*x1 + x2*x2;
00152   } while(w > 1.0 || w==0.0);
00153 
00154   return(sigma * x2 * sqrt(-2.0*log(w)/w));
00155 }
00156 
00157 #if 0
00158 
00159 /**************************************************************************
00160  * Discrete
00161  * Note that GSL v1.3 and earlier contains a bug in the discrete
00162  * random generator.  A patched version of the the generator is included
00163  * in gsl_discrete.c.
00164  *************************************************************************/
00165 
00166 
00167 // Create a discrete pdf
00168 pf_pdf_discrete_t *pf_pdf_discrete_alloc(int count, double *probs)
00169 {
00170   pf_pdf_discrete_t *pdf;
00171 
00172   pdf = calloc(1, sizeof(pf_pdf_discrete_t));
00173 
00174   pdf->prob_count = count;
00175   pdf->probs = malloc(count * sizeof(double));
00176   memcpy(pdf->probs, probs, count * sizeof(double));
00177   
00178   // Initialize the random number generator
00179   pdf->rng = gsl_rng_alloc(gsl_rng_taus);
00180   gsl_rng_set(pdf->rng, ++pf_pdf_seed);
00181 
00182   // Initialize the discrete distribution generator
00183   pdf->ran = gsl_ran_discrete_preproc(count, probs);
00184 
00185   return pdf;
00186 }
00187 
00188 
00189 // Destroy the pdf
00190 void pf_pdf_discrete_free(pf_pdf_discrete_t *pdf)
00191 {
00192   gsl_ran_discrete_free(pdf->ran);
00193   gsl_rng_free(pdf->rng);
00194   free(pdf->probs);  
00195   free(pdf);
00196   return;
00197 }
00198 
00199 
00200 // Compute the value of the probability of some element [i]
00201 double pf_pdf_discrete_value(pf_pdf_discrete_t *pdf, int i)
00202 {
00203   return pdf->probs[i];
00204 }
00205 
00206 
00207 // Generate a sample from the the pdf.
00208 int pf_pdf_discrete_sample(pf_pdf_discrete_t *pdf)
00209 {
00210   int i;
00211   
00212   i = gsl_ran_discrete(pdf->rng, pdf->ran);
00213   assert(i >= 0 && i < pdf->prob_count);
00214 
00215   return i;
00216 }
00217 
00218 
00219 #endif
00220 
00221 /// @endcond
00222