GMPHDUnicycle2DCamIdRCFilter.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 __GMPHD_UNICYCLE_2D_CAM_ID_FILTER_H
#define __GMPHD_UNICYCLE_2D_CAM_ID_FILTER_H

#include <Spaces.h>
#include <SpacesArma.h>
#include <GaussianMixture.h>
#include "PHDFilter.h"
#include <map>
#include <CameraParameters.h>

#define NO_ID -1
#define STEADY_ID -2

using namespace MipBaselib;
using namespace arma;

namespace MipAlgorithms{
 
 /* @{ */
 
 class GMPHDUnicycle2DCamIdRCFilterPars:public PHDFilterPars{
  public:
   // System model
   DMat           N_n_x;
   
   // Measurement model
   DMat           H;
   
   DMat           N_n_z;
   
   // Pruning thresholds
   Decimal         truncThresh;
   
   int           maxNumComponents;
   
   Decimal         mergThresh;
   
   int           partNumber;
   
   // Parameters for the computation of the actual detection probability of a target.
   Decimal         sensorDetectionRay;
   
   Decimal         targetDimension;
   
   Decimal         maxDetectionBearing;
   
   bool           useNewNoMeasWeight;
   
   CameraPar3DArma*    camPars;
   
   int           myId;
   
   Roto3DArma*       rotoQuadMobRob;
   
   Roto3DArma*       rotSonarQuad;
   
   Roto3DArma       rotCameraQuadTrans;
   
   Position3DArma*     posSonarQuad;
   
   bool          vertZSonarAlgo;
   
   string print();
   
   // Constructors
   GMPHDUnicycle2DCamIdRCFilterPars();
   
   GMPHDUnicycle2DCamIdRCFilterPars(int mId, Decimal pS, Decimal pD, SimTime t, DMat& nx, DMat& h, DMat& nz, Decimal tt, int mc, Decimal mt, int pN, Decimal sdr, Decimal td, Decimal mdb, Decimal flc, Decimal wc, Decimal hc, Roto3DHomoArma &qct, Decimal ppx, Decimal ppy, Roto3DArma& rotoQuadMob, Roto3DHomoArma& rotoSonarQuad, bool vzsm);
 };
 
 class GMPHDUnicycle2DCamIdRCFilterVars:public PHDFilterVars{
  
  public:
   
   // It saves the rototranslation between quadrotor and camera mapping
   DMat::fixed<3,3>       AMat;
   Position3DArma        bVec;
   Roto3DArma         CMat;
   
   // Vertical height by sonar in quadrotor frame
   Decimal           vHeight;
   // Height from z-axis of sonar in sonar reference frame
   Decimal           zHeight;
   
   // Attitude of the robot
   // Roll
   Decimal           roll;
   // Pitch
   Decimal           pitch;
   // Yaw
   Decimal           myYaw;
   Decimal           myLastYaw;
   
   // Coordinates of the image plane border
   Position2DArma        borders[4];
   
   // New plate measurements
   vector<Position2DArma>    newMeas;
   
   // New image border
   vector<Position2DArma>    newBorder;
   
   // Variable to store the previous velocity of the owner
   Odometry2DTimeArma      myLastOdom;
   Odometry2DTimeArma      myOdo;
   
   Decimal           lastMeasurementNumber;
   vector<Position>       lastMeasurements;
   
   GaussianMixtureJointId    prediction;
   
   vector<DCol>         predictedMeasures;
   vector<Decimal>       predictedMeasuresD;
   vector<Angle>        predictedMeasuresB;
   vector<DMat>         predictedCovariances;
   vector<DMat>         kalmanGains;
   vector<DMat>         stepKCovariances;
   vector<Decimal>       detectionProbability;
   
   int bestMyEstimate;
   
   vector<vector<Decimal> >   predictedMeasuresOth;
   vector<vector<Decimal> >   predictedCovariancesOth;
   vector<vector<DMat> >    kalmanGainsOth;
   vector<vector<DMat> >    stepKCovariancesOth;
   vector<vector<Decimal> >   detectionProbabilityOth;
   vector<vector<Decimal> >   sensorWeightsOth;
   
   vector<Decimal>       detectionProbabilitySum;
   Decimal           sensorWeightSum;
   vector<Decimal>       notDetectionProbabilityProd;
   
