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


nav2d_localizer
Author(s): Sebastian Kasperski
autogenerated on Sun Apr 2 2017 03:52:57