[Python] Issue with Calculating Normal Vectors to Planes and Angle Between Two Planes

I am encountering an issue with finding the normal vectors of two planes and calculating the angle between these two planes. I am using the RANSAC method to fit the planes to the given points, but the normal vectors I obtain do not appear to be perpendicular to the planes. Additionally, the calculated angle between the two planes is not as expected.

Context

I have generated two perfect matrices to represent two perpendicular planes. Here are the matrices:

left_points_measured = np.array([
[0.0, 0.0, 292.89321881],
[0.0, 1.0, 352.06658487],
[0.0, 2.0, 411.23995092],
[1.0, 0.0, 292.89321881],
[1.0, 1.0, 352.06658487],
[1.0, 2.0, 411.23995092],
[2.0, 0.0, 292.89321881],
[2.0, 1.0, 352.06658487],
[2.0, 2.0, 411.23995092],
[3.0, 0.0, 292.89321881],
[3.0, 1.0, 352.06658487],
[3.0, 2.0, 411.23995092],
[4.0, 0.0, 292.89321881],
[4.0, 1.0, 352.06658487],
[4.0, 2.0, 411.23995092],
[5.0, 0.0, 292.89321881],
[5.0, 1.0, 352.06658487],
[5.0, 2.0, 411.23995092],
[6.0, 0.0, 292.89321881],
[6.0, 1.0, 352.06658487],
[6.0, 2.0, 411.23995092],
[7.0, 0.0, 292.89321881],
[7.0, 1.0, 352.06658487],
[7.0, 2.0, 411.23995092]
])

right_points_measured = np.array([
[0.0, 4.0, 470.41331697],
[0.0, 5.0, 411.23995092],
[0.0, 6.0, 352.06658487],
[0.0, 7.0, 292.89321881],
[1.0, 4.0, 470.41331697],
[1.0, 5.0, 411.23995092],
[1.0, 6.0, 352.06658487],
[1.0, 7.0, 292.89321881],
[2.0, 4.0, 470.41331697],
[2.0, 5.0, 411.23995092],
[2.0, 6.0, 352.06658487],
[2.0, 7.0, 292.89321881],
[3.0, 4.0, 470.41331697],
[3.0, 5.0, 411.23995092],
[3.0, 6.0, 352.06658487],
[3.0, 7.0, 292.89321881],
[4.0, 4.0, 470.41331697],
[4.0, 5.0, 411.23995092],
[4.0, 6.0, 352.06658487],
[4.0, 7.0, 292.89321881],
[5.0, 4.0, 470.41331697],
[5.0, 5.0, 411.23995092],
[5.0, 6.0, 352.06658487],
[5.0, 7.0, 292.89321881],
[6.0, 4.0, 470.41331697],
[6.0, 5.0, 411.23995092],
[6.0, 6.0, 352.06658487],
[6.0, 7.0, 292.89321881],
[7.0, 4.0, 470.41331697],
[7.0, 5.0, 411.23995092],
[7.0, 6.0, 352.06658487],
[7.0, 7.0, 292.89321881]
])


Method Used

I am using the RANSAC method to fit the planes to the given points and calculate the normal vectors. Here is the code I am using:

import numpy as np
import plotly.graph_objs as go
import webbrowser
from sklearn.linear_model import RANSACRegressor
from sklearn.linear_model import LinearRegression

def fit_plane_and_plot(points, name):
# Extract x, y, z coordinates
X = points[:, :2] # (x, y) coordinates
z = points[:, 2] # z coordinate

# Use RANSAC to fit a plane
ransac = RANSACRegressor(LinearRegression())
ransac.fit(X, z)

# Coefficients of the plane: z = ax + by + c
a, b = ransac.estimator_.coef_
c = ransac.estimator_.intercept_

# Normal vector to the plane
normal_vector = np.array([a, b, -1])

# Choose a point on the plane (e.g., x = 3.5, y = 3.5)
x, y = 3.5, 3.5
z = a * x + b * y + c
point_on_plane = np.array([x, y, z])

# Normal line to the plane: r(t) = p0 + t * normal_vector
def normal_line(t):
return point_on_plane + t * normal_vector

# Test for some t values
t_values = np.linspace(0, 1, 50)
line_points = np.array([normal_line(t) for t in t_values])

# Create grids for plane visualization
x_range = np.linspace(0, 7, 10) # Range for x
y_range = np.linspace(0, 7, 10) # Range for y
x_grid, y_grid = np.meshgrid(x_range, y_range)

# Plane
z_plane = a * x_grid + b * y_grid + c

