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 00040 #ifndef PCL_SEARCH_IMPL_FLANN_SEARCH_H_ 00041 #define PCL_SEARCH_IMPL_FLANN_SEARCH_H_ 00042 00043 #include <pcl/search/flann_search.h> 00044 #include <pcl/kdtree/flann.h> 00045 00046 ////////////////////////////////////////////////////////////////////////////////////////////// 00047 template <typename PointT, typename FlannDistance> 00048 typename pcl::search::FlannSearch<PointT, FlannDistance>::IndexPtr 00049 pcl::search::FlannSearch<PointT, FlannDistance>::KdTreeIndexCreator::createIndex (MatrixConstPtr data) 00050 { 00051 return (IndexPtr (new flann::KDTreeSingleIndex<FlannDistance> (*data,flann::KDTreeSingleIndexParams (max_leaf_size_)))); 00052 } 00053 00054 ////////////////////////////////////////////////////////////////////////////////////////////// 00055 template <typename PointT, typename FlannDistance> 00056 typename pcl::search::FlannSearch<PointT, FlannDistance>::IndexPtr 00057 pcl::search::FlannSearch<PointT, FlannDistance>::KMeansIndexCreator::createIndex (MatrixConstPtr data) 00058 { 00059 return (IndexPtr (new flann::KMeansIndex<FlannDistance> (*data,flann::KMeansIndexParams ()))); 00060 } 00061 00062 ////////////////////////////////////////////////////////////////////////////////////////////// 00063 template <typename PointT, typename FlannDistance> 00064 pcl::search::FlannSearch<PointT, FlannDistance>::FlannSearch(bool sorted, FlannIndexCreatorPtr creator) : pcl::search::Search<PointT> ("FlannSearch",sorted), 00065 index_(), creator_ (creator), input_flann_(), eps_ (0), input_copied_for_flann_ (false), point_representation_ (new DefaultPointRepresentation<PointT>), 00066 dim_ (0), index_mapping_(), identity_mapping_() 00067 { 00068 dim_ = point_representation_->getNumberOfDimensions (); 00069 } 00070 00071 ////////////////////////////////////////////////////////////////////////////////////////////// 00072 template <typename PointT, typename FlannDistance> 00073 pcl::search::FlannSearch<PointT, FlannDistance>::~FlannSearch() 00074 { 00075 if (input_copied_for_flann_) 00076 delete [] input_flann_->ptr(); 00077 } 00078 00079 ////////////////////////////////////////////////////////////////////////////////////////////// 00080 template <typename PointT, typename FlannDistance> void 00081 pcl::search::FlannSearch<PointT, FlannDistance>::setInputCloud (const PointCloudConstPtr& cloud, const IndicesConstPtr& indices) 00082 { 00083 input_ = cloud; 00084 indices_ = indices; 00085 convertInputToFlannMatrix (); 00086 index_ = creator_->createIndex (input_flann_); 00087 index_->buildIndex (); 00088 } 00089 00090 ////////////////////////////////////////////////////////////////////////////////////////////// 00091 template <typename PointT, typename FlannDistance> int 00092 pcl::search::FlannSearch<PointT, FlannDistance>::nearestKSearch (const PointT &point, int k, std::vector<int> &indices, std::vector<float> &dists) const 00093 { 00094 assert (point_representation_->isValid (point) && "Invalid (NaN, Inf) point coordinates given to nearestKSearch!"); // remove this check as soon as FLANN does NaN checks internally 00095 bool can_cast = point_representation_->isTrivial (); 00096 00097 float* data = 0; 00098 if (!can_cast) 00099 { 00100 data = new float [point_representation_->getNumberOfDimensions ()]; 00101 point_representation_->vectorize (point,data); 00102 } 00103 00104 float* cdata = can_cast ? const_cast<float*> (reinterpret_cast<const float*> (&point)): data; 00105 const flann::Matrix<float> m (cdata ,1, point_representation_->getNumberOfDimensions ()); 00106 00107 flann::SearchParams p(-1); 00108 p.eps = eps_; 00109 p.sorted = sorted_results_; 00110 if (indices.size() != static_cast<unsigned int> (k)) 00111 indices.resize (k,-1); 00112 if (dists.size() != static_cast<unsigned int> (k)) 00113 dists.resize (k); 00114 flann::Matrix<int> i (&indices[0],1,k); 00115 flann::Matrix<float> d (&dists[0],1,k); 00116 int result = index_->knnSearch (m,i,d,k, p); 00117 00118 delete [] data; 00119 00120 if (!identity_mapping_) 00121 { 00122 for (size_t i = 0; i < static_cast<unsigned int> (k); ++i) 00123 { 00124 int& neighbor_index = indices[i]; 00125 neighbor_index = index_mapping_[neighbor_index]; 00126 } 00127 } 00128 return result; 00129 } 00130 00131 ////////////////////////////////////////////////////////////////////////////////////////////// 00132 template <typename PointT, typename FlannDistance> void 00133 pcl::search::FlannSearch<PointT, FlannDistance>::nearestKSearch ( 00134 const PointCloud& cloud, const std::vector<int>& indices, int k, std::vector< std::vector<int> >& k_indices, 00135 std::vector< std::vector<float> >& k_sqr_distances) const 00136 { 00137 if (indices.empty ()) 00138 { 00139 k_indices.resize (cloud.size ()); 00140 k_sqr_distances.resize (cloud.size ()); 00141 00142 if (! cloud.is_dense) // remove this check as soon as FLANN does NaN checks internally 00143 { 00144 for (size_t i = 0; i < cloud.size(); i++) 00145 { 00146 assert (point_representation_->isValid (cloud[i]) && "Invalid (NaN, Inf) point coordinates given to nearestKSearch!"); 00147 } 00148 } 00149 00150 bool can_cast = point_representation_->isTrivial (); 00151 00152 // full point cloud + trivial copy operation = no need to do any conversion/copying to the flann matrix! 00153 float* data=0; 00154 if (!can_cast) 00155 { 00156 data = new float[dim_*cloud.size ()]; 00157 for (size_t i = 0; i < cloud.size (); ++i) 00158 { 00159 float* out = data+i*dim_; 00160 point_representation_->vectorize (cloud[i],out); 00161 } 00162 } 00163 00164 // const cast is evil, but the matrix constructor won't change the data, and the 00165 // search won't change the matrix 00166 float* cdata = can_cast ? const_cast<float*> (reinterpret_cast<const float*> (&cloud[0])): data; 00167 const flann::Matrix<float> m (cdata ,cloud.size (), dim_, can_cast ? sizeof (PointT) : dim_ * sizeof (float) ); 00168 00169 flann::SearchParams p; 00170 p.sorted = sorted_results_; 00171 p.eps = eps_; 00172 index_->knnSearch (m,k_indices,k_sqr_distances,k, p); 00173 00174 delete [] data; 00175 } 00176 else // if indices are present, the cloud has to be copied anyway. Only copy the relevant parts of the points here. 00177 { 00178 k_indices.resize (indices.size ()); 00179 k_sqr_distances.resize (indices.size ()); 00180 00181 if (! cloud.is_dense) // remove this check as soon as FLANN does NaN checks internally 00182 { 00183 for (size_t i = 0; i < indices.size(); i++) 00184 { 00185 assert (point_representation_->isValid (cloud [indices[i]]) && "Invalid (NaN, Inf) point coordinates given to nearestKSearch!"); 00186 } 00187 } 00188 00189 float* data=new float [dim_*indices.size ()]; 00190 for (size_t i = 0; i < indices.size (); ++i) 00191 { 00192 float* out = data+i*dim_; 00193 point_representation_->vectorize (cloud[indices[i]],out); 00194 } 00195 const flann::Matrix<float> m (data ,indices.size (), point_representation_->getNumberOfDimensions ()); 00196 00197 flann::SearchParams p; 00198 p.sorted = sorted_results_; 00199 p.eps = eps_; 00200 index_->knnSearch (m,k_indices,k_sqr_distances,k, p); 00201 00202 delete[] data; 00203 } 00204 if (!identity_mapping_) 00205 { 00206 for (size_t j = 0; j < k_indices.size (); ++j) 00207 { 00208 for (size_t i = 0; i < static_cast<unsigned int> (k); ++i) 00209 { 00210 int& neighbor_index = k_indices[j][i]; 00211 neighbor_index = index_mapping_[neighbor_index]; 00212 } 00213 } 00214 } 00215 } 00216 00217 ////////////////////////////////////////////////////////////////////////////////////////////// 00218 template <typename PointT, typename FlannDistance> int 00219 pcl::search::FlannSearch<PointT, FlannDistance>::radiusSearch (const PointT& point, double radius, 00220 std::vector<int> &indices, std::vector<float> &distances, 00221 unsigned int max_nn) const 00222 { 00223 assert (point_representation_->isValid (point) && "Invalid (NaN, Inf) point coordinates given to radiusSearch!"); // remove this check as soon as FLANN does NaN checks internally 00224 bool can_cast = point_representation_->isTrivial (); 00225 00226 float* data = 0; 00227 if (!can_cast) 00228 { 00229 data = new float [point_representation_->getNumberOfDimensions ()]; 00230 point_representation_->vectorize (point,data); 00231 } 00232 00233 float* cdata = can_cast ? const_cast<float*> (reinterpret_cast<const float*> (&point)) : data; 00234 const flann::Matrix<float> m (cdata ,1, point_representation_->getNumberOfDimensions ()); 00235 00236 flann::SearchParams p; 00237 p.sorted = sorted_results_; 00238 p.eps = eps_; 00239 p.max_neighbors = max_nn > 0 ? max_nn : -1; 00240 std::vector<std::vector<int> > i (1); 00241 std::vector<std::vector<float> > d (1); 00242 int result = index_->radiusSearch (m,i,d,static_cast<float> (radius * radius), p); 00243 00244 delete [] data; 00245 indices = i [0]; 00246 distances = d [0]; 00247 00248 if (!identity_mapping_) 00249 { 00250 for (size_t i = 0; i < indices.size (); ++i) 00251 { 00252 int& neighbor_index = indices [i]; 00253 neighbor_index = index_mapping_ [neighbor_index]; 00254 } 00255 } 00256 return result; 00257 } 00258 00259 ////////////////////////////////////////////////////////////////////////////////////////////// 00260 template <typename PointT, typename FlannDistance> void 00261 pcl::search::FlannSearch<PointT, FlannDistance>::radiusSearch ( 00262 const PointCloud& cloud, const std::vector<int>& indices, double radius, std::vector< std::vector<int> >& k_indices, 00263 std::vector< std::vector<float> >& k_sqr_distances, unsigned int max_nn) const 00264 { 00265 if (indices.empty ()) // full point cloud + trivial copy operation = no need to do any conversion/copying to the flann matrix! 00266 { 00267 k_indices.resize (cloud.size ()); 00268 k_sqr_distances.resize (cloud.size ()); 00269 00270 if (! cloud.is_dense) // remove this check as soon as FLANN does NaN checks internally 00271 { 00272 for (size_t i = 0; i < cloud.size(); i++) 00273 { 00274 assert (point_representation_->isValid (cloud[i]) && "Invalid (NaN, Inf) point coordinates given to radiusSearch!"); 00275 } 00276 } 00277 00278 bool can_cast = point_representation_->isTrivial (); 00279 00280 float* data = 0; 00281 if (!can_cast) 00282 { 00283 data = new float[dim_*cloud.size ()]; 00284 for (size_t i = 0; i < cloud.size (); ++i) 00285 { 00286 float* out = data+i*dim_; 00287 point_representation_->vectorize (cloud[i],out); 00288 } 00289 } 00290 00291 float* cdata = can_cast ? const_cast<float*> (reinterpret_cast<const float*> (&cloud[0])) : data; 00292 const flann::Matrix<float> m (cdata ,cloud.size (), dim_, can_cast ? sizeof (PointT) : dim_ * sizeof (float)); 00293 00294 flann::SearchParams p; 00295 p.sorted = sorted_results_; 00296 p.eps = eps_; 00297 // here: max_nn==0: take all neighbors. flann: max_nn==0: return no neighbors, only count them. max_nn==-1: return all neighbors 00298 p.max_neighbors = max_nn != 0 ? max_nn : -1; 00299 index_->radiusSearch (m,k_indices,k_sqr_distances,static_cast<float> (radius * radius), p); 00300 00301 delete [] data; 00302 } 00303 else // if indices are present, the cloud has to be copied anyway. Only copy the relevant parts of the points here. 00304 { 00305 k_indices.resize (indices.size ()); 00306 k_sqr_distances.resize (indices.size ()); 00307 00308 if (! cloud.is_dense) // remove this check as soon as FLANN does NaN checks internally 00309 { 00310 for (size_t i = 0; i < indices.size(); i++) 00311 { 00312 assert (point_representation_->isValid (cloud [indices[i]]) && "Invalid (NaN, Inf) point coordinates given to radiusSearch!"); 00313 } 00314 } 00315 00316 float* data = new float [dim_ * indices.size ()]; 00317 for (size_t i = 0; i < indices.size (); ++i) 00318 { 00319 float* out = data+i*dim_; 00320 point_representation_->vectorize (cloud[indices[i]], out); 00321 } 00322 const flann::Matrix<float> m (data, cloud.size (), point_representation_->getNumberOfDimensions ()); 00323 00324 flann::SearchParams p; 00325 p.sorted = sorted_results_; 00326 p.eps = eps_; 00327 // here: max_nn==0: take all neighbors. flann: max_nn==0: return no neighbors, only count them. max_nn==-1: return all neighbors 00328 p.max_neighbors = max_nn != 0 ? max_nn : -1; 00329 index_->radiusSearch (m, k_indices, k_sqr_distances, static_cast<float> (radius * radius), p); 00330 00331 delete[] data; 00332 } 00333 if (!identity_mapping_) 00334 { 00335 for (size_t j = 0; j < k_indices.size (); ++j ) 00336 { 00337 for (size_t i = 0; i < k_indices[j].size (); ++i) 00338 { 00339 int& neighbor_index = k_indices[j][i]; 00340 neighbor_index = index_mapping_[neighbor_index]; 00341 } 00342 } 00343 } 00344 } 00345 00346 ////////////////////////////////////////////////////////////////////////////////////////////// 00347 template <typename PointT, typename FlannDistance> void 00348 pcl::search::FlannSearch<PointT, FlannDistance>::convertInputToFlannMatrix () 00349 { 00350 size_t original_no_of_points = indices_ && !indices_->empty () ? indices_->size () : input_->size (); 00351 00352 if (input_copied_for_flann_) 00353 delete input_flann_->ptr(); 00354 input_copied_for_flann_ = true; 00355 index_mapping_.clear(); 00356 identity_mapping_ = true; 00357 00358 //cloud_ = (float*)malloc (original_no_of_points * dim_ * sizeof (float)); 00359 //index_mapping_.reserve(original_no_of_points); 00360 //identity_mapping_ = true; 00361 00362 if (!indices_ || indices_->empty ()) 00363 { 00364 // best case: all points can be passed to flann without any conversions 00365 if (input_->is_dense && point_representation_->isTrivial ()) 00366 { 00367 // const cast is evil, but flann won't change the data 00368 input_flann_ = MatrixPtr (new flann::Matrix<float> (const_cast<float*>(reinterpret_cast<const float*>(&(*input_) [0])), original_no_of_points, point_representation_->getNumberOfDimensions (),sizeof (PointT))); 00369 input_copied_for_flann_ = false; 00370 } 00371 else 00372 { 00373 input_flann_ = MatrixPtr (new flann::Matrix<float> (new float[original_no_of_points*point_representation_->getNumberOfDimensions ()], original_no_of_points, point_representation_->getNumberOfDimensions ())); 00374 float* cloud_ptr = input_flann_->ptr(); 00375 for (size_t i = 0; i < original_no_of_points; ++i) 00376 { 00377 const PointT& point = (*input_)[i]; 00378 // Check if the point is invalid 00379 if (!point_representation_->isValid (point)) 00380 { 00381 identity_mapping_ = false; 00382 continue; 00383 } 00384 00385 index_mapping_.push_back (static_cast<int> (i)); // If the returned index should be for the indices vector 00386 00387 point_representation_->vectorize (point, cloud_ptr); 00388 cloud_ptr += dim_; 00389 } 00390 } 00391 00392 } 00393 else 00394 { 00395 input_flann_ = MatrixPtr (new flann::Matrix<float> (new float[original_no_of_points*point_representation_->getNumberOfDimensions ()], original_no_of_points, point_representation_->getNumberOfDimensions ())); 00396 float* cloud_ptr = input_flann_->ptr(); 00397 for (size_t indices_index = 0; indices_index < original_no_of_points; ++indices_index) 00398 { 00399 int cloud_index = (*indices_)[indices_index]; 00400 const PointT& point = (*input_)[cloud_index]; 00401 // Check if the point is invalid 00402 if (!point_representation_->isValid (point)) 00403 { 00404 identity_mapping_ = false; 00405 continue; 00406 } 00407 00408 index_mapping_.push_back (static_cast<int> (indices_index)); // If the returned index should be for the indices vector 00409 00410 point_representation_->vectorize (point, cloud_ptr); 00411 cloud_ptr += dim_; 00412 } 00413 } 00414 if (input_copied_for_flann_) 00415 input_flann_->rows = index_mapping_.size (); 00416 } 00417 00418 #define PCL_INSTANTIATE_FlannSearch(T) template class PCL_EXPORTS pcl::search::FlannSearch<T>; 00419 00420 #endif