Step 2.A : Calculating angle between the plane normal and the axis

In the last Article, we saw a problem with the RANSAC algorithm because we need to ensure that the estimated plane is parallel to the ground. From the image below we can see the orientation of the point cloud. So now we can delimit our problem to the following. We need to ensure that the angle between the y-axis and the normal vector of the plane is either 0° or within an acceptable threshold which in fact the authors did detail in their research as a maximum of 5 ° [1]

Fig 1. point cloud generation from a depth image

Fig 2. table of configurations for the modified algorithm

so for the whole RANSAC algorithm, we need to start with a couple of basic functions. First how to sample the random plane, how is it generated. Then we need to check that it’s within the threshold.

Generating the plane

This part is quite simple actually. We need to generate the coefficients of the plane and also we need the normal vector of said plane. Now if you recall your pre-calc classes or physics, we can use 3 points to form 2 vectors. Let’s observe fig 3, we can form two vectors LP and LM, with this all that is left to do is the cross product between these vectors to get a vector which is orthogonal to both vectors, giving us the normal vector to our plane.

Fig 3. Estimating a plane from 3 random points

To understand better here’s an example

Now the code to get the plane from 3 random points.

import numpy as np

def calculate_plane_coefficients(point1, point2, point3):
    # Create vectors between the points
    vector1 = (np.array(point1) - np.array(point2)).astype(float)
    vector2 = (np.array(point3) - np.array(point2)).astype(float)
    # Calculate the normal vector to the plane using cross product
    normal_vector = np.cross(vector1, vector2)
    # Normalize the normal vector
    normal_vector /= np.linalg.norm(normal_vector)
    # # Calculate the constant term D in the plane equation Ax + By + Cz + D = 0
    D = -np.dot(normal_vector, point1)
    
    # Coefficients [A, B, C, D] of the plane equation
    plane_coefficients = np.concatenate((normal_vector, [D]))
    
    return normal_vector, plane_coefficients

# Example usage
point1 = [4, -2, 7]
point2 = [2, 1, 4]
point3 = [5, 3, -2]

normal_vector, plane_coefficients = calculate_plane_coefficients(point1, point2, point3)
print("Normal Vector:", normal_vector)
print("Plane Coefficients:", plane_coefficients)

Now to monitor our threshold of 5° we can simply calculate the angle between the normal vector of the plane and the y-axis (0,1,0). We can do this with the following equation

def angle_between_vectors(vector1, vector2):
    dot_product = np.dot(vector1, vector2)
    magnitudes = np.linalg.norm(vector1) * np.linalg.norm(vector2)
    return np.arccos(dot_product / magnitudes)

Understanding these two simple but important functions will be helpful to grasp the idea of the modified RANSAC algorithm. In the next article I will present the code that segments the floor by combining these last two steps.

Reference

[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.