GMPHDUnicycle2DIdCentrFilter.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_CENTR_FILTER_H
#define __GMPHD_UNICYCLE_2D_ID_CENTR_FILTER_H

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

#define NO_ID -1
#define STEADY_ID -2

using namespace MipBaselib;
using namespace arma;

namespace MipAlgorithms{
 
 /* @{ */
 
 class GMPHDUnicycle2DIdCentrFilterPars: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;

   int sensorNum;
   vector<Pose> sensor;

   // Borders of the supervised area
   Decimal nBorder;
   Decimal sBorder;
   Decimal wBorder;
   Decimal eBorder;


   
   // constructors
   GMPHDUnicycle2DIdCentrFilterPars();
   GMPHDUnicycle2DIdCentrFilterPars(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, vector<Pose>& sP);
 };
 
 
 class GMPHDUnicycle2DIdCentrFilterVars:public PHDFilterVars{
   
  public:

   Pose myLastPose;
   Decimal lastMeasurementNumber;
   vector<Position> lastMeasurements;

   GaussianMixtureJointId prediction;
   
   vector<vector<DCol> > predictedMeasures;
   vector<vector<DMat> > predictedCovariances;
   vector<vector<DMat> > kalmanGains;
   vector<vector<DMat> > stepKCovariances;
   vector<vector<Decimal> > detectionProbability;

   bool firstStepAfterMeasurements;
   
   GaussianMixtureJointId belief;
   
   vector<int> lastOtherRobotPoseNum;
   map<int,Pose> lastOtherRobotPose;
   map<int,Pose> odometries;

   vector<vector <Position> > otherMeasures;

   GMPHDUnicycle2DIdCentrFilterVars(int nS);
 };

 
 class GMPHDUnicycle2DIdCentrFilter:public PHDFilter{
  private:
   GMPHDUnicycle2DIdCentrFilterVars* _gmvars;
   GMPHDUnicycle2DIdCentrFilterPars* _gmpars;

  public:
   GMPHDUnicycle2DIdCentrFilter();
   
   GMPHDUnicycle2DIdCentrFilter(GMPHDUnicycle2DIdCentrFilterPars inParams);
   
   void step(PHDFilterIn &in);
   
   string print();

   void getBelief(GaussianMixtureJointId &gm);
   
   string getObjectName() const {
    return "GMPHDUnicycle2DIdCentrFilter";
   }
   
   PHDFilterOutputType getOutputType(){
    return PHD_OUT_GMJID;
   }

  protected:
   void _computeOdometries(PHDFilterIn &in);
   
   void _predictBirthTarget(PHDFilterIn &in);
   void _predictExistingTarget();
   void _buildPHDUpdateComponents(PHDFilterIn &in, int n);
   void _updateWithNoDetection(PHDFilterIn &in);
   void _updateWithMeasurements(PHDFilterIn &in, int n);
   void _prune();
//    void _prune2();

   void _initialize(PHDFilterIn &in);
   void _normalStep(PHDFilterIn &in);
   
   Decimal _mahalanobisDist(GaussianComponent& a, GaussianComponent& b);

 };
 
 /* @} */
 
}


#endif