statistical_multiscale_interest_region_extraction.hpp
/*
* Software License Agreement (BSD License)
*
* Point Cloud Library (PCL) - www.pointclouds.org
* Copyright (c) 2011, Alexandru-Eugen Ichim
* 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_FEATURES_IMPL_STATISTICAL_MULTISCALE_INTEREST_REGION_EXTRACTION_H_
#define PCL_FEATURES_IMPL_STATISTICAL_MULTISCALE_INTEREST_REGION_EXTRACTION_H_
#include <pcl/features/statistical_multiscale_interest_region_extraction.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/common/distances.h>
#include <pcl/console/print.h> // for PCL_INFO, PCL_ERROR
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/johnson_all_pairs_shortest.hpp>
//////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointT> void
pcl::StatisticalMultiscaleInterestRegionExtraction<PointT>::generateCloudGraph ()
{
// generate a K-NNG (K-nearest neighbors graph)
pcl::KdTreeFLANN<PointT> kdtree;
kdtree.setInputCloud (input_);
using namespace boost;
using Weight = property<edge_weight_t, float>;
using Graph = adjacency_list<vecS, vecS, undirectedS, no_property, Weight>;
Graph cloud_graph;
for (std::size_t point_i = 0; point_i < input_->size (); ++point_i)
{
pcl::Indices k_indices (16);
std::vector<float> k_distances (16);
kdtree.nearestKSearch (static_cast<int> (point_i), 16, k_indices, k_distances);
for (std::size_t k_i = 0; k_i < k_indices.size (); ++k_i)
add_edge (point_i, k_indices[k_i], Weight (std::sqrt (k_distances[k_i])), cloud_graph);
}
const std::size_t E = num_edges (cloud_graph),
V = num_vertices (cloud_graph);
PCL_INFO ("The graph has %lu vertices and %lu edges.\n", V, E);
geodesic_distances_.clear ();
for (std::size_t i = 0; i < V; ++i)
{
std::vector<float> aux (V);
geodesic_distances_.push_back (aux);
}
johnson_all_pairs_shortest_paths (cloud_graph, geodesic_distances_);
PCL_INFO ("Done generating the graph\n");
}
//////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointT> bool
pcl::StatisticalMultiscaleInterestRegionExtraction<PointT>::initCompute ()
{
if (!PCLBase<PointT>::initCompute ())
{
PCL_ERROR ("[pcl::StatisticalMultiscaleInterestRegionExtraction::initCompute] PCLBase::initCompute () failed - no input cloud was given.\n");
return (false);
}
if (scale_values_.empty ())
{
PCL_ERROR ("[pcl::StatisticalMultiscaleInterestRegionExtraction::initCompute] No scale values were given\n");
return (false);
}
return (true);
}
//////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointT> void
pcl::StatisticalMultiscaleInterestRegionExtraction<PointT>::geodesicFixedRadiusSearch (std::size_t &query_index,
float &radius,
std::vector<int> &result_indices)
{
for (std::size_t i = 0; i < geodesic_distances_[query_index].size (); ++i)
if (i != query_index && geodesic_distances_[query_index][i] < radius)
result_indices.push_back (static_cast<int> (i));
}
//////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointT> void
pcl::StatisticalMultiscaleInterestRegionExtraction<PointT>::computeRegionsOfInterest (std::list<IndicesPtr> &rois)
{
if (!initCompute ())
{
PCL_ERROR ("StatisticalMultiscaleInterestRegionExtraction: not completely initialized\n");
return;
}
generateCloudGraph ();
computeF ();
extractExtrema (rois);
}
//////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointT> void
pcl::StatisticalMultiscaleInterestRegionExtraction<PointT>::computeF ()
{
PCL_INFO ("Calculating statistical information\n");
// declare and initialize data structure
F_scales_.resize (scale_values_.size ());
std::vector<float> point_density (input_->size ()),
F (input_->size ());
std::vector<std::vector<float> > phi (input_->size ());
std::vector<float> phi_row (input_->size ());
for (std::size_t scale_i = 0; scale_i < scale_values_.size (); ++scale_i)
{
float scale_squared = scale_values_[scale_i] * scale_values_[scale_i];
// calculate point density for each point x_i
for (std::size_t point_i = 0; point_i < input_->size (); ++point_i)
{
float point_density_i = 0.0;
for (std::size_t point_j = 0; point_j < input_->size (); ++point_j)
{
float d_g = geodesic_distances_[point_i][point_j];
float phi_i_j = 1.0f / std::sqrt (2.0f * static_cast<float> (M_PI) * scale_squared) * std::exp ( (-1) * d_g*d_g / (2.0f * scale_squared));
point_density_i += phi_i_j;
phi_row[point_j] = phi_i_j;
}
point_density[point_i] = point_density_i;
phi[point_i] = phi_row;
}
// compute weights for each pair (x_i, x_j), evaluate the operator A_hat
for (std::size_t point_i = 0; point_i < input_->size (); ++point_i)
{
float A_hat_normalization = 0.0;
PointT A_hat; A_hat.x = A_hat.y = A_hat.z = 0.0;
for (std::size_t point_j = 0; point_j < input_->size (); ++point_j)
{
float phi_hat_i_j = phi[point_i][point_j] / (point_density[point_i] * point_density[point_j]);
A_hat_normalization += phi_hat_i_j;
PointT aux = (*input_)[point_j];
aux.x *= phi_hat_i_j; aux.y *= phi_hat_i_j; aux.z *= phi_hat_i_j;
A_hat.x += aux.x; A_hat.y += aux.y; A_hat.z += aux.z;
}
A_hat.x /= A_hat_normalization; A_hat.y /= A_hat_normalization; A_hat.z /= A_hat_normalization;
// compute the invariant F
float aux = 2.0f / scale_values_[scale_i] * euclideanDistance<PointT, PointT> (A_hat, (*input_)[point_i]);
F[point_i] = aux * std::exp (-aux);
}
F_scales_[scale_i] = F;
}
}
//////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointT> void
pcl::StatisticalMultiscaleInterestRegionExtraction<PointT>::extractExtrema (std::list<IndicesPtr> &rois)
{
std::vector<std::vector<bool> > is_min (scale_values_.size ()),
is_max (scale_values_.size ());
// for each point, check if it is a local extrema on each scale
for (std::size_t scale_i = 0; scale_i < scale_values_.size (); ++scale_i)
{
std::vector<bool> is_min_scale (input_->size ()),
is_max_scale (input_->size ());
for (std::size_t point_i = 0; point_i < input_->size (); ++point_i)
{
std::vector<int> nn_indices;
geodesicFixedRadiusSearch (point_i, scale_values_[scale_i], nn_indices);
bool is_max_point = true, is_min_point = true;
for (const auto &nn_index : nn_indices)
if (F_scales_[scale_i][point_i] < F_scales_[scale_i][nn_index])
is_max_point = false;
else
is_min_point = false;
is_min_scale[point_i] = is_min_point;
is_max_scale[point_i] = is_max_point;
}
is_min[scale_i] = is_min_scale;
is_max[scale_i] = is_max_scale;
}
// look for points that are min/max over three consecutive scales
for (std::size_t scale_i = 1; scale_i < scale_values_.size () - 1; ++scale_i)
{
for (std::size_t point_i = 0; point_i < input_->size (); ++point_i)
if ((is_min[scale_i - 1][point_i] && is_min[scale_i][point_i] && is_min[scale_i + 1][point_i]) ||
(is_max[scale_i - 1][point_i] && is_max[scale_i][point_i] && is_max[scale_i + 1][point_i]))
{
// add the point to the result vector
IndicesPtr region (new pcl::Indices);
region->push_back (static_cast<int> (point_i));
// and also add its scale-sized geodesic neighborhood
std::vector<int> nn_indices;
geodesicFixedRadiusSearch (point_i, scale_values_[scale_i], nn_indices);
region->insert (region->end (), nn_indices.begin (), nn_indices.end ());
rois.push_back (region);
}
}
}
#define PCL_INSTANTIATE_StatisticalMultiscaleInterestRegionExtraction(T) template class PCL_EXPORTS pcl::StatisticalMultiscaleInterestRegionExtraction<T>;
#endif /* PCL_FEATURES_IMPL_STATISTICAL_MULTISCALE_INTEREST_REGION_EXTRACTION_H_ */
pcl::KdTreeFLANN
KdTreeFLANN is a generic type of 3D spatial locator using kD-tree structures.
Definition: kdtree_flann.h:132
pcl::KdTreeFLANN::nearestKSearch
int nearestKSearch(const PointT &point, unsigned int k, Indices &k_indices, std::vector< float > &k_sqr_distances) const override
Search for k-nearest neighbors for the given query point.
Definition: kdtree_flann.hpp:233
pcl::KdTreeFLANN::setInputCloud
void setInputCloud(const PointCloudConstPtr &cloud, const IndicesConstPtr &indices=IndicesConstPtr()) override
Provide a pointer to the input dataset.
Definition: kdtree_flann.hpp:102
pcl::PCLBase
PCL base class.
Definition: pcl_base.h:70
pcl::StatisticalMultiscaleInterestRegionExtraction
Class for extracting interest regions from unstructured point clouds, based on a multi scale statisti...
Definition: statistical_multiscale_interest_region_extraction.h:65
pcl::StatisticalMultiscaleInterestRegionExtraction::computeRegionsOfInterest
void computeRegionsOfInterest(std::list< IndicesPtr > &rois)
The method to be called in order to run the algorithm and produce the resulting set of regions of int...
Definition: statistical_multiscale_interest_region_extraction.hpp:122
pcl::StatisticalMultiscaleInterestRegionExtraction::generateCloudGraph
void generateCloudGraph()
Method that generates the underlying nearest neighbor graph based on the input point cloud.
Definition: statistical_multiscale_interest_region_extraction.hpp:53
distances.h
Define standard C methods to do distance calculations.
boost
Definition: boost_graph.h:48
pcl::Indices
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition: types.h:133
pcl::IndicesPtr
shared_ptr< Indices > IndicesPtr
Definition: pcl_base.h:58
M_PI
#define M_PI
Definition: pcl_macros.h:201
pcl::PointXYZRGB
A point structure representing Euclidean xyz coordinates, and the RGB color.
Definition: point_types.hpp:675