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