Point Cloud Library (PCL)  1.7.0
/tmp/buildd/pcl-1.7-1.7.0/search/include/pcl/search/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  *  Copyright (c) 2012-, Open Perception, Inc.
00007  *
00008  *  All rights reserved.
00009  *
00010  *  Redistribution and use in source and binary forms, with or without
00011  *  modification, are permitted provided that the following conditions
00012  *  are met:
00013  *
00014  *   * Redistributions of source code must retain the above copyright
00015  *     notice, this list of conditions and the following disclaimer.
00016  *   * Redistributions in binary form must reproduce the above
00017  *     copyright notice, this list of conditions and the following
00018  *     disclaimer in the documentation and/or other materials provided
00019  *     with the distribution.
00020  *   * Neither the name of the copyright holder(s) nor the names of its
00021  *     contributors may be used to endorse or promote products derived
00022  *     from this software without specific prior written permission.
00023  *
00024  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00025  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00026  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00027  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00028  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00029  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00030  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00031  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00032  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00033  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00034  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00035  *  POSSIBILITY OF SUCH DAMAGE.
00036  *
00037  */
00038 
00039 #ifndef PCL_SEARCH_SEARCH_H_
00040 #define PCL_SEARCH_SEARCH_H_
00041 
00042 #include <pcl/point_cloud.h>
00043 #include <pcl/for_each_type.h>
00044 #include <pcl/common/concatenate.h>
00045 
00046 namespace pcl
00047 {
00048   namespace search
00049   {
00050     /** \brief Generic search class. All search wrappers must inherit from this.
00051       *
00052       * Each search method must implement 2 different types of search:
00053       *   - \b nearestKSearch - search for K-nearest neighbors.
00054       *   - \b radiusSearch - search for all nearest neighbors in a sphere of a given radius
00055       *
00056       * The input to each search method can be given in 3 different ways:
00057       *   - as a query point
00058       *   - as a (cloud, index) pair
00059       *   - as an index
00060       *
00061       * For the latter option, it is assumed that the user specified the input
00062       * via a \ref setInputCloud () method first.
00063       *
00064       * \note In case of an error, all methods are supposed to return 0 as the number of neighbors found.
00065       *
00066       * \note libpcl_search deals with three-dimensional search problems. For higher
00067       * level dimensional search, please refer to the libpcl_kdtree module.
00068       *
00069       * \author Radu B. Rusu
00070       * \ingroup search
00071       */
00072     template<typename PointT>
00073     class Search
00074     {
00075       public:
00076         typedef pcl::PointCloud<PointT> PointCloud;
00077         typedef typename PointCloud::Ptr PointCloudPtr;
00078         typedef typename PointCloud::ConstPtr PointCloudConstPtr;
00079 
00080         typedef boost::shared_ptr<pcl::search::Search<PointT> > Ptr;
00081         typedef boost::shared_ptr<const pcl::search::Search<PointT> > ConstPtr;
00082 
00083         typedef boost::shared_ptr<std::vector<int> > IndicesPtr;
00084         typedef boost::shared_ptr<const std::vector<int> > IndicesConstPtr;
00085 
00086         /** Constructor. */
00087         Search (const std::string& name = "", bool sorted = false);
00088 
00089         /** Destructor. */
00090         virtual
00091         ~Search ()
00092         {
00093         }
00094 
00095         /** \brief Returns the search method name
00096           */
00097         virtual const std::string& 
00098         getName () const;
00099 
00100         /** \brief sets whether the results should be sorted (ascending in the distance) or not
00101           * \param[in] sorted should be true if the results should be sorted by the distance in ascending order.
00102           * Otherwise the results may be returned in any order.
00103           */
00104         virtual void 
00105         setSortedResults (bool sorted);
00106 
00107         /** \brief Gets whether the results should be sorted (ascending in the distance) or not
00108           * Otherwise the results may be returned in any order.
00109           */
00110         virtual bool 
00111         getSortedResults ();
00112 
00113         
00114         /** \brief Pass the input dataset that the search will be performed on.
00115           * \param[in] cloud a const pointer to the PointCloud data
00116           * \param[in] indices the point indices subset that is to be used from the cloud
00117           */
00118         virtual void
00119         setInputCloud (const PointCloudConstPtr& cloud, 
00120                        const IndicesConstPtr &indices = IndicesConstPtr ());
00121 
00122         /** \brief Get a pointer to the input point cloud dataset. */
00123         virtual PointCloudConstPtr
00124         getInputCloud () const
00125         {
00126           return (input_);
00127         }
00128 
00129         /** \brief Get a pointer to the vector of indices used. */
00130         virtual IndicesConstPtr
00131         getIndices () const
00132         {
00133           return (indices_);
00134         }
00135 
00136         /** \brief Search for the k-nearest neighbors for the given query point.
00137           * \param[in] point the given query point
00138           * \param[in] k the number of neighbors to search for
00139           * \param[out] k_indices the resultant indices of the neighboring points (must be resized to \a k a priori!)
00140           * \param[out] k_sqr_distances the resultant squared distances to the neighboring points (must be resized to \a k
00141           * a priori!)
00142           * \return number of neighbors found
00143           */
00144         virtual int
00145         nearestKSearch (const PointT &point, int k, std::vector<int> &k_indices,
00146                         std::vector<float> &k_sqr_distances) const = 0;
00147 
00148         /** \brief Search for k-nearest neighbors for the given query point.
00149           * This method accepts a different template parameter for the point type.
00150           * \param[in] point the given query point
00151           * \param[in] k the number of neighbors to search for
00152           * \param[out] k_indices the resultant indices of the neighboring points (must be resized to \a k a priori!)
00153           * \param[out] k_sqr_distances the resultant squared distances to the neighboring points (must be resized to \a k
00154           * a priori!)
00155           * \return number of neighbors found
00156           */
00157         template <typename PointTDiff> inline int
00158         nearestKSearchT (const PointTDiff &point, int k,
00159                          std::vector<int> &k_indices, std::vector<float> &k_sqr_distances) const
00160         {
00161           PointT p;
00162           // Copy all the data fields from the input cloud to the output one
00163           typedef typename pcl::traits::fieldList<PointT>::type FieldListInT;
00164           typedef typename pcl::traits::fieldList<PointTDiff>::type FieldListOutT;
00165           typedef typename pcl::intersect<FieldListInT, FieldListOutT>::type FieldList;
00166           pcl::for_each_type <FieldList> (pcl::NdConcatenateFunctor <PointTDiff, PointT> (point, p));
00167           return (nearestKSearch (p, k, k_indices, k_sqr_distances));
00168         }
00169 
00170         /** \brief Search for k-nearest neighbors for the given query point.
00171           *
00172           * \attention This method does not do any bounds checking for the input index
00173           * (i.e., index >= cloud.points.size () || index < 0), and assumes valid (i.e., finite) data.
00174           *
00175           * \param[in] cloud the point cloud data
00176           * \param[in] index a \a valid index in \a cloud representing a \a valid (i.e., finite) query point
00177           * \param[in] k the number of neighbors to search for
00178           * \param[out] k_indices the resultant indices of the neighboring points (must be resized to \a k a priori!)
00179           * \param[out] k_sqr_distances the resultant squared distances to the neighboring points (must be resized to \a k
00180           * a priori!)
00181           *
00182           * \return number of neighbors found
00183           *
00184           * \exception asserts in debug mode if the index is not between 0 and the maximum number of points
00185           */
00186         virtual int
00187         nearestKSearch (const PointCloud &cloud, int index, int k,
00188                         std::vector<int> &k_indices, 
00189                         std::vector<float> &k_sqr_distances) const;
00190 
00191         /** \brief Search for k-nearest neighbors for the given query point (zero-copy).
00192           *
00193           * \attention This method does not do any bounds checking for the input index
00194           * (i.e., index >= cloud.points.size () || index < 0), and assumes valid (i.e., finite) data.
00195           *
00196           * \param[in] index a \a valid index representing a \a valid query point in the dataset given
00197           * by \a setInputCloud. If indices were given in setInputCloud, index will be the position in
00198           * the indices vector.
00199           *
00200           * \param[in] k the number of neighbors to search for
00201           * \param[out] k_indices the resultant indices of the neighboring points (must be resized to \a k a priori!)
00202           * \param[out] k_sqr_distances the resultant squared distances to the neighboring points (must be resized to \a k
00203           * a priori!)
00204           * \return number of neighbors found
00205           *
00206           * \exception asserts in debug mode if the index is not between 0 and the maximum number of points
00207           */
00208         virtual int
00209         nearestKSearch (int index, int k,
00210                         std::vector<int> &k_indices, 
00211                         std::vector<float> &k_sqr_distances) const;
00212 
00213         /** \brief Search for the k-nearest neighbors for the given query point.
00214           * \param[in] cloud the point cloud data
00215           * \param[in] indices a vector of point cloud indices to query for nearest neighbors
00216           * \param[in] k the number of neighbors to search for
00217           * \param[out] k_indices the resultant indices of the neighboring points, k_indices[i] corresponds to the neighbors of the query point i
00218           * \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
00219           */
00220         virtual void
00221         nearestKSearch (const PointCloud& cloud, const std::vector<int>& indices, 
00222                         int k, std::vector< std::vector<int> >& k_indices,
00223                         std::vector< std::vector<float> >& k_sqr_distances) const;
00224 
00225         /** \brief Search for the k-nearest neighbors for the given query point. Use this method if the query points are of a different type than the points in the data set (e.g. PointXYZRGBA instead of PointXYZ).
00226           * \param[in] cloud the point cloud data
00227           * \param[in] indices a vector of point cloud indices to query for nearest neighbors
00228           * \param[in] k the number of neighbors to search for
00229           * \param[out] k_indices the resultant indices of the neighboring points, k_indices[i] corresponds to the neighbors of the query point i
00230           * \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
00231           * \note This method copies the input point cloud of type PointTDiff to a temporary cloud of type PointT and performs the batch search on the new cloud. You should prefer the single-point search if you don't use a search algorithm that accelerates batch NN search.
00232           */
00233         template <typename PointTDiff> void
00234         nearestKSearchT (const pcl::PointCloud<PointTDiff> &cloud, const std::vector<int>& indices, int k, std::vector< std::vector<int> > &k_indices,
00235                          std::vector< std::vector<float> > &k_sqr_distances) const
00236         {
00237           // Copy all the data fields from the input cloud to the output one
00238           typedef typename pcl::traits::fieldList<PointT>::type FieldListInT;
00239           typedef typename pcl::traits::fieldList<PointTDiff>::type FieldListOutT;
00240           typedef typename pcl::intersect<FieldListInT, FieldListOutT>::type FieldList;
00241 
00242           pcl::PointCloud<PointT> pc;
00243           if (indices.empty ())
00244           {
00245             pc.resize (cloud.size());
00246             for (size_t i = 0; i < cloud.size(); i++)
00247             {
00248               pcl::for_each_type <FieldList> (pcl::NdConcatenateFunctor <PointTDiff, PointT> (
00249                                               cloud[i], pc[i]));
00250             }
00251             nearestKSearch (pc,std::vector<int>(),k,k_indices,k_sqr_distances);
00252           }
00253           else
00254           {
00255             pc.resize (indices.size());
00256             for (size_t i = 0; i < indices.size(); i++)
00257             {
00258               pcl::for_each_type <FieldList> (pcl::NdConcatenateFunctor <PointTDiff, PointT> (
00259                                               cloud[indices[i]], pc[i]));
00260             }
00261             nearestKSearch (pc,std::vector<int>(),k,k_indices,k_sqr_distances);
00262           }
00263         }
00264 
00265         /** \brief Search for all the nearest neighbors of the query point in a given radius.
00266           * \param[in] point the given query point
00267           * \param[in] radius the radius of the sphere bounding all of p_q's neighbors
00268           * \param[out] k_indices the resultant indices of the neighboring points
00269           * \param[out] k_sqr_distances the resultant squared distances to the neighboring points
00270           * \param[in] max_nn if given, bounds the maximum returned neighbors to this value. If \a max_nn is set to
00271           * 0 or to a number higher than the number of points in the input cloud, all neighbors in \a radius will be
00272           * returned.
00273           * \return number of neighbors found in radius
00274           */
00275         virtual int
00276         radiusSearch (const PointT& point, double radius, std::vector<int>& k_indices,
00277                       std::vector<float>& k_sqr_distances, unsigned int max_nn = 0) const = 0;
00278 
00279         /** \brief Search for all the nearest neighbors of the query point in a given radius.
00280           * \param[in] point the given query point
00281           * \param[in] radius the radius of the sphere bounding all of p_q's neighbors
00282           * \param[out] k_indices the resultant indices of the neighboring points
00283           * \param[out] k_sqr_distances the resultant squared distances to the neighboring points
00284           * \param[in] max_nn if given, bounds the maximum returned neighbors to this value. If \a max_nn is set to
00285           * 0 or to a number higher than the number of points in the input cloud, all neighbors in \a radius will be
00286           * returned.
00287           * \return number of neighbors found in radius
00288           */
00289         template <typename PointTDiff> inline int
00290         radiusSearchT (const PointTDiff &point, double radius, std::vector<int> &k_indices,
00291                        std::vector<float> &k_sqr_distances, unsigned int max_nn = 0) const
00292         {
00293           PointT p;
00294           // Copy all the data fields from the input cloud to the output one
00295           typedef typename pcl::traits::fieldList<PointT>::type FieldListInT;
00296           typedef typename pcl::traits::fieldList<PointTDiff>::type FieldListOutT;
00297           typedef typename pcl::intersect<FieldListInT, FieldListOutT>::type FieldList;
00298           pcl::for_each_type <FieldList> (pcl::NdConcatenateFunctor <PointTDiff, PointT> (point, p));
00299           return (radiusSearch (p, radius, k_indices, k_sqr_distances, max_nn));
00300         }
00301 
00302         /** \brief Search for all the nearest neighbors of the query point in a given radius.
00303           *
00304           * \attention This method does not do any bounds checking for the input index
00305           * (i.e., index >= cloud.points.size () || index < 0), and assumes valid (i.e., finite) data.
00306           *
00307           * \param[in] cloud the point cloud data
00308           * \param[in] index a \a valid index in \a cloud representing a \a valid (i.e., finite) query point
00309           * \param[in] radius the radius of the sphere bounding all of p_q's neighbors
00310           * \param[out] k_indices the resultant indices of the neighboring points
00311           * \param[out] k_sqr_distances the resultant squared distances to the neighboring points
00312           * \param[in] max_nn if given, bounds the maximum returned neighbors to this value. If \a max_nn is set to
00313           * 0 or to a number higher than the number of points in the input cloud, all neighbors in \a radius will be
00314           * returned.
00315           * \return number of neighbors found in radius
00316           *
00317           * \exception asserts in debug mode if the index is not between 0 and the maximum number of points
00318           */
00319         virtual int
00320         radiusSearch (const PointCloud &cloud, int index, double radius,
00321                       std::vector<int> &k_indices, std::vector<float> &k_sqr_distances,
00322                       unsigned int max_nn = 0) const;
00323 
00324         /** \brief Search for all the nearest neighbors of the query point in a given radius (zero-copy).
00325           *
00326           * \attention This method does not do any bounds checking for the input index
00327           * (i.e., index >= cloud.points.size () || index < 0), and assumes valid (i.e., finite) data.
00328           *
00329           * \param[in] index a \a valid index representing a \a valid query point in the dataset given
00330           * by \a setInputCloud. If indices were given in setInputCloud, index will be the position in
00331           * the indices vector.
00332           *
00333           * \param[in] radius the radius of the sphere bounding all of p_q's neighbors
00334           * \param[out] k_indices the resultant indices of the neighboring points
00335           * \param[out] k_sqr_distances the resultant squared distances to the neighboring points
00336           * \param[in] max_nn if given, bounds the maximum returned neighbors to this value. If \a max_nn is set to
00337           * 0 or to a number higher than the number of points in the input cloud, all neighbors in \a radius will be
00338           * returned.
00339           * \return number of neighbors found in radius
00340           *
00341           * \exception asserts in debug mode if the index is not between 0 and the maximum number of points
00342           */
00343         virtual int
00344         radiusSearch (int index, double radius, std::vector<int> &k_indices,
00345                       std::vector<float> &k_sqr_distances, unsigned int max_nn = 0) const;
00346 
00347         /** \brief Search for all the nearest neighbors of the query point in a given radius.
00348           * \param[in] cloud the point cloud data
00349           * \param[in] indices the indices in \a cloud. If indices is empty, neighbors will be searched for all points.
00350           * \param[in] radius the radius of the sphere bounding all of p_q's neighbors
00351           * \param[out] k_indices the resultant indices of the neighboring points, k_indices[i] corresponds to the neighbors of the query point i
00352           * \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
00353           * \param[in] max_nn if given, bounds the maximum returned neighbors to this value. If \a max_nn is set to
00354           * 0 or to a number higher than the number of points in the input cloud, all neighbors in \a radius will be
00355           * returned.
00356           */
00357         virtual void
00358         radiusSearch (const PointCloud& cloud,
00359                       const std::vector<int>& indices,
00360                       double radius,
00361                       std::vector< std::vector<int> >& k_indices,
00362                       std::vector< std::vector<float> > &k_sqr_distances,
00363                       unsigned int max_nn = 0) const;
00364 
00365         /** \brief Search for all the nearest neighbors of the query points in a given radius.
00366           * \param[in] cloud the point cloud data
00367           * \param[in] indices a vector of point cloud indices to query for nearest neighbors
00368           * \param[in] radius the radius of the sphere bounding all of p_q's neighbors
00369           * \param[out] k_indices the resultant indices of the neighboring points, k_indices[i] corresponds to the neighbors of the query point i
00370           * \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
00371           * \param[in] max_nn if given, bounds the maximum returned neighbors to this value. If \a max_nn is set to
00372           * 0 or to a number higher than the number of points in the input cloud, all neighbors in \a radius will be
00373           * returned.
00374           * \note This method copies the input point cloud of type PointTDiff to a temporary cloud of type PointT and performs the batch search on the new cloud. You should prefer the single-point search if you don't use a search algorithm that accelerates batch NN search.
00375           */
00376         template <typename PointTDiff> void
00377         radiusSearchT (const pcl::PointCloud<PointTDiff> &cloud,
00378                        const std::vector<int>& indices,
00379                        double radius,
00380                        std::vector< std::vector<int> > &k_indices,
00381                        std::vector< std::vector<float> > &k_sqr_distances,
00382                        unsigned int max_nn = 0) const
00383         {
00384           // Copy all the data fields from the input cloud to the output one
00385           typedef typename pcl::traits::fieldList<PointT>::type FieldListInT;
00386           typedef typename pcl::traits::fieldList<PointTDiff>::type FieldListOutT;
00387           typedef typename pcl::intersect<FieldListInT, FieldListOutT>::type FieldList;
00388 
00389           pcl::PointCloud<PointT> pc;
00390           if (indices.empty ())
00391           {
00392             pc.resize (cloud.size ());
00393             for (size_t i = 0; i < cloud.size (); ++i)
00394               pcl::for_each_type <FieldList> (pcl::NdConcatenateFunctor <PointTDiff, PointT> (cloud[i], pc[i]));
00395             radiusSearch (pc, std::vector<int> (), radius, k_indices, k_sqr_distances, max_nn);
00396           }
00397           else
00398           {
00399             pc.resize (indices.size ());
00400             for (size_t i = 0; i < indices.size (); ++i)
00401               pcl::for_each_type <FieldList> (pcl::NdConcatenateFunctor <PointTDiff, PointT> (cloud[indices[i]], pc[i]));
00402             radiusSearch (pc, std::vector<int>(), radius, k_indices, k_sqr_distances, max_nn);
00403           }
00404         }
00405 
00406       protected:
00407         void 
00408         sortResults (std::vector<int>& indices, std::vector<float>& distances) const;
00409 
00410         PointCloudConstPtr input_;
00411         IndicesConstPtr indices_;
00412         bool sorted_results_;
00413         std::string name_;
00414         
00415       private:
00416         struct Compare
00417         {
00418           Compare (const std::vector<float>& distances)
00419           : distances_ (distances)
00420           {
00421           }
00422           
00423           bool 
00424           operator () (int first, int second) const
00425           {
00426             return (distances_ [first] < distances_[second]);
00427           }
00428 
00429           const std::vector<float>& distances_;
00430         };
00431     }; // class Search    
00432   } // namespace search
00433 } // namespace pcl
00434 
00435 #ifdef PCL_NO_PRECOMPILE
00436 #include <pcl/search/impl/search.hpp>
00437 #endif
00438 
00439 #endif  //#ifndef _PCL_SEARCH_SEARCH_H_