def is_line_plausible(self, left, right):
"""
determine if pixels describing two line are plausible lane lines based on curvature and distance.
:param left: Tuple of arrays containing the coordinates of detected pixels
:param right: Tuple of arrays containing the coordinates of detected pixels
:return:
"""
if len(left[0]) < 3 or len(right[0]) < 3:
return False
else:
new_left = Line(y=left[0], x=left[1])
new_right = Line(y=right[0], x=right[1])
return are_lanes_plausible(new_left, new_right)
def postprocess_lines(self):
lines = []
for points in self.lines:
for i in range(len(points) - 1):
x1 = points[i][0]
y1 = points[i][1]
x2 = points[i + 1][0]
y2 = points[i + 1][1]
lines += [Line(x1, y1, x2, y2)]
results = []
for y in range(64):
coords = []
for line in lines:
x = line.get_x(y)
if line.contains(x, y):
coords += [x]
results += [coords]
return results
from timeit import timeit
from utils.line import Line
from utils.sorter import radixSort
# Little test to check if algorithm is working
#intList = getRandomRepeatingListWithRange(15, 100)
#print(f"Initial: {intList}")
#radixSort(intList)
#print(f"Final: {intList}")
# Change this value to switch from and to test mode
testMode = False
testDivisor = 100
counts = [10000, 20000, 40000, 70000, 100000, 500000]
if testMode:
counts = [int(n / testDivisor) for n in counts]
numberOfTests = 1
randomLists = [getRandomList(count) for count in counts]
elapsedTimes = [
timeit(lambda: radixSort(list), number=numberOfTests)
for list in randomLists
]
lines = [Line((counts, elapsedTimes), "Caso aleatório", 'b')]
plot(lines, figname="products/radix_sort_graph.png")
from utils.plotter import plot from utils.generator import getRandomList, getDecrescentList from timeit import timeit from utils.line import Line from utils.sorter import insertionSort counts = [1000, 2000, 3000, 4000, 5000, 8000, 11000, 15000] randomLists = [getRandomList(count) for count in counts] worstLists = [getDecrescentList(count) for count in counts] elapsedTimes = [timeit(lambda: insertionSort(list), number = 1) for list in randomLists] worstElapsedTimes = [timeit(lambda: insertionSort(list), number = 1) for list in worstLists] lines = [Line((counts, elapsedTimes), "Caso aleatório", 'b'), Line((counts, worstElapsedTimes), "Pior caso", 'k')] plot(lines, figname = "products/insertion_sort_graph.png")
counts = [10000, 20000, 40000, 70000, 100000, 500000]
if testMode:
counts = [int(n / testDivisor) for n in counts]
numberOfTests = 1
randomLists = [getRandomList(count) for count in counts]
elapsedTimes = [
timeit(lambda: bucketSort(list), number=numberOfTests)
for list in randomLists
]
lines = [Line((counts, elapsedTimes), "Caso aleatório", 'b')]
plot(lines, "products/bucket_sort_graph.png")
bucketNumbers = [1, 10, 100, 1000, 10000, 100000]
bucketNumberTimes = [
timeit(lambda: bucketSort(getRandomList(100000), bucketNumber),
number=numberOfTests) for bucketNumber in bucketNumbers
]
lines = [
Line((bucketNumbers, bucketNumberTimes), "Lista com 100000 elementos", 'b')
]
plot(lines, 'products/bucket_number_graph.png', 'Número de buckets')
print(
"De acordo com as informações obtidas ao analisar o gráfico, sugiro uma quantidade de buckets igual ao tamanho da entrada, ou uma quantidade de buckets igual a 1% do tamanho da entrada."
)
def process_frame(self, frame):
"""
apply lane detection on a single image
:param frame: input frame
:return: processed frame
"""
orig_frame = np.copy(frame)
# undistort frame
frame = self.camera_calibrator.undistort(frame)
# apply sobel and color transforms to create a thresholded binary image.
frame = generate_lane_mask(frame, 400)
# apply perspective transform to get birds-eye view
frame = self.perspective_transformer.transform(frame)
left_detected = right_detected = False
left_x = left_y = right_x = right_y = []
# if lanes were detected in the past, algorithm will first try to find new lanes along the old one.
# this will improve performance
if self.left_line is not None and self.right_line is not None:
left_x, left_y = detect_lane_along_poly(frame, self.left_line.best_fit_poly, self.line_segments)
right_x, right_y = detect_lane_along_poly(frame, self.right_line.best_fit_poly, self.line_segments)
left_detected, right_detected = self.validate_lines(left_x, left_y, right_x, right_y)
# if no lanes were found a histogram search is performed
if not left_detected:
left_x, left_y = histogram_lane_detection(
frame, self.line_segments, (self.image_offset, frame.shape[1] // 2), h_window=7)
left_x, left_y = outlier_removal(left_x, left_y)
if not right_detected:
right_x, right_y = histogram_lane_detection(
frame, self.line_segments, (frame.shape[1] // 2, frame.shape[1] - self.image_offset), h_window=7)
right_x, right_y = outlier_removal(right_x, right_y)
if not left_detected or not right_detected:
left_detected, right_detected = self.validate_lines(left_x, left_y, right_x, right_y)
# updated left lane information
if left_detected:
# switch x and y since lines are almost vertical
if self.left_line is not None:
self.left_line.update(y=left_x, x=left_y)
else:
self.left_line = Line(self.n_frames, left_y, left_x)
# updated right lane information.
if right_detected:
# switch x and y since lines are almost vertical
if self.right_line is not None:
self.right_line.update(y=right_x, x=right_y)
else:
self.right_line = Line(self.n_frames, right_y, right_x)
# add calculated information onto the frame
if self.left_line is not None and self.right_line is not None:
self.dists.append(self.left_line.get_best_fit_distance(self.right_line))
self.center_poly = (self.left_line.best_fit_poly + self.right_line.best_fit_poly) / 2
self.curvature = calc_curvature(self.center_poly)
self.car_offset = (frame.shape[1] / 2 - self.center_poly(719)) * 3.7 / 700
self.render_predicted_lane_area(orig_frame)
self.display_dashboard(orig_frame)
return self.add_logo(orig_frame)
class LaneFinder:
def __init__(self):
"""
find lanes on images (or video frames) using Sobel operations, lane color extraction and sliding histogram.
"""
self.camera_calibrator = CameraCalibrator()
self.perspective_transformer = PerspectiveTransformer()
self.n_frames = 7
self.line_segments = 10
self.image_offset = 250
self.left_line = None
self.right_line = None
self.center_poly = None
self.curvature = 0.0
self.car_offset = 0.0
self.dists = []
def display_dashboard(self, img):
"""
display dashboard with information like lane curvature and car's center offset.
"""
font = cv2.FONT_HERSHEY_SIMPLEX
text = "Radius of curvature is {:.2f}m".format(self.curvature)
cv2.putText(img, text, (50, 50), font, 1, (255, 255, 255), 2)
left_or_right = 'left' if self.car_offset < 0 else 'right'
text = "Car is {:.2f}m {} of center".format(np.abs(self.car_offset), left_or_right)
cv2.putText(img, text, (50, 100), font, 1, (255, 255, 255), 2)
def render_predicted_lane_area(self, img):
"""
render the predicted lane area onto the image
"""
overlay = np.zeros([img.shape[0], img.shape[1], img.shape[2]])
mask = np.zeros([img.shape[0], img.shape[1]])
# lane area
lane_area = calculate_lane_area((self.left_line, self.right_line), img.shape[0], 20)
mask = cv2.fillPoly(mask, np.int32([lane_area]), 1)
mask = self.perspective_transformer.inverse_transform(mask)
overlay[mask == 1] = (128, 255, 0)
selection = (overlay != 0)
img[selection] = img[selection] * 0.5 + overlay[selection] * 0.5
# side lines
mask[:] = 0
mask = draw_poly(mask, self.left_line.best_fit_poly, 5, 255)
mask = draw_poly(mask, self.right_line.best_fit_poly, 5, 255)
mask = self.perspective_transformer.inverse_transform(mask)
img[mask == 255] = (255, 200, 2)
def add_logo(self, orig_frame):
background = Image.fromarray(orig_frame)
foreground = Image.open("../test_images/logo.png")
background.paste(foreground, (30, orig_frame.shape[0] - 80), mask=foreground.split()[3])
orig_frame = np.array(background.convert())
return orig_frame
def process_frame(self, frame):
"""
apply lane detection on a single image
:param frame: input frame
:return: processed frame
"""
orig_frame = np.copy(frame)
# undistort frame
frame = self.camera_calibrator.undistort(frame)
# apply sobel and color transforms to create a thresholded binary image.
frame = generate_lane_mask(frame, 400)
# apply perspective transform to get birds-eye view
frame = self.perspective_transformer.transform(frame)
left_detected = right_detected = False
left_x = left_y = right_x = right_y = []
# if lanes were detected in the past, algorithm will first try to find new lanes along the old one.
# this will improve performance
if self.left_line is not None and self.right_line is not None:
left_x, left_y = detect_lane_along_poly(frame, self.left_line.best_fit_poly, self.line_segments)
right_x, right_y = detect_lane_along_poly(frame, self.right_line.best_fit_poly, self.line_segments)
left_detected, right_detected = self.validate_lines(left_x, left_y, right_x, right_y)
# if no lanes were found a histogram search is performed
if not left_detected:
left_x, left_y = histogram_lane_detection(
frame, self.line_segments, (self.image_offset, frame.shape[1] // 2), h_window=7)
left_x, left_y = outlier_removal(left_x, left_y)
if not right_detected:
right_x, right_y = histogram_lane_detection(
frame, self.line_segments, (frame.shape[1] // 2, frame.shape[1] - self.image_offset), h_window=7)
right_x, right_y = outlier_removal(right_x, right_y)
if not left_detected or not right_detected:
left_detected, right_detected = self.validate_lines(left_x, left_y, right_x, right_y)
# updated left lane information
if left_detected:
# switch x and y since lines are almost vertical
if self.left_line is not None:
self.left_line.update(y=left_x, x=left_y)
else:
self.left_line = Line(self.n_frames, left_y, left_x)
# updated right lane information.
if right_detected:
# switch x and y since lines are almost vertical
if self.right_line is not None:
self.right_line.update(y=right_x, x=right_y)
else:
self.right_line = Line(self.n_frames, right_y, right_x)
# add calculated information onto the frame
if self.left_line is not None and self.right_line is not None:
self.dists.append(self.left_line.get_best_fit_distance(self.right_line))
self.center_poly = (self.left_line.best_fit_poly + self.right_line.best_fit_poly) / 2
self.curvature = calc_curvature(self.center_poly)
self.car_offset = (frame.shape[1] / 2 - self.center_poly(719)) * 3.7 / 700
self.render_predicted_lane_area(orig_frame)
self.display_dashboard(orig_frame)
return self.add_logo(orig_frame)
def validate_lines(self, left_x, left_y, right_x, right_y):
"""
compare two line to each other and to their last prediction.
:param left_x:
:param left_y:
:param right_x:
:param right_y:
:return: boolean tuple (left_detected, right_detected)
"""
left_detected = False
right_detected = False
if self.is_line_plausible((left_x, left_y), (right_x, right_y)):
left_detected = True
right_detected = True
elif self.left_line is not None and self.right_line is not None:
if self.is_line_plausible((left_x, left_y), (self.left_line.ally, self.left_line.allx)):
left_detected = True
if self.is_line_plausible((right_x, right_y), (self.right_line.ally, self.right_line.allx)):
right_detected = True
return left_detected, right_detected
def is_line_plausible(self, left, right):
"""
determine if pixels describing two line are plausible lane lines based on curvature and distance.
:param left: Tuple of arrays containing the coordinates of detected pixels
:param right: Tuple of arrays containing the coordinates of detected pixels
:return:
"""
if len(left[0]) < 3 or len(right[0]) < 3:
return False
else:
new_left = Line(y=left[0], x=left[1])
new_right = Line(y=right[0], x=right[1])
return are_lanes_plausible(new_left, new_right)
parser.add_argument("-epochs", "--epochs", type=int, default=1)
parser.add_argument("-lr", "--learning_rate", type=float,
default=0.025) # As starting value in paper
parser.add_argument("-negpow", "--negativepower", type=float, default=0.75)
args = parser.parse_args()
# Create dict of distribution when opening file
edgedistdict, nodedistdict, weights, nodedegrees, maxindex = makeDist(
args.graph_path, args.negativepower)
edgesaliassampler = VoseAlias(edgedistdict)
nodesaliassampler = VoseAlias(nodedistdict)
batchrange = int(len(edgedistdict) / args.batchsize)
print(maxindex)
line = Line(maxindex + 1, embed_dim=args.dimension, order=args.order)
opt = optim.SGD(line.parameters(),
lr=args.learning_rate,
momentum=0.9,
nesterov=True)
device = torch.device("cuda:0" if torch.cuda.is_available() else "cpu")
lossdata = {"it": [], "loss": []}
it = 0
print("\nTraining on {}...\n".format(device))
for epoch in range(args.epochs):
print("Epoch {}".format(epoch))
for b in trange(batchrange):
def correct_sharp_turns(path, img_shape):
theta = (3 / 4) * pi
min_dist_betw_tps = 5 * road_width
margin_fit = 3 * road_width
min_len_fit = 2 * road_width
max_len_fit = 6 * road_width
max_l = len(path) - 1
max_x, max_y = img_shape[1] - 1, img_shape[0] - 1
# get potential turning points using RDP with a low epsilon
turning_points = rdp(path, epsilon=1)
turning_points = turning_points[1:-1]
tp_indices = []
for tp in turning_points:
tuple_tp = tuple(tp)
tp_indices.append(path.index(tuple_tp))
# remove turning points to close to the start or the end of the path
tp_indices_new = []
for i_tp in tp_indices:
if 10 <= i_tp < len(path) - 10:
tp_indices_new.append(i_tp)
tp_indices = tp_indices_new
turning_points_new = []
angles = []
segment_data_tuples = []
tp_indices_new = []
# fit lines at turning points and store all relevant info
for i in tp_indices:
segment1, len_s1, s1_i_min, s1_i_max = get_segment(path,
i,
max_len_fit,
margin_fit,
after=False)
segment2, len_s2, s2_i_min, s2_i_max = get_segment(path,
i,
max_len_fit,
margin_fit,
after=True)
if len_s1 < min_len_fit:
line1 = Line(path[0], path[i])
s1_i_min = 0
else:
line1 = fit_line(segment1)
if len_s2 < min_len_fit:
line2 = Line(path[i], path[max_l])
s2_i_max = max_l
else:
line2 = fit_line(segment2)
segment1_data = segment1, len_s1, s1_i_min, s1_i_max
segment2_data = segment2, len_s2, s2_i_min, s2_i_max
if line1 and line2:
angle = line1.angle(line2)
if angle <= theta:
ip = line1.intersection(line2)
if ip:
ip = int(round(ip[0])), int(round(ip[1]))
cp = closest_point(ip, path[s1_i_max:s2_i_min])
line = Line(cp, ip)
tp = line.point_on_max_dist_from_p1(road_width)
tp = min(tp[0], max_x), min(tp[1], max_y)
turning_points_new.append(tp)
angles.append(angle)
segment_data_tuples.append((segment1_data, segment2_data))
tp_indices_new.append(i)
turning_points = turning_points_new
tp_indices = tp_indices_new
# sort turning points by angle and keep only the turning points with the smallest angle in their neighborhood
tp_indices_sorted_by_angle = sorted(range(len(angles)),
key=lambda k: angles[k])
to_keep = [True] * len(turning_points)
to_remove = set()
for i in tp_indices_sorted_by_angle:
if i in to_remove:
to_keep[i] = False
else:
tp_i = turning_points[i]
for j, tp_j in enumerate(turning_points):
if i != j and euclidean_distance(tp_i,
tp_j) < min_dist_betw_tps:
to_remove.add(j)
turning_points_new = []
angles_new = []
segment_data_tuples_new = []
tp_indices_new = []
for i, keep in enumerate(to_keep):
if keep:
turning_points_new.append(turning_points[i])
angles_new.append(angles[i])
segment_data_tuples_new.append(segment_data_tuples[i])
tp_indices_new.append(tp_indices[i])
turning_points = turning_points_new
angles = angles_new
segment_data_tuples = segment_data_tuples_new
tp_indices = tp_indices_new
# compute the distances between remaining adjacent turning points
distances = []
for i in range(len(turning_points) - 1):
dist = euclidean_distance(turning_points[i], turning_points[i + 1])
distances.append(dist)
# recompute turning points with lower margins for adjacent turns that are still close to each other
previous_close = False
for i in range(len(turning_points)):
if i < len(turning_points) - 1:
dist = distances[i]
if dist < 9 * road_width:
next_close = True
else:
next_close = False
else:
next_close = False
if previous_close or next_close:
j = tp_indices[i] # index tp in path
if previous_close:
dist_prev = distances[i - 1]
s1, len_s1, s1_i_min, s1_i_max = get_segment(path,
j,
dist_prev / 2,
dist_prev / 4,
after=False)
segment_data_tuples[i] = (s1, len_s1, s1_i_min,
s1_i_max), segment_data_tuples[i][1]
else:
s1, len_s1, _, s1_i_max = segment_data_tuples[i][0]
if next_close:
dist_next = distances[i]
s2, len_s2, s2_i_min, s2_i_max = get_segment(path,
j,
dist_next / 2,
dist_next / 4,
after=True)
segment_data_tuples[i] = segment_data_tuples[i][0], (s2,
len_s2,
s2_i_min,
s2_i_max)
previous_close = True
else:
s2, len_s2, s2_i_min, _ = segment_data_tuples[i][1]
previous_close = False
line1 = fit_line(s1)
line2 = fit_line(s2)
view = False
if view:
plt.figure()
px = [x for (x, y) in path]
py = [y for (x, y) in path]
plt.plot(px, py, c='g', linewidth=1)
s1x = [x for (x, y) in s1]
s1y = [y for (x, y) in s1]
plt.plot(s1x, s1y, c='blue', linewidth=4)
s2x = [x for (x, y) in s2]
s2y = [y for (x, y) in s2]
plt.plot(s2x, s2y, c='red', linewidth=4)
pixels_line1 = line1.pixels()
l1x = [x for (x, y) in pixels_line1]
l1y = [y for (x, y) in pixels_line1]
plt.plot(l1x, l1y, c='cyan', linewidth=2)
pixels_line2 = line2.pixels()
l2x = [x for (x, y) in pixels_line2]
l2y = [y for (x, y) in pixels_line2]
plt.plot(l2x, l2y, c='orange', linewidth=2)
plt.scatter([path[j][0]], [path[j][1]], s=50, c='green')
plt.scatter([turning_points[i][0]], [turning_points[i][1]],
s=50,
c='red')
if line1 and line2:
ip = line1.intersection(line2)
if ip:
ip = int(round(ip[0])), int(round(ip[1]))
cp = closest_point(ip, path[s1_i_max:s2_i_min])
line = Line(cp, ip)
tp = line.point_on_max_dist_from_p1(1.42 * road_width)
tp = min(tp[0], max_x), min(tp[1], max_y)
turning_points[i] = tp
if view:
plt.scatter([tp[0]], [tp[1]], s=50, c='blue')
plt.show()
# insert sharp turns in the path, connect all adjacent turning points that are still close to each other
path_new = []
tp_indices = []
previous_close = False
n = 0
for i in range(len(turning_points)):
tp = turning_points[i]
_, _, _, s1_i_max = segment_data_tuples[i][0]
_, _, s2_i_min, _ = segment_data_tuples[i][1]
if i < len(turning_points) - 1:
dist = distances[i]
if dist < 9 * road_width:
next_close = True
else:
next_close = False
else:
next_close = False
if previous_close:
if next_close:
turn = fill([], turning_points[i - 1], tp)[1:]
previous_close = True
else:
turn = fill([tp], turning_points[i - 1], path[s2_i_min])[1:]
previous_close = False
else:
path_new.extend(path[n:s1_i_max])
if next_close:
turn = fill([], path[s1_i_max], tp)
previous_close = True
else:
turn = fill([tp], path[s1_i_max], path[s2_i_min])
previous_close = False
path_new.extend(turn)
tp_indices.append(path_new.index(tp))
n = s2_i_min + 1
path_new.extend(path[n:])
return path_new, tp_indices