Point Cloud Library (PCL)
1.7.0
|
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 00013 /** \brief @b KLDAdaptiveParticleFilterTracker tracks the PointCloud which is given by 00014 setReferenceCloud within the measured PointCloud using particle filter method. 00015 The number of the particles changes adaptively based on KLD sampling [D. Fox, NIPS-01], [D.Fox, IJRR03]. 00016 * \author Ryohei Ueda 00017 * \ingroup tracking 00018 */ 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 00063 /** \brief Empty constructor. */ 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 00074 /** \brief set the bin size. 00075 * \param bin_size the size of a bin 00076 */ 00077 inline void setBinSize (const StateT& bin_size) { bin_size_ = bin_size; } 00078 00079 /** \brief get the bin size. */ 00080 inline StateT getBinSize () const { return (bin_size_); } 00081 00082 /** \brief set the maximum number of the particles. 00083 * \param nr the maximum number of the particles. 00084 */ 00085 inline void setMaximumParticleNum (unsigned int nr) { maximum_particle_number_ = nr; } 00086 00087 /** \brief get the maximum number of the particles.*/ 00088 inline unsigned int getMaximumParticleNum () const { return (maximum_particle_number_); } 00089 00090 /** \brief set epsilon to be used to calc K-L boundary. 00091 * \param eps epsilon 00092 */ 00093 inline void setEpsilon (double eps) { epsilon_ = eps; } 00094 00095 /** \brief get epsilon to be used to calc K-L boundary. */ 00096 inline double getEpsilon () const { return (epsilon_); } 00097 00098 /** \brief set delta to be used in chi-squared distribution. 00099 * \param delta delta of chi-squared distribution. 00100 */ 00101 inline void setDelta (double delta) { delta_ = delta; } 00102 00103 /** \brief get delta to be used in chi-squared distribution.*/ 00104 inline double getDelta () const { return (delta_); } 00105 00106 protected: 00107 00108 /** \brief return true if the two bins are equal. 00109 * \param a index of the bin 00110 * \param b index of the bin 00111 */ 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 00122 /** \brief return upper quantile of standard normal distribution. 00123 * \param[in] u ratio of quantile. 00124 */ 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 00169 /** \brief calculate K-L boundary. K-L boundary follows 1/2e*chi(k-1, 1-d)^2. 00170 * \param[in] k the number of bins and the first parameter of chi distribution. 00171 */ 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 00180 /** \brief insert a bin into the set of the bins. if that bin is already registered, 00181 return false. if not, return true. 00182 * \param bin a bin to be inserted. 00183 * \param B a set of the bins 00184 */ 00185 virtual bool 00186 insertIntoBins (std::vector<int> bin, std::vector<std::vector<int> > &B); 00187 00188 /** \brief This method should get called before starting the actual computation. */ 00189 virtual bool 00190 initCompute (); 00191 00192 /** \brief resampling phase of particle filter method. 00193 sampling the particles according to the weights calculated in weight method. 00194 in particular, "sample with replacement" is archieved by walker's alias method. 00195 */ 00196 virtual void 00197 resample (); 00198 00199 /** \brief the maximum number of the particles. */ 00200 unsigned int maximum_particle_number_; 00201 00202 /** \brief error between K-L distance and MLE*/ 00203 double epsilon_; 00204 00205 /** \brief probability of distance between K-L distance and MLE is less than epsilon_*/ 00206 double delta_; 00207 00208 /** \brief the size of a bin.*/ 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