susan.hpp
/*
* Software License Agreement (BSD License)
*
* Point Cloud Library (PCL) - www.pointclouds.org
* Copyright (c) 2010-2011, Willow Garage, 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 Willow Garage, Inc. 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.
*
*/
#ifndef PCL_SUSAN_IMPL_HPP_
#define PCL_SUSAN_IMPL_HPP_
#include <pcl/keypoints/susan.h>
#include <pcl/features/normal_3d.h>
#include <pcl/features/integral_image_normal.h>
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointInT, typename PointOutT, typename NormalT, typename IntensityT> void
pcl::SUSANKeypoint<PointInT, PointOutT, NormalT, IntensityT>::setNonMaxSupression (bool nonmax)
{
nonmax_ = nonmax;
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointInT, typename PointOutT, typename NormalT, typename IntensityT> void
pcl::SUSANKeypoint<PointInT, PointOutT, NormalT, IntensityT>::setGeometricValidation (bool validate)
{
geometric_validation_ = validate;
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointInT, typename PointOutT, typename NormalT, typename IntensityT> void
pcl::SUSANKeypoint<PointInT, PointOutT, NormalT, IntensityT>::setRadius (float radius)
{
search_radius_ = radius;
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointInT, typename PointOutT, typename NormalT, typename IntensityT> void
pcl::SUSANKeypoint<PointInT, PointOutT, NormalT, IntensityT>::setDistanceThreshold (float distance_threshold)
{
distance_threshold_ = distance_threshold;
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointInT, typename PointOutT, typename NormalT, typename IntensityT> void
pcl::SUSANKeypoint<PointInT, PointOutT, NormalT, IntensityT>::setAngularThreshold (float angular_threshold)
{
angular_threshold_ = angular_threshold;
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointInT, typename PointOutT, typename NormalT, typename IntensityT> void
pcl::SUSANKeypoint<PointInT, PointOutT, NormalT, IntensityT>::setIntensityThreshold (float intensity_threshold)
{
intensity_threshold_ = intensity_threshold;
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointInT, typename PointOutT, typename NormalT, typename IntensityT> void
pcl::SUSANKeypoint<PointInT, PointOutT, NormalT, IntensityT>::setNormals (const PointCloudNConstPtr &normals)
{
normals_ = normals;
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointInT, typename PointOutT, typename NormalT, typename IntensityT> void
pcl::SUSANKeypoint<PointInT, PointOutT, NormalT, IntensityT>::setSearchSurface (const PointCloudInConstPtr &cloud)
{
surface_ = cloud;
normals_.reset (new pcl::PointCloud<NormalT>);
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointInT, typename PointOutT, typename NormalT, typename IntensityT> void
pcl::SUSANKeypoint<PointInT, PointOutT, NormalT, IntensityT>::setNumberOfThreads (unsigned int nr_threads)
{
threads_ = nr_threads;
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// template <typename PointInT, typename PointOutT, typename NormalT, typename IntensityT> void
// pcl::SUSANKeypoint<PointInT, PointOutT, NormalT, IntensityT>::USAN (const PointInT& nucleus,
// const NormalT& nucleus_normal,
// const std::vector<int>& neighbors,
// const float& t,
// float& response,
// Eigen::Vector3f& centroid) const
// {
// float area = 0;
// response = 0;
// float x0 = nucleus.x;
// float y0 = nucleus.y;
// float z0 = nucleus.z;
// //xx xy xz yy yz zz
// std::vector<float> coefficients(6);
// memset (&coefficients[0], 0, sizeof (float) * 6);
// for (std::vector<int>::const_iterator index = neighbors.begin(); index != neighbors.end(); ++index)
// {
// if (pcl_isfinite (normals_->points[*index].normal_x))
// {
// Eigen::Vector3f diff = normals_->points[*index].getNormal3fMap () - nucleus_normal.getNormal3fMap ();
// float c = diff.norm () / t;
// c = -1 * pow (c, 6.0);
// c = exp (c);
// Eigen::Vector3f xyz = surface_->points[*index].getVector3fMap ();
// centroid += c * xyz;
// area += c;
// coefficients[0] += c * (x0 - xyz.x) * (x0 - xyz.x);
// coefficients[1] += c * (x0 - xyz.x) * (y0 - xyz.y);
// coefficients[2] += c * (x0 - xyz.x) * (z0 - xyz.z);
// coefficients[3] += c * (y0 - xyz.y) * (y0 - xyz.y);
// coefficients[4] += c * (y0 - xyz.y) * (z0 - xyz.z);
// coefficients[5] += c * (z0 - xyz.z) * (z0 - xyz.z);
// }
// }
// if (area > 0)
// {
// centroid /= area;
// if (area < geometric_threshold)
// response = geometric_threshold - area;
// // Look for edge direction
// // X direction
// if ((coefficients[3]/coefficients[0]) < 1 && (coefficients[5]/coefficients[0]) < 1)
// direction = Eigen::Vector3f (1, 0, 0);
// else
// {
// // Y direction
// if ((coefficients[0]/coefficients[3]) < 1 && (coefficients[5]/coefficients[3]) < 1)
// direction = Eigen::Vector3f (0, 1, 0);
// else
// {
// // Z direction
// if ((coefficients[0]/coefficients[5]) < 1 && (coefficients[3]/coefficients[5]) < 1)
// direction = Eigen::Vector3f (0, 1, 0);
// // Diagonal edge
// else
// {
// //XY direction
// if ((coefficients[2]/coeffcients[1]) < 1 && (coeffcients[4]/coeffcients[1]) < 1)
// {
// if (coeffcients[1] > 0)
// direction = Eigen::Vector3f (1,1,0);
// else
// direction = Eigen::Vector3f (-1,1,0);
// }
// else
// {
// //XZ direction
// if ((coefficients[1]/coeffcients[2]) > 1 && (coeffcients[4]/coeffcients[2]) < 1)
// {
// if (coeffcients[2] > 0)
// direction = Eigen::Vector3f (1,0,1);
// else
// direction = Eigen::Vector3f (-1,0,1);
// }
// //YZ direction
// else
// {
// if (coeffcients[4] > 0)
// direction = Eigen::Vector3f (0,1,1);
// else
// direction = Eigen::Vector3f (0,-1,1);
// }
// }
// }
// }
// }
// // std::size_t max_index = std::distance (coefficients.begin (), max);
// // switch (max_index)
// // {
// // case 0 : direction = Eigen::Vector3f (1, 0, 0); break;
// // case 1 : direction = Eigen::Vector3f (1, 1, 0); break;
// // case 2 : direction = Eigen::Vector3f (1, 0, 1); break;
// // case 3 : direction = Eigen::Vector3f (0, 1, 0); break;
// // case 4 : direction = Eigen::Vector3f (0, 1, 1); break;
// // case 5 : direction = Eigen::Vector3f (0, 0, 1); break;
// // }
// }
// }
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointInT, typename PointOutT, typename NormalT, typename IntensityT> bool
pcl::SUSANKeypoint<PointInT, PointOutT, NormalT, IntensityT>::initCompute ()
{
if (!Keypoint<PointInT, PointOutT>::initCompute ())
{
PCL_ERROR ("[pcl::%s::initCompute] init failed!\n", name_.c_str ());
return (false);
}
if (normals_->empty ())
{
PointCloudNPtr normals (new PointCloudN ());
normals->reserve (normals->size ());
if (!surface_->isOrganized ())
{
pcl::NormalEstimation<PointInT, NormalT> normal_estimation;
normal_estimation.setInputCloud (surface_);
normal_estimation.setRadiusSearch (search_radius_);
normal_estimation.compute (*normals);
}
else
{
IntegralImageNormalEstimation<PointInT, NormalT> normal_estimation;
normal_estimation.setNormalEstimationMethod (pcl::IntegralImageNormalEstimation<PointInT, NormalT>::SIMPLE_3D_GRADIENT);
normal_estimation.setInputCloud (surface_);
normal_estimation.setNormalSmoothingSize (5.0);
normal_estimation.compute (*normals);
}
normals_ = normals;
}
if (normals_->size () != surface_->size ())
{
PCL_ERROR ("[pcl::%s::initCompute] normals given, but the number of normals does not match the number of input points!\n", name_.c_str ());
return (false);
}
return (true);
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointInT, typename PointOutT, typename NormalT, typename IntensityT> bool
pcl::SUSANKeypoint<PointInT, PointOutT, NormalT, IntensityT>::isWithinNucleusCentroid (const Eigen::Vector3f& nucleus,
const Eigen::Vector3f& centroid,
const Eigen::Vector3f& nc,
const PointInT& point) const
{
Eigen::Vector3f pc = centroid - point.getVector3fMap ();
Eigen::Vector3f pn = nucleus - point.getVector3fMap ();
Eigen::Vector3f pc_cross_nc = pc.cross (nc);
return ((pc_cross_nc.norm () <= tolerance_) && (pc.dot (nc) >= 0) && (pn.dot (nc) <= 0));
}
// template <typename PointInT, typename PointOutT, typename NormalT, typename IntensityT> bool
// pcl::SUSANKeypoint<PointInT, PointOutT, NormalT, IntensityT>::isValidQueryPoint3D (int point_index) const
// {
// return (isFinite (surface_->points [point_index]) &&
// isFinite (normals_->points [point_index]));
// }
// template <typename PointInT, typename PointOutT, typename NormalT, typename IntensityT> bool
// pcl::SUSANKeypoint<PointInT, PointOutT, NormalT, IntensityT>::isValidQueryPoint2D (int point_index) const
// {
// return (isFinite (surface_->points [point_index]));
// }
// template <typename PointInT, typename PointOutT, typename NormalT, typename IntensityT> bool
// pcl::SUSANKeypoint<PointInT, PointOutT, NormalT, IntensityT>::isWithinSusan2D (int nucleus, int neighbor) const
// {
// return (fabs (intensity_ (surface_->points[nucleus]) -
// intensity_ (surface_->points[neighbor])) <= intensity_threshold_);
// }
// template <typename PointInT, typename PointOutT, typename NormalT, typename IntensityT> bool
// pcl::SUSANKeypoint<PointInT, PointOutT, NormalT, IntensityT>::isWithinSusan3D (int nucleus, int neighbor) const
// {
// Eigen::Vector3f nucleus_normal = normals_->point[nucleus].getVector3fMap ();
// return (1 - nucleus_normal.dot (normals_->points[*index].getNormalVector3fMap ()) <= angular_threshold_);
// }
// template <typename PointInT, typename PointOutT, typename NormalT, typename IntensityT> bool
// pcl::SUSANKeypoint<PointInT, PointOutT, NormalT, IntensityT>::isWithinSusanH (int nucleus, int neighbor) const
// {
// Eigen::Vector3f nucleus_normal = normals_->point[nucleus].getVector3fMap ();
// return ((1 - nucleus_normal.dot (normals_->points[*index].getNormalVector3fMap ()) <= angular_threshold_) ||
// (fabs (intensity_ (surface_->points[nucleus]) - intensity_ (surface_->points[neighbor])) <= intensity_threshold_));
// }
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointInT, typename PointOutT, typename NormalT, typename IntensityT> void
pcl::SUSANKeypoint<PointInT, PointOutT, NormalT, IntensityT>::detectKeypoints (PointCloudOut &output)
{
boost::shared_ptr<pcl::PointCloud<PointOutT> > response (new pcl::PointCloud<PointOutT> ());
response->reserve (surface_->size ());
// Check if the output has a "label" field
label_idx_ = pcl::getFieldIndex<PointOutT> (output, "label", out_fields_);
const int input_size = static_cast<int> (input_->size ());
//#ifdef _OPENMP
//#pragma omp parallel for shared (response) num_threads(threads_)
//#endif
for (int point_index = 0; point_index < input_size; ++point_index)
{
const PointInT& point_in = input_->points [point_index];
const NormalT& normal_in = normals_->points [point_index];
if (!isFinite (point_in) || !isFinite (normal_in))
continue;
else
{
Eigen::Vector3f nucleus = point_in.getVector3fMap ();
Eigen::Vector3f nucleus_normal = normals_->points [point_index].getNormalVector3fMap ();
float nucleus_intensity = intensity_ (point_in);
std::vector<int> nn_indices;
std::vector<float> nn_dists;
tree_->radiusSearch (point_in, search_radius_, nn_indices, nn_dists);
float area = 0;
Eigen::Vector3f centroid = Eigen::Vector3f::Zero ();
// Exclude nucleus from the usan
std::vector<int> usan; usan.reserve (nn_indices.size () - 1);
for (std::vector<int>::const_iterator index = nn_indices.begin(); index != nn_indices.end(); ++index)
{
if ((*index != point_index) && pcl_isfinite (normals_->points[*index].normal_x))
{
// if the point fulfill condition
if ((fabs (nucleus_intensity - intensity_ (input_->points[*index])) <= intensity_threshold_) ||
(1 - nucleus_normal.dot (normals_->points[*index].getNormalVector3fMap ()) <= angular_threshold_))
{
++area;
centroid += input_->points[*index].getVector3fMap ();
usan.push_back (*index);
}
}
}
float geometric_threshold = 0.5f * (static_cast<float> (nn_indices.size () - 1));
if ((area > 0) && (area < geometric_threshold))
{
// if no geometric validation required add the point to the response
if (!geometric_validation_)
{
PointOutT point_out;
point_out.getVector3fMap () = point_in.getVector3fMap ();
//point_out.intensity = geometric_threshold - area;
intensity_out_.set (point_out, geometric_threshold - area);
// if a label field is found use it to save the index
if (label_idx_ != -1)
{
// save the index in the cloud
uint32_t label = static_cast<uint32_t> (point_index);
memcpy (reinterpret_cast<char*> (&point_out) + out_fields_[label_idx_].offset,
&label, sizeof (uint32_t));
}
//#ifdef _OPENMP
//#pragma omp critical
//#endif
response->push_back (point_out);
}
else
{
centroid /= area;
Eigen::Vector3f dist = nucleus - centroid;
// Check the distance <= distance_threshold_
if (dist.norm () >= distance_threshold_)
{
// point is valid from distance point of view
Eigen::Vector3f nc = centroid - nucleus;
// Check the contiguity
std::vector<int>::const_iterator usan_it = usan.begin ();
for (; usan_it != usan.end (); ++usan_it)
{
if (!isWithinNucleusCentroid (nucleus, centroid, nc, input_->points[*usan_it]))
break;
}
// All points within usan lies on the segment [nucleus centroid]
if (usan_it == usan.end ())
{
PointOutT point_out;
point_out.getVector3fMap () = point_in.getVector3fMap ();
// point_out.intensity = geometric_threshold - area;
intensity_out_.set (point_out, geometric_threshold - area);
// if a label field is found use it to save the index
if (label_idx_ != -1)
{
// save the index in the cloud
uint32_t label = static_cast<uint32_t> (point_index);
memcpy (reinterpret_cast<char*> (&point_out) + out_fields_[label_idx_].offset,
&label, sizeof (uint32_t));
}
//#ifdef _OPENMP
//#pragma omp critical
//#endif
response->push_back (point_out);
}
}
}
}
}
}
response->height = 1;
response->width = static_cast<uint32_t> (response->size ());
if (!nonmax_)
{
output = *response;
for (size_t i = 0; i < response->size (); ++i)
keypoints_indices_->indices.push_back (i);
// we don not change the denseness
output.is_dense = input_->is_dense;
}
else
{
output.points.clear ();
output.points.reserve (response->points.size());
//#ifdef _OPENMP
//#pragma omp parallel for shared (output) num_threads(threads_)
//#endif
for (int idx = 0; idx < static_cast<int> (response->points.size ()); ++idx)
{
const PointOutT& point_in = response->points [idx];
const NormalT& normal_in = normals_->points [idx];
//const float intensity = response->points[idx].intensity;
const float intensity = intensity_out_ (response->points[idx]);
if (!isFinite (point_in) || !isFinite (normal_in) || (intensity == 0))
continue;
std::vector<int> nn_indices;
std::vector<float> nn_dists;
tree_->radiusSearch (idx, search_radius_, nn_indices, nn_dists);
bool is_minima = true;
for (std::vector<int>::const_iterator nn_it = nn_indices.begin(); nn_it != nn_indices.end(); ++nn_it)
{
// if (intensity > response->points[*nn_it].intensity)
if (intensity > intensity_out_ (response->points[*nn_it]))
{
is_minima = false;
break;
}
}
if (is_minima)
//#ifdef _OPENMP
//#pragma omp critical
//#endif
{
output.points.push_back (response->points[idx]);
keypoints_indices_->indices.push_back (idx);
}
}
output.height = 1;
output.width = static_cast<uint32_t> (output.points.size());
output.is_dense = true;
}
}
#define PCL_INSTANTIATE_SUSAN(T,U,N) template class PCL_EXPORTS pcl::SUSANKeypoint<T,U,N>;
#endif // #ifndef PCL_HARRIS_KEYPOINT_3D_IMPL_H_
pcl::SUSANKeypoint::setNormals
void setNormals(const PointCloudNConstPtr &normals)
set normals if precalculated normals are available.
Definition: susan.hpp:89
pcl::Normal
A point structure representing normal coordinates and the surface curvature estimate.
Definition: point_types.hpp:765
pcl::SUSANKeypoint::setRadius
void setRadius(float radius)
set the radius for normal estimation and non maxima supression.
Definition: susan.hpp:61
pcl::isFinite
bool isFinite(const PointT &pt)
Tests if the 3D components of a point are all finite param[in] pt point to be tested.
Definition: point_tests.h:53
pcl::SUSANKeypoint::setSearchSurface
virtual void setSearchSurface(const PointCloudInConstPtr &cloud)
Definition: susan.hpp:96
pcl::SUSANKeypoint::initCompute
bool initCompute()
Definition: susan.hpp:215
pcl::SUSANKeypoint::setNumberOfThreads
void setNumberOfThreads(unsigned int nr_threads)
Initialize the scheduler and set the number of threads to use.
Definition: susan.hpp:104
pcl::Keypoint::PointCloudInConstPtr
PointCloudIn::ConstPtr PointCloudInConstPtr
Definition: keypoint.h:69
pcl::SUSANKeypoint::setDistanceThreshold
void setDistanceThreshold(float distance_threshold)
Definition: susan.hpp:68
pcl::Feature::setRadiusSearch
void setRadiusSearch(double radius)
Set the sphere radius that is to be used for determining the nearest neighbors used for the feature e...
Definition: feature.h:200
pcl::NormalEstimation
NormalEstimation estimates local surface properties (surface normals and curvatures)at each 3D point...
Definition: normal_3d.h:197
pcl::Keypoint
Keypoint represents the base class for key points.
Definition: keypoint.h:55
pcl::SUSANKeypoint::setGeometricValidation
void setGeometricValidation(bool validate)
Filetr false positive using geometric criteria.
Definition: susan.hpp:54
pcl::SUSANKeypoint::PointCloudNPtr
PointCloudN::Ptr PointCloudNPtr
Definition: susan.h:69
pcl::SUSANKeypoint::setAngularThreshold
void setAngularThreshold(float angular_threshold)
set the angular_threshold value for detecting corners.
Definition: susan.hpp:75
pcl::IntegralImageNormalEstimation
Surface normal estimation on organized data using integral images.
Definition: integral_image_normal.h:68
pcl::SUSANKeypoint::isWithinNucleusCentroid
bool isWithinNucleusCentroid(const Eigen::Vector3f &nucleus, const Eigen::Vector3f &centroid, const Eigen::Vector3f &nc, const PointInT &point) const
return true if a point lies within the line between the nucleus and the centroid
Definition: susan.hpp:255
pcl::NormalEstimation::setInputCloud
virtual void setInputCloud(const PointCloudConstPtr &cloud)
Provide a pointer to the input dataset.
Definition: normal_3d.h:286
pcl::SUSANKeypoint::detectKeypoints
void detectKeypoints(PointCloudOut &output)
Abstract key point detection method.
Definition: susan.hpp:303
pcl::PointCloud< NormalT >
pcl::IntegralImageNormalEstimation::setNormalEstimationMethod
void setNormalEstimationMethod(NormalEstimationMethod normal_estimation_method)
Set the normal estimation method.
Definition: integral_image_normal.h:217
pcl::IntegralImageNormalEstimation::setInputCloud
virtual void setInputCloud(const typename PointCloudIn::ConstPtr &cloud)
Provide a pointer to the input dataset (overwrites the PCLBase::setInputCloud method) ...
Definition: integral_image_normal.h:235
pcl::SUSANKeypoint::setNonMaxSupression
void setNonMaxSupression(bool nonmax)
Apply non maxima suppression to the responses to keep strongest corners.
Definition: susan.hpp:47
pcl::Feature::compute
void compute(PointCloudOut &output)
Base method for feature estimation for all points given in <setInputCloud (), setIndices ()> using th...
Definition: feature.hpp:189
pcl::SUSANKeypoint::PointCloudNConstPtr
PointCloudN::ConstPtr PointCloudNConstPtr
Definition: susan.h:70
pcl::IntegralImageNormalEstimation::setNormalSmoothingSize
void setNormalSmoothingSize(float normal_smoothing_size)
Set the normal smoothing size.
Definition: integral_image_normal.h:193
pcl::SUSANKeypoint::setIntensityThreshold
void setIntensityThreshold(float intensity_threshold)
set the intensity_threshold value for detecting corners.
Definition: susan.hpp:82