MutLoc.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_H
#define __MUT_LOC_H

#include <Spaces.h>
#include <MIPMatrix.h>

namespace MipAlgorithms{
 enum FilterType{
  FINITE_MEM,
  PARFIL,
  INFINITE_MEM,
  NUM_FILTER_TYPE
 };
 
 static const char* FilterTypeName[NUM_FILTER_TYPE] ={
  "FINITE_MEM",
  "PAFIL",
  "INFINITE_MEM",
 };
};// end namespace MipAlgorithms


namespace MipAlgorithms{
 class MutLocFilInput{
  public:
   int        numMeas;
   vector<Pose>   measure;
   vector<MIPMatrix> measCov;
   Pose       myConf;
   MIPMatrix*    myCov;
   Pose       hisConf;
   MIPMatrix*    hisCov;
   Decimal      rightProb;
   bool       updateValue;

                        pair<bool,Decimal>                                      actualTime;

                        vector<Decimal>                                         priorWeights;
  
   MutLocFilInput();
  
   ~MutLocFilInput();
   
   string print();
 };
 
 class MutLocFilInput3D{
  public:
   int            numMeas;
   vector<Pose3D>      measure;
   vector<MIPMatrix>     measCov;
   Acceleration3D      myConfAcc;
   Velocity3D        myConfAngVel;
   Velocity3D        myConfLinVel;
   MIPMatrix*        myCov;
   Acceleration3D      hisConfAcc;
   Velocity3D        hisConfAngVel;
   Velocity3D        hisConfLinVel;
   MIPMatrix*        hisCov;
   Decimal          rightProb;
   bool           useVelMeasurements;
   
//    Acceleration3D      myPrevConfAcc;
//    Velocity3D      myPrevConfAngVel;
//    Acceleration3D      hisPrevConfAcc;
//    Velocity3D      hisPrevConfAngVel;
   
   
   bool       updateValue;
   
   bool        sysMeasUpdate;
   
   Velocity3D      myOmegaVel;
   
   pair<bool,Decimal>                                      actualTime;
   
   vector<Decimal>                                         priorWeights;
   
   MutLocFilInput3D();
   
   ~MutLocFilInput3D();
   
   string print();
 };
}; // end namespace MipAlgorithms


namespace MipAlgorithms{
 class MutLocFilter{
  
  public:
   
   virtual void step(MutLocFilInput &input)=0;  //filtraggio di tipo Kalman
   
   virtual void reset()=0;
   
   virtual int getEstimate(Pose *best)=0;
   
   virtual int getCovariance(MIPMatrix* cov){}
   
   virtual int getAllEstimates(vector<Pose> &all, vector<Decimal> &allMarks)=0;
   
   virtual string print()=0;
 
   MutLocFilter();
   
   ~MutLocFilter();
   
   void init();
   
   void uninit();
   
   bool initialized();
  
   bool firstPosteriorComputed(){
    return _firstPostComputed;
   }
   
   void setFirstPosteriorComputed(){
    _firstPostComputed = true;
   }
   
   
  private:
   
   bool _initialized;
   bool _firstPostComputed;
 };
 
 class MutLocFilter3D{
  
  public:
   
   virtual void step(MutLocFilInput3D &input)=0;  //filtraggio di tipo Kalman
   
   virtual void reset()=0;
   
   virtual int getEstimate(Pose3D *best,Velocity3D* bestVel=0)=0;
   
   virtual int getCovariance(MIPMatrix* cov){}
   
   virtual int getAllEstimates(vector<Pose3D> &all, vector<Decimal> &allMarks,vector<Velocity3D> &velEstimates)=0;
   
   virtual string print()=0;
 
   MutLocFilter3D();
   
   ~MutLocFilter3D();
   
   void init();
   
   void uninit();
   
   bool initialized();
  
   bool firstPosteriorComputed(){
    return _firstPostComputed;
   }
   
   void setFirstPosteriorComputed(){
    _firstPostComputed = true;
   }
   
   
  private:
   
   bool _initialized;
   bool _firstPostComputed;
 };
};// end namespace MipAlgorithms

#endif



/* @} */