GestureRecognitionToolkit  Version: 0.2.5
The Gesture Recognition Toolkit (GRT) is a cross-platform, open-source, c++ machine learning library for real-time gesture recognition.
ParticleFilter.h
Go to the documentation of this file.
1 
12 /*
13  GRT MIT License
14  Copyright (c) <2012> <Nicholas Gillian, Media Lab, MIT>
15 
16  Permission is hereby granted, free of charge, to any person obtaining a copy of this software
17  and associated documentation files (the "Software"), to deal in the Software without restriction,
18  including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense,
19  and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so,
20  subject to the following conditions:
21 
22  The above copyright notice and this permission notice shall be included in all copies or substantial
23  portions of the Software.
24 
25  THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT
26  LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.
27  IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
28  WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE
29  SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
30  */
31 
32 #ifndef GRT_PARTICLE_FILTER_HEADER
33 #define GRT_PARTICLE_FILTER_HEADER
34 
35 #include "Particle.h"
36 #include "../../Util/GRTCommon.h"
37 
38 GRT_BEGIN_NAMESPACE
39 
40 template<class PARTICLE,class SENSOR_DATA>
42 
43 public:
48  initialized = false;
49  verbose = true;
50  normWeights = true;
51  initMode = INIT_MODE_UNIFORM;
52  estimationMode = WEIGHTED_MEAN;
53  numParticles = 0;
54  stateVectorSize = 0;
55  numDeadParticles = 0;
56  wNorm = 0;
57  wDotProduct = 0;
58  minimumWeightThreshold = 1.0e-20;
59  resampleThreshold = 1.0e-5;
62  warningLog.setKey("[WARNING ParticleFilter]");
63  errorLog.setKey("[ERROR ParticleFilter]");
64  }
65 
70  *this = rhs;
71  }
72 
76  virtual ~ParticleFilter(){
77  }
78 
85  PARTICLE& operator[](const unsigned int &i){
86  return particles[i];
87  }
88 
95  const PARTICLE& operator[](const unsigned int &i) const {
96  return particles[i];
97  }
98 
105  PARTICLE& operator()(const unsigned int &i){
107  }
108 
115  const PARTICLE& operator()(const unsigned int &i) const {
117  }
118 
123  if( this != &rhs ){
124  this->initialized = rhs.initialized;
125  this->verbose = rhs.verbose;
126  this->normWeights = rhs.normWeights;
127  this->numParticles= rhs.numParticles;
128  this->stateVectorSize = rhs.stateVectorSize;
129  this->initMode = rhs.initMode;
130  this->estimationMode= rhs.estimationMode;
131  this->numDeadParticles = rhs.numDeadParticles;
132  this->wNorm = rhs.wNorm;
133  this->wDotProduct = rhs.wDotProduct;
138  this->x = rhs.x;
139  this->initModel = rhs.initModel;
140  this->processNoise = rhs.processNoise;
144  if( &rhs.particles == &rhs.particleDistributionA ){
146  }else{
148  }
149  this->cumsum = rhs.cumsum;
150  this->warningLog = rhs.warningLog;
151  this->errorLog = rhs.errorLog;
152  }
153  return *this;
154  }
155 
161 
162  //Clear any previous setup
163  clear();
164 
165  //Check to make sure each Vector in the initModel has 2 dimensions, these are min and max or mu and sigma depening on the init mode
166  for(unsigned int i=0; i<initModel.getSize(); i++){
167  if( initModel[i].getSize() != 2 ){
168  errorLog << __GRT_LOG__ << " The " << i << " dimension of the initModel does not have 2 dimensions!" << std::endl;
169  return false;
170  }
171  }
172 
173  stateVectorSize = initModel.getSize();
174  this->initModel = initModel;
175  this->processNoise = processNoise;
176  this->measurementNoise = measurementNoise;
178  initialized = true;
179 
180  if( !initParticles( numParticles ) ){
181  errorLog << __GRT_LOG__ << " Failed to init particles!" << std::endl;
182  clear();
183  return false;
184  }
185 
186  return true;
187  }
188 
195  virtual bool initParticles( const UINT numParticles ){
196 
197  if( !initialized ) return false;
198 
199  if( stateVectorSize != x.size() ){
200  return false;
201  }
202 
203  this->numParticles = numParticles;
204  particleDistributionA.clear();
205  particleDistributionB.clear();
206  cumsum.clear();
207  particleDistributionA.resize( numParticles, PARTICLE(stateVectorSize) );
208  particleDistributionB.resize( numParticles, PARTICLE(stateVectorSize) );
210  cumsum.resize( numParticles,0 );
211 
212  reset();
213 
214  return true;
215  }
216 
217 
224  virtual bool filter(SENSOR_DATA &data){
225 
226  if( !initialized ){
227  errorLog << __GRT_LOG__ << " The particle filter has not been initialized!" << std::endl;
228  return false;
229  }
230 
231  if( !preFilterUpdate( data ) ){
232  errorLog << __GRT_LOG__ << " Failed to complete preFilterUpdate!" << std::endl;
233  return false;
234  }
235 
236  unsigned int i = 0;
237  typename Vector< PARTICLE >::iterator iter;
238 
239  //The main particle prediction loop
240  for( iter = particles.begin(), i = 0; iter != particles.end(); ++iter, i++ ){
241  if( !predict( *iter ) ){
242  errorLog << __GRT_LOG__ << " Particle " << i << " failed prediction!" << std::endl;
243  return false;
244  }
245  }
246 
247  //The main particle update loop
248  for( iter = particles.begin(), i = 0; iter != particles.end(); ++iter, i++ ){
249  if( !update( *iter, data ) ){
250  errorLog << __GRT_LOG__ << " Particle " << i << " failed update!" << std::endl;
251  return false;
252  }
253  }
254 
255  //Normalize the particle weights so they sum to 1
256  if( normWeights ){
257  if( !normalizeWeights() ){
258  errorLog << __GRT_LOG__ << " Failed to normalize particle weights! " << std::endl;
259  return false;
260  }
261  }
262 
263  //Compute the final state estimate
264  if( !computeEstimate() ){
265  errorLog << __GRT_LOG__ << " Failed to compute the final estimat!" << std::endl;
266  return false;
267  }
268 
269  //Check to see if we should resample the particles for the next iteration
270  if( checkForResample() ){
271 
272  //Resample the particles
273  if( !resample() ){
274  errorLog << __GRT_LOG__ << " Failed to resample particles!" << std::endl;
275  return false;
276  }
277  }
278 
279  if( !postFilterUpdate( data ) ){
280  errorLog << __GRT_LOG__ << " Failed to complete postFilterUpdate!" << std::endl;
281  return false;
282  }
283 
284  return true;
285  }
286 
292  virtual bool clear(){
293  initialized = false;
294  numParticles = 0;
295  stateVectorSize = 0;
297  wNorm = 0;
298  wDotProduct = 0;
299  numDeadParticles = 0;
300  x.clear();
301  initModel.clear();
302  processNoise.clear();
303  measurementNoise.clear();
304  particleDistributionA.clear();
305  particleDistributionB.clear();
306  cumsum.clear();
307  return true;
308  }
309 
310  virtual bool reset(){
311 
312  if( !initialized ){
313  return false;
314  }
315 
316  for(unsigned int i=0; i<numParticles; i++){
317  for(unsigned int j=0; j<stateVectorSize; j++){
318  switch( initMode ){
319  case INIT_MODE_UNIFORM:
321  break;
322  case INIT_MODE_GAUSSIAN:
323  particles[i].x[j] = initModel[j][0] + rand.getRandomNumberGauss(0,initModel[j][1]);
324  break;
325  default:
326  errorLog << __GRT_LOG__ << " Unknown initMode!" << std::endl;
327  return false;
328  break;
329  }
330 
331  }
332  }
333 
334  return true;
335  }
336 
340  bool getInitialized() const {
341  return initialized;
342  }
343 
349  unsigned int getNumParticles() const{
350  return initialized ? (unsigned int)particles.size() : 0;
351  }
352 
358  unsigned int getStateVectorSize() const{
359  return initialized ? stateVectorSize : 0;
360  }
361 
367  unsigned int getNumDeadParticles() const{
368  return numDeadParticles;
369  }
370 
376  unsigned int getInitMode() const{
377  return initMode;
378  }
379 
385  unsigned int getEstimationMode() const{
386  return estimationMode;
387  }
388 
395  Float getEstimationLikelihood() const{
396  return estimationLikelihood;
397  }
398 
404  Float getWeightSum() const {
405  return wNorm;
406  }
407 
414  return x;
415  }
416 
424  }
425 
433  }
434 
441  return processNoise;
442  }
443 
450  bool setVerbose(const bool verbose){
451  this->verbose = verbose;
452  return true;
453  }
454 
462  this->normWeights = normWeights;
463  return true;
464  }
465 
475  this->resampleThreshold = resampleThreshold;
476  return true;
477  }
478 
485  bool setEstimationMode(const unsigned int estimationMode){
486  if( estimationMode == MEAN || estimationMode == WEIGHTED_MEAN ||
487  estimationMode == ROBUST_MEAN || estimationMode == BEST_PARTICLE )
488  {
489  this->estimationMode = estimationMode;
490  return true;
491  }
492  return false;
493  }
494 
501  bool setInitMode(const unsigned int initMode){
502  if( initMode == INIT_MODE_UNIFORM || initMode == INIT_MODE_GAUSSIAN )
503  {
504  this->initMode = initMode;
505  return true;
506  }
507  return false;
508  }
509 
520  if( this->initModel.size() == initModel.size() ){
521  this->initModel = initModel;
522  return true;
523  }
524  return false;
525  }
526 
534  if( this->processNoise.size() == processNoise.size() ){
535  this->processNoise = processNoise;
536  return true;
537  }
538  return false;
539  }
540 
548  if( this->measurementNoise.size() == measurementNoise.size() ){
549  this->measurementNoise = measurementNoise;
550  return true;
551  }
552  return false;
553  }
554 
555 protected:
562  virtual bool predict( PARTICLE &p ){
563  errorLog << __GRT_LOG__ << " Prediction function not implemented! This must be implemented by the derived class!" << std::endl;
564  return false;
565  }
566 
577  virtual bool update( PARTICLE &p, SENSOR_DATA &data ){
578  errorLog << __GRT_LOG__ << " Update function not implemented! This must be implemented by the derived class!" << std::endl;
579  return false;
580  }
581 
588  virtual bool normalizeWeights(){
589 
590  //Compute the total particle weight and number of dead particles
591  wNorm = 0;
592  wDotProduct = 0;
593  numDeadParticles = 0;
594  typename Vector< PARTICLE >::iterator iter;
595  for( iter = particles.begin(); iter != particles.end(); ++iter ){
596  if( grt_isinf( iter->w ) ){
597  numDeadParticles++;
598  iter->w = 0;
599  }else{
600  wNorm += iter->w;
601  }
602  }
603 
604  if( wNorm == 0 ){
605  if( verbose )
606  warningLog << __GRT_LOG__ << " Weight norm is zero!" << std::endl;
607  return true;
608  }
609 
610  //Normalized the weights so they sum to 1
611  Float weightUpdate = 1.0 / wNorm;
612  for( iter = particles.begin(); iter != particles.end(); ++iter ){
613 
614  //Normalize the weights (so they sum to 1)
615  iter->w *= weightUpdate;
616 
617  //Compute the total weights dot product (this is used later to test for degeneracy)
618  wDotProduct += iter->w * iter->w;
619  }
620  wDotProduct = 1.0 / wDotProduct;
621 
622  //cout << "wNorm: " << wNorm << " wDotProduct: " << wDotProduct << std::endl;
623 
624  return true;
625  }
626 
634  virtual bool computeEstimate(){
635 
636  typename Vector< PARTICLE >::iterator iter;
637  const unsigned int N = (unsigned int)x.size();
638  unsigned int bestIndex = 0;
639  unsigned int robustMeanParticleCounter = 0;
640  Float bestWeight = 0;
641  Float sum = 0;
643  switch( estimationMode ){
644  case MEAN:
645  for(unsigned int j=0; j<N; j++){
646  x[j] = 0;
647  }
648 
649  for( iter = particles.begin(); iter != particles.end(); ++iter ){
650  for(unsigned int j=0; j<N; j++){
651  x[j] += iter->x[j];
652  }
653  estimationLikelihood += grt_isnan(iter->w) ? 0 : iter->w;
654  }
655 
656  for(unsigned int j=0; j<N; j++){
657  x[j] /= Float(numParticles);
658  }
660  break;
661  case WEIGHTED_MEAN:
662  for(unsigned int j=0; j<N; j++){
663  x[j] = 0;
664  sum = 0;
665  for( iter = particles.begin(); iter != particles.end(); ++iter ){
666  x[j] += iter->x[j] * iter->w;
667  sum += iter->w;
668  }
669  x[j] /= sum;
670  }
671 
672  for( iter = particles.begin(); iter != particles.end(); ++iter ){
673  estimationLikelihood += grt_isnan(iter->w) ? 0 : iter->w;
674  }
676  break;
677  case ROBUST_MEAN:
678  //Reset x
679  for(unsigned int j=0; j<N; j++){
680  x[j] = 0;
681  }
682  sum = 0;
683 
684  //Find the particle with the best weight
685  for(unsigned int i=0; i<numParticles; i++){
686  if( particles[i].w > bestWeight ){
687  bestWeight = particles[i].w;
688  bestIndex = i;
689  }
690  }
691 
692  //Use all the particles within a given distance of that weight
693  for( iter = particles.begin(); iter != particles.end(); ++iter ){
694  if( fabs( iter->w - particles[bestIndex].w ) <= robustMeanWeightDistance ){
695  for(unsigned int j=0; j<N; j++){
696  x[j] += iter->x[j] * iter->w;
697  }
698  estimationLikelihood += grt_isnan(iter->w) ? 0 : iter->w;
699  sum += iter->w;
700  robustMeanParticleCounter++;
701  }
702  }
703 
704  //Normalize x
705  for(unsigned int j=0; j<N; j++){
706  x[j] /= sum;
707  }
708  estimationLikelihood /= Float(robustMeanParticleCounter);
709  break;
710  case BEST_PARTICLE:
711  for(unsigned int i=0; i<numParticles; i++){
712  if( particles[i].w > bestWeight ){
713  bestWeight = particles[i].w;
714  bestIndex = i;
715  }
716  }
717  x = particles[bestIndex].x;
718  estimationLikelihood = grt_isnan(particles[bestIndex].w) ? 0 : particles[bestIndex].w;
719  break;
720  default:
721  errorLog << __GRT_LOG__ << " Unknown estimation mode!" << std::endl;
722  return false;
723  break;
724  }
725  return true;
726  }
727 
734  virtual bool checkForResample(){
735  return wNorm < resampleThreshold;
736  }
737 
744  virtual bool resample(){
745 
746  Vector< PARTICLE > *tempParticles = NULL;
747 
748  //If the particles are currently reference the first particle distribution, then set the tempParticles to the second distribution
749  if( &particles == &particleDistributionA ) tempParticles = &particleDistributionB;
750  else tempParticles = &particleDistributionA;
751 
752  //Select any weight that is above the minimum weight threshold
753  Vector< IndexedDouble > weights;
754  weights.reserve(numParticles);
755  for(unsigned int i=0; i<numParticles; i++){
756  if( particles[i].w >= minimumWeightThreshold ){
757  weights.push_back( IndexedDouble(i,particles[i].w) );
758  }
759  }
760 
761  //Sort the weights
762  sort(weights.begin(),weights.end(),IndexedDouble::sortIndexedDoubleByValueAscending);
763  const unsigned int numWeights = (unsigned int)weights.size();
764  unsigned int randIndex = 0;
765 
766  //If there are no valid weights then we just pick N random particles
767  if( numWeights == 0 ){
768  for(unsigned int n=0; n<numParticles; n++){
769  (*tempParticles)[n] = particles[ rand.getRandomNumberInt(0, numParticles) ];
770  }
773  return true;
774  }
775 
776  //Compute the cumulative sum
777  cumsum[0] = weights[0].value;
778  for(unsigned int i=1; i<numWeights; i++){
779  cumsum[i] = cumsum[i-1] + weights[i].value;
780  }
781 
782  //Resample the weights
783  const unsigned int numRandomParticles = (unsigned int) round(numParticles/100.0*10.0);
784  for(unsigned int n=0; n<numParticles; n++){
785 
786  if( numParticles-n > numParticles-numRandomParticles){
787 
788  //Pick a random number between 0 and the max cumsum
789  Float randValue = rand.getRandomNumberUniform(0,cumsum[numWeights-1]);
790  randIndex = 0;
791 
792  //Find which bin the rand value falls into, set the random index to this value
793  for(unsigned int i=0; i<numWeights; i++){
794  if( randValue <= cumsum[i] ){
795  randIndex = i;
796  break;
797  }
798  }
799 
800  (*tempParticles)[n] = particles[ weights[randIndex].index ];
801  }else{
802  //Randomly initalize the weakest particles
803  PARTICLE &p = (*tempParticles)[n];
804  for(unsigned int j=0; j<stateVectorSize; j++){
805  switch( initMode ){
806  case INIT_MODE_UNIFORM:
807  p.x[j] = rand.getRandomNumberUniform(initModel[j][0],initModel[j][1]);
808  break;
809  case INIT_MODE_GAUSSIAN:
810  p.x[j] = initModel[j][0] + rand.getRandomNumberGauss(0,initModel[j][1]);
811  break;
812  default:
813  errorLog << __GRT_LOG__ << " Unknown initMode!" << std::endl;
814  return false;
815  break;
816  }
817 
818  }
819  }
820  }
821 
822  //Swap the particle references
825 
826  return true;
827  }
828 
834  virtual bool preFilterUpdate( SENSOR_DATA &data ){
835  return true;
836  }
837 
843  virtual bool postFilterUpdate( SENSOR_DATA &data ){
844  return true;
845  }
846 
855  Float gauss(Float x,Float mu,Float sigma){
856  return 1.0/(SQRT_TWO_PI*sigma) * exp( -SQR(x-mu)/(2.0*SQR(sigma)) );
857  }
858 
870  Float rbf(const Float x,const Float mu,Float sigma,Float weight=1.0){
871  return weight * exp( -SQR( fabs(x-mu) / sigma ) );
872  }
873 
885  Float rbf(const VectorFloat &x,const VectorFloat &mu,Float sigma,Float weight=1.0){
886  Float sum = 0;
887  const unsigned int N = (unsigned int)x.size();
888  for(UINT i=0; i<N; i++){
889  sum += fabs(x[i]-mu[i]);
890  }
891  return weight * exp( -SQR(sum/sigma) );
892  }
893 
900  Float SQR(const Float x){ return x*x; }
901 
902  bool initialized;
903  bool verbose;
904  bool normWeights;
905  unsigned int numParticles;
906  unsigned int stateVectorSize;
907  unsigned int initMode;
908  unsigned int estimationMode;
909  unsigned int numDeadParticles;
913  Float wNorm;
914  Float wDotProduct;
925  WarningLog warningLog;
926  ErrorLog errorLog;
927 
928 public:
929  enum InitModes{INIT_MODE_UNIFORM=0,INIT_MODE_GAUSSIAN};
930  enum EstimationModes{MEAN=0,WEIGHTED_MEAN,ROBUST_MEAN,BEST_PARTICLE};
931 
932 };
933 
934 GRT_END_NAMESPACE
935 
936 #endif //GRT_PARTICLE_FILTER_HEADER
Float getWeightSum() const
virtual bool update(PARTICLE &p, SENSOR_DATA &data)
bool setNormalizeWeights(const bool normWeights)
virtual bool initParticles(const UINT numParticles)
virtual bool filter(SENSOR_DATA &data)
bool setEstimationMode(const unsigned int estimationMode)
bool initialized
A flag that indicates if the filter has been initialized.
virtual bool postFilterUpdate(SENSOR_DATA &data)
VectorFloat x
The state estimation.
bool verbose
A flag that indicates if warning and info messages should be printed.
ParticleFilter & operator=(const ParticleFilter &rhs)
Float rbf(const Float x, const Float mu, Float sigma, Float weight=1.0)
bool setInitModel(const Vector< VectorFloat > initModel)
This file contains the Random class, a useful wrapper for generating cross platform random functions...
Definition: Random.h:46
virtual bool resize(const unsigned int size)
Definition: Vector.h:133
unsigned int getStateVectorSize() const
Float rbf(const VectorFloat &x, const VectorFloat &mu, Float sigma, Float weight=1.0)
virtual bool setKey(const std::string &key)
sets the key that gets written at the start of each message, this will be written in the format &#39;key ...
Definition: Log.h:166
UINT getSize() const
Definition: Vector.h:201
Float getRandomNumberGauss(Float mu=0.0, Float sigma=1.0)
Definition: Random.cpp:142
Float robustMeanWeightDistance
The distance parameter used in the ROBUST_MEAN estimation mode.
bool setProcessNoise(const VectorFloat &processNoise)
bool setResampleThreshold(const Float resampleThreshold)
unsigned int initMode
The mode used to initialize the particles, this should be one of the InitModes enums.
Float wDotProduct
Stores the dot product of all the weights, used to test for degeneracy.
Float estimationLikelihood
The likelihood of the estimated state.
virtual bool checkForResample()
virtual ~ParticleFilter()
PARTICLE & operator()(const unsigned int &i)
unsigned int getEstimationMode() const
const PARTICLE & operator()(const unsigned int &i) const
Float SQR(const Float x)
bool getInitialized() const
ParticleFilter(const ParticleFilter &rhs)
Vector< VectorFloat > initModel
The noise model for the initial starting guess.
bool normWeights
A flag that indicates if the weights should be normalized at each filter iteration.
const PARTICLE & operator[](const unsigned int &i) const
unsigned int estimationMode
The estimation mode (used to compute the state estimation)
bool setVerbose(const bool verbose)
Vector< PARTICLE > getParticles()
unsigned int getInitMode() const
VectorFloat processNoise
The noise covariance in the system.
Vector< PARTICLE > getOldParticles()
VectorFloat cumsum
The cumulative sum Vector used for resampling the particles.
VectorFloat measurementNoise
The noise covariance in the measurement.
virtual bool clear()
bool setMeasurementNoise(const VectorFloat &measurementNoise)
unsigned int getNumParticles() const
unsigned int getNumDeadParticles() const
virtual bool computeEstimate()
Float resampleThreshold
The threshold below which the particles will be resampled.
virtual bool normalizeWeights()
unsigned int numParticles
The number of particles in the filter.
Random rand
A random number generator.
Float wNorm
Stores the total weight norm value.
Float gauss(Float x, Float mu, Float sigma)
Float getRandomNumberUniform(Float minRange=0.0, Float maxRange=1.0)
Definition: Random.cpp:129
Vector< PARTICLE > & particles
A reference to the current active particle Vector.
int getRandomNumberInt(int minRange, int maxRange)
Definition: Random.cpp:59
Vector< PARTICLE > particleDistributionB
A Vector of particles, this will either hold the particles before or after a resample.
Float minimumWeightThreshold
Any weight below this value will not be resampled.
unsigned int stateVectorSize
The size of the state Vector (x)
VectorFloat setProcessNoise() const
virtual bool init(const unsigned int numParticles, const Vector< VectorFloat > &initModel, const VectorFloat &processNoise, const VectorFloat &measurementNoise)
Float getEstimationLikelihood() const
PARTICLE & operator[](const unsigned int &i)
bool setInitMode(const unsigned int initMode)
virtual bool predict(PARTICLE &p)
virtual bool resample()
virtual bool preFilterUpdate(SENSOR_DATA &data)
VectorFloat getStateEstimation() const
Vector< PARTICLE > particleDistributionA
A Vector of particles, this will either hold the particles before or after a resample.