Point Cloud Library (PCL)  1.7.0
/tmp/buildd/pcl-1.7-1.7.0/tracking/include/pcl/tracking/particle_filter.h
00001 #ifndef PCL_TRACKING_PARTICLE_FILTER_H_
00002 #define PCL_TRACKING_PARTICLE_FILTER_H_
00003 
00004 #include <pcl/tracking/tracking.h>
00005 #include <pcl/tracking/tracker.h>
00006 #include <pcl/tracking/coherence.h>
00007 #include <pcl/filters/passthrough.h>
00008 #include <pcl/octree/octree.h>
00009 
00010 #include <Eigen/Dense>
00011 
00012 namespace pcl
00013 {
00014 
00015   namespace tracking
00016   {
00017     /** \brief @b ParticleFilterTracker tracks the PointCloud which is given by
00018         setReferenceCloud within the measured PointCloud using particle filter method.
00019       * \author Ryohei Ueda
00020       * \ingroup tracking
00021       */
00022     template <typename PointInT, typename StateT>
00023     class ParticleFilterTracker: public Tracker<PointInT, StateT>
00024     {
00025       protected:
00026         using Tracker<PointInT, StateT>::deinitCompute;
00027         
00028       public:
00029         using Tracker<PointInT, StateT>::tracker_name_;
00030         using Tracker<PointInT, StateT>::search_;
00031         using Tracker<PointInT, StateT>::input_;
00032         using Tracker<PointInT, StateT>::indices_;
00033         using Tracker<PointInT, StateT>::getClassName;
00034         
00035         typedef Tracker<PointInT, StateT> BaseClass;
00036         
00037         typedef typename Tracker<PointInT, StateT>::PointCloudIn PointCloudIn;
00038         typedef typename PointCloudIn::Ptr PointCloudInPtr;
00039         typedef typename PointCloudIn::ConstPtr PointCloudInConstPtr;
00040 
00041         typedef typename Tracker<PointInT, StateT>::PointCloudState PointCloudState;
00042         typedef typename PointCloudState::Ptr PointCloudStatePtr;
00043         typedef typename PointCloudState::ConstPtr PointCloudStateConstPtr;
00044 
00045         typedef PointCoherence<PointInT> Coherence;
00046         typedef boost::shared_ptr< Coherence > CoherencePtr;
00047         typedef boost::shared_ptr< const Coherence > CoherenceConstPtr;
00048 
00049         typedef PointCloudCoherence<PointInT> CloudCoherence;
00050         typedef boost::shared_ptr< CloudCoherence > CloudCoherencePtr;
00051         typedef boost::shared_ptr< const CloudCoherence > CloudCoherenceConstPtr;
00052         
00053         /** \brief Empty constructor. */
00054         ParticleFilterTracker ()
00055         : iteration_num_ (1)
00056         , particle_num_ ()
00057         , min_indices_ (1)
00058         , ref_ ()
00059         , particles_ ()
00060         , coherence_ ()
00061         , step_noise_covariance_ ()
00062         , initial_noise_covariance_ ()
00063         , initial_noise_mean_ ()
00064         , resample_likelihood_thr_ (0.0)
00065         , occlusion_angle_thr_ (M_PI / 2.0)
00066         , alpha_ (15.0)
00067         , representative_state_ ()
00068         , trans_ ()
00069         , use_normal_ (false)
00070         , motion_ ()
00071         , motion_ratio_ (0.25)
00072         , pass_x_ ()
00073         , pass_y_ ()
00074         , pass_z_ ()
00075         , transed_reference_vector_ ()
00076         , change_detector_ ()
00077         , changed_ (false)
00078         , change_counter_ (0)
00079         , change_detector_filter_ (10)
00080         , change_detector_interval_ (10)
00081         , change_detector_resolution_ (0.01)
00082         , use_change_detector_ (false)
00083         {
00084           tracker_name_ = "ParticleFilterTracker";
00085           pass_x_.setFilterFieldName ("x");
00086           pass_y_.setFilterFieldName ("y");
00087           pass_z_.setFilterFieldName ("z");
00088           pass_x_.setKeepOrganized (false);
00089           pass_y_.setKeepOrganized (false);
00090           pass_z_.setKeepOrganized (false);
00091         }
00092         
00093         /** \brief Set the number of iteration.
00094           * \param[in] iteration_num the number of iteration.
00095           */
00096         inline void
00097         setIterationNum (const int iteration_num) { iteration_num_ = iteration_num; }
00098 
00099         /** \brief Get the number of iteration. */
00100         inline int
00101         getIterationNum () const { return iteration_num_; }
00102 
00103         /** \brief Set the number of the particles.
00104           * \param[in] particle_num the number of the particles.
00105           */
00106         inline void
00107         setParticleNum (const int particle_num) { particle_num_ = particle_num; }
00108 
00109         /** \brief Get the number of the particles. */
00110         inline int
00111         getParticleNum () const { return particle_num_; }
00112 
00113         /** \brief Set a pointer to a reference dataset to be tracked.
00114           * \param[in] ref a pointer to a PointCloud message
00115           */
00116         inline void
00117         setReferenceCloud (const PointCloudInConstPtr &ref) { ref_ = ref; }
00118 
00119         /** \brief Get a pointer to a reference dataset to be tracked. */
00120         inline PointCloudInConstPtr const
00121         getReferenceCloud () { return ref_; }
00122 
00123         /** \brief Set the PointCloudCoherence as likelihood.
00124           * \param[in] coherence a pointer to PointCloudCoherence.
00125           */
00126         inline void
00127         setCloudCoherence (const CloudCoherencePtr &coherence) { coherence_ = coherence; }
00128         
00129         /** \brief Get the PointCloudCoherence to compute likelihood. */
00130         inline CloudCoherencePtr
00131         getCloudCoherence () const { return coherence_; }
00132         
00133 
00134         /** \brief Set the covariance of step noise.
00135           * \param[in] step_noise_covariance the diagonal elements of covariance matrix of step noise.
00136           */
00137         inline void
00138         setStepNoiseCovariance (const std::vector<double> &step_noise_covariance)
00139         {
00140           step_noise_covariance_ = step_noise_covariance;
00141         }
00142 
00143         /** \brief Set the covariance of the initial noise. It will be used when initializing the particles.
00144           * \param[in] initial_noise_covariance the diagonal elements of covariance matrix of initial noise.
00145           */
00146         inline void
00147         setInitialNoiseCovariance (const std::vector<double> &initial_noise_covariance)
00148         {
00149           initial_noise_covariance_ = initial_noise_covariance;
00150         }
00151 
00152         /** \brief Set the mean of the initial noise. It will be used when initializing the particles.
00153           * \param[in] initial_noise_mean the mean values of initial noise.
00154           */
00155         inline void
00156         setInitialNoiseMean (const std::vector<double> &initial_noise_mean)
00157         {
00158           initial_noise_mean_ = initial_noise_mean;
00159         }
00160 
00161         /** \brief Set the threshold to re-initialize the particles.
00162           * \param[in] resample_likelihood_thr threshold to re-initialize.
00163           */
00164         inline void
00165         setResampleLikelihoodThr (const double resample_likelihood_thr)
00166         {
00167           resample_likelihood_thr_ = resample_likelihood_thr;
00168         }
00169         
00170         /** \brief Set the threshold of angle to be considered occlusion (default: pi/2).
00171           * ParticleFilterTracker does not take the occluded points into account according to the angle
00172           * between the normal and the position. 
00173           * \param[in] occlusion_angle_thr threshold of angle to be considered occlusion.
00174           */
00175         inline void
00176         setOcclusionAngleThe (const double occlusion_angle_thr)
00177         {
00178           occlusion_angle_thr_ = occlusion_angle_thr;
00179         }
00180         
00181         /** \brief Set the minimum number of indices (default: 1).
00182           * ParticleFilterTracker does not take into account the hypothesis
00183           * whose the number of points is smaller than the minimum indices.
00184           * \param[in] min_indices the minimum number of indices.
00185           */
00186         inline void
00187         setMinIndices (const int min_indices) { min_indices_ = min_indices; }
00188 
00189         /** \brief Set the transformation from the world coordinates to the frame of the particles.
00190           * \param[in] trans Affine transformation from the worldcoordinates to the frame of the particles.
00191           */
00192         inline void setTrans (const Eigen::Affine3f &trans) { trans_ = trans; }
00193         
00194         /** \brief Get the transformation from the world coordinates to the frame of the particles. */
00195         inline Eigen::Affine3f getTrans () const { return trans_; }
00196         
00197         /** \brief Get an instance of the result of tracking.
00198           * This function returns the particle that represents the transform between the reference point cloud at the 
00199           * beginning and the best guess about its location in the most recent frame.
00200           */
00201         virtual inline StateT getResult () const { return representative_state_; }
00202         
00203         /** \brief Convert a state to affine transformation from the world coordinates frame.
00204           * \param[in] particle an instance of StateT.
00205           */
00206         Eigen::Affine3f toEigenMatrix (const StateT& particle)
00207         {
00208           return particle.toEigenMatrix ();
00209         }
00210 
00211         /** \brief Get a pointer to a pointcloud of the particles. */
00212         inline PointCloudStatePtr getParticles () const { return particles_; }
00213 
00214         /** \brief Normalize the weight of a particle using \f$ exp(1- alpha ( w - w_{min}) / (w_max - w_min)) \f$
00215           * \note This method is described in [P.Azad et. al, ICRA11].
00216           * \param[in] w the weight to be normalized
00217           * \param[in] w_min the minimum weight of the particles
00218           * \param[in] w_max the maximum weight of the particles
00219           */
00220         inline double normalizeParticleWeight (double w, double w_min, double w_max)
00221         {
00222           return exp (1.0 - alpha_ * (w - w_min) / (w_max - w_min));
00223         }
00224 
00225         /** \brief Set the value of alpha.
00226           *  \param[in] alpha the value of alpha
00227           */
00228         inline void setAlpha (double alpha) { alpha_ = alpha; }
00229         
00230         /** \brief Get the value of alpha. */
00231         inline double getAlpha () { return alpha_; }
00232 
00233         /** \brief Set the value of use_normal_.
00234           * \param[in] use_normal the value of use_normal_.
00235           */
00236         inline void setUseNormal (bool use_normal) { use_normal_ = use_normal; }
00237 
00238         /** \brief Get the value of use_normal_. */
00239         inline bool getUseNormal () { return use_normal_; }
00240 
00241         /** \brief Set the value of use_change_detector_.
00242           * \param[in] use_change_detector the value of use_change_detector_.
00243           */
00244         inline void setUseChangeDetector (bool use_change_detector) { use_change_detector_ = use_change_detector; }
00245 
00246         /** \brief Get the value of use_change_detector_. */
00247         inline bool getUseChangeDetector () { return use_change_detector_; }
00248 
00249         /** \brief Set the motion ratio
00250           * \param[in] motion_ratio the ratio of hypothesis to use motion model.
00251          */
00252         inline void setMotionRatio (double motion_ratio) { motion_ratio_ = motion_ratio; }
00253 
00254         /** \brief Get the motion ratio. */
00255         inline double getMotionRatio () { return motion_ratio_;}
00256 
00257         /** \brief Set the number of interval frames to run change detection.
00258           * \param[in] change_detector_interval the number of interval frames.
00259           */
00260         inline void setIntervalOfChangeDetection (unsigned int change_detector_interval)
00261         {
00262           change_detector_interval_ = change_detector_interval;
00263         }
00264 
00265         /** \brief Get the number of interval frames to run change detection. */
00266         inline unsigned int getIntervalOfChangeDetection ()
00267         {
00268           return change_detector_interval_;
00269         }
00270 
00271         /** \brief Set the minimum amount of points required within leaf node to become serialized in change detection
00272           * \param[in] change_detector_filter the minimum amount of points required within leaf node
00273          */
00274         inline void setMinPointsOfChangeDetection (unsigned int change_detector_filter)
00275         {
00276           change_detector_filter_ = change_detector_filter;
00277         }
00278 
00279         /** \brief Set the resolution of change detection.
00280           * \param[in] resolution resolution of change detection octree
00281           */
00282         inline void setResolutionOfChangeDetection (double resolution) { change_detector_resolution_ = resolution; }
00283 
00284         /** \brief Get the resolution of change detection. */
00285         inline double getResolutionOfChangeDetection () { return change_detector_resolution_; }
00286         
00287         /** \brief Get the minimum amount of points required within leaf node to become serialized in change detection. */
00288         inline unsigned int getMinPointsOfChangeDetection ()
00289         {
00290           return change_detector_filter_;
00291         }
00292         
00293         /** \brief Get the adjustment ratio. */
00294         inline double
00295         getFitRatio() const { return fit_ratio_; }
00296         
00297         /** \brief Reset the particles to restart tracking*/
00298         virtual inline void resetTracking ()
00299         {
00300           if (particles_)
00301             particles_->points.clear ();
00302         }
00303 
00304       protected:
00305 
00306         /** \brief Compute the parameters for the bounding box of hypothesis pointclouds.
00307           * \param[out] x_min the minimum value of x axis.
00308           * \param[out] x_max the maximum value of x axis.
00309           * \param[out] y_min the minimum value of y axis.
00310           * \param[out] y_max the maximum value of y axis.
00311           * \param[out] z_min the minimum value of z axis.
00312           * \param[out] z_max the maximum value of z axis.
00313           */
00314         void calcBoundingBox (double &x_min, double &x_max,
00315                               double &y_min, double &y_max,
00316                               double &z_min, double &z_max);
00317 
00318         /** \brief Crop the pointcloud by the bounding box calculated from hypothesis and the reference pointcloud.
00319           * \param[in] cloud a pointer to pointcloud to be cropped.
00320           * \param[out] output a pointer to be assigned the cropped pointcloud.
00321           */
00322         void cropInputPointCloud (const PointCloudInConstPtr &cloud, PointCloudIn &output);
00323                                   
00324         
00325         
00326         /** \brief Compute a reference pointcloud transformed to the pose that hypothesis represents.
00327           * \param[in] hypothesis a particle which represents a hypothesis.
00328           * \param[in] indices the indices which should be taken into account.
00329           * \param[out] cloud the resultant point cloud model dataset which is transformed to hypothesis.
00330          **/
00331         void computeTransformedPointCloud (const StateT& hypothesis,
00332                                            std::vector<int>& indices,
00333                                            PointCloudIn &cloud);
00334 
00335         /** \brief Compute a reference pointcloud transformed to the pose that hypothesis represents and calculate 
00336           * indices taking occlusion into account.
00337           * \param[in] hypothesis a particle which represents a hypothesis.
00338           * \param[in] indices the indices which should be taken into account.
00339           * \param[out] cloud the resultant point cloud model dataset which is transformed to hypothesis.
00340           **/
00341         void computeTransformedPointCloudWithNormal (const StateT& hypothesis,
00342                                                      std::vector<int>& indices,
00343                                                      PointCloudIn &cloud);
00344 
00345         /** \brief Compute a reference pointcloud transformed to the pose that hypothesis represents and calculate 
00346           * indices without taking occlusion into account.
00347           * \param[in] hypothesis a particle which represents a hypothesis.
00348           * \param[out] cloud the resultant point cloud model dataset which is transformed to hypothesis.
00349          **/
00350         void computeTransformedPointCloudWithoutNormal (const StateT& hypothesis,
00351                                                         PointCloudIn &cloud);
00352 
00353         
00354         /** \brief This method should get called before starting the actual computation. */
00355         virtual bool initCompute ();
00356         
00357         /** \brief Weighting phase of particle filter method. Calculate the likelihood of all of the particles and set the weights. */
00358         virtual void weight ();
00359         
00360         /** \brief Resampling phase of particle filter method. Sampling the particles according to the weights calculated 
00361           * in weight method. In particular, "sample with replacement" is archieved by walker's alias method.
00362           */
00363         virtual void resample ();
00364         
00365         /** \brief Calculate the weighted mean of the particles and set it as the result. */
00366         virtual void update ();
00367 
00368         /** \brief Normalize the weights of all the particels. */
00369         virtual void normalizeWeight ();
00370 
00371         /** \brief Initialize the particles. initial_noise_covariance_ and initial_noise_mean_ are used for Gaussian sampling. */
00372         void initParticles (bool reset);
00373         
00374         /** \brief Track the pointcloud using particle filter method. */
00375         virtual void computeTracking ();
00376         
00377         /** \brief Implementation of "sample with replacement" using Walker's alias method.
00378             about Walker's alias method, you can check the paper below:
00379             @article{355749,
00380              author = {Walker, Alastair J.},
00381              title = {An Efficient Method for Generating Discrete
00382              Random Variables with General Distributions},
00383              journal = {ACM Trans. Math. Softw.},
00384              volume = {3},
00385              number = {3},
00386              year = {1977},
00387              issn = {0098-3500},
00388              pages = {253--256},
00389              doi = {http://doi.acm.org/10.1145/355744.355749},
00390              publisher = {ACM},
00391              address = {New York, NY, USA},
00392              }
00393              \param a an alias table, which generated by genAliasTable.
00394              \param q a table of weight, which generated by genAliasTable.
00395          */
00396         int sampleWithReplacement (const std::vector<int>& a, const std::vector<double>& q);
00397         
00398         /** \brief Generate the tables for walker's alias method. */
00399         void genAliasTable (std::vector<int> &a, std::vector<double> &q, const PointCloudStateConstPtr &particles);
00400 
00401         /** \brief Resampling the particle with replacement. */
00402         void 
00403         resampleWithReplacement ();
00404         
00405         /** \brief Resampling the particle in deterministic way. */
00406         void 
00407         resampleDeterministic ();
00408 
00409         /** \brief Run change detection and return true if there is a change.
00410           * \param[in] input a pointer to the input pointcloud.
00411           */
00412         bool 
00413         testChangeDetection (const PointCloudInConstPtr &input);
00414         
00415         /** \brief The number of iteration of particlefilter. */
00416         int iteration_num_;
00417 
00418         /** \brief The number of the particles. */
00419         int particle_num_;
00420 
00421         /** \brief The minimum number of points which the hypothesis should have. */
00422         int min_indices_;
00423   
00424         /** \brief Adjustment of the particle filter. */
00425         double fit_ratio_;
00426 
00427         /** \brief A pointer to reference point cloud. */
00428         PointCloudInConstPtr ref_;
00429 
00430         /** \brief A pointer to the particles  */
00431         PointCloudStatePtr particles_;
00432 
00433         /** \brief A pointer to PointCloudCoherence. */
00434         CloudCoherencePtr coherence_;
00435 
00436         /** \brief The diagonal elements of covariance matrix of the step noise. the covariance matrix is used
00437           * at every resample method.
00438           */
00439         std::vector<double> step_noise_covariance_;
00440 
00441         /** \brief The diagonal elements of covariance matrix of the initial noise. the covariance matrix is used
00442           * when initialize the particles.
00443           */
00444         std::vector<double> initial_noise_covariance_;
00445         
00446         /** \brief The mean values of initial noise. */
00447         std::vector<double> initial_noise_mean_;
00448 
00449         /** \brief The threshold for the particles to be re-initialized. */
00450         double resample_likelihood_thr_;
00451 
00452         /** \brief The threshold for the points to be considered as occluded. */
00453         double occlusion_angle_thr_;
00454 
00455         /** \brief The weight to be used in normalization of the weights of the particles. */
00456         double alpha_;
00457         
00458         /** \brief The result of tracking. */
00459         StateT representative_state_;
00460 
00461         /** \brief An affine transformation from the world coordinates frame to the origin of the particles. */
00462         Eigen::Affine3f trans_;
00463 
00464         /** \brief A flag to use normal or not. defaults to false. */
00465         bool use_normal_;
00466 
00467         /** \brief Difference between the result in t and t-1. */
00468         StateT motion_;
00469 
00470         /** \brief Ratio of hypothesis to use motion model. */
00471         double motion_ratio_;
00472 
00473         /** \brief Pass through filter to crop the pointclouds within the hypothesis bounding box. */
00474         pcl::PassThrough<PointInT> pass_x_;
00475         /** \brief Pass through filter to crop the pointclouds within the hypothesis bounding box. */
00476         pcl::PassThrough<PointInT> pass_y_;
00477         /** \brief Pass through filter to crop the pointclouds within the hypothesis bounding box. */
00478         pcl::PassThrough<PointInT> pass_z_;
00479 
00480         /** \brief A list of the pointers to pointclouds. */
00481         std::vector<PointCloudInPtr> transed_reference_vector_;
00482 
00483         /** \brief Change detector used as a trigger to track. */
00484         boost::shared_ptr<pcl::octree::OctreePointCloudChangeDetector<PointInT> > change_detector_;
00485 
00486         /** \brief A flag to be true when change of pointclouds is detected. */
00487         bool changed_;
00488 
00489         /** \brief A counter to skip change detection. */
00490         unsigned int change_counter_;
00491         
00492         /** \brief Minimum points in a leaf when calling change detector. defaults to 10. */
00493         unsigned int change_detector_filter_;
00494 
00495         /** \brief The number of interval frame to run change detection. defaults to 10. */
00496         unsigned int change_detector_interval_;
00497 
00498         /** \brief Resolution of change detector. defaults to 0.01. */
00499         double change_detector_resolution_;
00500         
00501         /** \brief The flag which will be true if using change detection. */
00502         bool use_change_detector_;
00503     };
00504   }
00505 }
00506 
00507 // #include <pcl/tracking/impl/particle_filter.hpp>
00508 #ifdef PCL_NO_PRECOMPILE
00509 #include <pcl/tracking/impl/particle_filter.hpp>
00510 #endif
00511 
00512 #endif //PCL_TRACKING_PARTICLE_FILTER_H_