rsd.hpp
/*
* Software License Agreement (BSD License)
*
* Point Cloud Library (PCL) - www.pointclouds.org
* Copyright (c) 2009, 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_FEATURES_IMPL_RSD_H_
#define PCL_FEATURES_IMPL_RSD_H_
#include <cfloat>
#include <pcl/features/rsd.h>
//////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointInT, typename PointNT, typename PointOutT> Eigen::MatrixXf
pcl::computeRSD (const pcl::PointCloud<PointInT> &surface, const pcl::PointCloud<PointNT> &normals,
const std::vector<int> &indices, double max_dist,
int nr_subdiv, double plane_radius, PointOutT &radii, bool compute_histogram)
{
// Check if the full histogram has to be saved or not
Eigen::MatrixXf histogram;
if (compute_histogram)
histogram = Eigen::MatrixXf::Zero (nr_subdiv, nr_subdiv);
// Check if enough points are provided or not
if (indices.size () < 2)
{
radii.r_max = 0;
radii.r_min = 0;
return histogram;
}
// Initialize minimum and maximum angle values in each distance bin
std::vector<std::vector<double> > min_max_angle_by_dist (nr_subdiv);
for (auto& minmax: min_max_angle_by_dist)
{
minmax.resize (2);
minmax[0] = std::numeric_limits<double>::max();
minmax[1] = -std::numeric_limits<double>::max();
}
min_max_angle_by_dist[0][0] = min_max_angle_by_dist[0][1] = 0.0;
// Compute distance by normal angle distribution for points
std::vector<int>::const_iterator i, begin (indices.begin()), end (indices.end());
for (i = begin+1; i != end; ++i)
{
// compute angle between the two lines going through normals (disregard orientation!)
double cosine = normals.points[*i].normal[0] * normals.points[*begin].normal[0] +
normals.points[*i].normal[1] * normals.points[*begin].normal[1] +
normals.points[*i].normal[2] * normals.points[*begin].normal[2];
if (cosine > 1) cosine = 1;
if (cosine < -1) cosine = -1;
double angle = std::acos (cosine);
if (angle > M_PI/2) angle = M_PI - angle; /// \note: orientation is neglected!
// Compute point to point distance
double dist = sqrt ((surface.points[*i].x - surface.points[*begin].x) * (surface.points[*i].x - surface.points[*begin].x) +
(surface.points[*i].y - surface.points[*begin].y) * (surface.points[*i].y - surface.points[*begin].y) +
(surface.points[*i].z - surface.points[*begin].z) * (surface.points[*i].z - surface.points[*begin].z));
if (dist > max_dist)
continue; /// \note: we neglect points that are outside the specified interval!
// compute bins and increase
int bin_d = static_cast<int> (std::floor (nr_subdiv * dist / max_dist));
if (compute_histogram)
{
int bin_a = std::min (nr_subdiv-1, static_cast<int> (std::floor (nr_subdiv * angle / (M_PI/2))));
histogram(bin_a, bin_d)++;
}
// update min-max values for distance bins
if (min_max_angle_by_dist[bin_d][0] > angle) min_max_angle_by_dist[bin_d][0] = angle;
if (min_max_angle_by_dist[bin_d][1] < angle) min_max_angle_by_dist[bin_d][1] = angle;
}
// Estimate radius from min and max lines
double Amint_Amin = 0, Amint_d = 0;
double Amaxt_Amax = 0, Amaxt_d = 0;
for (int di=0; di<nr_subdiv; di++)
{
// combute the members of A'*A*r = A'*D
if (min_max_angle_by_dist[di][1] >= 0)
{
double p_min = min_max_angle_by_dist[di][0];
double p_max = min_max_angle_by_dist[di][1];
double f = (di+0.5)*max_dist/nr_subdiv;
Amint_Amin += p_min * p_min;
Amint_d += p_min * f;
Amaxt_Amax += p_max * p_max;
Amaxt_d += p_max * f;
}
}
float min_radius = Amint_Amin == 0.0f ? float (plane_radius) : float (std::min (Amint_d/Amint_Amin, plane_radius));
float max_radius = Amaxt_Amax == 0.0f ? float (plane_radius) : float (std::min (Amaxt_d/Amaxt_Amax, plane_radius));
// Small correction of the systematic error of the estimation (based on analysis with nr_subdiv_ = 5)
min_radius *= 1.1f;
max_radius *= 0.9f;
if (min_radius < max_radius)
{
radii.r_min = min_radius;
radii.r_max = max_radius;
}
else
{
radii.r_max = min_radius;
radii.r_min = max_radius;
}
return histogram;
}
//////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointNT, typename PointOutT> Eigen::MatrixXf
pcl::computeRSD (const pcl::PointCloud<PointNT> &normals,
const std::vector<int> &indices, const std::vector<float> &sqr_dists, double max_dist,
int nr_subdiv, double plane_radius, PointOutT &radii, bool compute_histogram)
{
// Check if the full histogram has to be saved or not
Eigen::MatrixXf histogram;
if (compute_histogram)
histogram = Eigen::MatrixXf::Zero (nr_subdiv, nr_subdiv);
// Check if enough points are provided or not
if (indices.size () < 2)
{
radii.r_max = 0;
radii.r_min = 0;
return histogram;
}
// Initialize minimum and maximum angle values in each distance bin
std::vector<std::vector<double> > min_max_angle_by_dist (nr_subdiv);
min_max_angle_by_dist[0].resize (2);
min_max_angle_by_dist[0][0] = min_max_angle_by_dist[0][1] = 0.0;
for (int di=1; di<nr_subdiv; di++)
{
min_max_angle_by_dist[di].resize (2);
min_max_angle_by_dist[di][0] = +DBL_MAX;
min_max_angle_by_dist[di][1] = -DBL_MAX;
}
// Compute distance by normal angle distribution for points
std::vector<int>::const_iterator i, begin (indices.begin()), end (indices.end());
for (i = begin+1; i != end; ++i)
{
// compute angle between the two lines going through normals (disregard orientation!)
double cosine = normals.points[*i].normal[0] * normals.points[*begin].normal[0] +
normals.points[*i].normal[1] * normals.points[*begin].normal[1] +
normals.points[*i].normal[2] * normals.points[*begin].normal[2];
if (cosine > 1) cosine = 1;
if (cosine < -1) cosine = -1;
double angle = std::acos (cosine);
if (angle > M_PI/2) angle = M_PI - angle; /// \note: orientation is neglected!
// Compute point to point distance
double dist = sqrt (sqr_dists[i-begin]);
if (dist > max_dist)
continue; /// \note: we neglect points that are outside the specified interval!
// compute bins and increase
int bin_d = static_cast<int> (std::floor (nr_subdiv * dist / max_dist));
if (compute_histogram)
{
int bin_a = std::min (nr_subdiv-1, static_cast<int> (std::floor (nr_subdiv * angle / (M_PI/2))));
histogram(bin_a, bin_d)++;
}
// update min-max values for distance bins
if (min_max_angle_by_dist[bin_d][0] > angle) min_max_angle_by_dist[bin_d][0] = angle;
if (min_max_angle_by_dist[bin_d][1] < angle) min_max_angle_by_dist[bin_d][1] = angle;
}
// Estimate radius from min and max lines
double Amint_Amin = 0, Amint_d = 0;
double Amaxt_Amax = 0, Amaxt_d = 0;
for (int di=0; di<nr_subdiv; di++)
{
// combute the members of A'*A*r = A'*D
if (min_max_angle_by_dist[di][1] >= 0)
{
double p_min = min_max_angle_by_dist[di][0];
double p_max = min_max_angle_by_dist[di][1];
double f = (di+0.5)*max_dist/nr_subdiv;
Amint_Amin += p_min * p_min;
Amint_d += p_min * f;
Amaxt_Amax += p_max * p_max;
Amaxt_d += p_max * f;
}
}
float min_radius = Amint_Amin == 0.0f ? float (plane_radius) : float (std::min (Amint_d/Amint_Amin, plane_radius));
float max_radius = Amaxt_Amax == 0.0f ? float (plane_radius) : float (std::min (Amaxt_d/Amaxt_Amax, plane_radius));
// Small correction of the systematic error of the estimation (based on analysis with nr_subdiv_ = 5)
min_radius *= 1.1f;
max_radius *= 0.9f;
if (min_radius < max_radius)
{
radii.r_min = min_radius;
radii.r_max = max_radius;
}
else
{
radii.r_max = min_radius;
radii.r_min = max_radius;
}
return histogram;
}
//////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointInT, typename PointNT, typename PointOutT> void
pcl::RSDEstimation<PointInT, PointNT, PointOutT>::computeFeature (PointCloudOut &output)
{
// Check if search_radius_ was set
if (search_radius_ < 0)
{
PCL_ERROR ("[pcl::%s::computeFeature] A search radius needs to be set!\n", getClassName ().c_str ());
output.width = output.height = 0;
output.points.clear ();
return;
}
// List of indices and corresponding squared distances for a neighborhood
// \note resize is irrelevant for a radiusSearch ().
std::vector<int> nn_indices;
std::vector<float> nn_sqr_dists;
// Check if the full histogram has to be saved or not
if (save_histograms_)
{
// Reserve space for the output histogram dataset
histograms_.reset (new std::vector<Eigen::MatrixXf, Eigen::aligned_allocator<Eigen::MatrixXf> >);
histograms_->reserve (output.points.size ());
// Iterating over the entire index vector
for (std::size_t idx = 0; idx < indices_->size (); ++idx)
{
// Compute and store r_min and r_max in the output cloud
this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_sqr_dists);
//histograms_->push_back (computeRSD (*surface_, *normals_, nn_indices, search_radius_, nr_subdiv_, plane_radius_, output.points[idx], true));
histograms_->push_back (computeRSD (*normals_, nn_indices, nn_sqr_dists, search_radius_, nr_subdiv_, plane_radius_, output.points[idx], true));
}
}
else
{
// Iterating over the entire index vector
for (std::size_t idx = 0; idx < indices_->size (); ++idx)
{
// Compute and store r_min and r_max in the output cloud
this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_sqr_dists);
//computeRSD (*surface_, *normals_, nn_indices, search_radius_, nr_subdiv_, plane_radius_, output.points[idx], false);
computeRSD (*normals_, nn_indices, nn_sqr_dists, search_radius_, nr_subdiv_, plane_radius_, output.points[idx], false);
}
}
}
#define PCL_INSTANTIATE_RSDEstimation(T,NT,OutT) template class PCL_EXPORTS pcl::RSDEstimation<T,NT,OutT>;
#endif // PCL_FEATURES_IMPL_RSD_H_
pcl::PointCloud::height
std::uint32_t height
The point cloud height (if organized as an image-structure).
Definition: point_cloud.h:402
pcl::PointCloud::points
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
Definition: point_cloud.h:397
pcl::RSDEstimation::computeFeature
void computeFeature(PointCloudOut &output) override
Estimate the estimates the Radius-based Surface Descriptor (RSD) at a set of points given by <setInpu...
Definition: rsd.hpp:248
pcl::PointCloud< PointInT >
pcl::PointCloud::width
std::uint32_t width
The point cloud width (if organized as an image-structure).
Definition: point_cloud.h:400
pcl::computeRSD
Eigen::MatrixXf computeRSD(const pcl::PointCloud< PointInT > &surface, const pcl::PointCloud< PointNT > &normals, const std::vector< int > &indices, double max_dist, int nr_subdiv, double plane_radius, PointOutT &radii, bool compute_histogram=false)
Estimate the Radius-based Surface Descriptor (RSD) for a given point based on its spatial neighborhoo...
Definition: rsd.hpp:49