Point Cloud Library (PCL) 1.15.0
Loading...
Searching...
No Matches
fpfh_omp.hpp
1/*
2 * Software License Agreement (BSD License)
3 *
4 * Point Cloud Library (PCL) - www.pointclouds.org
5 * Copyright (c) 2010-2011, Willow Garage, Inc.
6 * Copyright (c) 2012-, Open Perception, Inc.
7 *
8 * All rights reserved.
9 *
10 * Redistribution and use in source and binary forms, with or without
11 * modification, are permitted provided that the following conditions
12 * are met:
13 *
14 * * Redistributions of source code must retain the above copyright
15 * notice, this list of conditions and the following disclaimer.
16 * * Redistributions in binary form must reproduce the above
17 * copyright notice, this list of conditions and the following
18 * disclaimer in the documentation and/or other materials provided
19 * with the distribution.
20 * * Neither the name of the copyright holder(s) nor the names of its
21 * contributors may be used to endorse or promote products derived
22 * from this software without specific prior written permission.
23 *
24 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
25 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
26 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
27 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
28 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
29 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
30 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
31 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
32 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
33 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
34 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
35 * POSSIBILITY OF SUCH DAMAGE.
36 *
37 * $Id$
38 *
39 */
40
41#pragma once
42
43#include <pcl/features/fpfh_omp.h>
44
45#include <pcl/common/point_tests.h> // for pcl::isFinite
46
47#include <numeric>
48
49
50//////////////////////////////////////////////////////////////////////////////////////////////
51template <typename PointInT, typename PointNT, typename PointOutT> void
53{
54 if (nr_threads == 0)
55#ifdef _OPENMP
56 threads_ = omp_get_num_procs();
57#else
58 threads_ = 1;
59#endif
60 else
61 threads_ = nr_threads;
62}
63
64//////////////////////////////////////////////////////////////////////////////////////////////
65template <typename PointInT, typename PointNT, typename PointOutT> void
67{
68 std::vector<int> spfh_indices_vec;
69 std::vector<int> spfh_hist_lookup (surface_->size ());
70
71 // Build a list of (unique) indices for which we will need to compute SPFH signatures
72 // (We need an SPFH signature for every point that is a neighbor of any point in input_[indices_])
73 if (surface_ != input_ ||
74 indices_->size () != surface_->size ())
75 {
76 pcl::Indices nn_indices (k_); // \note These resizes are irrelevant for a radiusSearch ().
77 std::vector<float> nn_dists (k_);
78
79 std::set<int> spfh_indices_set;
80 for (std::size_t idx = 0; idx < indices_->size (); ++idx)
81 {
82 int p_idx = (*indices_)[idx];
83 if (!isFinite ((*input_)[p_idx]) ||
84 this->searchForNeighbors (p_idx, search_parameter_, nn_indices, nn_dists) == 0)
85 continue;
86
87 spfh_indices_set.insert (nn_indices.begin (), nn_indices.end ());
88 }
89 spfh_indices_vec.resize (spfh_indices_set.size ());
90 std::copy (spfh_indices_set.cbegin (), spfh_indices_set.cend (), spfh_indices_vec.begin ());
91 }
92 else
93 {
94 // Special case: When a feature must be computed at every point, there is no need for a neighborhood search
95 spfh_indices_vec.resize (indices_->size ());
96 std::iota(spfh_indices_vec.begin (), spfh_indices_vec.end (),
97 static_cast<decltype(spfh_indices_vec)::value_type>(0));
98 }
99
100 // Initialize the arrays that will store the SPFH signatures
101 const auto data_size = spfh_indices_vec.size ();
102 hist_f1_.setZero (data_size, nr_bins_f1_);
103 hist_f2_.setZero (data_size, nr_bins_f2_);
104 hist_f3_.setZero (data_size, nr_bins_f3_);
105
106 pcl::Indices nn_indices (k_); // \note These resizes are irrelevant for a radiusSearch ().
107 std::vector<float> nn_dists (k_);
108
109 // Compute SPFH signatures for every point that needs them
110
111#pragma omp parallel for \
112 default(none) \
113 shared(spfh_hist_lookup, spfh_indices_vec) \
114 firstprivate(nn_indices, nn_dists) \
115 num_threads(threads_)
116 for (std::ptrdiff_t i = 0; i < static_cast<std::ptrdiff_t> (spfh_indices_vec.size ()); ++i)
117 {
118 // Get the next point index
119 int p_idx = spfh_indices_vec[i];
120
121 // Find the neighborhood around p_idx
122 if (!isFinite ((*surface_)[p_idx]) ||
123 this->searchForNeighbors (*surface_, p_idx, search_parameter_, nn_indices, nn_dists) == 0)
124 continue;
125
126 // Estimate the SPFH signature around p_idx
127 this->computePointSPFHSignature (*surface_, *normals_, p_idx, i, nn_indices, hist_f1_, hist_f2_, hist_f3_);
128
129 // Populate a lookup table for converting a point index to its corresponding row in the spfh_hist_* matrices
130 spfh_hist_lookup[p_idx] = i;
131 }
132
133 // Initialize the array that will store the FPFH signature
134 int nr_bins = nr_bins_f1_ + nr_bins_f2_ + nr_bins_f3_;
135
136 nn_indices.clear();
137 nn_dists.clear();
138
139 // Iterate over the entire index vector
140#pragma omp parallel for \
141 default(none) \
142 shared(nr_bins, output, spfh_hist_lookup) \
143 firstprivate(nn_dists, nn_indices) \
144 num_threads(threads_)
145 for (std::ptrdiff_t idx = 0; idx < static_cast<std::ptrdiff_t> (indices_->size ()); ++idx)
146 {
147 // Find the indices of point idx's neighbors...
148 if (!isFinite ((*input_)[(*indices_)[idx]]) ||
149 this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_dists) == 0)
150 {
151 for (int d = 0; d < nr_bins; ++d)
152 output[idx].histogram[d] = std::numeric_limits<float>::quiet_NaN ();
153
154 output.is_dense = false;
155 continue;
156 }
157
158
159 // ... and remap the nn_indices values so that they represent row indices in the spfh_hist_* matrices
160 // instead of indices into surface_->points
161 for (auto &nn_index : nn_indices)
162 nn_index = spfh_hist_lookup[nn_index];
163
164 // Compute the FPFH signature (i.e. compute a weighted combination of local SPFH signatures) ...
165 Eigen::VectorXf fpfh_histogram = Eigen::VectorXf::Zero (nr_bins);
166 weightPointSPFHSignature (hist_f1_, hist_f2_, hist_f3_, nn_indices, nn_dists, fpfh_histogram);
167
168 // ...and copy it into the output cloud
169 for (int d = 0; d < nr_bins; ++d)
170 output[idx].histogram[d] = fpfh_histogram[d];
171 }
172
173}
174
175#define PCL_INSTANTIATE_FPFHEstimationOMP(T,NT,OutT) template class PCL_EXPORTS pcl::FPFHEstimationOMP<T,NT,OutT>;
176
FPFHEstimationOMP estimates the Fast Point Feature Histogram (FPFH) descriptor for a given point clou...
Definition fpfh_omp.h:75
void setNumberOfThreads(unsigned int nr_threads=0)
Initialize the scheduler and set the number of threads to use.
Definition fpfh_omp.hpp:52
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition types.h:133