KdTree

 1 #include <pcl/point_cloud.h>
 2 #include <pcl/kdtree/kdtree_flann.h>
 3 
 4 #include <iostream>
 5 #include <vector>
 6 #include <ctime>
 7 
 8 int
 9 main (int argc, char** argv)
10 {
11   srand (time (NULL));
12 
13   pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
14 
15   // Generate pointcloud data
16   cloud->width = 1000;
17   cloud->height = 1;
18   cloud->points.resize (cloud->width * cloud->height);
19 
20   for (size_t i = 0; i < cloud->points.size (); ++i)
21   {
22     cloud->points[i].x = 1024.0f * rand () / (RAND_MAX + 1.0f);
23     cloud->points[i].y = 1024.0f * rand () / (RAND_MAX + 1.0f);
24     cloud->points[i].z = 1024.0f * rand () / (RAND_MAX + 1.0f);
25   }
26 
27   pcl::KdTreeFLANN<pcl::PointXYZ> kdtree;
28 
29   kdtree.setInputCloud (cloud);
30 
31   pcl::PointXYZ searchPoint;
32 
33   searchPoint.x = 1024.0f * rand () / (RAND_MAX + 1.0f);
34   searchPoint.y = 1024.0f * rand () / (RAND_MAX + 1.0f);
35   searchPoint.z = 1024.0f * rand () / (RAND_MAX + 1.0f);
36 
37   // K nearest neighbor search
38 
39   int K = 10;
40 
41   std::vector<int> pointIdxNKNSearch(K);
42   std::vector<float> pointNKNSquaredDistance(K);
43 
44   std::cout << "K nearest neighbor search at (" << searchPoint.x 
45             << " " << searchPoint.y 
46             << " " << searchPoint.z
47             << ") with K=" << K << std::endl;
48 
49   if ( kdtree.nearestKSearch (searchPoint, K, pointIdxNKNSearch, pointNKNSquaredDistance) > 0 )
50   {
51     for (size_t i = 0; i < pointIdxNKNSearch.size (); ++i)
52       std::cout << "    "  <<   cloud->points[ pointIdxNKNSearch[i] ].x 
53                 << " " << cloud->points[ pointIdxNKNSearch[i] ].y 
54                 << " " << cloud->points[ pointIdxNKNSearch[i] ].z 
55                 << " (squared distance: " << pointNKNSquaredDistance[i] << ")" << std::endl;
56   }
57 
58   // Neighbors within radius search
59 
60   std::vector<int> pointIdxRadiusSearch;
61   std::vector<float> pointRadiusSquaredDistance;
62 
63   float radius = 256.0f * rand () / (RAND_MAX + 1.0f);
64 
65   std::cout << "Neighbors within radius search at (" << searchPoint.x 
66             << " " << searchPoint.y 
67             << " " << searchPoint.z
68             << ") with radius=" << radius << std::endl;
69 
70 
71   if ( kdtree.radiusSearch (searchPoint, radius, pointIdxRadiusSearch, pointRadiusSquaredDistance) > 0 )
72   {
73     for (size_t i = 0; i < pointIdxRadiusSearch.size (); ++i)
74       std::cout << "    "  <<   cloud->points[ pointIdxRadiusSearch[i] ].x 
75                 << " " << cloud->points[ pointIdxRadiusSearch[i] ].y 
76                 << " " << cloud->points[ pointIdxRadiusSearch[i] ].z 
77                 << " (squared distance: " << pointRadiusSquaredDistance[i] << ")" << std::endl;
78   }
79 
80 
81   return 0;
82 }