PHDFilter.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 __PHD_FILTER_H
#define __PHD_FILTER_H

#include <Spaces.h>
#include <SimTime.h>
#include <SpacesArma.h>
#include <armadillo>
#include <math.h>
#include <LogTrace.h>
#include <GaussianMixture.h>

using namespace MipBaselib;
using namespace arma;

namespace MipAlgorithms{
 
 /* @{ */
 
 enum PHDFilterType{
  GM_LINEAR_NO_ID,
  GM_UNICYCLE_2D_ID,
  GM_UNICYCLE_2D_ID_RC, // robocentric non-cooperative filter
  GM_UNICYCLE_2D_ID_COOP,
  GM_UNICYCLE_2D_ID_COOP_2,  // non-robocentric cooperative filter bad (divided by the probability of detection)
  GM_UNICYCLE_2D_ID_RC_COOP, // robocentric cooperative filter
  GM_UNICYCLE_2D_ID_3_COOP,  // non-robocentric cooperative filter good
  GM_UNICYCLE_2D_ID_4_COOP,  // non-robocentric cooperative filter good
  GM_UNICYCLE_2D_ID_CENTR,
  GM_UNICYCLE_2D_NO_ID, // non-robocentric non-cooperative filter
  GM_UNICYCLE_2D_ID_RC_CAM,
  NO_PHD_EKF_ONLY,
  GMID_PHD_UNYCYCLE_2D_RC,
  PHD_FILTER_TYPE_NUM
 };
 
 static const char* PHDFilterTypeName[PHD_FILTER_TYPE_NUM] ={
   "GMLinearNoId",
   "GMUnicycle2DId",
   "GMUnicycle2DIdRoboCentric",
   "GMUnicycle2DIdCoop",
   "GMUnicycle2DIdCoop2",
   "GMUnicycle2DIdRoboCentricCoop",
   "GMUnicycle2DId3Coop",
   "GMUnicycle2DId4Coop",
   "GMUnicycle2DIdCentr",
   "GMUnicycle2DNoId",
   "GMUnicycle2DIdRoboCentricCam",
   "NoPHD:EKFOnly",
   "GMIDUnicycle2DRC"
 };
 
 enum PHDFilterOutputType{
  PHD_OUT_NULL,
  PHD_OUT_GM,
  PHD_OUT_GMJID,
  PHD_OUT_GMID,
  PHD_FILTER_OUTPUT_TYPE_NUM
 };
 
 
 
 class PHDFilterIn{
  public:
   Pose myPose;
   vector<Position> measures;
   vector< vector <Position> > otherMeasures;
   vector< int > otherIds;

   vector<Pose> hisPose;
   vector<int> hisId;
   
   // Added for PHD with camera: Marco Cognetti
   // Height from sonar
   Decimal height;
   // Measurements from camera
   vector<Position2DArma> measuresCam;
   // Attitude
   Angle roll;
   Angle pitch;
   Angle yaw;
   // Odometry
   Odometry3DTimeArma myOdom;
 };
 
 
 class PHDFilterPars {
  public:
   
   Decimal pSurvival;
   
   Decimal pDetection;
   
   SimTime timeBetweenUpdates;
   
   PHDFilterPars();
   
   PHDFilterPars(Decimal pS, Decimal pD, SimTime t);
 };
 
 
 class PHDFilterVars {
  public:
   bool _firstStep;
   SimTimer _timeFromLastUpdate;
   
   SimTimer _timeFromLastPrediction;
   
   PHDFilterVars();
 };
 
 
 class PHDFilter : public  MIPObject{
  protected:
   PHDFilterVars* _vars;
   PHDFilterPars* _pars;
  public:
   PHDFilter();
   
   PHDFilter(PHDFilterPars inParams);
   
   virtual void step(PHDFilterIn &in)=0;
   
   virtual string print()=0;
   
   virtual PHDFilterOutputType getOutputType()=0;
   
   virtual void getBelief(GaussianMixtureJointId &gm){}
   
   virtual void getBelief(GaussianMixture &gm){}
   
   virtual void getBelief(GaussianMixtureID &gm){}
   
  protected:
   
   virtual void _initialize(PHDFilterIn &in)=0;
   virtual void _normalStep(PHDFilterIn &in)=0;
 };
 
 /* @} */
 
}


#endif