measurementmodel.cpp
Go to the documentation of this file.
00001 // $Id$
00002 // Copyright (C) 2002 Klaas Gadeyne <first dot last at gmail dot com>
00003 //
00004 // This program is free software; you can redistribute it and/or modify
00005 // it under the terms of the GNU Lesser General Public License as published by
00006 // the Free Software Foundation; either version 2.1 of the License, or
00007 // (at your option) any later version.
00008 //
00009 // This program is distributed in the hope that it will be useful,
00010 // but WITHOUT ANY WARRANTY; without even the implied warranty of
00011 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00012 // GNU Lesser General Public License for more details.
00013 //
00014 // You should have received a copy of the GNU Lesser General Public License
00015 // along with this program; if not, write to the Free Software
00016 // Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
00017 //
00018 
00019 #include "measurementmodel.h"
00020 #include <iostream>
00021 
00022 using namespace std;
00023 
00024 // Constructor
00025 template<typename MeasVar, typename StateVar>
00026 MeasurementModel<MeasVar,StateVar>::MeasurementModel(ConditionalPdf<MeasVar,StateVar>* measurementpdf)
00027 {
00028 #ifdef __CONSTRUCTOR__
00029   cout << "MeasurementModel::Constructor" << endl;
00030 #endif // __CONSTRUCTOR__
00031   if (measurementpdf != NULL)
00032     {
00033       switch(measurementpdf->NumConditionalArgumentsGet())
00034         {
00035         case 1:
00036           {
00037             _systemWithoutSensorParams = true;
00038             _MeasurementPdf  = measurementpdf;
00039             break;
00040           }
00041         case 2:
00042           {
00043             _systemWithoutSensorParams = false;
00044             _MeasurementPdf  = measurementpdf;
00045             break;
00046           }
00047         default:{
00048           cerr << "MeasurementModel::Constructor : MeasPdf can only have 1 or 2 conditional Arguments (x and u, in that order!))" << endl;
00049           exit(-BFL_ERRMISUSE);
00050         }
00051         }
00052     }
00053 }
00054 
00055 // Destructor
00056 template<typename MeasVar, typename StateVar>
00057  MeasurementModel<MeasVar,StateVar>::~MeasurementModel()
00058 {
00059 #ifdef __DESTRUCTOR__
00060   cout << "MeasurementModel::Destructor" << endl;
00061 #endif // __DESTRUCTOR__
00062   /* KG: Probably a memory leak
00063      Who should clean this up? Sometimes the user will have created
00064      this Pdf, sometimes not (eg. by copy constructor).  If we allways
00065      delete it here.
00066      There has to be a cleaner way to implement this!
00067   */
00068   // delete _MeasurementPdf;
00069 }
00070 
00071 // BUG: Should have copy constructor here?
00072 
00073 // Get Measurement Size
00074 template<typename MeasVar, typename StateVar> int
00075 MeasurementModel<MeasVar,StateVar>::MeasurementSizeGet() const
00076 {
00077   return _MeasurementPdf->DimensionGet();
00078 }
00079 
00080 template<typename MeasVar, typename StateVar> bool
00081 MeasurementModel<MeasVar,StateVar>::SystemWithoutSensorParams() const
00082 {
00083   return _systemWithoutSensorParams;
00084 }
00085 
00086 // Get MeasurementPdf
00087 template<typename MeasVar, typename StateVar> ConditionalPdf<MeasVar,StateVar>*
00088 MeasurementModel<MeasVar,StateVar>::MeasurementPdfGet()
00089 {
00090   return _MeasurementPdf;
00091 }
00092 
00093 // Set MeasurementPdf
00094 template<typename MeasVar, typename StateVar> void
00095 MeasurementModel<MeasVar,StateVar>::MeasurementPdfSet(ConditionalPdf<MeasVar,StateVar> * pdf)
00096 {
00097   assert(pdf != NULL);
00098   switch(pdf->NumConditionalArgumentsGet())
00099     {
00100     case 1:
00101       {
00102         _systemWithoutSensorParams = true;
00103         _MeasurementPdf  = pdf;
00104         break;
00105       }
00106     case 2:
00107       {
00108         _systemWithoutSensorParams = false;
00109         _MeasurementPdf  = pdf;
00110         break;
00111       }
00112     default:
00113       {
00114         cerr << "MeasurementModel::Constructor : MeasPdf can only have 1 or 2 conditional Arguments (x and u, in that order!))" << endl;
00115         exit(-BFL_ERRMISUSE);
00116       }
00117     }
00118 }
00119 
00120 template<typename MeasVar, typename StateVar> MeasVar
00121 MeasurementModel<MeasVar,StateVar>::Simulate (const StateVar& x,
00122                                               const StateVar& s,
00123                                               int sampling_method,
00124                                               void * sampling_args)
00125 {
00126   assert(_systemWithoutSensorParams == false);
00127   _MeasurementPdf->ConditionalArgumentSet(0,x);
00128   _MeasurementPdf->ConditionalArgumentSet(1,s);
00129   Sample<MeasVar> Simulated(MeasurementSizeGet());
00130   _MeasurementPdf->SampleFrom(Simulated, sampling_method,sampling_args);
00131   MeasVar result = Simulated.ValueGet();
00132   return result;
00133 }
00134 
00135 
00136 template<typename MeasVar, typename StateVar> MeasVar
00137 MeasurementModel<MeasVar,StateVar>::Simulate(const StateVar& x,
00138                                              int sampling_method,
00139                                              void * sampling_args)
00140 {
00141   assert(_systemWithoutSensorParams == true);
00142   _MeasurementPdf->ConditionalArgumentSet(0,x);
00143   Sample<StateVar> Simulated(MeasurementSizeGet());
00144   _MeasurementPdf->SampleFrom(Simulated, sampling_method,sampling_args);
00145   MeasVar result = Simulated.ValueGet();
00146   return result;
00147 }
00148 
00149 template <typename MeasVar, typename StateVar> Probability
00150 MeasurementModel<MeasVar,StateVar>::ProbabilityGet (const MeasVar& z,
00151                                                     const StateVar& x,
00152                                                     const StateVar& s)
00153 {
00154   assert(_systemWithoutSensorParams == false);
00155   _MeasurementPdf->ConditionalArgumentSet(0,x);
00156   _MeasurementPdf->ConditionalArgumentSet(1,s);
00157   return _MeasurementPdf->ProbabilityGet(z);
00158 }
00159 
00160 template <typename MeasVar, typename StateVar> Probability
00161 MeasurementModel<MeasVar,StateVar>::ProbabilityGet (const MeasVar& z,
00162                                                     const StateVar& x)
00163 {
00164   assert(_systemWithoutSensorParams == true);
00165   _MeasurementPdf->ConditionalArgumentSet(0,x);
00166   return _MeasurementPdf->ProbabilityGet(z);
00167 }


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 Mon Feb 11 2019 03:45:12