def VehicleDetector_Test(svm, pic_dir):
vd = VehicleDetector.VehicleDetector(svm)
font = cv2.FONT_HERSHEY_PLAIN
# Read the filenames from the list.txt file
init_frame = 1000
filenames = io_util.load_fnames(pic_dir)[init_frame:init_frame + 50]
for i in range(len(filenames)):
display.show_progressbar(i, filenames)
ftmp = cv2.resize(
cv2.imread(os.path.join(pic_dir, filenames[i]),
cv2.IMREAD_GRAYSCALE).astype('uint8'), (320, 240))
boxes = vd.update(ftmp)
for vehicle_box in boxes:
[x1, y1, x2, y2] = vehicle_box['box'].astype(np.int64)
box_id = vehicle_box['id']
cv2.rectangle(ftmp, (x1, y1), (x2, y2), color=(255, 0, 0))
cv2.putText(ftmp,
"Id:{}".format(box_id), (x1, y1 - 5),
font,
1,
color=(255, 0, 0))
cv2.imshow('img', ftmp)
cv2.waitKey(1)
print ""
cv2.destroyAllWindows()
def LaneDetector_Test(pic_dir, ipm, LaneMarker, laneDetectorModel):
filenames = io_util.load_fnames(pic_dir)
ftmp1 = cv2.imread(os.path.join(pic_dir, filenames[0]),
cv2.IMREAD_GRAYSCALE).astype('uint8')
for i in range(len(filenames)):
display.show_progressbar(i, filenames)
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'))
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]))
cv2.line(ftmp, pt1, pt2, (255, 0, 0), 5)
cv2.imshow('img', ftmp.astype('uint8'))
cv2.waitKey(15)
print ""
cv2.destroyAllWindows()
def get_vps(relativepath, boxes_fn):
lanewidth = 5
filenames = io_util.load_fnames(relativepath)
init_img = cv2.imread(os.path.join(relativepath, filenames[0]),
cv2.IMREAD_GRAYSCALE).astype('uint8')
lanemarker = LaneMarker.NietoLaneMarker()
vpEstimator = VPEstimator.MSAC_LS(init_img.shape)
boxes_dict = io_util.load_json(relativepath, boxes_fn)
data = []
for i in range(len(filenames)):
fname = filenames[i]
display.show_progressbar(i, filenames)
color_img = cv2.imread(os.path.join(relativepath, filenames[i]))
gray_img = cv2.cvtColor(color_img, cv2.COLOR_BGR2GRAY).astype('uint8')
# block region of the image - top, text, and vehicles
blockedimg = image_util.blockTopImage(gray_img, 0.6)
#cv2.imshow('block',blockedimg.astype('uint8'))
image_util.block_EBRON(blockedimg)
if fname in boxes_dict:
for box_dict in boxes_dict[fname]:
[x1, y1, x2, y2] = box_dict['box']
blockedimg[y1:y2, x1:x2] = 0
color_img[y1:y2, x1:x2] = 0
threshimg = image_util.thresholdImage(lanemarker.detect(blockedimg, lanewidth))
lines = cv2.HoughLines(threshimg, 1, cv2.cv.CV_PI/180, 30)
if lines is None:
continue
linesegments = VPEstimator.preprocess_lines(lines[0])
if len(linesegments) < 5:
continue
vps, lineSegmentsClusters, numInliers = vpEstimator.multipleVPEstimation(linesegments, 1)
for line in linesegments:
[x1, y1, x2, y2] = line
cv2.line(threshimg, (int(x1), int(y1)), (int(x2), int(y2)), (255, 255, 255), 1)
cv2.line(color_img, (int(x1), int(y1)), (int(x2), int(y2)), (0, 255, 0), 1)
if vps is not None:
x, y = vps[0][0][0], vps[0][1][0]
cv2.circle(color_img, (int(x), int(y)), 5, (255, 0, 0), thickness=3)
cv2.circle(threshimg, (int(x), int(y)), 5, (255, 255, 255),thickness=3)
data.append([x, y])
cv2.imshow('color image', color_img)
#cv2.imshow('thresh image', threshimg)
cv2.waitKey(1pospost)
#break
print ""
cv2.destroyAllWindows()
return np.array(data)
def ObjectTracker_Test(pic_dir):
# Read the filenames from the list.txt file
filenames = io_util.load_fnames(pic_dir)
init_frame = 15
confidence_thresh = 7
filenames = io_util.load_fnames(pic_dir)[init_frame:init_frame + 50]
ftmp1 = cv2.imread(os.path.join(pic_dir, filenames[init_frame]),
cv2.IMREAD_GRAYSCALE).astype('uint8')
# Top car
left = 295
right = 335
top = 225
bot = 265
# Alternatively, track the side car
# left = 440
# right = 550
# top = 205
# bot = 305
tracker = ObjectTracker.dlib_ObjectTracker(ftmp1, [left, top, right, bot])
[x1, y1, x2, y2] = tracker.get_position().astype(np.int64)
cv2.rectangle(ftmp1, (x1, y1), (x2, y2), color=(255, 0, 0))
cv2.imshow('img', ftmp1)
cv2.waitKey(1)
for i in xrange(init_frame, len(filenames)):
display.show_progressbar(i, filenames)
ftmp = cv2.imread(os.path.join(pic_dir, filenames[i]),
cv2.IMREAD_GRAYSCALE).astype('uint8')
value = tracker.update(ftmp)
[x1, y1, x2, y2] = tracker.get_position().astype(np.int64)
cv2.rectangle(ftmp, (x1, y1), (x2, y2), color=(255, 0, 0))
cv2.imshow('img', ftmp)
if value < confidence_thresh:
print '\n\tlost object'
break
else:
cv2.waitKey(1)
print ""
cv2.destroyAllWindows()
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_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 vehicle_detector_main(relativepath,
display_frames,
dlib_detector_fname,
verbose,
forward_confidence_thresh,
backward_confidence_thresh,
run_backwards_tracking=True):
if verbose:
print 'Starting vehicle detector main for {}'.format(relativepath)
# # Read the filenames from the list.txt file
filenames = io_util.load_fnames(relativepath)
# Get the shape from the first image
tmp_img = cv2.imread(os.path.join(relativepath, filenames[0]),
cv2.IMREAD_GRAYSCALE)
imgshape = tmp_img.shape
imgheight, imgwidth = imgshape
# Confidence value where if we fall below this, we stop backwards tracking
confidence_threshold = forward_confidence_thresh
previous_track_confidence = backward_confidence_thresh
# Set up the vehicle detector object
vehicleDetector = VehicleDetector.VehicleDetector(
dlib_detector_fname, confidence_threshold, previous_track_confidence)
# Output dictionary where key = filename,
# value = list of boxes + ids of objects in that image
fname_boxes_dict = {}
for fi in range(len(filenames)):
if verbose:
display.show_progressbar(fi, filenames)
fname = filenames[fi]
img = cv2.imread(os.path.join(relativepath, fname),
cv2.IMREAD_GRAYSCALE)
# Add any detected boxes to the dictionary
vehicle_boxes = vehicleDetector.update(img)
if fname not in fname_boxes_dict:
fname_boxes_dict[fname] = []
for box_dict in vehicle_boxes:
fname_boxes_dict[fname].append(box_dict)
# For new detected boxes, initialize an object tracker to go backwards
# and detect boxes in previous frames, until the confidence falls below
# a certain threshold
new_box_dicts = vehicleDetector.get_new_boxes()
if run_backwards_tracking:
vehicleDetector.initialize_backwards(img)
for fj in range(fi - 1, -1, -1):
prev_fname = filenames[fj]
prev_img = cv2.imread(os.path.join(relativepath, prev_fname),
cv2.IMREAD_GRAYSCALE)
boxes = vehicleDetector.update_backwards(prev_img)
if boxes is None:
break
else:
if prev_fname in fname_boxes_dict:
fname_boxes_dict[prev_fname] += boxes
else:
fname_boxes_dict[prev_fname] = boxes
# for box_dict in new_box_dicts:
# #if verbose:
# # print '\n\t Starting backwards track'
# new_box = box_dict['box']
# new_box_id = box_dict['id']
# new_object_tracker = ObjectTracker.dlib_ObjectTracker(img, new_box,
# new_box_id, store_size=1)
# for fj in range(fi - 1, -1, -1):
# prev_fname = filenames[fj]
# prev_img = cv2.imread(os.path.join(relativepath,prev_fname),
# cv2.IMREAD_GRAYSCALE)
# confidence = new_object_tracker.update(prev_img)
# if confidence < previous_track_confidence:
# break
# else:
# if prev_fname in fname_boxes_dict:
# fname_boxes_dict[prev_fname].append(
# new_object_tracker.get_dict())
# else:
# fname_boxes_dict[prev_fname] = [
# new_object_tracker.get_dict()]
# #if verbose:
# # print '\t Ended backwards track at ' + filenames[fj]
if verbose:
print ""
# Create the output json file of the boxes
detector_fn = dlib_detector_fname.split('/')[-1].split('.svm')[0]
output_fname = os.path.join(
relativepath,
'boxes_output_{}_{}_run2.json'.format(detector_fn,
confidence_threshold))
with open(output_fname, 'w') as outfile:
json.dump(fname_boxes_dict,
outfile,
indent=4,
sort_keys=True,
cls=io_util.NumpyEncoder)
# Display the images with the boxes and ids on them
if display_frames:
for fname in filenames:
boxes = fname_boxes_dict[fname]
img = cv2.imread(os.path.join(relativepath, fname))
display.draw_boxdicts(img, boxes, (255, 0, 0))
cv2.imshow('With box and id labels', img)
cv2.waitKey(1)
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)