Step 2.B : Adapting the RANSAC algorithm for Ground detection

Now that we have the tools necessary from previous articles, we can proceed to modify a bit the RANSAC algorithm for floor detection.

1. Step 1: Image filtering
2. Step 2: Modified RANSAC algorithm for floor detection
3. Step 2.A : Calculating angle between the plane normal and the axis

Following the pseudocode:

Where we know know how to randomly sample a plane, it’s coefficients and the normalized Normal vector, with this we can restrain the angle within an acceptable angle range with respect to the floor.

Then we calculate the distance from each point to the plane and find the inliers which are the data points corresponding to the floor data points. also we calculate the standard deviation of this group of inliers. If these inliers are within the selected parameters the they are the best inliers so far.

code in python:

import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
import math
import numpy as np

def fit_plane_ransac(data, threshold=0.01, max_iterations=100):
    i = 0
    bestSupport = 0
    best_plane = [0,0,0,0]
    best_inliers = []
    BestStDeviation = 10e10
    while i  < max_iterations:
        # Randomly sample three points
        sample_indices = np.random.choice(len(data), 3, replace=False)
        p1, p2, p3 = data[sample_indices]
        # Fit a plane to the sampled points
        normalVector, Currplane = calculate_plane_coefficients(p1,p2,p3)
        diff = angle_between_vectors(normalVector,(0,1,0))
        diff = np.degrees(diff)
        if(diff < 4):
            d = -np.dot(normalVector, p1)
            Currplane = np.concatenate((normalVector, [d]))
            # Calculate distances from all points to the plane
            distances = np.abs(np.dot(data, normalVector) + d) / np.linalg.norm(normalVector)
            # Count inliers (points within the threshold distance from the plane)
            inliers = data[distances < threshold]
            std = np.std(inliers)
            # Update best plane if the current one has more inliers
            if (len(inliers) > bestSupport) or ((len(inliers) == bestSupport) and (std < BestStDeviation)):
                bestSupport = len(inliers)
                best_plane = Currplane
                best_inliers = inliers
                BestStDeviation = std
            else:
                i = i + 1
                
            
    return best_plane, best_inliers

rs = np.random.seed(42)
np.random.seed(rs)


# Fit a plane using RANSAC
best_plane, inliers = fit_plane_ransac(subsampled_points, threshold=75, max_iterations=200)

print("Best Plane Parameters:", best_plane)

# Plot the data and the best-fit plane
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D

print(best_plane)

fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')
ax.scatter(subsampled_points[:,0], subsampled_points[:,1], subsampled_points[:,2],alpha=0.15,  label='Noisy 3D Data', c = subsampled_points[:,2],cmap = 'viridis')
ax.scatter(inliers[:,0], inliers[:,1], inliers[:,2],alpha=0.95, color='red', label='Inliers')
ax.set_xlabel('X')
ax.set_ylabel('Y')
ax.set_zlabel('Z')
ax.legend()
plt.title('RANSAC Plane Fitting in 3D')
plt.show()

finally we plot the image with the inliers which are in red. This code is a bit slow and the are some things that I do plan to enhance but for now in order to keep advancing I skip to the next objective of the paper which is object detection with Google’s MediaPipe.