Point Cloud Library (PCL)  1.14.1-dev
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 namespace pcl {
7 namespace tracking {
8 //////////////////////////////////////////////////////////////////////////////////////////////
9 template <typename PointInT, typename StateT>
10 void
12  unsigned int nr_threads)
13 {
14  if (nr_threads == 0)
15 #ifdef _OPENMP
16  threads_ = omp_get_num_procs();
17 #else
18  threads_ = 1;
19 #endif
20  else
21  threads_ = nr_threads;
22 }
23 
24 //////////////////////////////////////////////////////////////////////////////////////////////
25 template <typename PointInT, typename StateT>
26 void
28 {
29  if (!use_normal_) {
30  // clang-format off
31 #pragma omp parallel for \
32  default(none) \
33  num_threads(threads_)
34  // clang-format on
35  for (int i = 0; i < particle_num_; i++)
36  this->computeTransformedPointCloudWithoutNormal((*particles_)[i],
37  *transed_reference_vector_[i]);
38 
39  PointCloudInPtr coherence_input(new PointCloudIn);
40  this->cropInputPointCloud(input_, *coherence_input);
41  if (change_counter_ == 0) {
42  // test change detector
43  if (!use_change_detector_ || this->testChangeDetection(coherence_input)) {
44  changed_ = true;
45  change_counter_ = change_detector_interval_;
46  coherence_->setTargetCloud(coherence_input);
47  coherence_->initCompute();
48  // clang-format off
49 #pragma omp parallel for \
50  default(none) \
51  num_threads(threads_)
52  // clang-format on
53  for (int i = 0; i < particle_num_; i++) {
54  IndicesPtr indices;
55  coherence_->compute(
56  transed_reference_vector_[i], indices, (*particles_)[i].weight);
57  }
58  }
59  else
60  changed_ = false;
61  }
62  else {
63  --change_counter_;
64  coherence_->setTargetCloud(coherence_input);
65  coherence_->initCompute();
66  // clang-format off
67 #pragma omp parallel for \
68  default(none) \
69  num_threads(threads_)
70  // clang-format on
71  for (int i = 0; i < particle_num_; i++) {
72  IndicesPtr indices;
73  coherence_->compute(
74  transed_reference_vector_[i], indices, (*particles_)[i].weight);
75  }
76  }
77  }
78  else {
79  std::vector<IndicesPtr> indices_list(particle_num_);
80  for (int i = 0; i < particle_num_; i++) {
81  indices_list[i] = IndicesPtr(new pcl::Indices);
82  }
83  // clang-format off
84 #pragma omp parallel for \
85  default(none) \
86  shared(indices_list) \
87  num_threads(threads_)
88  // clang-format on
89  for (int i = 0; i < particle_num_; i++) {
90  this->computeTransformedPointCloudWithNormal(
91  (*particles_)[i], *indices_list[i], *transed_reference_vector_[i]);
92  }
93 
94  PointCloudInPtr coherence_input(new PointCloudIn);
95  this->cropInputPointCloud(input_, *coherence_input);
96 
97  coherence_->setTargetCloud(coherence_input);
98  coherence_->initCompute();
99  // clang-format off
100 #pragma omp parallel for \
101  default(none) \
102  shared(indices_list) \
103  num_threads(threads_)
104  // clang-format on
105  for (int i = 0; i < particle_num_; i++) {
106  coherence_->compute(
107  transed_reference_vector_[i], indices_list[i], (*particles_)[i].weight);
108  }
109  }
110 
111  normalizeWeight();
112 }
113 } // namespace tracking
114 } // namespace pcl
115 
116 #define PCL_INSTANTIATE_KLDAdaptiveParticleFilterOMPTracker(T, ST) \
117  template class PCL_EXPORTS pcl::tracking::KLDAdaptiveParticleFilterOMPTracker<T, ST>;
118 
119 #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.
typename Tracker< PointInT, StateT >::PointCloudIn PointCloudIn
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition: types.h:133
shared_ptr< Indices > IndicesPtr
Definition: pcl_base.h:58