How to extract a set of points in a point cloud data using PCL?

Question

I have a point cloud data, where by clicking a point, I want to extract points surrounding the clicked point within a radius. I want to also push the extracted points into a new cloud. Using Pointpickingevent, I am able to click one point and push it into the cloud. How do I extract a set of points, say points surrounding 0.02cm radius from the clicked point and push them into a new cloud?

Answer 1

Given a point cloud:

pcl::PointCloud<pcl::PointXYZ>::Ptr cloud

A Kdtree is then generated to perform an efficient range search:

pcl::KdTreeFLANN<pcl::PointXYZ> kdtree;
kdtree.setInputCloud (cloud);

Then, given a point and a radius:

pcl::PointXYZ searchPoint(1,2,3);
float radius = 4;

You can get all the points that are at a distance radius from the point searchPoint:

std::vector<int> pointIdxRadiusSearch; //to store index of surrounding points 
std::vector<float> pointRadiusSquaredDistance; // to store distance to surrounding points

if ( kdtree.radiusSearch (searchPoint, radius, pointIdxRadiusSearch, pointRadiusSquaredDistance) > 0 )
{
    for (size_t i = 0; i < pointIdxRadiusSearch.size (); ++i)
        std::cout << "    "  <<   cloud->points[ pointIdxRadiusSearch[i] ].x 
                << " " << cloud->points[ pointIdxRadiusSearch[i] ].y 
                << " " << cloud->points[ pointIdxRadiusSearch[i] ].z 
                << " (squared distance: " << pointRadiusSquaredDistance[i] << ")" << std::endl;
}

You can print all the surrounding points and their distance to the searchPoint to check the code functional correctness.

Finally, create a cloud with the obtained points:

pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_cluster (new pcl::PointCloud<pcl::PointXYZ>);
for (size_t i = 0; i < pointIdxRadiusSearch.size (); ++i)
    cloud_cluster->points.push_back(cloud->points[ pointIdxRadiusSearch[i] ]);
cloud_cluster->width = cloud_cluster->points.size ();
cloud_cluster->height = 1;
cloud_cluster->is_dense = true;