Point Cloud Library (PCL)
1.7.0
|
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_