PaFil.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 __PAR_FIL_H_
#define __PAR_FIL_H_

#include "MutLoc.h"

namespace MipAlgorithms{
 typedef enum{
  ABS,
  REL
 }ParticleFilterType;
};


namespace MipAlgorithms{
 class Particle {     // Per ogni singola particella 
 //ho incominciato a scrivere i campi che sicuramente ci dovranno essere e che usa il resampling
  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();
   }
 };
}; // end namespace MipAlgorithms


namespace MipAlgorithms{
 class ParFilParams{
  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;
   ParFilParams(){
   }
   
   ParFilParams( 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; 
   }
 
   ParFilParams(const ParFilParams &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 ParFilParams &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 ParFilParams: " << 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();
   }
 };
}; // end namespace MipAlgorithms


namespace MipAlgorithms{
 class ParFilVariables{
  public:
   int step;
   vector<Particle> map;
   Pose Ixi_tmeno1;
   Pose Jxj_tmeno1;
   Pose bestIxJ_mean;
   Pose bestIxJ_winner;
   int bestParticle;
   Pose myLastConf;
   Pose hisLastConf;
   
   
   ParFilVariables (){
    step = 0;
    Ixi_tmeno1 = Pose (); 
    Jxj_tmeno1 = Pose (); 
    bestIxJ_mean = Pose ();
    bestIxJ_winner = Pose ();
    bestParticle=0;
    myLastConf = Pose();  
    hisLastConf = Pose(); 
   }
   
   ParFilVariables(const ParFilVariables &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!!!!!!!!!
    }
   
   ParFilVariables operator=(ParFilVariables &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;
   }
   
   ~ParFilVariables(){
    //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();
   }
 };
};// end namespace MipAlgorithms


namespace MipAlgorithms{
 class ParticleFilter : public MutLocFilter{
  public:
   
   //costruttori
   ParticleFilter(){
   }
 
   ParticleFilter(ParFilParams in){
    par = in;
    var = (ParFilVariables*) new ParFilVariables();
    init();
   }
   
   //metodi virtuali della classe genitore
   void step(MutLocFilInput &input);
   
   void reset(){} //qua dobbiamo eliminare i risultati delle operazioni fatte
   
   void getInternalEstimate(MutLocFilInput &input, vector<Particle> &last_map);
   
   void getInternalEstimate_abs(MutLocFilInput &input, vector<Particle> &last_map);
   
   void getInternalEstimate_rel(MutLocFilInput &input, vector<Particle> &last_map);
   
   int getEstimate(Pose* best);
   
   int getAllEstimates(vector<Pose> &all, vector<Decimal> &allMarks);
   
   void updateVar(MutLocFilInput &input, vector<Particle> &last_map, int &caso);
   
   string print();
 
   void reset(ParFilParams 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 (MutLocFilInput &input, int &caso);
   
   void ComputeWeights_abs (MutLocFilInput &input, int &caso);
   
   void ComputeWeights_rel (MutLocFilInput &input, int &caso);
   
   vector<Particle> Resample();
   
   void OutputPerformance (MutLocFilInput &input) ;
   
  protected:
   ParFilParams par;
   ParFilVariables *var;
   
  private:
   void CreateMap_abs(MutLocFilInput &input);
   
   void CreateMap_rel(MutLocFilInput &input);
   
   void PropagateMap(MutLocFilInput &input, int &caso);
   
   void PropagateMap_abs(MutLocFilInput &input, int &caso);
   
   void PropagateMap_rel(MutLocFilInput &input, int &caso);
   
   Pose* gaussPose(int &factor);
   
   void normalStep(MutLocFilInput &input);
   
   void firstStep(MutLocFilInput &input);
   
   void NormalizeWeights(int &caso);
 };
}; // end namespace MipAlgorithms

#endif