OctreePointCloudSearch

This class provides several methods for spatial neighbor search based on octree structure.

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

View Code