Point Cloud Library (PCL)
1.3.1
|
00001 /* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Copyright (c) 2010, Willow Garage, Inc. 00005 * All rights reserved. 00006 * 00007 * Redistribution and use in source and binary forms, with or without 00008 * modification, are permitted provided that the following conditions 00009 * are met: 00010 * 00011 * * Redistributions of source code must retain the above copyright 00012 * notice, this list of conditions and the following disclaimer. 00013 * * Redistributions in binary form must reproduce the above 00014 * copyright notice, this list of conditions and the following 00015 * disclaimer in the documentation and/or other materials provided 00016 * with the distribution. 00017 * * Neither the name of Willow Garage, Inc. nor the names of its 00018 * contributors may be used to endorse or promote products derived 00019 * from this software without specific prior written permission. 00020 * 00021 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00022 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00023 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00024 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00025 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00026 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00027 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00028 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00029 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00030 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00031 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00032 * POSSIBILITY OF SUCH DAMAGE. 00033 * 00034 */ 00035 00036 #ifndef PCL_KDTREE_KDTREE_IMPL_FLANN_H_ 00037 #define PCL_KDTREE_KDTREE_IMPL_FLANN_H_ 00038 00039 #include "pcl/kdtree/kdtree_flann.h" 00040 #include <pcl/console/print.h> 00041 #include <flann/flann.hpp> 00042 00044 template <typename PointT, typename Dist> void 00045 pcl::KdTreeFLANN<PointT, Dist>::setInputCloud (const PointCloudConstPtr &cloud, const IndicesConstPtr &indices) 00046 { 00047 cleanup (); // Perform an automatic cleanup of structures 00048 00049 if (!initParameters()) 00050 return; 00051 00052 input_ = cloud; 00053 indices_ = indices; 00054 00055 if (input_ == NULL) 00056 return; 00057 00058 m_lock_.lock (); 00059 // Allocate enough data 00060 if (!input_) 00061 { 00062 PCL_ERROR ("[pcl::KdTreeANN::setInputCloud] Invalid input!\n"); 00063 return; 00064 } 00065 if (indices != NULL) 00066 convertCloudToArray (*input_, *indices_); 00067 else 00068 convertCloudToArray (*input_); 00069 00070 initData (); 00071 m_lock_.unlock (); 00072 } 00073 00075 template <typename PointT, typename Dist> int 00076 pcl::KdTreeFLANN<PointT, Dist>::nearestKSearch (const PointT &point, int k, 00077 std::vector<int> &k_indices, 00078 std::vector<float> &k_distances) 00079 { 00080 if (!point_representation_->isValid (point)) 00081 { 00082 //PCL_ERROR_STREAM ("[pcl::KdTreeFLANN::nearestKSearch] Invalid query point given!" << point); 00083 return (0); 00084 } 00085 00086 if (k_indices.size () < (size_t)k) 00087 k_indices.resize (k); 00088 if (k_distances.size () < (size_t)k) 00089 k_distances.resize (k); 00090 00091 std::vector<float> tmp (dim_); 00092 point_representation_->vectorize ((PointT)point, tmp); 00093 00094 flann::Matrix<int> k_indices_mat (&k_indices[0], 1, k); 00095 flann::Matrix<float> k_distances_mat (&k_distances[0], 1, k); 00096 flann_index_->knnSearch (flann::Matrix<float>(&tmp[0], 1, dim_), k_indices_mat, k_distances_mat, k, flann::SearchParams (-1 ,epsilon_)); 00097 00098 // Do mapping to original point cloud 00099 if (!identity_mapping_) 00100 { 00101 for (size_t i = 0; i < (size_t)k; ++i) 00102 { 00103 int& neighbor_index = k_indices[i]; 00104 neighbor_index = index_mapping_[neighbor_index]; 00105 } 00106 } 00107 00108 return (k); 00109 } 00110 00112 template <typename PointT, typename Dist> int 00113 pcl::KdTreeFLANN<PointT, Dist>::radiusSearch (const PointT &point, double radius, std::vector<int> &k_indices, 00114 std::vector<float> &k_squared_distances, int max_nn) const 00115 { 00116 static flann::Matrix<int> indices_empty; 00117 static flann::Matrix<float> dists_empty; 00118 00119 if (!point_representation_->isValid (point)) 00120 { 00121 //PCL_ERROR_STREAM ("[pcl::KdTreeFLANN::radiusSearch] Invalid query point given!" << point); 00122 return 0; 00123 } 00124 00125 std::vector<float> tmp(dim_); 00126 point_representation_->vectorize ((PointT)point, tmp); 00127 radius *= radius; // flann uses squared radius 00128 00129 size_t size; 00130 if (indices_ == NULL) // no indices set, use full size of point cloud: 00131 size = input_->points.size (); 00132 else 00133 size = indices_->size (); 00134 00135 int neighbors_in_radius = 0; 00136 if (k_indices.size () == size && k_squared_distances.size () == size) // preallocated vectors 00137 { 00138 // if using preallocated vectors we ignore max_nn as we are sure to have enought space 00139 // to store all neighbors found in radius 00140 flann::Matrix<int> k_indices_mat (&k_indices[0], 1, k_indices.size()); 00141 flann::Matrix<float> k_distances_mat (&k_squared_distances[0], 1, k_squared_distances.size()); 00142 neighbors_in_radius = flann_index_->radiusSearch (flann::Matrix<float>(&tmp[0], 1, dim_), 00143 k_indices_mat, k_distances_mat, radius, flann::SearchParams (-1, epsilon_, sorted_)); 00144 } 00145 else // need to do search twice, first to find how many neighbors and allocate the vectors 00146 { 00147 neighbors_in_radius = flann_index_->radiusSearch (flann::Matrix<float>(&tmp[0], 1, dim_), 00148 indices_empty, dists_empty, radius, flann::SearchParams (-1, epsilon_, sorted_)); 00149 if (max_nn > 0) 00150 { 00151 neighbors_in_radius = std::min(neighbors_in_radius, max_nn); 00152 } 00153 k_indices.resize (neighbors_in_radius); 00154 k_squared_distances.resize (neighbors_in_radius); 00155 if (neighbors_in_radius == 0) 00156 { 00157 return (0); 00158 } 00159 00160 flann::Matrix<int> k_indices_mat (&k_indices[0], 1, k_indices.size()); 00161 flann::Matrix<float> k_distances_mat (&k_squared_distances[0], 1, k_squared_distances.size()); 00162 flann_index_->radiusSearch (flann::Matrix<float>(&tmp[0], 1, dim_), 00163 k_indices_mat, k_distances_mat, radius, flann::SearchParams (-1, epsilon_, sorted_)); 00164 00165 } 00166 00167 // Do mapping to original point cloud 00168 if (!identity_mapping_) { 00169 for (int i = 0; i < neighbors_in_radius; ++i) 00170 { 00171 int& neighbor_index = k_indices[i]; 00172 neighbor_index = index_mapping_[neighbor_index]; 00173 } 00174 } 00175 00176 return (neighbors_in_radius); 00177 } 00178 00180 template <typename PointT, typename Dist> void 00181 pcl::KdTreeFLANN<PointT, Dist>::cleanup () 00182 { 00183 delete flann_index_; 00184 00185 m_lock_.lock (); 00186 // Data array cleanup 00187 free (cloud_); 00188 cloud_ = NULL; 00189 index_mapping_.clear(); 00190 00191 if (indices_) 00192 indices_.reset (); 00193 00194 m_lock_.unlock (); 00195 } 00196 00198 template <typename PointT, typename Dist> bool 00199 pcl::KdTreeFLANN<PointT, Dist>::initParameters () 00200 { 00201 epsilon_ = 0.0; // default error bound value 00202 dim_ = point_representation_->getNumberOfDimensions (); // Number of dimensions - default is 3 = xyz 00203 // Create the kd_tree representation 00204 return (true); 00205 } 00206 00208 template <typename PointT, typename Dist> void 00209 pcl::KdTreeFLANN<PointT, Dist>::initData () 00210 { 00211 flann_index_ = new FLANNIndex(flann::Matrix<float>(cloud_, index_mapping_.size(), dim_), 00212 flann::KDTreeSingleIndexParams(15)); // max 15 points/leaf 00213 flann_index_->buildIndex(); 00214 } 00215 00217 template <typename PointT, typename Dist> void 00218 pcl::KdTreeFLANN<PointT, Dist>::convertCloudToArray (const PointCloud &cloud) 00219 { 00220 // No point in doing anything if the array is empty 00221 if (cloud.points.empty ()) 00222 { 00223 cloud_ = NULL; 00224 return; 00225 } 00226 00227 int original_no_of_points = cloud.points.size(); 00228 00229 cloud_ = (float*)malloc (original_no_of_points * dim_ * sizeof(float)); 00230 float* cloud_ptr = cloud_; 00231 index_mapping_.reserve(original_no_of_points); 00232 identity_mapping_ = true; 00233 00234 for (int cloud_index = 0; cloud_index < original_no_of_points; ++cloud_index) 00235 { 00236 const PointT point = cloud.points[cloud_index]; 00237 // Check if the point is invalid 00238 if (!point_representation_->isValid (point)) 00239 { 00240 identity_mapping_ = false; 00241 continue; 00242 } 00243 00244 index_mapping_.push_back(cloud_index); 00245 00246 point_representation_->vectorize(point, cloud_ptr); 00247 cloud_ptr += dim_; 00248 } 00249 } 00250 00252 template <typename PointT, typename Dist> void 00253 pcl::KdTreeFLANN<PointT, Dist>::convertCloudToArray (const PointCloud &cloud, const std::vector<int> &indices) 00254 { 00255 // No point in doing anything if the array is empty 00256 if (cloud.points.empty ()) 00257 { 00258 cloud_ = NULL; 00259 return; 00260 } 00261 00262 int original_no_of_points = indices.size(); 00263 00264 cloud_ = (float*)malloc (original_no_of_points * dim_ * sizeof (float)); 00265 float* cloud_ptr = cloud_; 00266 index_mapping_.reserve(original_no_of_points); 00267 identity_mapping_ = true; 00268 00269 for (int indices_index = 0; indices_index < original_no_of_points; ++indices_index) 00270 { 00271 int cloud_index = indices[indices_index]; 00272 const PointT point = cloud.points[cloud_index]; 00273 // Check if the point is invalid 00274 if (!point_representation_->isValid(point)) { 00275 identity_mapping_ = false; 00276 continue; 00277 } 00278 00279 index_mapping_.push_back(indices_index); // If the returned index should be for the indices vector 00280 //index_mapping_.push_back(cloud_index); // If the returned index should be for the ros cloud 00281 00282 point_representation_->vectorize(point, cloud_ptr); 00283 cloud_ptr += dim_; 00284 } 00285 } 00286 00287 #define PCL_INSTANTIATE_KdTreeFLANN(T) template class PCL_EXPORTS pcl::KdTreeFLANN<T>; 00288 00289 #endif //#ifndef _PCL_KDTREE_KDTREE_IMPL_FLANN_H_ 00290