Point Cloud Library (PCL)  1.7.1
kld_adaptive_particle_filter_omp.h
1 #ifndef PCL_TRACKING_KLD_ADAPTIVE_PARTICLE_FILTER_OMP_H_
2 #define PCL_TRACKING_KLD_ADAPTIVE_PARTICLE_FILTER_OMP_H_
3 
4 #include <pcl/tracking/tracking.h>
5 #include <pcl/tracking/kld_adaptive_particle_filter.h>
6 #include <pcl/tracking/coherence.h>
7 
8 namespace pcl
9 {
10  namespace tracking
11  {
12  /** \brief @b KLDAdaptiveParticleFilterOMPTracker tracks the PointCloud which is given by
13  setReferenceCloud within the measured PointCloud using particle filter method.
14  The number of the particles changes adaptively based on KLD sampling [D. Fox, NIPS-01], [D.Fox, IJRR03].
15  and the computation of the weights of the particles is parallelized using OpenMP.
16  * \author Ryohei Ueda
17  * \ingroup tracking
18  */
19  template <typename PointInT, typename StateT>
20  class KLDAdaptiveParticleFilterOMPTracker: public KLDAdaptiveParticleFilterTracker<PointInT, StateT>
21  {
22  public:
23  using Tracker<PointInT, StateT>::tracker_name_;
24  using Tracker<PointInT, StateT>::search_;
25  using Tracker<PointInT, StateT>::input_;
26  using Tracker<PointInT, StateT>::indices_;
27  using Tracker<PointInT, StateT>::getClassName;
28  using KLDAdaptiveParticleFilterTracker<PointInT, StateT>::particles_;
29  using KLDAdaptiveParticleFilterTracker<PointInT, StateT>::change_detector_;
30  using KLDAdaptiveParticleFilterTracker<PointInT, StateT>::change_counter_;
31  using KLDAdaptiveParticleFilterTracker<PointInT, StateT>::change_detector_interval_;
32  using KLDAdaptiveParticleFilterTracker<PointInT, StateT>::use_change_detector_;
33  using KLDAdaptiveParticleFilterTracker<PointInT, StateT>::pass_x_;
34  using KLDAdaptiveParticleFilterTracker<PointInT, StateT>::pass_y_;
35  using KLDAdaptiveParticleFilterTracker<PointInT, StateT>::pass_z_;
36  using KLDAdaptiveParticleFilterTracker<PointInT, StateT>::alpha_;
37  using KLDAdaptiveParticleFilterTracker<PointInT, StateT>::changed_;
38  using KLDAdaptiveParticleFilterTracker<PointInT, StateT>::coherence_;
39  using KLDAdaptiveParticleFilterTracker<PointInT, StateT>::use_normal_;
40  using KLDAdaptiveParticleFilterTracker<PointInT, StateT>::particle_num_;
41  using KLDAdaptiveParticleFilterTracker<PointInT, StateT>::change_detector_filter_;
42  using KLDAdaptiveParticleFilterTracker<PointInT, StateT>::transed_reference_vector_;
43  //using KLDAdaptiveParticleFilterTracker<PointInT, StateT>::calcLikelihood;
44  using KLDAdaptiveParticleFilterTracker<PointInT, StateT>::normalizeWeight;
45  using KLDAdaptiveParticleFilterTracker<PointInT, StateT>::normalizeParticleWeight;
46  using KLDAdaptiveParticleFilterTracker<PointInT, StateT>::calcBoundingBox;
47 
48  typedef Tracker<PointInT, StateT> BaseClass;
49 
50  typedef typename Tracker<PointInT, StateT>::PointCloudIn PointCloudIn;
51  typedef typename PointCloudIn::Ptr PointCloudInPtr;
52  typedef typename PointCloudIn::ConstPtr PointCloudInConstPtr;
53 
54  typedef typename Tracker<PointInT, StateT>::PointCloudState PointCloudState;
55  typedef typename PointCloudState::Ptr PointCloudStatePtr;
56  typedef typename PointCloudState::ConstPtr PointCloudStateConstPtr;
57 
58  typedef PointCoherence<PointInT> Coherence;
59  typedef boost::shared_ptr< Coherence > CoherencePtr;
60  typedef boost::shared_ptr< const Coherence > CoherenceConstPtr;
61 
62  typedef PointCloudCoherence<PointInT> CloudCoherence;
63  typedef boost::shared_ptr< CloudCoherence > CloudCoherencePtr;
64  typedef boost::shared_ptr< const CloudCoherence > CloudCoherenceConstPtr;
65 
66  /** \brief Initialize the scheduler and set the number of threads to use.
67  * \param nr_threads the number of hardware threads to use (0 sets the value back to automatic)
68  */
69  KLDAdaptiveParticleFilterOMPTracker (unsigned int nr_threads = 0)
70  : KLDAdaptiveParticleFilterTracker<PointInT, StateT> ()
71  , threads_ (nr_threads)
72  {
73  tracker_name_ = "KLDAdaptiveParticleFilterOMPTracker";
74  }
75 
76  /** \brief Initialize the scheduler and set the number of threads to use.
77  * \param nr_threads the number of hardware threads to use (0 sets the value back to automatic)
78  */
79  inline void
80  setNumberOfThreads (unsigned int nr_threads = 0) { threads_ = nr_threads; }
81 
82  protected:
83  /** \brief The number of threads the scheduler should use. */
84  unsigned int threads_;
85 
86  /** \brief weighting phase of particle filter method.
87  calculate the likelihood of all of the particles and set the weights.
88  */
89  virtual void weight ();
90 
91  };
92  }
93 }
94 
95 //#include <pcl/tracking/impl/particle_filter_omp.hpp>
96 #ifdef PCL_NO_PRECOMPILE
97 #include <pcl/tracking/impl/kld_adaptive_particle_filter_omp.hpp>
98 #endif
99 
100 #endif