Point Cloud Library (PCL) 1.12.0
Loading...
Searching...
No Matches
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
11namespace pcl {
12namespace 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 */
18template <typename PointInT, typename StateT>
19class ParticleFilterTracker : public Tracker<PointInT, StateT> {
20protected:
21 using Tracker<PointInT, StateT>::deinitCompute;
22
23public:
24 using Tracker<PointInT, StateT>::tracker_name_;
25 using Tracker<PointInT, StateT>::search_;
26 using Tracker<PointInT, StateT>::input_;
27 using Tracker<PointInT, StateT>::indices_;
28 using Tracker<PointInT, StateT>::getClassName;
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;
45 using CoherenceConstPtr = typename Coherence::ConstPtr;
46
48 using CloudCoherencePtr = typename CloudCoherence::Ptr;
49 using CloudCoherenceConstPtr = typename CloudCoherence::ConstPtr;
50
51 /** \brief Empty constructor. */
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 {
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
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
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
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
387
388 /** \brief Get the minimum amount of points required within leaf node to
389 * become serialized in change detection. */
390 inline unsigned int
395
396 /** \brief Get the adjustment ratio. */
397 inline double
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
411protected:
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 actua 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
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. */
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. */
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 ...
PointCloudConstPtr input_
The input point cloud dataset.
Definition pcl_base.h:147
IndicesPtr indices_
A pointer to the vector of point indices to use.
Definition pcl_base.h:150
bool deinitCompute()
This method should get called after finishing the actual computation.
Definition pcl_base.hpp:174
PassThrough passes points in a cloud based on constraints for one particular field of the point type.
Definition passthrough.h:82
void setFilterFieldName(const std::string &field_name)
Provide the name of the field to be used for filtering data.
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.
PointCoherence< PointInT > Coherence
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.
Tracker< PointInT, StateT > BaseClass
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.
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.
PointCloudCoherence< PointInT > CloudCoherence
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 actua 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
PointCoherence is a base class to compute coherence between the two points.
Definition coherence.h:15
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
SearchPtr search_
A pointer to the spatial search object.
Definition tracker.h:93
std::string tracker_name_
The tracker name.
Definition tracker.h:90
pcl::PointCloud< StateT > PointCloudState
Definition tracker.h:74
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