def detect_indexes_on_lane_points_for_distance_marks( self, mark_count, start_dist, dist_gap): initial_steer = self.initial_steer curve_length = self.curve_length c, lw, rw = get_car_path(initial_steer, distance=curve_length) lw = self.add_3rd_dim(lw) def create_horizontal_line_at_depth(distance_from_camera, left_limit=-CAR_T / 2, right_limit=CAR_T / 2, n=2): x = np.expand_dims(np.linspace(left_limit, right_limit, num=n), axis=1) y = np.ones((n, 1)) # * CAMERA_HEIGHT - TODO some hardcoded value z = np.ones((n, 1)) * distance_from_camera xy = np.concatenate((x, y), axis=1) xyz = np.concatenate((xy, z), axis=1) return xyz def get_idx_closest_point_to(points, point): dists = list(map(lambda x: np.linalg.norm(x - point), points)) min_idx = dists.index(min(dists)) return min_idx indexes = [] for dist in np.arange(start_dist, start_dist + mark_count * dist_gap, dist_gap): line_at_dist = create_horizontal_line_at_depth(dist) indexes.append(get_idx_closest_point_to(lw, line_at_dist[0])) return indexes
def get_car_path(self, steer_factor, distance=20): """ :param steer_factor: [-1, 1] (max left max right) :return: """ num_points = self.num_points idx = np.clip(int(num_points/2. * steer_factor + num_points/2.), 0, num_points-1) r = get_radius(self.angles[idx]) c, lw, rw = get_car_path(r, distance=distance) return c, lw, rw
def render(self, image, radius): mark_count = self.mark_count start_dist = self.start_dist gap_dist = self.gap_dist curve_length = self.curve_length center_color = self.center_color center_width = self.center_width side_color = self.side_color side_width = self.side_width indexes = self.detect_indexes_on_lane_points_for_distance_marks( mark_count, start_dist, gap_dist) c, lw, rw = get_car_path(radius, distance=curve_length) # print(lw[2:]) c = self.add_3rd_dim(c) lw = self.add_3rd_dim(lw) rw = self.add_3rd_dim(rw) image = self.render_line(image, c, color=center_color, thickness=center_width) image = self.render_line(image, lw, color=side_color, thickness=side_width) image = self.render_line(image, rw, color=side_color, thickness=side_width) # for index in indexes: # image = self.render_line(image, np.array([lw[index], rw[index]]), color=side_color, # thickness=side_width) # Render Distance lines for distance in self.distance_mark: c, lw, rw = get_car_line_mark(radius, distance) line = np.array([lw, rw]) line = self.add_3rd_dim(line) # line = self.add_3rd_dim(np.array([[10, -1], [10, 1]])) # print(line) image = self.render_line(image, line, color=(0, 0, 255), thickness=side_width, filter_nonmonotonic=False, filter_negative=False) return image
def render(self, image, radius): mark_count = self.mark_count start_dist = self.start_dist gap_dist = self.gap_dist curve_length = self.curve_length center_color = self.center_color center_width = self.center_width side_color = self.side_color side_width = self.side_width indexes = self.detect_indexes_on_lane_points_for_distance_marks(mark_count, start_dist, gap_dist) c, lw, rw = get_car_path(radius, distance=curve_length) # print(lw[2:]) c = self.add_3rd_dim(c) lw = self.add_3rd_dim(lw) rw = self.add_3rd_dim(rw) if SUPPORT_LINES: x_factor = 1 #stanga dreapta y_factor = -1# sus jos z_factor = 1 #fata spate wheel_point_y = z_factor * (2.634 + 3) wheel_point_X = x_factor * (0.50874 + 0.37776 + 0.) corner_y = z_factor * self.distance_mark corner_x = wheel_point_X end_y = corner_y end_x = -x_factor * 20 corner_vertical_z = y_factor * 10 center_car_point_y = z_factor * (2.634 + 3) center_car_point_x = 0 center_border_y = z_factor * self.distance_mark center_border_x = 0 support_lw = [center_car_point_x, center_car_point_y] support_rw = [center_border_x, center_border_y] line = np.array([support_lw, support_rw]) line = self.add_3rd_dim(line) # print("center line", line) image = self.render_line(image, line, color=(0, 0, 255), thickness=side_width, filter_nonmonotonic=False, filter_negative=False) support_lw = [wheel_point_X, wheel_point_y] support_rw = [corner_x, corner_y] line = np.array([support_lw, support_rw]) line = self.add_3rd_dim(line) # print("border line", line) image = self.render_line(image, line, color=(0, 0, 255), thickness=side_width, filter_nonmonotonic=False, filter_negative=False) support_lw = [corner_x, corner_y] support_rw = [end_x, end_y] line = np.array([support_lw, support_rw]) line = self.add_3rd_dim(line) # print("front line", line) image = self.render_line(image, line, color=(0, 0, 255), thickness=side_width, filter_nonmonotonic=False, filter_negative=False) support_lw = [corner_x, 0, corner_y] support_rw = [corner_x, corner_vertical_z, corner_y] line = np.array([support_lw, support_rw]) line = self.add_3rd_dim(line, height_provided=True) # print("vertical line", line) image = self.render_line(image, line, color=(0, 0, 255), thickness=side_width, filter_nonmonotonic=False, filter_negative=False) # w, h, _ = image.shape # w2 = w // 2 # h2 = h // 2 # image[w2 - 2:w2 + 2, :] = (255, 0, 0) # image[:, h2 - 2:h2 + 2] = (255, 0, 0) return image