PCL/OpenNI tutorial 3: Cloud processing (advanced)

From robotica.unileon.es
Jump to: navigation, search

Go to root: PhD-3D-Object-Tracking


Most of the techniques seen in the previous tutorial focused on preprocessing, that is, performing certain operations on the cloud to get it ready for further analysis or work. Downsampling, removing outliers, surface smoothing, estimating the normals...

This new tutorial will teach you many interesting things that can be done with point clouds after the preparation step, such as registration, segmentation and model matching, and other advanced operations.

Registration

Registration is the technique of aligning two point clouds, like pieces of a puzzle. To be precise, the algorithm finds a set of correspondences between them, which would mean that there is an area of the scene that has been captured in both clouds. A linear transformation is then computed, which outputs a matrix that contains a rotation and a translation. These are the operations that you would apply to one of the clouds so it would get in place with respect to the other, with the intersecting areas overlapping.

The best results are achieved with clouds that are very similar, so you should try to minimize the transformations between them. Meaning, do not run like crazy with a Kinect in your hands, grabbing frames, and expect PCL to match the clouds. It is better to move the sensor gingerly and at steady intervals. If you use a robotic arm or a rotating tabletop with precise angles, even better.

Registration is a very useful technique because it lets you retrieve full, complete and continous models of a scene or object. It is also expensive for the CPU. Optimizations have been developed that allow to do cloud matching in real time with the GPU, like KinFu does.


