Fit a line in polar coordinates (Hough space)

PolarLine.py

import numpy as np
import matplotlib.pyplot as plt

def fitLinePolar(x, y, x_ref, y_ref):
    """
    Fits a line to the given data points using the polar (Hough) formulation.
    The data is centered at (x_ref, y_ref).

    Parameters:
        x (np.ndarray): Array of x data points.
        y (np.ndarray): Array of y data points.
        x_ref (float): Reference x-coordinate (center of the coordinate system).
        y_ref (float): Reference y-coordinate (center of the coordinate system).

    Returns:
        rho (float): Distance from the reference point to the line.
        theta (float): Angle from the x-axis to the normal vector of the line (in radians).
    """
    # Center the data by subtracting the reference point
    x_centered = x - x_ref
    y_centered = y - y_ref

    # Stack the centered data for covariance computation
    data = np.vstack((x_centered, y_centered))

    # Compute the covariance matrix
    covariance_matrix = np.cov(data)

    # Compute eigenvalues and eigenvectors
    eigenvalues, eigenvectors = np.linalg.eig(covariance_matrix)

    # Find the eigenvector corresponding to the smallest eigenvalue (normal to the line)
    idx = np.argmin(eigenvalues)
    normal = eigenvectors[:, idx]

    # Ensure the normal vector points upwards
    if normal[1] < 0:
        normal = -normal

    # Compute theta (angle from the x-axis to the normal vector)
    theta = np.arctan2(normal[1], normal[0])

    # Compute rho (mean projection of centered data onto the normal vector)
    rho = np.mean(x_centered * normal[0] + y_centered * normal[1])

    return rho, theta

def polarLine(x, rho, theta, x_ref, y_ref):
    """
    Computes y values for the fitted line at given x positions using the polar line equation.

    Parameters:
        x (np.ndarray): Array of x data points.
        rho (float): Distance from the reference point to the line.
        theta (float): Angle from the x-axis to the normal vector of the line (in radians).
        x_ref (float): Reference x-coordinate (center of the coordinate system).
        y_ref (float): Reference y-coordinate (center of the coordinate system).

    Returns:
        y_fit (np.ndarray): Array of y values corresponding to x on the fitted line.
    """
    # Compute y values using the line equation in polar coordinates
    cos_theta = np.cos(theta)
    sin_theta = np.sin(theta)

    # Handle case when sin_theta is close to zero
    if np.abs(sin_theta) > 1e-6:
        y_fit = y_ref + (rho - (x - x_ref) * cos_theta) / sin_theta
    else:
        # Line is horizontal, y = constant
        y_fit = np.full_like(x, y_ref + rho / sin_theta)

    return y_fit




def polarToClassical(rho, theta, x_ref, y_ref):
    """
    Converts polar line parameters to classical line parameters y = m * x + k.

    Parameters:
        rho (float): Distance from the reference point to the line.
        theta (float): Angle from the x-axis to the normal vector (in radians).
        x_ref (float): Reference x-coordinate.
        y_ref (float): Reference y-coordinate.

    Returns:
        m (float): Slope of the line.
        k (float): Intercept of the line.
    """
    cos_theta = np.cos(theta)
    sin_theta = np.sin(theta)
    
    # Check for vertical line (sin_theta close to zero)
    if np.abs(sin_theta) > 1e-6:
        m = -cos_theta / sin_theta
        k = (rho - x_ref * cos_theta - y_ref * sin_theta) / sin_theta + y_ref
    else:
        m = np.inf  # Slope is infinite (vertical line)
        k = None    # Intercept is undefined
    
    return m, k

def classicalToPolar(m, k, x_ref, y_ref):
    """
    Converts classical line parameters y = m * x + k to polar line parameters.

    Parameters:
        m (float): Slope of the line.
        k (float): Intercept of the line.
        x_ref (float): Reference x-coordinate.
        y_ref (float): Reference y-coordinate.

    Returns:
        rho (float): Distance from the reference point to the line.
        theta (float): Angle from the x-axis to the normal vector (in radians).
    """

    # Handle vertical line case
    if np.isinf(m):
        theta = 0  # Normal vector points along x-axis
        rho = (x_ref - (k if k is not None else 0)) * np.cos(theta) + (y_ref) * np.sin(theta)

    else:
        # Normal vector components
        normal = np.array([-m, 1])
        norm = np.linalg.norm(normal)
        normal_unit = normal / norm

        # Angle of the normal vector
        theta = np.arctan2(normal_unit[1], normal_unit[0])

        # Any point on the line (e.g., x = 0, y = k)
        x_line = 0
        y_line = k

        # Compute rho using the reference point
        dx = x_line - x_ref
        dy = y_line - y_ref
        rho = dx * normal_unit[0] + dy * normal_unit[1]
    
    return rho, theta


# Main script
if __name__ == "__main__":

    # Step 1: Generate a line segment with some added noise

    # Original line parameters (slope and intercept)
    m_true = 0.5  # True slope
    k_true = 2    # True intercept

    # Generate x values
    x = np.linspace(50, 150, 100)

    # Generate y values with Gaussian noise
    noise = np.random.normal(0, 0.5, size=x.shape)
    y = m_true * x + k_true + noise

    # Define reference point (e.g., center of the image)
    x_ref = 100
    y_ref = 100

    ###


    # Step 2: Fit the line using the polar fitting function
    rho, theta = fitLinePolar(x, y, x_ref, y_ref)

    # Step 3: Evaluate the fit using the evaluation function
    y_fit = polarLine(x, rho, theta, x_ref, y_ref)


    ###


    # Print the original and fitted line parameters
    print(f"Original line: y = {m_true} * x + {k_true}")

    # Compute the original line parameters in polar coordinates
    rho_true, theta_true = classicalToPolar(m_true, k_true, x_ref, y_ref)
    print(f"Original line (polar): rho = {rho_true:.4f}, theta = {np.degrees(theta_true):.4f} degrees")


    # Compute the fitted line parameters in classical coordinates
    m_fit, k_fit = polarToClassical(rho, theta, x_ref, y_ref)

    print(f"Fitted line (polar): rho = {rho:.4f}, theta = {np.degrees(theta):.4f} degrees")

    if m_fit != np.inf:
        print(f"Fitted line (classical): y = {m_fit:.4f} * x + {k_fit:.4f}")
    else:
        print(f"Fitted line is vertical, x = {k_fit:.4f}")



    ### Make the plots


    # Plot the noisy data points
    plt.scatter(x, y, label='Noisy data points', color='blue')

    # Plot the reference point
    plt.plot(x_ref, y_ref, 'ro', label='Reference point')


    # Plot the fitted line
    plt.plot(x, y_fit, 'r-', label='Fitted line')

    # Add labels and legend
    plt.xlabel('x')
    plt.ylabel('y')
    plt.title('Line Fitting in Polar Coordinates')
    plt.legend()
    plt.grid(True)
    plt.show()