def get_nearby_lanes(relativepath, threshold=0.125,
lane_fname='lanes_output_new.json', vp_fname='vp.json'):
print 'Finding nearby lanes for {}'.format(relativepath)
filenames = io_util.load_fnames(relativepath)
fname_lanes_dict = io_util.load_json(relativepath, lane_fname)
vp_dict = io_util.load_json(relativepath, vp_fname)
vp = vp_dict['vp']
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)
new_fname_json = {}
for fname in filenames:
if fname in fname_lanes_dict:
lanes = fname_lanes_dict[fname]
distances = []
for lane in lanes:
dist = distToCenter(lane, ipm)
distances.append((dist, lane))
if len(distances) >= 2:
distances.sort(key=lambda x:abs(x[0]))
if abs(abs(distances[0][0]) - abs(distances[1][0])) < threshold:
if (distances[0][0] <= 0 and distances[1][0] >= 0):
new_fname_json[fname] = [distances[0][1], distances[1][1]]
elif (distances[0][0] >= 0 and distances[1][0] <= 0):
new_fname_json[fname] = [distances[1][1], distances[0][1]]
else:
print abs(abs(distances[0][0]) - abs(distances[1][0]))
return new_fname_json
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'
run_LineTracker_Test = False
run_LaneDetector_Test = True
filenames = io_util.load_fnames(relativepath)
initimg = cv2.imread(os.path.join(relativepath, filenames[0]),
cv2.IMREAD_GRAYSCALE).astype('uint8')
laneMarker = LaneMarker.NietoLaneMarker()
laneDetector = LaneDetector.PolyRANSAC_LaneDetector(verbose=False)
laneDetectorModel = LaneDetector.LaneDetectorModel([], initimg.shape,
laneDetector)
origPts, destPts = image_util.locateROI(initimg.shape,
x_frac=1 / 3.,
y_frac_top=1 / 12.,
y_frac_bot=1 / 6.,
vp=vp)
show_roi = False
if show_roi:
for ii in range(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)
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 visualize_lane_changes(relativepath):
lanes_fn = 'nearby_lanes.json'
vp_fn = 'vp.json'
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']
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)
lpts = []
rpts = []
for i in range(0, 900):
fname = filenames[i]
if fname in fname_lanes_dict:
lanes = fname_lanes_dict[fname]
j = 0
for lane in lanes:
[x1, y1] = ipm.pointToIPM(lane[0])
[x2, y2] = ipm.pointToIPM(lane[1])
if j == 0:
lpts.append((x1 + x2) / 2.)
j = 1
else:
rpts.append((x1 + x2) / 2.)
else:
lpts.append(None)
rpts.append(None)
l = []
r = []
window_size = 1
for i in range(len(lpts)):
if i < window_size:
l.append(0)
elif lpts[i] is None or lpts[i - window_size] is None:
l.append(0)
else:
l.append(lpts[i] - lpts[i - window_size])
l = np.array(l)
for i in range(len(rpts)):
if i < window_size:
r.append(0)
elif rpts[i] is None or rpts[i - window_size] is None:
r.append(0)
else:
r.append(rpts[i] - rpts[i - window_size])
r = np.array(r)
def movingaverage(values, window=15):
weights = np.repeat(1.0, window) / window
sma = np.convolve(values, weights, 'same')
return sma
plt.figure(1)
#plt.scatter(np.arange(l.shape[0]), l, color='b', label='Raw',s=5)
#plt.scatter(np.arange(l.shape[0]), movingaverage(l), color='r', label='Average',s=5)
plt.plot(np.arange(l.shape[0]), (l + r) / 2, color='g', label='Raw')
plt.plot(np.arange(l.shape[0]),
movingaverage((l + r) / 2),
color='r',
label='Moving Average')
plt.plot(np.arange(l.shape[0]),
medfilt(movingaverage((l + r) / 2), 15),
color='b',
label='Median and Moving Average')
plt.ylim([-5, 5])
plt.xlabel('Frame')
plt.ylabel('$\Delta$x of Lane Boundaries (in pixels)')
plt.legend(loc='lower left')
plt.show()
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)