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 #endif