test_auto_search.cpp
Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Point Cloud Library (PCL) - www.pointclouds.org
00005  *  Copyright (c) 2010-2011, Willow Garage, Inc.
00006  *
00007  *  All rights reserved.
00008  *
00009  *  Redistribution and use in source and binary forms, with or without
00010  *  modification, are permitted provided that the following conditions
00011  *  are met:
00012  *
00013  *   * Redistributions of source code must retain the above copyright
00014  *     notice, this list of conditions and the following disclaimer.
00015  *   * Redistributions in binary form must reproduce the above
00016  *     copyright notice, this list of conditions and the following
00017  *     disclaimer in the documentation and/or other materials provided
00018  *     with the distribution.
00019  *   * Neither the name of the copyright holder(s) nor the names of its
00020  *     contributors may be used to endorse or promote products derived
00021  *     from this software without specific prior written permission.
00022  *
00023  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00024  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00025  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00026  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00027  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00028  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00029  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00030  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00031  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00032  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00033  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00034  *  POSSIBILITY OF SUCH DAMAGE.
00035  *
00036  * $Id: test_feature.cpp 2177 2011-08-23 13:58:35Z shapovalov $
00037  *
00038  */
00039 #include <iostream>
00040 #include <gtest/gtest.h>
00041 #include <pcl/common/time.h>
00042 #include <pcl/search/pcl_search.h>
00043 #include <pcl/point_cloud.h>
00044 #include <pcl/point_types.h>
00045 #include <pcl/io/pcd_io.h>
00046 
00047 using namespace std;
00048 using namespace pcl;
00049 
00050 PointCloud<PointXYZ> cloud, cloud_big;
00051 
00052 void
00053 init ()
00054 {
00055   float resolution = 0.1;
00056   for (float z = -0.5f; z <= 0.5f; z += resolution)
00057     for (float y = -0.5f; y <= 0.5f; y += resolution)
00058       for (float x = -0.5f; x <= 0.5f; x += resolution)
00059         cloud.points.push_back (PointXYZ (x, y, z));
00060   cloud.width  = cloud.points.size ();
00061   cloud.height = 1;
00062 
00063   cloud_big.width  = 640;
00064   cloud_big.height = 480;
00065   srand (time (NULL));
00066   // Randomly create a new point cloud
00067   for (size_t i = 0; i < cloud_big.width * cloud_big.height; ++i)
00068     cloud_big.points.push_back (PointXYZ (1024 * rand () / (RAND_MAX + 1.0),
00069                                          1024 * rand () / (RAND_MAX + 1.0),
00070                                          1024 * rand () / (RAND_MAX + 1.0)));
00071 }
00072 
00073 // helper class for priority queue
00074 class prioPointQueueEntry
00075 {
00076   public:
00077     prioPointQueueEntry ()
00078     {
00079     }
00080     prioPointQueueEntry (PointXYZ& point_arg, double pointDistance_arg, int pointIdx_arg)
00081     {
00082       point_ = point_arg;
00083       pointDistance_ = pointDistance_arg;
00084       pointIdx_ = pointIdx_arg;
00085     }
00086 
00087     bool
00088     operator< (const prioPointQueueEntry& rhs_arg) const
00089     {
00090       return (this->pointDistance_ < rhs_arg.pointDistance_);
00091     }
00092 
00093     PointXYZ point_;
00094     double pointDistance_;int pointIdx_;
00095 };
00096 
00097 TEST (PCL, Octree_Pointcloud_Nearest_K_Neighbour_Search)
00098 {
00099   const unsigned int test_runs = 1;
00100   unsigned int test_id;
00101 
00102   // instantiate point cloud
00103   PointCloud<PointXYZ>::Ptr cloudIn (new PointCloud<PointXYZ> ());
00104 
00105   size_t i;
00106   srand (time (NULL));
00107   unsigned int K;
00108 
00109   std::priority_queue<prioPointQueueEntry, std::vector<prioPointQueueEntry, Eigen::aligned_allocator<prioPointQueueEntry> > > pointCandidates;
00110 
00111   // create octree
00112   pcl::search::Search<PointXYZ>* octree = new  pcl::search::AutotunedSearch<PointXYZ>(pcl::search::OCTREE);
00113 
00114   std::vector<int> k_indices;
00115   std::vector<float> k_sqr_distances;
00116 
00117   std::vector<int> k_indices_bruteforce;
00118   std::vector<float> k_sqr_distances_bruteforce;
00119 
00120   for (test_id = 0; test_id < test_runs; test_id++)
00121   {
00122     // define a random search point
00123 
00124     PointXYZ searchPoint (10.0 * ((double)rand () / (double)RAND_MAX), 10.0 * ((double)rand () / (double)RAND_MAX),
00125                           10.0 * ((double)rand () / (double)RAND_MAX));
00126 
00127     K = rand () % 10;
00128 
00129     // generate point cloud
00130     cloudIn->width = 1000;
00131     cloudIn->height = 1;
00132     cloudIn->points.resize (cloudIn->width * cloudIn->height);
00133     for (i = 0; i < 1000; i++)
00134     {
00135       cloudIn->points[i] = PointXYZ (5.0 * ((double)rand () / (double)RAND_MAX),
00136                                      10.0 * ((double)rand () / (double)RAND_MAX),
00137                                      10.0 * ((double)rand () / (double)RAND_MAX));
00138     }
00139 
00140 
00141     double pointDist;
00142 
00143     k_indices.clear();
00144     k_sqr_distances.clear();
00145 
00146     k_indices_bruteforce.clear();
00147     k_sqr_distances_bruteforce.clear();
00148 
00149     // push all points and their distance to the search point into a priority queue - bruteforce approach.
00150     for (i = 0; i < cloudIn->points.size (); i++)
00151     {
00152       pointDist = ((cloudIn->points[i].x - searchPoint.x) * (cloudIn->points[i].x - searchPoint.x)
00153           + (cloudIn->points[i].y - searchPoint.y) * (cloudIn->points[i].y - searchPoint.y) + (cloudIn->points[i].z
00154           - searchPoint.z) * (cloudIn->points[i].z - searchPoint.z));
00155 
00156       prioPointQueueEntry pointEntry (cloudIn->points[i], pointDist, i);
00157 
00158       pointCandidates.push (pointEntry);
00159     }
00160 
00161     // pop priority queue until we have the nearest K elements
00162     while (pointCandidates.size () > K)
00163       pointCandidates.pop ();
00164 
00165     // copy results into vectors
00166     while (pointCandidates.size ())
00167     {
00168       k_indices_bruteforce.push_back (pointCandidates.top ().pointIdx_);
00169       k_sqr_distances_bruteforce.push_back (pointCandidates.top ().pointDistance_);
00170 
00171       pointCandidates.pop ();
00172     }
00173 
00174     // octree nearest neighbor search
00175   octree->setInputCloud (cloudIn);
00176     octree->nearestKSearch (searchPoint, (int)K, k_indices, k_sqr_distances);
00177 
00178     ASSERT_EQ ( k_indices.size() , k_indices_bruteforce.size() );
00179 
00180     // compare nearest neighbor results of octree with bruteforce search
00181     i = 0;
00182     while (k_indices_bruteforce.size ())
00183     {
00184       ASSERT_EQ ( k_indices.back() , k_indices_bruteforce.back() );
00185       EXPECT_NEAR (k_sqr_distances.back(), k_sqr_distances_bruteforce.back(), 1e-4);
00186 
00187       k_indices_bruteforce.pop_back();
00188       k_indices.pop_back();
00189 
00190       k_sqr_distances_bruteforce.pop_back();
00191       k_sqr_distances.pop_back();
00192     }
00193   }
00194 }
00195 
00196 TEST (PCL, Octree_Pointcloud_Approx_Nearest_Neighbour_Search)
00197 {
00198   const unsigned int test_runs = 100;
00199   unsigned int test_id;
00200 
00201   unsigned int bestMatchCount = 0;
00202 
00203   // instantiate point cloud
00204   PointCloud<PointXYZ>::Ptr cloudIn (new PointCloud<PointXYZ> ());
00205 
00206   size_t i;
00207   srand (time (NULL));
00208 
00209   // create octree
00210   pcl::search::Search<PointXYZ>* octree = new pcl::search::AutotunedSearch<PointXYZ>(pcl::search::OCTREE);
00211 
00212 
00213   for (test_id = 0; test_id < test_runs; test_id++)
00214   {
00215     // define a random search point
00216 
00217     PointXYZ searchPoint (10.0 * ((double)rand () / (double)RAND_MAX), 10.0 * ((double)rand () / (double)RAND_MAX),
00218                           10.0 * ((double)rand () / (double)RAND_MAX));
00219 
00220     // generate point cloud
00221     cloudIn->width = 1000;
00222     cloudIn->height = 1;
00223     cloudIn->points.resize (cloudIn->width * cloudIn->height);
00224     for (i = 0; i < 1000; i++)
00225     {
00226       cloudIn->points[i] = PointXYZ (5.0 * ((double)rand () / (double)RAND_MAX),
00227                                      10.0 * ((double)rand () / (double)RAND_MAX),
00228                                      10.0 * ((double)rand () / (double)RAND_MAX));
00229     }
00230 
00231 
00232     // brute force search
00233     double pointDist;
00234     double BFdistance = numeric_limits<double>::max ();
00235     int BFindex = 0;
00236 
00237     for (i = 0; i < cloudIn->points.size (); i++)
00238     {
00239       pointDist = ((cloudIn->points[i].x - searchPoint.x) * (cloudIn->points[i].x - searchPoint.x)
00240           + (cloudIn->points[i].y - searchPoint.y) * (cloudIn->points[i].y - searchPoint.y) + (cloudIn->points[i].z
00241           - searchPoint.z) * (cloudIn->points[i].z - searchPoint.z));
00242 
00243       if (pointDist<BFdistance)
00244       {
00245         BFindex = i;
00246         BFdistance = pointDist;
00247       }
00248 
00249     }
00250 
00251     int ANNindex;
00252     float ANNdistance;
00253 
00254     // octree approx. nearest neighbor search
00255   octree->setInputCloud (cloudIn);
00256     octree->approxNearestSearch (searchPoint,  ANNindex, ANNdistance);
00257 
00258     if (BFindex==ANNindex)
00259     {
00260       EXPECT_NEAR (ANNdistance, BFdistance, 1e-4);
00261       bestMatchCount++;
00262     }
00263 
00264   }
00265 
00266   // we should have found the absolute nearest neighbor at least once
00267   ASSERT_EQ ( (bestMatchCount > 0) , true);
00268 }
00269 
00270 
00271 TEST (PCL, KdTreeWrapper_nearestKSearch)
00272 {
00273 
00274   pcl::search::Search<PointXYZ>* kdtree = new pcl::search::AutotunedSearch<PointXYZ>(pcl::search::KDTREE);
00275   kdtree->setInputCloud (cloud.makeShared ());
00276   PointXYZ test_point (0.01f, 0.01f, 0.01f);
00277   unsigned int no_of_neighbors = 20;
00278   multimap<float, int> sorted_brute_force_result;
00279   for (size_t i = 0; i < cloud.points.size (); ++i)
00280   {
00281     float distance = euclideanDistance (cloud.points[i], test_point);
00282     sorted_brute_force_result.insert (make_pair (distance, (int) i));
00283   }
00284   float max_dist = 0.0f;
00285   unsigned int counter = 0;
00286   for (multimap<float, int>::iterator it = sorted_brute_force_result.begin (); it != sorted_brute_force_result.end () && counter < no_of_neighbors; ++it)
00287   {
00288     max_dist = max (max_dist, it->first);
00289     ++counter;
00290   }
00291 
00292   vector<int> k_indices;
00293   k_indices.resize (no_of_neighbors);
00294   vector<float> k_distances;
00295   k_distances.resize (no_of_neighbors);
00296 
00297   kdtree->nearestKSearch (test_point, no_of_neighbors, k_indices, k_distances);
00298 
00299   EXPECT_EQ (k_indices.size (), no_of_neighbors);
00300 
00301   // Check if all found neighbors have distance smaller than max_dist
00302   for (size_t i = 0; i < k_indices.size (); ++i)
00303   {
00304     const PointXYZ& point = cloud.points[k_indices[i]];
00305     bool ok = euclideanDistance (test_point, point) <= max_dist;
00306     if (!ok)
00307       ok = (fabs (euclideanDistance (test_point, point)) - max_dist) <= 1e-6;
00308     //if (!ok)  cerr << k_indices[i] << " is not correct...\n";
00309     //else      cerr << k_indices[i] << " is correct...\n";
00310     EXPECT_EQ (ok, true);
00311   }
00312 
00313   ScopeTime scopeTime ("FLANN nearestKSearch");
00314   {
00315     pcl::search::Search<PointXYZ>* kdtree = new pcl::search::AutotunedSearch<PointXYZ>(pcl::search::KDTREE);
00316     //kdtree->initSearchDS ();
00317     kdtree->setInputCloud (cloud_big.makeShared ());
00318     for (size_t i = 0; i < cloud_big.points.size (); ++i)
00319       kdtree->nearestKSearch (cloud_big.points[i], no_of_neighbors, k_indices, k_distances);
00320   }
00321 
00322 }       
00323 
00324 
00325 /* Function to auto evaluate the best search structure for the given dataset */
00326 TEST (PCL, AutoTunedSearch_Evaluate)
00327 {
00328   pcl::search::Search<PointXYZ>* search = new pcl::search::AutotunedSearch<PointXYZ>(pcl::search::AUTO_TUNED);
00329 
00330   pcl::PCDReader pcd;
00331   pcl::PointCloud<PointXYZ>::Ptr cloudIn (new pcl::PointCloud<PointXYZ>);
00332 
00333   if (pcd.read ("office1.pcd", *cloudIn) == -1)
00334   {
00335     std::cout <<"Couldn't read input cloud" << std::endl;
00336     return;
00337   }
00338 
00339  search->evaluateSearchMethods (cloudIn, pcl::search::NEAREST_K_SEARCH);
00340  search->evaluateSearchMethods (cloudIn, pcl::search::NEAREST_RADIUS_SEARCH);
00341 }
00342 
00343 
00344 
00345 int 
00346 main(int argc, char** argv)
00347 {
00348   testing::InitGoogleTest (&argc, argv);
00349   init ();
00350 
00351   // Testing using explicit instantiation of inherited class
00352   pcl::search::Search<PointXYZ>* kdtree = new pcl::search::AutotunedSearch<PointXYZ>(pcl::search::KDTREE);
00353   kdtree->setInputCloud (cloud.makeShared ());
00354 
00355   return (RUN_ALL_TESTS ());
00356 };
00357 /* ]--- */


pcl
Author(s): Open Perception
autogenerated on Wed Aug 26 2015 15:34:22