Point Cloud Library (PCL)  1.7.0
/tmp/buildd/pcl-1.7-1.7.0/tracking/include/pcl/tracking/kld_adaptive_particle_filter.h
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