寻找交叉口区域基于轨迹与线轨迹

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点之间的距离。

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

Answer 2

事实证明，这款造型优美的套装已经拥有大量的便利功能，让我对此非常感兴趣。

from shapely.geometry import Point, LineString, MultiPoint
# I assume that self.line is of type LineString (i.e. a line trajectory)
region_polygon = self.line.buffer(self.lane_width)
# line.buffer essentially generates a nice interpolated bounding polygon around the trajectory.
# Now we can identify all the other points in the other trajectory that intersects with the region_polygon that we just generated. You can also use .intersection if you want to simply generate two polygon trajectories and find the intersecting polygon as well.
is_in_region = [region_polygon.intersects(point) for point in points]