def show_lanes(Irgb, line1, line2):
Irgb = util_camera.draw_line(Irgb, line1)
Irgb = util_camera.draw_line(Irgb, line2)
cv2.namedWindow("win1: Detected Lanes")
cv2.imshow("win1: Detected Lanes", Irgb)
print "(press <enter> to continue, with the imdisplay window active)"
cv2.waitKey(0)
def draw_epipolar_lines(Irgb1, Irgb2, pts1, pts2, H):
""" Draws epipolar lines, and displays them to the user in an
interactive manner.
Input:
nparray Irgb1, Irgb2: H x W x 3
nparray pts1, pts2: N x 2
nparray H: 3x3
"""
h, w = Irgb1.shape[0:2]
for i, pt1 in enumerate(pts1):
Irgb1_ = Irgb1.copy()
Irgb2_ = Irgb2.copy()
pt2 = pts2[i]
pt1_h = np.hstack((pt1, np.array([1.0])))
pt2_h = np.hstack((pt2, np.array([1.0])))
epiline2 = np.dot(util_camera.make_crossprod_mat(pt2_h), np.dot(H, pt1_h))
epiline1 = np.dot(H.T, epiline2)
epiline1 = epiline1 / epiline1[2]
epiline2 = epiline2 / epiline2[2]
print "Epiline1 is: slope={0} y-int={1}".format(-epiline1[0] / epiline1[1],
-epiline1[2] / epiline1[1])
print "Epiline2 is: slope={0} y-int={1}".format(-epiline2[0] / epiline2[1],
-epiline2[2] / epiline2[1])
Irgb1_ = util_camera.draw_line(Irgb1_, epiline1)
Irgb2_ = util_camera.draw_line(Irgb2_, epiline2)
cv.Circle(cv.fromarray(Irgb1_), tuple(intrnd(*pts1[i])), 3, (255, 0, 0))
cv.Circle(cv.fromarray(Irgb2_), tuple(intrnd(*pts2[i])), 3, (255, 0, 0))
print "(Displaying epipolar lines from img1 to img2. Press <enter> to continue.)"
cv2.namedWindow('display1')
cv2.imshow('display1', Irgb1_)
cv2.namedWindow('display2')
cv2.imshow('display2', Irgb2_)
cv2.waitKey(0)
def show_lanes(Irgb, line1, line2):
Irgb = util_camera.draw_line(Irgb, line1)
Irgb = util_camera.draw_line(Irgb, line2)
cv2.namedWindow("win1: Detected Lanes")
cv2.imshow("win1: Detected Lanes", Irgb)
print "(press <enter> to continue, with the imdisplay window active)"
cv2.waitKey(0)
def main():
args = parse_args()
threshold1 = args.Tlow
threshold2 = 2 * args.Tlow # Canny recommends a ratio of 1:2
win1 = args.win1
win2 = args.win2
imgsdir = args.imgsdir
if not os.path.isdir(imgsdir):
imgpaths = [imgsdir]
else:
imgpaths = util.get_imgpaths(imgsdir, n=args.n)
for i, imgpath in enumerate(imgpaths):
print("({0}/{1}): Image={2}".format(i + 1, len(imgpaths), imgpath))
I = cv2.imread(imgpath, cv2.CV_LOAD_IMAGE_GRAYSCALE)
line1, line2 = detect_lanes(I,
threshold1=threshold1,
threshold2=threshold2,
apertureSize=args.ksize)
if line1 == None and line2 == None:
print(" Error: Couldn't find lanes.")
continue
if line1 == None:
print(" Error: Couldn't find left lane")
if line2 == None:
print(" Error: Couldn't find right lane.")
#Irgb = plot_lines(I, line1, line2)
Irgb = cv2.imread(imgpath, cv2.CV_LOAD_IMAGE_COLOR)
Irgb = util_camera.draw_line(Irgb, line1, (255, 0, 0))
Irgb = util_camera.draw_line(Irgb, line2, (0, 255, 0))
# Draw subwindows on image
Irgb = draw_subwindow(Irgb, win1, colour=(255, 0, 0))
Irgb = draw_subwindow(Irgb, win2, colour=(0, 255, 0))
cv2.imwrite('{0}_lines.png'.format(util.get_filename(imgpath)), Irgb)
print " LeftLane: {0} RightLane: {1}".format(line1, line2)
print("Done.")
def main():
args = parse_args()
threshold1 = args.Tlow
threshold2 = 2 * args.Tlow # Canny recommends a ratio of 1:2
win1 = args.win1
win2 = args.win2
imgsdir = args.imgsdir
if not os.path.isdir(imgsdir):
imgpaths = [imgsdir]
else:
imgpaths = util.get_imgpaths(imgsdir, n=args.n)
for i, imgpath in enumerate(imgpaths):
print("({0}/{1}): Image={2}".format(i+1, len(imgpaths), imgpath))
I = cv2.imread(imgpath, cv2.CV_LOAD_IMAGE_GRAYSCALE)
line1, line2 = detect_lanes(I, threshold1=threshold1, threshold2=threshold2, apertureSize=args.ksize)
if line1 == None and line2 == None:
print(" Error: Couldn't find lanes.")
continue
if line1 == None:
print(" Error: Couldn't find left lane")
if line2 == None:
print(" Error: Couldn't find right lane.")
#Irgb = plot_lines(I, line1, line2)
Irgb = cv2.imread(imgpath, cv2.CV_LOAD_IMAGE_COLOR)
Irgb = util_camera.draw_line(Irgb, line1, (255, 0, 0))
Irgb = util_camera.draw_line(Irgb, line2, (0, 255, 0))
# Draw subwindows on image
Irgb = draw_subwindow(Irgb, win1, colour=(255, 0, 0))
Irgb = draw_subwindow(Irgb, win2, colour=(0, 255, 0))
cv2.imwrite('{0}_lines.png'.format(util.get_filename(imgpath)), Irgb)
print " LeftLane: {0} RightLane: {1}".format(line1, line2)
print("Done.")
def main():
# K matrix given by the Caltech Lanes dataset (CameraInfo.txt)
K = np.array([[309.4362, 0, 317.9034], [0, 344.2161, 256.5352], [0, 0, 1]])
line1 = np.array([1.30459272, 1., -589.16024465])
line2 = np.array([-1.26464497, 1., 228.18829664])
win1 = (0.4, 0.60, 0.2, 0.25)
win2 = (0.62, 0.60, 0.2, 0.25)
lane_width = 3.66 # 3.66 meters
imgpath = 'imgs_sample/f00001.png'
I = cv2.imread(imgpath, cv2.CV_LOAD_IMAGE_GRAYSCALE)
#### Draw lines on _Irgbline.png to sanity check lane detection
Irgb = cv2.imread(imgpath, cv2.CV_LOAD_IMAGE_COLOR)
Irgb = util_camera.draw_line(Irgb, line1, (0, 255, 0))
Irgb = util_camera.draw_line(Irgb, line2, (255, 0, 0))
cv2.imwrite("_Irgbline.png", Irgb)
H = estimate_planar_homography(I, line1, line2, K, win1, win2, lane_width)
print H
print " rank(H): {0}".format(np.linalg.matrix_rank(H))
print " det(H): {0}".format(np.linalg.det(H))
if np.linalg.matrix_rank(H) == 3:
print "The following should be identity (inv(H) * H):"
print np.dot(numpy.linalg.inv(H), H)
print "(Evaluating a few world points to see where they lie on the image)"
Irgb = cv2.imread(imgpath, cv2.CV_LOAD_IMAGE_COLOR)
# world point: (X, Z, 1), i.e. point on world plane (road)
world_pts = [
# Points in MIDDLE of lane, going towards horizon
((0, 0, 1), (0, 1, 1), (0, 2, 1), (0, 4, 1), (0, 8, 1), (0, 16, 1),
(0, 32, 1), (0, 10000, 1)),
# Points on LEFT lane, going towards horizon
((-1.83, 0, 1), (-1.83, 2, 1), (-1.83, 4, 1), (-1.83, 8, 1),
(-1.83, 10000, 1)),
# Points on RIGHT lane, going towards horizon
((1.83, 0, 1), (1.83, 2, 1), (1.83, 4, 1), (1.83, 8, 1), (1.83, 10000,
1))
]
colours = [(255, 0, 0), (0, 255, 0), (0, 0, 255)]
for i, sub_pts in enumerate(world_pts):
clr = colours[i % len(colours)]
for pt in sub_pts:
pt_np = np.array(pt)
pt_img = np.dot(H, pt_np)
pt_img = pt_img / pt_img[2]
print "(i={0}) World {1} -> {2}".format(i, pt, pt_img)
cv2.circle(Irgb, (intrnd(pt_img[0]), intrnd(pt_img[1])), 3, clr)
print
cv2.imwrite("_Irgb_pts.png", Irgb)
print "Done."
def main():
# K matrix given by the Caltech Lanes dataset (CameraInfo.txt)
K = np.array([[309.4362, 0, 317.9034],
[0, 344.2161, 256.5352],
[0, 0, 1 ]])
line1 = np.array([ 1.30459272, 1., -589.16024465])
line2 = np.array([ -1.26464497, 1., 228.18829664])
win1 = (0.4, 0.60, 0.2, 0.25)
win2 = (0.62, 0.60, 0.2, 0.25)
lane_width = 3.66 # 3.66 meters
imgpath = 'imgs_sample/f00001.png'
I = cv2.imread(imgpath, cv2.CV_LOAD_IMAGE_GRAYSCALE)
#### Draw lines on _Irgbline.png to sanity check lane detection
Irgb = cv2.imread(imgpath, cv2.CV_LOAD_IMAGE_COLOR)
Irgb = util_camera.draw_line(Irgb, line1, (0, 255, 0))
Irgb = util_camera.draw_line(Irgb, line2, (255, 0, 0))
cv2.imwrite("_Irgbline.png", Irgb)
H = estimate_planar_homography(I, line1, line2, K, win1, win2, lane_width)
print H
print " rank(H): {0}".format(np.linalg.matrix_rank(H))
print " det(H): {0}".format(np.linalg.det(H))
if np.linalg.matrix_rank(H) == 3:
print "The following should be identity (inv(H) * H):"
print np.dot(numpy.linalg.inv(H), H)
print "(Evaluating a few world points to see where they lie on the image)"
Irgb = cv2.imread(imgpath, cv2.CV_LOAD_IMAGE_COLOR)
# world point: (X, Z, 1), i.e. point on world plane (road)
world_pts = [
# Points in MIDDLE of lane, going towards horizon
((0, 0, 1), (0, 1, 1), (0, 2, 1), (0, 4, 1), (0, 8, 1), (0, 16, 1), (0, 32, 1), (0, 10000, 1)),
# Points on LEFT lane, going towards horizon
((-1.83, 0, 1), (-1.83, 2, 1), (-1.83, 4, 1), (-1.83, 8, 1), (-1.83, 10000, 1)),
# Points on RIGHT lane, going towards horizon
((1.83, 0, 1), (1.83, 2, 1), (1.83, 4, 1), (1.83, 8, 1), (1.83, 10000, 1))
]
colours = [(255, 0, 0), (0, 255, 0), (0, 0, 255)]
for i, sub_pts in enumerate(world_pts):
clr = colours[i % len(colours)]
for pt in sub_pts:
pt_np = np.array(pt)
pt_img = np.dot(H, pt_np)
pt_img = pt_img / pt_img[2]
print "(i={0}) World {1} -> {2}".format(i, pt, pt_img)
cv2.circle(Irgb, (intrnd(pt_img[0]), intrnd(pt_img[1])), 3, clr)
print
cv2.imwrite("_Irgb_pts.png", Irgb)
print "Done."
def draw_epipolar_lines(Irgb1, Irgb2, pts1, pts2, H):
""" Draws epipolar lines, and displays them to the user in an
interactive manner.
Input:
nparray Irgb1, Irgb2: H x W x 3
nparray pts1, pts2: N x 2
nparray H: 3x3
"""
h, w = Irgb1.shape[0:2]
for i, pt1 in enumerate(pts1):
Irgb1_ = Irgb1.copy()
Irgb2_ = Irgb2.copy()
pt2 = pts2[i]
pt1_h = np.hstack((pt1, np.array([1.0])))
pt2_h = np.hstack((pt2, np.array([1.0])))
epiline2 = np.dot(util_camera.make_crossprod_mat(pt2_h),
np.dot(H, pt1_h))
epiline1 = np.dot(H.T, epiline2)
epiline1 = epiline1 / epiline1[2]
epiline2 = epiline2 / epiline2[2]
print "Epiline1 is: slope={0} y-int={1}".format(
-epiline1[0] / epiline1[1], -epiline1[2] / epiline1[1])
print "Epiline2 is: slope={0} y-int={1}".format(
-epiline2[0] / epiline2[1], -epiline2[2] / epiline2[1])
Irgb1_ = util_camera.draw_line(Irgb1_, epiline1)
Irgb2_ = util_camera.draw_line(Irgb2_, epiline2)
cv.Circle(cv.fromarray(Irgb1_), tuple(intrnd(*pts1[i])), 3,
(255, 0, 0))
cv.Circle(cv.fromarray(Irgb2_), tuple(intrnd(*pts2[i])), 3,
(255, 0, 0))
print "(Displaying epipolar lines from img1 to img2. Press <enter> to continue.)"
cv2.namedWindow('display1')
cv2.imshow('display1', Irgb1_)
cv2.namedWindow('display2')
cv2.imshow('display2', Irgb2_)
cv2.waitKey(0)
