Point Cloud Library (PCL)  1.7.1
particle_filter.h
1 #ifndef PCL_TRACKING_PARTICLE_FILTER_H_
2 #define PCL_TRACKING_PARTICLE_FILTER_H_
3 
4 #include <pcl/tracking/tracking.h>
5 #include <pcl/tracking/tracker.h>
6 #include <pcl/tracking/coherence.h>
7 #include <pcl/filters/passthrough.h>
8 #include <pcl/octree/octree.h>
9 
10 #include <Eigen/Dense>
11 
12 namespace pcl
13 {
14 
15  namespace tracking
16  {
17  /** \brief @b ParticleFilterTracker tracks the PointCloud which is given by
18  setReferenceCloud within the measured PointCloud using particle filter method.
19  * \author Ryohei Ueda
20  * \ingroup tracking
21  */
22  template <typename PointInT, typename StateT>
23  class ParticleFilterTracker: public Tracker<PointInT, StateT>
24  {
25  protected:
27 
28  public:
34 
36 
40 
44 
46  typedef boost::shared_ptr< Coherence > CoherencePtr;
47  typedef boost::shared_ptr< const Coherence > CoherenceConstPtr;
48 
50  typedef boost::shared_ptr< CloudCoherence > CloudCoherencePtr;
51  typedef boost::shared_ptr< const CloudCoherence > CloudCoherenceConstPtr;
52 
53  /** \brief Empty constructor. */
55  : iteration_num_ (1)
56  , particle_num_ ()
57  , min_indices_ (1)
58  , ref_ ()
59  , particles_ ()
60  , coherence_ ()
65  , occlusion_angle_thr_ (M_PI / 2.0)
66  , alpha_ (15.0)
68  , trans_ ()
69  , use_normal_ (false)
70  , motion_ ()
71  , motion_ratio_ (0.25)
72  , pass_x_ ()
73  , pass_y_ ()
74  , pass_z_ ()
76  , change_detector_ ()
77  , changed_ (false)
78  , change_counter_ (0)
82  , use_change_detector_ (false)
83  {
84  tracker_name_ = "ParticleFilterTracker";
88  pass_x_.setKeepOrganized (false);
89  pass_y_.setKeepOrganized (false);
90  pass_z_.setKeepOrganized (false);
91  }
92 
93  /** \brief Set the number of iteration.
94  * \param[in] iteration_num the number of iteration.
95  */
96  inline void
97  setIterationNum (const int iteration_num) { iteration_num_ = iteration_num; }
98 
99  /** \brief Get the number of iteration. */
100  inline int
101  getIterationNum () const { return iteration_num_; }
102 
103  /** \brief Set the number of the particles.
104  * \param[in] particle_num the number of the particles.
105  */
106  inline void
107  setParticleNum (const int particle_num) { particle_num_ = particle_num; }
108 
109  /** \brief Get the number of the particles. */
110  inline int
111  getParticleNum () const { return particle_num_; }
112 
113  /** \brief Set a pointer to a reference dataset to be tracked.
114  * \param[in] ref a pointer to a PointCloud message
115  */
116  inline void
118 
119  /** \brief Get a pointer to a reference dataset to be tracked. */
120  inline PointCloudInConstPtr const
121  getReferenceCloud () { return ref_; }
122 
123  /** \brief Set the PointCloudCoherence as likelihood.
124  * \param[in] coherence a pointer to PointCloudCoherence.
125  */
126  inline void
127  setCloudCoherence (const CloudCoherencePtr &coherence) { coherence_ = coherence; }
128 
129  /** \brief Get the PointCloudCoherence to compute likelihood. */
130  inline CloudCoherencePtr
131  getCloudCoherence () const { return coherence_; }
132 
133 
134  /** \brief Set the covariance of step noise.
135  * \param[in] step_noise_covariance the diagonal elements of covariance matrix of step noise.
136  */
137  inline void
138  setStepNoiseCovariance (const std::vector<double> &step_noise_covariance)
139  {
140  step_noise_covariance_ = step_noise_covariance;
141  }
142 
143  /** \brief Set the covariance of the initial noise. It will be used when initializing the particles.
144  * \param[in] initial_noise_covariance the diagonal elements of covariance matrix of initial noise.
145  */
146  inline void
147  setInitialNoiseCovariance (const std::vector<double> &initial_noise_covariance)
148  {
149  initial_noise_covariance_ = initial_noise_covariance;
150  }
151 
152  /** \brief Set the mean of the initial noise. It will be used when initializing the particles.
153  * \param[in] initial_noise_mean the mean values of initial noise.
154  */
155  inline void
156  setInitialNoiseMean (const std::vector<double> &initial_noise_mean)
157  {
158  initial_noise_mean_ = initial_noise_mean;
159  }
160 
161  /** \brief Set the threshold to re-initialize the particles.
162  * \param[in] resample_likelihood_thr threshold to re-initialize.
163  */
164  inline void
165  setResampleLikelihoodThr (const double resample_likelihood_thr)
166  {
167  resample_likelihood_thr_ = resample_likelihood_thr;
168  }
169 
170  /** \brief Set the threshold of angle to be considered occlusion (default: pi/2).
171  * ParticleFilterTracker does not take the occluded points into account according to the angle
172  * between the normal and the position.
173  * \param[in] occlusion_angle_thr threshold of angle to be considered occlusion.
174  */
175  inline void
176  setOcclusionAngleThe (const double occlusion_angle_thr)
177  {
178  occlusion_angle_thr_ = occlusion_angle_thr;
179  }
180 
181  /** \brief Set the minimum number of indices (default: 1).
182  * ParticleFilterTracker does not take into account the hypothesis
183  * whose the number of points is smaller than the minimum indices.
184  * \param[in] min_indices the minimum number of indices.
185  */
186  inline void
187  setMinIndices (const int min_indices) { min_indices_ = min_indices; }
188 
189  /** \brief Set the transformation from the world coordinates to the frame of the particles.
190  * \param[in] trans Affine transformation from the worldcoordinates to the frame of the particles.
191  */
192  inline void setTrans (const Eigen::Affine3f &trans) { trans_ = trans; }
193 
194  /** \brief Get the transformation from the world coordinates to the frame of the particles. */
195  inline Eigen::Affine3f getTrans () const { return trans_; }
196 
197  /** \brief Get an instance of the result of tracking.
198  * This function returns the particle that represents the transform between the reference point cloud at the
199  * beginning and the best guess about its location in the most recent frame.
200  */
201  virtual inline StateT getResult () const { return representative_state_; }
202 
203  /** \brief Convert a state to affine transformation from the world coordinates frame.
204  * \param[in] particle an instance of StateT.
205  */
206  Eigen::Affine3f toEigenMatrix (const StateT& particle)
207  {
208  return particle.toEigenMatrix ();
209  }
210 
211  /** \brief Get a pointer to a pointcloud of the particles. */
212  inline PointCloudStatePtr getParticles () const { return particles_; }
213 
214  /** \brief Normalize the weight of a particle using \f$ exp(1- alpha ( w - w_{min}) / (w_max - w_min)) \f$
215  * \note This method is described in [P.Azad et. al, ICRA11].
216  * \param[in] w the weight to be normalized
217  * \param[in] w_min the minimum weight of the particles
218  * \param[in] w_max the maximum weight of the particles
219  */
220  inline double normalizeParticleWeight (double w, double w_min, double w_max)
221  {
222  return exp (1.0 - alpha_ * (w - w_min) / (w_max - w_min));
223  }
224 
225  /** \brief Set the value of alpha.
226  * \param[in] alpha the value of alpha
227  */
228  inline void setAlpha (double alpha) { alpha_ = alpha; }
229 
230  /** \brief Get the value of alpha. */
231  inline double getAlpha () { return alpha_; }
232 
233  /** \brief Set the value of use_normal_.
234  * \param[in] use_normal the value of use_normal_.
235  */
236  inline void setUseNormal (bool use_normal) { use_normal_ = use_normal; }
237 
238  /** \brief Get the value of use_normal_. */
239  inline bool getUseNormal () { return use_normal_; }
240 
241  /** \brief Set the value of use_change_detector_.
242  * \param[in] use_change_detector the value of use_change_detector_.
243  */
244  inline void setUseChangeDetector (bool use_change_detector) { use_change_detector_ = use_change_detector; }
245 
246  /** \brief Get the value of use_change_detector_. */
247  inline bool getUseChangeDetector () { return use_change_detector_; }
248 
249  /** \brief Set the motion ratio
250  * \param[in] motion_ratio the ratio of hypothesis to use motion model.
251  */
252  inline void setMotionRatio (double motion_ratio) { motion_ratio_ = motion_ratio; }
253 
254  /** \brief Get the motion ratio. */
255  inline double getMotionRatio () { return motion_ratio_;}
256 
257  /** \brief Set the number of interval frames to run change detection.
258  * \param[in] change_detector_interval the number of interval frames.
259  */
260  inline void setIntervalOfChangeDetection (unsigned int change_detector_interval)
261  {
262  change_detector_interval_ = change_detector_interval;
263  }
264 
265  /** \brief Get the number of interval frames to run change detection. */
266  inline unsigned int getIntervalOfChangeDetection ()
267  {
269  }
270 
271  /** \brief Set the minimum amount of points required within leaf node to become serialized in change detection
272  * \param[in] change_detector_filter the minimum amount of points required within leaf node
273  */
274  inline void setMinPointsOfChangeDetection (unsigned int change_detector_filter)
275  {
276  change_detector_filter_ = change_detector_filter;
277  }
278 
279  /** \brief Set the resolution of change detection.
280  * \param[in] resolution resolution of change detection octree
281  */
282  inline void setResolutionOfChangeDetection (double resolution) { change_detector_resolution_ = resolution; }
283 
284  /** \brief Get the resolution of change detection. */
286 
287  /** \brief Get the minimum amount of points required within leaf node to become serialized in change detection. */
288  inline unsigned int getMinPointsOfChangeDetection ()
289  {
291  }
292 
293  /** \brief Get the adjustment ratio. */
294  inline double
295  getFitRatio() const { return fit_ratio_; }
296 
297  /** \brief Reset the particles to restart tracking*/
298  virtual inline void resetTracking ()
299  {
300  if (particles_)
301  particles_->points.clear ();
302  }
303 
304  protected:
305 
306  /** \brief Compute the parameters for the bounding box of hypothesis pointclouds.
307  * \param[out] x_min the minimum value of x axis.
308  * \param[out] x_max the maximum value of x axis.
309  * \param[out] y_min the minimum value of y axis.
310  * \param[out] y_max the maximum value of y axis.
311  * \param[out] z_min the minimum value of z axis.
312  * \param[out] z_max the maximum value of z axis.
313  */
314  void calcBoundingBox (double &x_min, double &x_max,
315  double &y_min, double &y_max,
316  double &z_min, double &z_max);
317 
318  /** \brief Crop the pointcloud by the bounding box calculated from hypothesis and the reference pointcloud.
319  * \param[in] cloud a pointer to pointcloud to be cropped.
320  * \param[out] output a pointer to be assigned the cropped pointcloud.
321  */
322  void cropInputPointCloud (const PointCloudInConstPtr &cloud, PointCloudIn &output);
323 
324 
325 
326  /** \brief Compute a reference pointcloud transformed to the pose that hypothesis represents.
327  * \param[in] hypothesis a particle which represents a hypothesis.
328  * \param[in] indices the indices which should be taken into account.
329  * \param[out] cloud the resultant point cloud model dataset which is transformed to hypothesis.
330  **/
331  void computeTransformedPointCloud (const StateT& hypothesis,
332  std::vector<int>& indices,
333  PointCloudIn &cloud);
334 
335  /** \brief Compute a reference pointcloud transformed to the pose that hypothesis represents and calculate
336  * indices taking occlusion into account.
337  * \param[in] hypothesis a particle which represents a hypothesis.
338  * \param[in] indices the indices which should be taken into account.
339  * \param[out] cloud the resultant point cloud model dataset which is transformed to hypothesis.
340  **/
341  void computeTransformedPointCloudWithNormal (const StateT& hypothesis,
342  std::vector<int>& indices,
343  PointCloudIn &cloud);
344 
345  /** \brief Compute a reference pointcloud transformed to the pose that hypothesis represents and calculate
346  * indices without taking occlusion into account.
347  * \param[in] hypothesis a particle which represents a hypothesis.
348  * \param[out] cloud the resultant point cloud model dataset which is transformed to hypothesis.
349  **/
350  void computeTransformedPointCloudWithoutNormal (const StateT& hypothesis,
351  PointCloudIn &cloud);
352 
353 
354  /** \brief This method should get called before starting the actual computation. */
355  virtual bool initCompute ();
356 
357  /** \brief Weighting phase of particle filter method. Calculate the likelihood of all of the particles and set the weights. */
358  virtual void weight ();
359 
360  /** \brief Resampling phase of particle filter method. Sampling the particles according to the weights calculated
361  * in weight method. In particular, "sample with replacement" is archieved by walker's alias method.
362  */
363  virtual void resample ();
364 
365  /** \brief Calculate the weighted mean of the particles and set it as the result. */
366  virtual void update ();
367 
368  /** \brief Normalize the weights of all the particels. */
369  virtual void normalizeWeight ();
370 
371  /** \brief Initialize the particles. initial_noise_covariance_ and initial_noise_mean_ are used for Gaussian sampling. */
372  void initParticles (bool reset);
373 
374  /** \brief Track the pointcloud using particle filter method. */
375  virtual void computeTracking ();
376 
377  /** \brief Implementation of "sample with replacement" using Walker's alias method.
378  about Walker's alias method, you can check the paper below:
379  @article{355749,
380  author = {Walker, Alastair J.},
381  title = {An Efficient Method for Generating Discrete
382  Random Variables with General Distributions},
383  journal = {ACM Trans. Math. Softw.},
384  volume = {3},
385  number = {3},
386  year = {1977},
387  issn = {0098-3500},
388  pages = {253--256},
389  doi = {http://doi.acm.org/10.1145/355744.355749},
390  publisher = {ACM},
391  address = {New York, NY, USA},
392  }
393  \param a an alias table, which generated by genAliasTable.
394  \param q a table of weight, which generated by genAliasTable.
395  */
396  int sampleWithReplacement (const std::vector<int>& a, const std::vector<double>& q);
397 
398  /** \brief Generate the tables for walker's alias method. */
399  void genAliasTable (std::vector<int> &a, std::vector<double> &q, const PointCloudStateConstPtr &particles);
400 
401  /** \brief Resampling the particle with replacement. */
402  void
404 
405  /** \brief Resampling the particle in deterministic way. */
406  void
408 
409  /** \brief Run change detection and return true if there is a change.
410  * \param[in] input a pointer to the input pointcloud.
411  */
412  bool
414 
415  /** \brief The number of iteration of particlefilter. */
417 
418  /** \brief The number of the particles. */
420 
421  /** \brief The minimum number of points which the hypothesis should have. */
423 
424  /** \brief Adjustment of the particle filter. */
425  double fit_ratio_;
426 
427  /** \brief A pointer to reference point cloud. */
429 
430  /** \brief A pointer to the particles */
432 
433  /** \brief A pointer to PointCloudCoherence. */
435 
436  /** \brief The diagonal elements of covariance matrix of the step noise. the covariance matrix is used
437  * at every resample method.
438  */
439  std::vector<double> step_noise_covariance_;
440 
441  /** \brief The diagonal elements of covariance matrix of the initial noise. the covariance matrix is used
442  * when initialize the particles.
443  */
444  std::vector<double> initial_noise_covariance_;
445 
446  /** \brief The mean values of initial noise. */
447  std::vector<double> initial_noise_mean_;
448 
449  /** \brief The threshold for the particles to be re-initialized. */
451 
452  /** \brief The threshold for the points to be considered as occluded. */
454 
455  /** \brief The weight to be used in normalization of the weights of the particles. */
456  double alpha_;
457 
458  /** \brief The result of tracking. */
460 
461  /** \brief An affine transformation from the world coordinates frame to the origin of the particles. */
462  Eigen::Affine3f trans_;
463 
464  /** \brief A flag to use normal or not. defaults to false. */
466 
467  /** \brief Difference between the result in t and t-1. */
468  StateT motion_;
469 
470  /** \brief Ratio of hypothesis to use motion model. */
472 
473  /** \brief Pass through filter to crop the pointclouds within the hypothesis bounding box. */
475  /** \brief Pass through filter to crop the pointclouds within the hypothesis bounding box. */
477  /** \brief Pass through filter to crop the pointclouds within the hypothesis bounding box. */
479 
480  /** \brief A list of the pointers to pointclouds. */
481  std::vector<PointCloudInPtr> transed_reference_vector_;
482 
483  /** \brief Change detector used as a trigger to track. */
484  boost::shared_ptr<pcl::octree::OctreePointCloudChangeDetector<PointInT> > change_detector_;
485 
486  /** \brief A flag to be true when change of pointclouds is detected. */
487  bool changed_;
488 
489  /** \brief A counter to skip change detection. */
490  unsigned int change_counter_;
491 
492  /** \brief Minimum points in a leaf when calling change detector. defaults to 10. */
494 
495  /** \brief The number of interval frame to run change detection. defaults to 10. */
497 
498  /** \brief Resolution of change detector. defaults to 0.01. */
500 
501  /** \brief The flag which will be true if using change detection. */
503  };
504  }
505 }
506 
507 // #include <pcl/tracking/impl/particle_filter.hpp>
508 #ifdef PCL_NO_PRECOMPILE
509 #include <pcl/tracking/impl/particle_filter.hpp>
510 #endif
511 
512 #endif //PCL_TRACKING_PARTICLE_FILTER_H_