MutLocFASTSlam.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 __MUT_LOC_FAST_SLAM_H
#define __MUT_LOC_FAST_SLAM_H

#include <Spaces.h>
#include <EKFilterUnicycle2D.h>
#include <Roulette.h>

#define EKFILTERUNICYCLE2D_NUMPARTICLES 1000

namespace MipAlgorithms{
 
 class MutLocFASTSlamParticle{
  
  public:
   
   
   vector<int> _ekfsIds;
   map<int, EKFilterUnicycle2D*> _ekfs;
   map<int, Decimal> _ekfsVals;
   
   vector<EKFilterUnicycle2D*> _stekfs;
   vector<Decimal> _stekfsVals;
   Decimal weigth;
   int weigthNum;
   
   
   MutLocFASTSlamParticle();
   
   MutLocFASTSlamParticle(const MutLocFASTSlamParticle& mfp);
   
   void operator=(const MutLocFASTSlamParticle& mfp);
   
   ~MutLocFASTSlamParticle();
   
   void addFilter(int id);
   
   void addFeature();
   
   void clearAll();
   
   const bool idActive(const int id) const;
   
   string print() const;
   
   void updateStaticFeatures(EKFilterUnicycle2DIn& in);
   
   DCol getProbabilitiesOfDetection(Decimal mB, Decimal mD, Decimal ray);
 };
 
 class MutLocFASTSlamInput{
  public:
   int        numMeas;
   vector<Position> measure;
   Pose       myConf;
   vector<int>    hisIds;
   vector<Pose>   hisConf;
   vector<int>    hisPFIds;
   vector<PosiFeatures> hisPF;
   
   pair<bool,Decimal> actualTime;
   
   MutLocFASTSlamInput();
  
   ~MutLocFASTSlamInput();
   
   string print();
 };
 
 
 
 class MutLocFASTSlam{
  
  int _counterMeasUpdate;
  
  MutLocFASTSlamParticle* _particle;
  MutLocFASTSlamParticle* _particleNew;
  
  MutLocFASTSlamParticle _particle1[EKFILTERUNICYCLE2D_NUMPARTICLES];
  MutLocFASTSlamParticle _particle2[EKFILTERUNICYCLE2D_NUMPARTICLES];
  static const int _numPart = EKFILTERUNICYCLE2D_NUMPARTICLES;
  
  vector<int> _ekfsId;
  
  
  public:
   
   void step(MutLocFASTSlamInput &input);  //filtraggio di tipo Kalman
   
//    virtual void reset()=0;
   
   virtual int getEstimate(Pose *best, int hisIds);
   
   virtual int getMeanEstimate(Pose *mean, int hisIds);
   
   virtual int getCovariance(MIPMatrix* cov){}
   
    virtual int getAllEstimates(vector<Pose> &all, vector<Decimal> &allMarks, int id);
   
    virtual int getAllFeatures(vector<Position> &all, vector<Decimal> &allMarks);
   
//    virtual string print()=0;
 
   MutLocFASTSlam();
   
   ~MutLocFASTSlam();
   
  private:
   
   bool _initialized;
   
   bool _findId(int id);
   void _addId(int id);
   void _sysUpdate(EKFilterUnicycle2DIn &in);
   
   void _firstStep(MutLocFASTSlamInput &input);
   void _normalStep(MutLocFASTSlamInput &input);
   void _resampling();
   void _resampling2();
   
   void _rateParticle(MutLocFASTSlamParticle &p, int i, MutLocFASTSlamInput input);
   
 };
 
};// end namespace MipAlgorithms

#endif



/* @} */