Point Cloud Library (PCL)  1.11.1
particle_filter.h
1 #pragma once
2 
3 #include <pcl/filters/passthrough.h>
4 #include <pcl/octree/octree_pointcloud_changedetector.h>
5 #include <pcl/tracking/coherence.h>
6 #include <pcl/tracking/tracker.h>
7 #include <pcl/tracking/tracking.h>
8 #include <pcl/memory.h>
9 
10 #include <Eigen/Dense>
11 
12 namespace pcl {
13 namespace tracking {
14 /** \brief @b ParticleFilterTracker tracks the PointCloud which is given by
15  * setReferenceCloud within the measured PointCloud using particle filter method.
16  * \author Ryohei Ueda
17  * \ingroup tracking
18  */
19 template <typename PointInT, typename StateT>
20 class ParticleFilterTracker : public Tracker<PointInT, StateT> {
21 protected:
23 
24 public:
30 
31  using Ptr = shared_ptr<ParticleFilterTracker<PointInT, StateT>>;
32  using ConstPtr = shared_ptr<const ParticleFilterTracker<PointInT, StateT>>;
33 
35 
37  using PointCloudInPtr = typename PointCloudIn::Ptr;
38  using PointCloudInConstPtr = typename PointCloudIn::ConstPtr;
39 
41  using PointCloudStatePtr = typename PointCloudState::Ptr;
42  using PointCloudStateConstPtr = typename PointCloudState::ConstPtr;
43 
45  using CoherencePtr = typename Coherence::Ptr;
47 
51 
52  /** \brief Empty constructor. */
54  : iteration_num_(1)
55  , particle_num_()
56  , min_indices_(1)
57  , ref_()
58  , particles_()
59  , coherence_()
61  , occlusion_angle_thr_(M_PI / 2.0)
62  , alpha_(15.0)
64  , use_normal_(false)
65  , motion_()
66  , motion_ratio_(0.25)
67  , pass_x_()
68  , pass_y_()
69  , pass_z_()
72  , changed_(false)
73  , change_counter_(0)
77  , use_change_detector_(false)
78  {
79  tracker_name_ = "ParticleFilterTracker";
86  }
87 
88  /** \brief Set the number of iteration.
89  * \param[in] iteration_num the number of iteration.
90  */
91  inline void
92  setIterationNum(const int iteration_num)
93  {
94  iteration_num_ = iteration_num;
95  }
96 
97  /** \brief Get the number of iteration. */
98  inline int
100  {
101  return iteration_num_;
102  }
103 
104  /** \brief Set the number of the particles.
105  * \param[in] particle_num the number of the particles.
106  */
107  inline void
108  setParticleNum(const int particle_num)
109  {
110  particle_num_ = particle_num;
111  }
112 
113  /** \brief Get the number of the particles. */
114  inline int
116  {
117  return particle_num_;
118  }
119 
120  /** \brief Set a pointer to a reference dataset to be tracked.
121  * \param[in] ref a pointer to a PointCloud message
122  */
123  inline void
125  {
126  ref_ = ref;
127  }
128 
129  /** \brief Get a pointer to a reference dataset to be tracked. */
130  inline PointCloudInConstPtr const
132  {
133  return ref_;
134  }
135 
136  /** \brief Set the PointCloudCoherence as likelihood.
137  * \param[in] coherence a pointer to PointCloudCoherence.
138  */
139  inline void
141  {
142  coherence_ = coherence;
143  }
144 
145  /** \brief Get the PointCloudCoherence to compute likelihood. */
146  inline CloudCoherencePtr
148  {
149  return coherence_;
150  }
151 
152  /** \brief Set the covariance of step noise.
153  * \param[in] step_noise_covariance the diagonal elements of covariance matrix
154  * of step noise.
155  */
156  inline void
157  setStepNoiseCovariance(const std::vector<double>& step_noise_covariance)
158  {
159  step_noise_covariance_ = step_noise_covariance;
160  }
161 
162  /** \brief Set the covariance of the initial noise. It will be used when
163  * initializing the particles.
164  * \param[in] initial_noise_covariance the diagonal elements of covariance matrix of
165  * initial noise.
166  */
167  inline void
168  setInitialNoiseCovariance(const std::vector<double>& initial_noise_covariance)
169  {
170  initial_noise_covariance_ = initial_noise_covariance;
171  }
172 
173  /** \brief Set the mean of the initial noise. It will be used when
174  * initializing the particles.
175  * \param[in] initial_noise_mean the mean values of initial noise.
176  */
177  inline void
178  setInitialNoiseMean(const std::vector<double>& initial_noise_mean)
179  {
180  initial_noise_mean_ = initial_noise_mean;
181  }
182 
183  /** \brief Set the threshold to re-initialize the particles.
184  * \param[in] resample_likelihood_thr threshold to re-initialize.
185  */
186  inline void
187  setResampleLikelihoodThr(const double resample_likelihood_thr)
188  {
189  resample_likelihood_thr_ = resample_likelihood_thr;
190  }
191 
192  /** \brief Set the threshold of angle to be considered occlusion (default:
193  * pi/2). ParticleFilterTracker does not take the occluded points into account
194  * according to the angle between the normal and the position.
195  * \param[in] occlusion_angle_thr threshold of angle to be considered occlusion.
196  */
197  inline void
198  setOcclusionAngleThe(const double occlusion_angle_thr)
199  {
200  occlusion_angle_thr_ = occlusion_angle_thr;
201  }
202 
203  /** \brief Set the minimum number of indices (default: 1).
204  * ParticleFilterTracker does not take into account the hypothesis
205  * whose the number of points is smaller than the minimum indices.
206  * \param[in] min_indices the minimum number of indices.
207  */
208  inline void
209  setMinIndices(const int min_indices)
210  {
211  min_indices_ = min_indices;
212  }
213 
214  /** \brief Set the transformation from the world coordinates to the frame of
215  * the particles.
216  * \param[in] trans Affine transformation from the worldcoordinates to the frame of
217  * the particles.
218  */
219  inline void
220  setTrans(const Eigen::Affine3f& trans)
221  {
222  trans_ = trans;
223  }
224 
225  /** \brief Get the transformation from the world coordinates to the frame of
226  * the particles. */
227  inline Eigen::Affine3f
228  getTrans() const
229  {
230  return trans_;
231  }
232 
233  /** \brief Get an instance of the result of tracking.
234  * This function returns the particle that represents the transform between
235  * the reference point cloud at the beginning and the best guess about its
236  * location in the most recent frame.
237  */
238  inline StateT
239  getResult() const override
240  {
241  return representative_state_;
242  }
243 
244  /** \brief Convert a state to affine transformation from the world coordinates
245  * frame.
246  * \param[in] particle an instance of StateT.
247  */
248  Eigen::Affine3f
249  toEigenMatrix(const StateT& particle)
250  {
251  return particle.toEigenMatrix();
252  }
253 
254  /** \brief Get a pointer to a pointcloud of the particles. */
255  inline PointCloudStatePtr
256  getParticles() const
257  {
258  return particles_;
259  }
260 
261  /** \brief Normalize the weight of a particle using \f$ std::exp(1- alpha ( w
262  * - w_{min}) / (w_max - w_min)) \f$
263  * \note This method is described in [P.Azad
264  * et. al, ICRA11].
265  * \param[in] w the weight to be normalized
266  * \param[in] w_min the minimum weight of the particles
267  * \param[in] w_max the maximum weight of the particles
268  */
269  inline double
270  normalizeParticleWeight(double w, double w_min, double w_max)
271  {
272  return std::exp(1.0 - alpha_ * (w - w_min) / (w_max - w_min));
273  }
274 
275  /** \brief Set the value of alpha.
276  * \param[in] alpha the value of alpha
277  */
278  inline void
279  setAlpha(double alpha)
280  {
281  alpha_ = alpha;
282  }
283 
284  /** \brief Get the value of alpha. */
285  inline double
287  {
288  return alpha_;
289  }
290 
291  /** \brief Set the value of use_normal_.
292  * \param[in] use_normal the value of use_normal_.
293  */
294  inline void
295  setUseNormal(bool use_normal)
296  {
297  use_normal_ = use_normal;
298  }
299 
300  /** \brief Get the value of use_normal_. */
301  inline bool
303  {
304  return use_normal_;
305  }
306 
307  /** \brief Set the value of use_change_detector_.
308  * \param[in] use_change_detector the value of use_change_detector_.
309  */
310  inline void
311  setUseChangeDetector(bool use_change_detector)
312  {
313  use_change_detector_ = use_change_detector;
314  }
315 
316  /** \brief Get the value of use_change_detector_. */
317  inline bool
319  {
320  return use_change_detector_;
321  }
322 
323  /** \brief Set the motion ratio
324  * \param[in] motion_ratio the ratio of hypothesis to use motion model.
325  */
326  inline void
327  setMotionRatio(double motion_ratio)
328  {
329  motion_ratio_ = motion_ratio;
330  }
331 
332  /** \brief Get the motion ratio. */
333  inline double
335  {
336  return motion_ratio_;
337  }
338 
339  /** \brief Set the number of interval frames to run change detection.
340  * \param[in] change_detector_interval the number of interval frames.
341  */
342  inline void
343  setIntervalOfChangeDetection(unsigned int change_detector_interval)
344  {
345  change_detector_interval_ = change_detector_interval;
346  }
347 
348  /** \brief Get the number of interval frames to run change detection. */
349  inline unsigned int
351  {
353  }
354 
355  /** \brief Set the minimum amount of points required within leaf node to
356  * become serialized in change detection
357  * \param[in] change_detector_filter the minimum amount of points required within leaf
358  * node
359  */
360  inline void
361  setMinPointsOfChangeDetection(unsigned int change_detector_filter)
362  {
363  change_detector_filter_ = change_detector_filter;
364  }
365 
366  /** \brief Set the resolution of change detection.
367  * \param[in] resolution resolution of change detection octree
368  */
369  inline void
371  {
372  change_detector_resolution_ = resolution;
373  }
374 
375  /** \brief Get the resolution of change detection. */
376  inline double
378  {
380  }
381 
382  /** \brief Get the minimum amount of points required within leaf node to
383  * become serialized in change detection. */
384  inline unsigned int
386  {
388  }
389 
390  /** \brief Get the adjustment ratio. */
391  inline double
392  getFitRatio() const
393  {
394  return fit_ratio_;
395  }
396 
397  /** \brief Reset the particles to restart tracking*/
398  virtual inline void
400  {
401  if (particles_)
402  particles_->points.clear();
403  }
404 
405 protected:
406  /** \brief Compute the parameters for the bounding box of hypothesis
407  * pointclouds.
408  * \param[out] x_min the minimum value of x axis.
409  * \param[out] x_max the maximum value of x axis.
410  * \param[out] y_min the minimum value of y axis.
411  * \param[out] y_max the maximum value of y axis.
412  * \param[out] z_min the minimum value of z axis.
413  * \param[out] z_max the maximum value of z axis.
414  */
415  void
416  calcBoundingBox(double& x_min,
417  double& x_max,
418  double& y_min,
419  double& y_max,
420  double& z_min,
421  double& z_max);
422 
423  /** \brief Crop the pointcloud by the bounding box calculated from hypothesis
424  * and the reference pointcloud.
425  * \param[in] cloud a pointer to pointcloud to be cropped.
426  * \param[out] output a pointer to be assigned the cropped pointcloud.
427  */
428  void
430 
431  /** \brief Compute a reference pointcloud transformed to the pose that hypothesis
432  represents.
433  * \param[in] hypothesis a particle which represents a hypothesis.
434  * \param[in] indices the indices which should be taken into account.
435  * \param[out] cloud the resultant point cloud model dataset which is transformed to
436  hypothesis.
437  **/
438  void
439  computeTransformedPointCloud(const StateT& hypothesis,
440  std::vector<int>& indices,
441  PointCloudIn& cloud);
442 
443  /** \brief Compute a reference pointcloud transformed to the pose that hypothesis
444  * represents and calculate indices taking occlusion into account.
445  * \param[in] hypothesis a particle which represents a hypothesis.
446  * \param[in] indices the indices which should be taken into account.
447  * \param[out] cloud the resultant point cloud model dataset which is transformed to
448  hypothesis.
449  **/
450  void
451  computeTransformedPointCloudWithNormal(const StateT& hypothesis,
452  std::vector<int>& indices,
453  PointCloudIn& cloud);
454 
455  /** \brief Compute a reference pointcloud transformed to the pose that hypothesis
456  * represents and calculate indices without taking occlusion into account.
457  * \param[in] hypothesis a particle which represents a hypothesis.
458  * \param[out] cloud the resultant point cloud model dataset which is transformed to
459  *hypothesis.
460  **/
461  void
462  computeTransformedPointCloudWithoutNormal(const StateT& hypothesis,
463  PointCloudIn& cloud);
464 
465  /** \brief This method should get called before starting the actua computation. */
466  bool
467  initCompute() override;
468 
469  /** \brief Weighting phase of particle filter method. Calculate the likelihood
470  * of all of the particles and set the weights. */
471  virtual void
472  weight();
473 
474  /** \brief Resampling phase of particle filter method. Sampling the particles
475  * according to the weights calculated in weight method. In particular,
476  * "sample with replacement" is archieved by walker's alias method.
477  */
478  virtual void
479  resample();
480 
481  /** \brief Calculate the weighted mean of the particles and set it as the result. */
482  virtual void
483  update();
484 
485  /** \brief Normalize the weights of all the particels. */
486  virtual void
487  normalizeWeight();
488 
489  /** \brief Initialize the particles. initial_noise_covariance_ and
490  * initial_noise_mean_ are used for Gaussian sampling. */
491  void
492  initParticles(bool reset);
493 
494  /** \brief Track the pointcloud using particle filter method. */
495  void
496  computeTracking() override;
497 
498  /** \brief Implementation of "sample with replacement" using Walker's alias method.
499  * about Walker's alias method, you can check the paper below: article{355749}, author
500  * = {Walker, Alastair J.}, title = {An Efficient Method for Generating Discrete
501  * Random Variables with General Distributions},
502  * journal = {ACM Trans. Math. Softw.},
503  * volume = {3},
504  * number = {3},
505  * year = {1977},
506  * issn = {0098-3500},
507  * pages = {253--256},
508  * doi = {http://doi.acm.org/10.1145/355744.355749},
509  * publisher = {ACM},
510  * address = {New York, NY, USA},
511  * }
512  * \param a an alias table, which generated by genAliasTable.
513  * \param q a table of weight, which generated by genAliasTable.
514  */
515  int
516  sampleWithReplacement(const std::vector<int>& a, const std::vector<double>& q);
517 
518  /** \brief Generate the tables for walker's alias method. */
519  void
520  genAliasTable(std::vector<int>& a,
521  std::vector<double>& q,
522  const PointCloudStateConstPtr& particles);
523 
524  /** \brief Resampling the particle with replacement. */
525  void
527 
528  /** \brief Resampling the particle in deterministic way. */
529  void
531 
532  /** \brief Run change detection and return true if there is a change.
533  * \param[in] input a pointer to the input pointcloud.
534  */
535  bool
537 
538  /** \brief The number of iteration of particlefilter. */
540 
541  /** \brief The number of the particles. */
543 
544  /** \brief The minimum number of points which the hypothesis should have. */
546 
547  /** \brief Adjustment of the particle filter. */
548  double fit_ratio_;
549 
550  /** \brief A pointer to reference point cloud. */
552 
553  /** \brief A pointer to the particles */
555 
556  /** \brief A pointer to PointCloudCoherence. */
558 
559  /** \brief The diagonal elements of covariance matrix of the step noise. the
560  * covariance matrix is used at every resample method.
561  */
562  std::vector<double> step_noise_covariance_;
563 
564  /** \brief The diagonal elements of covariance matrix of the initial noise.
565  * the covariance matrix is used when initialize the particles.
566  */
567  std::vector<double> initial_noise_covariance_;
568 
569  /** \brief The mean values of initial noise. */
570  std::vector<double> initial_noise_mean_;
571 
572  /** \brief The threshold for the particles to be re-initialized. */
574 
575  /** \brief The threshold for the points to be considered as occluded. */
577 
578  /** \brief The weight to be used in normalization of the weights of the
579  * particles. */
580  double alpha_;
581 
582  /** \brief The result of tracking. */
584 
585  /** \brief An affine transformation from the world coordinates frame to the
586  * origin of the particles. */
587  Eigen::Affine3f trans_;
588 
589  /** \brief A flag to use normal or not. defaults to false. */
591 
592  /** \brief Difference between the result in t and t-1. */
593  StateT motion_;
594 
595  /** \brief Ratio of hypothesis to use motion model. */
597 
598  /** \brief Pass through filter to crop the pointclouds within the hypothesis
599  * bounding box. */
601  /** \brief Pass through filter to crop the pointclouds within the hypothesis
602  * bounding box. */
604  /** \brief Pass through filter to crop the pointclouds within the hypothesis
605  * bounding box. */
607 
608  /** \brief A list of the pointers to pointclouds. */
609  std::vector<PointCloudInPtr> transed_reference_vector_;
610 
611  /** \brief Change detector used as a trigger to track. */
613 
614  /** \brief A flag to be true when change of pointclouds is detected. */
615  bool changed_;
616 
617  /** \brief A counter to skip change detection. */
618  unsigned int change_counter_;
619 
620  /** \brief Minimum points in a leaf when calling change detector. defaults
621  * to 10. */
623 
624  /** \brief The number of interval frame to run change detection. defaults
625  * to 10. */
627 
628  /** \brief Resolution of change detector. defaults to 0.01. */
630 
631  /** \brief The flag which will be true if using change detection. */
633 };
634 } // namespace tracking
635 } // namespace pcl
636 
637 // #include <pcl/tracking/impl/particle_filter.hpp>
638 #ifdef PCL_NO_PRECOMPILE
639 #include <pcl/tracking/impl/particle_filter.hpp>
640 #endif
pcl::tracking::ParticleFilterTracker::setOcclusionAngleThe
void setOcclusionAngleThe(const double occlusion_angle_thr)
Set the threshold of angle to be considered occlusion (default: pi/2).
Definition: particle_filter.h:198
pcl::tracking::ParticleFilterTracker::update
virtual void update()
Calculate the weighted mean of the particles and set it as the result.
Definition: particle_filter.hpp:375
pcl
Definition: convolution.h:46
pcl::tracking::ParticleFilterTracker::pass_z_
pcl::PassThrough< PointInT > pass_z_
Pass through filter to crop the pointclouds within the hypothesis bounding box.
Definition: particle_filter.h:606
pcl::tracking::ParticleFilterTracker::setResolutionOfChangeDetection
void setResolutionOfChangeDetection(double resolution)
Set the resolution of change detection.
Definition: particle_filter.h:370
pcl::tracking::ParticleFilterTracker::getUseNormal
bool getUseNormal()
Get the value of use_normal_.
Definition: particle_filter.h:302
pcl::tracking::ParticleFilterTracker
ParticleFilterTracker tracks the PointCloud which is given by setReferenceCloud within the measured P...
Definition: particle_filter.h:20
pcl::tracking::ParticleFilterTracker::getIterationNum
int getIterationNum() const
Get the number of iteration.
Definition: particle_filter.h:99
pcl::tracking::ParticleFilterTracker::setInitialNoiseCovariance
void setInitialNoiseCovariance(const std::vector< double > &initial_noise_covariance)
Set the covariance of the initial noise.
Definition: particle_filter.h:168
pcl::tracking::ParticleFilterTracker::resample_likelihood_thr_
double resample_likelihood_thr_
The threshold for the particles to be re-initialized.
Definition: particle_filter.h:573
pcl::PassThrough::setFilterFieldName
void setFilterFieldName(const std::string &field_name)
Provide the name of the field to be used for filtering data.
Definition: passthrough.h:111
pcl::tracking::ParticleFilterTracker::setMinPointsOfChangeDetection
void setMinPointsOfChangeDetection(unsigned int change_detector_filter)
Set the minimum amount of points required within leaf node to become serialized in change detection.
Definition: particle_filter.h:361
pcl::tracking::ParticleFilterTracker::ref_
PointCloudInConstPtr ref_
A pointer to reference point cloud.
Definition: particle_filter.h:551
pcl::tracking::ParticleFilterTracker::use_normal_
bool use_normal_
A flag to use normal or not.
Definition: particle_filter.h:590
pcl::tracking::PointCloudCoherence::ConstPtr
shared_ptr< const PointCloudCoherence< PointInT > > ConstPtr
Definition: coherence.h:62
pcl::tracking::ParticleFilterTracker::particles_
PointCloudStatePtr particles_
A pointer to the particles
Definition: particle_filter.h:554
pcl::tracking::ParticleFilterTracker::motion_ratio_
double motion_ratio_
Ratio of hypothesis to use motion model.
Definition: particle_filter.h:596
pcl::tracking::ParticleFilterTracker::computeTracking
void computeTracking() override
Track the pointcloud using particle filter method.
Definition: particle_filter.hpp:390
pcl::tracking::ParticleFilterTracker::setInitialNoiseMean
void setInitialNoiseMean(const std::vector< double > &initial_noise_mean)
Set the mean of the initial noise.
Definition: particle_filter.h:178
pcl::tracking::ParticleFilterTracker::setIterationNum
void setIterationNum(const int iteration_num)
Set the number of iteration.
Definition: particle_filter.h:92
pcl::tracking::ParticleFilterTracker::initial_noise_mean_
std::vector< double > initial_noise_mean_
The mean values of initial noise.
Definition: particle_filter.h:570
pcl::tracking::ParticleFilterTracker::pass_x_
pcl::PassThrough< PointInT > pass_x_
Pass through filter to crop the pointclouds within the hypothesis bounding box.
Definition: particle_filter.h:600
pcl::tracking::ParticleFilterTracker::CloudCoherenceConstPtr
typename CloudCoherence::ConstPtr CloudCoherenceConstPtr
Definition: particle_filter.h:50
pcl::tracking::ParticleFilterTracker::ParticleFilterTracker
ParticleFilterTracker()
Empty constructor.
Definition: particle_filter.h:53
pcl::tracking::ParticleFilterTracker::getMotionRatio
double getMotionRatio()
Get the motion ratio.
Definition: particle_filter.h:334
pcl::tracking::ParticleFilterTracker::setTrans
void setTrans(const Eigen::Affine3f &trans)
Set the transformation from the world coordinates to the frame of the particles.
Definition: particle_filter.h:220
pcl::tracking::ParticleFilterTracker::Ptr
shared_ptr< ParticleFilterTracker< PointInT, StateT > > Ptr
Definition: particle_filter.h:31
pcl::tracking::PointCoherence::Ptr
shared_ptr< PointCoherence< PointInT > > Ptr
Definition: coherence.h:17
pcl::tracking::ParticleFilterTracker::getIntervalOfChangeDetection
unsigned int getIntervalOfChangeDetection()
Get the number of interval frames to run change detection.
Definition: particle_filter.h:350
pcl::tracking::ParticleFilterTracker::computeTransformedPointCloud
void computeTransformedPointCloud(const StateT &hypothesis, std::vector< int > &indices, PointCloudIn &cloud)
Compute a reference pointcloud transformed to the pose that hypothesis represents.
Definition: particle_filter.hpp:275
pcl::PointCloud< PointInT >
pcl::tracking::ParticleFilterTracker::computeTransformedPointCloudWithNormal
void computeTransformedPointCloudWithNormal(const StateT &hypothesis, std::vector< int > &indices, PointCloudIn &cloud)
Compute a reference pointcloud transformed to the pose that hypothesis represents and calculate indic...
Definition: particle_filter.hpp:297
pcl::tracking::ParticleFilterTracker::change_counter_
unsigned int change_counter_
A counter to skip change detection.
Definition: particle_filter.h:618
pcl::octree::OctreePointCloudChangeDetector::Ptr
shared_ptr< OctreePointCloudChangeDetector< PointT, LeafContainerT, BranchContainerT > > Ptr
Definition: octree_pointcloud_changedetector.h:73
pcl::tracking::ParticleFilterTracker::resampleWithReplacement
void resampleWithReplacement()
Resampling the particle with replacement.
Definition: particle_filter.hpp:337
pcl::tracking::ParticleFilterTracker::getResult
StateT getResult() const override
Get an instance of the result of tracking.
Definition: particle_filter.h:239
pcl::tracking::ParticleFilterTracker::genAliasTable
void genAliasTable(std::vector< int > &a, std::vector< double > &q, const PointCloudStateConstPtr &particles)
Generate the tables for walker's alias method.
Definition: particle_filter.hpp:58
pcl::tracking::ParticleFilterTracker::getTrans
Eigen::Affine3f getTrans() const
Get the transformation from the world coordinates to the frame of the particles.
Definition: particle_filter.h:228
pcl::FilterIndices::setKeepOrganized
void setKeepOrganized(bool keep_organized)
Set whether the filtered points should be kept and set to the value given through setUserFilterValue ...
Definition: filter_indices.h:135
pcl::tracking::ParticleFilterTracker::toEigenMatrix
Eigen::Affine3f toEigenMatrix(const StateT &particle)
Convert a state to affine transformation from the world coordinates frame.
Definition: particle_filter.h:249
pcl::tracking::Tracker::tracker_name_
std::string tracker_name_
The tracker name.
Definition: tracker.h:90
pcl::tracking::ParticleFilterTracker::PointCloudIn
typename Tracker< PointInT, StateT >::PointCloudIn PointCloudIn
Definition: particle_filter.h:36
pcl::tracking::ParticleFilterTracker::getMinPointsOfChangeDetection
unsigned int getMinPointsOfChangeDetection()
Get the minimum amount of points required within leaf node to become serialized in change detection.
Definition: particle_filter.h:385
pcl::tracking::ParticleFilterTracker::ConstPtr
shared_ptr< const ParticleFilterTracker< PointInT, StateT > > ConstPtr
Definition: particle_filter.h:32
pcl::tracking::ParticleFilterTracker::setMinIndices
void setMinIndices(const int min_indices)
Set the minimum number of indices (default: 1).
Definition: particle_filter.h:209
pcl::tracking::ParticleFilterTracker::use_change_detector_
bool use_change_detector_
The flag which will be true if using change detection.
Definition: particle_filter.h:632
pcl::tracking::ParticleFilterTracker::getFitRatio
double getFitRatio() const
Get the adjustment ratio.
Definition: particle_filter.h:392
pcl::tracking::ParticleFilterTracker::getCloudCoherence
CloudCoherencePtr getCloudCoherence() const
Get the PointCloudCoherence to compute likelihood.
Definition: particle_filter.h:147
pcl::tracking::ParticleFilterTracker::coherence_
CloudCoherencePtr coherence_
A pointer to PointCloudCoherence.
Definition: particle_filter.h:557
pcl::tracking::ParticleFilterTracker::sampleWithReplacement
int sampleWithReplacement(const std::vector< int > &a, const std::vector< double > &q)
Implementation of "sample with replacement" using Walker's alias method.
Definition: particle_filter.hpp:42
pcl::tracking::ParticleFilterTracker::change_detector_resolution_
double change_detector_resolution_
Resolution of change detector.
Definition: particle_filter.h:629
pcl::tracking::ParticleFilterTracker::getResolutionOfChangeDetection
double getResolutionOfChangeDetection()
Get the resolution of change detection.
Definition: particle_filter.h:377
pcl::tracking::ParticleFilterTracker::setUseChangeDetector
void setUseChangeDetector(bool use_change_detector)
Set the value of use_change_detector_.
Definition: particle_filter.h:311
pcl::tracking::ParticleFilterTracker::occlusion_angle_thr_
double occlusion_angle_thr_
The threshold for the points to be considered as occluded.
Definition: particle_filter.h:576
pcl::tracking::ParticleFilterTracker::resetTracking
virtual void resetTracking()
Reset the particles to restart tracking.
Definition: particle_filter.h:399
pcl::tracking::ParticleFilterTracker::setParticleNum
void setParticleNum(const int particle_num)
Set the number of the particles.
Definition: particle_filter.h:108
pcl::tracking::ParticleFilterTracker::changed_
bool changed_
A flag to be true when change of pointclouds is detected.
Definition: particle_filter.h:615
pcl::tracking::ParticleFilterTracker::setStepNoiseCovariance
void setStepNoiseCovariance(const std::vector< double > &step_noise_covariance)
Set the covariance of step noise.
Definition: particle_filter.h:157
pcl::tracking::ParticleFilterTracker::getAlpha
double getAlpha()
Get the value of alpha.
Definition: particle_filter.h:286
pcl::tracking::ParticleFilterTracker::resample
virtual void resample()
Resampling phase of particle filter method.
Definition: particle_filter.hpp:330
M_PI
#define M_PI
Definition: pcl_macros.h:192
pcl::tracking::PointCoherence::ConstPtr
shared_ptr< const PointCoherence< PointInT > > ConstPtr
Definition: coherence.h:18
pcl::tracking::ParticleFilterTracker::transed_reference_vector_
std::vector< PointCloudInPtr > transed_reference_vector_
A list of the pointers to pointclouds.
Definition: particle_filter.h:609
pcl::tracking::ParticleFilterTracker::getUseChangeDetector
bool getUseChangeDetector()
Get the value of use_change_detector_.
Definition: particle_filter.h:318
pcl::tracking::ParticleFilterTracker::CloudCoherencePtr
typename CloudCoherence::Ptr CloudCoherencePtr
Definition: particle_filter.h:49
pcl::tracking::ParticleFilterTracker::min_indices_
int min_indices_
The minimum number of points which the hypothesis should have.
Definition: particle_filter.h:545
pcl::tracking::ParticleFilterTracker::particle_num_
int particle_num_
The number of the particles.
Definition: particle_filter.h:542
pcl::tracking::ParticleFilterTracker::change_detector_filter_
unsigned int change_detector_filter_
Minimum points in a leaf when calling change detector.
Definition: particle_filter.h:622
pcl::tracking::ParticleFilterTracker::testChangeDetection
bool testChangeDetection(const PointCloudInConstPtr &input)
Run change detection and return true if there is a change.
Definition: particle_filter.hpp:218
pcl::tracking::ParticleFilterTracker::cropInputPointCloud
void cropInputPointCloud(const PointCloudInConstPtr &cloud, PointCloudIn &output)
Crop the pointcloud by the bounding box calculated from hypothesis and the reference pointcloud.
Definition: particle_filter.hpp:162
pcl::tracking::ParticleFilterTracker::setCloudCoherence
void setCloudCoherence(const CloudCoherencePtr &coherence)
Set the PointCloudCoherence as likelihood.
Definition: particle_filter.h:140
pcl::tracking::ParticleFilterTracker::change_detector_
pcl::octree::OctreePointCloudChangeDetector< PointInT >::Ptr change_detector_
Change detector used as a trigger to track.
Definition: particle_filter.h:612
pcl::tracking::ParticleFilterTracker::resampleDeterministic
void resampleDeterministic()
Resampling the particle in deterministic way.
pcl::tracking::ParticleFilterTracker::fit_ratio_
double fit_ratio_
Adjustment of the particle filter.
Definition: particle_filter.h:548
pcl::PassThrough< PointInT >
pcl::tracking::ParticleFilterTracker::setAlpha
void setAlpha(double alpha)
Set the value of alpha.
Definition: particle_filter.h:279
pcl::tracking::ParticleFilterTracker::weight
virtual void weight()
Weighting phase of particle filter method.
Definition: particle_filter.hpp:232
pcl::tracking::ParticleFilterTracker::initParticles
void initParticles(bool reset)
Initialize the particles.
Definition: particle_filter.hpp:94
pcl::tracking::ParticleFilterTracker::PointCloudInConstPtr
typename PointCloudIn::ConstPtr PointCloudInConstPtr
Definition: particle_filter.h:38
pcl::tracking::ParticleFilterTracker::alpha_
double alpha_
The weight to be used in normalization of the weights of the particles.
Definition: particle_filter.h:580
pcl::tracking::ParticleFilterTracker::PointCloudState
typename Tracker< PointInT, StateT >::PointCloudState PointCloudState
Definition: particle_filter.h:40
pcl::tracking::ParticleFilterTracker::PointCloudInPtr
typename PointCloudIn::Ptr PointCloudInPtr
Definition: particle_filter.h:37
pcl::tracking::ParticleFilterTracker::CoherencePtr
typename Coherence::Ptr CoherencePtr
Definition: particle_filter.h:45
pcl::tracking::ParticleFilterTracker::motion_
StateT motion_
Difference between the result in t and t-1.
Definition: particle_filter.h:593
pcl::tracking::ParticleFilterTracker::change_detector_interval_
unsigned int change_detector_interval_
The number of interval frame to run change detection.
Definition: particle_filter.h:626
pcl::tracking::ParticleFilterTracker::normalizeWeight
virtual void normalizeWeight()
Normalize the weights of all the particels.
Definition: particle_filter.hpp:117
pcl::tracking::PointCloudCoherence
PointCloudCoherence is a base class to compute coherence between the two PointClouds.
Definition: coherence.h:59
pcl::tracking::PointCoherence
PointCoherence is a base class to compute coherence between the two points.
Definition: coherence.h:15
pcl::tracking::ParticleFilterTracker::initial_noise_covariance_
std::vector< double > initial_noise_covariance_
The diagonal elements of covariance matrix of the initial noise.
Definition: particle_filter.h:567
pcl::tracking::ParticleFilterTracker::getReferenceCloud
PointCloudInConstPtr const getReferenceCloud()
Get a pointer to a reference dataset to be tracked.
Definition: particle_filter.h:131
pcl::tracking::ParticleFilterTracker::getParticleNum
int getParticleNum() const
Get the number of the particles.
Definition: particle_filter.h:115
pcl::tracking::ParticleFilterTracker::setResampleLikelihoodThr
void setResampleLikelihoodThr(const double resample_likelihood_thr)
Set the threshold to re-initialize the particles.
Definition: particle_filter.h:187
pcl::tracking::ParticleFilterTracker::CoherenceConstPtr
typename Coherence::ConstPtr CoherenceConstPtr
Definition: particle_filter.h:46
pcl::tracking::ParticleFilterTracker::setMotionRatio
void setMotionRatio(double motion_ratio)
Set the motion ratio.
Definition: particle_filter.h:327
pcl::tracking::ParticleFilterTracker::computeTransformedPointCloudWithoutNormal
void computeTransformedPointCloudWithoutNormal(const StateT &hypothesis, PointCloudIn &cloud)
Compute a reference pointcloud transformed to the pose that hypothesis represents and calculate indic...
Definition: particle_filter.hpp:286
pcl::tracking::ParticleFilterTracker::iteration_num_
int iteration_num_
The number of iteration of particlefilter.
Definition: particle_filter.h:539
pcl::tracking::PointCloudCoherence::Ptr
shared_ptr< PointCloudCoherence< PointInT > > Ptr
Definition: coherence.h:61
pcl::tracking::ParticleFilterTracker::normalizeParticleWeight
double normalizeParticleWeight(double w, double w_min, double w_max)
Normalize the weight of a particle using .
Definition: particle_filter.h:270
pcl::tracking::ParticleFilterTracker::getParticles
PointCloudStatePtr getParticles() const
Get a pointer to a pointcloud of the particles.
Definition: particle_filter.h:256
pcl::tracking::ParticleFilterTracker::trans_
Eigen::Affine3f trans_
An affine transformation from the world coordinates frame to the origin of the particles.
Definition: particle_filter.h:587
pcl::tracking::ParticleFilterTracker::calcBoundingBox
void calcBoundingBox(double &x_min, double &x_max, double &y_min, double &y_max, double &z_min, double &z_max)
Compute the parameters for the bounding box of hypothesis pointclouds.
Definition: particle_filter.hpp:187
pcl::tracking::ParticleFilterTracker::PointCloudStatePtr
typename PointCloudState::Ptr PointCloudStatePtr
Definition: particle_filter.h:41
pcl::tracking::ParticleFilterTracker::PointCloudStateConstPtr
typename PointCloudState::ConstPtr PointCloudStateConstPtr
Definition: particle_filter.h:42
pcl::tracking::ParticleFilterTracker::representative_state_
StateT representative_state_
The result of tracking.
Definition: particle_filter.h:583
pcl::tracking::ParticleFilterTracker::setIntervalOfChangeDetection
void setIntervalOfChangeDetection(unsigned int change_detector_interval)
Set the number of interval frames to run change detection.
Definition: particle_filter.h:343
pcl::tracking::ParticleFilterTracker::step_noise_covariance_
std::vector< double > step_noise_covariance_
The diagonal elements of covariance matrix of the step noise.
Definition: particle_filter.h:562
pcl::tracking::Tracker
Tracker represents the base tracker class.
Definition: tracker.h:55
memory.h
Defines functions, macros and traits for allocating and using memory.
pcl::tracking::ParticleFilterTracker::setUseNormal
void setUseNormal(bool use_normal)
Set the value of use_normal_.
Definition: particle_filter.h:295
pcl::tracking::ParticleFilterTracker::initCompute
bool initCompute() override
This method should get called before starting the actua computation.
Definition: particle_filter.hpp:14
pcl::tracking::ParticleFilterTracker::setReferenceCloud
void setReferenceCloud(const PointCloudInConstPtr &ref)
Set a pointer to a reference dataset to be tracked.
Definition: particle_filter.h:124
pcl::tracking::ParticleFilterTracker::pass_y_
pcl::PassThrough< PointInT > pass_y_
Pass through filter to crop the pointclouds within the hypothesis bounding box.
Definition: particle_filter.h:603