GestureRecognitionToolkit  Version: 1.0 Revision: 04-03-15
The Gesture Recognition Toolkit (GRT) is a cross-platform, open-source, c++ machine learning library for real-time gesture recognition.
FSMParticleFilter.h
Go to the documentation of this file.
1 
27 #ifndef GRT_FSM_PARTICLE_FILTER_HEADER
28 #define GRT_FSM_PARTICLE_FILTER_HEADER
29 
30 #include "../../CoreAlgorithms/ParticleFilter/ParticleFilter.h"
31 #include "FSMParticle.h"
32 
33 namespace GRT{
34 
35 class FSMParticleFilter : public ParticleFilter< FSMParticle, VectorDouble >{
36 public:
37  FSMParticleFilter():errorLog("[ERROR FSMParticleFilter]"){
38  pt = NULL;
39  pe = NULL;
40  }
41 
42  virtual ~FSMParticleFilter(){
43 
44  }
45 
46  virtual bool predict( FSMParticle &p ){
47 
48  if( !initialized ){
49  errorLog << "predict( FSMParticle &p ) - Particle Filter has not been initialized!" << endl;
50  return false;
51  }
52 
53  if( pt == NULL || pe == NULL ){
54  errorLog << "predict( FSMParticle &p ) - pt or pe are NULL!" << endl;
55  return false;
56  }
57 
58  //Update the particles current state
59  p.currentState = random.getRandomNumberWeighted( pt->at( p.currentState ) );
60 
61  //Get the model for the current state
62  const vector< VectorDouble > &model = pe->at( p.currentState );
63 
64  //Pick a random sample from the model and set this as the particles state vector
65  const unsigned int K = (unsigned int)model.size();
66  if( K > 0 )
67  p.x = model[ random.getRandomNumberInt(0, K) ];
68 
69  return true;
70  }
71 
72  virtual bool update( FSMParticle &p, VectorDouble &data ){
73 
74  if( !initialized ){
75  errorLog << "update( FSMParticle &p, VectorDouble &data ) - Particle Filter has not been initialized!" << endl;
76  return false;
77  }
78 
79  if( p.x.size() != data.size() ){
80  errorLog << "update( FSMParticle &p, VectorDouble &data ) - x does not match data.size()!" << endl;
81  return false;
82  }
83 
84  //Estimate the particles weight, given its current state and the sensor data
85  p.w = 1;
86  const size_t N = p.x.size();
87  for(size_t i=0; i<N; i++){
88  p.w *= gauss(p.x[i], data[i], measurementNoise[i]);
89  }
90 
91  cout << "w: " << p.w << " p.x: ";
92  for(size_t i=0; i<N; i++){
93  cout << p.x[i] << " ";
94  }
95  cout << " data: ";
96  for(size_t i=0; i<N; i++){
97  cout << data[i] << " ";
98  }
99  cout << endl;
100 
101  return true;
102  }
103 
104  bool setLookupTables( vector< vector< IndexedDouble > > &pt, vector< vector< VectorDouble > > &pe ){
105 
106  this->pt = &pt;
107  this->pe = &pe;
108 
109  return true;
110  }
111 
112  Random random;
113  ErrorLog errorLog;
114  vector< vector< IndexedDouble > > *pt;
115  vector< vector< VectorDouble > > *pe;
116 };
117 
118 } //End of namespace GRT
119 
120 #endif
virtual bool predict(FSMParticle &p)
Definition: AdaBoost.cpp:25
VectorDouble measurementNoise
The noise covariance in the measurement.
bool initialized
A flag that indicates if the filter has been initialized.
double gauss(double x, double mu, double sigma)
int getRandomNumberWeighted(const vector< int > &values, const vector< double > &weights)
Definition: Random.h:104
int getRandomNumberInt(int minRange, int maxRange)
Definition: Random.h:87
virtual bool update(FSMParticle &p, VectorDouble &data)