3D RANSAC Algorithm for Lidar PCD Segmentation

Udacity-SensorFusion-Lidar-PCD

Theory of RANSAC

RANSAC stands for RANdom Sampling and Consensus. Its a robust model fitting algorithm, and its performance is often compared to that of the Linear Regression algorithm.
In this case, we are going to use RANSAC algorithm on Lidar point cloud(pcd) data to segment the ground plane from the other planes which could consist of vehicles, traffic-lights, or anything above the ground plane which needs to be classified for avoiding collision or for trajectory planning.

In this particular application RANSAC is preferred for ground plane segmentation due to inherent property to reject outliers. (Outliers can be considered as unusual observations which are likely to be rejected)

The core algorithm is fairly straightforward.
Step 1 :: Select a random set of points(3 points for a forming a plane)

Step 2 :: Calculate the parameters required for the plane equation.

3D Plane equations for 3 non-collinear points

Step 3 :: Calculate the deviation of all the points in the point cloud from the plane using a distance estimate.

Step 4 :: If the distance is within the THRESHOLD then add the point as an inlier.
Step 5 :: Store the plane points and points having the maximum number of inliers.
Step6:: Repeat the process again for MAX_ITERATIONS

After the RANSAC processing is complete, the plane having the maximum number of inliers is the best estimate of the ground plane. Any points cluster above this ground plane can be classified as an object, obstacle or traffic signs.

Practical Implementation of RANSAC in Python

The libraries required for running the RANSAC algorithm in python

import pandas as pd
import matplotlib.pyplot as plt
import random
import math
from mpl_toolkits.mplot3d import Axes3D
import open3d as o3d

Create a class for implementing RANSAC.

The 3 inputs required for implementing a basic RANSAC algorithm are:

1. The point cloud data (in either pcd or ASCII)
2. MAX_ITERATIONS, as described in Step 6 of the logic.
3. THRESHOLD, as described in Step 4 of the logic.

class RANSAC:
    """
    RANSAC Class
    """
    def __init__(self, point_cloud, max_iterations,            
                 distance_ratio_threshold):
        self.point_cloud = point_cloud
        self.max_iterations = max_iterations
        self.distance_ratio_threshold = distance_ratio_threshold

The RANSAC logic implementation can be found below:

Lines 7–15 — Creating a storage set of inliers_results, this is index of all points which are considered as inliers. A SET data structure is preferred to automatically remove the duplicate indexes generated. 3 Random points are generated for every iteration for creating the plane to find the inliers.

Lines 17–33 — These lines correspond to fetching the point data from the pcd/ASCII file format. This might change based on the format of the input data. In my case, in the ASCII file I had rows containing data like (x,y,z,R,G,B), so I don’t care about the R,G,B values. In the pcd file the data format was directly (x,y,z). I load the point cloud data as pandas dataframe so you can see some methods like .loc which fetch the data from a particular index location.

Lines 35–55 — In this section we are iterating through all the points in the point cloud, and trying to find inliers for the plane that we just generated through random points. Needless to say, we don’t want to consider the randomly generated points of the plane, so we don’t use them by adding the conditional logic at Line 38. You can ignore the try and excepts block, since they are just trying to fetch the data based on the type of PCD data format. Once we have the point, we calculate the distance of that point from the plane that we generated based on the Cartesian formulae in the Step3. If the calculated distance is within the THRESHOLD then we consider it as an inlier. After every iteration in MAX_ITERATIONS limit we try to find the plane which has the maximum number of inliers through the logic in lines 53–55.

Lines 58–70 — After getting the plane with the best count of inliers, we consider all the other points as outliers, and segregate and store them separately for visualization.

The complete code can be found here.

Needless to say, the time taken for generating the ground plane depends on the MAX_ITERATION hyperparameter. The general execution speed is very slow when compared to the C++ implementation.

Practical Implementation of RANSAC in C++

You can find the C++ implementation of RANSAC along with k-D tree based clustering here. This was part of my Sensor fusion nanodegree from Udacity.

Notes : If you want to use RANSAC for your projects, you can find its implementation in scikit-learn. The RANSAC algorithm is available in linear_model with much more hyperparameters for optimization.