Point Cloud Library (PCL)  1.7.0
/tmp/buildd/pcl-1.7-1.7.0/search/include/pcl/search/flann_search.h
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Point Cloud Library (PCL) - www.pointclouds.org
00005  *  Copyright (c) 2010-2011, Willow Garage, Inc.
00006  *
00007  *  All rights reserved.
00008  *
00009  *  Redistribution and use in source and binary forms, with or without
00010  *  modification, are permitted provided that the following conditions
00011  *  are met:
00012  *
00013  *   * Redistributions of source code must retain the above copyright
00014  *     notice, this list of conditions and the following disclaimer.
00015  *   * Redistributions in binary form must reproduce the above
00016  *     copyright notice, this list of conditions and the following
00017  *     disclaimer in the documentation and/or other materials provided
00018  *     with the distribution.
00019  *   * Neither the name of the copyright holder(s) nor the names of its
00020  *     contributors may be used to endorse or promote products derived
00021  *     from this software without specific prior written permission.
00022  *
00023  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00024  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00025  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00026  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00027  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00028  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00029  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00030  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00031  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00032  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00033  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00034  *  POSSIBILITY OF SUCH DAMAGE.
00035  *
00036  * $Id$
00037  */
00038 
00039 #ifndef PCL_SEARCH_FLANN_SEARCH_H_
00040 #define PCL_SEARCH_FLANN_SEARCH_H_
00041 
00042 #include <pcl/search/search.h>
00043 #include <pcl/common/time.h>
00044 #include <pcl/point_representation.h>
00045 
00046 namespace flann
00047 {
00048   template<typename T> class NNIndex;
00049   template<typename T> struct L2;
00050   template<typename T> struct L2_Simple;
00051   template<typename T> class Matrix;
00052 }
00053 
00054 namespace pcl
00055 {
00056   namespace search
00057   {
00058 
00059     /** \brief @b search::FlannSearch is a generic FLANN wrapper class for the new search interface.
00060       * It is able to wrap any FLANN index type, e.g. the kd tree as well as indices for high-dimensional
00061       * searches and intended as a more powerful and cleaner successor to KdTreeFlann.
00062       *
00063       * \author Andreas Muetzel
00064       * \ingroup search
00065       */
00066     template<typename PointT, typename FlannDistance=flann::L2_Simple <float> >
00067     class FlannSearch: public Search<PointT>
00068     {
00069       typedef typename Search<PointT>::PointCloud PointCloud;
00070       typedef typename Search<PointT>::PointCloudConstPtr PointCloudConstPtr;
00071 
00072       typedef boost::shared_ptr<std::vector<int> > IndicesPtr;
00073       typedef boost::shared_ptr<const std::vector<int> > IndicesConstPtr;
00074       typedef flann::NNIndex< FlannDistance > Index;
00075       typedef boost::shared_ptr<flann::NNIndex <FlannDistance > > IndexPtr;
00076       typedef boost::shared_ptr<flann::Matrix <float> > MatrixPtr;
00077       typedef boost::shared_ptr<const flann::Matrix <float> > MatrixConstPtr;
00078 
00079       typedef pcl::PointRepresentation<PointT> PointRepresentation;
00080       //typedef boost::shared_ptr<PointRepresentation> PointRepresentationPtr;
00081       typedef boost::shared_ptr<const PointRepresentation> PointRepresentationConstPtr;
00082 
00083       using Search<PointT>::input_;
00084       using Search<PointT>::indices_;
00085       using Search<PointT>::sorted_results_;
00086 
00087       public:
00088         typedef boost::shared_ptr<FlannSearch<PointT, FlannDistance> > Ptr;
00089         typedef boost::shared_ptr<const FlannSearch<PointT, FlannDistance> > ConstPtr;
00090 
00091         /** \brief Helper class that creates a FLANN index from a given FLANN matrix. To
00092           * use a FLANN index type with FlannSearch, implement this interface and
00093           * pass an object of the new type to the FlannSearch constructor.
00094           * See the implementation of KdTreeIndexCreator for an example.
00095           */
00096         class FlannIndexCreator
00097         {
00098           public:
00099           /** \brief Create a FLANN Index from the input data.
00100             * \param[in] data The FLANN matrix containing the input.
00101             * \return The FLANN index.
00102             */
00103             virtual IndexPtr createIndex (MatrixConstPtr data)=0;
00104 
00105           /** \brief destructor 
00106             */
00107             virtual ~FlannIndexCreator () {}
00108         };
00109         typedef boost::shared_ptr<FlannIndexCreator> FlannIndexCreatorPtr;
00110 
00111         /** \brief Creates a FLANN KdTreeSingleIndex from the given input data.
00112           */
00113         class KdTreeIndexCreator: public FlannIndexCreator
00114         {
00115           public:
00116           /** \param[in] max_leaf_size All FLANN kd trees created by this class will have
00117             * a maximum of max_leaf_size points per leaf node. Higher values make index creation
00118             * cheaper, but search more costly (and the other way around).
00119             */
00120             KdTreeIndexCreator (unsigned int max_leaf_size=15) : max_leaf_size_ (max_leaf_size){}
00121       
00122             /** \brief Empty destructor */
00123             virtual ~KdTreeIndexCreator () {}
00124 
00125           /** \brief Create a FLANN Index from the input data.
00126             * \param[in] data The FLANN matrix containing the input.
00127             * \return The FLANN index.
00128             */
00129             virtual IndexPtr createIndex (MatrixConstPtr data);
00130           private:
00131             unsigned int max_leaf_size_;
00132         };
00133 
00134         /** \brief Creates a FLANN KdTreeSingleIndex from the given input data.
00135           */
00136         class KMeansIndexCreator: public FlannIndexCreator
00137         {
00138           public:
00139           /** \param[in] max_leaf_size All FLANN kd trees created by this class will have
00140             * a maximum of max_leaf_size points per leaf node. Higher values make index creation
00141             * cheaper, but search more costly (and the other way around).
00142             */
00143             KMeansIndexCreator (){}
00144             
00145             /** \brief Empty destructor */
00146             virtual ~KMeansIndexCreator () {}
00147 
00148           /** \brief Create a FLANN Index from the input data.
00149             * \param[in] data The FLANN matrix containing the input.
00150             * \return The FLANN index.
00151             */
00152             virtual IndexPtr createIndex (MatrixConstPtr data);
00153           private:
00154         };
00155 
00156         FlannSearch (bool sorted = true, FlannIndexCreatorPtr creator = FlannIndexCreatorPtr (new KdTreeIndexCreator ()));
00157 
00158         /** \brief Destructor for FlannSearch. */
00159         virtual
00160         ~FlannSearch ();
00161 
00162 
00163         //void
00164         //setInputCloud (const PointCloudConstPtr &cloud, const IndicesConstPtr &indices = IndicesConstPtr ());
00165 
00166         /** \brief Set the search epsilon precision (error bound) for nearest neighbors searches.
00167           * \param[in] eps precision (error bound) for nearest neighbors searches
00168           */
00169         inline void
00170         setEpsilon (double eps)
00171         {
00172           eps_ = eps;
00173         }
00174 
00175         /** \brief Get the search epsilon precision (error bound) for nearest neighbors searches. */
00176         inline double
00177         getEpsilon ()
00178         {
00179           return (eps_);
00180         }
00181 
00182         /** \brief Provide a pointer to the input dataset.
00183           * \param[in] cloud the const boost shared pointer to a PointCloud message
00184           * \param[in] indices the point indices subset that is to be used from \a cloud
00185           */
00186         virtual void
00187         setInputCloud (const PointCloudConstPtr& cloud, const IndicesConstPtr& indices = IndicesConstPtr ());
00188 
00189         /** \brief Search for the k-nearest neighbors for the given query point.
00190           * \param[in] point the given query point
00191           * \param[in] k the number of neighbors to search for
00192           * \param[out] k_indices the resultant indices of the neighboring points (must be resized to \a k a priori!)
00193           * \param[out] k_sqr_distances the resultant squared distances to the neighboring points (must be resized to \a k
00194           * a priori!)
00195           * \return number of neighbors found
00196           */
00197         int
00198         nearestKSearch (const PointT &point, int k, std::vector<int> &k_indices, std::vector<float> &k_sqr_distances) const;
00199 
00200 
00201         /** \brief Search for the k-nearest neighbors for the given query point.
00202           * \param[in] cloud the point cloud data
00203           * \param[in] indices a vector of point cloud indices to query for nearest neighbors
00204           * \param[in] k the number of neighbors to search for
00205           * \param[out] k_indices the resultant indices of the neighboring points, k_indices[i] corresponds to the neighbors of the query point i
00206           * \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
00207           */
00208         virtual void
00209         nearestKSearch (const PointCloud& cloud, const std::vector<int>& indices, int k, 
00210                         std::vector< std::vector<int> >& k_indices, std::vector< std::vector<float> >& k_sqr_distances) const;
00211 
00212         /** \brief Search for all the nearest neighbors of the query point in a given radius.
00213           * \param[in] point the given query point
00214           * \param[in] radius the radius of the sphere bounding all of p_q's neighbors
00215           * \param[out] k_indices the resultant indices of the neighboring points
00216           * \param[out] k_sqr_distances the resultant squared distances to the neighboring points
00217           * \param[in] max_nn if given, bounds the maximum returned neighbors to this value. If \a max_nn is set to
00218           * 0 or to a number higher than the number of points in the input cloud, all neighbors in \a radius will be
00219           * returned.
00220           * \return number of neighbors found in radius
00221           */
00222         int
00223         radiusSearch (const PointT& point, double radius, 
00224                       std::vector<int> &k_indices, std::vector<float> &k_sqr_distances,
00225                       unsigned int max_nn = 0) const;
00226 
00227         /** \brief Search for the k-nearest neighbors for the given query point.
00228           * \param[in] cloud the point cloud data
00229           * \param[in] indices a vector of point cloud indices to query for nearest neighbors
00230           * \param[in] radius the radius of the sphere bounding all of p_q's neighbors
00231           * \param[out] k_indices the resultant indices of the neighboring points, k_indices[i] corresponds to the neighbors of the query point i
00232           * \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
00233           * \param[in] max_nn if given, bounds the maximum returned neighbors to this value
00234           */
00235         virtual void
00236         radiusSearch (const PointCloud& cloud, const std::vector<int>& indices, double radius, std::vector< std::vector<int> >& k_indices,
00237                 std::vector< std::vector<float> >& k_sqr_distances, unsigned int max_nn=0) const;
00238 
00239         /** \brief Provide a pointer to the point representation to use to convert points into k-D vectors.
00240           * \param[in] point_representation the const boost shared pointer to a PointRepresentation
00241           */
00242         inline void
00243         setPointRepresentation (const PointRepresentationConstPtr &point_representation)
00244         {
00245           point_representation_ = point_representation;
00246           dim_ = point_representation->getNumberOfDimensions ();
00247           if (input_) // re-create the tree, since point_represenation might change things such as the scaling of the point clouds.
00248             setInputCloud (input_, indices_);
00249         }
00250 
00251         /** \brief Get a pointer to the point representation used when converting points into k-D vectors. */
00252         inline PointRepresentationConstPtr const
00253         getPointRepresentation ()
00254         {
00255           return (point_representation_);
00256         }
00257 
00258       protected:
00259 
00260         /** \brief converts the input data to a format usable by FLANN
00261           */
00262         void convertInputToFlannMatrix();
00263 
00264         /** The FLANN index.
00265           */
00266         IndexPtr index_;
00267 
00268         /** The index creator, used to (re-) create the index when the search data is passed.
00269           */
00270         FlannIndexCreatorPtr creator_;
00271 
00272         /** Input data in FLANN format.
00273           */
00274         MatrixPtr input_flann_;
00275 
00276         /** Epsilon for approximate NN search.
00277           */
00278         float eps_;
00279         bool input_copied_for_flann_;
00280 
00281         PointRepresentationConstPtr point_representation_;
00282 
00283         int dim_;
00284 
00285         std::vector<int> index_mapping_;
00286         bool identity_mapping_;
00287 
00288     };
00289   }
00290 }
00291 
00292 #define PCL_INSTANTIATE_FlannSearch(T) template class PCL_EXPORTS pcl::search::FlannSearch<T>;
00293 
00294 #endif    // PCL_SEARCH_KDTREE_H_
00295