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
CloudCoherencePtr getCloudCoherence() const
Get the PointCloudCoherence to compute likelihood.
void setUseNormal(bool use_normal)
Set the value of use_normal_.
virtual void weight()
Weighting phase of particle filter method.
typename PointCloudState::Ptr PointCloudStatePtr
void setOcclusionAngleThe(const double occlusion_angle_thr)
Set the threshold of angle to be considered occlusion (default: pi/2).
void setKeepOrganized(bool keep_organized)
Set whether the filtered points should be kept and set to the value given through setUserFilterValue ...
Defines functions, macros and traits for allocating and using memory.
virtual void resetTracking()
Reset the particles to restart tracking.
void setTrans(const Eigen::Affine3f &trans)
Set the transformation from the world coordinates to the frame of the particles.
void setParticleNum(const int particle_num)
Set the number of the particles.
void setStepNoiseCovariance(const std::vector< double > &step_noise_covariance)
Set the covariance of step noise.
unsigned int getIntervalOfChangeDetection()
Get the number of interval frames to run change detection.
double getAlpha()
Get the value of alpha.
typename Tracker< PointInT, StateT >::PointCloudState PointCloudState
ParticleFilterTracker()
Empty constructor.
std::vector< PointCloudInPtr > transed_reference_vector_
A list of the pointers to pointclouds.
typename CloudCoherence::Ptr CloudCoherencePtr
void setResolutionOfChangeDetection(double resolution)
Set the resolution of change detection.
virtual void normalizeWeight()
Normalize the weights of all the particels.
std::vector< double > initial_noise_covariance_
The diagonal elements of covariance matrix of the initial noise.
void setFilterFieldName(const std::string &field_name)
Provide the name of the field to be used for filtering data.
Definition: passthrough.h:111
void setInitialNoiseCovariance(const std::vector< double > &initial_noise_covariance)
Set the covariance of the initial noise.
typename Coherence::ConstPtr CoherenceConstPtr
void computeTransformedPointCloudWithoutNormal(const StateT &hypothesis, PointCloudIn &cloud)
Compute a reference pointcloud transformed to the pose that hypothesis represents and calculate indic...
shared_ptr< const ParticleFilterTracker< PointInT, StateT > > ConstPtr
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.
unsigned int change_detector_filter_
Minimum points in a leaf when calling change detector.
int sampleWithReplacement(const std::vector< int > &a, const std::vector< double > &q)
Implementation of "sample with replacement" using Walker&#39;s alias method.
typename PointCloudIn::ConstPtr PointCloudInConstPtr
double getResolutionOfChangeDetection()
Get the resolution of change detection.
StateT motion_
Difference between the result in t and t-1.
double alpha_
The weight to be used in normalization of the weights of the particles.
PointCloudInConstPtr ref_
A pointer to reference point cloud.
unsigned int change_detector_interval_
The number of interval frame to run change detection.
typename Coherence::Ptr CoherencePtr
void resampleDeterministic()
Resampling the particle in deterministic way.
StateT representative_state_
The result of tracking.
void setIntervalOfChangeDetection(unsigned int change_detector_interval)
Set the number of interval frames to run change detection.
ParticleFilterTracker tracks the PointCloud which is given by setReferenceCloud within the measured P...
void genAliasTable(std::vector< int > &a, std::vector< double > &q, const PointCloudStateConstPtr &particles)
Generate the tables for walker&#39;s alias method.
pcl::PassThrough< PointInT > pass_y_
Pass through filter to crop the pointclouds within the hypothesis bounding box.
bool testChangeDetection(const PointCloudInConstPtr &input)
Run change detection and return true if there is a change.
int getParticleNum() const
Get the number of the particles.
#define M_PI
Definition: pcl_macros.h:192
shared_ptr< PointCoherence< PointInT > > Ptr
Definition: coherence.h:17
virtual void resample()
Resampling phase of particle filter method.
shared_ptr< const PointCoherence< PointInT > > ConstPtr
Definition: coherence.h:18
void setCloudCoherence(const CloudCoherencePtr &coherence)
Set the PointCloudCoherence as likelihood.
void setMotionRatio(double motion_ratio)
Set the motion ratio.
bool use_normal_
A flag to use normal or not.
pcl::PassThrough< PointInT > pass_z_
Pass through filter to crop the pointclouds within the hypothesis bounding box.
bool getUseNormal()
Get the value of use_normal_.
StateT getResult() const override
Get an instance of the result of tracking.
Eigen::Affine3f getTrans() const
Get the transformation from the world coordinates to the frame of the particles.
double resample_likelihood_thr_
The threshold for the particles to be re-initialized.
std::vector< double > initial_noise_mean_
The mean values of initial noise.
double normalizeParticleWeight(double w, double w_min, double w_max)
Normalize the weight of a particle using .
double motion_ratio_
Ratio of hypothesis to use motion model.
typename Tracker< PointInT, StateT >::PointCloudIn PointCloudIn
shared_ptr< OctreePointCloudChangeDetector< PointT, LeafContainerT, BranchContainerT > > Ptr
Eigen::Affine3f toEigenMatrix(const StateT &particle)
Convert a state to affine transformation from the world coordinates frame.
void setIterationNum(const int iteration_num)
Set the number of iteration.
bool initCompute() override
This method should get called before starting the actua computation.
PointCoherence is a base class to compute coherence between the two points.
Definition: coherence.h:15
void setResampleLikelihoodThr(const double resample_likelihood_thr)
Set the threshold to re-initialize the particles.
virtual void update()
Calculate the weighted mean of the particles and set it as the result.
double getMotionRatio()
Get the motion ratio.
void setUseChangeDetector(bool use_change_detector)
Set the value of use_change_detector_.
double occlusion_angle_thr_
The threshold for the points to be considered as occluded.
shared_ptr< ParticleFilterTracker< PointInT, StateT > > Ptr
void computeTransformedPointCloud(const StateT &hypothesis, std::vector< int > &indices, PointCloudIn &cloud)
Compute a reference pointcloud transformed to the pose that hypothesis represents.
int getIterationNum() const
Get the number of iteration.
PointCloudInConstPtr const getReferenceCloud()
Get a pointer to a reference dataset to be tracked.
Eigen::Affine3f trans_
An affine transformation from the world coordinates frame to the origin of the particles.
shared_ptr< PointCloudCoherence< PointInT > > Ptr
Definition: coherence.h:61
unsigned int getMinPointsOfChangeDetection()
Get the minimum amount of points required within leaf node to become serialized in change detection...
void setInitialNoiseMean(const std::vector< double > &initial_noise_mean)
Set the mean of the initial noise.
PointCloudCoherence is a base class to compute coherence between the two PointClouds.
Definition: coherence.h:59
CloudCoherencePtr coherence_
A pointer to PointCloudCoherence.
void setReferenceCloud(const PointCloudInConstPtr &ref)
Set a pointer to a reference dataset to be tracked.
void cropInputPointCloud(const PointCloudInConstPtr &cloud, PointCloudIn &output)
Crop the pointcloud by the bounding box calculated from hypothesis and the reference pointcloud...
pcl::PassThrough< PointInT > pass_x_
Pass through filter to crop the pointclouds within the hypothesis bounding box.
void setMinPointsOfChangeDetection(unsigned int change_detector_filter)
Set the minimum amount of points required within leaf node to become serialized in change detection...
int iteration_num_
The number of iteration of particlefilter.
typename CloudCoherence::ConstPtr CloudCoherenceConstPtr
typename PointCloudIn::Ptr PointCloudInPtr
void computeTracking() override
Track the pointcloud using particle filter method.
std::vector< double > step_noise_covariance_
The diagonal elements of covariance matrix of the step noise.
typename PointCloudState::ConstPtr PointCloudStateConstPtr
int particle_num_
The number of the particles.
Tracker represents the base tracker class.
Definition: tracker.h:55
double getFitRatio() const
Get the adjustment ratio.
bool use_change_detector_
The flag which will be true if using change detection.
bool changed_
A flag to be true when change of pointclouds is detected.
void setAlpha(double alpha)
Set the value of alpha.
double fit_ratio_
Adjustment of the particle filter.
void initParticles(bool reset)
Initialize the particles.
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.
double change_detector_resolution_
Resolution of change detector.
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...
void resampleWithReplacement()
Resampling the particle with replacement.
int min_indices_
The minimum number of points which the hypothesis should have.
bool getUseChangeDetector()
Get the value of use_change_detector_.
shared_ptr< const PointCloudCoherence< PointInT > > ConstPtr
Definition: coherence.h:62
unsigned int change_counter_
A counter to skip change detection.
PointCloudStatePtr particles_
A pointer to the particles.
std::string tracker_name_
The tracker name.
Definition: tracker.h:90
void setMinIndices(const int min_indices)
Set the minimum number of indices (default: 1).