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