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