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