How to use Random Sample Consensus model

In this tutorial we learn how to use a RandomSampleConsensus with a plane model to obtain the cloud fitting to this model.

Theoretical Primer

The abbreviation of “RANdom SAmple Consensus” is RANSAC, and it is an iterative method that is used to estimate parameters of a mathematical model from a set of data containing outliers. This algorithm was published by Fischler and Bolles in 1981. The RANSAC algorithm assumes that all of the data we are looking at is comprised of both inliers and outliers. Inliers can be explained by a model with a particular set of parameter values, while outliers do not fit that model in any circumstance. Another necessary assumption is that a procedure which can optimally estimate the parameters of the chosen model from the data is available.

From [WikipediaRANSAC]:

The input to the RANSAC algorithm is a set of observed data values, a parameterized model which can explain or be fitted to the observations, and some confidence parameters.

RANSAC achieves its goal by iteratively selecting a random subset of the original data. These data are hypothetical inliers and this hypothesis is then tested as follows:

  1. A model is fitted to the hypothetical inliers, i.e. all free parameters of the model are reconstructed from the inliers.

  2. All other data are then tested against the fitted model and, if a point fits well to the estimated model, also considered as a hypothetical inlier.

  3. The estimated model is reasonably good if sufficiently many points have been classified as hypothetical inliers.

  4. The model is reestimated from all hypothetical inliers, because it has only been estimated from the initial set of hypothetical inliers.

  5. Finally, the model is evaluated by estimating the error of the inliers relative to the model.

This procedure is repeated a fixed number of times, each time producing either a model which is rejected because too few points are classified as inliers or a refined model together with a corresponding error measure. In the latter case, we keep the refined model if its error is lower than the last saved model.

…

An advantage of RANSAC is its ability to do robust estimation of the model parameters, i.e., it can estimate the parameters with a high degree of accuracy even when a significant number of outliers are present in the data set. A disadvantage of RANSAC is that there is no upper bound on the time it takes to compute these parameters. When the number of iterations computed is limited the solution obtained may not be optimal, and it may not even be one that fits the data in a good way. In this way RANSAC offers a trade-off; by computing a greater number of iterations the probability of a reasonable model being produced is increased. Another disadvantage of RANSAC is that it requires the setting of problem-specific thresholds.

RANSAC can only estimate one model for a particular data set. As for any one-model approach when two (or more) models exist, RANSAC may fail to find either one.

_images/random_sample_example1.png _images/random_sample_example2.png

The pictures to the left and right (From [WikipediaRANSAC]) show a simple application of the RANSAC algorithm on a 2-dimensional set of data. The image on our left is a visual representation of a data set containing both inliers and outliers. The image on our right shows all of the outliers in red, and shows inliers in blue. The blue line is the result of the work done by RANSAC. In this case the model that we are trying to fit to the data is a line, and it looks like it’s a fairly good fit to our data.

The code

