ближайшая точка пересечения со многими строками в питоне

Question

Мне нужен хороший алгоритм для вычисления точки, ближайшей к набору линий в python, предпочтительно с использованием метода наименьших квадратов. Я нашел этот пост о реализации Python, которая не работает:

Нахождение центра нескольких линий с использованием метода наименьших квадратов в 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. ]])

углы, принадлежащие первому набору линий

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

начальные точки 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

Вот примерное решение с использованием метода, описанного в по этой ссылке

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

Answer 2

Вот последний код, который я в итоге использовал. Спасибо 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)

Answer 3

Если , это уравнение Википедии имеет любой вес:

тогда вы можете использовать:

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.

Конечно, это часть NumPy

Обратите внимание, что numpy дает вам достаточно инструментов, чтобы выразить это очень кратко уже