Fawkes API
Fawkes Development Version
|
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