python - 寻找交叉点基于区域的轨迹与线轨迹

Question

我有两个轨迹(即两个点列表)，我试图找到这两个轨迹的交点。但是，如果我将这些轨迹表示为线，我可能会错过现实世界的交叉点(只是错过)。

我想做的是将线表示为围绕点具有一定宽度的多边形，然后找到两个多边形彼此相交的位置。

我正在使用 python 空间库，但我想知道以前是否有人这样做过。这是不相交的线段的图片，因为它们只是错过了彼此。下面是表示两个对象轨迹的示例数据代码。

object_trajectory=np.array([[-3370.00427248,  3701.46800775],
   [-3363.69164715,  3702.21408203],
   [-3356.31277271,  3703.06477984],
   [-3347.25951787,  3704.10740164],
   [-3336.739511  ,  3705.3958357 ],
   [-3326.29355823,  3706.78035903],
   [-3313.4987339 ,  3708.2076586 ],
   [-3299.53433345,  3709.72507366],
   [-3283.15486406,  3711.47077376],
   [-3269.23487255,  3713.05635557]])
target_trajectory=np.array([[-3384.99966703,  3696.41922372],
   [-3382.43687562,  3696.6739521 ],
   [-3378.22995178,  3697.08802862],
   [-3371.98983789,  3697.71490469],
   [-3363.5900481 ,  3698.62666805],
   [-3354.28520354,  3699.67613798],
   [-3342.18581931,  3701.04853915],
   [-3328.51519511,  3702.57528111],
   [-3312.09691577,  3704.41961271],
   [-3297.85543763,  3706.00878621]])
plt.plot(object_trajectory[:,0],object_trajectory[:,1],'b',color='b')
plt.plot(vehicle_trajectory[:,0],vehicle_trajectory[:,1],'b',color='r')

Answer 1

假设您有两行由 numpy 数组 x1、y1、x2 和 y2 定义。

import numpy as np

您可以创建一个数组 distances[i, j]，其中包含第一行中第 i 个点与第 j 个点之间的距离第二行的第 th 个点。

distances = ((x1[:, None] - x2[None, :])**2 + (y1[:, None] - y2[None, :])**2)**0.5

然后您可以找到 distances 小于您要为交集定义的某个阈值的索引。如果您认为线条具有一定的厚度，则阈值将是该厚度的一半。

threshold = 0.1
intersections = np.argwhere(distances < threshold)

intersections 现在是一个 N×2 数组，包含所有被认为是“相交”的点对([i, 0] 是第一个点的索引行，[i, 1] 是第二行的索引)。如果您想从相交的每条线上获取所有索引的集合，您可以使用类似

first_intersection_indices = np.asarray(sorted(set(intersections[:, 0])))
second_intersection_indices = np.asarray(sorted(set(intersections[:, 1])))

从这里，您还可以通过仅取每个列表中任何连续值的中心值来确定有多少个交叉点。

L1 = []
current_intersection = []
for i in range(first_intersection_indices.shape[0]):
    if len(current_intersection) == 0:
        current_intersection.append(first_intersection_indices[i])
    elif first_intersection_indices[i] == current_intersection[-1]:
        current_intersection.append(first_intersection_indices[i])
    else:
        L1.append(int(np.median(current_intersection)))
        current_intersection = [first_intersection_indices[i]]
print(len(L1))

您可以使用这些来打印每个交叉点的坐标。

for i in L1:
    print(x1[i], y1[i])