test_organized.cpp
Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2010, 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  *
00035  */
00038 #include <gtest/gtest.h>
00039 
00040 #include <vector>
00041 
00042 #include <stdio.h>
00043 
00044 using namespace std;
00045 
00046 #include <pcl/common/time.h>
00047 #include <pcl/point_cloud.h>
00048 #include <pcl/point_types.h>
00049 
00050 using namespace pcl;
00051 
00052 #include <pcl/search/pcl_search.h>
00053 
00054 
00055 // helper class for priority queue
00056 class prioPointQueueEntry
00057 {
00058 public:
00059   prioPointQueueEntry ()
00060   {
00061   }
00062   prioPointQueueEntry (PointXYZ& point_arg, double pointDistance_arg, int pointIdx_arg)
00063   {
00064     point_ = point_arg;
00065     pointDistance_ = pointDistance_arg;
00066     pointIdx_ = pointIdx_arg;
00067   }
00068 
00069   bool
00070   operator< (const prioPointQueueEntry& rhs_arg) const
00071   {
00072     return (this->pointDistance_ < rhs_arg.pointDistance_);
00073   }
00074 
00075   PointXYZ point_;
00076   double pointDistance_;int pointIdx_;
00077 
00078 };
00079 
00080 TEST (PCL, Organized_Neighbor_Pointcloud_Nearest_K_Neighbour_Search)
00081 {
00082 
00083   const unsigned int test_runs = 2;
00084   unsigned int test_id;
00085 
00086   // instantiate point cloud
00087   PointCloud<PointXYZ>::Ptr cloudIn (new PointCloud<PointXYZ> ());
00088 
00089   size_t i;
00090 
00091   srand (time (NULL));
00092 
00093   unsigned int K;
00094 
00095   // create organized search
00096         search::OrganizedNeighbor<PointXYZ> organizedNeighborSearch;
00097 
00098   std::vector<int> k_indices;
00099   std::vector<float> k_sqr_distances;
00100 
00101   std::vector<int> k_indices_bruteforce;
00102   std::vector<float> k_sqr_distances_bruteforce;
00103 
00104   // typical focal length from kinect
00105   const double oneOverFocalLength = 0.0018;
00106   double x,y,z;
00107 
00108   int xpos, ypos, centerX, centerY;
00109 
00110   for (test_id = 0; test_id < test_runs; test_id++)
00111   {
00112     // define a random search point
00113 
00114     K = (rand () % 10)+1;
00115 
00116     // generate point cloud
00117     cloudIn->width = 128;
00118     cloudIn->height = 32;
00119     cloudIn->points.clear();
00120     cloudIn->points.reserve (cloudIn->width * cloudIn->height);
00121 
00122     centerX = cloudIn->width>>1;
00123     centerY = cloudIn->height>>1;
00124 
00125     for (ypos = -centerY; ypos < centerY; ypos++)
00126       for (xpos = -centerX; xpos < centerX; xpos++)
00127       {
00128         z = 15.0 * ((double)rand () / (double)(RAND_MAX+1.0))+20;
00129         y = (double)ypos*oneOverFocalLength*(double)z;
00130         x = (double)xpos*oneOverFocalLength*(double)z;
00131 
00132         cloudIn->points.push_back(PointXYZ (x, y, z));
00133       }
00134 
00135     unsigned int searchIdx = rand()%(cloudIn->width * cloudIn->height);
00136     const PointXYZ& searchPoint = cloudIn->points[searchIdx];
00137 
00138     k_indices.clear();
00139     k_sqr_distances.clear();
00140 
00141     // organized nearest neighbor search
00142     organizedNeighborSearch.setInputCloud (cloudIn);
00143     organizedNeighborSearch.nearestKSearch (searchPoint, (int)K, k_indices, k_sqr_distances);
00144 
00145 
00146     double pointDist;
00147 
00148     k_indices_bruteforce.clear();
00149     k_sqr_distances_bruteforce.clear();
00150 
00151     std::priority_queue<prioPointQueueEntry, std::vector<prioPointQueueEntry, Eigen::aligned_allocator<prioPointQueueEntry> > > pointCandidates;
00152 
00153 
00154     // push all points and their distance to the search point into a priority queue - bruteforce approach.
00155     for (i = 0; i < cloudIn->points.size (); i++)
00156     {
00157       pointDist = ((cloudIn->points[i].x - searchPoint.x) * (cloudIn->points[i].x - searchPoint.x) +
00158              /*+*/ (cloudIn->points[i].y - searchPoint.y) * (cloudIn->points[i].y - searchPoint.y) +
00159                    (cloudIn->points[i].z - searchPoint.z) * (cloudIn->points[i].z - searchPoint.z));
00160 
00161       prioPointQueueEntry pointEntry (cloudIn->points[i], pointDist, i);
00162 
00163       pointCandidates.push (pointEntry);
00164     }
00165 
00166     // pop priority queue until we have the nearest K elements
00167     while (pointCandidates.size () > K)
00168       pointCandidates.pop ();
00169 
00170     // copy results into vectors
00171     while (pointCandidates.size ())
00172     {
00173       k_indices_bruteforce.push_back (pointCandidates.top ().pointIdx_);
00174       k_sqr_distances_bruteforce.push_back (pointCandidates.top ().pointDistance_);
00175 
00176       pointCandidates.pop ();
00177     }
00178 
00179 
00180     ASSERT_EQ ( k_indices.size() , k_indices_bruteforce.size() );
00181 
00182     // compare nearest neighbor results of organized search  with bruteforce search
00183     for (i = 0; i < k_indices.size (); i++)
00184     {
00185       ASSERT_EQ ( k_indices[i] , k_indices_bruteforce.back() );
00186       EXPECT_NEAR (k_sqr_distances[i], k_sqr_distances_bruteforce.back(), 1e-4);
00187 
00188       k_indices_bruteforce.pop_back();
00189       k_sqr_distances_bruteforce.pop_back();
00190     }
00191 
00192   }
00193 
00194 }
00195 
00196 TEST (PCL, Organized_Neighbor_Pointcloud_Neighbours_Within_Radius_Search)
00197 {
00198 
00199   const unsigned int test_runs = 10;
00200   unsigned int test_id;
00201 
00202   size_t i,j;
00203 
00204   srand (time (NULL));
00205 
00206         search::OrganizedNeighbor<PointXYZ> organizedNeighborSearch;
00207 
00208   std::vector<int> k_indices;
00209   std::vector<float> k_sqr_distances;
00210 
00211   std::vector<int> k_indices_bruteforce;
00212   std::vector<float> k_sqr_distances_bruteforce;
00213 
00214   // typical focal length from kinect
00215   const double oneOverFocalLength = 0.0018;
00216   double x,y,z;
00217 
00218   int xpos, ypos, centerX, centerY, idx;
00219 
00220   for (test_id = 0; test_id < test_runs; test_id++)
00221   {
00222     // generate point cloud
00223     PointCloud<PointXYZ>::Ptr cloudIn (new PointCloud<PointXYZ> ());
00224 
00225     cloudIn->width = 640;
00226     cloudIn->height = 480;
00227     cloudIn->points.clear();
00228     cloudIn->points.resize (cloudIn->width * cloudIn->height);
00229 
00230     centerX = cloudIn->width>>1;
00231     centerY = cloudIn->height>>1;
00232 
00233     idx = 0;
00234     for (ypos = -centerY; ypos < centerY; ypos++)
00235       for (xpos = -centerX; xpos < centerX; xpos++)
00236       {
00237         z = 5.0 * ( ((double)rand () / (double)RAND_MAX))+5;
00238         y = ypos*oneOverFocalLength*z;
00239         x = xpos*oneOverFocalLength*z;
00240 
00241         cloudIn->points[idx++]= PointXYZ (x, y, z);
00242       }
00243 
00244     unsigned int randomIdx = rand()%(cloudIn->width * cloudIn->height);
00245 
00246     const PointXYZ& searchPoint = cloudIn->points[randomIdx];
00247 
00248     double pointDist;
00249     double searchRadius = 1.0 * ((double)rand () / (double)RAND_MAX);
00250 
00251     int minX = cloudIn->width;
00252     int minY = cloudIn->height;
00253     int maxX = 0;
00254     int maxY = 0;
00255 
00256     // bruteforce radius search
00257     vector<int> cloudSearchBruteforce;
00258     cloudSearchBruteforce.clear();
00259 
00260     for (i = 0; i < cloudIn->points.size (); i++)
00261     {
00262       pointDist = sqrt (
00263                         (cloudIn->points[i].x - searchPoint.x) * (cloudIn->points[i].x - searchPoint.x)
00264                       + (cloudIn->points[i].y - searchPoint.y) * (cloudIn->points[i].y - searchPoint.y)
00265                       + (cloudIn->points[i].z - searchPoint.z) * (cloudIn->points[i].z - searchPoint.z));
00266 
00267       if (pointDist <= searchRadius)
00268       {
00269         // add point candidates to vector list
00270         cloudSearchBruteforce.push_back ((int)i);
00271 
00272         minX = std::min<int>(minX, i%cloudIn->width);
00273         minY = std::min<int>(minY, i/cloudIn->width);
00274         maxX = std::max<int>(maxX, i%cloudIn->width);
00275         maxY = std::max<int>(maxY, i/cloudIn->width);
00276       }
00277     }
00278 
00279                 std::vector<int> cloudNWRSearch;
00280                 std::vector<float> cloudNWRRadius;
00281 
00282     // execute organized search
00283     organizedNeighborSearch.setInputCloud (cloudIn);
00284     organizedNeighborSearch.radiusSearch (searchPoint, searchRadius, cloudNWRSearch, cloudNWRRadius);
00285 
00286     // check if result from organized radius search can be also found in bruteforce search
00287     std::vector<int>::const_iterator current = cloudNWRSearch.begin();
00288     while (current != cloudNWRSearch.end())
00289     {
00290       pointDist = sqrt (
00291           (cloudIn->points[*current].x-searchPoint.x) * (cloudIn->points[*current].x-searchPoint.x) +
00292           (cloudIn->points[*current].y-searchPoint.y) * (cloudIn->points[*current].y-searchPoint.y) +
00293           (cloudIn->points[*current].z-searchPoint.z) * (cloudIn->points[*current].z-searchPoint.z)
00294       );
00295 
00296       ASSERT_EQ ( (pointDist<=searchRadius) , true);
00297 
00298       ++current;
00299     }
00300 
00301 
00302     // check if bruteforce result from organized radius search can be also found in bruteforce search
00303     current = cloudSearchBruteforce.begin();
00304     while (current != cloudSearchBruteforce.end())
00305     {
00306 
00307       pointDist = sqrt (
00308           (cloudIn->points[*current].x-searchPoint.x) * (cloudIn->points[*current].x-searchPoint.x) +
00309           (cloudIn->points[*current].y-searchPoint.y) * (cloudIn->points[*current].y-searchPoint.y) +
00310           (cloudIn->points[*current].z-searchPoint.z) * (cloudIn->points[*current].z-searchPoint.z)
00311       );
00312 
00313       ASSERT_EQ ( (pointDist<=searchRadius) , true);
00314 
00315       ++current;
00316     }
00317 
00318     for (i = 0; i < cloudSearchBruteforce.size (); i++) 
00319       for (j = 0; j < cloudNWRSearch.size (); j++) 
00320         if (cloudNWRSearch[i]== cloudSearchBruteforce[j])
00321           break;
00322 
00323     ASSERT_EQ (cloudNWRRadius.size() , cloudSearchBruteforce.size ());
00324 
00325     // check if result limitation works
00326     organizedNeighborSearch.radiusSearch (searchPoint, searchRadius, cloudNWRSearch, cloudNWRRadius, 5);
00327 
00328     ASSERT_EQ (cloudNWRRadius.size () <= 5, true);
00329   }
00330 }
00331 
00332 /* ---[ */
00333 int
00334 main (int argc, char** argv)
00335 {
00336   testing::InitGoogleTest (&argc, argv);
00337   return (RUN_ALL_TESTS ());
00338 }
00339 /* ]--- */


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