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