MutLocQuad.h

Go to the documentation of this file.

// ----------------------------------------------------------------------------
//
// $Id$
//
// Copyright 2008, 2009, 2010  Antonio Franchi
//
// This file is part of TeleKyb.
//
// 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
//
// ----------------------------------------------------------------------------

#include "baselib.h"
#include "LogTrace.h"
#include <Option.h>
#include "ProbMultiRegCam3D.h"
#include <PaFilBFLRelRelBearOnly3D.h>
#include <PaFilBFLRelRelBearOnly3Dvel.h>
#include <PaFilBFLRelRelBearOnly3DvelUpdate.h>
#include <DraWinGL.h>

#ifndef __PROB_MUTLOCQUAD_H_
#define __PROB_MUTLOCQUAD_H_

#define DEBUGMUTLOCQUAD 0

//#define WORLDGRAVITY 9.822
#define WORLDGRAVITY 9.81

typedef MipBaselib::IMUOdometry IMUData;

namespace kybalg{
 class IMUDataTime{
    private:
     IMUData   IMUDatas;
     long int   _ttSec;
     long int   _ttUSec;
     Angle    actualRoll;
     Angle    actualPitch;
    public:
      IMUDataTime();
      
      IMUDataTime(Acceleration3D la, Velocity3D av,long int ttSec, long int ttUSec,Angle roll,Angle pitch);
      
      IMUDataTime(IMUData bs, long int ttSec, long int ttUSec,Angle roll,Angle pitch);
      
      ~IMUDataTime();
      
      void setTimeSec(long int sec);
      
      void setTimeUSec(long int usec);
      
      void setTime(long int sec, long int usec);
      
      long int getTimeSec();
      
      long int getTimeUSec();
      
      bool overTime(long int sec, long int usec);
      
      bool underTime(long int sec, long int usec);
      
      Decimal getTime();
      
      bool underTime(Decimal sec);
      
      bool overTime(Decimal sec);
      
      void setXLinAcc(Decimal x);
      
      void setYLinAcc(Decimal y);
      
      void setZLinAcc(Decimal z);
      
      void setLinAcc(Acceleration3D la);
      
      void setXAngVel(Decimal x);
      
      void setYAngVel(Decimal y);
      
      void setZAngVel(Decimal z);
      
      void setAngVel(Velocity3D av);
      
      Acceleration3D getLinAcc();
      
      Velocity3D getAngVel();
      
      IMUData getIMUData();
      
      Angle getActualRoll();
      
      Angle getActualPitch();
      
      void setActualRoll(Angle roll);
      
      void setActualPitch(Angle pitch);
      
      void setActualRoll(Decimal roll);
      
      void setActualPitch(Decimal pitch);
  };
  
  class mutLocState{
    private: 
     Velocity3D  _vel;
     Pose3D  _pos;
     int   _id;
    public:
     
     mutLocState();
     
     mutLocState(int id, Pose3D pos,Velocity3D vel);
     
     ~mutLocState();
     
     Velocity3D getVel();
     
     Pose3D getPose();
     
     int getId();
     
     void setVel(Velocity3D vel);
     
     void setPose(Pose3D pos);
     
     void setId(int id);
     
     mutLocState& operator=(const mutLocState &A);
  };
  
 // A vector of mutLocState (one of each robot) performes the actual formation
 typedef vector<mutLocState>  actualForm;
 
 // A vector of actualForm performes the list of all the formations in each time step
 typedef vector<actualForm>  totalForm;
};

