Plane model segmentation

In this tutorial we will learn how to do a simple plane segmentation of a set of points, that is to find all the points within a point cloud that support a plane model. This tutorial supports the Extracting indices from a PointCloud tutorial, presented in the filtering section.

The code

First, create a file, let’s say, planar_segmentation.cpp in your favorite editor, and place the following inside it:

 1#include <iostream>
 2#include <pcl/ModelCoefficients.h>
 3#include <pcl/io/pcd_io.h>
 4#include <pcl/point_types.h>
 5#include <pcl/sample_consensus/method_types.h>
 6#include <pcl/sample_consensus/model_types.h>
 7#include <pcl/segmentation/sac_segmentation.h>
 8
 9int
10 main ()
11{
12  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
13
14  // Fill in the cloud data
15  cloud->width  = 15;
16  cloud->height = 1;
17  cloud->points.resize (cloud->width * cloud->height);
18
19  // Generate the data
20  for (auto& point: *cloud)
21  {
22    point.x = 1024 * rand () / (RAND_MAX + 1.0f);
23    point.y = 1024 * rand () / (RAND_MAX + 1.0f);
24    point.z = 1.0;
25  }
26
27  // Set a few outliers
28  (*cloud)[0].z = 2.0;
29  (*cloud)[3].z = -2.0;
30  (*cloud)[6].z = 4.0;
31
32  std::cerr << "Point cloud data: " << cloud->size () << " points" << std::endl;
33  for (const auto& point: *cloud)
34    std::cerr << "    " << point.x << " "
35                        << point.y << " "
36                        << point.z << std::endl;
37
38  pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
39  pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
40  // Create the segmentation object
41  pcl::SACSegmentation<pcl::PointXYZ> seg;
42  // Optional
43  seg.setOptimizeCoefficients (true);
44  // Mandatory
45  seg.setModelType (pcl::SACMODEL_PLANE);
46  seg.setMethodType (pcl::SAC_RANSAC);
47  seg.setDistanceThreshold (0.01);
48
49  seg.setInputCloud (cloud);
50  seg.segment (*inliers, *coefficients);
51
52  if (inliers->indices.size () == 0)
53  {
54    PCL_ERROR ("Could not estimate a planar model for the given dataset.\n");
55    return (-1);
56  }
57
58  std::cerr << "Model coefficients: " << coefficients->values[0] << " " 
59                                      << coefficients->values[1] << " "
60                                      << coefficients->values[2] << " " 
61                                      << coefficients->values[3] << std::endl;
62
63  std::cerr << "Model inliers: " << inliers->indices.size () << std::endl;
64  for (const auto& idx: inliers->indices)
65    std::cerr << idx << "    " << cloud->points[idx].x << " "
66                               << cloud->points[idx].y << " "
67                               << cloud->points[idx].z << std::endl;
68
69  return (0);
70}

The explanation

Now, let’s break down the code piece by piece.

Lines:

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

Important

Please visit https://pointclouds.org/documentation/group__sample__consensus.html for more information on various other implemented Sample Consensus models and robust estimators.

Lines:

  // Fill in the cloud data
  cloud->width  = 15;
  cloud->height = 1;
  cloud->points.resize (cloud->width * cloud->height);

  // Generate the data
  for (auto& point: *cloud)
  {
    point.x = 1024 * rand () / (RAND_MAX + 1.0f);
    point.y = 1024 * rand () / (RAND_MAX + 1.0f);
    point.z = 1.0;
  }

  // Set a few outliers
  (*cloud)[0].z = 2.0;
  (*cloud)[3].z = -2.0;
  (*cloud)[6].z = 4.0;

  std::cerr << "Point cloud data: " << cloud->size () << " points" << std::endl;
  for (const auto& point: *cloud)
    std::cerr << "    " << point.x << " "
                        << point.y << " "
                        << point.z << std::endl;

create the point cloud structure, fill in the respective values, and display the content on screen. Note that for the purpose of this tutorial, we manually added a few outliers in the data, by setting their z values different from 0.

Then, lines:

  pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
  pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
  // Create the segmentation object
  pcl::SACSegmentation<pcl::PointXYZ> seg;
  // Optional
  seg.setOptimizeCoefficients (true);
  // Mandatory
  seg.setModelType (pcl::SACMODEL_PLANE);
  seg.setMethodType (pcl::SAC_RANSAC);
  seg.setDistanceThreshold (0.01);

  seg.setInputCloud (cloud);
  seg.segment (*inliers, *coefficients);

create the SACSegmentation object and set the model and method type. This is also where we specify the “distance threshold”, which determines how close a point must be to the model in order to be considered an inlier. In this tutorial, we will use the RANSAC method (pcl::SAC_RANSAC) as the robust estimator of choice. Our decision is motivated by RANSAC’s simplicity (other robust estimators use it as a base and add additional, more complicated concepts). For more information about RANSAC, check its Wikipedia page.

Finally:

  std::cerr << "Model coefficients: " << coefficients->values[0] << " " 
                                      << coefficients->values[1] << " "
                                      << coefficients->values[2] << " " 
                                      << coefficients->values[3] << std::endl;

are used to show the contents of the inlier set, together with the estimated plane parameters (in ax + by + cz + d = 0 form).

Compiling and running the program

Add the following lines to your CMakeLists.txt file:

 1cmake_minimum_required(VERSION 3.5 FATAL_ERROR)
 2
 3project(planar_segmentation)
 4
 5find_package(PCL 1.2 REQUIRED)
 6
 7include_directories(${PCL_INCLUDE_DIRS})
 8link_directories(${PCL_LIBRARY_DIRS})
 9add_definitions(${PCL_DEFINITIONS})
10
11add_executable (planar_segmentation planar_segmentation.cpp)
12target_link_libraries (planar_segmentation ${PCL_LIBRARIES})

After you have made the executable, you can run it. Simply do:

$ ./planar_segmentation

You will see something similar to:

Point cloud data: 15 points
    0.352222 -0.151883 2
    -0.106395 -0.397406 1
    -0.473106 0.292602 1
    -0.731898 0.667105 -2
    0.441304 -0.734766 1
    0.854581 -0.0361733 1
    -0.4607 -0.277468 4
    -0.916762 0.183749 1
    0.968809 0.512055 1
    -0.998983 -0.463871 1
    0.691785 0.716053 1
    0.525135 -0.523004 1
    0.439387 0.56706 1
    0.905417 -0.579787 1
    0.898706 -0.504929 1
[pcl::SACSegmentation::initSAC] Setting the maximum number of iterations to 50
Model coefficients: 0 0 1 -1
Model inliers: 12
1    -0.106395 -0.397406 1
2    -0.473106 0.292602 1
4    0.441304 -0.734766 1
5    0.854581 -0.0361733 1
7    -0.916762 0.183749 1
8    0.968809 0.512055 1
9    -0.998983 -0.463871 1
10    0.691785 0.716053 1
11    0.525135 -0.523004 1
12    0.439387 0.56706 1
13    0.905417 -0.579787 1
14    0.898706 -0.504929 1

A graphical display of the segmentation process is shown below.

_images/planar_segmentation_2.png

Note that the coordinate axes are represented as red (x), green (y), and blue (z). The points are represented with red as the outliers, and green as the inliers of the plane model found.