Flowchart diagram showing an iteration of the registration algorithm (image from http://pointclouds.org/).


The previous diagram illustrates how the procedure is done. There are mainly two different methods to choose from:

  • ICP registration: ICP stands for Iterative Closest Point. It is an algorithm that will find the best transformation that minimizes the distance from the source point cloud to the target one. The problem is that it will do it by associating every point of the source cloud to its "twin" in the target cloud in a linear way, so it can be considered a brute force method. It the clouds are too big, the algorithm will take its time to finish, so try downsampling them first.
  • Feature-based registration: the algorithm finds a set of keypoints in each cloud, computes a local descriptor for each (we explain what local descriptors are in the next tutorial; they are like individual signatures of points), and then performs a search to see if the clouds have keypoints in common. If at least 3 correspondences are found, a transformation can be computed. For accurate results, several correspondences must be found. This method is (or should be) faster than the first, because matching is only done for the keypoints, not the whole cloud.

ICP will probably fail if the difference between the clouds is too big. Usually, you will use features first to perform an initial rough alignment of the clouds, and then use ICP to refine it with precision. Feature-based registration is basically identical to the 3D object recognition problem, so we will not look into it here. Features, descriptors and recognition will be dealt with in the next tutorials.


Iterative Closest Point (ICP)

PCL offers several implementations of the ICP algorithm. We will see the general, unoptimized one, but you may want to try one of the optimized variants listed below in the API section. The code for aligning (registering) two point clouds is the following: 

#include <pcl/io/pcd_io.h>
#include <pcl/registration/icp.h>

#include <iostream>

int
main(int argc, char** argv)
{
	// Objects for storing the point clouds.
	pcl::PointCloud<pcl::PointXYZ>::Ptr sourceCloud(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::PointCloud<pcl::PointXYZ>::Ptr targetCloud(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::PointCloud<pcl::PointXYZ>::Ptr finalCloud(new pcl::PointCloud<pcl::PointXYZ>);

	// Read two PCD files from disk.
	if (pcl::io::loadPCDFile<pcl::PointXYZ>(argv[1], *sourceCloud) != 0)
	{
		return -1;
	}
	if (pcl::io::loadPCDFile<pcl::PointXYZ>(argv[2], *targetCloud) != 0)
	{
		return -1;
	}

	// ICP object.
	pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> registration;
	registration.setInputSource(sourceCloud);
	registration.setInputTarget(targetCloud);

	registration.align(*finalCloud);
	if (registration.hasConverged())
	{
		std::cout << "ICP converged." << std::endl
				  << "The score is " << registration.getFitnessScore() << std::endl;
		std::cout << "Transformation matrix:" << std::endl;
		std::cout << registration.getFinalTransformation() << std::endl;
	}
	else std::cout << "ICP did not converge." << std::endl;
}

The cloud saved in "finalCloud" is the same as "sourceCloud" (it has the same points), but the transformation found by ICP has been applied, so when visualized next to "targetCloud" they should match properly (errors are inevitable and depend of many factors, like the quality of the sensor, the distance between the clouds, the algorithm used...).


  • Source point cloud.

  • Target point cloud.

  • Both clouds shown together before registration.

  • Both clouds shown together after registration.



Model fitting (RANSAC)

Given a set of data, it is possible to determine if a part of it fits a certain mathematical model, using an iterative method known as RANSAC (Random Sample Consensus). RANSAC works under the assumption that the data contains inliers (data can can be adjusted to the model, even with a little noise) and outliers (data that does not fit the model at all). The algoritm is non-deterministic: every iteration, the accuracy of the result improves. To be precise, the probability of the result being the correct one increases, though it will never be of 100%.


  • Set of data points containing outliers and inliers fitting a line model (image from Wikimedia Commons).

  • The same set, after applying RANSAC to find the parameters of the line model, discarding outliers (image from Wikimedia Commons).


PCL offers implementations of RANSAC to work with point clouds. For example, you can find which points in the cloud fit the model of a sphere, and the procedure will return the parameters of said model. This is how you would do it: 

#include <pcl/io/pcd_io.h>
#include <pcl/sample_consensus/ransac.h>
#include <pcl/sample_consensus/sac_model_sphere.h>

int
main(int argc, char** argv)
{
	// Objects for storing the point clouds.
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::PointCloud<pcl::PointXYZ>::Ptr inlierPoints(new pcl::PointCloud<pcl::PointXYZ>);

	// Read a PCD file from disk.
	if (pcl::io::loadPCDFile<pcl::PointXYZ>(argv[1], *cloud) != 0)
	{
		return -1;
	}

	// RANSAC objects: model and algorithm.
	pcl::SampleConsensusModelSphere<pcl::PointXYZ>::Ptr model(new pcl::SampleConsensusModelSphere<pcl::PointXYZ>(cloud));
	pcl::RandomSampleConsensus<pcl::PointXYZ> ransac(model);
	// Set the maximum allowed distance to the model.
	ransac.setDistanceThreshold(0.01);
	ransac.computeModel();

	std::vector<int> inlierIndices;
	ransac.getInliers(inlierIndices);

	// Copy all inliers of the model to another cloud.
	pcl::copyPointCloud<pcl::PointXYZ>(*cloud, inlierIndices, *inlierPoints);
}

There are many more models to choose from. The most important ones are:

Also, the RANSAC class offers some functions to configure its behavior. For example, with "setMaxIterations()" you can specify the maximum number of iterations the algoritm will run for, instead of using a distance threshold (if you do not set a stop condition, RANSAC will just run forever, getting a slightly better result each time).


Projecting points

After you have recovered the coefficients of a model, it is possible to project the points of a cloud onto said model. For example, if the model is a plane, after projection the points of the cloud would all lie on the plane. 

#include <pcl/io/pcd_io.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/filters/project_inliers.h>
#include <pcl/visualization/cloud_viewer.h>

#include <iostream>

int
main(int argc, char** argv)
{
	// Objects for storing the point clouds.
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloudNoPlane(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::PointCloud<pcl::PointXYZ>::Ptr planePoints(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::PointCloud<pcl::PointXYZ>::Ptr projectedPoints(new pcl::PointCloud<pcl::PointXYZ>);

	// Read a PCD file from disk.
	if (pcl::io::loadPCDFile<pcl::PointXYZ>(argv[1], *cloud) != 0)
	{
		return -1;
	}

	// Get the plane model, if present.
	pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
	pcl::SACSegmentation<pcl::PointXYZ> segmentation;
	segmentation.setInputCloud(cloud);
	segmentation.setModelType(pcl::SACMODEL_PLANE);
	segmentation.setMethodType(pcl::SAC_RANSAC);
	segmentation.setDistanceThreshold(0.01);
	segmentation.setOptimizeCoefficients(true);
	pcl::PointIndices::Ptr inlierIndices(new pcl::PointIndices);
	segmentation.segment(*inlierIndices, *coefficients);

	if (inlierIndices->indices.size() == 0)
		std::cout << "Could not find a plane in the scene." << std::endl;
	else
	{
		std::cerr << "Plane coefficients: " << coefficients->values[0] << " "
				  << coefficients->values[1] << " "
				  << coefficients->values[2] << " "
				  << coefficients->values[3] << std::endl;

		// Create a second point cloud that does not have the plane points.
		// Also, extract the plane points to visualize them later.
		pcl::ExtractIndices<pcl::PointXYZ> extract;
		extract.setInputCloud(cloud);
		extract.setIndices(inlierIndices);
		extract.filter(*planePoints);
		extract.setNegative(true);
		extract.filter(*cloudNoPlane);

		// Object for projecting points onto a model.
		pcl::ProjectInliers<pcl::PointXYZ> projection;
		// We have to specify what model we used.
		projection.setModelType(pcl::SACMODEL_PLANE);
		projection.setInputCloud(cloudNoPlane);
		// And we have to give the coefficients we got.
		projection.setModelCoefficients(coefficients);
		projection.filter(*projectedPoints);

		// Visualize everything.
		pcl::visualization::CloudViewer viewerPlane("Plane");
		viewerPlane.showCloud(planePoints);
		while (!viewerPlane.wasStopped())
		{
			// Do nothing but wait.
		}
		pcl::visualization::CloudViewer viewerProjection("Projected points");
		viewerProjection.showCloud(projectedPoints);
		while (!viewerProjection.wasStopped())
		{
			// Do nothing but wait.
		}
	}
}

After running the example, you will see that all points have been "flattened" onto the plane (if there was one in the scene, that is). The program will first show you the points that have been fitted to a plane. When you quit with Alt+F4 or Q, a second window will open displaying the results of the projection.


Segmentation

Segmentation consists of breaking the cloud apart in different pieces or sections: groups of points called clusters (which is why it is also referred to as clustering). The idea is to divide it in several parts to be processed independently. Ideally, every cluster would belong to the logical notion of "object". For example, for a cloud that shows 3 boxes on a table, 4 clusters would be created: one for the table, and one for each of the boxes.

There are many segmentation techniques, each one with its own criteria for grouping points together. Some consider the distance between points, some take into account the normals, or even the texture, too. Here I will give you an overview of each one.

Segmentation, together with all other methods seen so far, will allow you to retrieve the models of individual objects from a scene. Also, it is possible to segment certain shapes like planes or cylinders. If you need example point clouds to practice your segmentation skills with, take a look at the OSD (Object Segmentation Database).


Euclidean

Euclidean segmentation is the simplest of all. It checks the distance between two points. If it is less than a threshold, both are considered to belong in the same cluster. It works like a flood fill algorithm: a point in the cloud is "marked" as "chosen" for the cluster. Then, it spreads like a virus to all other points that are near enough, and from those to even more points, until none new can be added. Then, a new cluster is initialized, and the procedure starts again with the remaining unmarked points.

In PCL, Euclidean segmentation is done as follows: 

#include <pcl/io/pcd_io.h>
#include <pcl/segmentation/extract_clusters.h>

#include <iostream>

int
main(int argc, char** argv)
{
	// Object for storing the point cloud.
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);

	// Read a PCD file from disk.
	if (pcl::io::loadPCDFile<pcl::PointXYZ>(argv[1], *cloud) != 0)
	{
		return -1;
	}

	// kd-tree object for searches.
	pcl::search::KdTree<pcl::PointXYZ>::Ptr kdtree(new pcl::search::KdTree<pcl::PointXYZ>);
	kdtree->setInputCloud(cloud);

	// Euclidean clustering object.
	pcl::EuclideanClusterExtraction<pcl::PointXYZ> clustering;
	// Set cluster tolerance to 2cm (small values may cause objects to be divided
	// in several clusters, whereas big values may join objects in a same cluster).
	clustering.setClusterTolerance(0.02);
	// Set the minimum and maximum number of points that a cluster can have.
	clustering.setMinClusterSize(100);
	clustering.setMaxClusterSize(25000);
	clustering.setSearchMethod(kdtree);
	clustering.setInputCloud(cloud);
	std::vector<pcl::PointIndices> clusters;
	clustering.extract(clusters);

	// For every cluster...
	int currentClusterNum = 1;
	for (std::vector<pcl::PointIndices>::const_iterator i = clusters.begin(); i != clusters.end(); ++i)
	{
		// ...add all its points to a new cloud...
		pcl::PointCloud<pcl::PointXYZ>::Ptr cluster(new pcl::PointCloud<pcl::PointXYZ>);
		for (std::vector<int>::const_iterator point = i->indices.begin(); point != i->indices.end(); point++)
			cluster->points.push_back(cloud->points[*point]);
		cluster->width = cluster->points.size();
		cluster->height = 1;
		cluster->is_dense = true;

		// ...and save it to disk.
		if (cluster->points.size() <= 0)
			break;
		std::cout << "Cluster " << currentClusterNum << " has " << cluster->points.size() << " points." << std::endl;
		std::string fileName = "cluster" + boost::to_string(currentClusterNum) + ".pcd";
		pcl::io::savePCDFileASCII(fileName, *cluster);

		currentClusterNum++;
	}
}

You may have to play with the cluster tolerance parameter until you get good results. Also, it is recommended to perform planar segmentation (we will see it later) to remove the floor and/or table(s) from the scene, or the results may be wrong.

After the program finishes, you can visualize all clusters at the same time with: 

pcl_viewer cluster*


  • Original cloud.

  • Clusters recovered after Euclidean segmentation (each cluster is renderer with a different color).


All segmentation objects provide a "setIndices()" function. You can use it to provide the indices of the points you want to apply clustering to, instead of the whole cloud.


Conditional

Conditional Euclidean segmentation works the same way as the standard one seen above, with one exception. Apart from the distance check, points need also to meet a special, custom requirement for them to be added to a cluster. This condition is user-specified. It boils down to this: for every pair of points (the first one, the seed, is being processed in this moment, the second one, candidate, is a neighbor of the former that is being tested) a custom function will be called. This function has a certain signature: it receives

  1. a copy of the two points so we can perform our own tests, and
  2. the squared distance

and returns a boolean value. If the value is true, the candidate may be added to the cluster. If false, it will not, even if it passed the distance check. 

#include <pcl/io/pcd_io.h>
#include <pcl/segmentation/conditional_euclidean_clustering.h>

#include <iostream>

// If this function returns true, the candidate point will be added
// to the cluster of the seed point.
bool
customCondition(const pcl::PointXYZ& seedPoint, const pcl::PointXYZ& candidatePoint, float squaredDistance)
{
	// Do whatever you want here.
	if (candidatePoint.y < seedPoint.y)
		return false;

	return true;
}

int
main(int argc, char** argv)
{
	// Object for storing the point cloud.
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);

	// Read a PCD file from disk.
	if (pcl::io::loadPCDFile<pcl::PointXYZ>(argv[1], *cloud) != 0)
	{
		return -1;
	}

	// Conditional Euclidean clustering object.
	pcl::ConditionalEuclideanClustering<pcl::PointXYZ> clustering;
	clustering.setClusterTolerance(0.02);
	clustering.setMinClusterSize(100);
	clustering.setMaxClusterSize(25000);
	clustering.setInputCloud(cloud);
	// Set the function that will be called for every pair of points to check.
	clustering.setConditionFunction(&customCondition);
	std::vector<pcl::PointIndices> clusters;
	clustering.segment(clusters);

	// For every cluster...
	int currentClusterNum = 1;
	for (std::vector<pcl::PointIndices>::const_iterator i = clusters.begin(); i != clusters.end(); ++i)
	{
		// ...add all its points to a new cloud...
		pcl::PointCloud<pcl::PointXYZ>::Ptr cluster(new pcl::PointCloud<pcl::PointXYZ>);
		for (std::vector<int>::const_iterator point = i->indices.begin(); point != i->indices.end(); point++)
			cluster->points.push_back(cloud->points[*point]);
		cluster->width = cluster->points.size();
		cluster->height = 1;
		cluster->is_dense = true;

		// ...and save it to disk.
		if (cluster->points.size() <= 0)
			break;
		std::cout << "Cluster " << currentClusterNum << " has " << cluster->points.size() << " points." << std::endl;
		std::string fileName = "cluster" + boost::to_string(currentClusterNum) + ".pcd";
		pcl::io::savePCDFileASCII(fileName, *cluster);

		currentClusterNum++;
	}
}

The condition that is implemented above (checking if the candidate's Y coordinate is lower than the seed's) does not make a lot of sense, but I just wanted you to understand how the method works.

If you pass "true" to the constructor of "pcl::ConditionalEuclideanClustering", the object will store the clusters that were discarded for being too small or too big. You can later retrieve them with the getRemovedClusters() function.


Region growing

This type of segmentation (which is also a greedy-like, flood fill approach like the Euclidean one) groups together points that check a smoothness constraint. The angle between their normals and the difference of curvatures are checked to see if they could belong to the same smooth surface. Think about a box laying on a table: with Euclidean segmentation, both would be considered to be in the same cluster because they are "touching". With region growing segmentation, this would not happen, because there is a 90° (with ideal normal estimation, that is) difference between the normals of a point in the table and another one in the box's lateral. 

#include <pcl/io/pcd_io.h>
#include <pcl/search/kdtree.h>
#include <pcl/features/normal_3d.h>
#include <pcl/segmentation/region_growing.h>

#include <iostream>

int
main(int argc, char** argv)
{
	// Object for storing the point cloud.
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
	// Object for storing the normals.
	pcl::PointCloud<pcl::Normal>::Ptr normals(new pcl::PointCloud<pcl::Normal>);

	// Read a PCD file from disk.
	if (pcl::io::loadPCDFile<pcl::PointXYZ>(argv[1], *cloud) != 0)
	{
		return -1;
	}

	// kd-tree object for searches.
	pcl::search::KdTree<pcl::PointXYZ>::Ptr kdtree(new pcl::search::KdTree<pcl::PointXYZ>);
	kdtree->setInputCloud(cloud);

	// Estimate the normals.
	pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> normalEstimation;
	normalEstimation.setInputCloud(cloud);
	normalEstimation.setRadiusSearch(0.03);
	normalEstimation.setSearchMethod(kdtree);
	normalEstimation.compute(*normals);

	// Region growing clustering object.
	pcl::RegionGrowing<pcl::PointXYZ, pcl::Normal> clustering;
	clustering.setMinClusterSize(100);
	clustering.setMaxClusterSize(10000);
	clustering.setSearchMethod(kdtree);
	clustering.setNumberOfNeighbours(30);
	clustering.setInputCloud(cloud);
	clustering.setInputNormals(normals);
	// Set the angle in radians that will be the smoothness threshold
	// (the maximum allowable deviation of the normals).
	clustering.setSmoothnessThreshold(7.0 / 180.0 * M_PI); // 7 degrees.
	// Set the curvature threshold. The disparity between curvatures will be
	// tested after the normal deviation check has passed.
	clustering.setCurvatureThreshold(1.0);

	std::vector <pcl::PointIndices> clusters;
	clustering.extract(clusters);

	// For every cluster...
	int currentClusterNum = 1;
	for (std::vector<pcl::PointIndices>::const_iterator i = clusters.begin(); i != clusters.end(); ++i)
	{
		// ...add all its points to a new cloud...
		pcl::PointCloud<pcl::PointXYZ>::Ptr cluster(new pcl::PointCloud<pcl::PointXYZ>);
		for (std::vector<int>::const_iterator point = i->indices.begin(); point != i->indices.end(); point++)
			cluster->points.push_back(cloud->points[*point]);
		cluster->width = cluster->points.size();
		cluster->height = 1;
		cluster->is_dense = true;

		// ...and save it to disk.
		if (cluster->points.size() <= 0)
			break;
		std::cout << "Cluster " << currentClusterNum << " has " << cluster->points.size() << " points." << std::endl;
		std::string fileName = "cluster" + boost::to_string(currentClusterNum) + ".pcd";
		pcl::io::savePCDFileASCII(fileName, *cluster);

		currentClusterNum++;
	}
}
  • Input: Points, Minimum cluster size, Maximum cluster size, Neighbor count, Search method, Normals, Smoothness threshold, Curvature threshold, [Indices]
  • Output: Vector of Point indices (clusters)
  • Tutorial: Region growing segmentation
  • API: pcl::RegionGrowing
  • Download


Color-based

This method is based on the previous one but, instead of comparing the normals and the curvature of the points, it compares the RGB color. Like always, if the difference is less than a threshold, both are put in the same cluster. Obviously, for this to work you need a cloud with RGB information (with the type "PointXYZRGB"). You can use the OpenNI viewer program to retrieve them with the Kinect or Xtion.

Apart from the color check, this algorithm also uses two postprocessing steps. In the first one, neighboring clusters with similar average color are merged (this is controlled with a threshold, too). In the second one, clusters that are smaller than the minimum size are merged with their neighbors. 

#include <pcl/io/pcd_io.h>
#include <pcl/search/kdtree.h>
#include <pcl/segmentation/region_growing_rgb.h>

#include <iostream>

int
main(int argc, char** argv)
{
	// Object for storing the point cloud, with color information.
	pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);

	// Read a PCD file from disk.
	if (pcl::io::loadPCDFile<pcl::PointXYZRGB>(argv[1], *cloud) != 0)
	{
		return -1;
	}

	// kd-tree object for searches.
	pcl::search::KdTree<pcl::PointXYZRGB>::Ptr kdtree(new pcl::search::KdTree<pcl::PointXYZRGB>);
	kdtree->setInputCloud(cloud);

	// Color-based region growing clustering object.
	pcl::RegionGrowingRGB<pcl::PointXYZRGB> clustering;
	clustering.setInputCloud(cloud);
	clustering.setSearchMethod(kdtree);
	// Here, the minimum cluster size affects also the postprocessing step:
	// clusters smaller than this will be merged with their neighbors.
	clustering.setMinClusterSize(100);
	// Set the distance threshold, to know which points will be considered neighbors.
	clustering.setDistanceThreshold(10);
	// Color threshold for comparing the RGB color of two points.
	clustering.setPointColorThreshold(6);
	// Region color threshold for the postprocessing step: clusters with colors
	// within the threshold will be merged in one.
	clustering.setRegionColorThreshold(5);

	std::vector <pcl::PointIndices> clusters;
	clustering.extract(clusters);

	// For every cluster...
	int currentClusterNum = 1;
	for (std::vector<pcl::PointIndices>::const_iterator i = clusters.begin(); i != clusters.end(); ++i)
	{
		// ...add all its points to a new cloud...
		pcl::PointCloud<pcl::PointXYZRGB>::Ptr cluster(new pcl::PointCloud<pcl::PointXYZRGB>);
		for (std::vector<int>::const_iterator point = i->indices.begin(); point != i->indices.end(); point++)
			cluster->points.push_back(cloud->points[*point]);
		cluster->width = cluster->points.size();
		cluster->height = 1;
		cluster->is_dense = true;

		// ...and save it to disk.
		if (cluster->points.size() <= 0)
			break;
		std::cout << "Cluster " << currentClusterNum << " has " << cluster->points.size() << " points." << std::endl;
		std::string fileName = "cluster" + boost::to_string(currentClusterNum) + ".pcd";
		pcl::io::savePCDFileASCII(fileName, *cluster);

		currentClusterNum++;
	}
}


Min-Cut

The Min-Cut (minimum cut) algorithm is different from the previous ones. It performs binary segmentation: it divides the cloud in two clusters, one with points that do not belong to the object we are interested in (background points) and another with points that are considered part of the object (foreground points).

In order to do that, the algorithm uses a vertex graph, where each vertex (node of the graph) represents a point, plus two additional vertices called source and sink that will be connected to the others with edges with different penalties (weights). Then, edges are also established between neighboring points, with a weight value each that depends on the distance that separates them. The greater the distance, the higher the probability of the edge being cut. The algorithm also needs as input a point that is known to be the center of the object, and the radius (size). After everything has been set, the minimum cut will be found, and we will have a list of foreground points. 

#include <pcl/io/pcd_io.h>
#include <pcl/segmentation/min_cut_segmentation.h>

#include <iostream>

int
main(int argc, char** argv)
{
	// Object for storing the point cloud.
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);

	// Read a PCD file from disk.
	if (pcl::io::loadPCDFile<pcl::PointXYZ>(argv[1], *cloud) != 0)
	{
		return -1;
	}

	// Min-cut clustering object.
	pcl::MinCutSegmentation<pcl::PointXYZ> clustering;
	clustering.setInputCloud(cloud);
	// Create a cloud that lists all the points that we know belong to the object
	// (foreground points). We should set here the object's center.
	pcl::PointCloud<pcl::PointXYZ>::Ptr foregroundPoints(new pcl::PointCloud<pcl::PointXYZ>());
	pcl::PointXYZ point;
	point.x = 100.0;
	point.y = 100.0;
	point.z = 100.0;
	foregroundPoints->points.push_back(point);
	clustering.setForegroundPoints(foregroundPoints);
	// Set sigma, which affects the smooth cost calculation. It should be
	// set depending on the spacing between points in the cloud (resolution).
	clustering.setSigma(0.05);
	// Set the radius of the object we are looking for.
	clustering.setRadius(0.20);
	// Set the number of neighbors to look for. Increasing this also increases
	// the number of edges the graph will have.
	clustering.setNumberOfNeighbours(20);
	// Set the foreground penalty. It is the weight of the edges
	// that connect clouds points with the source vertex.
	clustering.setSourceWeight(0.6);

	std::vector <pcl::PointIndices> clusters;
	clustering.extract(clusters);

	std::cout << "Maximum flow is " << clustering.getMaxFlow() << "." << std::endl;

	// For every cluster...
	int currentClusterNum = 1;
	for (std::vector<pcl::PointIndices>::const_iterator i = clusters.begin(); i != clusters.end(); ++i)
	{
		// ...add all its points to a new cloud...
		pcl::PointCloud<pcl::PointXYZ>::Ptr cluster(new pcl::PointCloud<pcl::PointXYZ>);
		for (std::vector<int>::const_iterator point = i->indices.begin(); point != i->indices.end(); point++)
			cluster->points.push_back(cloud->points[*point]);
		cluster->width = cluster->points.size();
		cluster->height = 1;
		cluster->is_dense = true;

		// ...and save it to disk.
		if (cluster->points.size() <= 0)
			break;
		std::cout << "Cluster " << currentClusterNum << " has " << cluster->points.size() << " points." << std::endl;
		std::string fileName = "cluster" + boost::to_string(currentClusterNum) + ".pcd";
		pcl::io::savePCDFileASCII(fileName, *cluster);

		currentClusterNum++;
	}
}

More details about the min-cut algorithm can be found in the original paper (PDF).


RANSAC

As we saw in a previous section, PCL offers an implementation of the RANSAC algorithm to fit a set of points to a model. It will search the cloud and find a list of points that support the chosen model (plane, sphere...), and also compute the coefficients of the model. An object exists to perform easy segmentation of the points retrieved by the algorithm.

If what you want is to extract the points of a model with already known coefficients, see this tutorial:


Plane model

The following code lets you segment all planar surfaces from a point cloud. This is very important, because you will be able to detect things like the floor, ceiling, walls, or a table in a scene. If you know that the object you are looking for is sitting on a table, you can discard all points that are not supported by it, with the associated performance gain. Take a look at the pcl::ExtractPolygonalPrismData class, which is designed for this. After giving the coefficients of the plane and computing the convex hull, you can then create a 3D prism by extruding the hull a given height, and extract all points that lie inside it. 

#include <pcl/io/pcd_io.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>

#include <iostream>

int
main(int argc, char** argv)
{
	// Objects for storing the point clouds.
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::PointCloud<pcl::PointXYZ>::Ptr inlierPoints(new pcl::PointCloud<pcl::PointXYZ>);

	// Read a PCD file from disk.
	if (pcl::io::loadPCDFile<pcl::PointXYZ>(argv[1], *cloud) != 0)
	{
		return -1;
	}

	// Object for storing the plane model coefficients.
	pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
	// Create the segmentation object.
	pcl::SACSegmentation<pcl::PointXYZ> segmentation;
	segmentation.setInputCloud(cloud);
	// Configure the object to look for a plane.
	segmentation.setModelType(pcl::SACMODEL_PLANE);
	// Use RANSAC method.
	segmentation.setMethodType(pcl::SAC_RANSAC);
	// Set the maximum allowed distance to the model.
	segmentation.setDistanceThreshold(0.01);
	// Enable model coefficient refinement (optional).
	segmentation.setOptimizeCoefficients(true);

	pcl::PointIndices inlierIndices;
	segmentation.segment(inlierIndices, *coefficients);

	if (inlierIndices.indices.size() == 0)
		std::cout << "Could not find any points that fitted the plane model." << std::endl;
	else
	{
		std::cerr << "Model coefficients: " << coefficients->values[0] << " "
				  << coefficients->values[1] << " "
				  << coefficients->values[2] << " "
				  << coefficients->values[3] << std::endl;

		// Copy all inliers of the model to another cloud.
		pcl::copyPointCloud<pcl::PointXYZ>(*cloud, inlierIndices, *inlierPoints);
	}
}


Cylinder model

Segmenting a cylinder shape out of the cloud if analogous. There is only an additional parameter for specifying the radii: 

#include <pcl/io/pcd_io.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>

#include <iostream>

int
main(int argc, char** argv)
{
	// Objects for storing the point clouds.
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::PointCloud<pcl::PointXYZ>::Ptr inlierPoints(new pcl::PointCloud<pcl::PointXYZ>);

	// Read a PCD file from disk.
	if (pcl::io::loadPCDFile<pcl::PointXYZ>(argv[1], *cloud) != 0)
	{
		return -1;
	}

	// Object for storing the plane model coefficients.
	pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
	// Create the segmentation object.
	pcl::SACSegmentation<pcl::PointXYZ> segmentation;
	segmentation.setInputCloud(cloud);
	// Configure the object to look for a plane.
	segmentation.setModelType(pcl::SACMODEL_CYLINDER);
	// Use RANSAC method.
	segmentation.setMethodType(pcl::SAC_RANSAC);
	// Set the maximum allowed distance to the model.
	segmentation.setDistanceThreshold(0.01);
	// Enable model coefficient refinement (optional).
	segmentation.setOptimizeCoefficients(true);
	// Set minimum and maximum radii of the cylinder.
	segmentation.setRadiusLimits(0, 0.1);

	pcl::PointIndices inlierIndices;
	segmentation.segment(inlierIndices, *coefficients);

	if (inlierIndices.indices.size() == 0)
		std::cout << "Could not find any points that fitted the cylinder model." << std::endl;
	// Copy all inliers of the model to another cloud.
	else pcl::copyPointCloud<pcl::PointXYZ>(*cloud, inlierIndices, *inlierPoints);
}


Retrieving the hull

For a given set of points, PCL can compute the external hull of the shape, using the QHull library. The hull could be defined as the points that conform the outermost boundary of the set, like a shell showing the volume. There are two types of hull that you can calculate, the convex and the concave one. A convex hull can not have "holes", that is, the segment that connects any pair of points can never cross "empty" space (not inside the hull). A concave hull, on the other hand, usually takes less area, like you can see in the following images:


  • Concave hull.

  • Convex hull.


Concave hull

The following code shows how you can use PCL to compute the concave hull of the first plane found in the scene: 

#include <pcl/io/pcd_io.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/surface/concave_hull.h>
#include <pcl/visualization/cloud_viewer.h>

#include <iostream>

int
main(int argc, char** argv)
{
	// Objects for storing the point clouds.
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::PointCloud<pcl::PointXYZ>::Ptr plane(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::PointCloud<pcl::PointXYZ>::Ptr concaveHull(new pcl::PointCloud<pcl::PointXYZ>);

	// Read a PCD file from disk.
	if (pcl::io::loadPCDFile<pcl::PointXYZ>(argv[1], *cloud) != 0)
	{
		return -1;
	}

	// Get the plane model, if present.
	pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
	pcl::SACSegmentation<pcl::PointXYZ> segmentation;
	segmentation.setInputCloud(cloud);
	segmentation.setModelType(pcl::SACMODEL_PLANE);
	segmentation.setMethodType(pcl::SAC_RANSAC);
	segmentation.setDistanceThreshold(0.01);
	segmentation.setOptimizeCoefficients(true);
	pcl::PointIndices::Ptr inlierIndices(new pcl::PointIndices);
	segmentation.segment(*inlierIndices, *coefficients);

	if (inlierIndices->indices.size() == 0)
		std::cout << "Could not find a plane in the scene." << std::endl;
	else
	{
		// Copy the points of the plane to a new cloud.
		pcl::ExtractIndices<pcl::PointXYZ> extract;
		extract.setInputCloud(cloud);
		extract.setIndices(inlierIndices);
		extract.filter(*plane);

		// Object for retrieving the concave hull.
		pcl::ConcaveHull<pcl::PointXYZ> hull;
		hull.setInputCloud(plane);
		// Set alpha, which is the maximum length from a vertex to the center of the voronoi cell
		// (the smaller, the greater the resolution of the hull).
		hull.setAlpha(0.1);
		hull.reconstruct(*concaveHull);

		// Visualize the hull.
		pcl::visualization::CloudViewer viewerPlane("Concave hull");
		viewerPlane.showCloud(concaveHull);
		while (!viewerPlane.wasStopped())
		{
			// Do nothing but wait.
		}
	}
}

By choosing a smaller alpha value, you can improve the detail of the hull.


  • Table scene taken from the PCL dataset.

  • Concave hull of the table seen in the cloud (better seen fullscreen).


The class pcl::CropHull lets you find points that lie inside or outside a hull. One possible use, for a scene with a table and objects on it, would be:

  1. Find a plane in the scene (it should be the table).
  2. Compute the hull of the plane.
  3. Project the points of the cloud onto the plane.
  4. Find which projected points are lying inside the plane hull. Those belong to objects sitting on the table.



Convex hull

Computing the convex hull is done the same way, just changing the names accordingly. Also, there is no need to set the alpha parameter here: 

#include <pcl/io/pcd_io.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/surface/convex_hull.h>
#include <pcl/visualization/cloud_viewer.h>

#include <iostream>

int
main(int argc, char** argv)
{
	// Objects for storing the point clouds.
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::PointCloud<pcl::PointXYZ>::Ptr plane(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::PointCloud<pcl::PointXYZ>::Ptr convexHull(new pcl::PointCloud<pcl::PointXYZ>);

	// Read a PCD file from disk.
	if (pcl::io::loadPCDFile<pcl::PointXYZ>(argv[1], *cloud) != 0)
	{
		return -1;
	}

	// Get the plane model, if present.
	pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
	pcl::SACSegmentation<pcl::PointXYZ> segmentation;
	segmentation.setInputCloud(cloud);
	segmentation.setModelType(pcl::SACMODEL_PLANE);
	segmentation.setMethodType(pcl::SAC_RANSAC);
	segmentation.setDistanceThreshold(0.01);
	segmentation.setOptimizeCoefficients(true);
	pcl::PointIndices::Ptr inlierIndices(new pcl::PointIndices);
	segmentation.segment(*inlierIndices, *coefficients);

	if (inlierIndices->indices.size() == 0)
		std::cout << "Could not find a plane in the scene." << std::endl;
	else
	{
		// Copy the points of the plane to a new cloud.
		pcl::ExtractIndices<pcl::PointXYZ> extract;
		extract.setInputCloud(cloud);
		extract.setIndices(inlierIndices);
		extract.filter(*plane);

		// Object for retrieving the convex hull.
		pcl::ConvexHull<pcl::PointXYZ> hull;
		hull.setInputCloud(plane);
		hull.reconstruct(*convexHull);

		// Visualize the hull.
		pcl::visualization::CloudViewer viewerPlane("Convex hull");
		viewerPlane.showCloud(convexHull);
		while (!viewerPlane.wasStopped())
		{
			// Do nothing but wait.
		}
	}
}


Reconstruction

To learn about how to use PCL to build a smooth, parametric surface representation from a point cloud, you can read the following:


Triangulation

Triangulation is a a way of estimating the surface captured by a point cloud, by connecting points with each other, ending up with a continous polygon mesh (three-sided polygons, that is, triangles). After you retrieve this surface mesh, you can for example export it to a format that most 3D modelling tools will understand, like VTK (Visualization Toolkit), which can be opened in Blender or 3ds Max. PCL can also convert from VTK to PLY or OBJ.

The following code uses PCL's implementation of a greedy triangulation algorithm that works with local 2D projections. For every point, it looks "down" along the normal, and connects neighboring points. For more information, specially regarding the parameters, check the API or the original PCL tutorial: 

#include <pcl/io/pcd_io.h>
#include <pcl/features/normal_3d.h>
#include <pcl/surface/gp3.h>
#include <pcl/io/vtk_io.h>

int
main(int argc, char** argv)
{
	// Object for storing the point cloud.
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
	// Object for storing the normals.
	pcl::PointCloud<pcl::Normal>::Ptr normals(new pcl::PointCloud<pcl::Normal>);
	// Object for storing both the points and the normals.
	pcl::PointCloud<pcl::PointNormal>::Ptr cloudNormals(new pcl::PointCloud<pcl::PointNormal>);

	// Read a PCD file from disk.
	if (pcl::io::loadPCDFile<pcl::PointXYZ>(argv[1], *cloud) != 0)
	{
		return -1;
	}

	// Normal estimation.
	pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> normalEstimation;
	normalEstimation.setInputCloud(cloud);
	normalEstimation.setRadiusSearch(0.03);
	pcl::search::KdTree<pcl::PointXYZ>::Ptr kdtree(new pcl::search::KdTree<pcl::PointXYZ>);
	normalEstimation.setSearchMethod(kdtree);
	normalEstimation.compute(*normals);

	// The triangulation object requires the points and normals to be stored in the same structure.
	pcl::concatenateFields(*cloud, *normals, *cloudNormals);
	// Tree object for searches in this new object.
	pcl::search::KdTree<pcl::PointNormal>::Ptr kdtree2(new pcl::search::KdTree<pcl::PointNormal>);
	kdtree2->setInputCloud(cloudNormals);

	// Triangulation object.
	pcl::GreedyProjectionTriangulation<pcl::PointNormal> triangulation;
	// Output object, containing the mesh.
	pcl::PolygonMesh triangles;
	// Maximum distance between connected points (maximum edge length).
	triangulation.setSearchRadius(0.025);
	// Maximum acceptable distance for a point to be considered,
	// relative to the distance of the nearest point.
	triangulation.setMu(2.5);
	// How many neighbors are searched for.
	triangulation.setMaximumNearestNeighbors(100);
	// Points will not be connected to the current point
	// if their normals deviate more than the specified angle.
	triangulation.setMaximumSurfaceAngle(M_PI / 4); // 45 degrees.
	// If false, the direction of normals will not be taken into account
	// when computing the angle between them.
	triangulation.setNormalConsistency(false);
	// Minimum and maximum angle there can be in a triangle.
	// The first is not guaranteed, the second is.
	triangulation.setMinimumAngle(M_PI / 18); // 10 degrees.
	triangulation.setMaximumAngle(2 * M_PI / 3); // 120 degrees.

	// Triangulate the cloud.
	triangulation.setInputCloud(cloudNormals);
	triangulation.setSearchMethod(kdtree2);
	triangulation.reconstruct(triangles);

	// Save to disk.
	pcl::io::saveVTKFile("mesh.vtk", triangles);
}


VTK file produced by triangulation of a point cloud.



Go to root: PhD-3D-Object-Tracking

Links to articles:

PCL/OpenNI tutorial 0: The very basics

PCL/OpenNI tutorial 1: Installing and testing

PCL/OpenNI tutorial 2: Cloud processing (basic)

PCL/OpenNI tutorial 3: Cloud processing (advanced)

PCL/OpenNI tutorial 4: 3D object recognition (descriptors)

PCL/OpenNI tutorial 5: 3D object recognition (pipeline)

PCL/OpenNI troubleshooting

Retrieved from "https://robotica.unileon.es/index.php?title=PCL/OpenNI_tutorial_3:_Cloud_processing_(advanced)&oldid=4741"