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 * 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