   vector<vector<Decimal > >  dists;
   
   //map of current sensors in the current belief
   map <int, vector <int> >   currSens;
   
   bool             firstStepAfterMeasurements;
   
   GaussianMixtureJointId    belief;
   
   GaussianMixtureJointId   beliefOth;
   
   vector<int>         lastOtherRobotPoseNum;
   map<int,Pose2DArma>     lastOtherRobotPose;
   map<int,Pose2DArma>     odometries;
   
   GMPHDUnicycle2DCamIdRCFilterVars();
 };
 
 /* @{ */
 
 class GMPHDUnicycle2DCamIdRCFilter:public PHDFilter{
  protected:
   GMPHDUnicycle2DCamIdRCFilterVars* _gmvars;
   GMPHDUnicycle2DCamIdRCFilterPars* _gmpars;
   
  public:
   GMPHDUnicycle2DCamIdRCFilter();
   
   GMPHDUnicycle2DCamIdRCFilter(GMPHDUnicycle2DCamIdRCFilterPars inParams);
   
   void step(PHDFilterIn &in);
   
   string print();
   
   void getBelief(GaussianMixtureJointId &gm);
   
   void getBeliefWeightedSum(map<int,Col<Decimal>::fixed<3> > &out, SimTime* actTime=NULL);
   
   void getBestBelief(map<int,Col<Decimal>::fixed<3> > &out, SimTime* actTime=NULL);
   
   string exportAsStringWeightedSum();
   
   string exportAsStringWeightedSumImagePlane();
   
   string exportAsStringBestGuess();
   
   string exportAsStringBestGuessImagePlane();
   
   string exportAsStringAllGuessImagePlane();
   
   string exportAsStringAllGuess();
   
   string exportAsStringUAVData();
   
   string exportAsStringClusterGuess(const Decimal &per);
   
            string exportAsString();

            void exportAs3DRelCoordBestGuess(map<int,DCol::fixed<4> > &in);
   
   PHDFilterOutputType getOutputType(){
    return PHD_OUT_GMJID;
   }
   
  private:
   
   fstream streamFile;
   
   void _initialize(PHDFilterIn&);
   
   void _initializeWithMeas(PHDFilterIn&);
   
   void _normalStep(PHDFilterIn&);
   
   void _computeOdometries(PHDFilterIn&);
   
   void _computeOdometriesInit(PHDFilterIn &in);
   
   void _predictBirthTarget(PHDFilterIn&);
   
   void _predictExistingTarget();
   
   vector<Position2DArma> _plateMeasurements(vector<Position2DArma> &points, const Decimal &height);
   
   vector<Position2DArma> _plateMeasurementsAlongZCam(vector<Position2DArma> &points, const Decimal &height);
   
   vector<Position2DArma> _plateMeasurementsVert(vector<Position2DArma> &points, const Decimal &height);
   
   void _findMyBestEstimate();
   
   void _buildPHDUpdateComponents(PHDFilterIn &in);
   
   void _updateWithNoMeasurements();
   
   void _updateWithMeasurements(PHDFilterIn &in);
   
   void _prune();
   
   void _transRotoPlateAbsHeight();
   
   void _transRotoPlateRelHeight(DMat::fixed<3,3> &KFun, Position3DArma &b);
   
   Velocity2DArma _transPlateLinVel(const Velocity3DArma &vel);
   
   vector<Position2DArma> _findImagePlaneBorder(const Decimal &height);
   
   vector<Position2DArma> _findImagePlaneBorderAlongZCam(const Decimal &height);
   
   vector<Position2DArma> _findImagePlaneBorderVert();
   
   void _updateLinearCoeff();
   
   Decimal _computeProbDetVert(Position2DArma& point);
   
   Decimal _mahalanobisDist(GaussianComponent& a, GaussianComponent& b);
   
   void _pruneSimple();
   
   void _preProcessingData(PHDFilterIn& in);
   
   void _plateOdometry(PHDFilterIn &in);
   
   Decimal _transformPQRYD(const Velocity3DArma &pqr);
   
  public:
   Pose2DArma _integral(Odometry2DTimeArma& vA, Odometry2DTimeArma& vB, const Decimal& dT);
   
   string getObjectName() const {
    return "GMPHDUnicycle2DCamIdRCFilter";
   }
 };
   
   /* @} */
 
}
 
 
#endif