Point Cloud Library (PCL) 1.12.0
Loading...
Searching...
No Matches
pyramid_feature_matching.hpp
1/*
2 * Software License Agreement (BSD License)
3 *
4 * Point Cloud Library (PCL) - www.pointclouds.org
5 * Copyright (c) 2011, Alexandru-Eugen Ichim
6 * Willow Garage, Inc
7 * Copyright (c) 2012-, Open Perception, Inc.
8 *
9 * All rights reserved.
10 *
11 * Redistribution and use in source and binary forms, with or without
12 * modification, are permitted provided that the following conditions
13 * are met:
14 *
15 * * Redistributions of source code must retain the above copyright
16 * notice, this list of conditions and the following disclaimer.
17 * * Redistributions in binary form must reproduce the above
18 * copyright notice, this list of conditions and the following
19 * disclaimer in the documentation and/or other materials provided
20 * with the distribution.
21 * * Neither the name of the copyright holder(s) nor the names of its
22 * contributors may be used to endorse or promote products derived
23 * from this software without specific prior written permission.
24 *
25 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
26 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
27 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
28 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
29 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
30 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
31 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
32 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
33 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
34 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
35 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
36 * POSSIBILITY OF SUCH DAMAGE.
37 *
38 * $Id$
39 *
40 */
41
42#ifndef PCL_REGISTRATION_IMPL_PYRAMID_FEATURE_MATCHING_H_
43#define PCL_REGISTRATION_IMPL_PYRAMID_FEATURE_MATCHING_H_
44
45#include <pcl/common/point_tests.h> // for pcl::isFinite
46#include <pcl/console/print.h>
47#include <pcl/pcl_macros.h>
48
49namespace pcl {
50
51template <typename PointFeature>
52float
56{
57 // do a few consistency checks before and during the computation
58 if (pyramid_a->nr_dimensions != pyramid_b->nr_dimensions) {
59 PCL_ERROR("[pcl::PyramidFeatureMatching::comparePyramidFeatureHistograms] The two "
60 "given pyramids have different numbers of dimensions: %u vs %u\n",
61 pyramid_a->nr_dimensions,
62 pyramid_b->nr_dimensions);
63 return -1;
64 }
65 if (pyramid_a->nr_levels != pyramid_b->nr_levels) {
66 PCL_ERROR("[pcl::PyramidFeatureMatching::comparePyramidFeatureHistograms] The two "
67 "given pyramids have different numbers of levels: %u vs %u\n",
68 pyramid_a->nr_levels,
69 pyramid_b->nr_levels);
70 return -1;
71 }
72
73 // calculate for level 0 first
74 if (pyramid_a->hist_levels[0].hist.size() != pyramid_b->hist_levels[0].hist.size()) {
75 PCL_ERROR("[pcl::PyramidFeatureMatching::comparePyramidFeatureHistograms] The two "
76 "given pyramids have different numbers of bins on level 0: %u vs %u\n",
77 pyramid_a->hist_levels[0].hist.size(),
78 pyramid_b->hist_levels[0].hist.size());
79 return -1;
80 }
81 float match_count_level = 0.0f;
82 for (std::size_t bin_i = 0; bin_i < pyramid_a->hist_levels[0].hist.size(); ++bin_i) {
83 if (pyramid_a->hist_levels[0].hist[bin_i] < pyramid_b->hist_levels[0].hist[bin_i])
84 match_count_level += static_cast<float>(pyramid_a->hist_levels[0].hist[bin_i]);
85 else
86 match_count_level += static_cast<float>(pyramid_b->hist_levels[0].hist[bin_i]);
87 }
88
90 for (std::size_t level_i = 1; level_i < pyramid_a->nr_levels; ++level_i) {
91 if (pyramid_a->hist_levels[level_i].hist.size() !=
92 pyramid_b->hist_levels[level_i].hist.size()) {
93 PCL_ERROR(
94 "[pcl::PyramidFeatureMatching::comparePyramidFeatureHistograms] The two "
95 "given pyramids have different numbers of bins on level %u: %u vs %u\n",
96 level_i,
97 pyramid_a->hist_levels[level_i].hist.size(),
98 pyramid_b->hist_levels[level_i].hist.size());
99 return -1;
100 }
101
103 match_count_level = 0.0f;
104 for (std::size_t bin_i = 0; bin_i < pyramid_a->hist_levels[level_i].hist.size();
105 ++bin_i) {
106 if (pyramid_a->hist_levels[level_i].hist[bin_i] <
107 pyramid_b->hist_levels[level_i].hist[bin_i])
109 static_cast<float>(pyramid_a->hist_levels[level_i].hist[bin_i]);
110 else
112 static_cast<float>(pyramid_b->hist_levels[level_i].hist[bin_i]);
113 }
114
115 float level_normalization_factor = powf(2.0f, static_cast<float>(level_i));
116 match_count +=
118 }
119
120 // include self-similarity factors
121 float self_similarity_a = static_cast<float>(pyramid_a->nr_features),
122 self_similarity_b = static_cast<float>(pyramid_b->nr_features);
123 PCL_DEBUG("[pcl::PyramidFeatureMatching::comparePyramidFeatureHistograms] Self "
124 "similarity measures: %f, %f\n",
128
129 return match_count;
130}
131
132template <typename PointFeature>
134: nr_dimensions(0)
135, nr_levels(0)
136, nr_features(0)
137, feature_representation_(new DefaultPointRepresentation<PointFeature>)
138, is_computed_(false)
139, hist_levels()
140{}
141
142template <typename PointFeature>
143void
145 PointFeature>::PyramidFeatureHistogramLevel::initializeHistogramLevel()
146{
147 std::size_t total_vector_size = 1;
148 for (std::vector<std::size_t>::iterator dim_it = bins_per_dimension.begin();
149 dim_it != bins_per_dimension.end();
150 ++dim_it)
152
153 hist.resize(total_vector_size, 0);
154}
155
156template <typename PointFeature>
157bool
158PyramidFeatureHistogram<PointFeature>::initializeHistogram()
159{
160 // a few consistency checks before starting the computations
162 PCL_ERROR("[pcl::PyramidFeatureHistogram::initializeHistogram] PCLBase initCompute "
163 "failed\n");
164 return false;
165 }
166
167 if (dimension_range_input_.empty()) {
168 PCL_ERROR("[pcl::PyramidFeatureHistogram::initializeHistogram] Input dimension "
169 "range was not set\n");
170 return false;
171 }
172
173 if (dimension_range_target_.empty()) {
174 PCL_ERROR("[pcl::PyramidFeatureHistogram::initializeHistogram] Target dimension "
175 "range was not set\n");
176 return false;
177 }
178
179 if (dimension_range_input_.size() != dimension_range_target_.size()) {
180 PCL_ERROR("[pcl::PyramidFeatureHistogram::initializeHistogram] Input and target "
181 "dimension ranges do not agree in size: %u vs %u\n",
182 dimension_range_input_.size(),
183 dimension_range_target_.size());
184 return false;
185 }
186
187 nr_dimensions = dimension_range_target_.size();
188 nr_features = input_->size();
189 float D = 0.0f;
190 for (std::vector<std::pair<float, float>>::iterator range_it =
191 dimension_range_target_.begin();
192 range_it != dimension_range_target_.end();
193 ++range_it) {
194 float aux = range_it->first - range_it->second;
195 D += aux * aux;
196 }
197 D = std::sqrt(D);
198 nr_levels = static_cast<std::size_t>(std::ceil(std::log2(D)));
199 PCL_DEBUG("[pcl::PyramidFeatureHistogram::initializeHistogram] Pyramid will have %u "
200 "levels with a hyper-parallelepiped diagonal size of %f\n",
201 nr_levels,
202 D);
203
204 hist_levels.resize(nr_levels);
205 for (std::size_t level_i = 0; level_i < nr_levels; ++level_i) {
206 std::vector<std::size_t> bins_per_dimension(nr_dimensions);
207 std::vector<float> bin_step(nr_dimensions);
208 for (std::size_t dim_i = 0; dim_i < nr_dimensions; ++dim_i) {
209 bins_per_dimension[dim_i] = static_cast<std::size_t>(
210 std::ceil((dimension_range_target_[dim_i].second -
211 dimension_range_target_[dim_i].first) /
212 (powf(2.0f, static_cast<float>(level_i)) *
213 std::sqrt(static_cast<float>(nr_dimensions)))));
214 bin_step[dim_i] = powf(2.0f, static_cast<float>(level_i)) *
215 std::sqrt(static_cast<float>(nr_dimensions));
216 }
217 hist_levels[level_i] = PyramidFeatureHistogramLevel(bins_per_dimension, bin_step);
218
219 PCL_DEBUG("[pcl::PyramidFeatureHistogram::initializeHistogram] Created vector of "
220 "size %u at level %u\nwith #bins per dimension:",
221 hist_levels.back().hist.size(),
222 level_i);
223 for (std::size_t dim_i = 0; dim_i < nr_dimensions; ++dim_i)
224 PCL_DEBUG("%u ", bins_per_dimension[dim_i]);
225 PCL_DEBUG("\n");
226 }
227
228 return true;
229}
230
231template <typename PointFeature>
232unsigned int&
233PyramidFeatureHistogram<PointFeature>::at(std::vector<std::size_t>& access,
234 std::size_t& level)
235{
236 if (access.size() != nr_dimensions) {
237 PCL_ERROR(
238 "[pcl::PyramidFeatureHistogram::at] Cannot access histogram position because "
239 "the access point does not have the right number of dimensions\n");
240 return hist_levels.front().hist.front();
241 }
242 if (level >= hist_levels.size()) {
243 PCL_ERROR(
244 "[pcl::PyramidFeatureHistogram::at] Trying to access a too large level\n");
245 return hist_levels.front().hist.front();
246 }
247
248 std::size_t vector_position = 0;
249 std::size_t dim_accumulator = 1;
250
251 for (int i = static_cast<int>(access.size()) - 1; i >= 0; --i) {
253 dim_accumulator *= hist_levels[level].bins_per_dimension[i];
254 }
255
256 return hist_levels[level].hist[vector_position];
257}
258
259template <typename PointFeature>
260unsigned int&
261PyramidFeatureHistogram<PointFeature>::at(std::vector<float>& feature,
262 std::size_t& level)
263{
264 if (feature.size() != nr_dimensions) {
265 PCL_ERROR("[pcl::PyramidFeatureHistogram::at] The given feature vector does not "
266 "match the feature dimensions of the pyramid histogram: %u vs %u\n",
267 feature.size(),
268 nr_dimensions);
269 return hist_levels.front().hist.front();
270 }
271 if (level >= hist_levels.size()) {
272 PCL_ERROR(
273 "[pcl::PyramidFeatureHistogram::at] Trying to access a too large level\n");
274 return hist_levels.front().hist.front();
275 }
276
277 std::vector<std::size_t> access;
278 for (std::size_t dim_i = 0; dim_i < nr_dimensions; ++dim_i)
279 access.push_back(static_cast<std::size_t>(
280 std::floor((feature[dim_i] - dimension_range_target_[dim_i].first) /
281 hist_levels[level].bin_step[dim_i])));
282
283 return at(access, level);
284}
285
286template <typename PointFeature>
287void
288PyramidFeatureHistogram<PointFeature>::convertFeatureToVector(
289 const PointFeature& feature, std::vector<float>& feature_vector)
290{
291 // convert feature to vector representation
292 feature_vector.resize(feature_representation_->getNumberOfDimensions());
293 feature_representation_->vectorize(feature, feature_vector);
294
295 // adapt the values from the input range to the target range
296 for (std::size_t i = 0; i < feature_vector.size(); ++i)
297 feature_vector[i] =
298 (feature_vector[i] - dimension_range_input_[i].first) /
299 (dimension_range_input_[i].second - dimension_range_input_[i].first) *
300 (dimension_range_target_[i].second - dimension_range_target_[i].first) +
301 dimension_range_target_[i].first;
302}
303
304template <typename PointFeature>
305void
307{
308 if (!initializeHistogram())
309 return;
310
311 for (const auto& point : *input_) {
312 std::vector<float> feature_vector;
313 // NaN is converted to very high number that gives out of bound exception.
314 if (!pcl::isFinite(point))
315 continue;
316 convertFeatureToVector(point, feature_vector);
317 addFeature(feature_vector);
318 }
319
320 is_computed_ = true;
321}
322
323template <typename PointFeature>
324void
325PyramidFeatureHistogram<PointFeature>::addFeature(std::vector<float>& feature)
326{
327 for (std::size_t level_i = 0; level_i < nr_levels; ++level_i)
328 at(feature, level_i)++;
329}
330
331} // namespace pcl
332
333#define PCL_INSTANTIATE_PyramidFeatureHistogram(PointFeature) \
334 template class PCL_EXPORTS pcl::PyramidFeatureHistogram<PointFeature>;
335
336#endif /* PCL_REGISTRATION_IMPL_PYRAMID_FEATURE_MATCHING_H_ */
Iterator class for point clouds with or without given indices.
ConstCloudIterator(const PointCloud< PointT > &cloud)
std::size_t size() const
Size of the range the iterator is going through.
DefaultPointRepresentation extends PointRepresentation to define default behavior for common point ty...
Class that compares two sets of features by using a multiscale representation of the features inside ...
void compute()
The central method for inserting the feature set inside the pyramid and obtaining the complete pyrami...
static float comparePyramidFeatureHistograms(const PyramidFeatureHistogramPtr &pyramid_a, const PyramidFeatureHistogramPtr &pyramid_b)
Static method for comparing two pyramid histograms that returns a floating point value between 0 and ...
PyramidFeatureHistogram()
Empty constructor that instantiates the feature representation variable.
bool isFinite(const PointT &pt)
Tests if the 3D components of a point are all finite param[in] pt point to be tested return true if f...
Definition point_tests.h:55
Defines all the PCL and non-PCL macros used.