RelPaFil.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 __REL_PA_FIL_H_
#define __REL_PA_FIL_H_

#include "MutLoc.h"

namespace MipAlgorithms{
 class RelParticle {
  public:
   Decimal weight;
   Pose p;
   
   RelParticle(){
   }
   
   RelParticle ( Pose a, Decimal w){
    p=a;
    weight=w;
    }
   
   RelParticle(const RelParticle &A){
    p     = A.p;
    weight= A.weight;
   }
   
   void operator= (const RelParticle &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 RelPaFilParams{
  public:
   int numPart; 
   Decimal sigma_xx;
   Decimal sigma_yy;
   Decimal sigma_thetatheta;
   Decimal severity_xx;
   Decimal severity_yy;
   Decimal severity_thetatheta;
   int factor;
   int hisId;
   RelParticleFilterType type;
   int tempo_in;
   RelPaFilParams(){
   }
   
   RelPaFilParams( int N, Decimal sx , Decimal sy, Decimal sth, Decimal sev_x , Decimal sev_y, Decimal sev_th, int fac, int hId, RelParticleFilterType 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; 
   }
 
   RelPaFilParams(const RelPaFilParams &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 RelPaFilParams &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 RelPaFilParams: " << 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();
   }
 };
};


namespace MipAlgorithms{
 class RelPaFilVariables{
  public:
   int step;
   vector<RelParticle> map;
   Pose Ixi_tmeno1;
   Pose Jxj_tmeno1;
   Pose bestIxJ_mean;
   Pose bestIxJ_winner;
   int bestRelParticle;
   Pose myLastConf;
   Pose hisLastConf;
   
   
   RelPaFilVariables (){
    step = 0;
    Ixi_tmeno1 = Pose (); 
    Jxj_tmeno1 = Pose (); 
    bestIxJ_mean = Pose ();
    bestIxJ_winner = Pose ();
    bestRelParticle=0;
    myLastConf = Pose();  
    hisLastConf = Pose(); 
   }
   
   RelPaFilVariables(const RelPaFilVariables &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;
    bestRelParticle = A.bestRelParticle;
    myLastConf = A.myLastConf;  
    hisLastConf = A.hisLastConf; 
    map = A.map; //Rivedere!!!!!!!!!
    }
   
   RelPaFilVariables operator=(RelPaFilVariables &A){
    step  = A.step;
    Ixi_tmeno1 = A.Ixi_tmeno1;
    Jxj_tmeno1 = A.Jxj_tmeno1;
    bestIxJ_mean = A.bestIxJ_mean;
    bestIxJ_winner = A.bestIxJ_winner;
    bestRelParticle = A.bestRelParticle;
    myLastConf = A.myLastConf;  
    hisLastConf = A.hisLastConf;
   }
   
   ~RelPaFilVariables(){
    //RelParticle.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 << "bestRelParticle = " << bestRelParticle << endl;
    s << "myLastConf = " << myLastConf.print() << endl; 
    s << "hisLastConf = " << hisLastConf.print() << endl;
    return s.str();
   }
 };
};// end namespace MipAlgorithms



namespace MipAlgorithms{
 class RelParticleFilter : public MutLocFilter{
  public:
   
   //costruttori
   RelParticleFilter(){
   }
 
   RelParticleFilter(RelPaFilParams in){
    par = in;
    var = (RelPaFilVariables*) new RelPaFilVariables();
    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<RelParticle> &last_map);
   
   void getInternalEstimate_abs(MutLocFilInput &input, vector<RelParticle> &last_map);
   
   void getInternalEstimate_rel(MutLocFilInput &input, vector<RelParticle> &last_map);
   
   int getEstimate(Pose* best);
   
   int getAllEstimates(vector<Pose> &all, vector<Decimal> &allMarks);
   
   void updateVar(MutLocFilInput &input, vector<RelParticle> &last_map, int &caso);
   
   string print();
 
   void reset(RelPaFilParams 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<RelParticle> Resample();
   
   void OutputPerformance (MutLocFilInput &input) ;
   
  protected:
   RelPaFilParams par;
   RelPaFilVariables *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