371 lines
16 KiB
C++
371 lines
16 KiB
C++
/*
|
|
* 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 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$
|
|
*/
|
|
|
|
#pragma once
|
|
|
|
#include <pcl/search/search.h>
|
|
#include <pcl/common/time.h>
|
|
#include <pcl/point_representation.h>
|
|
|
|
namespace flann
|
|
{
|
|
template<typename T> class NNIndex;
|
|
template<typename T> struct L2;
|
|
template<typename T> struct L2_Simple;
|
|
template<typename T> class Matrix;
|
|
}
|
|
|
|
namespace pcl
|
|
{
|
|
namespace search
|
|
{
|
|
|
|
/** \brief @b search::FlannSearch is a generic FLANN wrapper class for the new search interface.
|
|
* It is able to wrap any FLANN index type, e.g. the kd tree as well as indices for high-dimensional
|
|
* searches and intended as a more powerful and cleaner successor to KdTreeFlann.
|
|
*
|
|
* By default, this class creates a single kd tree for indexing the input data. However, for high dimensions
|
|
* (> 10), it is often better to use the multiple randomized kd tree index provided by FLANN in combination with
|
|
* the \ref flann::L2 distance functor. During search in this type of index, the number of checks to perform before
|
|
* terminating the search can be controlled. Here is a code example if a high-dimensional 2-NN search:
|
|
*
|
|
* \code
|
|
* // Feature and distance type
|
|
* using FeatureT = SHOT352;
|
|
* using DistanceT = flann::L2<float>;
|
|
*
|
|
* // Search and index types
|
|
* using SearchT = search::FlannSearch<FeatureT, DistanceT>;
|
|
* using CreatorPtrT = typename SearchT::FlannIndexCreatorPtr;
|
|
* using IndexT = typename SearchT::KdTreeMultiIndexCreator;
|
|
* using RepresentationPtrT = typename SearchT::PointRepresentationPtr;
|
|
*
|
|
* // Features
|
|
* PointCloud<FeatureT>::Ptr query, target;
|
|
*
|
|
* // Fill query and target with calculated features...
|
|
*
|
|
* // Instantiate search object with 4 randomized trees and 256 checks
|
|
* SearchT search (true, CreatorPtrT (new IndexT (4)));
|
|
* search.setPointRepresentation (RepresentationPtrT (new DefaultFeatureRepresentation<FeatureT>));
|
|
* search.setChecks (256);
|
|
* search.setInputCloud (target);
|
|
*
|
|
* // Do search
|
|
* std::vector<Indices> k_indices;
|
|
* std::vector<std::vector<float> > k_sqr_distances;
|
|
* search.nearestKSearch (*query, Indices (), 2, k_indices, k_sqr_distances);
|
|
* \endcode
|
|
*
|
|
* \author Andreas Muetzel
|
|
* \author Anders Glent Buch (multiple randomized kd tree interface)
|
|
* \ingroup search
|
|
*/
|
|
template<typename PointT, typename FlannDistance=flann::L2_Simple <float> >
|
|
class FlannSearch: public Search<PointT>
|
|
{
|
|
using Search<PointT>::input_;
|
|
using Search<PointT>::indices_;
|
|
using Search<PointT>::sorted_results_;
|
|
|
|
public:
|
|
using Ptr = shared_ptr<FlannSearch<PointT, FlannDistance> >;
|
|
using ConstPtr = shared_ptr<const FlannSearch<PointT, FlannDistance> >;
|
|
|
|
using PointCloud = typename Search<PointT>::PointCloud;
|
|
using PointCloudConstPtr = typename Search<PointT>::PointCloudConstPtr;
|
|
|
|
using MatrixPtr = shared_ptr<flann::Matrix<float> >;
|
|
using MatrixConstPtr = shared_ptr<const flann::Matrix<float> >;
|
|
|
|
using Index = flann::NNIndex<FlannDistance>;
|
|
using IndexPtr = shared_ptr<flann::NNIndex<FlannDistance> >;
|
|
|
|
using PointRepresentation = pcl::PointRepresentation<PointT>;
|
|
using PointRepresentationPtr = typename PointRepresentation::Ptr;
|
|
using PointRepresentationConstPtr = typename PointRepresentation::ConstPtr;
|
|
|
|
/** \brief Helper class that creates a FLANN index from a given FLANN matrix. To
|
|
* use a FLANN index type with FlannSearch, implement this interface and
|
|
* pass an object of the new type to the FlannSearch constructor.
|
|
* See the implementation of KdTreeIndexCreator for an example.
|
|
*/
|
|
class FlannIndexCreator
|
|
{
|
|
public:
|
|
/** \brief Create a FLANN Index from the input data.
|
|
* \param[in] data The FLANN matrix containing the input.
|
|
* \return The FLANN index.
|
|
*/
|
|
virtual IndexPtr createIndex (MatrixConstPtr data)=0;
|
|
|
|
/** \brief destructor
|
|
*/
|
|
virtual ~FlannIndexCreator () {}
|
|
};
|
|
using FlannIndexCreatorPtr = shared_ptr<FlannIndexCreator>;
|
|
|
|
/** \brief Creates a FLANN KdTreeSingleIndex from the given input data.
|
|
*/
|
|
class KdTreeIndexCreator: public FlannIndexCreator
|
|
{
|
|
public:
|
|
/** \param[in] max_leaf_size All FLANN kd trees created by this class will have
|
|
* a maximum of max_leaf_size points per leaf node. Higher values make index creation
|
|
* cheaper, but search more costly (and the other way around).
|
|
*/
|
|
KdTreeIndexCreator (unsigned int max_leaf_size=15) : max_leaf_size_ (max_leaf_size){}
|
|
|
|
/** \brief Empty destructor */
|
|
~KdTreeIndexCreator () {}
|
|
|
|
/** \brief Create a FLANN Index from the input data.
|
|
* \param[in] data The FLANN matrix containing the input.
|
|
* \return The FLANN index.
|
|
*/
|
|
IndexPtr createIndex (MatrixConstPtr data) override;
|
|
private:
|
|
unsigned int max_leaf_size_;
|
|
};
|
|
|
|
/** \brief Creates a FLANN KdTreeSingleIndex from the given input data.
|
|
*/
|
|
class KMeansIndexCreator: public FlannIndexCreator
|
|
{
|
|
public:
|
|
/** \brief All FLANN kd trees created by this class will have
|
|
* a maximum of max_leaf_size points per leaf node. Higher values make index creation
|
|
* cheaper, but search more costly (and the other way around).
|
|
*/
|
|
KMeansIndexCreator (){}
|
|
|
|
/** \brief Empty destructor */
|
|
virtual ~KMeansIndexCreator () {}
|
|
|
|
/** \brief Create a FLANN Index from the input data.
|
|
* \param[in] data The FLANN matrix containing the input.
|
|
* \return The FLANN index.
|
|
*/
|
|
virtual IndexPtr createIndex (MatrixConstPtr data);
|
|
private:
|
|
};
|
|
|
|
/** \brief Creates a FLANN KdTreeIndex of multiple randomized trees from the given input data,
|
|
* suitable for feature matching. Note that in this case, it is often more efficient to use the
|
|
* \ref flann::L2 distance functor.
|
|
*/
|
|
class KdTreeMultiIndexCreator: public FlannIndexCreator
|
|
{
|
|
public:
|
|
/** \param[in] trees Number of randomized trees to create.
|
|
*/
|
|
KdTreeMultiIndexCreator (int trees = 4) : trees_ (trees) {}
|
|
|
|
/** \brief Empty destructor */
|
|
virtual ~KdTreeMultiIndexCreator () {}
|
|
|
|
/** \brief Create a FLANN Index from the input data.
|
|
* \param[in] data The FLANN matrix containing the input.
|
|
* \return The FLANN index.
|
|
*/
|
|
virtual IndexPtr createIndex (MatrixConstPtr data);
|
|
private:
|
|
int trees_;
|
|
};
|
|
|
|
FlannSearch (bool sorted = true, FlannIndexCreatorPtr creator = FlannIndexCreatorPtr (new KdTreeIndexCreator ()));
|
|
|
|
/** \brief Destructor for FlannSearch. */
|
|
|
|
~FlannSearch ();
|
|
|
|
|
|
//void
|
|
//setInputCloud (const PointCloudConstPtr &cloud, const IndicesConstPtr &indices = IndicesConstPtr ());
|
|
|
|
/** \brief Set the search epsilon precision (error bound) for nearest neighbors searches.
|
|
* \param[in] eps precision (error bound) for nearest neighbors searches
|
|
*/
|
|
inline void
|
|
setEpsilon (double eps)
|
|
{
|
|
eps_ = eps;
|
|
}
|
|
|
|
/** \brief Get the search epsilon precision (error bound) for nearest neighbors searches. */
|
|
inline double
|
|
getEpsilon ()
|
|
{
|
|
return (eps_);
|
|
}
|
|
|
|
/** \brief Set the number of checks to perform during approximate searches in multiple randomized trees.
|
|
* \param[in] checks number of checks to perform during approximate searches in multiple randomized trees.
|
|
*/
|
|
inline void
|
|
setChecks (int checks)
|
|
{
|
|
checks_ = checks;
|
|
}
|
|
|
|
/** \brief Get the number of checks to perform during approximate searches in multiple randomized trees. */
|
|
inline int
|
|
getChecks ()
|
|
{
|
|
return (checks_);
|
|
}
|
|
|
|
/** \brief Provide a pointer to the input dataset.
|
|
* \param[in] cloud the const boost shared pointer to a PointCloud message
|
|
* \param[in] indices the point indices subset that is to be used from \a cloud
|
|
*/
|
|
void
|
|
setInputCloud (const PointCloudConstPtr& cloud, const IndicesConstPtr& indices = IndicesConstPtr ()) override;
|
|
|
|
using Search<PointT>::nearestKSearch;
|
|
|
|
/** \brief Search for the k-nearest neighbors for the given query point.
|
|
* \param[in] point the given query point
|
|
* \param[in] k the number of neighbors to search for
|
|
* \param[out] k_indices the resultant indices of the neighboring points (must be resized to \a k a priori!)
|
|
* \param[out] k_sqr_distances the resultant squared distances to the neighboring points (must be resized to \a k
|
|
* a priori!)
|
|
* \return number of neighbors found
|
|
*/
|
|
int
|
|
nearestKSearch (const PointT &point, int k, Indices &k_indices, std::vector<float> &k_sqr_distances) const override;
|
|
|
|
|
|
/** \brief Search for the k-nearest neighbors for the given query point.
|
|
* \param[in] cloud the point cloud data
|
|
* \param[in] indices a vector of point cloud indices to query for nearest neighbors
|
|
* \param[in] k the number of neighbors to search for
|
|
* \param[out] k_indices the resultant indices of the neighboring points, k_indices[i] corresponds to the neighbors of the query point i
|
|
* \param[out] k_sqr_distances the resultant squared distances to the neighboring points, k_sqr_distances[i] corresponds to the neighbors of the query point i
|
|
*/
|
|
void
|
|
nearestKSearch (const PointCloud& cloud, const Indices& indices, int k,
|
|
std::vector<Indices>& k_indices, std::vector< std::vector<float> >& k_sqr_distances) const override;
|
|
|
|
/** \brief Search for all the nearest neighbors of the query point in a given radius.
|
|
* \param[in] point the given query point
|
|
* \param[in] radius the radius of the sphere bounding all of p_q's neighbors
|
|
* \param[out] k_indices the resultant indices of the neighboring points
|
|
* \param[out] k_sqr_distances the resultant squared distances to the neighboring points
|
|
* \param[in] max_nn if given, bounds the maximum returned neighbors to this value. If \a max_nn is set to
|
|
* 0 or to a number higher than the number of points in the input cloud, all neighbors in \a radius will be
|
|
* returned.
|
|
* \return number of neighbors found in radius
|
|
*/
|
|
int
|
|
radiusSearch (const PointT& point, double radius,
|
|
Indices &k_indices, std::vector<float> &k_sqr_distances,
|
|
unsigned int max_nn = 0) const override;
|
|
|
|
/** \brief Search for the k-nearest neighbors for the given query point.
|
|
* \param[in] cloud the point cloud data
|
|
* \param[in] indices a vector of point cloud indices to query for nearest neighbors
|
|
* \param[in] radius the radius of the sphere bounding all of p_q's neighbors
|
|
* \param[out] k_indices the resultant indices of the neighboring points, k_indices[i] corresponds to the neighbors of the query point i
|
|
* \param[out] k_sqr_distances the resultant squared distances to the neighboring points, k_sqr_distances[i] corresponds to the neighbors of the query point i
|
|
* \param[in] max_nn if given, bounds the maximum returned neighbors to this value
|
|
*/
|
|
void
|
|
radiusSearch (const PointCloud& cloud, const Indices& indices, double radius, std::vector<Indices>& k_indices,
|
|
std::vector< std::vector<float> >& k_sqr_distances, unsigned int max_nn=0) const override;
|
|
|
|
/** \brief Provide a pointer to the point representation to use to convert points into k-D vectors.
|
|
* \param[in] point_representation the const boost shared pointer to a PointRepresentation
|
|
*/
|
|
inline void
|
|
setPointRepresentation (const PointRepresentationConstPtr &point_representation)
|
|
{
|
|
point_representation_ = point_representation;
|
|
dim_ = point_representation->getNumberOfDimensions ();
|
|
if (input_) // re-create the tree, since point_representation might change things such as the scaling of the point clouds.
|
|
setInputCloud (input_, indices_);
|
|
}
|
|
|
|
/** \brief Get a pointer to the point representation used when converting points into k-D vectors. */
|
|
inline PointRepresentationConstPtr const
|
|
getPointRepresentation ()
|
|
{
|
|
return (point_representation_);
|
|
}
|
|
|
|
protected:
|
|
|
|
/** \brief converts the input data to a format usable by FLANN
|
|
*/
|
|
void convertInputToFlannMatrix();
|
|
|
|
/** The FLANN index.
|
|
*/
|
|
IndexPtr index_;
|
|
|
|
/** The index creator, used to (re-) create the index when the search data is passed.
|
|
*/
|
|
FlannIndexCreatorPtr creator_;
|
|
|
|
/** Input data in FLANN format.
|
|
*/
|
|
MatrixPtr input_flann_;
|
|
|
|
/** Epsilon for approximate NN search.
|
|
*/
|
|
float eps_;
|
|
|
|
/** Number of checks to perform for approximate NN search using the multiple randomized tree index
|
|
*/
|
|
int checks_;
|
|
|
|
bool input_copied_for_flann_;
|
|
|
|
PointRepresentationConstPtr point_representation_;
|
|
|
|
int dim_;
|
|
|
|
Indices index_mapping_;
|
|
bool identity_mapping_;
|
|
|
|
};
|
|
}
|
|
}
|
|
|
|
#define PCL_INSTANTIATE_FlannSearch(T) template class PCL_EXPORTS pcl::search::FlannSearch<T>;
|