test_octree.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  *
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/search/pcl_search.h>
00045 
00046 using namespace std;
00047 using namespace pcl;
00048 using namespace octree;
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, Octree_Pointcloud_Nearest_K_Neighbour_Search)
00075 {
00076   const unsigned int test_runs = 1;
00077   unsigned int test_id;
00078 
00079   // instantiate point cloud
00080   PointCloud<PointXYZ>::Ptr cloudIn (new PointCloud<PointXYZ> ());
00081 
00082   size_t i;
00083   srand (static_cast<unsigned int> (time (NULL)));
00084   unsigned int K;
00085 
00086   std::priority_queue<prioPointQueueEntry, pcl::PointCloud<prioPointQueueEntry>::VectorType> pointCandidates;
00087 
00088   // create octree
00089   pcl::search::Search<PointXYZ>* octree = new pcl::search::Octree<PointXYZ> (0.1);
00090 
00091   std::vector<int> k_indices;
00092   std::vector<float> k_sqr_distances;
00093 
00094   std::vector<int> k_indices_bruteforce;
00095   std::vector<float> k_sqr_distances_bruteforce;
00096 
00097   for (test_id = 0; test_id < test_runs; test_id++)
00098   {
00099     // define a random search point
00100     PointXYZ searchPoint (static_cast<float> (10.0 * (rand () / static_cast<double> (RAND_MAX))), 
00101                           static_cast<float> (10.0 * (rand () / static_cast<double> (RAND_MAX))),
00102                           static_cast<float> (10.0 * (rand () / static_cast<double> (RAND_MAX))));
00103 
00104     K = 1 + rand () % 10;
00105 
00106     // generate point cloud
00107     cloudIn->width = 1000;
00108     cloudIn->height = 1;
00109     cloudIn->points.resize (cloudIn->width * cloudIn->height);
00110     for (i = 0; i < 1000; i++)
00111     {
00112       cloudIn->points[i] = PointXYZ (static_cast<float> (5.0  * (rand () / static_cast<double> (RAND_MAX))),
00113                                      static_cast<float> (10.0 * (rand () / static_cast<double> (RAND_MAX))),
00114                                      static_cast<float> (10.0 * (rand () / static_cast<double> (RAND_MAX))));
00115     }
00116 
00117     double pointDist;
00118 
00119     k_indices.clear ();
00120     k_sqr_distances.clear ();
00121 
00122     k_indices_bruteforce.clear ();
00123     k_sqr_distances_bruteforce.clear ();
00124 
00125     // push all points and their distance to the search point into a priority queue - bruteforce approach.
00126     for (i = 0; i < cloudIn->points.size (); i++)
00127     {
00128       pointDist = ((cloudIn->points[i].x - searchPoint.x) * (cloudIn->points[i].x - searchPoint.x)
00129           + (cloudIn->points[i].y - searchPoint.y) * (cloudIn->points[i].y - searchPoint.y) + (cloudIn->points[i].z
00130           - searchPoint.z) * (cloudIn->points[i].z - searchPoint.z));
00131 
00132       prioPointQueueEntry pointEntry (cloudIn->points[i], pointDist, static_cast<int> (i));
00133 
00134       pointCandidates.push (pointEntry);
00135     }
00136 
00137     // pop priority queue until we have the nearest K elements
00138     while (pointCandidates.size () > K)
00139       pointCandidates.pop ();
00140 
00141     // copy results into vectors
00142     unsigned idx = static_cast<unsigned> (pointCandidates.size ());
00143     k_indices_bruteforce.resize (idx);
00144     k_sqr_distances_bruteforce.resize (idx);
00145     while (pointCandidates.size ())
00146     {
00147       --idx;
00148       k_indices_bruteforce [idx] = pointCandidates.top ().pointIdx_;
00149       k_sqr_distances_bruteforce [idx] = static_cast<float> (pointCandidates.top ().pointDistance_);
00150 
00151       pointCandidates.pop ();
00152     }
00153     // octree nearest neighbor search
00154     octree->setInputCloud (cloudIn);
00155     octree->nearestKSearch (searchPoint, static_cast<int> (K), k_indices, k_sqr_distances);
00156 
00157     ASSERT_EQ ( k_indices.size() , k_indices_bruteforce.size() );
00158 
00159     // compare nearest neighbor results of octree with bruteforce search
00160     i = 0;
00161     while (k_indices_bruteforce.size ())
00162     {
00163       ASSERT_EQ ( k_indices.back() , k_indices_bruteforce.back() );
00164       EXPECT_NEAR (k_sqr_distances.back(), k_sqr_distances_bruteforce.back(), 1e-4);
00165 
00166       k_indices_bruteforce.pop_back();
00167       k_indices.pop_back();
00168 
00169       k_sqr_distances_bruteforce.pop_back();
00170       k_sqr_distances.pop_back();
00171     }
00172   }
00173 }
00174 
00175 #if 0
00176 TEST (PCL, Octree_Pointcloud_Approx_Nearest_Neighbour_Search)
00177 {
00178   const unsigned int test_runs = 100;
00179   unsigned int test_id;
00180   unsigned int bestMatchCount = 0;
00181 
00182   // instantiate point cloud
00183   PointCloud<PointXYZ>::Ptr cloudIn (new PointCloud<PointXYZ> ());
00184 
00185   size_t i;
00186   srand (time (NULL));
00187 
00188   double voxelResolution = 0.1;
00189 
00190   // create octree
00191   pcl::search::Search<PointXYZ>* octree = new pcl::search::Octree<PointXYZ> (voxelResolution);
00192 
00193   for (test_id = 0; test_id < test_runs; test_id++)
00194   {
00195     // define a random search point
00196     PointXYZ searchPoint (10.0 * ((double)rand () / (double)RAND_MAX), 10.0 * ((double)rand () / (double)RAND_MAX),
00197                           10.0 * ((double)rand () / (double)RAND_MAX));
00198 
00199     // generate point cloud
00200     cloudIn->width = 1000;
00201     cloudIn->height = 1;
00202     cloudIn->points.resize (cloudIn->width * cloudIn->height);
00203     for (i = 0; i < 1000; i++)
00204       cloudIn->points[i] = PointXYZ (5.0 * ((double)rand () / (double)RAND_MAX),
00205                                      10.0 * ((double)rand () / (double)RAND_MAX),
00206                                      10.0 * ((double)rand () / (double)RAND_MAX));
00207     // brute force search
00208     double pointDist;
00209     double BFdistance = numeric_limits<double>::max ();
00210     int BFindex = 0;
00211 
00212     for (i = 0; i < cloudIn->points.size (); i++)
00213     {
00214       pointDist = ((cloudIn->points[i].x - searchPoint.x) * (cloudIn->points[i].x - searchPoint.x)
00215           + (cloudIn->points[i].y - searchPoint.y) * (cloudIn->points[i].y - searchPoint.y) + (cloudIn->points[i].z
00216           - searchPoint.z) * (cloudIn->points[i].z - searchPoint.z));
00217 
00218       if (pointDist < BFdistance)
00219       {
00220         BFindex = i;
00221         BFdistance = pointDist;
00222       }
00223     }
00224 
00225     int ANNindex;
00226     float ANNdistance;
00227 
00228     octree->setInputCloud (cloudIn);
00229     octree->approxNearestSearch (searchPoint, ANNindex, ANNdistance);
00230 
00231     if (BFindex == ANNindex)
00232     {
00233       EXPECT_NEAR (ANNdistance, BFdistance, 1e-4);
00234       bestMatchCount++;
00235     }
00236   }
00237   // we should have found the absolute nearest neighbor at least once
00238   //ASSERT_EQ ( (bestMatchCount > 0) , true);
00239 }
00240 #endif
00241 #if 0
00242 TEST (PCL, Octree_RadiusSearch_GPU)
00243 {
00244   PointCloud<PointXYZ>::Ptr cloudIn (new PointCloud<PointXYZ> ());
00245   // generate point cloud data
00246   cloudIn->width = 1000;
00247   cloudIn->height = 1;
00248   cloudIn->points.resize (cloudIn->width * cloudIn->height);
00249 
00250   int i=0;
00251   for (i = 0; i < 1000; i++)
00252   {
00253     cloudIn->points[i] = PointXYZ (10.0 * ((double)rand () / (double)RAND_MAX),
00254         10.0 * ((double)rand () / (double)RAND_MAX),
00255         5.0 * ((double)rand () / (double)RAND_MAX));
00256   }
00257 
00258   Search<PointXYZ>* octree = new pcl::octree::OctreeWrapper<PointXYZ>(0.1f);
00259   octree->setInputCloud(cloudIn);
00260 
00261   std::vector <PointXYZ > point;
00262   const PointXYZ searchPoint (10.0 * ((double)rand () / (double)RAND_MAX), 10.0 * ((double)rand () / (double)RAND_MAX),
00263       10.0 * ((double)rand () / (double)RAND_MAX));
00264   point.push_back(searchPoint);
00265   point.push_back(searchPoint);
00266   point.push_back(searchPoint);
00267   double searchRadius = 5.0 * ((double)rand () / (double)RAND_MAX);
00268   double radius =5;
00269   vector < double > radiuses;
00270   radiuses.push_back(radius);
00271   radiuses.push_back(radius);
00272   radiuses.push_back(radius);
00273   std::vector<std::vector<int> > k_indices;
00274   std::vector<std::vector<float> > k_distances;
00275   int max_nn = -1;
00276 
00277   octree->radiusSearch (point, radiuses, k_indices,k_distances,max_nn );
00278 }
00279 
00280 #endif
00281 TEST (PCL, Octree_Pointcloud_Neighbours_Within_Radius_Search)
00282 {
00283   const unsigned int test_runs = 100;
00284   unsigned int test_id;
00285 
00286   // instantiate point clouds
00287   PointCloud<PointXYZ>::Ptr cloudIn (new PointCloud<PointXYZ> ());
00288   PointCloud<PointXYZ>::Ptr cloudOut (new PointCloud<PointXYZ> ());
00289 
00290   size_t i;
00291 
00292   srand (static_cast<unsigned int> (time (NULL)));
00293 
00294   for (test_id = 0; test_id < test_runs; test_id++)
00295   {
00296     // define a random search point
00297     PointXYZ searchPoint (static_cast<float> (10.0 * (rand () / static_cast<double> (RAND_MAX))), 
00298                           static_cast<float> (10.0 * (rand () / static_cast<double> (RAND_MAX))),
00299                           static_cast<float> (10.0 * (rand () / static_cast<double> (RAND_MAX))));
00300 
00301     cloudIn->width = 1000;
00302     cloudIn->height = 1;
00303     cloudIn->points.resize (cloudIn->width * cloudIn->height);
00304 
00305     // generate point cloud data
00306     for (i = 0; i < 1000; i++)
00307     {
00308       cloudIn->points[i] = PointXYZ (static_cast<float> (10.0 * (rand () / static_cast<double> (RAND_MAX))),
00309                                      static_cast<float> (10.0 * (rand () / static_cast<double> (RAND_MAX))),
00310                                      static_cast<float> (5.0 *  (rand () / static_cast<double> (RAND_MAX))));
00311     }
00312 
00313     pcl::search::Search<PointXYZ>* octree = new pcl::search::Octree<PointXYZ> (0.001);
00314 
00315     // build octree
00316     double pointDist;
00317     double searchRadius = 5.0 * rand () / static_cast<double> (RAND_MAX);
00318     
00319     // bruteforce radius search
00320     vector<int> cloudSearchBruteforce;
00321     for (i = 0; i < cloudIn->points.size (); i++)
00322     {
00323       pointDist = sqrt (
00324                         (cloudIn->points[i].x - searchPoint.x) * (cloudIn->points[i].x - searchPoint.x)
00325                             + (cloudIn->points[i].y - searchPoint.y) * (cloudIn->points[i].y - searchPoint.y)
00326                             + (cloudIn->points[i].z - searchPoint.z) * (cloudIn->points[i].z - searchPoint.z));
00327 
00328       if (pointDist <= searchRadius)
00329       {
00330         // add point candidates to vector list
00331         cloudSearchBruteforce.push_back (static_cast<int> (i));
00332       }
00333     }
00334 
00335     vector<int> cloudNWRSearch;
00336     vector<float> cloudNWRRadius;
00337 
00338     // execute octree radius search
00339     octree->setInputCloud (cloudIn);
00340     octree->radiusSearch (searchPoint, searchRadius, cloudNWRSearch, cloudNWRRadius);
00341 
00342     ASSERT_EQ ( cloudNWRRadius.size() , cloudSearchBruteforce.size());
00343 
00344     // check if result from octree radius search can be also found in bruteforce search
00345     std::vector<int>::const_iterator current = cloudNWRSearch.begin();
00346     while (current != cloudNWRSearch.end())
00347     {
00348       pointDist = sqrt (
00349           (cloudIn->points[*current].x-searchPoint.x) * (cloudIn->points[*current].x-searchPoint.x) +
00350           (cloudIn->points[*current].y-searchPoint.y) * (cloudIn->points[*current].y-searchPoint.y) +
00351           (cloudIn->points[*current].z-searchPoint.z) * (cloudIn->points[*current].z-searchPoint.z)
00352       );
00353 
00354       ASSERT_EQ ( (pointDist<=searchRadius) , true);
00355 
00356       ++current;
00357     }
00358 
00359     // check if result limitation works
00360     octree->radiusSearch(searchPoint, searchRadius, cloudNWRSearch, cloudNWRRadius, 5);
00361     ASSERT_EQ ( cloudNWRRadius.size() <= 5, true);
00362   }
00363 }
00364 
00365 /* ---[ */
00366 int
00367 main (int argc, char** argv)
00368 {
00369   testing::InitGoogleTest (&argc, argv);
00370   return (RUN_ALL_TESTS ());
00371 }
00372 /* ]--- */


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