def main():
args = parse_args()
if not os.path.exists(IMGSDIR_CALIB):
print "(ERROR) Calibration images not found. Please place \
the LDWS_calibrate_short/ images in the current directory."
exit(1)
if not os.path.exists(IMGSDIR_TEST):
print "(ERROR) Test images not found. Please place the \
LDWS_test_short/ images in the current directory."
exit(1)
imgpaths_calib = util.get_imgpaths(IMGSDIR_CALIB)
if not os.path.isdir(args.imgsdir):
imgpaths_test = [args.imgsdir]
else:
imgpaths_test = util.get_imgpaths(args.imgsdir)
if args.reuse_calib:
print "(Reusing camera calibration matrix)"
K = np.array([[674.07224154, 0., 262.77722917],
[0., 670.26875783, 330.21546389], [0., 0., 1.]])
else:
print "(Estimating camera matrix...)"
t = time.time()
K = calibrate_camera.calibrate_camera(imgpaths_calib, 8, 8, 0.048)
dur = time.time() - t
print "(Finished. {0:.4f})".format(dur)
for i, imgpath in enumerate(imgpaths_test):
print "\n==== ({0}/{1}) Detecting lanes... [{2}]====".format(
i + 1, len(imgpaths_test),
os.path.split(imgpath)[1])
I = cv2.imread(imgpath, cv2.CV_LOAD_IMAGE_GRAYSCALE)
h, w = I.shape[0:2]
t = time.time()
line1, line2 = detect_lanes.detect_lanes(I,
win1=WIN_LEFT,
win2=WIN_RIGHT,
threshold1=110,
threshold2=220,
apertureSize=3,
show_edges=False)
dur = time.time() - t
print " Finished detecting lanes ({0:.4f}s)".format(dur)
if line1 == None or line2 == None:
print "({0}/{1}) Error: Couldn't find lanes.".format(
i + 1, len(imgpaths_test))
continue
# Choose 4 points on the lanes to estimate the planar homography
y1 = intrnd(0.45 * h)
y2 = intrnd(0.65 * h)
pts = np.array([
[compute_x(line1, y1), y1], # Left lane, far
[compute_x(line2, y1), y1], # Right lane, far
[compute_x(line1, y2), y2], # Left lane, close
[compute_x(line2, y2), y2]
]) # Right lane, close
# These world points have the origin at the middle of the lane,
# directly below the camera.
# Depths 5.0, 3.0 are chosen arbitrarily, as we don't have depth
# information. Thus, the computed H will only be accurate in the
# X axis.
pts_worldm = np.array([
[-LANE_W / 2.0, 5.0], # Left lane, far
[LANE_W / 2.0, 5.0], # Right lane, far
[-LANE_W / 2.0, 3.0], # Left lane, close
[LANE_W / 2.0, 3.0]
]) # Right lane, close
# These world points are scaled+translated to generate a
# reasonably-sized image w/ perspective effects removed.
# Note: the origin here is not the same origin as in pts_worldm!
pts_world = np.array([[0.0, 0.0], [LANE_W * 1e2, 0.0], [0.0, 500],
[LANE_W * 1e2, 500.0]])
pts_world[:,
0] += LANE_W * 1e2 # Let's see more of the horizontal image
pts_world[:, 1] += 200 # Let's see more down the road
# H_metric := Homography mapping the image (pixel coords) to the
# world plane defined by pts_worldm
H_metric = cv2.getPerspectiveTransform(pts.astype('float32'),
pts_worldm.astype('float32'))
H = cv2.getPerspectiveTransform(pts.astype('float32'),
pts_world.astype('float32'))
## Estimate where the camera is w.r.t. the world ref. frame
R, T = estimate_extrinsic_parameters(H_metric)
xdist = T[0] - (LANE_W / 2.0)
print " Distance from center of lane: X={0:.2f} meters".format(
xdist)
LEFT_THRESH = -1.0 # Stay within 1.0 meters of the center of the lane
RIGHT_THRESH = 1.0
if xdist >= RIGHT_THRESH:
print " WARNING: Camera center is awfully close to the \
RIGHT side of the lane!"
elif xdist <= LEFT_THRESH:
print " WARNING: Camera center is awfully close to the \
LEFT side of the lane!"
Irgb = cv2.imread(imgpath, cv2.CV_LOAD_IMAGE_COLOR)
Iipm = cv2.warpPerspective(Irgb.astype('float64'), H, (1000, 700))
cv2.namedWindow("win2: Perspective-rectified image")
cv2.imshow("win2: Perspective-rectified image", Iipm.astype('uint8'))
Irgb = detect_lanes.draw_subwindow(Irgb, WIN_LEFT)
Irgb = detect_lanes.draw_subwindow(Irgb, WIN_RIGHT)
for _pt in pts:
_pt = tuple([intrnd(x) for x in _pt])
cv2.circle(Irgb, _pt, 3, (0, 0, 255))
print " ({0}/{1}) Displaying detected lanes.".format(
i + 1, len(imgpaths_test))
show_lanes(Irgb, line1, line2)
print "Done."
def estimate_planar_homography(I, line1, line2, K, win1, win2, lane_width):
""" Estimates the planar homography H between the camera image
plane, and the World (ground) plane.
The World reference frame is directly below the camera, with:
- Z-axis parallel to the road
- X-axis on the road plane
- Y-axis pointing upward from the road plane
- origin is on the road plane (Y=0), and halfway between the
two lanes boundaries (center of road).
Input:
nparray I
nparray line1, line2: [a, b, c]
nparray K
The 3x3 camera intrinsic matrix.
tuple win1, win2: (float x, float y, float w, float h)
float lane_width
Output:
nparray H
where H is a 3x3 homography.
"""
h, w = I.shape[0:2]
x_win1 = intrnd(w * win1[0])
y_win1 = intrnd(h * win1[1])
x_win2 = intrnd(w * win2[0])
y_win2 = intrnd(h * win2[1])
pts = []
NUM = 10
h_win = intrnd(h * win1[3]) # Assume window heights same btwn left/right
for i in xrange(NUM):
frac = i / float(NUM)
y_cur = intrnd((y_win1 - (h_win / 2) + frac * h_win))
pt_i = (compute_x(line1, y_cur), y_cur)
pt_j = (compute_x(line2, y_cur), y_cur)
pts.append((pt_i, pt_j))
r1 = solve_for_r1(pts, K, lane_width)
vanishing_pt = np.cross(line1, line2)
vanishing_pt = vanishing_pt / vanishing_pt[2]
vanishing_pt[1] = vanishing_pt[1]
## DEBUG Plot points on image, save to file for visual verification
Irgb = util.to_rgb(I)
COLOURS = [(255, 0, 0), (0, 255, 0)]
for i, (pt_i, pt_j) in enumerate(pts):
clr = COLOURS[i % 2]
cv2.circle(Irgb, tuple(map(intrnd, pt_i)), 5, clr)
cv2.circle(Irgb, tuple(map(intrnd, pt_j)), 5, clr)
cv2.circle(Irgb, (intrnd(vanishing_pt[0]), intrnd(vanishing_pt[1])), 5,
(0, 0, 255))
cv2.imwrite("_Irgb.png", Irgb)
r3 = solve_for_r3(vanishing_pt, line1, line2, K)
T = solve_for_t(pts, K, r1, r3, lane_width)
print "T_pre:", T
T = T * (2.1798 / T[1]) # Height of camera is 2.1798 meters
T[2] = 1 # We want the ref. frame to be directly below camera (why 1?!)
#T = T / np.linalg.norm(T)
print "T_post:", T
H = np.zeros([3, 3])
H[:, 0] = r1
H[:, 1] = r3
H[:, 2] = T
return np.dot(K, H)
def main():
args = parse_args()
if not os.path.exists(IMGSDIR_CALIB):
print "(ERROR) Calibration images not found. Please place \
the LDWS_calibrate_short/ images in the current directory."
exit(1)
if not os.path.exists(IMGSDIR_TEST):
print "(ERROR) Test images not found. Please place the \
LDWS_test_short/ images in the current directory."
exit(1)
imgpaths_calib = util.get_imgpaths(IMGSDIR_CALIB)
if not os.path.isdir(args.imgsdir):
imgpaths_test = [args.imgsdir]
else:
imgpaths_test = util.get_imgpaths(args.imgsdir)
if args.reuse_calib:
print "(Reusing camera calibration matrix)"
K = np.array([[ 674.07224154, 0., 262.77722917],
[ 0., 670.26875783, 330.21546389],
[ 0., 0., 1. ]])
else:
print "(Estimating camera matrix...)"
t = time.time()
K = calibrate_camera.calibrate_camera(imgpaths_calib, 8, 8, 0.048)
dur = time.time() - t
print "(Finished. {0:.4f})".format(dur)
for i, imgpath in enumerate(imgpaths_test):
print "\n==== ({0}/{1}) Detecting lanes... [{2}]====".format(i+1, len(imgpaths_test), os.path.split(imgpath)[1])
I = cv2.imread(imgpath, cv2.CV_LOAD_IMAGE_GRAYSCALE)
h, w = I.shape[0:2]
t = time.time()
line1, line2 = detect_lanes.detect_lanes(I, win1=WIN_LEFT, win2=WIN_RIGHT,
threshold1=110, threshold2=220,
apertureSize=3,
show_edges=False)
dur = time.time() - t
print " Finished detecting lanes ({0:.4f}s)".format(dur)
if line1 == None or line2 == None:
print "({0}/{1}) Error: Couldn't find lanes.".format(i+1, len(imgpaths_test))
continue
# Choose 4 points on the lanes to estimate the planar homography
y1 = intrnd(0.45 * h)
y2 = intrnd(0.65 * h)
pts = np.array([[compute_x(line1, y1), y1], # Left lane, far
[compute_x(line2, y1), y1], # Right lane, far
[compute_x(line1, y2), y2], # Left lane, close
[compute_x(line2, y2), y2]]) # Right lane, close
# These world points have the origin at the middle of the lane,
# directly below the camera.
# Depths 5.0, 3.0 are chosen arbitrarily, as we don't have depth
# information. Thus, the computed H will only be accurate in the
# X axis.
pts_worldm = np.array([[-LANE_W/2.0, 5.0], # Left lane, far
[LANE_W/2.0, 5.0], # Right lane, far
[-LANE_W/2.0, 3.0], # Left lane, close
[LANE_W/2.0, 3.0]]) # Right lane, close
# These world points are scaled+translated to generate a
# reasonably-sized image w/ perspective effects removed.
# Note: the origin here is not the same origin as in pts_worldm!
pts_world = np.array([[0.0, 0.0],
[LANE_W*1e2, 0.0],
[0.0, 500],
[LANE_W*1e2, 500.0]])
pts_world[:,0] += LANE_W*1e2 # Let's see more of the horizontal image
pts_world[:,1] += 200 # Let's see more down the road
# H_metric := Homography mapping the image (pixel coords) to the
# world plane defined by pts_worldm
H_metric = cv2.getPerspectiveTransform(pts.astype('float32'), pts_worldm.astype('float32'))
H = cv2.getPerspectiveTransform(pts.astype('float32'), pts_world.astype('float32'))
## Estimate where the camera is w.r.t. the world ref. frame
R, T = estimate_extrinsic_parameters(H_metric)
xdist = T[0] - (LANE_W / 2.0)
print " Distance from center of lane: X={0:.2f} meters".format(xdist)
LEFT_THRESH = -1.0 # Stay within 1.0 meters of the center of the lane
RIGHT_THRESH = 1.0
if xdist >= RIGHT_THRESH:
print " WARNING: Camera center is awfully close to the \
RIGHT side of the lane!"
elif xdist <= LEFT_THRESH:
print " WARNING: Camera center is awfully close to the \
LEFT side of the lane!"
Irgb = cv2.imread(imgpath, cv2.CV_LOAD_IMAGE_COLOR)
Iipm = cv2.warpPerspective(Irgb.astype('float64'), H, (1000, 700))
cv2.namedWindow("win2: Perspective-rectified image")
cv2.imshow("win2: Perspective-rectified image", Iipm.astype('uint8'))
Irgb = detect_lanes.draw_subwindow(Irgb, WIN_LEFT)
Irgb = detect_lanes.draw_subwindow(Irgb, WIN_RIGHT)
for _pt in pts:
_pt = tuple([intrnd(x) for x in _pt])
cv2.circle(Irgb, _pt, 3, (0, 0, 255))
print " ({0}/{1}) Displaying detected lanes.".format(i+1, len(imgpaths_test))
show_lanes(Irgb, line1, line2)
print "Done."
def estimate_planar_homography(I, line1, line2, K, win1, win2, lane_width):
""" Estimates the planar homography H between the camera image
plane, and the World (ground) plane.
The World reference frame is directly below the camera, with:
- Z-axis parallel to the road
- X-axis on the road plane
- Y-axis pointing upward from the road plane
- origin is on the road plane (Y=0), and halfway between the
two lanes boundaries (center of road).
Input:
nparray I
nparray line1, line2: [a, b, c]
nparray K
The 3x3 camera intrinsic matrix.
tuple win1, win2: (float x, float y, float w, float h)
float lane_width
Output:
nparray H
where H is a 3x3 homography.
"""
h, w = I.shape[0:2]
x_win1 = intrnd(w * win1[0])
y_win1 = intrnd(h * win1[1])
x_win2 = intrnd(w * win2[0])
y_win2 = intrnd(h * win2[1])
pts = []
NUM = 10
h_win = intrnd(h*win1[3]) # Assume window heights same btwn left/right
for i in xrange(NUM):
frac = i / float(NUM)
y_cur = intrnd((y_win1-(h_win/2) + frac*h_win))
pt_i = (compute_x(line1, y_cur), y_cur)
pt_j = (compute_x(line2, y_cur), y_cur)
pts.append((pt_i, pt_j))
r1 = solve_for_r1(pts,
K,
lane_width)
vanishing_pt = np.cross(line1, line2)
vanishing_pt = vanishing_pt / vanishing_pt[2]
vanishing_pt[1] = vanishing_pt[1]
## DEBUG Plot points on image, save to file for visual verification
Irgb = util.to_rgb(I)
COLOURS = [(255, 0, 0), (0, 255, 0)]
for i, (pt_i, pt_j) in enumerate(pts):
clr = COLOURS[i % 2]
cv2.circle(Irgb, tuple(map(intrnd, pt_i)), 5, clr)
cv2.circle(Irgb, tuple(map(intrnd, pt_j)), 5, clr)
cv2.circle(Irgb, (intrnd(vanishing_pt[0]), intrnd(vanishing_pt[1])), 5, (0, 0, 255))
cv2.imwrite("_Irgb.png", Irgb)
r3 = solve_for_r3(vanishing_pt, line1, line2, K)
T = solve_for_t(pts, K, r1, r3, lane_width)
print "T_pre:", T
T = T * (2.1798 / T[1]) # Height of camera is 2.1798 meters
T[2] = 1 # We want the ref. frame to be directly below camera (why 1?!)
#T = T / np.linalg.norm(T)
print "T_post:", T
H = np.zeros([3,3])
H[:, 0] = r1
H[:, 1] = r3
H[:, 2] = T
return np.dot(K, H)
