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-2012, 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 * $Id: kdtree_flann.h 36261 2011-02-26 01:34:42Z mariusm $ 00038 * 00039 */ 00040 00041 #ifndef PCL_KDTREE_KDTREE_FLANN_H_ 00042 #define PCL_KDTREE_KDTREE_FLANN_H_ 00043 00044 #include <pcl/kdtree/kdtree.h> 00045 00046 // Forward declarations 00047 namespace flann 00048 { 00049 struct SearchParams; 00050 template <typename T> struct L2_Simple; 00051 template <typename T> class Index; 00052 } 00053 00054 namespace pcl 00055 { 00056 // Forward declarations 00057 template <typename T> class PointRepresentation; 00058 00059 /** \brief KdTreeFLANN is a generic type of 3D spatial locator using kD-tree structures. The class is making use of 00060 * the FLANN (Fast Library for Approximate Nearest Neighbor) project by Marius Muja and David Lowe. 00061 * 00062 * \author Radu B. Rusu, Marius Muja 00063 * \ingroup kdtree 00064 */ 00065 template <typename PointT, typename Dist = ::flann::L2_Simple<float> > 00066 class KdTreeFLANN : public pcl::KdTree<PointT> 00067 { 00068 public: 00069 using KdTree<PointT>::input_; 00070 using KdTree<PointT>::indices_; 00071 using KdTree<PointT>::epsilon_; 00072 using KdTree<PointT>::sorted_; 00073 using KdTree<PointT>::point_representation_; 00074 using KdTree<PointT>::nearestKSearch; 00075 using KdTree<PointT>::radiusSearch; 00076 00077 typedef typename KdTree<PointT>::PointCloud PointCloud; 00078 typedef typename KdTree<PointT>::PointCloudConstPtr PointCloudConstPtr; 00079 00080 typedef boost::shared_ptr<std::vector<int> > IndicesPtr; 00081 typedef boost::shared_ptr<const std::vector<int> > IndicesConstPtr; 00082 00083 typedef ::flann::Index<Dist> FLANNIndex; 00084 00085 // Boost shared pointers 00086 typedef boost::shared_ptr<KdTreeFLANN<PointT> > Ptr; 00087 typedef boost::shared_ptr<const KdTreeFLANN<PointT> > ConstPtr; 00088 00089 /** \brief Default Constructor for KdTreeFLANN. 00090 * \param[in] sorted set to true if the application that the tree will be used for requires sorted nearest neighbor indices (default). False otherwise. 00091 * 00092 * By setting sorted to false, the \ref radiusSearch operations will be faster. 00093 */ 00094 KdTreeFLANN (bool sorted = true); 00095 00096 /** \brief Copy constructor 00097 * \param[in] tree the tree to copy into this 00098 */ 00099 KdTreeFLANN (const KdTreeFLANN<PointT> &k); 00100 00101 /** \brief Copy operator 00102 * \param[in] tree the tree to copy into this 00103 */ 00104 inline KdTreeFLANN<PointT>& 00105 operator = (const KdTreeFLANN<PointT>& k) 00106 { 00107 KdTree<PointT>::operator=(k); 00108 flann_index_ = k.flann_index_; 00109 cloud_ = k.cloud_; 00110 index_mapping_ = k.index_mapping_; 00111 identity_mapping_ = k.identity_mapping_; 00112 dim_ = k.dim_; 00113 total_nr_points_ = k.total_nr_points_; 00114 param_k_ = k.param_k_; 00115 param_radius_ = k.param_radius_; 00116 return (*this); 00117 } 00118 00119 /** \brief Set the search epsilon precision (error bound) for nearest neighbors searches. 00120 * \param[in] eps precision (error bound) for nearest neighbors searches 00121 */ 00122 void 00123 setEpsilon (float eps); 00124 00125 void 00126 setSortedResults (bool sorted); 00127 00128 inline Ptr makeShared () { return Ptr (new KdTreeFLANN<PointT> (*this)); } 00129 00130 /** \brief Destructor for KdTreeFLANN. 00131 * Deletes all allocated data arrays and destroys the kd-tree structures. 00132 */ 00133 virtual ~KdTreeFLANN () 00134 { 00135 cleanup (); 00136 } 00137 00138 /** \brief Provide a pointer to the input dataset. 00139 * \param[in] cloud the const boost shared pointer to a PointCloud message 00140 * \param[in] indices the point indices subset that is to be used from \a cloud - if NULL the whole cloud is used 00141 */ 00142 void 00143 setInputCloud (const PointCloudConstPtr &cloud, const IndicesConstPtr &indices = IndicesConstPtr ()); 00144 00145 /** \brief Search for k-nearest neighbors for the given query point. 00146 * 00147 * \attention This method does not do any bounds checking for the input index 00148 * (i.e., index >= cloud.points.size () || index < 0), and assumes valid (i.e., finite) data. 00149 * 00150 * \param[in] point a given \a valid (i.e., finite) 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 * \exception asserts in debug mode if the index is not between 0 and the maximum number of points 00158 */ 00159 int 00160 nearestKSearch (const PointT &point, int k, 00161 std::vector<int> &k_indices, std::vector<float> &k_sqr_distances) const; 00162 00163 /** \brief Search for all the nearest neighbors of the query point in a given radius. 00164 * 00165 * \attention This method does not do any bounds checking for the input index 00166 * (i.e., index >= cloud.points.size () || index < 0), and assumes valid (i.e., finite) data. 00167 * 00168 * \param[in] point a given \a valid (i.e., finite) query point 00169 * \param[in] radius the radius of the sphere bounding all of p_q's neighbors 00170 * \param[out] k_indices the resultant indices of the neighboring points 00171 * \param[out] k_sqr_distances the resultant squared distances to the neighboring points 00172 * \param[in] max_nn if given, bounds the maximum returned neighbors to this value. If \a max_nn is set to 00173 * 0 or to a number higher than the number of points in the input cloud, all neighbors in \a radius will be 00174 * returned. 00175 * \return number of neighbors found in radius 00176 * 00177 * \exception asserts in debug mode if the index is not between 0 and the maximum number of points 00178 */ 00179 int 00180 radiusSearch (const PointT &point, double radius, std::vector<int> &k_indices, 00181 std::vector<float> &k_sqr_distances, unsigned int max_nn = 0) const; 00182 00183 private: 00184 /** \brief Internal cleanup method. */ 00185 void 00186 cleanup (); 00187 00188 /** \brief Converts a PointCloud to the internal FLANN point array representation. Returns the number 00189 * of points. 00190 * \param cloud the PointCloud 00191 */ 00192 void 00193 convertCloudToArray (const PointCloud &cloud); 00194 00195 /** \brief Converts a PointCloud with a given set of indices to the internal FLANN point array 00196 * representation. Returns the number of points. 00197 * \param[in] cloud the PointCloud data 00198 * \param[in] indices the point cloud indices 00199 */ 00200 void 00201 convertCloudToArray (const PointCloud &cloud, const std::vector<int> &indices); 00202 00203 private: 00204 /** \brief Class getName method. */ 00205 virtual std::string 00206 getName () const { return ("KdTreeFLANN"); } 00207 00208 /** \brief A FLANN index object. */ 00209 boost::shared_ptr<FLANNIndex> flann_index_; 00210 00211 /** \brief Internal pointer to data. */ 00212 float* cloud_; 00213 00214 /** \brief mapping between internal and external indices. */ 00215 std::vector<int> index_mapping_; 00216 00217 /** \brief whether the mapping bwwteen internal and external indices is identity */ 00218 bool identity_mapping_; 00219 00220 /** \brief Tree dimensionality (i.e. the number of dimensions per point). */ 00221 int dim_; 00222 00223 /** \brief The total size of the data (either equal to the number of points in the input cloud or to the number of indices - if passed). */ 00224 int total_nr_points_; 00225 00226 /** \brief The KdTree search parameters for K-nearest neighbors. */ 00227 boost::shared_ptr<flann::SearchParams> param_k_; 00228 00229 /** \brief The KdTree search parameters for radius search. */ 00230 boost::shared_ptr<flann::SearchParams> param_radius_; 00231 }; 00232 } 00233 00234 #ifdef PCL_NO_PRECOMPILE 00235 #include <pcl/kdtree/impl/kdtree_flann.hpp> 00236 #endif 00237 00238 #endif