255 lines
9.5 KiB
C++
255 lines
9.5 KiB
C++
/*
|
|
* Software License Agreement (BSD License)
|
|
*
|
|
* Copyright (c) 2011, Alexandru-Eugen Ichim
|
|
* 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.
|
|
*
|
|
* $Id$
|
|
*/
|
|
|
|
#ifndef PCL_KEYPOINTS_IMPL_SMOOTHEDSURFACESKEYPOINT_H_
|
|
#define PCL_KEYPOINTS_IMPL_SMOOTHEDSURFACESKEYPOINT_H_
|
|
|
|
#include <pcl/keypoints/smoothed_surfaces_keypoint.h>
|
|
|
|
//#include <pcl/io/pcd_io.h>
|
|
|
|
//////////////////////////////////////////////////////////////////////////////////////////////
|
|
template <typename PointT, typename PointNT> void
|
|
pcl::SmoothedSurfacesKeypoint<PointT, PointNT>::addSmoothedPointCloud (const PointCloudTConstPtr &cloud,
|
|
const PointCloudNTConstPtr &normals,
|
|
KdTreePtr &kdtree,
|
|
float &scale)
|
|
{
|
|
clouds_.push_back (cloud);
|
|
cloud_normals_.push_back (normals);
|
|
cloud_trees_.push_back (kdtree);
|
|
scales_.push_back (std::pair<float, std::size_t> (scale, scales_.size ()));
|
|
}
|
|
|
|
|
|
//////////////////////////////////////////////////////////////////////////////////////////////
|
|
template <typename PointT, typename PointNT> void
|
|
pcl::SmoothedSurfacesKeypoint<PointT, PointNT>::resetClouds ()
|
|
{
|
|
clouds_.clear ();
|
|
cloud_normals_.clear ();
|
|
scales_.clear ();
|
|
}
|
|
|
|
|
|
//////////////////////////////////////////////////////////////////////////////////////////////
|
|
template <typename PointT, typename PointNT> void
|
|
pcl::SmoothedSurfacesKeypoint<PointT, PointNT>::detectKeypoints (PointCloudT &output)
|
|
{
|
|
// Calculate differences for each cloud
|
|
std::vector<std::vector<float> > diffs (scales_.size ());
|
|
|
|
// The cloud with the smallest scale has no differences
|
|
std::vector<float> aux_diffs (input_->size (), 0.0f);
|
|
diffs[scales_[0].second] = aux_diffs;
|
|
|
|
cloud_trees_[scales_[0].second]->setInputCloud (clouds_[scales_[0].second]);
|
|
for (std::size_t scale_i = 1; scale_i < clouds_.size (); ++scale_i)
|
|
{
|
|
std::size_t cloud_i = scales_[scale_i].second,
|
|
cloud_i_minus_one = scales_[scale_i - 1].second;
|
|
diffs[cloud_i].resize (input_->size ());
|
|
PCL_INFO ("cloud_i %u cloud_i_minus_one %u\n", cloud_i, cloud_i_minus_one);
|
|
for (std::size_t point_i = 0; point_i < input_->size (); ++point_i)
|
|
diffs[cloud_i][point_i] = (*cloud_normals_[cloud_i])[point_i].getNormalVector3fMap ().dot (
|
|
(*clouds_[cloud_i])[point_i].getVector3fMap () - (*clouds_[cloud_i_minus_one])[point_i].getVector3fMap ());
|
|
|
|
// Setup kdtree for this cloud
|
|
cloud_trees_[cloud_i]->setInputCloud (clouds_[cloud_i]);
|
|
}
|
|
|
|
|
|
// Find minima and maxima in differences inside the input cloud
|
|
typename pcl::search::Search<PointT>::Ptr input_tree = cloud_trees_.back ();
|
|
for (pcl::index_t point_i = 0; point_i < static_cast<pcl::index_t> (input_->size ()); ++point_i)
|
|
{
|
|
pcl::Indices nn_indices;
|
|
std::vector<float> nn_distances;
|
|
input_tree->radiusSearch (point_i, input_scale_ * neighborhood_constant_, nn_indices, nn_distances);
|
|
|
|
bool is_min = true, is_max = true;
|
|
for (const auto &nn_index : nn_indices)
|
|
if (nn_index != point_i)
|
|
{
|
|
if (diffs[input_index_][point_i] < diffs[input_index_][nn_index])
|
|
is_max = false;
|
|
else if (diffs[input_index_][point_i] > diffs[input_index_][nn_index])
|
|
is_min = false;
|
|
}
|
|
|
|
// If the point is a local minimum/maximum, check if it is the same over all the scales
|
|
if (is_min || is_max)
|
|
{
|
|
bool passed_min = true, passed_max = true;
|
|
for (std::size_t scale_i = 0; scale_i < scales_.size (); ++scale_i)
|
|
{
|
|
std::size_t cloud_i = scales_[scale_i].second;
|
|
// skip input cloud
|
|
if (cloud_i == clouds_.size () - 1)
|
|
continue;
|
|
|
|
nn_indices.clear (); nn_distances.clear ();
|
|
cloud_trees_[cloud_i]->radiusSearch (point_i, scales_[scale_i].first * neighborhood_constant_, nn_indices, nn_distances);
|
|
|
|
bool is_min_other_scale = true, is_max_other_scale = true;
|
|
for (const auto &nn_index : nn_indices)
|
|
if (nn_index != point_i)
|
|
{
|
|
if (diffs[input_index_][point_i] < diffs[cloud_i][nn_index])
|
|
is_max_other_scale = false;
|
|
else if (diffs[input_index_][point_i] > diffs[cloud_i][nn_index])
|
|
is_min_other_scale = false;
|
|
}
|
|
|
|
if (is_min && !is_min_other_scale)
|
|
passed_min = false;
|
|
if (is_max && !is_max_other_scale)
|
|
passed_max = false;
|
|
|
|
if (!passed_min && !passed_max)
|
|
break;
|
|
}
|
|
|
|
// check if point was minimum/maximum over all the scales
|
|
if (passed_min || passed_max)
|
|
{
|
|
output.push_back ((*input_)[point_i]);
|
|
keypoints_indices_->indices.push_back (point_i);
|
|
}
|
|
}
|
|
}
|
|
|
|
output.header = input_->header;
|
|
output.width = output.size ();
|
|
output.height = 1;
|
|
|
|
// debug stuff
|
|
// for (std::size_t scale_i = 0; scale_i < scales_.size (); ++scale_i)
|
|
// {
|
|
// PointCloud<PointXYZI>::Ptr debug_cloud (new PointCloud<PointXYZI> ());
|
|
// debug_cloud->points.resize (input_->size ());
|
|
// debug_cloud->width = input_->width;
|
|
// debug_cloud->height = input_->height;
|
|
// for (std::size_t point_i = 0; point_i < input_->size (); ++point_i)
|
|
// {
|
|
// (*debug_cloud)[point_i].intensity = diffs[scales_[scale_i].second][point_i];
|
|
// (*debug_cloud)[point_i].x = (*input_)[point_i].x;
|
|
// (*debug_cloud)[point_i].y = (*input_)[point_i].y;
|
|
// (*debug_cloud)[point_i].z = (*input_)[point_i].z;
|
|
// }
|
|
|
|
// char str[512]; sprintf (str, "diffs_%2d.pcd", scale_i);
|
|
// io::savePCDFile (str, *debug_cloud);
|
|
// }
|
|
}
|
|
|
|
//////////////////////////////////////////////////////////////////////////////////////////////
|
|
template <typename PointT, typename PointNT> bool
|
|
pcl::SmoothedSurfacesKeypoint<PointT, PointNT>::initCompute ()
|
|
{
|
|
PCL_INFO ("SmoothedSurfacesKeypoint initCompute () called\n");
|
|
if ( !Keypoint<PointT, PointT>::initCompute ())
|
|
{
|
|
PCL_ERROR ("[pcl::SmoothedSurfacesKeypoints::initCompute] Keypoint::initCompute failed\n");
|
|
return false;
|
|
}
|
|
|
|
if (!normals_)
|
|
{
|
|
PCL_ERROR ("[pcl::SmoothedSurfacesKeypoints::initCompute] Input normals were not set\n");
|
|
return false;
|
|
}
|
|
if (clouds_.empty ())
|
|
{
|
|
PCL_ERROR ("[pcl::SmoothedSurfacesKeypoints::initCompute] No other clouds were set apart from the input\n");
|
|
return false;
|
|
}
|
|
|
|
if (input_->size () != normals_->size ())
|
|
{
|
|
PCL_ERROR ("[pcl::SmoothedSurfacesKeypoints::initCompute] The input cloud and the input normals differ in size\n");
|
|
return false;
|
|
}
|
|
|
|
for (std::size_t cloud_i = 0; cloud_i < clouds_.size (); ++cloud_i)
|
|
{
|
|
if (clouds_[cloud_i]->size () != input_->size ())
|
|
{
|
|
PCL_ERROR("[pcl::SmoothedSurfacesKeypoints::initCompute] Cloud %zu does not have "
|
|
"the same number of points as the input cloud\n",
|
|
cloud_i);
|
|
return false;
|
|
}
|
|
|
|
if (cloud_normals_[cloud_i]->size () != input_->size ())
|
|
{
|
|
PCL_ERROR("[pcl::SmoothedSurfacesKeypoints::initCompute] Normals for cloud %zu "
|
|
"do not have the same number of points as the input cloud\n",
|
|
cloud_i);
|
|
return false;
|
|
}
|
|
}
|
|
|
|
// Add the input cloud as the last index
|
|
scales_.push_back (std::pair<float, std::size_t> (input_scale_, scales_.size ()));
|
|
clouds_.push_back (input_);
|
|
cloud_normals_.push_back (normals_);
|
|
cloud_trees_.push_back (tree_);
|
|
// Sort the clouds by their scales
|
|
sort (scales_.begin (), scales_.end (), compareScalesFunction);
|
|
|
|
// Find the index of the input after sorting
|
|
for (std::size_t i = 0; i < scales_.size (); ++i)
|
|
if (scales_[i].second == scales_.size () - 1)
|
|
{
|
|
input_index_ = i;
|
|
break;
|
|
}
|
|
|
|
PCL_INFO ("Scales: ");
|
|
for (std::size_t i = 0; i < scales_.size (); ++i) PCL_INFO ("(%d %f), ", scales_[i].second, scales_[i].first);
|
|
PCL_INFO ("\n");
|
|
|
|
return (true);
|
|
}
|
|
|
|
|
|
#define PCL_INSTANTIATE_SmoothedSurfacesKeypoint(T,NT) template class PCL_EXPORTS pcl::SmoothedSurfacesKeypoint<T,NT>;
|
|
|
|
|
|
#endif /* PCL_KEYPOINTS_IMPL_SMOOTHEDSURFACESKEYPOINT_H_ */
|