gaussian.cpp
Go to the documentation of this file.
00001 // $Id: gaussian.cpp 33801 2010-12-21 12:20:54Z tdelaet $
00002 // Copyright (C) 2002 Klaas Gadeyne <first dot last at gmail dot com>
00003 // Copyright (C) 2008 Tinne De Laet <first dot last at mech dot kuleuven dot be>
00004 //
00005 // This program is free software; you can redistribute it and/or modify
00006 // it under the terms of the GNU Lesser General Public License as published by
00007 // the Free Software Foundation; either version 2.1 of the License, or
00008 // (at your option) any later version.
00009 //
00010 // This program is distributed in the hope that it will be useful,
00011 // but WITHOUT ANY WARRANTY; without even the implied warranty of
00012 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00013 // GNU Lesser General Public License for more details.
00014 //
00015 // You should have received a copy of the GNU Lesser General Public License
00016 // along with this program; if not, write to the Free Software
00017 // Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
00018 //
00019 #include "gaussian.h"
00020 
00021 #include "../wrappers/rng/rng.h" // Wrapper around several rng libraries
00022 
00023 #include <cmath> // For sqrt and exp
00024 #include <cassert>
00025 
00026 namespace BFL
00027 {
00028   using namespace MatrixWrapper;
00029 
00030   Gaussian::Gaussian (const ColumnVector& m, const SymmetricMatrix& s)
00031     : Pdf<ColumnVector> ( m.rows() )
00032     , _diff(DimensionGet())
00033     , _tempColumn(DimensionGet())
00034     , _samples(DimensionGet())
00035     , _sampleValue(DimensionGet())
00036     , _Low_triangle(DimensionGet(),DimensionGet())
00037   {
00038     // check if inputs are consistent
00039     assert (m.rows() == s.columns());
00040     _Mu = m;
00041     _Sigma = s;
00042     _Sigma_inverse.resize(DimensionGet());
00043     _Sigma_changed = true;
00044   }
00045 
00046   Gaussian::Gaussian (int dimension)
00047     : Pdf<ColumnVector>(dimension)
00048     , _diff(dimension)
00049     , _tempColumn(DimensionGet())
00050     , _samples(dimension)
00051     , _sampleValue(dimension)
00052     , _Low_triangle(dimension,dimension)
00053   {
00054     _Mu.resize(dimension);
00055     _Sigma.resize(dimension);
00056     _Sigma_inverse.resize(dimension);
00057     _Sigma_changed = true;
00058   }
00059 
00060   Gaussian::~Gaussian(){}
00061 
00062   std::ostream& operator<< (std::ostream& os, const Gaussian& g)
00063   {
00064     os << "\nMu:\n"    << g.ExpectedValueGet()
00065        << "\nSigma:\n" << g.CovarianceGet() << endl;
00066     return os;
00067   }
00068 
00069   //Clone function
00070   Gaussian* Gaussian::Clone() const
00071   {     
00072       return new Gaussian(*this);
00073   }
00074 
00075   Probability Gaussian::ProbabilityGet(const ColumnVector& input) const
00076   {
00077     // only calculate these variables if sigma has changed
00078     if (_Sigma_changed){
00079       _Sigma_changed = false;
00080       _Sigma_inverse = _Sigma.inverse();
00081       _sqrt_pow = 1 / sqrt(pow(M_PI*2,(double)DimensionGet()) * _Sigma.determinant());
00082     }
00083 
00084     _diff = input;
00085     _diff -= _Mu;
00086     _Sigma_inverse.multiply(_diff,_tempColumn);
00087     //_tempColumn = _Sigma_inverse * _diff; 
00088     Probability temp = _diff.transpose() * _tempColumn;
00089     //Probability temp = _diff.transpose() * (_Sigma_inverse * _diff);
00090     Probability result = exp(-0.5 * temp) * _sqrt_pow;
00091     return result;
00092   }
00093 
00094   // Redefined for optimal performance.  Eg. do Cholesky decomposition
00095   // only once when drawing multiple samples at once!
00096   // See method below for more info regarding the algorithms
00097   bool
00098   Gaussian::SampleFrom (vector<Sample<ColumnVector> >& list_samples, const int num_samples, int method, void * args) const
00099   {
00100     list_samples.resize(num_samples); // will break real-timeness if list_samples.size()!=num_samples
00101     vector<Sample<ColumnVector> >::iterator rit = list_samples.begin();
00102     switch(method)
00103       {
00104       case DEFAULT: // Cholesky Sampling
00105       case CHOLESKY:
00106         {
00107           bool result = _Sigma.cholesky_semidefinite(_Low_triangle);
00108           while (rit != list_samples.end())
00109             {
00110               for (unsigned int j=1; j < DimensionGet()+1; j++) _samples(j) = rnorm(0,1);
00111               _sampleValue = _Low_triangle * _samples ;
00112               _sampleValue +=  this->_Mu;
00113               rit->ValueSet(_sampleValue);
00114               rit++;
00115             }
00116           return result;
00117         }
00118       case BOXMULLER: // Implement box-muller here
00119         // Only for univariate distributions.
00120         return false;
00121       default:
00122         return false;
00123       }
00124   }
00125 
00126 
00127   bool
00128   Gaussian::SampleFrom (Sample<ColumnVector>& one_sample, int method, void * args) const
00129   {
00130     /*  Exact i.i.d. samples from a Gaussian can be drawn in several
00131         ways:
00132         - if the DimensionGet() = 1 or 2 (and the 2 variables are
00133         independant), we can use inversion sampling (Box-Muller
00134         method)
00135         - For larger dimensions, we use can use the Cholesky method or
00136         an approached based on conditional distributions.
00137         (see ripley87, P.98 (bibtex below)).  The Cholesky method is
00138         generally preferred and the only one implemented for now.
00139     */
00140     switch(method)
00141       {
00142       case DEFAULT: // Cholesky Sampling, see eg.
00143       case CHOLESKY: // Cholesky Sampling, see eg.
00144         /*
00145           @Book{                  ripley87,
00146           author        = {Ripley, Brian D.},
00147           title         = {Stochastic Simulation},
00148           publisher     = {John Wiley and Sons},
00149           year          = {1987},
00150           annote        = {ISBN 0271-6356, WBIB 1 519.245}
00151           }
00152           p.98
00153         */
00154         {
00155       bool result = _Sigma.cholesky_semidefinite(_Low_triangle);
00156 
00157           /* For now we keep it simple, and use the scythe library
00158              (although wrapped) with the uRNG that it uses itself only */
00159           /* Sample Gaussian._dimension samples from univariate
00160              gaussian This could be done using several available
00161              libraries, combined with different uniform RNG.  Both the
00162              library to be used and the uRNG could be implemented as
00163              #ifdef conditions, although I'm sure there must exist a
00164              cleaner way to implement this!
00165           */
00166           for (unsigned int j=1; j < DimensionGet()+1; j++) _samples(j) = rnorm(0,1);
00167           _sampleValue = (_Low_triangle * _samples) + this->_Mu;
00168           one_sample.ValueSet(_sampleValue);
00169           return result;
00170         }
00171       case BOXMULLER: // Implement box-muller here
00172         // Only for univariate distributions.
00173         return false;
00174       default:
00175         return false;
00176       }
00177   }
00178 
00179 
00180   ColumnVector
00181   Gaussian::ExpectedValueGet (  ) const
00182   {
00183     return _Mu;
00184   }
00185 
00186   SymmetricMatrix
00187   Gaussian::CovarianceGet () const
00188   {
00189     return _Sigma;
00190   }
00191 
00192   void
00193   Gaussian::ExpectedValueSet (const ColumnVector& mu)
00194   {
00195     _Mu = mu;
00196     if (this->DimensionGet() == 0)
00197       {
00198         this->DimensionSet(mu.rows());
00199       }
00200     assert(this->DimensionGet() == mu.rows());
00201   }
00202 
00203   void
00204   Gaussian::CovarianceSet (const SymmetricMatrix& cov)
00205   {
00206     _Sigma = cov;
00207     _Sigma_changed = true;
00208     if (this->DimensionGet() == 0)
00209       {
00210         this->DimensionSet(cov.rows());
00211       }
00212     assert(this->DimensionGet() == cov.rows());
00213   }
00214 
00215   void
00216   Gaussian::DimensionSet ( unsigned int dim )
00217   {
00218     Pdf<ColumnVector>::DimensionSet(dim);
00219     _diff.resize(DimensionGet());
00220     _tempColumn.resize(DimensionGet());
00221     _samples.resize(DimensionGet());
00222     _sampleValue.resize(DimensionGet());
00223     _Low_triangle.resize(DimensionGet(),DimensionGet());
00224   }
00225 
00226 } // End namespace BFL


bfl
Author(s): Klaas Gadeyne, Wim Meeussen, Tinne Delaet and many others. See web page for a full contributor list. ROS package maintained by Wim Meeussen.
autogenerated on Sun Oct 5 2014 22:29:52