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