MutLocQuadStereoCam.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
//
// ----------------------------------------------------------------------------

#ifndef __PROB_MUTLOCQUAD_STEREO_CAM_H_
#define __PROB_MUTLOCQUAD_STEREO_CAM_H_

#include "baselib.h"
#include "LogTrace.h"
#include <Option.h>
#include "ProbMultiRegStereoCam3D.h"
#include "MutLocQuad.h"
#include <PaFilBFLRelRel3D.h>
#include <DraWinGL.h>

#define DEBUGMUTLOCQUAD 0

//#define WORLDGRAVITY 9.822
#define WORLDGRAVITY 9.81

typedef MipBaselib::IMUOdometry IMUData;

namespace kybalg{
 
 class mutLocQuadStereoCamPar : public MutLocQuadPar{
  private:
   Decimal sigmaNoiseDist;
   Decimal rangeNoiseDist;
   Decimal minIntWeight;
   string rootFakeQuadIds;
  public:
   mutLocQuadStereoCamPar() : MutLocQuadPar(){};
   
   ~mutLocQuadStereoCamPar();
   
            mutLocQuadStereoCamPar(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, Decimal snd, Decimal rnd, Decimal miw, string fqif) : MutLocQuadPar(lt, at, pt, expandMethod, se, smrg, tts, rf, rtm, rfts, rfi, tmfmu, mno, ev, pf, pp, rfic, lfaf, rapf, sap, rvf, scfs, sra, sbe, zpt, false,0,0,0,0){
    sigmaNoiseDist = snd;
    rangeNoiseDist = rnd;
    minIntWeight  = miw;
    rootFakeQuadIds = fqif;
   };
   
   mutLocQuadStereoCamPar(const mutLocQuadStereoCamPar &A);
   
   mutLocQuadStereoCamPar& operator=(mutLocQuadStereoCamPar &A);
   
   Decimal getSigmaNoiseDist();
   
   Decimal getRangeNoiseDist();
   
   Decimal getMinIntWeight();
   
   string getRootFakeQuadIdsFile();
 };
 
 class mutLocQuadStereoCam : public MIPObject{
   private:
   // If one wants to use this class as a singletone class
   static mutLocQuadStereoCam*             _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
   mutLocQuadStereoCamPar               _par;
   // Multiple registration algorithm structure
   probMultiRegStereoCam3DCam*             _probMultiReg;
   // Stores the input from files
   vector< vector<bearDistId> >            inputMultiRegOff;
   // Ids list of the fake quadrotors
   vector<int>                     _fakeQuadIds;
   // 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::PaFilBFLRelRel3D*>      _filter;
   // 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<bearDistStr>                 _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;
   
  //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,bearDistStr &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 _performBearingOnlyMultipleRegistration(map <int, vector <Pose3D> > &likelihoods, vector<int> &likelihoodsActive, probMultiRegStereoCam3DQuadSol* probMultiRegQuadSol);

   void _insertValidQuadObservations(vector<bearDistId> &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:
   mutLocQuadStereoCam(mutLocQuadStereoCamPar mlqp);
   
   mutLocQuadStereoCam(mutLocQuadStereoCamPar mlqp, bool falsePos);
   
   mutLocQuadStereoCam(int argc,char** argv,mutLocQuadStereoCamPar mlqp);
   
   mutLocQuadStereoCam();
   
   ~mutLocQuadStereoCam();
   
   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<bearDistIdTime> _readBearingFromPos(ifstream &file,int id);
   
   vector<bearDistIdTime> _readBearingFromPosId(ifstream &file,int id,int opt);
   
   vector<Pose3DIdTime> _readBearingFromAbsPosId(ifstream &file,int id);
   
   vector<Velocity3D> _readVelFile(ifstream &file,int id);
   
   void _readIdsFile(ifstream &file,vector<int> &ris);
   
   void _readFakeQuadIdsFile(ifstream &file);
   
   vector<IMUDataTime> _readTState(ifstream &file,MIPMatrix rotAcc,MIPMatrix rotGyro,Position3D biasAcc,Position3D scalingAcc,Position3D biasAngVel,Position3D scalingAngVel);
   
   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<bearDistIdTime> > &inputTime,vector<vector<bearDistId> > &inputMultiRegOff,int myIdPos, vector< vector<IMUDataTime> > &inputTStateOffTemp, vector< vector<IMUDataTime> > &inputTStateOff,vector<int> IdsF);
   
   static mutLocQuadStereoCam* instance(mutLocQuadStereoCamPar* mlqp);
   
   actualForm step(bool &nextStep);
   
   string getObjectName() const{
     return "mutLocQuadStereoCam";
   }
 };  
  
} //end of namespace kybalg
#endif