ROS2 Point Cloud Floor Plane Segmentation

Problem Statement

Implement ransac_floor(pcd, max_iters, distance_thresh, min_inliers) that segments the dominant floor plane from a 3D point cloud.

Function Signature

def ransac_floor(
    pcd: np.ndarray,             # Nx3 array of (x, y, z) points
    max_iters: int = 100,
    distance_thresh: float = 0.02,
    min_inliers: int = 10
) -> tuple[np.ndarray, np.ndarray, tuple]:

Returns

(floor_indices, nonfloor_indices, plane_model) where:

• floor_indices: 1D integer array of indices into pcd that are on the floor
• nonfloor_indices: 1D integer array of remaining point indices
• plane_model: tuple (a, b, c, d) where ax + by + cz + d = 0 and the normal (a,b,c) is unit length

Algorithm

For each iteration:

1. Sample 3 random points from pcd
2. Fit a plane through those 3 points:
   • Compute two edge vectors: v1 = p1 - p0, v2 = p2 - p0
   • Normal: n = cross(v1, v2)
   • Skip this iteration if |n| < 1e-6 (collinear points)
   • Normalise: n = n / |n|
   • d = -dot(n, p0)
3. Count inliers: points where |ax + by + cz + d| < distance_thresh
4. Keep the model if it has more inliers than the current best

After all iterations:

• If best model has ≥ min_inliers inliers, return those indices as floor
• Otherwise return (np.array([]), np.arange(len(pcd)), (0,0,1,0))

🤖 Why This Matters in Real Robots

This is exactly what pcl::SACSegmentation with SACMODEL_PLANE does internally in the PCL library — the C++ library used in every industrial robot perception pipeline. Understanding the algorithm from scratch lets you tune it (iterations, threshold) and debug cases where it fails (multi-layer floors, reflective surfaces).