Point Cloud Library (PCL)
1.3.1
|
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 Willow Garage, Inc. 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: kdtree_flann.h 3020 2011-11-01 03:41:16Z rusu $ 00037 * 00038 */ 00039 00040 #ifndef PCL_KDTREE_KDTREE_FLANN_H_ 00041 #define PCL_KDTREE_KDTREE_FLANN_H_ 00042 00043 #include <cstdio> 00044 #include <pcl/kdtree/kdtree.h> 00045 #include <boost/thread/mutex.hpp> 00046 00047 namespace flann 00048 { 00049 template <typename Distance> 00050 class Index; 00051 00052 template <class T> 00053 struct L2_Simple; 00054 } 00055 00056 namespace pcl 00057 { 00064 template <typename PointT, typename Dist = flann::L2_Simple<float> > 00065 class KdTreeFLANN : public pcl::KdTree<PointT> 00066 { 00067 using KdTree<PointT>::input_; 00068 using KdTree<PointT>::indices_; 00069 using KdTree<PointT>::epsilon_; 00070 using KdTree<PointT>::sorted_; 00071 using KdTree<PointT>::point_representation_; 00072 00073 typedef typename KdTree<PointT>::PointCloud PointCloud; 00074 typedef typename KdTree<PointT>::PointCloudConstPtr PointCloudConstPtr; 00075 00076 typedef boost::shared_ptr <std::vector<int> > IndicesPtr; 00077 typedef boost::shared_ptr <const std::vector<int> > IndicesConstPtr; 00078 00079 typedef flann::Index <Dist> FLANNIndex; 00080 00081 public: 00082 // Boost shared pointers 00083 typedef boost::shared_ptr<KdTreeFLANN<PointT> > Ptr; 00084 typedef boost::shared_ptr<const KdTreeFLANN<PointT> > ConstPtr; 00085 00091 KdTreeFLANN (bool sorted = true) : pcl::KdTree<PointT> (sorted), flann_index_(NULL), cloud_(NULL) 00092 { 00093 cleanup (); 00094 } 00095 00104 KdTreeFLANN (KdTreeFLANN& tree) : pcl::KdTree<PointT> (tree) 00105 { 00106 shallowCopy (tree); 00107 } 00108 00109 inline Ptr makeShared () { return Ptr (new KdTreeFLANN<PointT> (*this)); } 00110 00111 KdTreeFLANN& operator= (const KdTreeFLANN& tree) 00112 { 00113 if (this != &tree) 00114 { 00115 shallowCopy (tree); 00116 } 00117 return (*this); 00118 } 00119 00120 00124 inline void 00125 shallowCopy (const KdTreeFLANN& tree) 00126 { 00127 flann_index_ = tree.flann_index_; 00128 cloud_ = tree.cloud_; 00129 index_mapping_ = tree.index_mapping_; 00130 dim_ = tree.dim_; 00131 sorted_ = tree.sorted_; 00132 } 00133 00134 00138 virtual ~KdTreeFLANN () 00139 { 00140 cleanup (); 00141 } 00142 00147 void 00148 setInputCloud (const PointCloudConstPtr &cloud, const IndicesConstPtr &indices = IndicesConstPtr ()); 00149 00158 int 00159 nearestKSearch (const PointT &point, int k, 00160 std::vector<int> &k_indices, std::vector<float> &k_distances); 00161 00171 inline int 00172 nearestKSearch (const PointCloud &cloud, int index, int k, 00173 std::vector<int> &k_indices, std::vector<float> &k_distances) 00174 { 00175 if (index >= (int)cloud.points.size ()) 00176 return (0); 00177 return (nearestKSearch (cloud.points[index], k, k_indices, k_distances)); 00178 } 00179 00189 inline int 00190 nearestKSearch (int index, int k, 00191 std::vector<int> &k_indices, std::vector<float> &k_distances) 00192 { 00193 if (indices_ == NULL) 00194 { 00195 if (index >= (int)input_->points.size ()) 00196 return (0); 00197 return (nearestKSearch (input_->points[index], k, k_indices, k_distances)); 00198 } 00199 else 00200 { 00201 if (index >= (int)indices_->size()) 00202 return (0); 00203 return (nearestKSearch (input_->points[(*indices_)[index]], k, k_indices, k_distances)); 00204 } 00205 } 00206 00215 int 00216 radiusSearch (const PointT &point, double radius, std::vector<int> &k_indices, 00217 std::vector<float> &k_distances, int max_nn = -1) const; 00218 00228 inline int 00229 radiusSearch (const PointCloud &cloud, int index, double radius, 00230 std::vector<int> &k_indices, std::vector<float> &k_distances, 00231 int max_nn = -1) const 00232 { 00233 if (index >= (int)cloud.points.size ()) 00234 return 0; 00235 return (radiusSearch(cloud.points[index], radius, k_indices, k_distances, max_nn)); 00236 } 00237 00247 inline int 00248 radiusSearch (int index, double radius, std::vector<int> &k_indices, 00249 std::vector<float> &k_distances, int max_nn = -1) const 00250 { 00251 if (indices_ == NULL) 00252 { 00253 if (index >= (int)input_->points.size ()) 00254 return 0; 00255 return (radiusSearch (input_->points[index], radius, k_indices, k_distances, max_nn)); 00256 } 00257 else 00258 { 00259 if (index >= (int)indices_->size ()) 00260 return 0; 00261 return (radiusSearch (input_->points[(*indices_)[index]], radius, k_indices, k_distances, max_nn)); 00262 } 00263 } 00264 00265 private: 00267 void cleanup (); 00268 00270 bool initParameters (); 00271 00273 void initData (); 00274 00279 void 00280 convertCloudToArray (const PointCloud &ros_cloud); 00281 00287 void 00288 convertCloudToArray (const PointCloud &cloud, const std::vector<int> &indices); 00289 00290 private: 00292 virtual std::string 00293 getName () const { return ("KdTreeFLANN"); } 00294 00295 boost::mutex m_lock_; 00296 00298 FLANNIndex* flann_index_; 00299 00301 float* cloud_; 00302 00304 std::vector<int> index_mapping_; 00305 00307 bool identity_mapping_; 00308 00310 int dim_; 00311 }; 00312 } 00313 00314 #endif