337 lines
12 KiB
C++
337 lines
12 KiB
C++
/*
|
|
* Software License Agreement (BSD License)
|
|
*
|
|
* Point Cloud Library (PCL) - www.pointclouds.org
|
|
* Copyright (c) 2011, Alexandru-Eugen Ichim
|
|
* Willow Garage, Inc
|
|
* Copyright (c) 2012-, Open Perception, Inc.
|
|
*
|
|
* All rights reserved.
|
|
*
|
|
* Redistribution and use in source and binary forms, with or without
|
|
* modification, are permitted provided that the following conditions
|
|
* are met:
|
|
*
|
|
* * Redistributions of source code must retain the above copyright
|
|
* notice, this list of conditions and the following disclaimer.
|
|
* * Redistributions in binary form must reproduce the above
|
|
* copyright notice, this list of conditions and the following
|
|
* disclaimer in the documentation and/or other materials provided
|
|
* with the distribution.
|
|
* * Neither the name of the copyright holder(s) nor the names of its
|
|
* contributors may be used to endorse or promote products derived
|
|
* from this software without specific prior written permission.
|
|
*
|
|
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
|
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
|
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
|
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
|
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
|
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
|
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
|
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
|
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
|
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
|
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
|
* POSSIBILITY OF SUCH DAMAGE.
|
|
*
|
|
* $Id$
|
|
*
|
|
*/
|
|
|
|
#ifndef PCL_REGISTRATION_IMPL_PYRAMID_FEATURE_MATCHING_H_
|
|
#define PCL_REGISTRATION_IMPL_PYRAMID_FEATURE_MATCHING_H_
|
|
|
|
#include <pcl/common/point_tests.h> // for pcl::isFinite
|
|
#include <pcl/console/print.h>
|
|
#include <pcl/pcl_macros.h>
|
|
|
|
namespace pcl {
|
|
|
|
template <typename PointFeature>
|
|
float
|
|
PyramidFeatureHistogram<PointFeature>::comparePyramidFeatureHistograms(
|
|
const PyramidFeatureHistogramPtr& pyramid_a,
|
|
const PyramidFeatureHistogramPtr& pyramid_b)
|
|
{
|
|
// do a few consistency checks before and during the computation
|
|
if (pyramid_a->nr_dimensions != pyramid_b->nr_dimensions) {
|
|
PCL_ERROR("[pcl::PyramidFeatureMatching::comparePyramidFeatureHistograms] The two "
|
|
"given pyramids have different numbers of dimensions: %u vs %u\n",
|
|
pyramid_a->nr_dimensions,
|
|
pyramid_b->nr_dimensions);
|
|
return -1;
|
|
}
|
|
if (pyramid_a->nr_levels != pyramid_b->nr_levels) {
|
|
PCL_ERROR("[pcl::PyramidFeatureMatching::comparePyramidFeatureHistograms] The two "
|
|
"given pyramids have different numbers of levels: %u vs %u\n",
|
|
pyramid_a->nr_levels,
|
|
pyramid_b->nr_levels);
|
|
return -1;
|
|
}
|
|
|
|
// calculate for level 0 first
|
|
if (pyramid_a->hist_levels[0].hist.size() != pyramid_b->hist_levels[0].hist.size()) {
|
|
PCL_ERROR("[pcl::PyramidFeatureMatching::comparePyramidFeatureHistograms] The two "
|
|
"given pyramids have different numbers of bins on level 0: %u vs %u\n",
|
|
pyramid_a->hist_levels[0].hist.size(),
|
|
pyramid_b->hist_levels[0].hist.size());
|
|
return -1;
|
|
}
|
|
float match_count_level = 0.0f;
|
|
for (std::size_t bin_i = 0; bin_i < pyramid_a->hist_levels[0].hist.size(); ++bin_i) {
|
|
if (pyramid_a->hist_levels[0].hist[bin_i] < pyramid_b->hist_levels[0].hist[bin_i])
|
|
match_count_level += static_cast<float>(pyramid_a->hist_levels[0].hist[bin_i]);
|
|
else
|
|
match_count_level += static_cast<float>(pyramid_b->hist_levels[0].hist[bin_i]);
|
|
}
|
|
|
|
float match_count = match_count_level;
|
|
for (std::size_t level_i = 1; level_i < pyramid_a->nr_levels; ++level_i) {
|
|
if (pyramid_a->hist_levels[level_i].hist.size() !=
|
|
pyramid_b->hist_levels[level_i].hist.size()) {
|
|
PCL_ERROR(
|
|
"[pcl::PyramidFeatureMatching::comparePyramidFeatureHistograms] The two "
|
|
"given pyramids have different numbers of bins on level %u: %u vs %u\n",
|
|
level_i,
|
|
pyramid_a->hist_levels[level_i].hist.size(),
|
|
pyramid_b->hist_levels[level_i].hist.size());
|
|
return -1;
|
|
}
|
|
|
|
float match_count_prev_level = match_count_level;
|
|
match_count_level = 0.0f;
|
|
for (std::size_t bin_i = 0; bin_i < pyramid_a->hist_levels[level_i].hist.size();
|
|
++bin_i) {
|
|
if (pyramid_a->hist_levels[level_i].hist[bin_i] <
|
|
pyramid_b->hist_levels[level_i].hist[bin_i])
|
|
match_count_level +=
|
|
static_cast<float>(pyramid_a->hist_levels[level_i].hist[bin_i]);
|
|
else
|
|
match_count_level +=
|
|
static_cast<float>(pyramid_b->hist_levels[level_i].hist[bin_i]);
|
|
}
|
|
|
|
float level_normalization_factor = powf(2.0f, static_cast<float>(level_i));
|
|
match_count +=
|
|
(match_count_level - match_count_prev_level) / level_normalization_factor;
|
|
}
|
|
|
|
// include self-similarity factors
|
|
float self_similarity_a = static_cast<float>(pyramid_a->nr_features),
|
|
self_similarity_b = static_cast<float>(pyramid_b->nr_features);
|
|
PCL_DEBUG("[pcl::PyramidFeatureMatching::comparePyramidFeatureHistograms] Self "
|
|
"similarity measures: %f, %f\n",
|
|
self_similarity_a,
|
|
self_similarity_b);
|
|
match_count /= std::sqrt(self_similarity_a * self_similarity_b);
|
|
|
|
return match_count;
|
|
}
|
|
|
|
template <typename PointFeature>
|
|
PyramidFeatureHistogram<PointFeature>::PyramidFeatureHistogram()
|
|
: nr_dimensions(0)
|
|
, nr_levels(0)
|
|
, nr_features(0)
|
|
, feature_representation_(new DefaultPointRepresentation<PointFeature>)
|
|
, is_computed_(false)
|
|
, hist_levels()
|
|
{}
|
|
|
|
template <typename PointFeature>
|
|
void
|
|
PyramidFeatureHistogram<
|
|
PointFeature>::PyramidFeatureHistogramLevel::initializeHistogramLevel()
|
|
{
|
|
std::size_t total_vector_size = 1;
|
|
for (std::vector<std::size_t>::iterator dim_it = bins_per_dimension.begin();
|
|
dim_it != bins_per_dimension.end();
|
|
++dim_it)
|
|
total_vector_size *= *dim_it;
|
|
|
|
hist.resize(total_vector_size, 0);
|
|
}
|
|
|
|
template <typename PointFeature>
|
|
bool
|
|
PyramidFeatureHistogram<PointFeature>::initializeHistogram()
|
|
{
|
|
// a few consistency checks before starting the computations
|
|
if (!PCLBase<PointFeature>::initCompute()) {
|
|
PCL_ERROR("[pcl::PyramidFeatureHistogram::initializeHistogram] PCLBase initCompute "
|
|
"failed\n");
|
|
return false;
|
|
}
|
|
|
|
if (dimension_range_input_.empty()) {
|
|
PCL_ERROR("[pcl::PyramidFeatureHistogram::initializeHistogram] Input dimension "
|
|
"range was not set\n");
|
|
return false;
|
|
}
|
|
|
|
if (dimension_range_target_.empty()) {
|
|
PCL_ERROR("[pcl::PyramidFeatureHistogram::initializeHistogram] Target dimension "
|
|
"range was not set\n");
|
|
return false;
|
|
}
|
|
|
|
if (dimension_range_input_.size() != dimension_range_target_.size()) {
|
|
PCL_ERROR("[pcl::PyramidFeatureHistogram::initializeHistogram] Input and target "
|
|
"dimension ranges do not agree in size: %u vs %u\n",
|
|
dimension_range_input_.size(),
|
|
dimension_range_target_.size());
|
|
return false;
|
|
}
|
|
|
|
nr_dimensions = dimension_range_target_.size();
|
|
nr_features = input_->size();
|
|
float D = 0.0f;
|
|
for (std::vector<std::pair<float, float>>::iterator range_it =
|
|
dimension_range_target_.begin();
|
|
range_it != dimension_range_target_.end();
|
|
++range_it) {
|
|
float aux = range_it->first - range_it->second;
|
|
D += aux * aux;
|
|
}
|
|
D = std::sqrt(D);
|
|
nr_levels = static_cast<std::size_t>(std::ceil(std::log2(D)));
|
|
PCL_DEBUG("[pcl::PyramidFeatureHistogram::initializeHistogram] Pyramid will have %u "
|
|
"levels with a hyper-parallelepiped diagonal size of %f\n",
|
|
nr_levels,
|
|
D);
|
|
|
|
hist_levels.resize(nr_levels);
|
|
for (std::size_t level_i = 0; level_i < nr_levels; ++level_i) {
|
|
std::vector<std::size_t> bins_per_dimension(nr_dimensions);
|
|
std::vector<float> bin_step(nr_dimensions);
|
|
for (std::size_t dim_i = 0; dim_i < nr_dimensions; ++dim_i) {
|
|
bins_per_dimension[dim_i] = static_cast<std::size_t>(
|
|
std::ceil((dimension_range_target_[dim_i].second -
|
|
dimension_range_target_[dim_i].first) /
|
|
(powf(2.0f, static_cast<float>(level_i)) *
|
|
std::sqrt(static_cast<float>(nr_dimensions)))));
|
|
bin_step[dim_i] = powf(2.0f, static_cast<float>(level_i)) *
|
|
std::sqrt(static_cast<float>(nr_dimensions));
|
|
}
|
|
hist_levels[level_i] = PyramidFeatureHistogramLevel(bins_per_dimension, bin_step);
|
|
|
|
PCL_DEBUG("[pcl::PyramidFeatureHistogram::initializeHistogram] Created vector of "
|
|
"size %u at level %u\nwith #bins per dimension:",
|
|
hist_levels.back().hist.size(),
|
|
level_i);
|
|
for (std::size_t dim_i = 0; dim_i < nr_dimensions; ++dim_i)
|
|
PCL_DEBUG("%u ", bins_per_dimension[dim_i]);
|
|
PCL_DEBUG("\n");
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
template <typename PointFeature>
|
|
unsigned int&
|
|
PyramidFeatureHistogram<PointFeature>::at(std::vector<std::size_t>& access,
|
|
std::size_t& level)
|
|
{
|
|
if (access.size() != nr_dimensions) {
|
|
PCL_ERROR(
|
|
"[pcl::PyramidFeatureHistogram::at] Cannot access histogram position because "
|
|
"the access point does not have the right number of dimensions\n");
|
|
return hist_levels.front().hist.front();
|
|
}
|
|
if (level >= hist_levels.size()) {
|
|
PCL_ERROR(
|
|
"[pcl::PyramidFeatureHistogram::at] Trying to access a too large level\n");
|
|
return hist_levels.front().hist.front();
|
|
}
|
|
|
|
std::size_t vector_position = 0;
|
|
std::size_t dim_accumulator = 1;
|
|
|
|
for (int i = static_cast<int>(access.size()) - 1; i >= 0; --i) {
|
|
vector_position += access[i] * dim_accumulator;
|
|
dim_accumulator *= hist_levels[level].bins_per_dimension[i];
|
|
}
|
|
|
|
return hist_levels[level].hist[vector_position];
|
|
}
|
|
|
|
template <typename PointFeature>
|
|
unsigned int&
|
|
PyramidFeatureHistogram<PointFeature>::at(std::vector<float>& feature,
|
|
std::size_t& level)
|
|
{
|
|
if (feature.size() != nr_dimensions) {
|
|
PCL_ERROR("[pcl::PyramidFeatureHistogram::at] The given feature vector does not "
|
|
"match the feature dimensions of the pyramid histogram: %u vs %u\n",
|
|
feature.size(),
|
|
nr_dimensions);
|
|
return hist_levels.front().hist.front();
|
|
}
|
|
if (level >= hist_levels.size()) {
|
|
PCL_ERROR(
|
|
"[pcl::PyramidFeatureHistogram::at] Trying to access a too large level\n");
|
|
return hist_levels.front().hist.front();
|
|
}
|
|
|
|
std::vector<std::size_t> access;
|
|
for (std::size_t dim_i = 0; dim_i < nr_dimensions; ++dim_i)
|
|
access.push_back(static_cast<std::size_t>(
|
|
std::floor((feature[dim_i] - dimension_range_target_[dim_i].first) /
|
|
hist_levels[level].bin_step[dim_i])));
|
|
|
|
return at(access, level);
|
|
}
|
|
|
|
template <typename PointFeature>
|
|
void
|
|
PyramidFeatureHistogram<PointFeature>::convertFeatureToVector(
|
|
const PointFeature& feature, std::vector<float>& feature_vector)
|
|
{
|
|
// convert feature to vector representation
|
|
feature_vector.resize(feature_representation_->getNumberOfDimensions());
|
|
feature_representation_->vectorize(feature, feature_vector);
|
|
|
|
// adapt the values from the input range to the target range
|
|
for (std::size_t i = 0; i < feature_vector.size(); ++i)
|
|
feature_vector[i] =
|
|
(feature_vector[i] - dimension_range_input_[i].first) /
|
|
(dimension_range_input_[i].second - dimension_range_input_[i].first) *
|
|
(dimension_range_target_[i].second - dimension_range_target_[i].first) +
|
|
dimension_range_target_[i].first;
|
|
}
|
|
|
|
template <typename PointFeature>
|
|
void
|
|
PyramidFeatureHistogram<PointFeature>::compute()
|
|
{
|
|
if (!initializeHistogram())
|
|
return;
|
|
|
|
for (const auto& point : *input_) {
|
|
std::vector<float> feature_vector;
|
|
// NaN is converted to very high number that gives out of bound exception.
|
|
if (!pcl::isFinite(point))
|
|
continue;
|
|
convertFeatureToVector(point, feature_vector);
|
|
addFeature(feature_vector);
|
|
}
|
|
|
|
is_computed_ = true;
|
|
}
|
|
|
|
template <typename PointFeature>
|
|
void
|
|
PyramidFeatureHistogram<PointFeature>::addFeature(std::vector<float>& feature)
|
|
{
|
|
for (std::size_t level_i = 0; level_i < nr_levels; ++level_i)
|
|
at(feature, level_i)++;
|
|
}
|
|
|
|
} // namespace pcl
|
|
|
|
#define PCL_INSTANTIATE_PyramidFeatureHistogram(PointFeature) \
|
|
template class PCL_EXPORTS pcl::PyramidFeatureHistogram<PointFeature>;
|
|
|
|
#endif /* PCL_REGISTRATION_IMPL_PYRAMID_FEATURE_MATCHING_H_ */
|