nearest intersection point to many lines in python

Question

I need a good algorithm for calculating the point that is closest to a collection of lines in python, preferably by using least squares. I found this post on a python implementation that doesn't work:

Finding the centre of multiple lines using least squares approach in Python

And I found this resource in Matlab that everyone seems to like... but I'm not sure how to convert it to python:

https://www.mathworks.com/matlabcentral/fileexchange/37192-intersection-point-of-lines-in-3d-space

I find it hard to believe that someone hasn't already done this... surely this is part of numpy or a standard package, right? I'm probably just not searching for the right terms - but I haven't been able to find it yet. I'd be fine with defining lines by two points each or by a point and a direction. Any help would be greatly appreciated!

Here's an example set of points that I'm working with:

initial XYZ points for the first set of lines

array([[-7.07107037,  7.07106748,  1. ],
       [-7.34818339,  6.78264559,  1. ],
       [-7.61352972,  6.48335745,  1. ],
       [-7.8667115 ,  6.17372055,  1. ],
       [-8.1072994 ,  5.85420065,  1. ]])

the angles that belong to the first set of lines

[-44.504854, -42.029223, -41.278573, -37.145774, -34.097022]

initial XYZ points for the second set of lines

array([[ 0., -20. ,  1. ],
       [ 7.99789129e-01, -19.9839984,  1. ],
       [ 1.59830153e+00, -19.9360366,  1. ],
       [ 2.39423914e+00, -19.8561769,  1. ],
       [ 3.18637019e+00, -19.7445510,  1. ]])

the angles that belong to the second set of lines

[89.13244, 92.39087, 94.86425, 98.91849, 99.83488]

The solution should be the origin or very near it (the data is just a little noisy, which is why the lines don't perfectly intersect at a single point).

Answer 1

Here's a numpy solution using the method described in this link

def intersect(P0,P1):
    """P0 and P1 are NxD arrays defining N lines.
    D is the dimension of the space. This function 
    returns the least squares intersection of the N
    lines from the system given by eq. 13 in 
    http://cal.cs.illinois.edu/~johannes/research/LS_line_intersect.pdf.
    """
    # generate all line direction vectors 
    n = (P1-P0)/np.linalg.norm(P1-P0,axis=1)[:,np.newaxis] # normalized

    # generate the array of all projectors 
    projs = np.eye(n.shape[1]) - n[:,:,np.newaxis]*n[:,np.newaxis]  # I - n*n.T
    # see fig. 1 

    # generate R matrix and q vector
    R = projs.sum(axis=0)
    q = (projs @ P0[:,:,np.newaxis]).sum(axis=0)

    # solve the least squares problem for the 
    # intersection point p: Rp = q
    p = np.linalg.lstsq(R,q,rcond=None)[0]

    return p

Works

Edit: here is a generator for noisy test data

n = 6
P0 = np.stack((np.array([5,5])+3*np.random.random(size=2) for i in range(n)))
a = np.linspace(0,2*np.pi,n)+np.random.random(size=n)*np.pi/5.0
P1 = np.array([5+5*np.sin(a),5+5*np.cos(a)]).T

Answer 2

If this wikipedia equation carries any weight:

then you can use:

def nearest_intersection(points, dirs):
    """
    :param points: (N, 3) array of points on the lines
    :param dirs: (N, 3) array of unit direction vectors
    :returns: (3,) array of intersection point
    """
    dirs_mat = dirs[:, :, np.newaxis] @ dirs[:, np.newaxis, :]
    points_mat = points[:, :, np.newaxis]
    I = np.eye(3)
    return np.linalg.lstsq(
        (I - dirs_mat).sum(axis=0),
        ((I - dirs_mat) @ points_mat).sum(axis=0),
        rcond=None
    )[0]

If you want help deriving / checking that equation from first principles, then math.stackexchange.com would be a better place to ask.

surely this is part of numpy

Note that numpy gives you enough tools to express this very concisely already

Answer 3

Here's the final code that I ended up using. Thanks to kevinkayaks and everyone else who responded! Your help is very much appreciated!!!

The first half of this function simply converts the two collections of points and angles to direction vectors. I believe the rest of it is basically the same as what Eric and Eugene proposed. I just happened to have success first with Kevin's and ran with it until it was an end-to-end solution for me.

import numpy as np

def LS_intersect(p0,a0,p1,a1):
    """
    :param p0 : Nx2 (x,y) position coordinates
    :param p1 : Nx2 (x,y) position coordinates
    :param a0 : angles in degrees for each point in p0
    :param a1 : angles in degrees for each point in p1    
    :return: least squares intersection point of N lines from eq. 13 in 
             http://cal.cs.illinois.edu/~johannes/research/LS_line_intersect.pdf
    """    

    ang = np.concatenate( (a0,a1) ) # create list of angles
    # create direction vectors with magnitude = 1
    n = []
    for a in ang:
        n.append([np.cos(np.radians(a)), np.sin(np.radians(a))])
    pos = np.concatenate((p0[:,0:2],p1[:,0:2])) # create list of points
    n = np.array(n)

    # generate the array of all projectors 
    nnT = np.array([np.outer(nn,nn) for nn in n ]) 
    ImnnT = np.eye(len(pos[0]))-nnT # orthocomplement projectors to n

    # now generate R matrix and q vector
    R = np.sum(ImnnT,axis=0)
    q = np.sum(np.array([np.dot(m,x) for m,x in zip(ImnnT,pos)]),axis=0)

    # and solve the least squares problem for the intersection point p 
    return np.linalg.lstsq(R,q,rcond=None)[0]


#sample data 
pa = np.array([[-7.07106638,  7.07106145,  1.        ],
       [-7.34817263,  6.78264524,  1.        ],
       [-7.61354115,  6.48336347,  1.        ],
       [-7.86671133,  6.17371816,  1.        ],
       [-8.10730426,  5.85419995,  1.        ]])
paa = [-44.504854321138524, -42.02922380123842, -41.27857390748773, -37.145774853341386, -34.097022454778674]

pb = np.array([[-8.98220431e-07, -1.99999962e+01,  1.00000000e+00],
       [ 7.99789129e-01, -1.99839984e+01,  1.00000000e+00],
       [ 1.59830153e+00, -1.99360366e+01,  1.00000000e+00],
       [ 2.39423914e+00, -1.98561769e+01,  1.00000000e+00],
       [ 3.18637019e+00, -1.97445510e+01,  1.00000000e+00]])
pba = [88.71923357743934, 92.55801427272372, 95.3038321024299, 96.50212060095349, 100.24177145619092]

print("Should return (-0.03211692,  0.14173216)")
solution = LS_intersect(pa,paa,pb,pba)
print(solution)