Constrained least-squares solver for 3D plane intersection

plane_intersection.py

"""
Constrained least-squares solver for finding the best intersection point
that lies on one of N planes while minimizing distance to all planes.
"""

import numpy as np


def _normalize_planes(N, d, eps=1e-12):
    """
    Normalize planes so each normal has unit length.

    Args:
        N: (m, 3) plane normals
        d: (m,) plane offsets
        eps: threshold for detecting zero-magnitude normals

    Returns:
        N_normalized: (m, 3)
        d_normalized: (m,)

    Raises:
        ValueError: if any normal has near-zero magnitude
    """
    norms = np.linalg.norm(N, axis=1)
    if np.any(norms < eps):
        raise ValueError("At least one plane has near-zero normal magnitude.")
    return N / norms[:, None], d / norms


def _constrained_least_squares_on_plane(N, d, a, b, ridge=1e-10):
    """
    Solve: min ||Nx - d||^2  subject to  a^T x = b

    Uses KKT conditions to form a 4x4 linear system.
    Adds small Tikhonov regularization for numerical stability when
    planes don't fully constrain 3D space (e.g., 1-2 planes).

    Args:
        N: (m, 3) normalized plane normals
        d: (m,) normalized plane offsets
        a: (3,) constraint normal (the plane we must lie on)
        b: scalar constraint offset
        ridge: small regularization for under-constrained systems

    Returns:
        x: (3,) solution point

    Raises:
        ValueError: if the KKT system is singular
    """
    H = 2.0 * (N.T @ N + ridge * np.eye(3))
    g = 2.0 * (N.T @ d)

    K = np.zeros((4, 4), dtype=float)
    K[:3, :3] = H
    K[:3, 3] = a
    K[3, :3] = a

    rhs = np.zeros(4, dtype=float)
    rhs[:3] = g
    rhs[3] = b

    # Check conditioning before solve
    cond = np.linalg.cond(K)
    if cond > 1e12:
        raise ValueError("KKT system is ill-conditioned (degenerate geometry).")

    try:
        sol = np.linalg.solve(K, rhs)
    except np.linalg.LinAlgError as e:
        raise ValueError(f"Failed to solve KKT system: {e}") from e

    return sol[:3]


def best_point_on_one_plane(N, d):
    """
    Find the point that lies on one of the planes and minimizes
    the total squared distance to all planes.

    Args:
        N: (m, 3) array of plane normals (need not be unit length)
        d: (m,) array of plane offsets (plane equation: n^T x = d)

    Returns:
        x_best: (3,) the optimal point
        plane_index: int, index of the plane the point lies on

    Raises:
        ValueError: if input is invalid or geometry is degenerate
    """
    N = np.asarray(N, dtype=float)
    d = np.asarray(d, dtype=float)

    if N.ndim != 2 or N.shape[1] != 3:
        raise ValueError(f"N must have shape (m, 3), got {N.shape}")
    if d.ndim != 1 or d.shape[0] != N.shape[0]:
        raise ValueError(f"d must have shape ({N.shape[0]},), got {d.shape}")
    if N.shape[0] < 1:
        raise ValueError("Need at least one plane.")

    N_norm, d_norm = _normalize_planes(N, d)

    candidates = []
    scores = []

    for i in range(N_norm.shape[0]):
        a = N_norm[i]
        b = d_norm[i]
        x_i = _constrained_least_squares_on_plane(N_norm, d_norm, a, b)

        residual = N_norm @ x_i - d_norm
        sse = float(residual @ residual)

        candidates.append(x_i)
        scores.append(sse)

    best_idx = int(np.argmin(scores))
    return candidates[best_idx], best_idx