In this tutorial, we show how to divide a pointcloud into a number of supervoxel clusters using pcl::SupervoxelClustering, and then how to use and visualize the adjacency information and supervoxels themselves.
Segmentation algorithms aim to group pixels in images into perceptually meaningful regions which conform to object boundaries. Graph-based approaches, such as Markov Random Field (MRF) and Conditional Random Field (CRF), have become popular, as they merge relational low-level context within the image with object level class knowledge. The cost of solving pixel-level graphs led to the development of mid-level inference schemes which do not use pixels directly, but rather use groupings of pixels, known as superpixels, as the base level for nodes. Superpixels are formed by over-segmenting the image into small regions based on local low-level features, reducing the number of nodes which must be considered for inference.
Due to their strong impact on the quality of the eventual segmentation, it is important that superpixels have certain characteristics. Of these, avoiding violating object boundaries is the most vital, as failing to do so will decrease the accuracy of classifiers used later - since they will be forced to consider pixels which belong to more than one class. Additionally, even if the classifier does manage a correct output, the final pixel level segmentation will necessarily contain errors. Another useful quality is regular distribution over the area being segmented, as this will produce a simpler graph for later steps.
Voxel Cloud Connectivity Segmentation (VCCS) is a recent “superpixel” method which generates volumetric over-segmentations of 3D point cloud data, known as supervoxels. Supervoxels adhere to object boundaries better than state-of-the-art 2D methods, while remaining efficient enough to use in online applications. VCCS uses a region growing variant of k-means clustering for generating its labeling of points directly within a voxel octree structure. Supervoxels have two important properties; they are evenly distributed across the 3D space, and they cannot cross boundaries unless the underlying voxels are spatial connected. The former is accomplished by seeding supervoxels directly in the cloud, rather than the projected plane, while the latter uses an octree structure which maintains adjacency information of leaves. Supervoxels maintain adjacency relations in voxelized 3D space; specifically, 26-adjacency- that is neighboring voxels are those that share a face, edge, or vertex, as seen below.
The adjacency graph of supervoxels (and the underlying voxels) is maintained efficiently within the octree by specifying that neighbors are voxels within R_voxel of one another, where R_voxel specifies the octree leaf resolution. This adjacency graph is used extensively for both the region growing used to generate the supervoxels, as well as determining adjacency of the resulting supervoxels themselves.
VCCS is a region growing method which incrementally expand supervoxels from a set of seed points distributed evenly in space on a grid with resolution R_seed. To maintain efficiency, VCCS does not search globally, but rather only considers points within R_seed of the seed center. Additionally, seeds which are isolated are filtered out by establishing a small search radius R_search around each seed and removing seeds which do not have sufficient neighbor voxels connected to them.
The various sizing parameters which affect supervoxel clustering. R_seed and R_voxel must both be set by the user.
Expansion from the seed points is governed by a distance measure calculated in a feature space consisting of spatial extent, color, and normals. The spatial distance D_s is normalized by the seeding resolution, color distance D_c is the euclidean distance in normalized RGB space, and normal distance D_n measures the angle between surface normal vectors.
Weighting equation used in supervoxel clustering. w_c, w_s, and w_n, the color, spatial, and normal weights, respectively, are user controlled parameters.
Supervoxels are grown iteratively, using a local k-means clustering which considers connectivity and flow. The general process is as follows. Beginning at the voxel nearest the cluster center, we flow outward to adjacent voxels and compute the distance from each of these to the supervoxel center using the distance equation above. If the distance is the smallest this voxel has seen, its label is set, and using the adjacency graph, we add its neighbors which are further from the center to our search queue for this label. We then proceed to the next supervoxel, so that each level outwards from the center is considered at the same time for all supervoxels (a 2d version of this is seen in the figure below). We proceed iteratively outwards until we have reached the edge of the search volume for each supervoxel (or have no more neighbors to check).
Search order in the adjacency octree for supervoxel cluster expansion. Dotted edges in the adjacency graph are not searched, since they have already been considered earlier in the queue.
Alright, let’s get to the code... but if you want further details on how supervoxels work (and if you use them in an academic work) please reference the following publication:
@InProceedings{Papon13CVPR,
author={Jeremie Papon and Alexey Abramov and Markus Schoeler and Florentin W\"{o}rg\"{o}tter},
title={Voxel Cloud Connectivity Segmentation - Supervoxels for Point Clouds},
booktitle={Computer Vision and Pattern Recognition (CVPR), 2013 IEEE Conference on},
month = {June 22-27},
year = {2013},
address = {Portland, Oregon},
}
Oh, and for a more complicated example which uses Supervoxels, see pcl/examples/segmentation/supervoxel_clustering.cpp.
First, grab a pcd file made from a kinect or similar device - here we shall use milk_cartoon_all_small_clorox.pcd which is available in the pcl trunk here).
Next, copy and paste the following code into your editor and save it as supervoxel_clustering.cpp (or download the source file here).
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156 157 158 159 160 161 162 163 164 165 | #include <pcl/console/parse.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/segmentation/supervoxel_clustering.h>
// Types
typedef pcl::PointXYZRGBA PointT;
typedef pcl::PointCloud<PointT> PointCloudT;
typedef pcl::PointNormal PointNT;
typedef pcl::PointCloud<PointNT> PointNCloudT;
typedef pcl::PointXYZL PointLT;
typedef pcl::PointCloud<PointLT> PointLCloudT;
void addSupervoxelConnectionsToViewer (PointT &supervoxel_center,
PointCloudT &adjacent_supervoxel_centers,
std::string supervoxel_name,
boost::shared_ptr<pcl::visualization::PCLVisualizer> & viewer);
int
main (int argc, char ** argv)
{
if (argc < 2)
{
pcl::console::print_error ("Syntax is: %s <pcd-file> \n "
"--NT Dsables the single cloud transform \n"
"-v <voxel resolution>\n-s <seed resolution>\n"
"-c <color weight> \n-z <spatial weight> \n"
"-n <normal_weight>\n", argv[0]);
return (1);
}
PointCloudT::Ptr cloud = boost::make_shared <PointCloudT> ();
pcl::console::print_highlight ("Loading point cloud...\n");
if (pcl::io::loadPCDFile<PointT> (argv[1], *cloud))
{
pcl::console::print_error ("Error loading cloud file!\n");
return (1);
}
bool use_transform = ! pcl::console::find_switch (argc, argv, "--NT");
float voxel_resolution = 0.008f;
bool voxel_res_specified = pcl::console::find_switch (argc, argv, "-v");
if (voxel_res_specified)
pcl::console::parse (argc, argv, "-v", voxel_resolution);
float seed_resolution = 0.1f;
bool seed_res_specified = pcl::console::find_switch (argc, argv, "-s");
if (seed_res_specified)
pcl::console::parse (argc, argv, "-s", seed_resolution);
float color_importance = 0.2f;
if (pcl::console::find_switch (argc, argv, "-c"))
pcl::console::parse (argc, argv, "-c", color_importance);
float spatial_importance = 0.4f;
if (pcl::console::find_switch (argc, argv, "-z"))
pcl::console::parse (argc, argv, "-z", spatial_importance);
float normal_importance = 1.0f;
if (pcl::console::find_switch (argc, argv, "-n"))
pcl::console::parse (argc, argv, "-n", normal_importance);
////////////////////////////// //////////////////////////////
////// This is how to use supervoxels
////////////////////////////// //////////////////////////////
pcl::SupervoxelClustering<PointT> super (voxel_resolution, seed_resolution, use_transform);
super.setInputCloud (cloud);
super.setColorImportance (color_importance);
super.setSpatialImportance (spatial_importance);
super.setNormalImportance (normal_importance);
std::map <uint32_t, pcl::Supervoxel<PointT>::Ptr > supervoxel_clusters;
pcl::console::print_highlight ("Extracting supervoxels!\n");
super.extract (supervoxel_clusters);
pcl::console::print_info ("Found %d supervoxels\n", supervoxel_clusters.size ());
boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer (new pcl::visualization::PCLVisualizer ("3D Viewer"));
viewer->setBackgroundColor (0, 0, 0);
PointCloudT::Ptr voxel_centroid_cloud = super.getVoxelCentroidCloud ();
viewer->addPointCloud (voxel_centroid_cloud, "voxel centroids");
viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE,2.0, "voxel centroids");
viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_OPACITY,0.95, "voxel centroids");
PointCloudT::Ptr colored_voxel_cloud = super.getColoredVoxelCloud ();
viewer->addPointCloud (colored_voxel_cloud, "colored voxels");
viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_OPACITY,0.8, "colored voxels");
PointNCloudT::Ptr sv_normal_cloud = super.makeSupervoxelNormalCloud (supervoxel_clusters);
//We have this disabled so graph is easy to see, uncomment to see supervoxel normals
//viewer->addPointCloudNormals<PointNormal> (sv_normal_cloud,1,0.05f, "supervoxel_normals");
pcl::console::print_highlight ("Getting supervoxel adjacency\n");
std::multimap<uint32_t, uint32_t> supervoxel_adjacency;
super.getSupervoxelAdjacency (supervoxel_adjacency);
//To make a graph of the supervoxel adjacency, we need to iterate through the supervoxel adjacency multimap
std::multimap<uint32_t,uint32_t>::iterator label_itr = supervoxel_adjacency.begin ();
for ( ; label_itr != supervoxel_adjacency.end (); )
{
//First get the label
uint32_t supervoxel_label = label_itr->first;
//Now get the supervoxel corresponding to the label
pcl::Supervoxel<PointT>::Ptr supervoxel = supervoxel_clusters.at (supervoxel_label);
//Now we need to iterate through the adjacent supervoxels and make a point cloud of them
PointCloudT adjacent_supervoxel_centers;
std::multimap<uint32_t,uint32_t>::iterator adjacent_itr = supervoxel_adjacency.equal_range (supervoxel_label).first;
for ( ; adjacent_itr!=supervoxel_adjacency.equal_range (supervoxel_label).second; ++adjacent_itr)
{
pcl::Supervoxel<PointT>::Ptr neighbor_supervoxel = supervoxel_clusters.at (adjacent_itr->second);
adjacent_supervoxel_centers.push_back (neighbor_supervoxel->centroid_);
}
//Now we make a name for this polygon
std::stringstream ss;
ss << "supervoxel_" << supervoxel_label;
//This function is shown below, but is beyond the scope of this tutorial - basically it just generates a "star" polygon mesh from the points given
addSupervoxelConnectionsToViewer (supervoxel->centroid_, adjacent_supervoxel_centers, ss.str (), viewer);
//Move iterator forward to next label
label_itr = supervoxel_adjacency.upper_bound (supervoxel_label);
}
while (!viewer->wasStopped ())
{
viewer->spinOnce (100);
}
return (0);
}
void
addSupervoxelConnectionsToViewer (PointT &supervoxel_center,
PointCloudT &adjacent_supervoxel_centers,
std::string supervoxel_name,
boost::shared_ptr<pcl::visualization::PCLVisualizer> & viewer)
{
vtkSmartPointer<vtkPoints> points = vtkSmartPointer<vtkPoints>::New ();
vtkSmartPointer<vtkCellArray> cells = vtkSmartPointer<vtkCellArray>::New ();
vtkSmartPointer<vtkPolyLine> polyLine = vtkSmartPointer<vtkPolyLine>::New ();
//Iterate through all adjacent points, and add a center point to adjacent point pair
PointCloudT::iterator adjacent_itr = adjacent_supervoxel_centers.begin ();
for ( ; adjacent_itr != adjacent_supervoxel_centers.end (); ++adjacent_itr)
{
points->InsertNextPoint (supervoxel_center.data);
points->InsertNextPoint (adjacent_itr->data);
}
// Create a polydata to store everything in
vtkSmartPointer<vtkPolyData> polyData = vtkSmartPointer<vtkPolyData>::New ();
// Add the points to the dataset
polyData->SetPoints (points);
polyLine->GetPointIds ()->SetNumberOfIds(points->GetNumberOfPoints ());
for(unsigned int i = 0; i < points->GetNumberOfPoints (); i++)
polyLine->GetPointIds ()->SetId (i,i);
cells->InsertNextCell (polyLine);
// Add the lines to the dataset
polyData->SetLines (cells);
viewer->addModelFromPolyData (polyData,supervoxel_name);
}
|
We start by defining convenience types in order not to clutter the code.
typedef pcl::PointXYZRGBA PointT;
typedef pcl::PointCloud<PointT> PointCloudT;
typedef pcl::PointNormal PointNT;
typedef pcl::PointCloud<PointNT> PointNCloudT;
typedef pcl::PointXYZL PointLT;
typedef pcl::PointCloud<PointLT> PointLCloudT;
Then we load the input cloud based on the input argument
PointCloudT::Ptr cloud = boost::make_shared <PointCloudT> ();
pcl::console::print_highlight ("Loading point cloud...\n");
if (pcl::io::loadPCDFile<PointT> (argv[1], *cloud))
{
pcl::console::print_error ("Error loading cloud file!\n");
return (1);
}
Next we check the input arguments and set default values. You can play with the various parameters to see how they affect the supervoxels, but briefly:
bool use_transform = ! pcl::console::find_switch (argc, argv, "--NT");
float voxel_resolution = 0.008f;
bool voxel_res_specified = pcl::console::find_switch (argc, argv, "-v");
if (voxel_res_specified)
pcl::console::parse (argc, argv, "-v", voxel_resolution);
float seed_resolution = 0.1f;
bool seed_res_specified = pcl::console::find_switch (argc, argv, "-s");
if (seed_res_specified)
pcl::console::parse (argc, argv, "-s", seed_resolution);
float color_importance = 0.2f;
if (pcl::console::find_switch (argc, argv, "-c"))
pcl::console::parse (argc, argv, "-c", color_importance);
float spatial_importance = 0.4f;
if (pcl::console::find_switch (argc, argv, "-z"))
pcl::console::parse (argc, argv, "-z", spatial_importance);
float normal_importance = 1.0f;
if (pcl::console::find_switch (argc, argv, "-n"))
pcl::console::parse (argc, argv, "-n", normal_importance);
We are now ready to setup the supervoxel clustering. We use the class SupervoxelClustering, which implements the clustering process and give it the parameters.
pcl::SupervoxelClustering<PointT> super (voxel_resolution, seed_resolution, use_transform);
super.setInputCloud (cloud);
super.setColorImportance (color_importance);
super.setSpatialImportance (spatial_importance);
super.setNormalImportance (normal_importance);
Then we initialize the data structure which will be used to extract the supervoxels, and run the algorithm. The data structure is a map from labels to shared pointers of Supervoxel templated on the input point type. Supervoxels have the following fields:
std::map <uint32_t, pcl::Supervoxel<PointT>::Ptr > supervoxel_clusters;
pcl::console::print_highlight ("Extracting supervoxels!\n");
super.extract (supervoxel_clusters);
pcl::console::print_info ("Found %d supervoxels\n", supervoxel_clusters.size ());
We then load a viewer and use some of the getter functions of SupervoxelClustering to pull out clouds to display. voxel_centroid_cloud contains the voxel centroids coming out of the octree (basically the downsampled original cloud), and colored_voxel_cloud are the voxels colored according to their supervoxel labels (random colors). sv_normal_cloud contains a cloud of the supervoxel normals, but we don’t display it here so that the graph is visible.
boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer (new pcl::visualization::PCLVisualizer ("3D Viewer"));
viewer->setBackgroundColor (0, 0, 0);
PointCloudT::Ptr voxel_centroid_cloud = super.getVoxelCentroidCloud ();
viewer->addPointCloud (voxel_centroid_cloud, "voxel centroids");
viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE,2.0, "voxel centroids");
viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_OPACITY,0.95, "voxel centroids");
PointCloudT::Ptr colored_voxel_cloud = super.getColoredVoxelCloud ();
viewer->addPointCloud (colored_voxel_cloud, "colored voxels");
viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_OPACITY,0.8, "colored voxels");
PointNCloudT::Ptr sv_normal_cloud = super.makeSupervoxelNormalCloud (supervoxel_clusters);
//We have this disabled so graph is easy to see, uncomment to see supervoxel normals
//viewer->addPointCloudNormals<PointNormal> (sv_normal_cloud,1,0.05f, "supervoxel_normals");
Finally, we extract the supervoxel adjacency list (in the form of a multimap of label adjacencies).
pcl::console::print_highlight ("Getting supervoxel adjacency\n");
std::multimap<uint32_t, uint32_t> supervoxel_adjacency;
super.getSupervoxelAdjacency (supervoxel_adjacency);
Then we iterate through the multimap, creating a point cloud of the centroids of each supervoxel’s neighbors.
std::multimap<uint32_t,uint32_t>::iterator label_itr = supervoxel_adjacency.begin ();
for ( ; label_itr != supervoxel_adjacency.end (); )
{
//First get the label
uint32_t supervoxel_label = label_itr->first;
//Now get the supervoxel corresponding to the label
pcl::Supervoxel<PointT>::Ptr supervoxel = supervoxel_clusters.at (supervoxel_label);
//Now we need to iterate through the adjacent supervoxels and make a point cloud of them
PointCloudT adjacent_supervoxel_centers;
std::multimap<uint32_t,uint32_t>::iterator adjacent_itr = supervoxel_adjacency.equal_range (supervoxel_label).first;
for ( ; adjacent_itr!=supervoxel_adjacency.equal_range (supervoxel_label).second; ++adjacent_itr)
{
pcl::Supervoxel<PointT>::Ptr neighbor_supervoxel = supervoxel_clusters.at (adjacent_itr->second);
adjacent_supervoxel_centers.push_back (neighbor_supervoxel->centroid_);
}
Then we create a string label for the supervoxel graph we will draw and call addSupervoxelConnectionsToViewer, a drawing helper function implemented later in the tutorial code. The details of addSupervoxelConnectionsToViewer are beyond the scope of this tutorial, but all it does is draw a star polygon mesh of the supervoxel centroid to all of its neighbors centroids. We need to do this like this because adding individual lines using the addLine functionality of pcl_visualizer is too slow for large numbers of lines.
//Now we make a name for this polygon
std::stringstream ss;
ss << "supervoxel_" << supervoxel_label;
//This function is shown below, but is beyond the scope of this tutorial - basically it just generates a "star" polygon mesh from the points given
addSupervoxelConnectionsToViewer (supervoxel->centroid_, adjacent_supervoxel_centers, ss.str (), viewer);
//Move iterator forward to next label
label_itr = supervoxel_adjacency.upper_bound (supervoxel_label);
This results in a supervoxel graph that looks like this for seed size of 0.1m (top) and 0.05m (middle). The bottom is the original cloud, given for reference.:
Create a CMakeLists.txt file with the following content (or download it here):
1 2 3 4 5 6 7 8 9 10 11 12 | cmake_minimum_required(VERSION 2.8 FATAL_ERROR)
project(supervoxel_clustering)
find_package(PCL 1.7 REQUIRED)
include_directories(${PCL_INCLUDE_DIRS})
link_directories(${PCL_LIBRARY_DIRS})
add_definitions(${PCL_DEFINITIONS})
add_executable (supervoxel_clustering supervoxel_clustering.cpp)
target_link_libraries (supervoxel_clustering ${PCL_LIBRARIES})
|
After you have made the executable, you can run it like so, assuming the pcd file is in the same folder as the executable:
$ ./supervoxel_clustering milk_cartoon_all_small_clorox.pcd
Don’t be afraid to play around with the parameters (especially the seed size, -s) to see what happens. The pcd file name should always be the first parameter!