namespace kybalg{
 class MutLocQuadPar{
  private:
   // Linear tollerance
   Decimal  _linearTollerance;
   // Angular tollerance
   Decimal  _angularTollerance;
   // Projection tollerance
   Decimal  _projTollerance;
   // Zenith tollerance
   Decimal  _zenithTollerance;
   // Expansion method
   int    _expandMethod;
   // Specify if one wants to store the estimates
   bool   _saveEstimates;
   // Specify if one wants to store the output of the multiple registration algorithm
   bool   _saveMultiRegCam;
   // Threshold time in seconds
   Decimal  _thrTimeSec;
   // Threshold time in microseconds
   Decimal  _thrTimeUSec;
   // Root of the files where the bearing-only measurements are stored
   string  _rootFileMeasOff;
   // Root of the files where the IMU measurements are stored
   string  _rootFileTStateOff;
   // Root of the files where the list of ids is stored
   string  _rootFileIdsOff;
   // Root of the files where the configuration file of IMU is stored
   string  _rootFileIMUConfigOff;
   // Root of the folder where the files are stored
   string  _rootFiles;
   // Time for measurement update
   Decimal  _timeForMeasUpdate;
   // Minimum number of observation in feature set to be take in account in multiple registration algorithm
   int    _minObSize;
   // Specify if one wants to visualize the OpenGL window
   bool    _embVis;
   // Specify if one wants to visualize the actual estimates
   bool   _printForm;
   // Specify if one wants to visualize all the particles
   bool   _printFormDetails;
   // Specify if one wants to load data from ground truth
   bool    _loadFromAbsFile;
   // Root of ground truth files
   string  _rootAbsPosFile;
   // Specify if one wants to save all the particles plus ground truth as last coordinate
   bool    _saveAllParticles;
   // Specify if one wants to load velocity files
   bool    _readVelFile;
   // Specify if one wants to save all the particles state
   bool    _saveCompleteStateFilter;
   // Specify if one wants to save data in relative or absolute (it needs to load files from ground truth) reference frame
   bool    _saveRelAbs;
   // Specify if one wants to save the best particle
   bool    _saveBestEstimate;
   // Specify if one wants to run only the filter or the algorithm with multireg. True if only filter; false otherwise
   bool   _doOnlyFilter;
   // Specify which filter one wants to run (0 for acceleration, 1 for velocity). The acceleration filter is set by default
   int    _filterType;
   // Specify the noise acting on the bearing measurements
   Decimal  _noiseBear;
   // Specify the delay between the owner of the filter and the other robots in the formation
   Decimal  _delayBear;
            // Specify the delay between the owner of the filter and the other robots in the formation
            Decimal  _timeStartOffset;
   
  public:
   
   MutLocQuadPar();
   
   ~MutLocQuadPar();
   
            MutLocQuadPar(Decimal lt, Decimal at, Decimal pt,int expandMethod,bool se,bool smrg,Decimal tts,string rf,string rtm,string rfts,string rfi, Decimal tmfmu,int mno,bool ev,bool pf,bool pp,string rfic,bool lfaf,string rapf, bool sap, bool rvf,bool scfs, bool sra,bool sbe,Decimal zpt, bool dof,int ftp,Decimal noi,Decimal dor, Decimal sot);
   
   MutLocQuadPar(const MutLocQuadPar &A);
   
   MutLocQuadPar& operator=(MutLocQuadPar &A);
   
   Decimal getLinearTollerance();
   
   Decimal getAngularTollerance();
   
   Decimal getProjTollerance();
   
   int getExpandMethod();
   
   bool getSaveEstimates();
   
   bool getSaveMultiRegOutput();
   
   Decimal getTimeForMeasUpdate();
   
   string getRootMeasFile();
   
   string getRootTStateFile();
   
   string getRootIdsFile();
   
   string getRootIMUConfigFile(); 
   
   Decimal getThrTimeSec();
   
   int getMinNumObs();
   
   bool getEmbVis();
   
   bool getPrintForm();
   
   bool getPrintFormDetails();
   
   string getRootFiles();
   
   bool getLoadFromAbsFile();
   
   string getRootAbsFile();
   
   bool getSaveAllParticles();
   
   bool getReadVelFile();
   
   bool getSaveCompleteStateFilter();
   
   bool getSaveRelAbs();
   
   bool getSaveBestEstim();
   
   bool getZenithTollerance();
   
   bool getDoOnlyFilter();
   
   int getFilterType();
   
   Decimal getNoiseMeasBear();
   
   Decimal getDelayMeasBear();

            Decimal getTimeStartOffset();
   
   void setSaveCompleteStateFilter(bool state);
   
 };
 
