let’s divide an conquer
Quick Intro
If you already have a solid understanding of trigonometry, and vector calculus and the RANSAC algorithm, you may find this article a bit too easy. If you´re like me and barely understood the algorithm # 1 presented in [1], stick around because we are going to tackle the problem step by step.
Asked chat GPT for help
I asked our good friend to generate some code for a base RANSAC algorithm in 3D, here’s the result.
import numpy as np
def fit_plane_ransac(data, threshold=0.1, max_iterations=100):
best_plane = None
best_inliers = []
for _ in range(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
normal = np.cross(p2 - p1, p3 - p1)
normal /= np.linalg.norm(normal)
d = -np.dot(normal, p1)
plane = np.concatenate((normal, [d]))
# Calculate distances from all points to the plane
distances = np.abs(np.dot(data, normal) + d) / np.linalg.norm(normal)
# Count inliers (points within the threshold distance from the plane)
inliers = data[distances < threshold]
# Update best plane if the current one has more inliers
if len(inliers) > len(best_inliers):
best_plane = plane
best_inliers = inliers
return best_plane, best_inliers
# Example usage
# Generate random 3D noisy data points around a plane
np.random.seed(42)
plane_normal = np.array([1, 1, 1])
plane_point = np.array([0, 0, 0])
noise_level = 0.1
num_points = 200
data = plane_point + np.random.randn(num_points, 3) * noise_level
# Add a few outliers
outliers = np.random.randn(10, 3) * 5
data = np.concatenate((data, outliers))
# Fit a plane using RANSAC
best_plane, inliers = fit_plane_ransac(data, threshold=0.5, max_iterations=100)
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
fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')
ax.scatter(data[:,0], data[:,1], data[:,2], label='Noisy 3D Data')
ax.scatter(inliers[:,0], inliers[:,1], inliers[:,2], color='red', label='Inliers')
xx, yy = np.meshgrid(range(-5, 6), range(-5, 6))
zz = (-best_plane[0] * xx - best_plane[1] * yy - best_plane[3]) * 1. / best_plane[2]
ax.plot_surface(xx, yy, zz, alpha=0.5, color='green', label='Best-fit Plane')
ax.set_xlabel('X')
ax.set_ylabel('Y')
ax.set_zlabel('Z')
ax.legend()
plt.title('RANSAC Plane Fitting in 3D')
plt.show()
Now after seeing this and slowly reading the presented algorithm 1 as well as actually see the point cloud generated from the depth camera images.
now I have a better understanding of what the modified algorithm. I apologize if my approach is a bit disorganized, but I’m showing you how I got to understand the algorithm, now that I have an basic idea let’s start from the beginning. What is the RANSAC algorithm anyway?
RANSAC
The acronym stands for Random sample consensus, basically we start with a random estimation line on a data cloud and we calculate how many inliers or data point are the closest to that line, considering a threshold T. The line that is closest to more datapoints is considered the best approximation given a max number of iterations (here’s a video if that wasn’t clear). One of the uses is to differentiate real data of sensor from noise in the system.
How does this relate to the paper? well for starters, as I showed the RANSAC algorithm can be modeled with a plane instead of a line. Second, our inliers can be points from the 3D point cloud, things are slowly falling into place. let’s take figure 3 (the point cloud) as an example, how would we know what is the floor. We can use a plane, imagine we use this plane like scanner from top to bottom, where do you think the plane will be close to a large number of data points? Near the floor! There’s a problem though, The way the algorithm is constructed It will estimate a plane randomly each time and we couldn’t know for sure even if we are going to find a plane that is parallel to the floor. This is why the authors introduced the modified algorithm. Since this article is running a bit long, and I only planned to introduce the idea, in the next one I’ll show more of the code.
References
[1] Z. Li, F. Song, B. C. Clark, D. R. Grooms, and C. Liu, “A Wearable Device for Indoor Imminent Danger Detection and Avoidance With Region-Based Ground Segmentation,” IEEE Access, vol. 8, pp. 184808–184821, 2020, doi: https://doi.org/10.1109/access.2020.3028527.
