search.hpp
/*
* Software License Agreement (BSD License)
*
* Point Cloud Library (PCL) - www.pointclouds.org
* 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.
*
*/
#ifndef PCL_SEARCH_SEARCH_IMPL_HPP_
#define PCL_SEARCH_SEARCH_IMPL_HPP_
#include <pcl/search/search.h>
///////////////////////////////////////////////////////////////////////////////////////////
template <typename PointT>
pcl::search::Search<PointT>::Search (const std::string& name, bool sorted)
: input_ ()
, indices_ ()
, sorted_results_ (sorted)
, name_ (name)
{
}
///////////////////////////////////////////////////////////////////////////////////////////
template <typename PointT> const std::string&
pcl::search::Search<PointT>::getName () const
{
return (name_);
}
///////////////////////////////////////////////////////////////////////////////////////////
template <typename PointT> void
pcl::search::Search<PointT>::setSortedResults (bool sorted)
{
sorted_results_ = sorted;
}
///////////////////////////////////////////////////////////////////////////////////////////
template <typename PointT> bool
pcl::search::Search<PointT>::getSortedResults ()
{
return (sorted_results_);
}
///////////////////////////////////////////////////////////////////////////////////////////
template <typename PointT> void
pcl::search::Search<PointT>::setInputCloud (
const PointCloudConstPtr& cloud, const IndicesConstPtr &indices)
{
input_ = cloud;
indices_ = indices;
}
///////////////////////////////////////////////////////////////////////////////////////////
template <typename PointT> int
pcl::search::Search<PointT>::nearestKSearch (
const PointCloud &cloud, int index, int k,
std::vector<int> &k_indices, std::vector<float> &k_sqr_distances) const
{
assert (index >= 0 && index < static_cast<int> (cloud.points.size ()) && "Out-of-bounds error in nearestKSearch!");
return (nearestKSearch (cloud.points[index], k, k_indices, k_sqr_distances));
}
///////////////////////////////////////////////////////////////////////////////////////////
template <typename PointT> int
pcl::search::Search<PointT>::nearestKSearch (
int index, int k,
std::vector<int> &k_indices,
std::vector<float> &k_sqr_distances) const
{
if (indices_ == NULL)
{
assert (index >= 0 && index < static_cast<int> (input_->points.size ()) && "Out-of-bounds error in nearestKSearch!");
return (nearestKSearch (input_->points[index], k, k_indices, k_sqr_distances));
}
else
{
assert (index >= 0 && index < static_cast<int> (indices_->size ()) && "Out-of-bounds error in nearestKSearch!");
if (index >= static_cast<int> (indices_->size ()) || index < 0)
return (0);
return (nearestKSearch (input_->points[(*indices_)[index]], k, k_indices, k_sqr_distances));
}
}
///////////////////////////////////////////////////////////////////////////////////////////
template <typename PointT> void
pcl::search::Search<PointT>::nearestKSearch (
const PointCloud& cloud, const std::vector<int>& indices,
int k, std::vector< std::vector<int> >& k_indices,
std::vector< std::vector<float> >& k_sqr_distances) const
{
if (indices.empty ())
{
k_indices.resize (cloud.size ());
k_sqr_distances.resize (cloud.size ());
for (size_t i = 0; i < cloud.size (); i++)
nearestKSearch (cloud, static_cast<int> (i), k, k_indices[i], k_sqr_distances[i]);
}
else
{
k_indices.resize (indices.size ());
k_sqr_distances.resize (indices.size ());
for (size_t i = 0; i < indices.size (); i++)
nearestKSearch (cloud, indices[i], k, k_indices[i], k_sqr_distances[i]);
}
}
///////////////////////////////////////////////////////////////////////////////////////////
template <typename PointT> int
pcl::search::Search<PointT>::radiusSearch (
const PointCloud &cloud, int index, double radius,
std::vector<int> &k_indices, std::vector<float> &k_sqr_distances,
unsigned int max_nn) const
{
assert (index >= 0 && index < static_cast<int> (cloud.points.size ()) && "Out-of-bounds error in radiusSearch!");
return (radiusSearch(cloud.points[index], radius, k_indices, k_sqr_distances, max_nn));
}
///////////////////////////////////////////////////////////////////////////////////////////
template <typename PointT> int
pcl::search::Search<PointT>::radiusSearch (
int index, double radius, std::vector<int> &k_indices,
std::vector<float> &k_sqr_distances, unsigned int max_nn ) const
{
if (indices_ == NULL)
{
assert (index >= 0 && index < static_cast<int> (input_->points.size ()) && "Out-of-bounds error in radiusSearch!");
return (radiusSearch (input_->points[index], radius, k_indices, k_sqr_distances, max_nn));
}
else
{
assert (index >= 0 && index < static_cast<int> (indices_->size ()) && "Out-of-bounds error in radiusSearch!");
return (radiusSearch (input_->points[(*indices_)[index]], radius, k_indices, k_sqr_distances, max_nn));
}
}
///////////////////////////////////////////////////////////////////////////////////////////
template <typename PointT> void
pcl::search::Search<PointT>::radiusSearch (
const PointCloud& cloud,
const std::vector<int>& indices,
double radius,
std::vector< std::vector<int> >& k_indices,
std::vector< std::vector<float> > &k_sqr_distances,
unsigned int max_nn) const
{
if (indices.empty ())
{
k_indices.resize (cloud.size ());
k_sqr_distances.resize (cloud.size ());
for (size_t i = 0; i < cloud.size (); i++)
radiusSearch (cloud, static_cast<int> (i), radius,k_indices[i], k_sqr_distances[i], max_nn);
}
else
{
k_indices.resize (indices.size ());
k_sqr_distances.resize (indices.size ());
for (size_t i = 0; i < indices.size (); i++)
radiusSearch (cloud,indices[i],radius,k_indices[i],k_sqr_distances[i], max_nn);
}
}
///////////////////////////////////////////////////////////////////////////////////////////
template <typename PointT> void
pcl::search::Search<PointT>::sortResults (
std::vector<int>& indices, std::vector<float>& distances) const
{
std::vector<int> order (indices.size ());
for (size_t idx = 0; idx < order.size (); ++idx)
order [idx] = static_cast<int> (idx);
Compare compare (distances);
sort (order.begin (), order.end (), compare);
std::vector<int> sorted (indices.size ());
for (size_t idx = 0; idx < order.size (); ++idx)
sorted [idx] = indices[order [idx]];
indices = sorted;
// sort the according distances.
sort (distances.begin (), distances.end ());
}
#define PCL_INSTANTIATE_Search(T) template class PCL_EXPORTS pcl::search::Search<T>;
#endif //#ifndef _PCL_SEARCH_SEARCH_IMPL_HPP_
pcl::search::Search< PointXYZRGB >::PointCloudConstPtr
PointCloud::ConstPtr PointCloudConstPtr
Definition: search.h:79
pcl::search::Search< PointXYZRGB >::IndicesConstPtr
boost::shared_ptr< const std::vector< int > > IndicesConstPtr
Definition: search.h:85
pcl::search::Search::getSortedResults
virtual bool getSortedResults()
Gets whether the results should be sorted (ascending in the distance) or not Otherwise the results ma...
Definition: search.hpp:69
pcl::PointCloud::points
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
Definition: point_cloud.h:410
pcl::search::Search::setInputCloud
virtual void setInputCloud(const PointCloudConstPtr &cloud, const IndicesConstPtr &indices=IndicesConstPtr())
Pass the input dataset that the search will be performed on.
Definition: search.hpp:76
pcl::search::Search::radiusSearch
virtual int radiusSearch(const PointT &point, double radius, std::vector< int > &k_indices, std::vector< float > &k_sqr_distances, unsigned int max_nn=0) const =0
Search for all the nearest neighbors of the query point in a given radius.
pcl::search::Search::setSortedResults
virtual void setSortedResults(bool sorted)
sets whether the results should be sorted (ascending in the distance) or not
Definition: search.hpp:62
pcl::search::Search::sortResults
void sortResults(std::vector< int > &indices, std::vector< float > &distances) const
Definition: search.hpp:195
pcl::PointCloud
PointCloud represents the base class in PCL for storing collections of 3D points. ...
Definition: projection_matrix.h:53
pcl::search::Search::nearestKSearch
virtual int nearestKSearch(const PointT &point, int k, std::vector< int > &k_indices, std::vector< float > &k_sqr_distances) const =0
Search for the k-nearest neighbors for the given query point.
pcl::search::Search::Search
Search(const std::string &name="", bool sorted=false)
Constructor.
Definition: search.hpp:45
pcl::search::Search::getName
virtual const std::string & getName() const
Returns the search method name.
Definition: search.hpp:55
pcl::PointCloud::size
size_t size() const
Definition: point_cloud.h:448