Point Cloud Library (PCL) 1.13.0
Loading...
Searching...
No Matches
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
6namespace pcl {
7namespace tracking {
8//////////////////////////////////////////////////////////////////////////////////////////////
9template <typename PointInT, typename StateT>
10void
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//////////////////////////////////////////////////////////////////////////////////////////////
25template <typename PointInT, typename StateT>
26void
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