Create a file, let’s say, random_sample_consensus.cpp in your favorite editor and place the following inside:

  1#include <iostream>
  2#include <thread>
  3
  4#include <pcl/console/parse.h>
  5#include <pcl/point_cloud.h> // for PointCloud
  6#include <pcl/common/io.h> // for copyPointCloud
  7#include <pcl/point_types.h>
  8#include <pcl/sample_consensus/ransac.h>
  9#include <pcl/sample_consensus/sac_model_plane.h>
 10#include <pcl/sample_consensus/sac_model_sphere.h>
 11#include <pcl/visualization/pcl_visualizer.h>
 12
 13using namespace std::chrono_literals;
 14
 15pcl::visualization::PCLVisualizer::Ptr
 16simpleVis (pcl::PointCloud<pcl::PointXYZ>::ConstPtr cloud)
 17{
 18  // --------------------------------------------
 19  // -----Open 3D viewer and add point cloud-----
 20  // --------------------------------------------
 21  pcl::visualization::PCLVisualizer::Ptr viewer (new pcl::visualization::PCLVisualizer ("3D Viewer"));
 22  viewer->setBackgroundColor (0, 0, 0);
 23  viewer->addPointCloud<pcl::PointXYZ> (cloud, "sample cloud");
 24  viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "sample cloud");
 25  //viewer->addCoordinateSystem (1.0, "global");
 26  viewer->initCameraParameters ();
 27  return (viewer);
 28}
 29
 30int
 31main(int argc, char** argv)
 32{
 33  // initialize PointClouds
 34  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
 35  pcl::PointCloud<pcl::PointXYZ>::Ptr final (new pcl::PointCloud<pcl::PointXYZ>);
 36
 37  // populate our PointCloud with points
 38  cloud->width    = 500;
 39  cloud->height   = 1;
 40  cloud->is_dense = false;
 41  cloud->points.resize (cloud->width * cloud->height);
 42  for (pcl::index_t i = 0; i < static_cast<pcl::index_t>(cloud->size ()); ++i)
 43  {
 44    if (pcl::console::find_argument (argc, argv, "-s") >= 0 || pcl::console::find_argument (argc, argv, "-sf") >= 0)
 45    {
 46      (*cloud)[i].x = 1024 * rand () / (RAND_MAX + 1.0);
 47      (*cloud)[i].y = 1024 * rand () / (RAND_MAX + 1.0);
 48      if (i % 5 == 0)
 49        (*cloud)[i].z = 1024 * rand () / (RAND_MAX + 1.0);
 50      else if(i % 2 == 0)
 51        (*cloud)[i].z =  sqrt( 1 - ((*cloud)[i].x * (*cloud)[i].x)
 52                                      - ((*cloud)[i].y * (*cloud)[i].y));
 53      else
 54        (*cloud)[i].z =  - sqrt( 1 - ((*cloud)[i].x * (*cloud)[i].x)
 55                                        - ((*cloud)[i].y * (*cloud)[i].y));
 56    }
 57    else
 58    {
 59      (*cloud)[i].x = 1024 * rand () / (RAND_MAX + 1.0);
 60      (*cloud)[i].y = 1024 * rand () / (RAND_MAX + 1.0);
 61      if( i % 2 == 0)
 62        (*cloud)[i].z = 1024 * rand () / (RAND_MAX + 1.0);
 63      else
 64        (*cloud)[i].z = -1 * ((*cloud)[i].x + (*cloud)[i].y);
 65    }
 66  }
 67
 68  std::vector<int> inliers;
 69
 70  // created RandomSampleConsensus object and compute the appropriated model
 71  pcl::SampleConsensusModelSphere<pcl::PointXYZ>::Ptr
 72    model_s(new pcl::SampleConsensusModelSphere<pcl::PointXYZ> (cloud));
 73  pcl::SampleConsensusModelPlane<pcl::PointXYZ>::Ptr
 74    model_p (new pcl::SampleConsensusModelPlane<pcl::PointXYZ> (cloud));
 75  if(pcl::console::find_argument (argc, argv, "-f") >= 0)
 76  {
 77    pcl::RandomSampleConsensus<pcl::PointXYZ> ransac (model_p);
 78    ransac.setDistanceThreshold (.01);
 79    ransac.computeModel();
 80    ransac.getInliers(inliers);
 81  }
 82  else if (pcl::console::find_argument (argc, argv, "-sf") >= 0 )
 83  {
 84    pcl::RandomSampleConsensus<pcl::PointXYZ> ransac (model_s);
 85    ransac.setDistanceThreshold (.01);
 86    ransac.computeModel();
 87    ransac.getInliers(inliers);
 88  }
 89
 90  // copies all inliers of the model computed to another PointCloud
 91  pcl::copyPointCloud (*cloud, inliers, *final);
 92
 93  // creates the visualization object and adds either our original cloud or all of the inliers
 94  // depending on the command line arguments specified.
 95  pcl::visualization::PCLVisualizer::Ptr viewer;
 96  if (pcl::console::find_argument (argc, argv, "-f") >= 0 || pcl::console::find_argument (argc, argv, "-sf") >= 0)
 97    viewer = simpleVis(final);
 98  else
 99    viewer = simpleVis(cloud);
100  while (!viewer->wasStopped ())
101  {
102    viewer->spinOnce (100);
103    std::this_thread::sleep_for(100ms);
104  }
105  return 0;
106 }

The explanation

