CLPaFil.h

Go to the documentation of this file.

// ----------------------------------------------------------------------------
//
// $Id$
//
// Copyright 2008, 2009, 2010, 2011, 2012  Antonio Franchi and Paolo Stegagno    
//
// This file is part of MIP.
//
// MIP is free software: you can redistribute it and/or modify
// it under the terms of the GNU General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// (at your option) any later version.
//
// MIP is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with MIP. If not, see <http://www.gnu.org/licenses/>.
//
// Contact info: antonio.franchi@tuebingen.mpg.de stegagno@diag.uniroma1.it
//
// ----------------------------------------------------------------------------



/* @{ */

#ifndef __CL_PAR_FIL_H_
#define __CL_PAR_FIL_H_

class Particle {
 public:
  Decimal weight;
  Pose p;
  
  Particle(){
  }
  
  Particle ( Pose a, Decimal w){
   p=a;
   weight=w;
   }

  Particle(const Particle &A){
   p     = A.p;
   weight= A.weight;
  }

  void operator= (const Particle &A){
   p     = A.p;
   weight= A.weight;
  } 
  
  string print(){
   stringstream s;
   s << "weight:" << this -> weight << endl << "Pose:" << this -> p.print();
   return s.str();
  }
};

class CLPaFilParams{
 public:
/*  
  FilterType filterType;
*/
  int numPart; 
  Decimal sigma_xx;
  Decimal sigma_yy;
  Decimal sigma_thetatheta;
  Decimal severity_xx;
  Decimal severity_yy;
  Decimal severity_thetatheta;
  int factor;
  int hisId;
  ParticleFilterType type;
  int tempo_in;
  CLPaFilParams(){
  }
  
  CLPaFilParams( int N, Decimal sx , Decimal sy, Decimal sth, Decimal sev_x , Decimal sev_y, Decimal sev_th, int fac, int hId, ParticleFilterType ty, int tmp){
   sigma_xx     = sx;
   sigma_yy    = sy;
   sigma_thetatheta = sth;
   severity_xx = sev_x;
   severity_yy = sev_y;
   severity_thetatheta = sev_th;
   numPart    = N;
   factor   = fac;
   hisId  = hId;
   type  = ty;
   tempo_in = tmp; 
  }

  CLPaFilParams(const CLPaFilParams &A){
   sigma_xx     = A.sigma_xx;
   sigma_yy    =  A.sigma_yy;
   sigma_thetatheta = A.sigma_thetatheta;
   severity_xx = A.severity_xx;
   severity_yy = A.severity_yy;
   severity_thetatheta = A.severity_thetatheta;
   numPart    = A.numPart;
   factor  = A.factor;
   hisId  = A.hisId;
   type  = A.type;  
   tempo_in = A.tempo_in; 
  }

  void operator= (const CLPaFilParams &A){
   sigma_xx      = A.sigma_xx;
   sigma_yy     =  A.sigma_yy;
   sigma_thetatheta = A.sigma_thetatheta;
   severity_xx = A.severity_xx;
   severity_yy = A.severity_yy;
   severity_thetatheta = A.severity_thetatheta;
   numPart     = A.numPart;
   factor  = A.factor;
   hisId  = A.hisId;
   type  = A.type;
   tempo_in = A.tempo_in;
  } 
  
  string print(){
   stringstream s;
   s << "Stampa CLPaFilParams: " << endl;
   s << "sigma_xx = " << sigma_xx << endl;
   s << "sigma_yy = " << sigma_yy << endl;
   s << "sigma_thetatheta = " << sigma_thetatheta << endl;
   s << "severity_xx = " << severity_xx << endl;
   s << "severity_yy = " << severity_yy << endl;
   s << "severity_thetatheta = " << severity_thetatheta << endl;
   s << "numPart = " << numPart << endl;
   s << "factor = " << factor << endl;
   s << "hisId = " << hisId << endl;
   s << "tempo_corrente = " << tempo_in << endl;
   if (type==ABS) s << "type = ABS" << endl;
   else  s << "type = REL" << endl;
   return s.str();
  }
};

class CLPaFilVariables{
 public:
  int step;
  vector<Particle> map;
  Pose Ixi_tmeno1;
  Pose Jxj_tmeno1;
  Pose bestIxJ_mean;
  Pose bestIxJ_winner;
  int bestParticle;
  Pose myLastConf;
  Pose hisLastConf;
  
  
  CLPaFilVariables (){
   step = 0;
   Ixi_tmeno1 = Pose (); 
   Jxj_tmeno1 = Pose (); 
   bestIxJ_mean = Pose ();
   bestIxJ_winner = Pose ();
   bestParticle=0;
   myLastConf = Pose();  
   hisLastConf = Pose(); 
  }
  
