discreteconditionalpdf.cpp
Go to the documentation of this file.
00001 // $Id: discreteconditionalpdf.cpp 30031 2009-03-11 14:56:23Z tdelaet $
00002 // Copyright (C) 2003 Klaas Gadeyne <first dot last at gmail dot com>
00003 //               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 "discreteconditionalpdf.h"
00020 #include "../wrappers/rng/rng.h"
00021 #include <vector>
00022 #include <iostream>
00023 
00024 namespace BFL
00025 {
00026   using namespace std;
00027 
00028   DiscreteConditionalPdf::DiscreteConditionalPdf(int num_states,
00029                                                  int num_conditional_arguments,
00030                                                  int cond_arg_dimensions[])
00031     : ConditionalPdf<int,int>(1,num_conditional_arguments)
00032     , _num_states(num_states)
00033     , _probs(num_states)
00034     , _valuelist(num_states+1)
00035   {
00036     _cond_arg_dims_p = new int[num_conditional_arguments];
00037     int total_dim = 1;
00038     for ( int arg = 0 ; arg < num_conditional_arguments; arg++)
00039       {
00040         _cond_arg_dims_p[arg] = cond_arg_dimensions[arg];
00041         total_dim *= cond_arg_dimensions[arg];
00042       }
00043     _total_dimension = total_dim * num_states;
00044 #ifdef __CONSTRUCTOR__
00045     cout << "DCPdf Constructor: total dimension allocated: "
00046          << _total_dimension << endl;
00047 #endif // __CONSTRUCTOR__
00048     _probability_p = new double[_total_dimension];
00049   }
00050 
00051   DiscreteConditionalPdf::~DiscreteConditionalPdf()
00052   {
00053 #ifdef __DCPDFDEBUG__
00054     cout << "DCPdf Destructor:" << endl;
00055 #endif // __DCPDFDEBUG__
00056     //delete _probability_p;
00057     //delete _cond_arg_dims_p;
00058   }
00059 
00060 
00061   DiscreteConditionalPdf::DiscreteConditionalPdf(const DiscreteConditionalPdf & pdf)
00062     : ConditionalPdf<int,int>(pdf)
00063     ,_num_states(pdf.NumStatesGet())
00064     , _probs(pdf.NumStatesGet())
00065     , _valuelist(pdf.NumStatesGet()+1)
00066   {
00067     _cond_arg_dims_p = new int[pdf.NumConditionalArgumentsGet()];
00068     int total_dim = 1;
00069     for ( unsigned int arg = 0 ; arg < NumConditionalArgumentsGet(); arg++)
00070       {
00071         _cond_arg_dims_p[arg] = pdf._cond_arg_dims_p[arg];
00072         total_dim *= _cond_arg_dims_p[arg];
00073       }
00074     total_dim *= _num_states;
00075     _total_dimension = total_dim;
00076     _probability_p = new double[total_dim];
00077     for (int prob = 0 ; prob < total_dim ; prob++)
00078       {
00079         _probability_p[prob] = pdf._probability_p[prob];
00080       }
00081   }
00082 
00083   //Clone function
00084   DiscreteConditionalPdf* DiscreteConditionalPdf::Clone() const
00085   {
00086       return new DiscreteConditionalPdf(*this);
00087   }
00088 
00089   // Get the number of discrete states
00090   unsigned int DiscreteConditionalPdf::NumStatesGet() const
00091   {
00092      return _num_states;
00093   }
00094 
00095   // Calculate index (used by ProbabilityGet and ProbabilitySet)
00096   int DiscreteConditionalPdf::IndexGet(const int& input,
00097                                        const std::vector<int>& condargs) const
00098   {
00099     int index = 0;
00100     int blocksize = 1;
00101 
00102     // The first hyperdimension is that of input itself
00103     index += input * blocksize;
00104     blocksize *= NumStatesGet();
00105     // The other ons are those of the conditional args
00106     for (unsigned int arg = 0 ; arg < NumConditionalArgumentsGet() ; arg++ )
00107       {
00108         index += condargs[arg] * blocksize;
00109         blocksize *= (this->_cond_arg_dims_p)[arg];
00110       }
00111 #ifdef __INDEXDEBUG__
00112     cout << "DCPdf::IndexGet -> Index = " << index << endl;
00113 #endif // __INDEXDEBUG__
00114     return index;
00115   }
00116 
00117 
00118 
00119   Probability DiscreteConditionalPdf::ProbabilityGet(const int& input) const
00120   {
00121     unsigned int index = IndexGet(input, ConditionalArgumentsGet());
00122     double prob = (this->_probability_p)[index];
00123     return prob;
00124   }
00125 
00126   // Typical for discrete Pdf's
00127   void DiscreteConditionalPdf::ProbabilitySet(const double& prob,
00128                                               const int& input,
00129                                               const std::vector<int>& condargs) const
00130   {
00131     int index = this->IndexGet(input, condargs);
00132     _probability_p[index] = prob;
00133   }
00134 
00135   bool DiscreteConditionalPdf::SampleFrom(Sample<int>& one_sample, int method, void * args) const
00136   {
00137     // Get the elements of which to sample from
00138     int startindex = IndexGet(0,ConditionalArgumentsGet());
00139     double SumWeights = 0.0; double CumSum=0.0;
00140     unsigned int index;
00141 
00142     for ( index = 0; index < NumStatesGet() ; index++ )
00143       {
00144         _probs[index] = _probability_p[startindex+index];
00145         CumSum += _probs[index];
00146 #ifdef __DCPDFDEBUG__
00147 #define TABWIDTH 10
00148         cout << setw(TABWIDTH) << _probs[index];
00149 #endif // __DCPDFDEBUG__
00150       }
00151     SumWeights = CumSum;
00152     _valuelist[0] = 0.0; CumSum = 0.0;
00153     for ( index = 1; index <= NumStatesGet() ; index++ )
00154       {
00155         CumSum += _probs[index-1]/SumWeights;
00156         _valuelist[index] = CumSum;
00157       }
00158     // Check if last element of valuelist is +- 1
00159     assert ( (_valuelist[NumStatesGet()] >= 1.0 - NUMERIC_PRECISION) &&
00160              (_valuelist[NumStatesGet()] <= 1.0 + NUMERIC_PRECISION) );
00161 
00162     _valuelist[NumStatesGet()]=1;
00163 
00164     // Sample from univariate uniform rng between 0 and 1;
00165     double unif_sample; unif_sample = runif();
00166     // Compare where we should be: THIS CAN BE MADE FASTER: TODO
00167     index = 0;
00168     while ( unif_sample > _valuelist[index] )
00169       {
00170         assert(index <= NumStatesGet());
00171         index++;
00172       }
00173     one_sample.ValueSet(index-1);
00174     return true;
00175   }
00176 
00177 
00178   bool
00179   DiscreteConditionalPdf::SampleFrom(vector<Sample<int> >& list_samples,
00180                                      int num_samples, int method,
00181                                      void * args) const
00182   {
00183     list_samples.resize(num_samples); // will break real-timeness if list_samples.size()!=num_samples
00184     return Pdf<int>::SampleFrom(list_samples, num_samples,method,args);
00185   }
00186 
00187 
00188 } // 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 Thu Feb 11 2016 22:31:56