Point Cloud Library (PCL)  1.3.1
auto.hpp
Go to the documentation of this file.
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
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines