measurementmodel.cpp
Go to the documentation of this file.
1 // $Id$
2 // Copyright (C) 2002 Klaas Gadeyne <first dot last at gmail dot com>
3 //
4 // This program is free software; you can redistribute it and/or modify
5 // it under the terms of the GNU Lesser General Public License as published by
6 // the Free Software Foundation; either version 2.1 of the License, or
7 // (at your option) any later version.
8 //
9 // This program is distributed in the hope that it will be useful,
10 // but WITHOUT ANY WARRANTY; without even the implied warranty of
11 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
12 // GNU Lesser General Public License for more details.
13 //
14 // You should have received a copy of the GNU Lesser General Public License
15 // along with this program; if not, write to the Free Software
16 // Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
17 //
18 
19 #include "measurementmodel.h"
20 #include <iostream>
21 
22 using namespace std;
23 
24 // Constructor
25 template<typename MeasVar, typename StateVar>
27 {
28 #ifdef __CONSTRUCTOR__
29  cout << "MeasurementModel::Constructor" << endl;
30 #endif // __CONSTRUCTOR__
31  if (measurementpdf != NULL)
32  {
33  switch(measurementpdf->NumConditionalArgumentsGet())
34  {
35  case 1:
36  {
37  _systemWithoutSensorParams = true;
38  _MeasurementPdf = measurementpdf;
39  break;
40  }
41  case 2:
42  {
43  _systemWithoutSensorParams = false;
44  _MeasurementPdf = measurementpdf;
45  break;
46  }
47  default:{
48  cerr << "MeasurementModel::Constructor : MeasPdf can only have 1 or 2 conditional Arguments (x and u, in that order!))" << endl;
49  exit(-BFL_ERRMISUSE);
50  }
51  }
52  }
53 }
54 
55 // Destructor
56 template<typename MeasVar, typename StateVar>
58 {
59 #ifdef __DESTRUCTOR__
60  cout << "MeasurementModel::Destructor" << endl;
61 #endif // __DESTRUCTOR__
62  /* KG: Probably a memory leak
63  Who should clean this up? Sometimes the user will have created
64  this Pdf, sometimes not (eg. by copy constructor). If we allways
65  delete it here.
66  There has to be a cleaner way to implement this!
67  */
68  // delete _MeasurementPdf;
69 }
70 
71 // BUG: Should have copy constructor here?
72 
73 // Get Measurement Size
74 template<typename MeasVar, typename StateVar> int
76 {
77  return _MeasurementPdf->DimensionGet();
78 }
79 
80 template<typename MeasVar, typename StateVar> bool
82 {
83  return _systemWithoutSensorParams;
84 }
85 
86 // Get MeasurementPdf
87 template<typename MeasVar, typename StateVar> ConditionalPdf<MeasVar,StateVar>*
89 {
90  return _MeasurementPdf;
91 }
92 
93 // Set MeasurementPdf
94 template<typename MeasVar, typename StateVar> void
96 {
97  assert(pdf != NULL);
98  switch(pdf->NumConditionalArgumentsGet())
99  {
100  case 1:
101  {
102  _systemWithoutSensorParams = true;
103  _MeasurementPdf = pdf;
104  break;
105  }
106  case 2:
107  {
108  _systemWithoutSensorParams = false;
109  _MeasurementPdf = pdf;
110  break;
111  }
112  default:
113  {
114  cerr << "MeasurementModel::Constructor : MeasPdf can only have 1 or 2 conditional Arguments (x and u, in that order!))" << endl;
115  exit(-BFL_ERRMISUSE);
116  }
117  }
118 }
119 
120 template<typename MeasVar, typename StateVar> MeasVar
122  const StateVar& s,
123  int sampling_method,
124  void * sampling_args)
125 {
126  assert(_systemWithoutSensorParams == false);
127  _MeasurementPdf->ConditionalArgumentSet(0,x);
128  _MeasurementPdf->ConditionalArgumentSet(1,s);
129  Sample<MeasVar> Simulated(MeasurementSizeGet());
130  _MeasurementPdf->SampleFrom(Simulated, sampling_method,sampling_args);
131  MeasVar result = Simulated.ValueGet();
132  return result;
133 }
134 
135 
136 template<typename MeasVar, typename StateVar> MeasVar
138  int sampling_method,
139  void * sampling_args)
140 {
141  assert(_systemWithoutSensorParams == true);
142  _MeasurementPdf->ConditionalArgumentSet(0,x);
143  Sample<StateVar> Simulated(MeasurementSizeGet());
144  _MeasurementPdf->SampleFrom(Simulated, sampling_method,sampling_args);
145  MeasVar result = Simulated.ValueGet();
146  return result;
147 }
148 
149 template <typename MeasVar, typename StateVar> Probability
151  const StateVar& x,
152  const StateVar& s)
153 {
154  assert(_systemWithoutSensorParams == false);
155  _MeasurementPdf->ConditionalArgumentSet(0,x);
156  _MeasurementPdf->ConditionalArgumentSet(1,s);
157  return _MeasurementPdf->ProbabilityGet(z);
158 }
159 
160 template <typename MeasVar, typename StateVar> Probability
162  const StateVar& x)
163 {
164  assert(_systemWithoutSensorParams == true);
165  _MeasurementPdf->ConditionalArgumentSet(0,x);
166  return _MeasurementPdf->ProbabilityGet(z);
167 }
#define StateVar
Definition: asirfilter.h:22
#define BFL_ERRMISUSE
T & ValueGet()
Get the value of the Sample.
#define MeasVar
Definition: asirfilter.h:23
Class representing a probability (a double between 0 and 1)
Definition: bfl_constants.h:39
unsigned int NumConditionalArgumentsGet() const
Get the Number of conditional arguments.


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 28 2022 21:56:33