平面法向量计算及两平面夹角计算异常问题排查

我现在遇到了一个问题：在求两个平面的法向量并计算两平面夹角时，用RANSAC方法拟合平面得到的法向量看起来并不垂直于平面，而且计算出的两平面夹角也和预期不符。

上下文 #

我生成了两个完美的矩阵来代表两个垂直的平面，矩阵如下：

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]
])

使用方法 #

我用RANSAC方法拟合平面并计算法向量，代码如下：

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)

问题 #

当我绘制平面和法向量时，预期法向量应该垂直于平面，但实际绘制出来的法向量并不垂直。另外，计算出的两平面夹角大约是170度，而我预期应该是90度，因为这两个平面本来是垂直的。

疑问 #

你能帮我理解为什么法向量不垂直于平面，以及为什么计算出的夹角不符合预期吗？是我计算法向量的方法有错误，还是计算两平面夹角的步骤出了问题？

我还尝试过反转矩阵，但也没有解决问题。

备注：内容来源于stack exchange，提问作者JPannetier