Point Cloud Library (PCL)
1.3.1
|
00001 /* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Copyright (c) 2009, 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 * Author: Siddharth Choudhary (itzsid@gmail.com) 00035 * 00036 */ 00037 00038 #ifndef PCL_SEARCH_AUTO_TUNED_SEARCH_IMPL 00039 #define PCL_SEARCH_AUTO_TUNED_SEARCH_IMPL 00040 00041 #include <pcl/pcl_base.h> 00042 #include <vector> 00043 #include "pcl/search/auto.h" 00044 #include <pcl/common/time.h> 00045 00047 template<typename PointT> 00048 void 00049 pcl::search::AutotunedSearch<PointT>::initSearchDS (int spatial_locator) 00050 { 00051 // specifies the data-structure to be used using spatial_locator flag 00052 if (spatial_locator == KDTREE) 00053 // initialize kdtree 00054 search_.reset (new pcl::search::KdTree<PointT> ()); 00055 else if (spatial_locator == ORGANIZED_INDEX) 00056 search_.reset (new pcl::search::OrganizedNeighbor<PointT> ()); 00057 else if (spatial_locator == OCTREE) 00058 search_.reset (new pcl::search::Octree<PointT> (0.1f)); 00059 else 00060 PCL_ERROR ("[pcl::search::AutotunedSearch::initSearchDS] Spatial locator (%d) unknown!\n", spatial_locator); 00061 spatial_loc_ = spatial_locator; 00062 } 00063 00065 template<typename PointT> 00066 void 00067 pcl::search::AutotunedSearch<PointT>::evaluateSearchMethods (const PointCloudConstPtr& cloud, const int search_type) 00068 { 00069 // Evaluates different search data-structures for the given data and sets it to the ds having minimum time 00070 unsigned int searchIdx; 00071 while (1) 00072 { 00073 searchIdx = rand () % (cloud->width * cloud->height); 00074 if (cloud->points[searchIdx].z < 100) 00075 break; 00076 } 00077 00078 double time_kdtree, time_octree, time_organized_data; 00079 if (search_type == NEAREST_K_SEARCH) 00080 { 00081 unsigned int no_of_neighbors = 20; 00082 std::vector<int> k_indices; 00083 k_indices.resize (no_of_neighbors); 00084 std::vector<float> k_distances; 00085 k_distances.resize (no_of_neighbors); 00086 //std::cout << "\n---------------\nKDTree\n---------------\n"; 00087 double time1 = getTime (); 00088 search_.reset (new KdTree<PointT> ()); 00089 search_->setInputCloud (cloud); 00090 search_->nearestKSearch (cloud->points[searchIdx], no_of_neighbors, k_indices, k_distances); 00091 /* 00092 std::cout << "Neighbors are:" << std::endl; 00093 for (int i = 0; i < 20; i++) 00094 { 00095 std::cout << k_indices[i] << '\t'; 00096 } 00097 std::cout << std::endl; 00098 std::cout << "Number of Neighbors: " << k_indices.size () << std::endl; 00099 */ 00100 k_indices.clear (); 00101 k_distances.clear (); 00102 00103 //std::cout << "\n---------------\nOrganizedData\n---------------\n"; 00104 double time2 = getTime (); 00105 search_.reset (new pcl::search::OrganizedNeighbor<PointT> ()); 00106 search_->setInputCloud (cloud); 00107 search_->nearestKSearch (cloud->points[searchIdx], no_of_neighbors, k_indices, k_distances); 00108 //std::cout << "Neighbors are: " << std::endl; 00109 /* 00110 for (int i = 0;i < 20; i++) 00111 { 00112 std::cout << k_indices[i] << '\t'; 00113 } 00114 std::cout << std::endl; 00115 std::cout << "Number of Neigbhors: " << k_indices.size () << std::endl; 00116 */ 00117 k_indices.clear (); 00118 k_distances.clear (); 00119 00120 //std::cout << "\n---------------\nOctree\n---------------\n"; 00121 double time3 = getTime (); 00122 search_.reset (new pcl::search::Octree<PointT> (0.1f)); 00123 search_->setInputCloud (cloud); 00124 search_->nearestKSearch (cloud->points[searchIdx], no_of_neighbors, k_indices, k_distances); 00125 /* 00126 std::cout << "Neighbors are: " << std::endl; 00127 for (int i = 0;i < 20; i++) 00128 { 00129 std::cout << k_indices[i] << '\t'; 00130 } 00131 std::cout << std::endl; 00132 std::cout << "Number of Neighbors: " << k_indices.size () << std::endl; 00133 */ 00134 k_indices.clear (); 00135 k_distances.clear (); 00136 00137 time_kdtree = time2 - time1; 00138 time_organized_data = time3 - time2; 00139 time_octree = getTime () - time3; 00140 00141 PCL_INFO("[pcl::search::AutotunedSearch::evaluateSearchMethods::NEAREST_K_SEARCH] Time Taken: KDTree: %lf OranizedData: %lf Octree: %lf\n",time_kdtree,time_organized_data,time_octree); 00142 } 00143 else if (search_type == NEAREST_RADIUS_SEARCH) 00144 { 00145 double searchRadius = 1.0 * ((double)rand () / (double)RAND_MAX); 00146 00147 std::vector<int> k_indices; 00148 std::vector<float> k_distances; 00149 //std::cout << "\n---------------\nKDTree\n---------------\n"; 00150 double time1 = getTime (); 00151 search_.reset (new KdTree<PointT> ()); 00152 search_->setInputCloud (cloud); 00153 search_->radiusSearch (cloud->points[searchIdx], searchRadius, k_indices, k_distances); 00154 /* 00155 std::cout << "Neighbors are:" << std::endl; 00156 00157 for(int i = 0;i < 20; ++i) 00158 { 00159 std::cout << k_indices[i] << '\t'; 00160 } 00161 std::cout << std::endl; 00162 std::cout << "Number of Neighbors: " << k_indices.size () << std::endl; 00163 */ 00164 k_indices.clear (); 00165 k_distances.clear (); 00166 00167 //std::cout << "\n---------------\nOrganizedData\n---------------\n"; 00168 double time2 = getTime (); 00169 search_.reset (new OrganizedNeighbor<PointT> ()); 00170 search_->setInputCloud (cloud); 00171 search_->radiusSearch (cloud->points[searchIdx], searchRadius, k_indices, k_distances); 00172 /* 00173 std::cout << "Neighbors are: " << std::endl; 00174 for (int i = 0; i < 20; ++i) 00175 { 00176 std::cout << k_indices[i] << '\t'; 00177 } 00178 std::cout << std::endl; 00179 std::cout << "Number of Neigbhors: " << k_indices.size () << std::endl; 00180 */ 00181 k_indices.clear (); 00182 k_distances.clear (); 00183 00184 //std::cout << "\n---------------\nOctree\n---------------\n"; 00185 double time3 = getTime (); 00186 search_.reset (new Octree<PointT> (0.1f)); 00187 search_->setInputCloud (cloud); 00188 search_->radiusSearch (cloud->points[searchIdx], searchRadius, k_indices, k_distances); 00189 /* 00190 std::cout << "Neighbors are: " << std::endl; 00191 for (int i = 0; i < 20; ++i) 00192 { 00193 std::cout << k_indices[i] << '\t'; 00194 } 00195 std::cout << std::endl; 00196 std::cout << "Number of Neighbors: " << k_indices.size () << std::endl; 00197 */ 00198 k_indices.clear (); 00199 k_distances.clear (); 00200 00201 time_kdtree = time2 - time1; 00202 time_organized_data = time3 - time2; 00203 time_octree = getTime () - time3; 00204 00205 PCL_INFO("[pcl::search::AutotunedSearch::evaluateSearchMethods::NEAREST_K_SEARCH] Time Taken: KDTree: %lf OranizedData: %lf Octree: %lf\n",time_kdtree,time_organized_data,time_octree); 00206 } 00207 else 00208 { 00209 PCL_ERROR ("[pcl::search::AutotunedSearch::evaluateSearchMethods] Only NEAREST_K_SEARCH and NEAREST_RADIUS_SEARCH supported\n"); 00210 return; 00211 } 00212 // Set the datastructure according to which takes the minimum time 00213 if (time_kdtree < time_organized_data && time_kdtree < time_octree) 00214 { 00215 spatial_loc_ = KDTREE; 00216 search_.reset (new KdTree<PointT> ()); 00217 } 00218 else if (time_octree < time_kdtree && time_octree < time_organized_data) 00219 { 00220 spatial_loc_ = OCTREE; 00221 search_.reset (new pcl::search::Octree<PointT> (0.1f)); 00222 } 00223 else if (time_organized_data < time_kdtree && time_organized_data < time_octree) 00224 { 00225 spatial_loc_ = OCTREE; 00226 search_.reset (new pcl::search::OrganizedNeighbor<PointT> ()); 00227 } 00228 // search_->setInputCloud (cloud); 00229 } 00230 00232 template<typename PointT> 00233 void 00234 pcl::search::AutotunedSearch<PointT>::setInputCloud (const PointCloudConstPtr &cloud, const IndicesConstPtr &indices) 00235 { 00236 input_ = cloud; 00237 indices_ = indices; 00238 search_->setInputCloud (cloud, indices); 00239 } 00240 00242 template<typename PointT> 00243 void 00244 pcl::search::AutotunedSearch<PointT>::setInputCloud (const PointCloudConstPtr& cloud) 00245 { 00246 input_ = cloud; 00247 search_->setInputCloud (cloud); 00248 } 00249 00251 template<typename PointT> 00252 int 00253 pcl::search::AutotunedSearch<PointT>::nearestKSearch (const PointT &point, int k, std::vector<int> &k_indices, 00254 std::vector<float> &k_sqr_distances) 00255 { 00256 return (search_->nearestKSearch (point, k, k_indices, k_sqr_distances)); 00257 } 00258 00260 template<typename PointT> 00261 int 00262 pcl::search::AutotunedSearch<PointT>::nearestKSearch (const PointCloud &cloud, int index, int k, 00263 std::vector<int> &k_indices, 00264 std::vector<float> &k_sqr_distances) 00265 { 00266 return (search_->nearestKSearch (cloud, index, k, k_indices, k_sqr_distances)); 00267 } 00268 00270 template<typename PointT> 00271 int 00272 pcl::search::AutotunedSearch<PointT>::nearestKSearch (int index, int k, std::vector<int> &k_indices, 00273 std::vector<float> &k_sqr_distances) 00274 { 00275 return (search_->nearestKSearch (index, k, k_indices, k_sqr_distances)); 00276 } 00277 00279 template<typename PointT> 00280 int 00281 pcl::search::AutotunedSearch<PointT>::radiusSearch (const PointT &point, const double radius, 00282 std::vector<int> &k_indices, std::vector<float> &k_distances, 00283 int max_nn) const 00284 { 00285 return (search_->radiusSearch (point, radius, k_indices, k_distances, max_nn)); 00286 } 00287 00289 template<typename PointT> 00290 int 00291 pcl::search::AutotunedSearch<PointT>::radiusSearch (const PointCloud &cloud, int index, double radius, 00292 std::vector<int> &k_indices, std::vector<float> &k_distances, 00293 int max_nn) 00294 { 00295 return (search_->radiusSearch (cloud, index, radius, k_indices, k_distances, max_nn)); 00296 } 00297 00299 template<typename PointT> 00300 int 00301 pcl::search::AutotunedSearch<PointT>::radiusSearch (int index, double radius, std::vector<int> &k_indices, 00302 std::vector<float> &k_distances, int max_nn) const 00303 { 00304 return (search_->radiusSearch (index, radius, k_indices, k_distances, max_nn)); 00305 } 00306 00308 template<typename PointT> 00309 void 00310 pcl::search::AutotunedSearch<PointT>::approxNearestSearch (const PointCloudConstPtr &cloud_arg, int query_index_arg, 00311 int &result_index_arg, float &sqr_distance_arg) 00312 { 00313 if (spatial_loc_ == OCTREE) 00314 search_->approxNearestSearch (cloud_arg, query_index_arg, result_index_arg, sqr_distance_arg); 00315 else 00316 PCL_ERROR ("[pcl::search::AutotunedSearch::approxNearestSearch] approxNearestSearch() works only for OCTREE structure\n"); 00317 } 00318 00320 template<typename PointT> 00321 void 00322 pcl::search::AutotunedSearch<PointT>::approxNearestSearch (const PointT &p_q_arg, int &result_index_arg, 00323 float &sqr_distance_arg) 00324 { 00325 if (spatial_loc_ == OCTREE) 00326 search_->approxNearestSearch (p_q_arg, result_index_arg, sqr_distance_arg); 00327 else 00328 PCL_ERROR ("[pcl::search::AutotunedSearch::approxNearestSearch] approxNearestSearch() works only for OCTREE structure\n"); 00329 } 00330 00332 template<typename PointT> 00333 void 00334 pcl::search::AutotunedSearch<PointT>::approxNearestSearch (int query_index_arg, int &result_index_arg, 00335 float &sqr_distance_arg) 00336 { 00337 if (spatial_loc_ == OCTREE) 00338 search_->approxNearestSearch (query_index_arg, result_index_arg, sqr_distance_arg); 00339 else 00340 PCL_ERROR ("[pcl::search::AutotunedSearch::approxNearestSearch] approxNearestSearch() works only for OCTREE structure\n"); 00341 } 00342 00343 #define PCL_INSTANTIATE_AutotunedSearch(T) template class PCL_EXPORTS pcl::search::AutotunedSearch<T>; 00344 00345 #endif //#ifndef PCL_SEARCH_AUTO_TUNED_SEARCH_IMPL