Point Cloud Library (PCL) 1.13.0
kld_adaptive_particle_filter_omp.h
1#pragma once
2
3#include <pcl/tracking/coherence.h>
4#include <pcl/tracking/kld_adaptive_particle_filter.h>
5#include <pcl/tracking/tracking.h>
6
7namespace pcl {
8namespace tracking {
9/** \brief @b KLDAdaptiveParticleFilterOMPTracker tracks the PointCloud which is given
10 * by setReferenceCloud within the measured PointCloud using particle filter method. The
11 * number of the particles changes adaptively based on KLD sampling [D. Fox, NIPS-01],
12 * [D.Fox, IJRR03]. and the computation of the weights of the particles is parallelized
13 * using OpenMP.
14 * \author Ryohei Ueda
15 * \ingroup tracking
16 */
17template <typename PointInT, typename StateT>
19: public KLDAdaptiveParticleFilterTracker<PointInT, StateT> {
20public:
21 using Tracker<PointInT, StateT>::tracker_name_;
22 using Tracker<PointInT, StateT>::search_;
23 using Tracker<PointInT, StateT>::input_;
24 using Tracker<PointInT, StateT>::indices_;
25 using Tracker<PointInT, StateT>::getClassName;
31 using KLDAdaptiveParticleFilterTracker<PointInT, StateT>::pass_x_;
32 using KLDAdaptiveParticleFilterTracker<PointInT, StateT>::pass_y_;
33 using KLDAdaptiveParticleFilterTracker<PointInT, StateT>::pass_z_;
34 using KLDAdaptiveParticleFilterTracker<PointInT, StateT>::alpha_;
35 using KLDAdaptiveParticleFilterTracker<PointInT, StateT>::changed_;
41 // using KLDAdaptiveParticleFilterTracker<PointInT, StateT>::calcLikelihood;
45
47
48 using Ptr = shared_ptr<KLDAdaptiveParticleFilterOMPTracker<PointInT, StateT>>;
49 using ConstPtr =
50 shared_ptr<const KLDAdaptiveParticleFilterOMPTracker<PointInT, StateT>>;
51
53 using PointCloudInPtr = typename PointCloudIn::Ptr;
54 using PointCloudInConstPtr = typename PointCloudIn::ConstPtr;
55
57 using PointCloudStatePtr = typename PointCloudState::Ptr;
58 using PointCloudStateConstPtr = typename PointCloudState::ConstPtr;
59
61 using CoherencePtr = typename Coherence::Ptr;
63
67
68 /** \brief Initialize the scheduler and set the number of threads to use.
69 * \param nr_threads the number of hardware threads to use (0 sets the value
70 * back to automatic)
71 */
72 KLDAdaptiveParticleFilterOMPTracker(unsigned int nr_threads = 0)
73 : KLDAdaptiveParticleFilterTracker<PointInT, StateT>()
74 {
75 tracker_name_ = "KLDAdaptiveParticleFilterOMPTracker";
76
77 setNumberOfThreads(nr_threads);
78 }
79
80 /** \brief Initialize the scheduler and set the number of threads to use.
81 * \param nr_threads the number of hardware threads to use (0 sets the value back to
82 * automatic)
83 */
84 void
85 setNumberOfThreads(unsigned int nr_threads = 0);
86
87protected:
88 /** \brief The number of threads the scheduler should use. */
89 unsigned int threads_;
90
91 /** \brief weighting phase of particle filter method. calculate the likelihood of all
92 * of the particles and set the weights.
93 */
94 void
95 weight() override;
96};
97} // namespace tracking
98} // namespace pcl
99
100//#include <pcl/tracking/impl/particle_filter_omp.hpp>
101#ifdef PCL_NO_PRECOMPILE
102#include <pcl/tracking/impl/kld_adaptive_particle_filter_omp.hpp>
103#endif
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
KLDAdaptiveParticleFilterOMPTracker tracks the PointCloud which is given by setReferenceCloud within ...
unsigned int threads_
The number of threads the scheduler should use.
KLDAdaptiveParticleFilterOMPTracker(unsigned int nr_threads=0)
Initialize the scheduler and set the number of threads to use.
void setNumberOfThreads(unsigned int nr_threads=0)
Initialize the scheduler and set the number of threads to use.
void weight() override
weighting phase of particle filter method.
KLDAdaptiveParticleFilterTracker tracks the PointCloud which is given by setReferenceCloud within the...
shared_ptr< KLDAdaptiveParticleFilterTracker< PointInT, StateT > > Ptr
shared_ptr< const KLDAdaptiveParticleFilterTracker< PointInT, StateT > > ConstPtr
typename PointCloudState::ConstPtr PointCloudStateConstPtr
typename Tracker< PointInT, StateT >::PointCloudIn PointCloudIn
typename Tracker< PointInT, StateT >::PointCloudState PointCloudState
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.
CloudCoherencePtr coherence_
A pointer to PointCloudCoherence.
bool changed_
A flag to be true when change of pointclouds is detected.
unsigned int change_detector_filter_
Minimum points in a leaf when calling change detector.
pcl::octree::OctreePointCloudChangeDetector< PointInT >::Ptr change_detector_
Change detector used as a trigger to track.
pcl::PassThrough< PointInT > pass_z_
Pass through filter to crop the pointclouds within the hypothesis bounding box.
std::vector< PointCloudInPtr > transed_reference_vector_
A list of the pointers to pointclouds.
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 alpha_
The weight to be used in normalization of the weights of the particles.
bool use_normal_
A flag to use normal or not.
PointCloudStatePtr particles_
A pointer to the particles
unsigned int change_detector_interval_
The number of interval frame to run change detection.
double normalizeParticleWeight(double w, double w_min, double w_max)
Normalize the weight of a particle using .
int particle_num_
The number of the particles.
pcl::PassThrough< PointInT > pass_x_
Pass through filter to crop the pointclouds within the hypothesis bounding box.
bool use_change_detector_
The flag which will be true if using change detection.
virtual void normalizeWeight()
Normalize the weights of all the particels.
PointCloudCoherence is a base class to compute coherence between the two PointClouds.
Definition: coherence.h:59
shared_ptr< PointCloudCoherence< PointInT > > Ptr
Definition: coherence.h:61
shared_ptr< const PointCloudCoherence< PointInT > > ConstPtr
Definition: coherence.h:62
PointCoherence is a base class to compute coherence between the two points.
Definition: coherence.h:15
shared_ptr< const PointCoherence< PointInT > > ConstPtr
Definition: coherence.h:18
shared_ptr< PointCoherence< PointInT > > Ptr
Definition: coherence.h:17
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