GMPHDUnicycle2DIdCoopFilter.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_ID_COOP_FILTER_H
#define __GMPHD_UNICYCLE_2D_ID_COOP_FILTER_H

#include <Spaces.h>
#include <GaussianMixture.h>
#include "PHDFilter.h"

#define NO_ID -1
#define STEADY_ID -2

using namespace MipBaselib;
using namespace arma;

namespace MipAlgorithms{

 /* @{ */

 class GMPHDUnicycle2DIdCoopFilterPars:public PHDFilterPars{
  public:
   
   int myId;

   // 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;

   // constructors
   GMPHDUnicycle2DIdCoopFilterPars();
   GMPHDUnicycle2DIdCoopFilterPars(int mI, Decimal pS, Decimal pD, Time t, DMat& nx, DMat& h, DMat& nz, Decimal tt, int mc, Decimal mt, int pN, Decimal sdr, Decimal td, Decimal mdb, bool unw=false);
 };


 class GMPHDUnicycle2DIdCoopFilterVars:public PHDFilterVars{

  public:

   Pose myLastPose;
   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;

//   vector<int> sP;
   //map of current sensors in the current belief
   map <int, vector <int> > currSens;


   bool firstStepAfterMeasurements;

   GaussianMixtureJointId belief;

   GaussianMixtureJointId beliefOth;

   vector<int> lastOtherRobotPoseNum;
   map<int,Pose> lastOtherRobotPose;
   map<int,Pose> odometries;

   GMPHDUnicycle2DIdCoopFilterVars();
 };


 class GMPHDUnicycle2DIdCoopFilter:public PHDFilter{
  protected:
   GMPHDUnicycle2DIdCoopFilterVars* _gmvars;
   GMPHDUnicycle2DIdCoopFilterPars* _gmpars;

  public:
   GMPHDUnicycle2DIdCoopFilter();

   GMPHDUnicycle2DIdCoopFilter(GMPHDUnicycle2DIdCoopFilterPars inParams);
   
   void step(PHDFilterIn &in);
   
   string print();
   
   void getBelief(GaussianMixtureJointId &gm);
   Pose getMyBestPose();
   void setMyPose(Pose &p);
   
   string getObjectName() const {
    return "GMPHDUnicycle2DIdCoopFilter";
   }
   
   
   PHDFilterOutputType getOutputType(){
    return PHD_OUT_GMJID;
   }

  protected:
   void _computeOdometries(PHDFilterIn &in);

   void _predictBirthTarget(PHDFilterIn &in);
   virtual void _predictExistingTarget();

   void _findMyBestEstimate();

   virtual void _buildPHDUpdateComponents(PHDFilterIn &in);
   virtual void _updateWithMeasurements(PHDFilterIn &in);
   virtual void _updateWithNoMeasurements(PHDFilterIn &in);

   void _transformOtherMeasurements(PHDFilterIn &in);
   void _findAllSensors(PHDFilterIn &in);

   void _buildOtherPHDUpdateComponents(PHDFilterIn &in);
   virtual void _buildOtherPHDUpdateComponentsSingle(PHDFilterIn &in, int sensId);
   virtual void _updateWithOtherNoDetection(PHDFilterIn &in, int measIndex);
   virtual void _updateWithOtherMeasurements(PHDFilterIn &in, int measIndex);

   void _prune();
   void _pruneSimple();
//    void _prune2();

   virtual void _initialize(PHDFilterIn &in);
   void _normalStep(PHDFilterIn &in);

   Decimal _mahalanobisDist(GaussianComponent& a, GaussianComponent& b);

 };

 /* @} */

}


#endif