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 }
