Point Cloud Library (PCL) 1.15.0
Loading...
Searching...
No Matches
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
6namespace pcl {
7namespace tracking {
8//////////////////////////////////////////////////////////////////////////////////////////////
9template <typename PointInT, typename StateT>
10void
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//////////////////////////////////////////////////////////////////////////////////////////////
24template <typename PointInT, typename StateT>
25void
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.
typename Tracker< PointInT, StateT >::PointCloudIn PointCloudIn
void setNumberOfThreads(unsigned int nr_threads=0)
Initialize the scheduler and set the number of threads to use.
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition types.h:133
shared_ptr< Indices > IndicesPtr
Definition pcl_base.h:58