GMPHDUnicycle2DIdFilter.h
Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00030
00031 #ifndef __GMPHD_UNICYCLE_2D_ID_FILTER_H
00032 #define __GMPHD_UNICYCLE_2D_ID_FILTER_H
00033
00034 #include <Spaces.h>
00035 #include <GaussianMixture.h>
00036 #include "PHDFilter.h"
00037 #include <map>
00038
00039 #define NO_ID -1
00040 #define STEADY_ID -2
00041
00042 using namespace MipBaselib;
00043 using namespace arma;
00044
00045 namespace MipAlgorithms{
00046
00048
00049
00053 class GMPHDUnicycle2DIdFilterPars:public PHDFilterPars{
00054 public:
00055
00056
00058 DMat N_n_x;
00059
00060
00062 DMat H;
00064 DMat N_n_z;
00065
00066
00069 Decimal truncThresh;
00071 int maxNumComponents;
00074 Decimal mergThresh;
00076 int partNumber;
00077
00078
00080 Decimal sensorDetectionRay;
00082 Decimal targetDimension;
00084 Decimal maxDetectionBearing;
00085
00086
00087
00089 GMPHDUnicycle2DIdFilterPars();
00091 GMPHDUnicycle2DIdFilterPars(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);
00092 };
00093
00094
00098 class GMPHDUnicycle2DIdFilterVars:public PHDFilterVars{
00099
00100 public:
00101
00102 Pose myLastPose;
00103 Decimal lastMeasurementNumber;
00104 vector<Position> lastMeasurements;
00105
00106 GaussianMixtureJointId prediction;
00107
00108 vector<DCol> predictedMeasures;
00109 vector<DMat> predictedCovariances;
00110 vector<DMat> kalmanGains;
00111 vector<DMat> stepKCovariances;
00112 vector<Decimal> detectionProbability;
00113
00114 bool firstStepAfterMeasurements;
00115
00116 GaussianMixtureJointId belief;
00117
00118 vector<int> lastOtherRobotPoseNum;
00119 map<int,Pose> lastOtherRobotPose;
00120 map<int,Pose> odometries;
00121
00122 GMPHDUnicycle2DIdFilterVars();
00123 };
00124
00125
00129 class GMPHDUnicycle2DIdFilter:public PHDFilter{
00130 private:
00131 GMPHDUnicycle2DIdFilterVars* _gmvars;
00132 GMPHDUnicycle2DIdFilterPars* _gmpars;
00133
00134 public:
00135 GMPHDUnicycle2DIdFilter();
00136
00137 GMPHDUnicycle2DIdFilter(GMPHDUnicycle2DIdFilterPars inParams);
00138
00139 void step(PHDFilterIn &in);
00140
00141 string print();
00142
00143 void getBelief(GaussianMixtureJointId &gm);
00144
00145 string getObjectName() const {
00146 return "GMPHDUnicycle2DIdFilter";
00147 }
00148
00149 PHDFilterOutputType getOutputType(){
00150 return PHD_OUT_GMJID;
00151 }
00152
00153
00154 protected:
00155 void _computeOdometries(PHDFilterIn &in);
00156
00157 void _predictBirthTarget(PHDFilterIn &in);
00158 void _predictExistingTarget();
00159 void _buildPHDUpdateComponents(PHDFilterIn &in);
00160 void _updateWithMeasurements(PHDFilterIn &in);
00161 void _prune();
00162
00163
00164 void _initialize(PHDFilterIn &in);
00165 void _normalStep(PHDFilterIn &in);
00166
00167 Decimal _mahalanobisDist(GaussianComponent& a, GaussianComponent& b);
00168
00169 };
00170
00171
00172
00173 }
00174
00175
00176 #endif
00177
00178
00179
00180
00181
00182
00183
00184