Latest revision as of 23:09, 1 November 2015

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...).

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%.

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:

• pcl::SampleConsensusModelCircle2D: A 2D circle on the X-Y plane.
• pcl::SampleConsensusModelCircle3D: A 3D circle (any plane).
• pcl::SampleConsensusModelCone: A cone.
• pcl::SampleConsensusModelCylinder: A cylinder.
• pcl::SampleConsensusModelLine: A line.
• pcl::SampleConsensusModelPlane: A plane.
• pcl::SampleConsensusModelSphere: A sphere.
• pcl::SampleConsensusModelStick: A stick (a line with user-given minimum/maximum width).

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++;
	}
}