PaFilBFL.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 __PA_FIL_BFL_H_
#define __PA_FIL_BFL_H_

#include <filter/bootstrapfilter.h>

#include <model/systemmodel.h>
#include <model/measurementmodel.h>

#include <iostream>
#include <fstream>

#include <pdf/conditionalpdf.h>
#include <pdf/gaussian.h>

#include "MutLoc.h"


using namespace MatrixWrapper;
using namespace BFL;
using namespace std;



namespace MipAlgorithms{
 class PaFilBFLPar{
  public:
   int particleNumber;
   Decimal randomPercentage;
   
   PaFilBFLPar(int parNum = 100, Decimal ranPercent = 0.0);
   
   PaFilBFLPar(const PaFilBFLPar & pf){
    particleNumber = pf.particleNumber;
    randomPercentage = pf.randomPercentage;
   }
   
   PaFilBFLPar & operator=(const PaFilBFLPar & pf){
    particleNumber = pf.particleNumber;
    randomPercentage = pf.randomPercentage;
   }
   
   ~PaFilBFLPar();
 };
};

namespace MipAlgorithms{
 class PaFilBFL : public MutLocFilter{
  public:
   
   PaFilBFL();
   
   PaFilBFL(PaFilBFLPar par);
   
   ~PaFilBFL();
   
   virtual void step(MutLocFilInput &input)=0;
   
   virtual void reset()=0;
   
   virtual int getEstimate(Pose* );
   
   virtual int getCovariance(MIPMatrix* cov);
   
   virtual int getAllEstimates(vector<Pose> &estimates, vector<Decimal> &weights);
   
   virtual string print();
   
  protected:
   
   BootstrapFilter<ColumnVector,ColumnVector>* _filter;
   
   PaFilBFLPar _par;
   
   bool _ignoreMeasures;
   bool _nextStepResamplingOnMeasures;
   
   bool _lastStepSystemUpdate;
   
 };
}; //End of MipAlgorithms namespace

namespace MipAlgorithms{
 class PaFilBFL3D : public MutLocFilter3D{
  public:
   
   PaFilBFL3D();
   
   PaFilBFL3D(PaFilBFLPar par);
   
   ~PaFilBFL3D();
   
   virtual void step(MutLocFilInput3D &input)=0;
   
   virtual void reset()=0;
   
   virtual int getEstimate(Pose3D* ,Velocity3D* bestVel=0);
   
   virtual int getCovariance(MIPMatrix* cov);
   
   virtual int getAllEstimates(vector<Pose3D> &estimates, vector<Decimal> &weights,vector<Velocity3D> &velEstimates);
   
   virtual Decimal getLastOdomUpdateTime();
   
   virtual string print();
   
  protected:
   
   BootstrapFilter<ColumnVector,ColumnVector>* _filter;
   
   PaFilBFLPar _par;
   
   bool _ignoreMeasures;
   bool _nextStepResamplingOnMeasures;
   
   bool _lastStepSystemUpdate;
   
 };
}; //End of MipAlgorithms namespace

#endif