Often times, when processing our point clouds, we might need to perform operations that require accessing a local region of a point cloud or manipulating the neighborhood of specific points. Since point clouds store data in a one-dimensional data structure, these kinds of operations are inherently complex. In order to solve this issue, PCL provides two spatial data structures, named the kd-tree and the octree, which can provide an alternative and more structured representation of any point cloud.

As the name suggests, an octree is basically a tree structure in which each node has eight children, and which can be used to partition the 3D space. In contrast, the kd-tree is a binary tree in which nodes represent k-dimensional points. Both data structures are very interesting, but, in this particular example, we are going to learn how to use the octree to search and retrieve all the points surrounding a specific point. The example can be found in the source directory of the chapter10_tutorials package, and it's named pcl_partitioning.cpp:

#include <ros/ros.h> 
#include <pcl/point_cloud.h> 
#include <pcl_conversions/pcl_conversions.h> 
#include <sensor_msgs/PointCloud2.h> 
#include <pcl/octree/octree.h> 
 
class cloudHandler 
{ 
public: 
    cloudHandler() 
    { 
        pcl_sub = nh.subscribe("pcl_downsampled", 10, 
         &cloudHandler::cloudCB, this); pcl_pub = 
         nh.advertise<sensor_msgs::PointCloud2>("pcl_partitioned", 
         1); 
    } 
 
    void cloudCB(const sensor_msgs::PointCloud2 &input) 
    { 
        pcl::PointCloud<pcl::PointXYZ> cloud; 
        pcl::PointCloud<pcl::PointXYZ> cloud_partitioned; 
        sensor_msgs::PointCloud2 output; 
 
        pcl::fromROSMsg(input, cloud); 
 
        float resolution = 128.0f; 
        pcl::octree::OctreePointCloudSearch<pcl::PointXYZ> octree 
         (resolution); 
 
        octree.setInputCloud (cloud.makeShared()); 
        octree.addPointsFromInputCloud (); 
 
        pcl::PointXYZ center_point; 
        center_point.x = 0 ; 
        center_point.y = 0.4; 
        center_point.z = -1.4; 
 
        float radius = 0.5; 
        std::vector<int> radiusIdx; 
        std::vector<float> radiusSQDist; 
        if (octree.radiusSearch (center_point, radius, radiusIdx, 
         radiusSQDist) > 0) 
        { 
            for (size_t i = 0; i < radiusIdx.size (); ++i) 
            { 
                cloud_partitioned.points.push_back 
                 (cloud.points[radiusIdx[i]]); 
            } 
        } 
 
        pcl::toROSMsg(cloud_partitioned, output); 
        output.header.frame_id = "odom"; 
        pcl_pub.publish(output); 
    } 
 
protected: 
    ros::NodeHandle nh; 
    ros::Subscriber pcl_sub; 
    ros::Publisher pcl_pub; 
}; 
 
main(int argc, char **argv) 
{ 
    ros::init(argc, argv, "pcl_partitioning"); 
 
    cloudHandler handler; 
 
    ros::spin(); 
 
    return 0; 
} 

As usual, this example uses the pcl_downsampled topic as an input source of point clouds and publishes the resulting partitioned point cloud to the pcl_partitioned topic. The handler function starts by converting the input point cloud to a PCL point cloud. The next step is to create an octree-searching algorithm, which requires passing a resolution value that will determine the size of the voxels at the lowest level of the tree and, consequently, other properties such as the tree's depth. The algorithm also requires to be given the point cloud to explicitly load the points:

float resolution = 128.0f; 
pcl::octree::OctreePointCloudSearch<pcl::PointXYZ> 
     octree(resolution);

octree.setInputCloud (cloud.makeShared()); 
octree.addPointsFromInputCloud (); 

The next step is to define the center point of the partition; in this case, it has been handpicked to be close to the top of the point cloud:

pcl::PointXYZ center_point; 
center_point.x = 0; 
center_point.y = 0.4; 
center_point.z = -1.4; 

We can now perform a search in a radius around that specific point using the radiusSearch function from the octree search algorithm. This particular function is used to output arguments that return the indices of the points that fall in that radius and the squared distance from those points to the center point provided. With those indices, we can then create a new point cloud containing only the points belonging to the partition:

float radius = 0.5; 
std::vector<int> radiusIdx; 
std::vector<float> radiusSQDist; 
if (octree.radiusSearch (center_point, radius, radiusIdx, 
 radiusSQDist) > 0) 
{ 
  for (size_t i = 0; i < radiusIdx.size (); ++i) 
  { 
    cloud_partitioned.points.push_back 
     (cloud.points[radiusIdx[i]]); 
  } 
} 

Finally, the point cloud is converted to the PointCloud2 message type and published in the output topic:

pcl::toROSMsg( cloud_partitioned, output ); 
output.header.frame_id = "odom"; 
pcl_pub.publish( output ); 

In order to run this example, we need to run the usual chain of nodes, starting
with roscore:

    $ roscore  

In the second terminal, we can run the pcl_read example and a source of data:

    $ roscd chapter10_tutorials/data
    $ rosrun chapter10_tutorials pcl_read  

In the third terminal, we will run the filtering example:

    $ rosrun chapter10_tutorials pcl_filter  

In the fourth terminal, we will run the downsampling example:

    $ rosrun chapter10_tutorials pcl_downsampling  

Finally, we will run this example:

    $ rosrun chapter10_tutorials pcl_partitioning  

In the following image, we can see the end result of the partitioning process. Since we handpicked the point to be close to the top of the point cloud, we managed to extract part of the cup and the table. This example only shows a tiny fraction of the potential of the octree data structure, but it's a good starting point to further your understanding: