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 * $Id$ 00038 */ 00039 00040 #ifndef PCL_SEARCH_KDTREE_H_ 00041 #define PCL_SEARCH_KDTREE_H_ 00042 00043 #include <pcl/search/search.h> 00044 #include <pcl/kdtree/kdtree_flann.h> 00045 00046 namespace pcl 00047 { 00048 // Forward declarations 00049 template <typename T> class PointRepresentation; 00050 00051 namespace search 00052 { 00053 /** \brief @b search::KdTree is a wrapper class which inherits the pcl::KdTree class for performing search 00054 * functions using KdTree structure. KdTree is a generic type of 3D spatial locator using kD-tree structures. 00055 * The class is making use of the FLANN (Fast Library for Approximate Nearest Neighbor) project 00056 * by Marius Muja and David Lowe. 00057 * 00058 * \author Radu B. Rusu 00059 * \ingroup search 00060 */ 00061 template<typename PointT> 00062 class KdTree: public Search<PointT> 00063 { 00064 public: 00065 typedef typename Search<PointT>::PointCloud PointCloud; 00066 typedef typename Search<PointT>::PointCloudConstPtr PointCloudConstPtr; 00067 00068 typedef boost::shared_ptr<std::vector<int> > IndicesPtr; 00069 typedef boost::shared_ptr<const std::vector<int> > IndicesConstPtr; 00070 00071 using pcl::search::Search<PointT>::indices_; 00072 using pcl::search::Search<PointT>::input_; 00073 using pcl::search::Search<PointT>::getIndices; 00074 using pcl::search::Search<PointT>::getInputCloud; 00075 using pcl::search::Search<PointT>::nearestKSearch; 00076 using pcl::search::Search<PointT>::radiusSearch; 00077 using pcl::search::Search<PointT>::sorted_results_; 00078 00079 typedef boost::shared_ptr<KdTree<PointT> > Ptr; 00080 typedef boost::shared_ptr<const KdTree<PointT> > ConstPtr; 00081 00082 typedef boost::shared_ptr<pcl::KdTreeFLANN<PointT> > KdTreeFLANNPtr; 00083 typedef boost::shared_ptr<const pcl::KdTreeFLANN<PointT> > KdTreeFLANNConstPtr; 00084 typedef boost::shared_ptr<const PointRepresentation<PointT> > PointRepresentationConstPtr; 00085 00086 /** \brief Constructor for KdTree. 00087 * 00088 * \param[in] sorted set to true if the nearest neighbor search results 00089 * need to be sorted in ascending order based on their distance to the 00090 * query point 00091 * 00092 */ 00093 KdTree (bool sorted = true); 00094 00095 /** \brief Destructor for KdTree. */ 00096 virtual 00097 ~KdTree () 00098 { 00099 } 00100 00101 /** \brief Provide a pointer to the point representation to use to convert points into k-D vectors. 00102 * \param[in] point_representation the const boost shared pointer to a PointRepresentation 00103 */ 00104 void 00105 setPointRepresentation (const PointRepresentationConstPtr &point_representation); 00106 00107 /** \brief Get a pointer to the point representation used when converting points into k-D vectors. */ 00108 inline PointRepresentationConstPtr 00109 getPointRepresentation () const 00110 { 00111 return (tree_->getPointRepresentation ()); 00112 } 00113 00114 /** \brief Sets whether the results have to be sorted or not. 00115 * \param[in] sorted_results set to true if the radius search results should be sorted 00116 */ 00117 void 00118 setSortedResults (bool sorted_results); 00119 00120 /** \brief Set the search epsilon precision (error bound) for nearest neighbors searches. 00121 * \param[in] eps precision (error bound) for nearest neighbors searches 00122 */ 00123 void 00124 setEpsilon (float eps); 00125 00126 /** \brief Get the search epsilon precision (error bound) for nearest neighbors searches. */ 00127 inline float 00128 getEpsilon () const 00129 { 00130 return (tree_->getEpsilon ()); 00131 } 00132 00133 /** \brief Provide a pointer to the input dataset. 00134 * \param[in] cloud the const boost shared pointer to a PointCloud message 00135 * \param[in] indices the point indices subset that is to be used from \a cloud 00136 */ 00137 void 00138 setInputCloud (const PointCloudConstPtr& cloud, 00139 const IndicesConstPtr& indices = IndicesConstPtr ()); 00140 00141 /** \brief Search for the k-nearest neighbors for the given query point. 00142 * \param[in] point the given query point 00143 * \param[in] k the number of neighbors to search for 00144 * \param[out] k_indices the resultant indices of the neighboring points (must be resized to \a k a priori!) 00145 * \param[out] k_sqr_distances the resultant squared distances to the neighboring points (must be resized to \a k 00146 * a priori!) 00147 * \return number of neighbors found 00148 */ 00149 int 00150 nearestKSearch (const PointT &point, int k, 00151 std::vector<int> &k_indices, 00152 std::vector<float> &k_sqr_distances) const; 00153 00154 /** \brief Search for all the nearest neighbors of the query point in a given radius. 00155 * \param[in] point the given query point 00156 * \param[in] radius the radius of the sphere bounding all of p_q's neighbors 00157 * \param[out] k_indices the resultant indices of the neighboring points 00158 * \param[out] k_sqr_distances the resultant squared distances to the neighboring points 00159 * \param[in] max_nn if given, bounds the maximum returned neighbors to this value. If \a max_nn is set to 00160 * 0 or to a number higher than the number of points in the input cloud, all neighbors in \a radius will be 00161 * returned. 00162 * \return number of neighbors found in radius 00163 */ 00164 int 00165 radiusSearch (const PointT& point, double radius, 00166 std::vector<int> &k_indices, 00167 std::vector<float> &k_sqr_distances, 00168 unsigned int max_nn = 0) const; 00169 protected: 00170 /** \brief A pointer to the internal KdTreeFLANN object. */ 00171 KdTreeFLANNPtr tree_; 00172 }; 00173 } 00174 } 00175 00176 #define PCL_INSTANTIATE_KdTree(T) template class PCL_EXPORTS pcl::search::KdTree<T>; 00177 00178 #endif // PCL_SEARCH_KDTREE_H_ 00179