test_kdtree.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 <iostream>
00039 #include <gtest/gtest.h>
00040 #include <pcl/common/time.h>
00041 #include <pcl/search/pcl_search.h>
00042 #include <pcl/point_cloud.h>
00043 #include <pcl/point_types.h>
00044 #include <pcl/common/distances.h>
00045 
00046 using namespace std;
00047 using namespace pcl;
00048 
00049 PointCloud<PointXYZ> cloud, cloud_big;
00050 
00051 void
00052 init ()
00053 {
00054   float resolution = 0.1f;
00055   for (float z = -0.5f; z <= 0.5f; z += resolution)
00056     for (float y = -0.5f; y <= 0.5f; y += resolution)
00057       for (float x = -0.5f; x <= 0.5f; x += resolution)
00058         cloud.points.push_back (PointXYZ (x, y, z));
00059   cloud.width = static_cast<uint32_t> (cloud.points.size ());
00060   cloud.height = 1;
00061 
00062   cloud_big.width = 640;
00063   cloud_big.height = 480;
00064   srand (static_cast<unsigned int> (time (NULL)));
00065   // Randomly create a new point cloud
00066   for (size_t i = 0; i < cloud_big.width * cloud_big.height; ++i)
00067     cloud_big.points.push_back (PointXYZ (static_cast<float> (1024 * rand () / (RAND_MAX + 1.0)), 
00068                                           static_cast<float> (1024 * rand () / (RAND_MAX + 1.0)),
00069                                           static_cast<float> (1024 * rand () / (RAND_MAX + 1.0))));
00070 }
00071 
00072 /* Test for KdTree nearestKSearch */TEST (PCL, KdTree_nearestKSearch)
00073 {
00074   pcl::search::Search<PointXYZ>* kdtree = new pcl::search::KdTree<PointXYZ> ();
00075   kdtree->setInputCloud (cloud.makeShared ());
00076   PointXYZ test_point (0.01f, 0.01f, 0.01f);
00077   unsigned int no_of_neighbors = 20;
00078   multimap<float, int> sorted_brute_force_result;
00079   for (size_t i = 0; i < cloud.points.size (); ++i)
00080   {
00081     float distance = euclideanDistance (cloud.points[i], test_point);
00082     sorted_brute_force_result.insert (make_pair (distance, static_cast<int> (i)));
00083   }
00084   float max_dist = 0.0f;
00085   unsigned int counter = 0;
00086   for (multimap<float, int>::iterator it = sorted_brute_force_result.begin (); it != sorted_brute_force_result.end ()
00087       && counter < no_of_neighbors; ++it)
00088   {
00089     max_dist = max (max_dist, it->first);
00090     ++counter;
00091   }
00092 
00093   vector<int> k_indices;
00094   k_indices.resize (no_of_neighbors);
00095   vector<float> k_distances;
00096   k_distances.resize (no_of_neighbors);
00097 
00098   kdtree->nearestKSearch (test_point, no_of_neighbors, k_indices, k_distances);
00099 
00100   //if (k_indices.size () != no_of_neighbors)  cerr << "Found "<<k_indices.size ()<<" instead of "<<no_of_neighbors<<" neighbors.\n";
00101   EXPECT_EQ (k_indices.size (), no_of_neighbors);
00102 
00103   // Check if all found neighbors have distance smaller than max_dist
00104   for (size_t i = 0; i < k_indices.size (); ++i)
00105   {
00106     const PointXYZ& point = cloud.points[k_indices[i]];
00107     bool ok = euclideanDistance (test_point, point) <= max_dist;
00108     if (!ok)
00109     ok = (fabs (euclideanDistance (test_point, point)) - max_dist) <= 1e-6;
00110     //if (!ok)  cerr << k_indices[i] << " is not correct...\n";
00111     //else      cerr << k_indices[i] << " is correct...\n";
00112     EXPECT_EQ (ok, true);
00113   }
00114 
00115   ScopeTime scopeTime ("FLANN nearestKSearch");
00116   {
00117     pcl::search::Search<PointXYZ>* kdtree = new pcl::search::KdTree<PointXYZ>();
00118     //kdtree->initSearchDS ();
00119     kdtree->setInputCloud (cloud_big.makeShared ());
00120     for (size_t i = 0; i < cloud_big.points.size (); ++i)
00121     kdtree->nearestKSearch (cloud_big.points[i], no_of_neighbors, k_indices, k_distances);
00122   }
00123 }
00124 
00125 
00126 /* Test the templated NN search (for different query point types) */
00127 TEST (PCL, KdTree_differentPointT)
00128 {
00129   unsigned int no_of_neighbors = 20;
00130 
00131   pcl::search::Search<PointXYZ>* kdtree = new pcl::search::KdTree<PointXYZ> ();
00132   //kdtree->initSearchDS ();
00133   kdtree->setInputCloud (cloud_big.makeShared ());
00134 
00135   PointCloud<PointXYZRGB> cloud_rgb;
00136 
00137   copyPointCloud (cloud_big, cloud_rgb);
00138 
00139   std::vector< std::vector< float > > dists;
00140   std::vector< std::vector< int > > indices;
00141   kdtree->nearestKSearchT (cloud_rgb, std::vector<int> (),no_of_neighbors,indices,dists);
00142 
00143   vector<int> k_indices;
00144   k_indices.resize (no_of_neighbors);
00145   vector<float> k_distances;
00146   k_distances.resize (no_of_neighbors);
00147 
00148   vector<int> k_indices_t;
00149   k_indices_t.resize (no_of_neighbors);
00150   vector<float> k_distances_t;
00151   k_distances_t.resize (no_of_neighbors);
00152 
00153   for (size_t i = 0; i < cloud_rgb.points.size (); ++i)
00154   {
00155     kdtree->nearestKSearchT<pcl::PointXYZRGB> (cloud_rgb.points[i], no_of_neighbors, k_indices_t, k_distances_t);
00156     kdtree->nearestKSearch (cloud_big.points[i], no_of_neighbors, k_indices, k_distances);
00157     EXPECT_EQ (k_indices.size (), indices[i].size ());
00158     EXPECT_EQ (k_distances.size (), dists[i].size ());
00159     for (size_t j=0; j< no_of_neighbors; j++)
00160     {
00161       EXPECT_TRUE (k_indices[j] == indices[i][j] || k_distances[j] == dists[i][j]);
00162       EXPECT_TRUE (k_indices[j] == k_indices_t[j]);
00163       EXPECT_TRUE (k_distances[j] == k_distances_t[j]);
00164     }
00165   }
00166 }
00167 
00168 /* Test for KdTree nearestKSearch with multiple query points */
00169 TEST (PCL, KdTree_multipointKnnSearch)
00170 {
00171   unsigned int no_of_neighbors = 20;
00172 
00173   pcl::search::Search<PointXYZ>* kdtree = new pcl::search::KdTree<PointXYZ> ();
00174   //kdtree->initSearchDS ();
00175   kdtree->setInputCloud (cloud_big.makeShared ());
00176 
00177   std::vector< std::vector< float > > dists;
00178   std::vector< std::vector< int > > indices;
00179   kdtree->nearestKSearch (cloud_big, std::vector<int> (),no_of_neighbors,indices,dists);
00180 
00181   vector<int> k_indices;
00182   k_indices.resize (no_of_neighbors);
00183   vector<float> k_distances;
00184   k_distances.resize (no_of_neighbors);
00185 
00186   for (size_t i = 0; i < cloud_big.points.size (); ++i)
00187   {
00188     kdtree->nearestKSearch (cloud_big.points[i], no_of_neighbors, k_indices, k_distances);
00189     EXPECT_EQ (k_indices.size (), indices[i].size ());
00190     EXPECT_EQ (k_distances.size (), dists[i].size ());
00191     for (size_t j=0; j< no_of_neighbors; j++)
00192     {
00193       EXPECT_TRUE( k_indices[j]==indices[i][j] || k_distances[j] == dists[i][j]);
00194     }
00195   }
00196 }
00197 
00198 int
00199 main (int argc, char** argv)
00200 {
00201   testing::InitGoogleTest (&argc, argv);
00202   init ();
00203 
00204   // Testing using explicit instantiation of inherited class
00205   pcl::search::Search<PointXYZ>* kdtree = new pcl::search::KdTree<PointXYZ> ();
00206   kdtree->setInputCloud (cloud.makeShared ());
00207 
00208   return (RUN_ALL_TESTS ());
00209 }
00210 


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