return x_grid, y_grid, z_plane, line_points, points, name, normal_vector

def plot_planes_and_normals(left_points, right_points):
# Use plotly for an interactive plot
fig = go.Figure()

# Add planes and normal vectors for left and right points
x_grid_left, y_grid_left, z_plane_left, line_points_left, points_left, name_left, normal_vector_left = fit_plane_and_plot(left_points, "Left Plane")
x_grid_right, y_grid_right, z_plane_right, line_points_right, points_right, name_right, normal_vector_right = fit_plane_and_plot(right_points, "Right Plane")

# Plot the planes
fig.add_trace(go.Surface(
x=x_grid_left,
y=y_grid_left,
z=z_plane_left,
colorscale='Blues',
opacity=0.5,
name=name_left
))

fig.add_trace(go.Surface(
x=x_grid_right,
y=y_grid_right,
z=z_plane_right,
colorscale='Reds',
opacity=0.5,
name=name_right
))

# Plot the normal lines
fig.add_trace(go.Scatter3d(
x=line_points_left[:, 0],
y=line_points_left[:, 1],
z=line_points_left[:, 2],
mode='lines',
line=dict(color='blue', width=2),
name='Left Normal Vector'
))

fig.add_trace(go.Scatter3d(
x=line_points_right[:, 0],
y=line_points_right[:, 1],
z=line_points_right[:, 2],
mode='lines',
line=dict(color='red', width=2),
name='Right Normal Vector'
))

# Plot the points
fig.add_trace(go.Scatter3d(
x=points_left[:, 0],
y=points_left[:, 1],
z=points_left[:, 2],
mode='markers',
marker=dict(color='green', size=4),
name='Left Points'
))

fig.add_trace(go.Scatter3d(
x=points_right[:, 0],
y=points_right[:, 1],
z=points_right[:, 2],
mode='markers',
marker=dict(color='purple', size=4),
name='Right Points'
))

# Add labels and legend
fig.update_layout(scene=dict(
xaxis=dict(title='X'),
yaxis=dict(title='Y', range=[0, 7]), # Set Y-axis limits
zaxis=dict(title='Z')
), title='Left and Right Planes and Normal Vectors')

# Save the figure to an HTML file and open it in a browser
file_path = 'plot_superposition.html'
fig.write_html(file_path)
webbrowser.open(file_path)

# Calculate the orientation angles of the planes
def calculate_angles(normal_vector):
norm = np.linalg.norm(normal_vector)
alpha = np.arccos(normal_vector[0] / norm)
beta = np.arccos(normal_vector[1] / norm)
gamma = np.arccos(normal_vector[2] / norm)
return np.degrees(alpha), np.degrees(beta), np.degrees(gamma)

alpha_left, beta_left, gamma_left = calculate_angles(normal_vector_left)
alpha_right, beta_right, gamma_right = calculate_angles(normal_vector_right)

print(f"Angles for the left plane:")
print(f"Angle with x-axis (alpha): {alpha_left:.2f} degrees")
print(f"Angle with y-axis (beta): {beta_left:.2f} degrees")
print(f"Angle with z-axis (gamma): {gamma_left:.2f} degrees")

print(f"Angles for the right plane:")
print(f"Angle with x-axis (alpha): {alpha_right:.2f} degrees")
print(f"Angle with y-axis (beta): {beta_right:.2f} degrees")
print(f"Angle with z-axis (gamma): {gamma_right:.2f} degrees")

# Calculate the angle between the two planes
dot_product = np.dot(normal_vector_left, normal_vector_right)
norm_left = np.linalg.norm(normal_vector_left)
norm_right = np.linalg.norm(normal_vector_right)
cos_angle = dot_product / (norm_left * norm_right)
angle = np.arccos(np.clip(cos_angle, -1.0, 1.0))
angle_deg = np.degrees(angle)

print(f"Angle between the two planes: {angle_deg:.2f} degrees")

# Call the function to plot the planes and normal vectors
plot_planes_and_normals(left_points_measured, right_points_measured)


Problem

When I plot the planes and the normal vectors, I expect the normal vectors to be perpendicular to the planes. However, the plotted normal vectors are not. Additionally, the calculated angle between the two planes is around 170 degrees, whereas I expect an angle of 90 degrees since the planes are supposed to be perpendicular.

Question

Can you help me understand why the normal vectors are not perpendicular to the planes and why the calculated angle is not as expected? Is there an error in my method of calculating the normal vectors or in the calculation of the angle between the planes?

Example

I tried to inverse the matrix

Continue reading...