Point Cloud Library (PCL)  1.10.0-dev
particle_filter.h
1 #pragma once
2 
3 #include <boost/shared_ptr.hpp>
4 
5 #include <pcl/tracking/tracking.h>
6 #include <pcl/tracking/tracker.h>
7 #include <pcl/tracking/coherence.h>
8 #include <pcl/filters/passthrough.h>
9 #include <pcl/octree/octree_pointcloud_changedetector.h>
10 
11 #include <Eigen/Dense>
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 
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
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 ...
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.
This file defines compatibility wrappers for low level I/O functions.
Definition: convolution.h:45
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:110
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.
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_.
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 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.
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: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 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:56
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.
boost::shared_ptr< T > shared_ptr
Alias for boost::shared_ptr.
Definition: pcl_macros.h:90
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:92
void setMinIndices(const int min_indices)
Set the minimum number of indices (default: 1).