python中多条直线的最近交点

下面是使用中描述的方法的numpy解决方案 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

编辑:这是一个噪声测试数据发生器

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

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]

如果你想从第一原理推导/检验这个等式,那么数学.stackexchange.com是个好问的地方。

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

这是我最后使用的代码。感谢kevinkayaks和其他所有回应的人!非常感谢您的帮助!!!

此函数的前半部分只是将两个点和角度集合转换为方向向量。我相信剩下的和埃里克和尤金的提议基本相同。我只是碰巧在凯文的电脑上取得了第一个成功,然后就开始使用它,直到它成为我的端到端解决方案。

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)