ParticleMutLocCamOffTask.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 __PARTICLE_MUT_LOC_CAM_OFF_TASK_H_
#define __PARTICLE_MUT_LOC_CAM_OFF_TASK_H_


#include <Task.h>
#include <Resource.h>
#include <MobileRob.h>
#include <DraWin.h>
#include <SharedMem.h>
#include <Time.h>

#include <MultiRegCam.h>
#include <ProbMultiRegCam.h>
#include <CommonOptions.h>
#include <MultiReg.h>
#include <ProbMultiReg.h>
#include <PaFilBFL.h>
#include <PaFilBFLAbsRel.h>
#include <PaFilBFLRelRel.h>
#include <PaFilBFLRelRelBearOnly.h>
#include <CommModule.h>


using namespace MipAlgorithms;
using namespace MipResources;

namespace MipTasks{
 /* @{ */
 
 
 enum FrameOffTypes{
  FIXED_OFF_FRAME,
  ATTACHED_OFF_FRAME,
  FRAME_OFF_TYPES_NUM
 };
 
 static const char* FrameOffTypesNames[FRAME_OFF_TYPES_NUM] = {
  "fixed",
  "attached"
 };
 
 
 class ParticleMutLocCamOffOptions : public Options {
  public:
   DecimalOption* bLinearToll;
   DecimalOption* bAngularToll;
   IntOption* bObs0Trials;
   IntOption* bObs1Trials;
   IntOption* bMinInliersNum;
   DecimalOption* bPrewarpingToll;
   
   DecimalOption* mLinearToll;
   DecimalOption* mAngularToll;
   DecimalOption* mMaxVisuale;
   DecimalOption* mDiscretizationStep;
   
   DecimalOption* bmDiscretization;
   
   IntOption* minObservationSize;
   
   IntOption* findTarget;
   IntOption* targetId;
   
   BoolOption*     banIdActive;
   
   StringOption* myFrameType;
   StringOption* hisFrameType;
   BoolOption* feedbackInMultiReg;
   BoolOption* bearingOnlyMeas;

   DecimalOption* mcLinearToll;
   DecimalOption* mcAngularToll;
   DecimalOption* mcProjToll;
   BoolOption*     mcDrawRealPos;
                        IntOption* mcExpMethod;

   
   BoolOption*     saveEstimates;
                        BoolOption*     saveBestEstimate;
   BoolOption*     saveRealPos;
   BoolOption*     saveMultiRegCam;
   BoolOption*     doOffline;
   DecimalOption* thrTimeSec;
   DecimalOption* thrTimeUSec;
   StringOption* rootFileMeasOff;
   StringOption* rootFileTStateOff;
   IntOption* measType;

            DecimalOption* timeForMeasUpdate;

   ParticleMutLocCamOffOptions();
   
   OptionGroupsType getGroup();
   string getObjectName() const {
    return "ParticleMutLocCamOffOptions";
   }
 };
 
 
 class ParticleMutLocCamOffSharedMem : public SharedMem{
  public:
   void initSharedMem();
   
   PosiFeatures getMyProtrusions(MotionModuleTState &state);
   
   PosiFeatures getMyObservation(MotionModuleTState &state);
   
   vector<Angle> getMyCamObs(MotionModuleTState &state);
   
   void setFormation(PoseFeatures &form);
   
   void setEstimates(PoseFeatures &est);
   
   Pose getTarget();
   
   void setTarget(Pose &t);
   
   bool doMloc();
   
   bool printFormation();
   
   bool printFormationDetails();
   
   vector<int> getAvoidParticleMutLocCamOffIds();
 };
 
 
 class ParticleMutLocCamOffTask : public Task{
  private:
   static const TaskPlate _plate = PMO_TASK;
   static const long int  _maxDurSec  = 0,     _maxDurUsec = 500000;
   static const long int  _maxSamplPeriodSec = 1, _maxSamplPeriodUsec = 0;
   static const long int  _minSamplPeriodSec = 0, _minSamplPeriodUsec = 400000;
   
   // resources
   MobileRob*        _mobileRob;  
   DraWin*          _draWin;   
   unsigned int       _drawList; 
   bool           _dwOn;      
   ParticleMutLocCamOffSharedMem* _sharedMem;
   CommModule*        _commModule;
   
   FrameOffTypes _myFrameType;
   FrameOffTypes _hisFrameType;
   
   // options
   ParticleMutLocCamOffOptions _options;
   
