test_search.cpp
Go to the documentation of this file.
00001 #include <vector>
00002 #include <string>
00003 #include <sstream>
00004 #include <pcl/io/pcd_io.h>
00005 #include <pcl/common/time.h>
00006 #include <pcl/console/parse.h>
00007 #include <pcl/search/kdtree.h>
00008 #include <pcl/search/brute_force.h>
00009 
00010 using namespace std;
00011 
00012 int
00013 main (int argc, char ** argv)
00014 {
00015   if (argc < 2)
00016   {
00017     pcl::console::print_info ("Syntax is: %s [-pcd <pcd-file>] (-radius <radius> [-knn <k>] | -knn <k> )\n", argv[0]);
00018     return (1);
00019   }
00020 
00021   string pcd_path;
00022   bool use_pcd_file = pcl::console::find_switch (argc, argv, "-pcd");
00023   if (use_pcd_file)
00024     pcl::console::parse (argc, argv, "-pcd", pcd_path);
00025 
00026   float radius = -1;
00027   if (pcl::console::find_switch (argc, argv, "-radius"))
00028     pcl::console::parse (argc, argv, "-radius", radius);
00029 
00030   int k = -1;
00031   if (pcl::console::find_switch (argc, argv, "-knn"))
00032     pcl::console::parse (argc, argv, "-knn", k);
00033 
00034   if (radius < 0 && k < 0)
00035   {
00036     cout << "please specify at least one of the options -radius and -knn" << endl;
00037     return (1);
00038   }
00039 
00040   pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZRGB>);
00041 
00042   if (use_pcd_file)
00043     pcl::io::loadPCDFile (pcd_path, *cloud);
00044   else
00045   {
00046     cloud->resize (1000000);
00047     for (unsigned idx = 0; idx < cloud->size (); ++idx)
00048     {
00049       (*cloud)[idx].x = static_cast<float> (rand () / RAND_MAX);
00050       (*cloud)[idx].y = static_cast<float> (rand () / RAND_MAX);
00051       (*cloud)[idx].z = static_cast<float> (rand () / RAND_MAX);
00052     }
00053   }
00054 
00055   pcl::search::KdTree<pcl::PointXYZRGB> tree;
00056   pcl::PointXYZRGB query;
00057   query.x = 0.5;
00058   query.y = 0.5;
00059   query.z = 0.5;
00060 
00061   vector<int> kd_indices;
00062   vector<float> kd_distances;
00063   vector<int> bf_indices;
00064   vector<float> bf_distances;
00065 
00066   double start, stop;
00067   double kd_setup;
00068   double kd_search;
00069   double bf_setup;
00070   double bf_search;
00071 
00072   if (k > 0)
00073   {
00074     start = pcl::getTime ();
00075     tree.setInputCloud (cloud);
00076     stop = pcl::getTime ();
00077     cout << "setting up kd tree: " << (kd_setup = stop - start) << endl;
00078 
00079     start = pcl::getTime ();
00080     tree.nearestKSearchT (query, k, kd_indices, kd_distances);
00081     stop = pcl::getTime ();
00082     cout << "single search with kd tree; " << (kd_search = stop - start) << " :: " << kd_indices[0] << " , " << kd_distances [0] << endl;
00083 
00084     pcl::search::BruteForce<pcl::PointXYZRGB> brute_force;
00085     start = pcl::getTime ();
00086     brute_force.setInputCloud (cloud);
00087     stop = pcl::getTime ();
00088     cout << "setting up brute force search: " << (bf_setup = stop - start) << endl;
00089 
00090     start = pcl::getTime ();
00091     brute_force.nearestKSearchT (query, k, bf_indices, bf_distances);
00092     stop = pcl::getTime ();
00093     cout << "single search with brute force; " << (bf_search = stop - start) << " :: " << bf_indices[0] << " , " << bf_distances [0] << endl;
00094     cout << "amortization after searches: " << (kd_setup - bf_setup) / (bf_search - kd_search) << endl;
00095   }
00096   else
00097   {
00098     start = pcl::getTime ();
00099     tree.setInputCloud (cloud);
00100     stop = pcl::getTime ();
00101     cout << "setting up kd tree: " << (kd_setup = stop - start) << endl;
00102 
00103     start = pcl::getTime ();
00104     tree.radiusSearch (query, radius, kd_indices, kd_distances, k);
00105     stop = pcl::getTime ();
00106     cout << "single search with kd tree; " << (kd_search = stop - start) << " :: " << kd_indices[0] << " , " << kd_distances [0] << endl;
00107 
00108     pcl::search::BruteForce<pcl::PointXYZRGB> brute_force;
00109     start = pcl::getTime ();
00110     brute_force.setInputCloud (cloud);
00111     stop = pcl::getTime ();
00112     cout << "setting up brute force search: " << (bf_setup = stop - start) << endl;
00113 
00114     start = pcl::getTime ();
00115     brute_force.radiusSearch (query, radius, bf_indices, bf_distances, k);
00116     stop = pcl::getTime ();
00117     cout << "single search with brute force; " << (bf_search = stop - start) << " :: " << bf_indices[0] << " , " << bf_distances [0] << endl;
00118     cout << "amortization after searches: " << (kd_setup - bf_setup) / (bf_search - kd_search) << endl;
00119   }
00120 
00121   if (kd_indices.size () != bf_indices.size ())
00122   {
00123     cerr << "size of results do not match " <<kd_indices.size () << " vs. " << bf_indices.size () << endl;
00124   }
00125   else
00126   {
00127     cerr << "size of result: " <<kd_indices.size () << endl;
00128     for (unsigned idx = 0; idx < kd_indices.size (); ++idx)
00129     {
00130       if (kd_indices[idx] != bf_indices[idx] && kd_distances[idx] != bf_distances[idx])
00131       {
00132         cerr << "results do not match: " << idx << " nearest neighbor: "
00133                 << kd_indices[idx] << " with distance: " << kd_distances[idx] << " vs. "
00134                 << bf_indices[idx] << " with distance: " << bf_distances[idx] << endl;
00135       }
00136     }
00137   }
00138   return (0);
00139 }


pcl
Author(s): Open Perception
autogenerated on Wed Aug 26 2015 15:35:54