def __detect_intersections_between_clusters(self, cluster_index_i: int,
cluster_index_j: int) -> None:
"""Detects the intersections between cluster `cluster_index_i` and cluster `cluster_index_j`.
:param cluster_index_i: Index of first segment cluster to be intersected.
:param cluster_index_j: Index of second segment cluster to be intersected.
"""
cluster_i = self.segments[cluster_index_i]
cluster_j = self.segments[cluster_index_j]
print((cluster_index_i, cluster_index_j),
(len(cluster_i), len(cluster_j)))
self.cluster_cluster_intersections[cluster_index_i,
cluster_index_j] = {}
cluster_cluster_intersections_raw = []
for i, segment_i in enumerate(cluster_i):
intersections_with_i = {}
angle_i = utils.angle_x(segment_i[0:2], segment_i[2:4])
for j, segment_j in enumerate(cluster_j):
print((segment_i, segment_j))
angle_j = utils.angle_x(segment_j[0:2], segment_j[2:4])
d_angle = utils.angle_diff(angle_i, angle_j)
if np.pi / 2 - d_angle > self.params.angle_threshold:
continue
intersection = utils.segment_segment_intersection(
seg1=segment_i, seg2=segment_j)
if intersection is not False:
intersections_with_i[j] = intersection
cluster_cluster_intersections_raw.append(intersection)
self.cluster_cluster_intersections[
cluster_index_i, cluster_index_j][i] = intersections_with_i
if cluster_index_j >= cluster_index_i:
self.raw_intersections.extend(cluster_cluster_intersections_raw)
def connect(self, p1, p2):
"""Connects two positions in the graph, adding nodes and edges as needed"""
p1 = tuple(p1)
p2 = tuple(p2)
v1 = Vec2d(*p1)
v2 = Vec2d(*p2)
if p1 not in self.graph:
self.graph[p1] = {}
if p2 not in self.graph:
self.graph[p2] = {}
diff = v2 - v1
cost = diff.length() / self.speed
angle = diff.angle()
rev_angle = angle_diff(angle, math.pi)
self.node(p1, angle).connect(cost, self.node(p2, angle))
self.node(p2, rev_angle).connect(cost, self.node(p1, rev_angle))
def __clean_clusters_angle(self, mean_angles: np.ndarray,
max_angle_variation_mean: float) -> None:
"""Removes all segments whose angle deviates more than the passed parameter from the mean cluster angle.
:param mean_angles: List of cluster means.
:param max_angle_variation_mean: Maximal angle variation to allow between the cluster segments and the associated mean angle.
"""
for cluster_index, (cluster, mean_angle) in enumerate(
zip(self.cluster_list, mean_angles)):
invalid_indices = []
for segment_index, segment in enumerate(cluster):
# Retrieve angle in [0, pi] of current segment.
angle = utils.angle_x(segment[0:2], segment[2:4])
# Compute angle difference between current segment and mean angle of cluster.
d_angle = utils.angle_diff(angle, mean_angle)
if d_angle > max_angle_variation_mean:
invalid_indices.append(segment_index)
self.cluster_list[cluster_index] = np.delete(cluster,
invalid_indices,
axis=0)
def node(self, position, angle):
position = tuple(position)
if position not in self.graph:
edges = self.graph[position] = {}
else:
edges = self.graph[position]
if angle in edges:
node = edges[angle]
else:
node = Node()
node.position = position
node.angle = angle
node.est_cost_there = 0 # TODO
for a, n in edges.items():
if angle is None or a is None:
cost = 0
else:
cost = abs(angle_diff(a, angle)) / self.turning_speed
n.connect(cost, node)
node.connect(cost, n)
edges[angle] = node
return node
