Point Cloud Library (PCL)  1.9.1-dev
particle_filter_omp.hpp
1 #ifndef PCL_TRACKING_IMPL_PARTICLE_OMP_FILTER_H_
2 #define PCL_TRACKING_IMPL_PARTICLE_OMP_FILTER_H_
3 
4 #include <pcl/tracking/particle_filter_omp.h>
5 
6 //////////////////////////////////////////////////////////////////////////////////////////////
7 template <typename PointInT, typename StateT> void
9 {
10  if (nr_threads == 0)
11 #ifdef _OPENMP
12  threads_ = omp_get_num_procs();
13 #else
14  threads_ = 1;
15 #endif
16  else
17  threads_ = nr_threads;
18 }
19 
20 //////////////////////////////////////////////////////////////////////////////////////////////
21 template <typename PointInT, typename StateT> void
23 {
24  if (!use_normal_)
25  {
26 #ifdef _OPENMP
27 #pragma omp parallel for num_threads(threads_)
28 #endif
29  for (int i = 0; i < particle_num_; i++)
30  this->computeTransformedPointCloudWithoutNormal (particles_->points[i], *transed_reference_vector_[i]);
31 
32  PointCloudInPtr coherence_input (new PointCloudIn);
33  this->cropInputPointCloud (input_, *coherence_input);
34  if (change_counter_ == 0)
35  {
36  // test change detector
37  if (!use_change_detector_ || this->testChangeDetection (coherence_input))
38  {
39  changed_ = true;
40  change_counter_ = change_detector_interval_;
41  coherence_->setTargetCloud (coherence_input);
42  coherence_->initCompute ();
43 #ifdef _OPENMP
44 #pragma omp parallel for num_threads(threads_)
45 #endif
46  for (int i = 0; i < particle_num_; i++)
47  {
48  IndicesPtr indices; // dummy
49  coherence_->compute (transed_reference_vector_[i], indices, particles_->points[i].weight);
50  }
51  }
52  else
53  changed_ = false;
54  }
55  else
56  {
57  --change_counter_;
58  coherence_->setTargetCloud (coherence_input);
59  coherence_->initCompute ();
60 #ifdef _OPENMP
61 #pragma omp parallel for num_threads(threads_)
62 #endif
63  for (int i = 0; i < particle_num_; i++)
64  {
65  IndicesPtr indices; // dummy
66  coherence_->compute (transed_reference_vector_[i], indices, particles_->points[i].weight);
67  }
68  }
69  }
70  else
71  {
72  std::vector<IndicesPtr> indices_list (particle_num_);
73  for (int i = 0; i < particle_num_; i++)
74  {
75  indices_list[i] = IndicesPtr (new std::vector<int>);
76  }
77 #ifdef _OPENMP
78 #pragma omp parallel for num_threads(threads_)
79 #endif
80  for (int i = 0; i < particle_num_; i++)
81  {
82  this->computeTransformedPointCloudWithNormal (particles_->points[i], *indices_list[i], *transed_reference_vector_[i]);
83  }
84 
85  PointCloudInPtr coherence_input (new PointCloudIn);
86  this->cropInputPointCloud (input_, *coherence_input);
87 
88  coherence_->setTargetCloud (coherence_input);
89  coherence_->initCompute ();
90 #ifdef _OPENMP
91 #pragma omp parallel for num_threads(threads_)
92 #endif
93  for (int i = 0; i < particle_num_; i++)
94  {
95  coherence_->compute (transed_reference_vector_[i], indices_list[i], particles_->points[i].weight);
96  }
97  }
98 
99  normalizeWeight ();
100 }
101 
102 #define PCL_INSTANTIATE_ParticleFilterOMPTracker(T,ST) template class PCL_EXPORTS pcl::tracking::ParticleFilterOMPTracker<T,ST>;
103 
104 #endif
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.
boost::shared_ptr< Indices > IndicesPtr
Definition: pcl_base.h:61
typename Tracker< PointInT, StateT >::PointCloudIn PointCloudIn
typename PointCloudIn::Ptr PointCloudInPtr