test_organized_index.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 Willow Garage, Inc. 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  *
00037  */
00038 #include <gtest/gtest.h>
00039 #include <vector>
00040 #include <stdio.h>
00041 #include <pcl/common/time.h>
00042 #include <pcl/point_cloud.h>
00043 #include <pcl/point_types.h>
00044 #include <pcl/io/pcd_io.h>
00045 #include <pcl/search/pcl_search.h>
00046 
00047 using namespace std;
00048 using namespace pcl;
00049 
00050 // helper class for priority queue
00051 class prioPointQueueEntry
00052 {
00053   public:
00054     prioPointQueueEntry ()
00055     {
00056     }
00057     prioPointQueueEntry (PointXYZ& point_arg, double pointDistance_arg, int pointIdx_arg)
00058     {
00059       point_ = point_arg;
00060       pointDistance_ = pointDistance_arg;
00061       pointIdx_ = pointIdx_arg;
00062     }
00063 
00064     bool
00065     operator< (const prioPointQueueEntry& rhs_arg) const
00066     {
00067       return (this->pointDistance_ < rhs_arg.pointDistance_);
00068     }
00069 
00070     PointXYZ point_;
00071     double pointDistance_;int pointIdx_;
00072 };
00073 
00074 TEST (PCL, Organized_Neighbor_Search_Pointcloud_Nearest_K_Neighbour_Search)
00075 {
00076 
00077   const unsigned int test_runs = 2;
00078   unsigned int test_id;
00079 
00080   // instantiate point cloud
00081   PointCloud<PointXYZ>::Ptr cloudIn (new PointCloud<PointXYZ> ());
00082 
00083   size_t i;
00084 
00085   srand (time (NULL));
00086 
00087   unsigned int K;
00088 
00089   // create organized search
00090   pcl::search::Search<PointXYZ>* organizedNeighborSearch = new pcl::search::OrganizedNeighbor<PointXYZ>();
00091 
00092   std::vector<int> k_indices;
00093   std::vector<float> k_sqr_distances;
00094 
00095   std::vector<int> k_indices_bruteforce;
00096   std::vector<float> k_sqr_distances_bruteforce;
00097 
00098   // typical focal length from kinect
00099   const double oneOverFocalLength = 0.0018;
00100   double x,y,z;
00101 
00102   int xpos, ypos, centerX, centerY;
00103 
00104   for (test_id = 0; test_id < test_runs; test_id++)
00105   {
00106     // define a random search point
00107 
00108     K = (rand () % 10)+1;
00109 
00110     // generate point cloud
00111     cloudIn->width = 128;
00112     cloudIn->height = 32;
00113     cloudIn->points.clear();
00114     cloudIn->points.reserve (cloudIn->width * cloudIn->height);
00115 
00116     centerX = cloudIn->width>>1;
00117     centerY = cloudIn->height>>1;
00118 
00119     for (ypos = -centerY; ypos < centerY; ypos++)
00120       for (xpos = -centerX; xpos < centerX; xpos++)
00121       {
00122         z = 15.0 * ((double)rand () / (double)(RAND_MAX+1.0))+20;
00123         y = (double)ypos*oneOverFocalLength*(double)z;
00124         x = (double)xpos*oneOverFocalLength*(double)z;
00125 
00126         cloudIn->points.push_back(PointXYZ (x, y, z));
00127       }
00128 
00129     unsigned int searchIdx = rand()%(cloudIn->width * cloudIn->height);
00130     const PointXYZ& searchPoint = cloudIn->points[searchIdx];
00131 
00132     k_indices.clear();
00133     k_sqr_distances.clear();
00134 
00135     // organized nearest neighbor search
00136     organizedNeighborSearch->setInputCloud (cloudIn);
00137 //    organizedNeighborSearch->nearestKSearch (searchPoint, (int)K, k_indices, k_sqr_distances);
00138 
00139 
00140     double pointDist;
00141 
00142     k_indices_bruteforce.clear();
00143     k_sqr_distances_bruteforce.clear();
00144 
00145     std::priority_queue<prioPointQueueEntry> pointCandidates;
00146 
00147 
00148     // push all points and their distance to the search point into a priority queue - bruteforce approach.
00149     for (i = 0; i < cloudIn->points.size (); i++)
00150     {
00151       pointDist = ((cloudIn->points[i].x - searchPoint.x) * (cloudIn->points[i].x - searchPoint.x) +
00152              /*+*/ (cloudIn->points[i].y - searchPoint.y) * (cloudIn->points[i].y - searchPoint.y) +
00153                    (cloudIn->points[i].z - searchPoint.z) * (cloudIn->points[i].z - searchPoint.z));
00154 
00155       prioPointQueueEntry pointEntry (cloudIn->points[i], pointDist, i);
00156 
00157       pointCandidates.push (pointEntry);
00158     }
00159 
00160     // pop priority queue until we have the nearest K elements
00161     while (pointCandidates.size () > K)
00162       pointCandidates.pop ();
00163 
00164     // copy results into vectors
00165     while (pointCandidates.size ())
00166     {
00167       k_indices_bruteforce.push_back (pointCandidates.top ().pointIdx_);
00168       k_sqr_distances_bruteforce.push_back (pointCandidates.top ().pointDistance_);
00169 
00170       pointCandidates.pop ();
00171     }
00172 
00173     //FAILS: ASSERT_EQ ( k_indices.size() , k_indices_bruteforce.size() );
00174 
00175     // compare nearest neighbor results of organized search  with bruteforce search
00176     for (i = 0; i < k_indices.size (); i++)
00177     {
00178       ASSERT_EQ ( k_indices[i] , k_indices_bruteforce.back() );
00179       EXPECT_NEAR (k_sqr_distances[i], k_sqr_distances_bruteforce.back(), 1e-4);
00180 
00181       k_indices_bruteforce.pop_back();
00182       k_sqr_distances_bruteforce.pop_back();
00183     }
00184   }
00185 }
00186 
00187 
00188 TEST (PCL, Organized_Neighbor_Search_Pointcloud_Nearest_K_Neighbour_Search_Kinect_Data)
00189 {
00190   const unsigned int test_runs = 2;
00191   unsigned int test_id;
00192 
00193   // instantiate point cloud
00194 
00195   size_t i;
00196 
00197   srand (time (NULL));
00198 
00199   unsigned int K;
00200 
00201   // create organized search
00202   pcl::search::Search<PointXYZ>* organizedNeighborSearch = new pcl::search::OrganizedNeighbor<PointXYZ>();
00203 
00204   std::vector<int> k_indices;
00205   std::vector<float> k_sqr_distances;
00206 
00207   std::vector<int> k_indices_bruteforce;
00208   std::vector<float> k_sqr_distances_bruteforce;
00209 
00210   // typical focal length from kinect
00211   pcl::PointCloud<PointXYZ>::Ptr cloudIn (new pcl::PointCloud<PointXYZ>);
00212   pcl::PCDReader pcd;
00213 
00214   unsigned int searchIdx;
00215 
00216   if (pcd.read ("office1.pcd", *cloudIn) == -1)
00217   {
00218     std::cout <<"Couldn't read input cloud" << std::endl;
00219     return;
00220   }
00221 
00222   while(1)
00223   {
00224    searchIdx = rand()%(cloudIn->width * cloudIn->height);
00225    if(cloudIn->points[searchIdx].z <100)break;
00226   }
00227 
00228   for (test_id = 0; test_id < test_runs; test_id++)
00229   {
00230     // define a random search point
00231 
00232     K = (rand () % 10)+1;
00233 //     K = 16;
00234     // generate point cloud
00235     const PointXYZ& searchPoint = cloudIn->points[searchIdx];
00236 
00237     k_indices.clear();
00238     k_sqr_distances.clear();
00239 
00240     // organized nearest neighbor search
00241     organizedNeighborSearch->setInputCloud (cloudIn);
00242     organizedNeighborSearch->nearestKSearch (searchIdx, (int)K, k_indices, k_sqr_distances);
00243  
00244     double pointDist;
00245 
00246     k_indices_bruteforce.clear();
00247     k_sqr_distances_bruteforce.clear();
00248 
00249     std::priority_queue<prioPointQueueEntry> pointCandidates;
00250 
00251 
00252     // push all points and their distance to the search point into a priority queue - bruteforce approach.
00253     for (i = 0; i < cloudIn->points.size (); i++)
00254     {
00255       pointDist = ((cloudIn->points[i].x - searchPoint.x) * (cloudIn->points[i].x - searchPoint.x) +
00256              /*+*/ (cloudIn->points[i].y - searchPoint.y) * (cloudIn->points[i].y - searchPoint.y) +
00257                    (cloudIn->points[i].z - searchPoint.z) * (cloudIn->points[i].z - searchPoint.z));
00258 
00259       prioPointQueueEntry pointEntry (cloudIn->points[i], pointDist, i);
00260 
00261       pointCandidates.push (pointEntry);
00262     }
00263 
00264     // pop priority queue until we have the nearest K elements
00265     while (pointCandidates.size () > K)
00266       pointCandidates.pop ();
00267 
00268     // copy results into vectors
00269     while (pointCandidates.size ())
00270     {
00271       k_indices_bruteforce.push_back (pointCandidates.top ().pointIdx_);
00272       k_sqr_distances_bruteforce.push_back (pointCandidates.top ().pointDistance_);
00273 
00274       pointCandidates.pop ();
00275     }
00276     ASSERT_EQ ( k_indices.size() , k_indices_bruteforce.size() );
00277   }
00278 }
00279 
00280 TEST (PCL, Organized_Neighbor_Search_Pointcloud_Neighbours_Within_Radius_Search)
00281 {
00282   const unsigned int test_runs = 10;
00283   unsigned int test_id;
00284 
00285   size_t i,j;
00286 
00287   srand (time (NULL));
00288   
00289   pcl::search::Search<PointXYZ>* organizedNeighborSearch = new pcl::search::OrganizedNeighbor<PointXYZ>();
00290 
00291   std::vector<int> k_indices;
00292   std::vector<float> k_sqr_distances;
00293 
00294   std::vector<int> k_indices_bruteforce;
00295   std::vector<float> k_sqr_distances_bruteforce;
00296 
00297   // typical focal length from kinect
00298   const double oneOverFocalLength = 0.0018;
00299   double x,y,z;
00300 
00301   int xpos, ypos, centerX, centerY, idx;
00302 
00303   for (test_id = 0; test_id < test_runs; test_id++)
00304   {
00305     // generate point cloud
00306     PointCloud<PointXYZ>::Ptr cloudIn (new PointCloud<PointXYZ> ());
00307 
00308     cloudIn->width = 640;
00309     cloudIn->height = 480;
00310     cloudIn->points.clear();
00311     cloudIn->points.resize (cloudIn->width * cloudIn->height);
00312 
00313     centerX = cloudIn->width>>1;
00314     centerY = cloudIn->height>>1;
00315 
00316     idx = 0;
00317     for (ypos = -centerY; ypos < centerY; ypos++)
00318       for (xpos = -centerX; xpos < centerX; xpos++)
00319       {
00320         z = 5.0 * ( ((double)rand () / (double)RAND_MAX))+5;
00321         y = ypos*oneOverFocalLength*z;
00322         x = xpos*oneOverFocalLength*z;
00323 
00324         cloudIn->points[idx++]= PointXYZ (x, y, z);
00325       }
00326 
00327     unsigned int randomIdx = rand()%(cloudIn->width * cloudIn->height);
00328 
00329     const PointXYZ& searchPoint = cloudIn->points[randomIdx];
00330 
00331     double pointDist;
00332    double searchRadius = 1.0 * ((double)rand () / (double)RAND_MAX);
00333 //   double searchRadius = 1/10;
00334 
00335     int minX = cloudIn->width;
00336     int minY = cloudIn->height;
00337     int maxX = 0;
00338     int maxY = 0;
00339 
00340     // bruteforce radius search
00341     vector<int> cloudSearchBruteforce;
00342     cloudSearchBruteforce.clear();
00343 
00344     for (i = 0; i < cloudIn->points.size (); i++)
00345     {
00346       pointDist = sqrt (
00347                         (cloudIn->points[i].x - searchPoint.x) * (cloudIn->points[i].x - searchPoint.x)
00348                       + (cloudIn->points[i].y - searchPoint.y) * (cloudIn->points[i].y - searchPoint.y)
00349                       + (cloudIn->points[i].z - searchPoint.z) * (cloudIn->points[i].z - searchPoint.z));
00350 
00351       if (pointDist <= searchRadius)
00352       {
00353         // add point candidates to vector list
00354         cloudSearchBruteforce.push_back ((int)i);
00355 
00356         minX = std::min<int>(minX, i%cloudIn->width);
00357         minY = std::min<int>(minY, i/cloudIn->width);
00358         maxX = std::max<int>(maxX, i%cloudIn->width);
00359         maxY = std::max<int>(maxY, i/cloudIn->width);
00360       }
00361     }
00362 
00363     vector<int> cloudNWRSearch;
00364     vector<float> cloudNWRRadius;
00365 
00366     organizedNeighborSearch->setInputCloud (cloudIn);
00367 
00368     organizedNeighborSearch->radiusSearch (randomIdx, searchRadius, cloudNWRSearch, cloudNWRRadius, INT_MAX); //,INT_MAX);
00369    
00370       
00371     // check if result from organized radius search can be also found in bruteforce search
00372     std::vector<int>::const_iterator current = cloudNWRSearch.begin();
00373     while (current != cloudNWRSearch.end())
00374     {
00375       pointDist = sqrt (
00376           (cloudIn->points[*current].x-searchPoint.x) * (cloudIn->points[*current].x-searchPoint.x) +
00377           (cloudIn->points[*current].y-searchPoint.y) * (cloudIn->points[*current].y-searchPoint.y) +
00378           (cloudIn->points[*current].z-searchPoint.z) * (cloudIn->points[*current].z-searchPoint.z)
00379       );
00380 
00381       ASSERT_EQ ( (pointDist<=searchRadius) , true);
00382 
00383       ++current;
00384     }
00385 
00386 
00387     // check if bruteforce result from organized radius search can be also found in bruteforce search
00388     current = cloudSearchBruteforce.begin();
00389     while (current != cloudSearchBruteforce.end())
00390     {
00391 
00392       pointDist = sqrt (
00393           (cloudIn->points[*current].x-searchPoint.x) * (cloudIn->points[*current].x-searchPoint.x) +
00394           (cloudIn->points[*current].y-searchPoint.y) * (cloudIn->points[*current].y-searchPoint.y) +
00395           (cloudIn->points[*current].z-searchPoint.z) * (cloudIn->points[*current].z-searchPoint.z)
00396       );
00397 
00398       ASSERT_EQ ( (pointDist<=searchRadius) , true);
00399 
00400       ++current;
00401     }
00402 
00403     for (i = 0; i < cloudSearchBruteforce.size (); i++) 
00404       for (j = 0; j < cloudNWRSearch.size (); j++) 
00405         if (cloudNWRSearch[i]== cloudSearchBruteforce[j])
00406           break;
00407 
00408     ASSERT_EQ (cloudNWRRadius.size() , cloudSearchBruteforce.size ());
00409 
00410     // check if result limitation works
00411     organizedNeighborSearch->radiusSearch (searchPoint, searchRadius, cloudNWRSearch, cloudNWRRadius, 5);
00412 
00413     ASSERT_EQ (cloudNWRRadius.size () <= 5, true);
00414   }
00415 }
00416 
00417 
00418 TEST (PCL, Organized_Neighbor_Search_Pointcloud_Neighbours_Within_Radius_Search_Benchmark_Test)
00419 {
00420   const unsigned int test_runs = 10;
00421   unsigned int test_id;
00422   srand (time (NULL));
00423 
00424   pcl::search::Search<PointXYZ>* organizedNeighborSearch = new pcl::search::OrganizedNeighbor<PointXYZ>();
00425 
00426   std::vector<int> k_indices;
00427   std::vector<float> k_sqr_distances;
00428 
00429   std::vector<int> k_indices_bruteforce;
00430   std::vector<float> k_sqr_distances_bruteforce;
00431 
00432   // typical focal length from kinect
00433   const double oneOverFocalLength = 0.0018;
00434   double x,y,z;
00435 
00436   int xpos, ypos, centerX, centerY, idx;
00437   
00438   double radiusSearchTime = 0, radiusSearchLPTime = 0;
00439 
00440   for (test_id = 0; test_id < test_runs; test_id++)
00441   {
00442     // generate point cloud
00443 
00444     PointCloud<PointXYZ>::Ptr cloudIn (new PointCloud<PointXYZ> ());
00445 
00446     cloudIn->width = 1024;
00447     cloudIn->height = 768;
00448     cloudIn->points.clear();
00449     cloudIn->points.resize (cloudIn->width * cloudIn->height);
00450 
00451     centerX = cloudIn->width>>1;
00452     centerY = cloudIn->height>>1;
00453 
00454     idx = 0;
00455     for (ypos = -centerY; ypos < centerY; ypos++)
00456       for (xpos = -centerX; xpos < centerX; xpos++)
00457       {
00458         z = 5.0 * ( ((double)rand () / (double)RAND_MAX))+5;
00459         y = ypos*oneOverFocalLength*z;
00460         x = xpos*oneOverFocalLength*z;
00461 
00462         cloudIn->points[idx++]= PointXYZ (x, y, z);
00463       }
00464 
00465     unsigned int randomIdx = rand()%(cloudIn->width * cloudIn->height);
00466 
00467     const PointXYZ& searchPoint = cloudIn->points[randomIdx];
00468 
00469     double pointDist;
00470     double searchRadius = 1.0 * ((double)rand () / (double)RAND_MAX);
00471 
00472     int minX = cloudIn->width;
00473     int minY = cloudIn->height;
00474     int maxX = 0;
00475     int maxY = 0;
00476 
00477     // bruteforce radius search
00478     vector<int> cloudSearchBruteforce;
00479     cloudSearchBruteforce.clear();
00480 
00481     for (size_t i = 0; i < cloudIn->points.size (); i++)
00482     {
00483       pointDist = sqrt (
00484                         (cloudIn->points[i].x - searchPoint.x) * (cloudIn->points[i].x - searchPoint.x)
00485                       + (cloudIn->points[i].y - searchPoint.y) * (cloudIn->points[i].y - searchPoint.y)
00486                       + (cloudIn->points[i].z - searchPoint.z) * (cloudIn->points[i].z - searchPoint.z));
00487 
00488       if (pointDist <= searchRadius)
00489       {
00490         // add point candidates to vector list
00491         cloudSearchBruteforce.push_back ((int)i);
00492 
00493         minX = std::min<int>(minX, i%cloudIn->width);
00494         minY = std::min<int>(minY, i/cloudIn->width);
00495         maxX = std::max<int>(maxX, i%cloudIn->width);
00496         maxY = std::max<int>(maxY, i/cloudIn->width);
00497       }
00498     }
00499 
00500 
00501     vector<int> cloudNWRSearch;
00502     vector<float> cloudNWRRadius;
00503     
00504     double check_time = getTime();
00505     organizedNeighborSearch->setInputCloud (cloudIn);
00506     organizedNeighborSearch->radiusSearch (randomIdx, searchRadius, cloudNWRSearch, cloudNWRRadius, INT_MAX); //,INT_MAX);
00507     
00508     double check_time2 = getTime();
00509     
00510     radiusSearchLPTime += check_time2 - check_time;
00511 
00512     organizedNeighborSearch->setInputCloud (cloudIn);
00513     organizedNeighborSearch->radiusSearch (randomIdx, searchRadius, cloudNWRSearch, cloudNWRRadius, INT_MAX); //,INT_MAX);
00514 
00515     radiusSearchTime += getTime() - check_time2;
00516   }
00517 }
00518 
00519 TEST (PCL, Organized_Neighbor_Search_Pointcloud_Neighbours_Within_Radius_Search_Kinect_Data)
00520 {
00521 
00522   pcl::PointCloud<PointXYZ>::Ptr cloud (new pcl::PointCloud<PointXYZ>);
00523   pcl::PointCloud<PointXYZ>::Ptr cloudIn (new pcl::PointCloud<PointXYZ>);
00524     pcl::PCDReader pcd;
00525 
00526 
00527   if (pcd.read ("office1.pcd", *cloudIn) == -1)
00528   {
00529         std::cout <<"Couldn't read input cloud" << std::endl; 
00530     return;
00531   }
00532   const unsigned int test_runs = 10;
00533   unsigned int test_id;
00534   srand (time (NULL));
00535 
00536   pcl::search::Search<PointXYZ>* organizedNeighborSearch = new pcl::search::OrganizedNeighbor<PointXYZ>();
00537 
00538   std::vector<int> k_indices;
00539   std::vector<float> k_sqr_distances;
00540 
00541   std::vector<int> k_indices_bruteforce;
00542   std::vector<float> k_sqr_distances_bruteforce;
00543 
00544   // typical focal length from kinect
00545   unsigned int randomIdx;
00546 
00547   while (1)
00548   {
00549           randomIdx = rand()%(cloudIn->width * cloudIn->height);
00550           if(cloudIn->points[randomIdx].z <100)break;
00551   }
00552 
00553   cout << "**Search Point:** " << cloudIn->points[randomIdx].x << " " << cloudIn->points[randomIdx].y << " " << cloudIn->points[randomIdx].z << endl;
00554 
00555   std::cout << "+--------+-----------------+----------------+-------------------+" << std::endl;
00556   std::cout << "| Search | Organized       | Organized      | Number of         |" << std::endl;
00557   std::cout << "| Radius | Neighbor Search | Data Index     | Nearest Neighbors |" << std::endl;
00558   std::cout << "+========+=================+================+===================+" << std::endl;
00559 
00560   for (test_id = 0; test_id < test_runs; test_id++)
00561   {
00562 
00563     double searchRadius = 1.0 * (test_id*1.0/10*1.0);
00564     double sum_time = 0, sum_time2 = 0;
00565 
00566     vector<int> cloudNWRSearch;
00567     vector<float> cloudNWRRadius;
00568 
00569     vector<int> cloudNWRSearch2;
00570     vector<float> cloudNWRRadius2;
00571 
00572     for (int iter = 0; iter < 100; iter++)
00573     {
00574 
00575       cloudNWRSearch2.clear();
00576       cloudNWRRadius2.clear();
00577 
00578       double check_time = getTime();
00579       organizedNeighborSearch->setInputCloud (cloudIn);
00580       organizedNeighborSearch->radiusSearch (randomIdx, searchRadius, cloudNWRSearch2, cloudNWRRadius2, INT_MAX); //,INT_MAX);
00581 
00582       double check_time2 = getTime();
00583       sum_time+= check_time2 - check_time;
00584      }
00585 
00586 
00587      for(int iter=0;iter<100;iter++)
00588      {
00589        cloudNWRSearch.clear();
00590        cloudNWRRadius.clear();
00591        double check_time = getTime();
00592        organizedNeighborSearch->setInputCloud (cloudIn);
00593        organizedNeighborSearch->approxRadiusSearch (cloudIn, randomIdx, searchRadius, cloudNWRSearch, cloudNWRRadius, INT_MAX); //,INT_MAX);
00594 
00595        double check_time2 = getTime();
00596        sum_time2+= check_time2 - check_time;
00597      }
00598 
00599  //  ASSERT_EQ(cloudNWRRadius.size(), cloudNWRRadius2.size());
00600  //  ASSERT_EQ(cloudNWRSearch.size(), cloudNWRSearch2.size());
00601   
00602 
00603     printf("| %.3lf  | %0.5lf         | %0.5lf        | %6lu            |\n",searchRadius, sum_time/100, sum_time2/100, cloudNWRSearch.size());
00604     std::cout << "+--------+-----------------+----------------+-------------------+" << std::endl;
00605 
00606     const PointXYZ& searchPoint = cloudIn->points[randomIdx];
00607 
00608     double pointDist;
00609 
00610     int minX = cloudIn->width;
00611     int minY = cloudIn->height;
00612     int maxX = 0;
00613     int maxY = 0;
00614 
00615     // bruteforce radius search
00616     vector<int> cloudSearchBruteforce;
00617     cloudSearchBruteforce.clear();
00618 
00619     for (size_t i = 0; i < cloudIn->points.size (); i++)
00620     {
00621       pointDist = sqrt (
00622                         (cloudIn->points[i].x - searchPoint.x) * (cloudIn->points[i].x - searchPoint.x)
00623                       + (cloudIn->points[i].y - searchPoint.y) * (cloudIn->points[i].y - searchPoint.y)
00624                       + (cloudIn->points[i].z - searchPoint.z) * (cloudIn->points[i].z - searchPoint.z));
00625 
00626       if (pointDist <= searchRadius)
00627       {
00628         // add point candidates to vector list
00629         cloudSearchBruteforce.push_back ((int)i);
00630 
00631         minX = std::min<int>(minX, i%cloudIn->width);
00632         minY = std::min<int>(minY, i/cloudIn->width);
00633         maxX = std::max<int>(maxX, i%cloudIn->width);
00634         maxY = std::max<int>(maxY, i/cloudIn->width);
00635       }
00636     }
00637 
00638     // check if result from organized radius search can be also found in bruteforce search
00639     std::vector<int>::const_iterator current = cloudNWRSearch.begin();
00640     while (current != cloudNWRSearch.end())
00641     {
00642       pointDist = sqrt (
00643           (cloudIn->points[*current].x-searchPoint.x) * (cloudIn->points[*current].x-searchPoint.x) +
00644           (cloudIn->points[*current].y-searchPoint.y) * (cloudIn->points[*current].y-searchPoint.y) +
00645           (cloudIn->points[*current].z-searchPoint.z) * (cloudIn->points[*current].z-searchPoint.z)
00646       );
00647 
00648       ASSERT_EQ ( (pointDist<=searchRadius) , true);
00649 
00650       ++current;
00651     }
00652 
00653 
00654     // check if bruteforce result from organized radius search can be also found in bruteforce search
00655     current = cloudSearchBruteforce.begin();
00656     while (current != cloudSearchBruteforce.end())
00657     {
00658 
00659       pointDist = sqrt (
00660           (cloudIn->points[*current].x-searchPoint.x) * (cloudIn->points[*current].x-searchPoint.x) +
00661           (cloudIn->points[*current].y-searchPoint.y) * (cloudIn->points[*current].y-searchPoint.y) +
00662           (cloudIn->points[*current].z-searchPoint.z) * (cloudIn->points[*current].z-searchPoint.z)
00663       );
00664 
00665       ASSERT_EQ ( (pointDist<=searchRadius) , true);
00666 
00667       ++current;
00668     }
00669     ASSERT_EQ (cloudNWRRadius.size() , cloudSearchBruteforce.size ());
00670 
00671     // check if result limitation works
00672     organizedNeighborSearch->radiusSearch (searchPoint, searchRadius, cloudNWRSearch, cloudNWRRadius, 5);
00673 
00674     ASSERT_EQ (cloudNWRRadius.size () <= 5, true);
00675   }
00676 }
00677 
00678 /* ---[ */
00679 int
00680 main (int argc, char** argv)
00681 {
00682 
00683   testing::InitGoogleTest (&argc, argv);
00684   return (RUN_ALL_TESTS ());
00685 }
00686 /* ]--- */


pcl
Author(s): Open Perception
autogenerated on Mon Oct 6 2014 03:18:24