kld_adaptive_particle_filter.h
Go to the documentation of this file.
00001 #ifndef PCL_TRACKING_KLD_ADAPTIVE_PARTICLE_FILTER_H_
00002 #define PCL_TRACKING_KLD_ADAPTIVE_PARTICLE_FILTER_H_
00003 
00004 #include <pcl/tracking/tracking.h>
00005 #include <pcl/tracking/particle_filter.h>
00006 #include <pcl/tracking/coherence.h>
00007 
00008 namespace pcl
00009 {
00010   namespace tracking
00011   {
00012 
00019     template <typename PointInT, typename StateT>
00020     class KLDAdaptiveParticleFilterTracker: public ParticleFilterTracker<PointInT, StateT>
00021     {
00022     public:
00023       using Tracker<PointInT, StateT>::tracker_name_;
00024       using Tracker<PointInT, StateT>::search_;
00025       using Tracker<PointInT, StateT>::input_;
00026       using Tracker<PointInT, StateT>::getClassName;
00027       using ParticleFilterTracker<PointInT, StateT>::transed_reference_vector_;
00028       using ParticleFilterTracker<PointInT, StateT>::coherence_;
00029       using ParticleFilterTracker<PointInT, StateT>::initParticles;
00030       using ParticleFilterTracker<PointInT, StateT>::weight;
00031       using ParticleFilterTracker<PointInT, StateT>::update;
00032       using ParticleFilterTracker<PointInT, StateT>::iteration_num_;
00033       using ParticleFilterTracker<PointInT, StateT>::particle_num_;
00034       using ParticleFilterTracker<PointInT, StateT>::particles_;
00035       using ParticleFilterTracker<PointInT, StateT>::use_normal_;
00036       using ParticleFilterTracker<PointInT, StateT>::use_change_detector_;
00037       using ParticleFilterTracker<PointInT, StateT>::change_detector_resolution_;
00038       using ParticleFilterTracker<PointInT, StateT>::change_detector_;
00039       using ParticleFilterTracker<PointInT, StateT>::motion_;
00040       using ParticleFilterTracker<PointInT, StateT>::motion_ratio_;
00041       using ParticleFilterTracker<PointInT, StateT>::step_noise_covariance_;
00042       using ParticleFilterTracker<PointInT, StateT>::representative_state_;
00043       using ParticleFilterTracker<PointInT, StateT>::sampleWithReplacement;
00044 
00045       typedef Tracker<PointInT, StateT> BaseClass;
00046       
00047       typedef typename Tracker<PointInT, StateT>::PointCloudIn PointCloudIn;
00048       typedef typename PointCloudIn::Ptr PointCloudInPtr;
00049       typedef typename PointCloudIn::ConstPtr PointCloudInConstPtr;
00050 
00051       typedef typename Tracker<PointInT, StateT>::PointCloudState PointCloudState;
00052       typedef typename PointCloudState::Ptr PointCloudStatePtr;
00053       typedef typename PointCloudState::ConstPtr PointCloudStateConstPtr;
00054 
00055       typedef PointCoherence<PointInT> Coherence;
00056       typedef boost::shared_ptr< Coherence > CoherencePtr;
00057       typedef boost::shared_ptr< const Coherence > CoherenceConstPtr;
00058 
00059       typedef PointCloudCoherence<PointInT> CloudCoherence;
00060       typedef boost::shared_ptr< CloudCoherence > CloudCoherencePtr;
00061       typedef boost::shared_ptr< const CloudCoherence > CloudCoherenceConstPtr;
00062 
00064       KLDAdaptiveParticleFilterTracker ()
00065       : ParticleFilterTracker<PointInT, StateT> ()
00066       , maximum_particle_number_ ()
00067       , epsilon_ (0)
00068       , delta_ (0.99)
00069       , bin_size_ ()
00070       {
00071         tracker_name_ = "KLDAdaptiveParticleFilterTracker";
00072       }
00073 
00077       inline void setBinSize (const StateT& bin_size) { bin_size_ = bin_size; }
00078       
00080       inline StateT getBinSize () const { return (bin_size_); }
00081 
00085       inline void setMaximumParticleNum (unsigned int nr) { maximum_particle_number_ = nr; }
00086 
00088       inline unsigned int getMaximumParticleNum () const { return (maximum_particle_number_); }
00089 
00093       inline void setEpsilon (double eps) { epsilon_ = eps; }
00094 
00096       inline double getEpsilon () const { return (epsilon_); }
00097 
00101       inline void setDelta (double delta) { delta_ = delta; }
00102 
00104       inline double getDelta () const { return (delta_); }
00105       
00106     protected:
00107 
00112       virtual bool 
00113       equalBin (std::vector<int> a, std::vector<int> b)
00114       {
00115         int dimension = StateT::stateDimension ();
00116         for (int i = 0; i < dimension; i++)
00117           if (a[i] != b[i])
00118             return (false);
00119         return (true);
00120       }
00121 
00125       double 
00126       normalQuantile (double u)
00127       {
00128         const double a[9] = {  1.24818987e-4, -1.075204047e-3, 5.198775019e-3,
00129                                -0.019198292004, 0.059054035642,-0.151968751364,
00130                                0.319152932694,-0.5319230073,   0.797884560593};
00131         const double b[15] = { -4.5255659e-5,   1.5252929e-4,  -1.9538132e-5,
00132                                -6.76904986e-4,  1.390604284e-3,-7.9462082e-4,
00133                                -2.034254874e-3, 6.549791214e-3,-0.010557625006,
00134                                0.011630447319,-9.279453341e-3, 5.353579108e-3,
00135                                -2.141268741e-3, 5.35310549e-4,  0.999936657524};
00136         double w, y, z;
00137         int i;
00138 
00139         if (u == 0.)
00140           return (0.5);
00141         y = u / 2.0;
00142         if (y < -6.)
00143           return (0.0);
00144         if (y > 6.)
00145           return (1.0);
00146         if (y < 0.0)
00147           y = - y;
00148         if (y < 1.0)
00149         {
00150           w = y * y;
00151           z = a[0];
00152           for (i = 1; i < 9; i++)
00153             z = z * w + a[i];
00154           z *= (y * 2.0);
00155         }
00156         else
00157         {
00158           y -= 2.0;
00159           z = b[0];
00160           for (i = 1; i < 15; i++)
00161             z = z * y + b[i];
00162         }
00163 
00164         if (u < 0.0)
00165           return ((1. - z) / 2.0);
00166         return ((1. + z) / 2.0);
00167       }
00168 
00172       virtual 
00173       double calcKLBound (int k)
00174       {
00175         double z = normalQuantile (delta_);
00176         double chi = 1.0 - 2.0 / (9.0 * (k - 1)) + sqrt (2.0 / (9.0 * (k - 1))) * z;
00177         return ((k - 1.0) / (2.0 * epsilon_) * chi * chi * chi);
00178       }
00179 
00185       virtual bool 
00186       insertIntoBins (std::vector<int> bin, std::vector<std::vector<int> > &B);
00187             
00189       virtual bool 
00190       initCompute ();
00191 
00196       virtual void 
00197       resample ();
00198 
00200       unsigned int maximum_particle_number_;
00201 
00203       double epsilon_;
00204 
00206       double delta_;
00207 
00209       StateT bin_size_;
00210     };
00211   }
00212 }
00213 
00214 #ifdef PCL_NO_PRECOMPILE
00215 #include <pcl/tracking/impl/kld_adaptive_particle_filter.hpp>
00216 #endif
00217 
00218 #endif


pcl
Author(s): Open Perception
autogenerated on Wed Aug 26 2015 15:25:10