Point Cloud Library (PCL) 1.13.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
54 const PyramidFeatureHistogramPtr& pyramid_a,
55 const PyramidFeatureHistogramPtr& pyramid_b)
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
89 float match_count = match_count_level;
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
102 float match_count_prev_level = match_count_level;
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])
108 match_count_level +=
109 static_cast<float>(pyramid_a->hist_levels[level_i].hist[bin_i]);
110 else
111 match_count_level +=
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 +=
117 (match_count_level - match_count_prev_level) / level_normalization_factor;
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",
125 self_similarity_a,
126 self_similarity_b);
127 match_count /= std::sqrt(self_similarity_a * self_similarity_b);
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 (const auto& bin : bins_per_dimension) {
149 total_vector_size *= bin;
150 }
151
152 hist.resize(total_vector_size, 0);
153}
154
155template <typename PointFeature>
156bool
157PyramidFeatureHistogram<PointFeature>::initializeHistogram()
158{
159 // a few consistency checks before starting the computations
160 if (!PCLBase<PointFeature>::initCompute()) {
161 PCL_ERROR("[pcl::PyramidFeatureHistogram::initializeHistogram] PCLBase initCompute "
162 "failed\n");
163 return false;
164 }
165
166 if (dimension_range_input_.empty()) {
167 PCL_ERROR("[pcl::PyramidFeatureHistogram::initializeHistogram] Input dimension "
168 "range was not set\n");
169 return false;
170 }
171
172 if (dimension_range_target_.empty()) {
173 PCL_ERROR("[pcl::PyramidFeatureHistogram::initializeHistogram] Target dimension "
174 "range was not set\n");
175 return false;
176 }
177
178 if (dimension_range_input_.size() != dimension_range_target_.size()) {
179 PCL_ERROR("[pcl::PyramidFeatureHistogram::initializeHistogram] Input and target "
180 "dimension ranges do not agree in size: %u vs %u\n",
181 dimension_range_input_.size(),
182 dimension_range_target_.size());
183 return false;
184 }
185
186 nr_dimensions = dimension_range_target_.size();
187 nr_features = input_->size();
188 float D = 0.0f;
189 for (const auto& dim : dimension_range_target_) {
190 float aux = dim.first - dim.second;
191 D += aux * aux;
192 }
193 D = std::sqrt(D);
194 nr_levels = static_cast<std::size_t>(std::ceil(std::log2(D)));
195 PCL_DEBUG("[pcl::PyramidFeatureHistogram::initializeHistogram] Pyramid will have %u "
196 "levels with a hyper-parallelepiped diagonal size of %f\n",
197 nr_levels,
198 D);
199
200 hist_levels.resize(nr_levels);
201 for (std::size_t level_i = 0; level_i < nr_levels; ++level_i) {
202 std::vector<std::size_t> bins_per_dimension(nr_dimensions);
203 std::vector<float> bin_step(nr_dimensions);
204 for (std::size_t dim_i = 0; dim_i < nr_dimensions; ++dim_i) {
205 bins_per_dimension[dim_i] = static_cast<std::size_t>(
206 std::ceil((dimension_range_target_[dim_i].second -
207 dimension_range_target_[dim_i].first) /
208 (powf(2.0f, static_cast<float>(level_i)) *
209 std::sqrt(static_cast<float>(nr_dimensions)))));
210 bin_step[dim_i] = powf(2.0f, static_cast<float>(level_i)) *
211 std::sqrt(static_cast<float>(nr_dimensions));
212 }
213 hist_levels[level_i] = PyramidFeatureHistogramLevel(bins_per_dimension, bin_step);
214
215 PCL_DEBUG("[pcl::PyramidFeatureHistogram::initializeHistogram] Created vector of "
216 "size %u at level %u\nwith #bins per dimension:",
217 hist_levels.back().hist.size(),
218 level_i);
219 for (std::size_t dim_i = 0; dim_i < nr_dimensions; ++dim_i)
220 PCL_DEBUG("%u ", bins_per_dimension[dim_i]);
221 PCL_DEBUG("\n");
222 }
223
224 return true;
225}
226
227template <typename PointFeature>
228unsigned int&
229PyramidFeatureHistogram<PointFeature>::at(std::vector<std::size_t>& access,
230 std::size_t& level)
231{
232 if (access.size() != nr_dimensions) {
233 PCL_ERROR(
234 "[pcl::PyramidFeatureHistogram::at] Cannot access histogram position because "
235 "the access point does not have the right number of dimensions\n");
236 return hist_levels.front().hist.front();
237 }
238 if (level >= hist_levels.size()) {
239 PCL_ERROR(
240 "[pcl::PyramidFeatureHistogram::at] Trying to access a too large level\n");
241 return hist_levels.front().hist.front();
242 }
243
244 std::size_t vector_position = 0;
245 std::size_t dim_accumulator = 1;
246
247 for (int i = static_cast<int>(access.size()) - 1; i >= 0; --i) {
248 vector_position += access[i] * dim_accumulator;
249 dim_accumulator *= hist_levels[level].bins_per_dimension[i];
250 }
251
252 return hist_levels[level].hist[vector_position];
253}
254
255template <typename PointFeature>
256unsigned int&
257PyramidFeatureHistogram<PointFeature>::at(std::vector<float>& feature,
258 std::size_t& level)
259{
260 if (feature.size() != nr_dimensions) {
261 PCL_ERROR("[pcl::PyramidFeatureHistogram::at] The given feature vector does not "
262 "match the feature dimensions of the pyramid histogram: %u vs %u\n",
263 feature.size(),
264 nr_dimensions);
265 return hist_levels.front().hist.front();
266 }
267 if (level >= hist_levels.size()) {
268 PCL_ERROR(
269 "[pcl::PyramidFeatureHistogram::at] Trying to access a too large level\n");
270 return hist_levels.front().hist.front();
271 }
272
273 std::vector<std::size_t> access;
274 for (std::size_t dim_i = 0; dim_i < nr_dimensions; ++dim_i)
275 access.push_back(static_cast<std::size_t>(
276 std::floor((feature[dim_i] - dimension_range_target_[dim_i].first) /
277 hist_levels[level].bin_step[dim_i])));
278
279 return at(access, level);
280}
281
282template <typename PointFeature>
283void
284PyramidFeatureHistogram<PointFeature>::convertFeatureToVector(
285 const PointFeature& feature, std::vector<float>& feature_vector)
286{
287 // convert feature to vector representation
288 feature_vector.resize(feature_representation_->getNumberOfDimensions());
289 feature_representation_->vectorize(feature, feature_vector);
290
291 // adapt the values from the input range to the target range
292 for (std::size_t i = 0; i < feature_vector.size(); ++i)
293 feature_vector[i] =
294 (feature_vector[i] - dimension_range_input_[i].first) /
295 (dimension_range_input_[i].second - dimension_range_input_[i].first) *
296 (dimension_range_target_[i].second - dimension_range_target_[i].first) +
297 dimension_range_target_[i].first;
298}
299
300template <typename PointFeature>
301void
303{
304 if (!initializeHistogram())
305 return;
306
307 for (const auto& point : *input_) {
308 std::vector<float> feature_vector;
309 // NaN is converted to very high number that gives out of bound exception.
310 if (!pcl::isFinite(point))
311 continue;
312 convertFeatureToVector(point, feature_vector);
313 addFeature(feature_vector);
314 }
315
316 is_computed_ = true;
317}
318
319template <typename PointFeature>
320void
321PyramidFeatureHistogram<PointFeature>::addFeature(std::vector<float>& feature)
322{
323 for (std::size_t level_i = 0; level_i < nr_levels; ++level_i)
324 at(feature, level_i)++;
325}
326
327} // namespace pcl
328
329#define PCL_INSTANTIATE_PyramidFeatureHistogram(PointFeature) \
330 template class PCL_EXPORTS pcl::PyramidFeatureHistogram<PointFeature>;
331
332#endif /* PCL_REGISTRATION_IMPL_PYRAMID_FEATURE_MATCHING_H_ */
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.