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