 class MutLocQuad : public MIPObject{
   private:
   // If one wants to use this class as a singletone class
   static MutLocQuad*                  _instance;
   // Specify if one wants to run the algorithm only with particle filters
   bool                         _doOnlyFilter;
   // Specify if one wants to save the particle filter inputs
   bool                         _saveInputFilterFile;
   //Stores if the drawing win has to be created
   bool                        _dwOn;
   // Pointer to the OpenGL window
   MipResources::DraWinGL*               _draWin;
   unsigned int                    _drawList;
   // Specify if one wants to visualize the estimates
   bool                         _printFormation;
   // Specify if one wants to visualize the entire particle sets
   bool                         _printFormationDetails;
   // Parameters of the algorithm
   MutLocQuadPar                    _par;
   // Multiple registration algorithm structure
   ProbMultiRegCam3DCam*                _probMultiReg;
   // Stores the input from files
   vector< vector<AngleBearId> >            inputMultiRegOff;
   // Map that stores the observed id and relative positions only to test particle filters
   vector<map<int,Position3D> >            idPosFilter;
   // It stores the observed id and relative positions only to test particle filters
   vector<map<int,Angle> >               _idYawFilter;
   // It stores the time in sec and microsec
   vector<pair<long int,long int> >           _timeOffline;
   vector<Decimal>                        _timeOff;
   // Variable to count the actual step of measures
   int                         countMeasOff;
   // It stores the IMU data
   vector< vector<IMUDataTime> >             _tStateOff;
   // There are roll and pitch of the formation over time
   vector< map<int, pair<Angle,Angle> > >        _rollPitch;
   // Bank of particle fitlers
//    map<int,MipAlgorithms::PaFilBFLRelRelBearOnly3D*>  _filter;
   map<int,MipAlgorithms::PaFilBFL3D*>  _filter;
   
   // Bank of particle fitlers (with velocity input)
   map<int,MipAlgorithms::PaFilBFLRelRelBearOnly3Dvel*> _filterVel;
   // List of filter activated
   vector<int>                     _activatedFilters;
   
   // Used in the constructor to store data from files
   // It stores the bearing-only measurements
   map<int,vector<bearStr> >              _measFromMultiBeaReg;
   // It stores the orientations
   map<int,vector<Orientation3D> >           _measFromMultiBeaRegOri;
   // It stores the ids
   vector<int>                     _measFromMultiBeaRegIds;
   
   // Angular velocity in horizonthal frame of the owner
   vector<Velocity3D>                 _myOmegaVel;
   
   // Variables only to compare results
   // Absolute position of the owner
   vector<Pose3D>                   _absPosOwner;
   // Absolute position of other robots (not the owner)
   vector<map<int,Pose3D> >               _absPosOther; 
   // Absolute velocity of other robots (not the owner)
   vector<map<int,Velocity3D> >              _velAbsOther;
   // Absolute velocity of the owner
   vector<Velocity3D>                 _absVelOwner;
   
   
   // Self information = informations about the owner
   // Id
   int                         _myId;
   // Pose
   Pose3D                       _myPose;
   // IMU output
   IMUDataTime                     _myIMU;
   // Bearing-only measurements
   vector<bearStr>                   _myBearObs;
   
   // Others information = other robot informations
   //Bearing-only observations
   map<int, vector<bearStr> >             _othersBearObs;
   // Ids
   vector<int>                     _othersBearObsNums;
   //map<int,Pose3D>     _othersPose;
   // IMU outputs
   map<int,IMUDataTime>                _othersIMU;
   
   // Results
   map<int,Pose3D>                   _estimates;
   map<int,Velocity3D>                 _velEstimates;
   vector<int>                     _activatedEstimates;
   map<int,Timer>                   _timeFromLastMessage;
   vector<PoseFeature3D>                _formation;
   
   // Saving information
   map<int,fstream*>                  _savefile;
   // Save best particle
   map<int,fstream*>                  _saveBest;
   // Save ground truth
   fstream                       _saveRealPosFile;
   // Save output of multiple registration algorithm
   fstream                       _saveMultiRegCamFile;
   fstream                       _saveBestEstimate;
   fstream                       _readFile;
   // Save info about multiple registration algorithm
   fstream                       _logMultiRegCamFile;
   fstream                       _logMultiRegCamOff;
   //Save time steps
   fstream                       _logTimeOff;
   // Save all the particles
   map<int,fstream*>                  _saveAllP;
   // Save filters' input
   fstream                       _saveInputFilter;
   
   // Printing information inside the code
   stringstream      s;
 
   map<int,int>                     banned;
   Decimal                       _lastMeasUpdate;
   
   // Delay information
   map<int,Decimal>                  _delayTimeMeas;
   
   // Counter for saving particles
   int countSamples;
   int saveEachNSamples;
   
  //Methods
  private:   
   void _readDataFromFiles();
   
