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