def LineTracker_Test(pic_dir, ipm, laneMarker, laneDetector, init_line=None):
# Read the filenames from the list.txt file
filenames = io_util.load_fnames(pic_dir)
ftmp1 = cv2.imread(os.path.join(pic_dir, filenames[0]),
cv2.IMREAD_GRAYSCALE).astype('uint8')
if init_line is None:
init_line = image_util.Line(274, 281, 125, 399)
x1, y1 = ipm.pointToIPM((init_line.x1, init_line.y1))
x2, y2 = ipm.pointToIPM((init_line.x2, init_line.y2))
ipm_line = image_util.Line(x1, y1, x2, y2)
tracker = LaneDetector.LineTracker(ipm_line, ftmp1.shape)
display.draw_lines(ftmp1, [init_line])
cv2.imshow('img', ftmp1)
for i in range(len(filenames)):
print '\r{}'.format(filenames[i]),
ftmp = cv2.imread(os.path.join(pic_dir, filenames[i]),
cv2.IMREAD_GRAYSCALE).astype('float')
ipm_img = ipm.imageToIPM(ftmp)
lanemarker_img = laneMarker.detect(ipm_img, 5)
threshold_img = image_util.thresholdImage(lanemarker_img, 95)
# cv2.imshow('ipm', ipm_img.astype('uint8'))
# cv2.imshow('lanemarker', lanemarker_img.astype('uint8'))
# cv2.imshow('threshold_img', threshold_img.astype('uint8'))
lane = tracker.update(threshold_img, laneDetector)
if lane is not None:
xns, yns = convertIPMLaneToLane(lane, ipm)
pt1 = (int(xns[0]), int(yns[0]))
pt2 = (int(xns[-1]), int(yns[-1]))
cv2.line(ftmp, pt1, pt2, (255, 0, 0), 5)
cv2.imshow('img', ftmp.astype('uint8'))
cv2.waitKey(15)
else:
break
print ""
cv2.destroyAllWindows()
def clusterLines(self, lines, quantile):
# Meanshift clustering runs into errors when we have isolated lanes
# To handle, add lines on either side
newlines = np.array([]).reshape(0, 4)
for line in lines:
newlines = np.vstack([newlines, line])
[x1, y1, x2, y2] = line
line1 = np.array([x1 + 0.01, y1, x2 + 0.01, y2])
line2 = np.array([x1 - 0.01, y1, x2 - 0.01, y2])
newlines = np.vstack([newlines, line1])
newlines = np.vstack([newlines, line2])
bandwidth = estimate_bandwidth(newlines, quantile=quantile)
while bandwidth == 0:
print newlines
quantile = quantile * 2.
bandwidth = estimate_bandwidth(newlines, quantile=quantile)
print bandwidth
ms = MeanShift(bandwidth=bandwidth, bin_seeding=True)
ms.fit(newlines)
cluster_centers = ms.cluster_centers_
if len(cluster_centers.shape) > 2:
cluster_centers = cluster_centers[0]
if self.verbose:
labels = ms.labels_
labels_unique = np.unique(labels)
n_clusters_ = len(labels_unique)
print "{} clusters: ".format(n_clusters_)
print cluster_centers
cluster_centers_lines = []
for center in cluster_centers:
x1, y1, x2, y2 = center
new_line = image_util.Line(x1, y1, x2, y2)
cluster_centers_lines.append(new_line)
return cluster_centers_lines
def following_main(relativepath,
lanes_fn='nearby_lanes.json',
spd_fn='speed_acclimit.json', vp_fn='vp.json',
boxes_fn='boxes_output_detector_session2_raw_8.5.json',
cv_display=True):
filenames = io_util.load_fnames(relativepath)
fname_lanes_dict = io_util.load_json(relativepath, lanes_fn)
vp_dict = io_util.load_json(relativepath, vp_fn)
vp = vp_dict['vp']
fname_boxes_dict = io_util.load_json(relativepath, boxes_fn)
speed_dict = io_util.load_json(relativepath, spd_fn)
initimg = cv2.imread(os.path.join(relativepath, filenames[0]), cv2.IMREAD_GRAYSCALE)
imgshape = initimg.shape
imgheight, imgwidth = imgshape
origPts, destPts = image_util.locateROI(imgshape, x_frac=1/10.,
y_frac_top=1/60., y_frac_bot=1/6.,
vp=vp)
for ii in xrange(len(origPts)):
if ii == (len(origPts) - 1):
cv2.line(initimg, tuple(origPts[ii]), tuple(origPts[0]), (255, 255, 255), 2)
else:
cv2.line(initimg, tuple(origPts[ii]), tuple(origPts[ii+1]), (255, 255, 255), 2)
cv2.imshow('initimg', initimg)
cv2.waitKey(0)
ipm = IPM.IPM(imgshape, imgshape, origPts, destPts)
lanemarker = LaneMarker.NietoLaneMarker()
follow_dict = {}
for i in range(len(filenames)):
fname = filenames[i]
display.show_progressbar(i, filenames)
if fname in fname_lanes_dict:
img = cv2.imread(os.path.join(relativepath, fname))
gray_img = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY).astype('float')
image_util.block_EBRON(gray_img)
new_pts = []
if fname in fname_boxes_dict:
box_dicts = fname_boxes_dict[fname]
for box_dict in box_dicts:
x1, y1, x2, y2 = np.array(box_dict['box']).astype(np.int64)
gray_img[y1:y2,x1:x2] = 0
bottom_center = [(x1 + x2)/2., y2]
new_pt = ipm.pointToIPM(bottom_center)
new_pts.append(new_pt)
ipm_img = ipm.imageToIPM(gray_img)
tmp = cv2.cvtColor(ipm_img.astype('uint8').copy(), cv2.COLOR_GRAY2BGR)
lanes = fname_lanes_dict[fname]
lines = []
for lane in lanes:
[x1, y1] = ipm.pointToIPM(lane[0])
[x2, y2] = ipm.pointToIPM(lane[1])
lines.append(image_util.Line(x1, y1, x2, y2))
slope, b = computeXLineDifferences(lines[0], lines[1])
for new_pt in new_pts:
[x, y] = new_pt
if (x > lines[0].x1 and x > lines[0].x2 and
x < lines[1].x1 and x < lines[1].x2 and
y < imgshape[0] and y > 0):
cv2.circle(tmp, (int(x), int(y)), 5, (255, 0, 0), thickness=3)
conversion = 12./(slope * y + b)
distance = conversion*(imgshape[0] - y)
#print distance
follow_dict[fname] = {'distance': distance, 'speed':speed_dict[fname]}
display.draw_lines(tmp, lines, color=(0, 255, 0))
if cv_display:
cv2.imshow('Tmp', tmp)
cv2.waitKey(1)
print ""
cv2.destroyAllWindows()
io_util.save_json(relativepath, 'follow_distance_speed.json', follow_dict)
print 'Done'
def lane_detection_validation(relativepath,
test_lanes_fn,
true_lanes_fn='manual_lanes.json',
stepsize=15,
vp_fn='vp.json'):
print 'Lane Detection Validation for {}'.format(relativepath)
filenames = io_util.load_fnames(relativepath)
test_dict = io_util.load_json(relativepath, test_lanes_fn)
hand_dict = io_util.load_json(relativepath, true_lanes_fn)
vp_dict = io_util.load_json(relativepath, vp_fn)
vp = vp_dict['vp']
true_positives = 0.
false_positives = 0.
false_negatives = 0.
for fname in filenames[::stepsize]:
hand_lanes = []
if fname in hand_dict:
for line in hand_dict[fname]:
[x1, y1] = line[0]
[x2, y2] = line[1]
hand_lanes.append(image_util.Line(x1, y1, x2, y2))
test_lanes = []
if fname in test_dict:
for line in test_dict[fname]:
[x1, y1] = line[0]
[x2, y2] = line[1]
test_lanes.append(image_util.Line(x1, y1, x2, y2))
test_lanes = remove_duplicates(test_lanes)
#hand_lanes = get_nearby_lanes(hand_lanes, vp)
#test_lanes = get_nearby_lanes(test_lanes, vp)
img = cv2.imread(os.path.join(relativepath, fname))
display.draw_lines(img, hand_lanes, (255, 0, 0))
display.draw_lines(img, test_lanes, (0, 0, 255))
cv2.imshow('orig', img)
for test_line in test_lanes[:]:
for hand_line in hand_lanes[:]:
if lines_close_enough(test_line, hand_line):
hand_lanes.remove(hand_line)
test_lanes.remove(test_line)
true_positives += 1
break
false_positives += len(test_lanes)
false_negatives += len(hand_lanes)
if len(test_lanes) > 0:
img = cv2.imread(os.path.join(relativepath, fname))
# hand_lanes = []
# if fname in hand_dict:
# for line in hand_dict[fname]:
# [x1, y1] = line[0]
# [x2, y2] = line[1]
# hand_lanes.append(image_util.Line(x1, y1, x2, y2))
# test_lanes = []
# if fname in test_dict:
# for line in test_dict[fname]:
# [x1, y1] = line[0]
# [x2, y2] = line[1]
# test_lanes.append(image_util.Line(x1, y1, x2, y2))
display.draw_lines(img, hand_lanes, (255, 0, 0))
display.draw_lines(img, test_lanes, (0, 0, 255))
cv2.imshow('FP - truth is blue, red is code', img)
# angle_diff = abs(hand_lanes[0].getAngle() - test_lanes[0].getAngle())/90.
# print angle_diff*90.
# intercept_diff = abs(hand_lanes[0].getIntercept() - test_lanes[0].getIntercept())
# print intercept_diff
#print 'FP ',
for i in range(len(test_lanes)):
print fname, test_lanes[i].x1, test_lanes[i].y1, test_lanes[
i].x2, test_lanes[i].y2
cv2.waitKey(0)
if len(hand_lanes) > 0:
#print 'FN ',
#for i in range(len(hand_lanes)):
# print fname, hand_lanes[i].x1, hand_lanes[i].y1, hand_lanes[i].x2, hand_lanes[i].y2
img = cv2.imread(os.path.join(relativepath, fname))
display.draw_lines(img, hand_lanes, (255, 0, 0))
display.draw_lines(img, test_lanes, (0, 0, 255))
cv2.imshow('FN - truth is blue, red is code', img)
cv2.waitKey(0)
precision = 0
recall = 0
f1 = 0
if true_positives != 0:
precision = true_positives / (true_positives + false_positives)
recall = true_positives / (true_positives + false_negatives)
f1 = 2 * (precision * recall) / (precision + recall)
print '\tPrecision: {} ({}/{})'.format(precision, true_positives,
(true_positives + false_positives))
print '\tRecall: {} ({}/{})'.format(recall, true_positives,
(true_positives + false_negatives))
print '\tF1 Score: {}'.format(f1)
return true_positives, false_positives, false_negatives
def lane_detector_main(relativepath, boxes_fn, vp_fn, display_, verbose):
# Read the filenames from the list.txt file
filenames = io_util.load_fnames(relativepath)
#filenames = filenames[700:]
initimg = cv2.imread(os.path.join(relativepath, filenames[0]),
cv2.IMREAD_GRAYSCALE)
imgshape = initimg.shape
imgheight, imgwidth = imgshape
# Initialize the objects
lanemarker = LaneMarker.NietoLaneMarker()
laneDetector = LaneDetector.PolyRANSAC_LaneDetector(verbose=False)
laneDetectorModel = LaneDetector.LaneDetectorModel([], imgshape,
laneDetector)
vp_dict = io_util.load_json(relativepath, vp_fn)
vp = vp_dict['vp']
origPts, destPts = image_util.locateROI(imgshape,
x_frac=1 / 3.,
y_frac_top=1 / 25.,
y_frac_bot=1 / 6.,
vp=vp)
# for ii in xrange(len(origPts)):
# if ii == (len(origPts) - 1):
# cv2.line(initimg, tuple(origPts[ii]), tuple(origPts[0]), (255, 255, 255), 2)
# else:
# cv2.line(initimg, tuple(origPts[ii]), tuple(origPts[ii+1]), (255, 255, 255), 2)
#cv2.imshow('initimg', initimg)
#cv2.waitKey(0)
if verbose:
print 'Starting IPM'
ipm = IPM.IPM(imgshape, imgshape, origPts, destPts)
if verbose:
print 'Done with IPM'
# Dictionary where key = filename,
# value = list of boxes of objects in that image
fname_boxes_dict = io_util.load_json(relativepath, boxes_fn)
fname_lanes_dict = {}
# For each image
for fi in range(len(filenames)):
display.show_progressbar(fi, filenames)
fname = filenames[fi]
img = cv2.imread(os.path.join(relativepath, fname))
image_util.block_EBRON(img)
gray_img = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY).astype('float')
# Check for found vehicles in the image. For each one, zero out the
# region where the vehicle has been found
if fname in fname_boxes_dict:
box_dicts = fname_boxes_dict[fname]
for box_dict in box_dicts:
x1, y1, x2, y2 = np.array(box_dict['box']).astype(np.int64)
gray_img[y1:y2, x1:x2] = 0
if display_:
cv2.rectangle(img, (x1, y1), (x2, y2), color=(255, 0, 0))
# Perform IPM, lane marking detection, and thresholding
ipm_img = ipm.imageToIPM(gray_img)
lanemarker_img = lanemarker.detect(ipm_img, 5)
threshold_img = image_util.thresholdImage(lanemarker_img, 95)
if display_ and verbose:
cv2.imshow('IPMImage', ipm_img.astype('uint8'))
cv2.imshow('LaneMarker Raw', lanemarker_img.astype('uint8'))
cv2.imshow('laneMarker Thresholded', threshold_img.astype('uint8'))
cv2.waitKey(1)
# Update the lane detector model with the image to get the lanes
degree = 1
lanes = laneDetectorModel.updateLaneModel(threshold_img, degree)
# Save lanes
for lane in lanes:
xns, yns = convertIPMLaneToLane(lane, ipm)
pt1 = (int(xns[0]), int(yns[0]))
pt2 = (int(xns[-1]), int(yns[-1]))
if display_:
cv2.line(img, pt1, pt2, (255, 0, 0), 5)
if fname in fname_lanes_dict:
fname_lanes_dict[fname].append([pt1, pt2])
else:
fname_lanes_dict[fname] = [[pt1, pt2]]
# For new lanes, perform backward tracking with a LineTracker object
# (same steps as above)
new_lanes = laneDetectorModel.get_new_lanes()
for new_lane in new_lanes:
new_line = new_lane.asLine()
# if verbose:
# print '\t Starting backwards track'
prev_lane_tracker = LaneDetector.LineTracker(new_line, imgshape)
for fj in range(fi - 1, -1, -1):
prev_fname = filenames[fj]
prev_img = cv2.imread(os.path.join(relativepath, prev_fname))
image_util.block_EBRON(prev_img)
prev_gray_img = cv2.cvtColor(
prev_img, cv2.COLOR_BGR2GRAY).astype('float')
if prev_fname in fname_boxes_dict:
box_dicts = fname_boxes_dict[prev_fname]
for box_dict in box_dicts:
left, top, right, bot = box_dict['box']
prev_gray_img[top:bot, left:right] = 0
prev_ipm_img = ipm.imageToIPM(prev_gray_img)
prev_lanemarker_img = lanemarker.detect(prev_ipm_img, 5)
prev_threshold_img = image_util.thresholdImage(
prev_lanemarker_img, 95)
prev_lane = prev_lane_tracker.update(prev_threshold_img,
laneDetector, degree)
if prev_lane is None:
#if verbose:
# print '\t Breaking, no suitable backwards lane found'
break
else:
xns, yns = convertIPMLaneToLane(prev_lane, ipm)
pt1 = (int(xns[0]), int(yns[0]))
pt2 = (int(xns[-1]), int(yns[-1]))
if prev_fname not in fname_lanes_dict:
fname_lanes_dict[prev_fname] = [[pt1, pt2]]
else:
# Backwards lane pruning. But note this compares lines
# in the non-IPM space
line = image_util.Lane(xns, yns).asLine()
is_new_line = True
for [ppt1, ppt2] in fname_lanes_dict[prev_fname]:
prev_line = image_util.Line(
ppt1[0], ppt1[1], ppt2[0], ppt2[1])
if line.checkLineIntersection(prev_line):
is_new_line = False
break
if is_new_line:
fname_lanes_dict[prev_fname].append([pt1, pt2])
else:
# if verbose:
# print '\t Breaking, intersected with previous lane'
break
#if verbose:
# print '\t Ended backwards track at ' + filenames[fj]
if display_:
cv2.imshow('frame', img)
cv2.waitKey(1)
print ""
output_fname = os.path.join(relativepath, 'lanes_output_new.json')
with open(output_fname, 'w') as outfile:
json.dump(fname_lanes_dict, outfile, indent=4, sort_keys=True)
print 'Wrote json output file'
cv2.destroyAllWindows()
def speed_estimation_main(
relativepath,
uniform_conversion=False,
lanes_fn='nearby_lanes.json',
true_spd_fn='true_speed.json',
vp_fn='vp.json',
boxes_fn='boxes_output_detector_session2_raw_8.5.json'):
print 'Starting speed estimation for {}'.format(relativepath)
filenames = io_util.load_fnames(relativepath)
fname_lanes_dict = io_util.load_json(relativepath, lanes_fn)
vp_dict = io_util.load_json(relativepath, vp_fn)
vp = vp_dict['vp']
fname_boxes_dict = io_util.load_json(relativepath, boxes_fn)
initimg = cv2.imread(os.path.join(relativepath, filenames[0]),
cv2.IMREAD_GRAYSCALE)
imgshape = initimg.shape
imgheight, imgwidth = imgshape
origPts, destPts = image_util.locateROI(imgshape,
x_frac=1 / 3.,
y_frac_top=1 / 25.,
y_frac_bot=1 / 6.,
vp=vp)
ipm = IPM.IPM(imgshape, imgshape, origPts, destPts)
lanemarker = LaneMarker.NietoLaneMarker()
if uniform_conversion:
slopes = []
bs = []
for fname in filenames:
if fname in fname_lanes_dict:
lanes = fname_lanes_dict[fname]
lines = []
for lane in lanes:
[x1, y1] = ipm.pointToIPM(lane[0])
[x2, y2] = ipm.pointToIPM(lane[1])
lines.append(image_util.Line(x1, y1, x2, y2))
slope, b = computeXLineDifferences(lines[0], lines[1])
slopes.append(slope)
bs.append(b)
mean_slope = np.mean(np.array(slopes))
mean_b = np.mean(np.array(bs))
speed_detector = SpeedEstimator.Lane_SpeedEstimator(
lanemarker, mean_slope, mean_b)
else:
speed_detector = SpeedEstimator.Lane_SpeedEstimator(lanemarker)
output_left = {}
output_right = {}
for i in range(len(filenames)):
fname = filenames[i]
display.show_progressbar(i, filenames)
if fname in fname_lanes_dict:
img = cv2.imread(os.path.join(relativepath, fname))
gray_img = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY).astype('float')
image_util.block_EBRON(gray_img)
if fname in fname_boxes_dict:
box_dicts = fname_boxes_dict[fname]
for box_dict in box_dicts:
x1, y1, x2, y2 = np.array(box_dict['box']).astype(np.int64)
gray_img[y1:y2, x1:x2] = 0
ipm_img = ipm.imageToIPM(gray_img)
tmp = cv2.cvtColor(
ipm_img.astype('uint8').copy(), cv2.COLOR_GRAY2BGR)
lanes = fname_lanes_dict[fname]
lines = []
for lane in lanes:
[x1, y1] = ipm.pointToIPM(lane[0])
[x2, y2] = ipm.pointToIPM(lane[1])
lines.append(image_util.Line(x1, y1, x2, y2))
display.draw_lines(tmp, lines, color=(0, 255, 0))
cv2.imshow('Tmp', tmp)
cv2.waitKey(0)
speed_est_L, speed_est_R = speed_detector.update(
ipm_img, lines[0], lines[1])
output_left[fname] = speed_est_L
output_right[fname] = speed_est_R
print ""
if uniform_conversion:
io_util.save_json(relativepath, 'speed_raw_left_uni.json', output_left)
io_util.save_json(relativepath, 'speed_raw_right_uni.json',
output_right)
else:
io_util.save_json(relativepath, 'speed_raw_left.json', output_left)
io_util.save_json(relativepath, 'speed_raw_right.json', output_right)