   int searchIMUData(vector<IMUDataTime> &inputIMUTime, Decimal time, Decimal thr);
   
   int searchAbsPos(vector<Pose3DIdTime> &inputPosTime, Decimal time, Decimal thr);
   
   Position3D _computeRelPos(Pose3DIdTime pos,Position3D posOther,bearStr &bearObs);
   
   Position3D _computeRelCoord(Position3D myVel,Orientation3D myOri,Position3D velOther);
   
   Pose3D _computeRelCoord(Pose3D myPose,Pose3D otherPose);
   
   void _doOnePropagationFilterStep(map <int, vector <Pose3D> > &likelihoods, vector<int> &likelihoodsActive,map <int, vector <Velocity3D> > &velLikelihoods);
   
   bool _activeFilter(int hisId);
   
   void _completeMutLocFilInput(MipAlgorithms::MutLocFilInput3D &fStepIn, int hisId);
   
   void _completeMutLocFilInputVelUpdate(MipAlgorithms::MutLocFilInput3D &fStepIn, int hisId);
   
   void _performBearingOnlyMultipleRegistration(map <int, vector <Pose3D> > &likelihoods, vector<int> &likelihoodsActive, ProbMultiRegCam3DQuadSol* probMultiRegQuadSol);

   void _insertValidQuadObservations(vector<AngleBearId> &allObservations);
   
   void _activateFilter(int hisId);
   
   bool _activeEstimate(int hisId);
   
   void _computeFormation(actualForm &af);
   
   Pose3D _computeAbsCoord(Pose3D owner,Pose3D other);
   
   Position3D _computeAbsCoord(Pose3D owner,Position3D other);
   
   void _draw();
   
   void selectParticles(vector<Decimal> weights, vector<int> &posBest,int numVis);
   
  public:
   MutLocQuad(MutLocQuadPar mlqp);
   MutLocQuad(int argc,char** argv,MutLocQuadPar mlqp);
   
   MutLocQuad();
   
   ~MutLocQuad();
   
   Velocity3D transformPQRRDPDYD(Velocity3D pqr,Angle roll,Angle pitch);
   
   Position3D quadNoRollPitch(Position3D point,Angle roll,Angle pitch);
   Velocity3D quadNoRollPitch(Velocity3D point,Angle roll,Angle pitch);
   Acceleration3D quadNoRollPitch(Acceleration3D point,Angle roll, Angle pitch);
   //void quadNoRollPitch(Decimal x, Decimal y, Decimal z, Angle roll, Angle pitch, Decimal* out);
   
   vector<BearIdTime> _readBearingFromPos(ifstream &file,int id);
   
   vector<BearIdTime> _readBearingFromPosId(ifstream &file,int id,int opt);
   
   vector<Pose3DIdTime> _readBearingFromAbsPosId(ifstream &file,int id);
   
   vector<BearIdTime> _readBearing(ifstream &file,int id);
   
   vector<Velocity3D> _readVelFile(ifstream &file,int id);
   
   void _readIdsFile(ifstream &file,vector<int> &ris);
   
   vector<IMUDataTime> _readTState(ifstream &file,MIPMatrix rotAcc,MIPMatrix rotGyro,Position3D biasAcc,Position3D scalingAcc,Position3D biasAngVel,Position3D scalingAngVel,int id);
   
   void _readIMUConfigFile(ifstream &file,vector<MIPMatrix> &RotAcc,vector<MIPMatrix> &rotGyro,vector<Position3D>& biasAcc,vector<Position3D>& scalingAcc,vector<Position3D>& biasAngVel,vector<Position3D>& scalingAngVel);
   
   void _computeInputMultiRegOff(vector<vector<BearIdTime> > &inputTime,vector<vector<AngleBearId> > &inputMultiRegOff,int myIdPos, vector< vector<IMUDataTime> > &inputTStateOffTemp, vector< vector<IMUDataTime> > &inputTStateOff,vector<int> IdsF);
   
   void _plateLinVel();
   
   void _fillFilterMeas(MipAlgorithms::MutLocFilInput3D& fStepIn, int hisId);
   
   static MutLocQuad* instance(MutLocQuadPar* mlqp);
   
   actualForm step(bool &nextStep);
   
   string getObjectName() const{
     return "mutLocQuad";
   }

            void computeStartingTime();
 };  
  
} //end of namespace kybalg
#endif