   // algorithms
   MultiReg*        _multiReg;
   ProbMultiReg*      _probMultiReg;
   MultiRegCam*      _multiRegCam;
   ProbMultiRegCam*     _probMultiRegCam;
   vector< vector<AngleId> >     inputMultiRegOff;
                        vector<pair<long int,long int> > _timeOffline;          //It stores the time in sec and microsec
   vector<Decimal>          _timeOff;
   int countMeasOff;                      //Variable to count the actual step of measures
   vector< vector<Pose> >  _tStateOff;
   PoseFeatures            _lastValidFormation;

   map<int,MutLocFilter*> _filter;
   vector<int>       _activatedFilters;

   map<int,vector<Angle> > _measFromMultiBeaReg;
   map<int,vector<Angle> > _measFromMultiBeaRegOri;
   vector<int>   _measFromMultiBeaRegIds;
   
   // funzionalities
   bool    _domultireg;
   bool    _dofilter;
   vector<int> ignoreIds;
   bool    _printFormation;
   bool    _printFormationDetails;
   
   // self information
   int       _myId;
                        Pose      _myPose;
   PosiFeatures  _myPosiFeatures;
   Observation   _myObservation;
   vector<Angle> _myCamObs;
   
   // others information
   map<int,PosiFeatures>    _othersPosiFeatures;
   vector<int>         _othersPosiFeaturesNums;
   map<int, vector<Angle> > _othersCamObs;
   vector<int>         _othersCamObsNums;
   map<int,Pose>        _othersPose;
   vector<int>         _othersPoseNums;
   
   // results
   map<int,Pose>  _estimates;
   vector<int>   _activatedEstimates;
   map<int,Timer> _timeFromLastMessage;
   PoseFeatures  _formation;
   
   fstream _savefile;
                        fstream _saveRealPosFile;
                        fstream _saveMultiRegCamFile;
                        fstream _saveBestEstimate;
                        fstream _readFile;
                        fstream _logMultiRegCamFile;
                        fstream _logMultiRegCamOff;
                        fstream _logTimeOff;
                        stringstream s;
                        
  public:
   ParticleMutLocCamOffTask(ResourcePointers resources,int argc, const char* argv[]);
   
   ~ParticleMutLocCamOffTask();
 
   TaskPlate getPlate() const {
    return _plate;
   }
   Time getMaxDuration(){
    return Time(_maxDurSec,_maxDurUsec);
   }
   Time getMaxSamplPeriod(){
    return Time(_maxSamplPeriodSec,_maxSamplPeriodUsec);
   }
   Time getMinSamplPeriod(){
    return Time(_minSamplPeriodSec,_minSamplPeriodUsec);
   }
   TaskOutputs run();
  
  private:
   
   void _readSharedMemory();
   
   void _comunicate();

   void _readDataFromFiles();

   void _doOnePropagationFilterStep(map <int, vector <Pose> > &likelihoods, vector<int> &likelihoodsActive);
   
   void _draw();
   
   void _writeSharedMem();
   
   void _computeFormation();
   
   void _insertValidObservations(vector<Observation> &allObservations);

   void _insertValidCamObservations(vector<AngleId> &allObservations);

   void _insertValidCamOffObservations(vector<AngleId> &allObservations);

   void _eraseDuplicateCamInput(vector<AngleId> &input);

   void _performNotBearingOnlyMultipleRegistration(map <int, vector <Pose> > &likelihoods, vector<int> &likelihoodsActive, MultiRegSol& multiRegSol);

   bool _performBearingOnlyMultipleRegistration(map <int, vector <Pose> > &likelihoods, vector<int> &likelihoodsActive, MultiRegCamSol* multiRegCamSol, ProbMultiRegCamSol* probMultiRegCamSol);
   
   void _completeMutLocFilInput(MutLocFilInput &fStepIn, int hisId);
   
   bool _activeFilter(int hisId);
   
   bool _activeEstimate(int hisId);
   
   void _activateFilter(int hisId);

   vector<AngleIdTime> _readMeasFile(ifstream &file,int id);

   vector<AngleIdTime> _readLaserFileBO(ifstream &file,int id);

   void _readIdsFile(ifstream &file,vector<int> &ris);

   vector<Pose> _readTState(ifstream &file);

   void _computeInputMultiRegCamOff(vector<vector<AngleIdTime> > &inputTime,vector<vector<AngleId> > &inputMultiRegOff,int myIdPos, vector< vector<Pose> > &inputTStateOffTemp, vector< vector<Pose> > &inputTStateOff);

   void selectParticles(vector<Decimal> weights, vector<int> &posBest, int numVis);
 };
 
 /* @} */
 
};


#endif