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) 2009-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 */ 00038 00039 #ifndef PCL_KDTREE_KDTREE_IMPL_FLANN_H_ 00040 #define PCL_KDTREE_KDTREE_IMPL_FLANN_H_ 00041 00042 #include <cstdio> 00043 #include <pcl/kdtree/kdtree_flann.h> 00044 #include <pcl/kdtree/flann.h> 00045 #include <pcl/console/print.h> 00046 00047 /////////////////////////////////////////////////////////////////////////////////////////// 00048 template <typename PointT, typename Dist> 00049 pcl::KdTreeFLANN<PointT, Dist>::KdTreeFLANN (bool sorted) 00050 : pcl::KdTree<PointT> (sorted) 00051 , flann_index_ (), cloud_ (NULL) 00052 , index_mapping_ (), identity_mapping_ (false) 00053 , dim_ (0), total_nr_points_ (0) 00054 , param_k_ (new ::flann::SearchParams (-1 , epsilon_)) 00055 , param_radius_ (new ::flann::SearchParams (-1, epsilon_, sorted)) 00056 { 00057 } 00058 00059 /////////////////////////////////////////////////////////////////////////////////////////// 00060 template <typename PointT, typename Dist> 00061 pcl::KdTreeFLANN<PointT, Dist>::KdTreeFLANN (const KdTreeFLANN<PointT> &k) 00062 : pcl::KdTree<PointT> (false) 00063 , flann_index_ (), cloud_ (NULL) 00064 , index_mapping_ (), identity_mapping_ (false) 00065 , dim_ (0), total_nr_points_ (0) 00066 , param_k_ (new ::flann::SearchParams (-1 , epsilon_)) 00067 , param_radius_ (new ::flann::SearchParams (-1, epsilon_, false)) 00068 { 00069 *this = k; 00070 } 00071 00072 /////////////////////////////////////////////////////////////////////////////////////////// 00073 template <typename PointT, typename Dist> void 00074 pcl::KdTreeFLANN<PointT, Dist>::setEpsilon (float eps) 00075 { 00076 epsilon_ = eps; 00077 param_k_.reset (new ::flann::SearchParams (-1 , epsilon_)); 00078 param_radius_.reset (new ::flann::SearchParams (-1 , epsilon_, sorted_)); 00079 } 00080 00081 /////////////////////////////////////////////////////////////////////////////////////////// 00082 template <typename PointT, typename Dist> void 00083 pcl::KdTreeFLANN<PointT, Dist>::setSortedResults (bool sorted) 00084 { 00085 sorted_ = sorted; 00086 param_k_.reset (new ::flann::SearchParams (-1, epsilon_)); 00087 param_radius_.reset (new ::flann::SearchParams (-1, epsilon_, sorted_)); 00088 } 00089 00090 /////////////////////////////////////////////////////////////////////////////////////////// 00091 template <typename PointT, typename Dist> void 00092 pcl::KdTreeFLANN<PointT, Dist>::setInputCloud (const PointCloudConstPtr &cloud, const IndicesConstPtr &indices) 00093 { 00094 cleanup (); // Perform an automatic cleanup of structures 00095 00096 epsilon_ = 0.0f; // default error bound value 00097 dim_ = point_representation_->getNumberOfDimensions (); // Number of dimensions - default is 3 = xyz 00098 00099 input_ = cloud; 00100 indices_ = indices; 00101 00102 // Allocate enough data 00103 if (!input_) 00104 { 00105 PCL_ERROR ("[pcl::KdTreeFLANN::setInputCloud] Invalid input!\n"); 00106 return; 00107 } 00108 if (indices != NULL) 00109 { 00110 convertCloudToArray (*input_, *indices_); 00111 } 00112 else 00113 { 00114 convertCloudToArray (*input_); 00115 } 00116 total_nr_points_ = static_cast<int> (index_mapping_.size ()); 00117 if (total_nr_points_ == 0) 00118 { 00119 PCL_ERROR ("[pcl::KdTreeFLANN::setInputCloud] Cannot create a KDTree with an empty input cloud!\n"); 00120 return; 00121 } 00122 00123 flann_index_.reset (new FLANNIndex (::flann::Matrix<float> (cloud_, 00124 index_mapping_.size (), 00125 dim_), 00126 ::flann::KDTreeSingleIndexParams (15))); // max 15 points/leaf 00127 flann_index_->buildIndex (); 00128 } 00129 00130 /////////////////////////////////////////////////////////////////////////////////////////// 00131 template <typename PointT, typename Dist> int 00132 pcl::KdTreeFLANN<PointT, Dist>::nearestKSearch (const PointT &point, int k, 00133 std::vector<int> &k_indices, 00134 std::vector<float> &k_distances) const 00135 { 00136 assert (point_representation_->isValid (point) && "Invalid (NaN, Inf) point coordinates given to nearestKSearch!"); 00137 00138 if (k > total_nr_points_) 00139 k = total_nr_points_; 00140 00141 k_indices.resize (k); 00142 k_distances.resize (k); 00143 00144 std::vector<float> query (dim_); 00145 point_representation_->vectorize (static_cast<PointT> (point), query); 00146 00147 ::flann::Matrix<int> k_indices_mat (&k_indices[0], 1, k); 00148 ::flann::Matrix<float> k_distances_mat (&k_distances[0], 1, k); 00149 // Wrap the k_indices and k_distances vectors (no data copy) 00150 flann_index_->knnSearch (::flann::Matrix<float> (&query[0], 1, dim_), 00151 k_indices_mat, k_distances_mat, 00152 k, *param_k_); 00153 00154 // Do mapping to original point cloud 00155 if (!identity_mapping_) 00156 { 00157 for (size_t i = 0; i < static_cast<size_t> (k); ++i) 00158 { 00159 int& neighbor_index = k_indices[i]; 00160 neighbor_index = index_mapping_[neighbor_index]; 00161 } 00162 } 00163 00164 return (k); 00165 } 00166 00167 /////////////////////////////////////////////////////////////////////////////////////////// 00168 template <typename PointT, typename Dist> int 00169 pcl::KdTreeFLANN<PointT, Dist>::radiusSearch (const PointT &point, double radius, std::vector<int> &k_indices, 00170 std::vector<float> &k_sqr_dists, unsigned int max_nn) const 00171 { 00172 assert (point_representation_->isValid (point) && "Invalid (NaN, Inf) point coordinates given to radiusSearch!"); 00173 00174 std::vector<float> query (dim_); 00175 point_representation_->vectorize (static_cast<PointT> (point), query); 00176 00177 // Has max_nn been set properly? 00178 if (max_nn == 0 || max_nn > static_cast<unsigned int> (total_nr_points_)) 00179 max_nn = total_nr_points_; 00180 00181 std::vector<std::vector<int> > indices(1); 00182 std::vector<std::vector<float> > dists(1); 00183 00184 ::flann::SearchParams params (*param_radius_); 00185 if (max_nn == static_cast<unsigned int>(total_nr_points_)) 00186 params.max_neighbors = -1; // return all neighbors in radius 00187 else 00188 params.max_neighbors = max_nn; 00189 00190 int neighbors_in_radius = flann_index_->radiusSearch (::flann::Matrix<float> (&query[0], 1, dim_), 00191 indices, 00192 dists, 00193 static_cast<float> (radius * radius), 00194 params); 00195 00196 k_indices = indices[0]; 00197 k_sqr_dists = dists[0]; 00198 00199 // Do mapping to original point cloud 00200 if (!identity_mapping_) 00201 { 00202 for (int i = 0; i < neighbors_in_radius; ++i) 00203 { 00204 int& neighbor_index = k_indices[i]; 00205 neighbor_index = index_mapping_[neighbor_index]; 00206 } 00207 } 00208 00209 return (neighbors_in_radius); 00210 } 00211 00212 /////////////////////////////////////////////////////////////////////////////////////////// 00213 template <typename PointT, typename Dist> void 00214 pcl::KdTreeFLANN<PointT, Dist>::cleanup () 00215 { 00216 // Data array cleanup 00217 if (cloud_) 00218 { 00219 free (cloud_); 00220 cloud_ = NULL; 00221 } 00222 index_mapping_.clear (); 00223 00224 if (indices_) 00225 indices_.reset (); 00226 } 00227 00228 /////////////////////////////////////////////////////////////////////////////////////////// 00229 template <typename PointT, typename Dist> void 00230 pcl::KdTreeFLANN<PointT, Dist>::convertCloudToArray (const PointCloud &cloud) 00231 { 00232 // No point in doing anything if the array is empty 00233 if (cloud.points.empty ()) 00234 { 00235 cloud_ = NULL; 00236 return; 00237 } 00238 00239 int original_no_of_points = static_cast<int> (cloud.points.size ()); 00240 00241 cloud_ = static_cast<float*> (malloc (original_no_of_points * dim_ * sizeof (float))); 00242 float* cloud_ptr = cloud_; 00243 index_mapping_.reserve (original_no_of_points); 00244 identity_mapping_ = true; 00245 00246 for (int cloud_index = 0; cloud_index < original_no_of_points; ++cloud_index) 00247 { 00248 // Check if the point is invalid 00249 if (!point_representation_->isValid (cloud.points[cloud_index])) 00250 { 00251 identity_mapping_ = false; 00252 continue; 00253 } 00254 00255 index_mapping_.push_back (cloud_index); 00256 00257 point_representation_->vectorize (cloud.points[cloud_index], cloud_ptr); 00258 cloud_ptr += dim_; 00259 } 00260 } 00261 00262 /////////////////////////////////////////////////////////////////////////////////////////// 00263 template <typename PointT, typename Dist> void 00264 pcl::KdTreeFLANN<PointT, Dist>::convertCloudToArray (const PointCloud &cloud, const std::vector<int> &indices) 00265 { 00266 // No point in doing anything if the array is empty 00267 if (cloud.points.empty ()) 00268 { 00269 cloud_ = NULL; 00270 return; 00271 } 00272 00273 int original_no_of_points = static_cast<int> (indices.size ()); 00274 00275 cloud_ = static_cast<float*> (malloc (original_no_of_points * dim_ * sizeof (float))); 00276 float* cloud_ptr = cloud_; 00277 index_mapping_.reserve (original_no_of_points); 00278 // its a subcloud -> false 00279 // true only identity: 00280 // - indices size equals cloud size 00281 // - indices only contain values between 0 and cloud.size - 1 00282 // - no index is multiple times in the list 00283 // => index is complete 00284 // But we can not guarantee that => identity_mapping_ = false 00285 identity_mapping_ = false; 00286 00287 for (std::vector<int>::const_iterator iIt = indices.begin (); iIt != indices.end (); ++iIt) 00288 { 00289 // Check if the point is invalid 00290 if (!point_representation_->isValid (cloud.points[*iIt])) 00291 continue; 00292 00293 // map from 0 - N -> indices [0] - indices [N] 00294 index_mapping_.push_back (*iIt); // If the returned index should be for the indices vector 00295 00296 point_representation_->vectorize (cloud.points[*iIt], cloud_ptr); 00297 cloud_ptr += dim_; 00298 } 00299 } 00300 00301 #define PCL_INSTANTIATE_KdTreeFLANN(T) template class PCL_EXPORTS pcl::KdTreeFLANN<T>; 00302 00303 #endif //#ifndef _PCL_KDTREE_KDTREE_IMPL_FLANN_H_ 00304