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