GMPHDUnicycle2DNoIdFilter.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_NO_ID_FILTER_H
#define __GMPHD_UNICYCLE_2D_NO_ID_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 GMPHDUnicycle2DNoIdFilterPars: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;
   
   // constructors
   GMPHDUnicycle2DNoIdFilterPars();
   GMPHDUnicycle2DNoIdFilterPars(Decimal pS, Decimal pD, Time t, DMat& nx, DMat& h, DMat& nz, Decimal tt, int mc, Decimal mt, int pN);
 };
 
 
 class GMPHDUnicycle2DNoIdFilterVars:public PHDFilterVars{
   
  public:

   Pose myLastPose;
   Decimal lastMeasurementNumber;

   GaussianMixtureJointId prediction;
   
   vector<DCol> predictedMeasures;
   vector<DMat> predictedCovariances;
   vector<DMat> kalmanGains;
   vector<DMat> stepKCovariances;
   bool firstStepAfterMeasurements;
   
   GaussianMixtureJointId belief;
   
   GMPHDUnicycle2DNoIdFilterVars();
   
   vector<int> lastOtherRobotPoseNum;
   map<int,Pose> lastOtherRobotPose;
   map<int,Pose> odometries;
 };
 
 
 class GMPHDUnicycle2DNoIdFilter:public PHDFilter{
  private:
   GMPHDUnicycle2DNoIdFilterVars* _gmvars;
   GMPHDUnicycle2DNoIdFilterPars* _gmpars;

  public:
   GMPHDUnicycle2DNoIdFilter();
   
   GMPHDUnicycle2DNoIdFilter(GMPHDUnicycle2DNoIdFilterPars inParams);
   
   void step(PHDFilterIn &in);
   
   string print();

   void getBelief(GaussianMixtureJointId &gm);

   string getObjectName() const {
    return "GMPHDUnicycle2DNoIdFilter";
   }
   
   PHDFilterOutputType getOutputType(){
    return PHD_OUT_GM;
   }
   
   
  protected:
   void _computeOdometries(PHDFilterIn &in);
   
   void _predictBirthTarget(PHDFilterIn &in);
   void _predictExistingTarget();
   void _buildPHDUpdateComponents(PHDFilterIn &in);
   void _updateWithMeasurements(PHDFilterIn &in);
   void _prune();
//    void _prune2();

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

 };
 
 /* @} */
 
}


#endif