  CLPaFilVariables(const CLPaFilVariables &A){
   //un nuovo oggetto parFilVariables coi campi uguali
   step  = A.step;
   Ixi_tmeno1 = A.Ixi_tmeno1;
   Jxj_tmeno1 = A.Jxj_tmeno1;
   bestIxJ_mean = A.bestIxJ_mean;
   bestIxJ_winner = A.bestIxJ_winner;
   bestParticle = A.bestParticle;
   myLastConf = A.myLastConf;  
   hisLastConf = A.hisLastConf; 
   map = A.map; //Rivedere!!!!!!!!!
   }
  
  CLPaFilVariables operator=(CLPaFilVariables &A){
   step  = A.step;
   Ixi_tmeno1 = A.Ixi_tmeno1;
   Jxj_tmeno1 = A.Jxj_tmeno1;
   bestIxJ_mean = A.bestIxJ_mean;
   bestIxJ_winner = A.bestIxJ_winner;
   bestParticle = A.bestParticle;
   myLastConf = A.myLastConf;  
   hisLastConf = A.hisLastConf;
  }
  
  ~CLPaFilVariables(){
   //Particle.clear();
   map.clear();
  }
  
  string print(){
   stringstream s;
   s << endl;
   s << "Step: " << step << endl;
   s << "Ixi_tmeno1 = " << Ixi_tmeno1.print() << endl;
   s << "Jxj_tmeno1 = " << Jxj_tmeno1.print() << endl;
   s << "bestIxJ_mean = " << bestIxJ_mean.print() << endl;
   s << "bestIxJ_winner = " << bestIxJ_winner.print() << endl;
   s << "bestParticle = " << bestParticle << endl;
   s << "myLastConf = " << myLastConf.print() << endl; 
   s << "hisLastConf = " << hisLastConf.print() << endl;
   return s.str();
  }
};


class ParticleFilter : public LocFilter{
 public:
  
  //costruttori
  ParticleFilter(){
  }

  ParticleFilter(CLPaFilParams in){
   par = in;
   var = (CLPaFilVariables*) new CLPaFilVariables();
   init();
  }
  
  //metodi virtuali della classe genitore
  void step(LocFilInput &input);
  
  void reset(){} //qua dobbiamo eliminare i risultati delle operazioni fatte
  
  void getInternalEstimate(LocFilInput &input, vector<Particle> &last_map);
  
  void getInternalEstimate_abs(LocFilInput &input, vector<Particle> &last_map);
  
  void getInternalEstimate_rel(LocFilInput &input, vector<Particle> &last_map);
  
  int getEstimate(Pose* best);
  
  int getAllEstimates(vector<Pose> &all, vector<Decimal> &allMarks);
  
  void updateVar(LocFilInput &input, vector<Particle> &last_map, int &caso);
  
  string print();

  void reset(CLPaFilParams inPar){
   assert(initialized());
   par = inPar;
   var->step = 0;
   var->Ixi_tmeno1 = Pose();
   var->Jxj_tmeno1 = Pose();
   var->map.clear();
  }
  
  int getStep(){
   assert(initialized());
   return var->step;
  }

  Pose getIxi(){
   assert(initialized());
   return var->Ixi_tmeno1;
  }
  
  Pose getJxj(){
   assert(initialized());
   return var->Jxj_tmeno1;
  }
  
  void ComputeWeights (LocFilInput &input, int &caso);
  
  void ComputeWeights_abs (LocFilInput &input, int &caso);
  
  void ComputeWeights_rel (LocFilInput &input, int &caso);
  
   vector<Particle> Resample();
  
  void OutputPerformance (LocFilInput &input) ;
  
 protected:
  CLPaFilParams par;
  CLPaFilVariables *var;
  
 private:
  void CreateMap_abs(LocFilInput &input);
  
  void CreateMap_rel(LocFilInput &input);
  
  void PropagateMap(LocFilInput &input, int &caso);
  
  void PropagateMap_abs(LocFilInput &input, int &caso);
  
  void PropagateMap_rel(LocFilInput &input, int &caso);
  
  Pose* gaussPose(int &factor);
  
  void normalStep(LocFilInput &input);
  
  void firstStep(LocFilInput &input);
  
  void NormalizeWeights(int &caso);
};

 

#endif