GMIDPHDUnicycle2DRCFilter.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 __GMIDPHD_UNICYCLE_2D_RC_FILTER_H
#define __GMIDPHD_UNICYCLE_2D_RC_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 GMIDPHDUnicycle2DRCFilterPars: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
   GMIDPHDUnicycle2DRCFilterPars();
   GMIDPHDUnicycle2DRCFilterPars(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 GMIDPHDUnicycle2DRCFilterVars:public PHDFilterVars{

  public:

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

   GaussianMixtureID prediction;

   vector<DCol> predictedMeasures;
   vector<Decimal> predictedMeasuresD;
   vector<Angle> predictedMeasuresB;
   vector<DMat> predictedCovariances;
   vector<DMat> kalmanGains;
   vector<DMat> stepKCovariances;
   vector<Decimal> detectionProbability;
   
   bool firstStepAfterMeasurements;
   
   GaussianMixtureID belief;
   
   vector<int> lastOtherRobotPoseNum;
   map<int,Pose> lastOtherRobotPose;
   map<int,Pose> odometries;

   GMIDPHDUnicycle2DRCFilterVars();
 };
 
 
 class GMIDPHDUnicycle2DRCFilter:public PHDFilter{
  protected:
   GMIDPHDUnicycle2DRCFilterVars* _gmvars;
   GMIDPHDUnicycle2DRCFilterPars* _gmpars;

  public:
   GMIDPHDUnicycle2DRCFilter();

   GMIDPHDUnicycle2DRCFilter(GMIDPHDUnicycle2DRCFilterPars inParams);
   
   void step(PHDFilterIn &in);
   
   string print();
   
   void getBelief(GaussianMixtureID &gm);
   Pose getMyBestPose();
   void setMyPose(Pose &p);
   
   string getObjectName() const {
    return "GMIDPHDUnicycle2DRCFilter";
   }
   
   PHDFilterOutputType getOutputType(){
    return PHD_OUT_GMID;
   }
   
  protected:
   void _computeOdometries(PHDFilterIn &in);
   
   void _predictBirthTarget(PHDFilterIn &in);
   void _predictExistingTarget();
   
   void _buildPHDUpdateComponents(PHDFilterIn &in);
   void _updateWithNoMeasurements(PHDFilterIn &in);
   void _updateWithMeasurements(PHDFilterIn &in);
   
   void _prune();
   void _pruneSimple();
   
   void _initialize(PHDFilterIn &in);
   void _normalStep(PHDFilterIn &in);
   
   Decimal _mahalanobisDist(GaussianComponent& a, GaussianComponent& b);
 };

 /* @} */

}


#endif