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