The following source code initializes two PointClouds and fills one of them with points. The majority of these points are placed in the cloud according to a model, but a fraction (1/5) of them are given arbitrary locations.

  // initialize PointClouds
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
  pcl::PointCloud<pcl::PointXYZ>::Ptr final (new pcl::PointCloud<pcl::PointXYZ>);

  // populate our PointCloud with points
  cloud->width    = 500;
  cloud->height   = 1;
  cloud->is_dense = false;
  cloud->points.resize (cloud->width * cloud->height);
  for (pcl::index_t i = 0; i < static_cast<pcl::index_t>(cloud->size ()); ++i)
  {
    if (pcl::console::find_argument (argc, argv, "-s") >= 0 || pcl::console::find_argument (argc, argv, "-sf") >= 0)
    {
      (*cloud)[i].x = 1024 * rand () / (RAND_MAX + 1.0);
      (*cloud)[i].y = 1024 * rand () / (RAND_MAX + 1.0);
      if (i % 5 == 0)
        (*cloud)[i].z = 1024 * rand () / (RAND_MAX + 1.0);
      else if(i % 2 == 0)
        (*cloud)[i].z =  sqrt( 1 - ((*cloud)[i].x * (*cloud)[i].x)
                                      - ((*cloud)[i].y * (*cloud)[i].y));
      else
        (*cloud)[i].z =  - sqrt( 1 - ((*cloud)[i].x * (*cloud)[i].x)
                                        - ((*cloud)[i].y * (*cloud)[i].y));
    }
    else
    {
      (*cloud)[i].x = 1024 * rand () / (RAND_MAX + 1.0);
      (*cloud)[i].y = 1024 * rand () / (RAND_MAX + 1.0);
      if( i % 2 == 0)
        (*cloud)[i].z = 1024 * rand () / (RAND_MAX + 1.0);
      else
        (*cloud)[i].z = -1 * ((*cloud)[i].x + (*cloud)[i].y);
    }
  }

Next we create a vector of ints that can store the locations of our inlier points from our PointCloud and now we can build our RandomSampleConsensus object using either a plane or a sphere model from our input cloud.

  std::vector<int> inliers;

  // created RandomSampleConsensus object and compute the appropriated model
  pcl::SampleConsensusModelSphere<pcl::PointXYZ>::Ptr
    model_s(new pcl::SampleConsensusModelSphere<pcl::PointXYZ> (cloud));
  pcl::SampleConsensusModelPlane<pcl::PointXYZ>::Ptr
    model_p (new pcl::SampleConsensusModelPlane<pcl::PointXYZ> (cloud));
  if(pcl::console::find_argument (argc, argv, "-f") >= 0)
  {
    pcl::RandomSampleConsensus<pcl::PointXYZ> ransac (model_p);
    ransac.setDistanceThreshold (.01);
    ransac.computeModel();
    ransac.getInliers(inliers);
  }
  else if (pcl::console::find_argument (argc, argv, "-sf") >= 0 )
  {
    pcl::RandomSampleConsensus<pcl::PointXYZ> ransac (model_s);
    ransac.setDistanceThreshold (.01);
    ransac.computeModel();
    ransac.getInliers(inliers);
  }

This last bit of code copies all of the points that fit our model to another cloud and then display either that or our original cloud in the viewer.

  // copies all inliers of the model computed to another PointCloud
  pcl::copyPointCloud (*cloud, inliers, *final);

  // creates the visualization object and adds either our original cloud or all of the inliers
  // depending on the command line arguments specified.
  pcl::visualization::PCLVisualizer::Ptr viewer;
  if (pcl::console::find_argument (argc, argv, "-f") >= 0 || pcl::console::find_argument (argc, argv, "-sf") >= 0)
    viewer = simpleVis(final);
  else
    viewer = simpleVis(cloud);

There is some extra code that relates to the display of the PointClouds in the 3D Viewer, but I’m not going to explain that here.

Compiling and running the program

Add the following lines to your CMakeLists.txt file:

 1cmake_minimum_required(VERSION 3.5 FATAL_ERROR)
 2
 3project(random_sample_consensus)
 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 (random_sample_consensus random_sample_consensus.cpp)
12target_link_libraries (random_sample_consensus ${PCL_LIBRARIES})

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

$ ./random_sample_consensus

to have a viewer window display that shows you the original PointCloud (with outliers) we have created.

_images/ransac_outliers_plane.png

Hit ‘r’ on your keyboard to scale and center the viewer. You can then click and drag to rotate the view. You can tell there is very little organization to this PointCloud and that it contains many outliers. Pressing ‘q’ on your keyboard will close the viewer and end the program. Now if you run the program with the following argument:

$ ./random_sample_consensus -f

the program will display only the indices of the original PointCloud which satisfy the particular model we have chosen (in this case plane) as found by RandomSampleConsens in the viewer.

_images/ransac_inliers_plane.png

Again hit ‘r’ to scale and center the view and then click and drag with the mouse to rotate around the cloud. You can see there are no longer any points that do not lie within the plane model in this PointCloud. Hit ‘q’ to exit the viewer and program.

There is also an example using a sphere in this program. If you run it with:

$ ./random_sample_consensus -s

It will generate and display a spherical cloud and some outliers as well.

_images/ransac_outliers_sphere.png

Then when you run the program with:

$ ./random_sample_consensus -sf

It will show you the result of applying RandomSampleConsensus to this data set with a spherical model.

_images/ransac_inliers_sphere.png
WikipediaRANSAC(1,2)

https://en.wikipedia.org/wiki/RANSAC