python中多条直线的最近交点

Question

我需要一个好的算法来计算最接近python中的一组线的点，最好使用最小二乘法。我在一个不能工作的python实现上找到了这篇文章：

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

我在Matlab里找到了大家都喜欢的资源。。。但我不知道如何将其转换为python：

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

我觉得很难相信有人还没有这么做。。。当然，这是numpy的一部分或是一个标准包，对吧？我可能只是找不到合适的术语，但我还没找到。我可以用两个点或者一个点和一个方向来定义线。任何帮助将不胜感激！在

下面是我正在使用的一组示例点：

第一组直线的初始XYZ点

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

属于第一组直线的角度

^{pr2}$

第二组直线的初始XYZ点

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

属于第二组线的角度

[89.13244, 92.39087, 94.86425, 98.91849, 99.83488]

解决方案应该是原点或者离原点很近（数据只是有点嘈杂，这就是为什么直线不能在一个点上完全相交）。在

Answer 1

这是我最后使用的代码。感谢kevinkayaks和其他回应的人！非常感谢你的帮助！！！在

此函数的前半部分只是将点和角度的两个集合转换为方向向量。我相信剩下的基本上和埃里克和尤金的提议是一样的。我只是碰巧在Kevin's上取得了成功，并一直坚持到它对我来说是一个端到端的解决方案。在

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)

Answer 2

如果this wikipedia equation有任何重量：

然后您可以使用：

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]

如果你想从第一原理推导/检验这个等式，那么math.stackexchange.com网站会是一个更好的问的地方。在

surely this is part of numpy

请注意，numpy已经为您提供了足够的工具来非常简洁地表达这一点

Answer 3

下面是一个使用this link中描述的方法的numpy解决方案

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

工程

编辑：这里有一个噪声测试数据发生器

^{pr2}$