def undistort_images():
"""Executed when script file is called directly"""
# get env vars
load_dotenv(find_dotenv())
repo_dir = os.environ['REPO_DIR']
# set working dirs
cal_img_dir = os.path.join(repo_dir, 'camera_cal')
dump_dir = os.path.join(repo_dir, 'data')
out_img_dir = os.path.join(repo_dir, 'output_images')
test_img_dir = os.path.join(repo_dir, 'test_images')
# find corners
obj_pts, img_pts = find_corners(cal_img_dir)
# calibrate camera
img_path = os.path.join(test_img_dir, 'test1.jpg')
pickle_path = os.path.join(dump_dir, 'calibration.p')
M, dist = calibrate_camera(obj_pts, img_pts, img_path, pickle_path)
# undistort test images
undistort_img_dir(test_img_dir, M, dist, out_img_dir)
# undistort a chessboard image
chessboard_path = os.path.join(out_img_dir, 'calibration3.jpg')
undistort_chessboard(chessboard_path, M, dist, out_img_dir)
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 test_koopa_singleimage(SHOW_EPIPOLAR=False):
""" Here, I want to do "undo" the perspective distortion in the
image, based on the fact that I know:
- The camera matrix K
- The scene is planar
- The location of four points in the image that create a
rectangle in the world plane with known metric lengths
Note: I don't have to necessarily know the metric lengths,
but at least I need to know length ratios to construct
a corrected image with length ratios preserved.
In the literature, this is also known as: perspective rectification,
perspective correction.
"""
imgpath = os.path.join(IMGSDIR_KOOPA_MED, 'DSCN0643.png')
pts1 = np.array([
[373.0, 97.0], # A (upper-left red corner)
[650.0, 95.0], # B (upper-right red corner)
[674.0, 215.0], # C (lower-right red corner)
[503.0, 322.0], # D (lower-left green corner)
[494.0, 322.0], # E (lower-right blue corner)
[303.0, 321.0], # F (lower-left blue corner)
[332.0, 225.0], # G (upper-left blue corner)
[335.0, 215.0], # H (lower-left red corner)
[475.0, 135.0], # K (tip of pen)
[507.0, 225.0], # L (upper-left green corner)
[362.0, 248.0], # M (tip of glue bottle)
])
worldpts = get_worldplane_coords()
assert len(pts1) == len(worldpts)
calib_imgpaths = util.get_imgpaths(IMGSDIR_CALIB_MED)
print "(Calibrating camera...)"
if True:
print "(Using pre-computed camera matrix K!)"
K = np.array([[158.23796519, 0.0, 482.07814366],
[0., 28.53758493, 333.32239125], [0., 0., 1.]])
else:
K = calibrate_camera.calibrate_camera(calib_imgpaths, 9, 6, 0.023)
print "Finished calibrating camera, K is:"
print K
Kinv = np.linalg.inv(K)
pts1_norm = normalize_coords(pts1, K)
HL = estimate_planar_homography(pts1_norm, worldpts)
print "Estimated homography, H is:"
print HL
print "rank(H):", np.linalg.matrix_rank(HL)
if np.linalg.matrix_rank(HL) != 3:
print " Oh no! H is not full rank!"
#### Normalize H := H / sigma_2(H_)
U, S, V = np.linalg.svd(HL)
H = HL / S[1]
#### Enforce positive depth constraint
flag_flipped = False
for i, pt1 in enumerate(pts1_norm):
pt2 = worldpts[i]
val = np.dot(np.hstack((pt2, [1.0])), np.dot(H, np.hstack(
(pt1, [1.0]))))
if val < 0:
if flag_flipped:
print "WOAH, flipping twice?!"
pdb.set_trace()
print "FLIP IT! val_{0} was: {1}".format(i, val)
H = H * -1
flag_flipped = True
# (Sanity check positive depth constraint)
for i, pt1 in enumerate(pts1_norm):
pt2 = worldpts[i]
val = np.dot(np.hstack((pt2, [1.0])), np.dot(H, np.hstack(
(pt1, [1.0]))))
if val < 0:
print "WOAH, positive depth constraint violated!?"
pdb.set_trace()
#### Check projection error from I1 -> I2.
errs = []
for i, pt1 in enumerate(pts1_norm):
pt1_h = np.hstack((pt1, np.array([1.0])))
pt2_h = np.hstack((worldpts[i], np.array([1.0])))
pt2_pred = np.dot(H, pt1_h)
pt2_pred = pt2_pred / pt2_pred[2]
errs.append(np.linalg.norm(pt2_h - pt2_pred))
print "Projection error: {0} (Mean: {1} std: {2})".format(
sum(errs), np.mean(errs), np.std(errs))
#### Check if planar epipolar constraint is satisfied:
#### x2_hat * H * x1 = 0.0
errs = []
for i, pt1 in enumerate(pts1_norm):
pt1_h = np.hstack((pt1, [1.0]))
pt2_hat = util_camera.make_crossprod_mat(worldpts[i])
errs.append(np.linalg.norm(np.dot(pt2_hat, np.dot(H, pt1_h))))
print "Epipolar constraint error: {0} (Mean: {1} std: {2})".format(
sum(errs), np.mean(errs), np.std(errs))
#### Sanity-check that world points project to pixel points
Hinv = np.linalg.inv(H)
errs = []
for i, worldpt in enumerate(worldpts):
pt_img = np.hstack((pts1[i], [1.0]))
p = np.dot(Hinv, np.hstack((worldpt, [1.0])))
p /= p[2]
p = np.dot(K, p)
errs.append(np.linalg.norm(pt_img - p))
print "world2img errors (in pixels): {0} (mean={1} std={2})".format(
sum(errs), np.mean(errs), np.std(errs))
#### Perform Inverse Perspective Mapping (undo perspective effects)
## Pixel coordinates of a region of the image known to:
## a.) Lie on the plane
## b.) Has known metric lengths
Hinv = np.linalg.inv(H)
# Define hardcoded pixel/world coordinates on the poster plane
# TODO: We could try generalizing this to new images by:
# a.) Asking the user to choose four image points, and enter
# in metric length information (simplest)
# b.) Automatically detect four corner points that correspond
# to a world rectangle (somehow). If there are no
# reference lengths, then I believe the best thing we can
# do is do a perspective-correction up to an affine
# transformation (since we don't know the correct X/Y
# lengths).
pts_pix = np.array(
[
[372.0, 97.0], # Upperleft of redbox (x,y)
[651.0, 95.0], # Upperright of redbox (x,y)
[303.0, 321.0], # Lowerleft of bluebox (x,y)
[695.0, 324.0]
], # Lowerright of greenbox (x,y)
)
pts_world = np.zeros([0, 2])
# Populate pts_world via H (maps image plane -> world plane)
for pt_pix in pts_pix:
pt_world = homo2pt(np.dot(H, np.dot(Kinv, pt2homo(pt_pix))))
pts_world = np.vstack((pts_world, pt_world))
pts_world *= 1000.0 # Let's get a reasonable-sized image please!
# These are hardcoded world coords, but, we can auto-gen them via
# H, Kinv as above. Isn't geometry nice?
#pts_world = np.array([[0.0, 0.0],
# [175.0, 0.0],
# [0.0, 134.0],
# [175.0, 134.0]])
IPM0, IPM1 = compute_IPM(pts_pix, pts_world)
print 'IPM0 is:'
print IPM0
print 'IPM1 is:'
print IPM1
I = cv2.imread(imgpath, cv.CV_LOAD_IMAGE_COLOR).astype('float64')
I = (2.0 * I) + 40 # Up that contrast! Orig. images are super dark.
h, w = I.shape[0:2]
# Determine bounding box of IPM-corrected image
a = homo2pt(np.dot(IPM1, pt2homo([0.0, 0.0]))) # upper-left corner
b = homo2pt(np.dot(IPM1, pt2homo([w - 1, 0.0]))) # upper-right corner
c = homo2pt(np.dot(IPM1, pt2homo([0.0, h - 1]))) # lower-left corner
d = homo2pt(np.dot(IPM1, pt2homo([w - 1, h - 1]))) # lower-right corner
w_out = intrnd(max(b[0] - a[0], d[0] - c[0]))
h_out = intrnd(max(c[1] - a[1], d[1] - b[1]))
print "New image dimensions: ({0} x {1}) (Orig: {2} x {3})".format(
w_out, h_out, w, h)
# Warp the entire image I with the homography IPM1
Iwarp = cv2.warpPerspective(I, IPM1, (w_out, h_out))
# Draw selected points on I
for pt in pts_pix:
cv2.circle(I, tupint(pt), 5, (0, 255, 0))
# Draw rectified-rectangle in Iwarp
a = homo2pt(np.dot(IPM1, pt2homo(pts_pix[0])))
b = homo2pt(np.dot(IPM1, pt2homo(pts_pix[3])))
cv2.rectangle(Iwarp, tupint(a), tupint(b), (0, 255, 0))
#Iwarp = transform_image.transform_image_forward(I, IPM)
print "(Displaying before/after images, press <any key> to exit.)"
cv2.namedWindow('original')
cv2.imshow('original', I.astype('uint8'))
cv2.namedWindow('corrected')
cv2.imshow('corrected', Iwarp.astype('uint8'))
cv2.waitKey(0)
def test_koopa(SHOW_EPIPOLAR=False):
""" Estimate the homography H between two image views of the planar
poster. Also decomposes H into {R, (1/d)T, N}.
"""
imgpath1 = os.path.join(IMGSDIR_KOOPA_SMALL, 'DSCN0643.png')
imgpath2 = os.path.join(IMGSDIR_KOOPA_SMALL, 'DSCN0648.png')
img1 = cv2.imread(imgpath1)
img2 = cv2.imread(imgpath2)
pts1_ = (
(150.0, 39.0), # Upperleft corner redbox
(220.0, 47.0), # Koopa's left-eye
(191.0, 55.0), # Tip of Koopa's pencil
(122.0, 128.0), # Lowerleft corner of bluebox
(144.0, 100.0), # Tip of glue bottle
(278.0, 129.0), # Lowerright corner of greenbox
)
pts2_ = (
(276.0, 34.0), # Upperleft corner redbox
(327.0, 61.0), # Koopa's left-eye
(286.0, 57.0), # Tip of Koopa's pencil
(141.0, 86.0), # Lowerleft corner of bluebox
(188.0, 75.0), # Tip of glue bottle
(237.0, 154.0), # Lowerright corner of greenbox
)
calib_imgpaths = util.get_imgpaths(IMGSDIR_CALIB_SMALL)
print "(Calibrating camera...)"
K = calibrate_camera.calibrate_camera(calib_imgpaths, 9, 6, 0.023)
print "Finished calibrating camera, K is:"
print K
print "(Estimating homography...)"
pts1 = tup2nparray(pts1_)
pts2 = tup2nparray(pts2_)
pts1_norm = normalize_coords(pts1, K)
pts2_norm = normalize_coords(pts2, K)
# H goes from img1 -> img2
H_ = estimate_planar_homography(pts1_norm, pts2_norm)
print "Estimated homography, H is:",
print H_
print "rank(H):", np.linalg.matrix_rank(H_)
if np.linalg.matrix_rank(H_) != 3:
print " Oh no! H is not full rank!"
#### Normalize H := H / sigma_2(H_)
U, S, V = np.linalg.svd(H_)
H = H_ / S[1]
#### Enforce positive depth constraint
flag_flipped = False
for i, pt1 in enumerate(pts1_norm):
pt2 = pts2_norm[i]
val = np.dot(np.hstack((pt2, [1.0])), np.dot(H, np.hstack(
(pt1, [1.0]))))
if val < 0:
if flag_flipped:
print "WOAH, flipping twice?!"
pdb.set_trace()
print "FLIP IT! val_{0} was: {1}".format(i, val)
H = H * -1
flag_flipped = True
# (Sanity check positive depth constraint)
for i, pt1 in enumerate(pts1_norm):
pt2 = pts2_norm[i]
val = np.dot(np.hstack((pt2, [1.0])), np.dot(H, np.hstack(
(pt1, [1.0]))))
if val < 0:
print "WOAH, positive depth constraint violated!?"
pdb.set_trace()
#### Check projection error from I1 -> I2.
errs = []
for i, pt1 in enumerate(pts1_norm):
pt1_h = np.hstack((pt1, np.array([1.0])))
pt2_h = np.hstack((pts2_norm[i], np.array([1.0])))
pt2_pred = np.dot(H, pt1_h)
pt2_pred = pt2_pred / pt2_pred[2]
errs.append(np.linalg.norm(pt2_h - pt2_pred))
print "Projection error: {0} (Mean: {1} std: {2})".format(
sum(errs), np.mean(errs), np.std(errs))
#### Check if planar epipolar constraint is satisfied:
#### x2_hat * H * x1 = 0.0
errs = []
for i, pt1 in enumerate(pts1_norm):
pt1_h = np.hstack((pt1, [1.0]))
pt2_hat = util_camera.make_crossprod_mat(pts2_norm[i])
errs.append(np.linalg.norm(np.dot(pt2_hat, np.dot(H, pt1_h))))
print "Epipolar constraint error: {0} (Mean: {1} std: {2})".format(
sum(errs), np.mean(errs), np.std(errs))
#### Draw epipolar lines
Irgb1 = cv2.imread(imgpath1, cv2.CV_LOAD_IMAGE_COLOR)
Irgb2 = cv2.imread(imgpath2, cv2.CV_LOAD_IMAGE_COLOR)
if SHOW_EPIPOLAR:
draw_epipolar_lines(Irgb1, Irgb2, pts1, pts2, H)
decomps = decompose_H(H)
print
for i, (R, Ts, N) in enumerate(decomps):
print "==== Decomposition {0} ====".format(i)
print " R is:", R
print " Ts is:", Ts
print " N is:", N
H_redone = (R + np.dot(np.array([Ts]).T, np.array([N])))
print "norm((R+Ts*N.T) - H, 'fro'):", np.linalg.norm(
H - H_redone, 'fro')
print " det(R)={0} rank(R)={1}".format(np.linalg.det(R),
np.linalg.matrix_rank(R))
#### Sanity check that H_redone still maps I1 to I2
errs = []
for ii, pt1 in enumerate(pts1_norm):
pt1_h = np.hstack(([pt1, [1.0]]))
pt2_h = np.hstack((pts2_norm[ii], [1.0]))
pt1_proj = np.dot(H_redone, pt1_h)
pt1_proj /= pt1_proj[2]
print pt1_proj
print pt2_h
print
errs.append(np.linalg.norm(pt1_proj - pt2_h))
print "Reprojection error: {0} (mean={1}, std={2})".format(
sum(errs), np.mean(errs), np.std(errs))
def test_koopa(SHOW_EPIPOLAR=False):
""" Estimate the homography H between two image views of the planar
poster. Also decomposes H into {R, (1/d)T, N}.
"""
imgpath1 = os.path.join(IMGSDIR_KOOPA_SMALL, 'DSCN0643.png')
imgpath2 = os.path.join(IMGSDIR_KOOPA_SMALL, 'DSCN0648.png')
img1 = cv2.imread(imgpath1)
img2 = cv2.imread(imgpath2)
pts1_ = ((150.0, 39.0), # Upperleft corner redbox
(220.0, 47.0), # Koopa's left-eye
(191.0, 55.0), # Tip of Koopa's pencil
(122.0, 128.0), # Lowerleft corner of bluebox
(144.0, 100.0), # Tip of glue bottle
(278.0, 129.0), # Lowerright corner of greenbox
)
pts2_ = ((276.0, 34.0), # Upperleft corner redbox
(327.0, 61.0), # Koopa's left-eye
(286.0, 57.0), # Tip of Koopa's pencil
(141.0, 86.0), # Lowerleft corner of bluebox
(188.0, 75.0), # Tip of glue bottle
(237.0, 154.0), # Lowerright corner of greenbox
)
calib_imgpaths = util.get_imgpaths(IMGSDIR_CALIB_SMALL)
print "(Calibrating camera...)"
K = calibrate_camera.calibrate_camera(calib_imgpaths, 9, 6, 0.023)
print "Finished calibrating camera, K is:"
print K
print "(Estimating homography...)"
pts1 = tup2nparray(pts1_)
pts2 = tup2nparray(pts2_)
pts1_norm = normalize_coords(pts1, K)
pts2_norm = normalize_coords(pts2, K)
# H goes from img1 -> img2
H_ = estimate_planar_homography(pts1_norm, pts2_norm)
print "Estimated homography, H is:",
print H_
print "rank(H):", np.linalg.matrix_rank(H_)
if np.linalg.matrix_rank(H_) != 3:
print " Oh no! H is not full rank!"
#### Normalize H := H / sigma_2(H_)
U, S, V = np.linalg.svd(H_)
H = H_ / S[1]
#### Enforce positive depth constraint
flag_flipped = False
for i, pt1 in enumerate(pts1_norm):
pt2 = pts2_norm[i]
val = np.dot(np.hstack((pt2, [1.0])), np.dot(H, np.hstack((pt1, [1.0]))))
if val < 0:
if flag_flipped:
print "WOAH, flipping twice?!"
pdb.set_trace()
print "FLIP IT! val_{0} was: {1}".format(i, val)
H = H * -1
flag_flipped = True
# (Sanity check positive depth constraint)
for i, pt1 in enumerate(pts1_norm):
pt2 = pts2_norm[i]
val = np.dot(np.hstack((pt2, [1.0])), np.dot(H, np.hstack((pt1, [1.0]))))
if val < 0:
print "WOAH, positive depth constraint violated!?"
pdb.set_trace()
#### Check projection error from I1 -> I2.
errs = []
for i, pt1 in enumerate(pts1_norm):
pt1_h = np.hstack((pt1, np.array([1.0])))
pt2_h = np.hstack((pts2_norm[i], np.array([1.0])))
pt2_pred = np.dot(H, pt1_h)
pt2_pred = pt2_pred / pt2_pred[2]
errs.append(np.linalg.norm(pt2_h - pt2_pred))
print "Projection error: {0} (Mean: {1} std: {2})".format(sum(errs), np.mean(errs), np.std(errs))
#### Check if planar epipolar constraint is satisfied:
#### x2_hat * H * x1 = 0.0
errs = []
for i, pt1 in enumerate(pts1_norm):
pt1_h = np.hstack((pt1, [1.0]))
pt2_hat = util_camera.make_crossprod_mat(pts2_norm[i])
errs.append(np.linalg.norm(np.dot(pt2_hat, np.dot(H, pt1_h))))
print "Epipolar constraint error: {0} (Mean: {1} std: {2})".format(sum(errs), np.mean(errs), np.std(errs))
#### Draw epipolar lines
Irgb1 = cv2.imread(imgpath1, cv2.CV_LOAD_IMAGE_COLOR)
Irgb2 = cv2.imread(imgpath2, cv2.CV_LOAD_IMAGE_COLOR)
if SHOW_EPIPOLAR:
draw_epipolar_lines(Irgb1, Irgb2, pts1, pts2, H)
decomps = decompose_H(H)
print
for i, (R, Ts, N) in enumerate(decomps):
print "==== Decomposition {0} ====".format(i)
print " R is:", R
print " Ts is:", Ts
print " N is:", N
H_redone = (R + np.dot(np.array([Ts]).T, np.array([N])))
print "norm((R+Ts*N.T) - H, 'fro'):", np.linalg.norm(H - H_redone, 'fro')
print " det(R)={0} rank(R)={1}".format(np.linalg.det(R), np.linalg.matrix_rank(R))
#### Sanity check that H_redone still maps I1 to I2
errs = []
for ii, pt1 in enumerate(pts1_norm):
pt1_h = np.hstack(([pt1, [1.0]]))
pt2_h = np.hstack((pts2_norm[ii], [1.0]))
pt1_proj = np.dot(H_redone, pt1_h)
pt1_proj /= pt1_proj[2]
print pt1_proj
print pt2_h
print
errs.append(np.linalg.norm(pt1_proj - pt2_h))
print "Reprojection error: {0} (mean={1}, std={2})".format(sum(errs), np.mean(errs), np.std(errs))
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 test_koopa_singleimage(SHOW_EPIPOLAR=False):
""" Here, I want to do "undo" the perspective distortion in the
image, based on the fact that I know:
- The camera matrix K
- The scene is planar
- The location of four points in the image that create a
rectangle in the world plane with known metric lengths
Note: I don't have to necessarily know the metric lengths,
but at least I need to know length ratios to construct
a corrected image with length ratios preserved.
In the literature, this is also known as: perspective rectification,
perspective correction.
"""
imgpath = os.path.join(IMGSDIR_KOOPA_MED, 'DSCN0643.png')
pts1 = np.array([[373.0, 97.0], # A (upper-left red corner)
[650.0, 95.0], # B (upper-right red corner)
[674.0, 215.0], # C (lower-right red corner)
[503.0, 322.0], # D (lower-left green corner)
[494.0, 322.0], # E (lower-right blue corner)
[303.0, 321.0], # F (lower-left blue corner)
[332.0, 225.0], # G (upper-left blue corner)
[335.0, 215.0], # H (lower-left red corner)
[475.0, 135.0], # K (tip of pen)
[507.0, 225.0], # L (upper-left green corner)
[362.0, 248.0], # M (tip of glue bottle)
])
worldpts = get_worldplane_coords()
assert len(pts1) == len(worldpts)
calib_imgpaths = util.get_imgpaths(IMGSDIR_CALIB_MED)
print "(Calibrating camera...)"
if True:
print "(Using pre-computed camera matrix K!)"
K = np.array([[ 158.23796519, 0.0, 482.07814366],
[ 0., 28.53758493, 333.32239125],
[ 0., 0., 1. ]])
else:
K = calibrate_camera.calibrate_camera(calib_imgpaths, 9, 6, 0.023)
print "Finished calibrating camera, K is:"
print K
Kinv = np.linalg.inv(K)
pts1_norm = normalize_coords(pts1, K)
HL = estimate_planar_homography(pts1_norm, worldpts)
print "Estimated homography, H is:"
print HL
print "rank(H):", np.linalg.matrix_rank(HL)
if np.linalg.matrix_rank(HL) != 3:
print " Oh no! H is not full rank!"
#### Normalize H := H / sigma_2(H_)
U, S, V = np.linalg.svd(HL)
H = HL / S[1]
#### Enforce positive depth constraint
flag_flipped = False
for i, pt1 in enumerate(pts1_norm):
pt2 = worldpts[i]
val = np.dot(np.hstack((pt2, [1.0])), np.dot(H, np.hstack((pt1, [1.0]))))
if val < 0:
if flag_flipped:
print "WOAH, flipping twice?!"
pdb.set_trace()
print "FLIP IT! val_{0} was: {1}".format(i, val)
H = H * -1
flag_flipped = True
# (Sanity check positive depth constraint)
for i, pt1 in enumerate(pts1_norm):
pt2 = worldpts[i]
val = np.dot(np.hstack((pt2, [1.0])), np.dot(H, np.hstack((pt1, [1.0]))))
if val < 0:
print "WOAH, positive depth constraint violated!?"
pdb.set_trace()
#### Check projection error from I1 -> I2.
errs = []
for i, pt1 in enumerate(pts1_norm):
pt1_h = np.hstack((pt1, np.array([1.0])))
pt2_h = np.hstack((worldpts[i], np.array([1.0])))
pt2_pred = np.dot(H, pt1_h)
pt2_pred = pt2_pred / pt2_pred[2]
errs.append(np.linalg.norm(pt2_h - pt2_pred))
print "Projection error: {0} (Mean: {1} std: {2})".format(sum(errs), np.mean(errs), np.std(errs))
#### Check if planar epipolar constraint is satisfied:
#### x2_hat * H * x1 = 0.0
errs = []
for i, pt1 in enumerate(pts1_norm):
pt1_h = np.hstack((pt1, [1.0]))
pt2_hat = util_camera.make_crossprod_mat(worldpts[i])
errs.append(np.linalg.norm(np.dot(pt2_hat, np.dot(H, pt1_h))))
print "Epipolar constraint error: {0} (Mean: {1} std: {2})".format(sum(errs), np.mean(errs), np.std(errs))
#### Sanity-check that world points project to pixel points
Hinv = np.linalg.inv(H)
errs = []
for i, worldpt in enumerate(worldpts):
pt_img = np.hstack((pts1[i], [1.0]))
p = np.dot(Hinv, np.hstack((worldpt, [1.0])))
p /= p[2]
p = np.dot(K, p)
errs.append(np.linalg.norm(pt_img - p))
print "world2img errors (in pixels): {0} (mean={1} std={2})".format(sum(errs), np.mean(errs), np.std(errs))
#### Perform Inverse Perspective Mapping (undo perspective effects)
## Pixel coordinates of a region of the image known to:
## a.) Lie on the plane
## b.) Has known metric lengths
Hinv = np.linalg.inv(H)
# Define hardcoded pixel/world coordinates on the poster plane
# TODO: We could try generalizing this to new images by:
# a.) Asking the user to choose four image points, and enter
# in metric length information (simplest)
# b.) Automatically detect four corner points that correspond
# to a world rectangle (somehow). If there are no
# reference lengths, then I believe the best thing we can
# do is do a perspective-correction up to an affine
# transformation (since we don't know the correct X/Y
# lengths).
pts_pix = np.array([[372.0, 97.0], # Upperleft of redbox (x,y)
[651.0, 95.0], # Upperright of redbox (x,y)
[303.0, 321.0], # Lowerleft of bluebox (x,y)
[695.0, 324.0]], # Lowerright of greenbox (x,y)
)
pts_world = np.zeros([0, 2])
# Populate pts_world via H (maps image plane -> world plane)
for pt_pix in pts_pix:
pt_world = homo2pt(np.dot(H, np.dot(Kinv, pt2homo(pt_pix))))
pts_world = np.vstack((pts_world, pt_world))
pts_world *= 1000.0 # Let's get a reasonable-sized image please!
# These are hardcoded world coords, but, we can auto-gen them via
# H, Kinv as above. Isn't geometry nice?
#pts_world = np.array([[0.0, 0.0],
# [175.0, 0.0],
# [0.0, 134.0],
# [175.0, 134.0]])
IPM0, IPM1 = compute_IPM(pts_pix, pts_world)
print 'IPM0 is:'
print IPM0
print 'IPM1 is:'
print IPM1
I = cv2.imread(imgpath, cv.CV_LOAD_IMAGE_COLOR).astype('float64')
I = (2.0*I) + 40 # Up that contrast! Orig. images are super dark.
h, w = I.shape[0:2]
# Determine bounding box of IPM-corrected image
a = homo2pt(np.dot(IPM1, pt2homo([0.0, 0.0]))) # upper-left corner
b = homo2pt(np.dot(IPM1, pt2homo([w-1, 0.0]))) # upper-right corner
c = homo2pt(np.dot(IPM1, pt2homo([0.0, h-1]))) # lower-left corner
d = homo2pt(np.dot(IPM1, pt2homo([w-1, h-1]))) # lower-right corner
w_out = intrnd(max(b[0] - a[0], d[0] - c[0]))
h_out = intrnd(max(c[1] - a[1], d[1] - b[1]))
print "New image dimensions: ({0} x {1}) (Orig: {2} x {3})".format(w_out, h_out, w, h)
# Warp the entire image I with the homography IPM1
Iwarp = cv2.warpPerspective(I, IPM1, (w_out, h_out))
# Draw selected points on I
for pt in pts_pix:
cv2.circle(I, tupint(pt), 5, (0, 255, 0))
# Draw rectified-rectangle in Iwarp
a = homo2pt(np.dot(IPM1, pt2homo(pts_pix[0])))
b = homo2pt(np.dot(IPM1, pt2homo(pts_pix[3])))
cv2.rectangle(Iwarp, tupint(a), tupint(b), (0, 255, 0))
#Iwarp = transform_image.transform_image_forward(I, IPM)
print "(Displaying before/after images, press <any key> to exit.)"
cv2.namedWindow('original')
cv2.imshow('original', I.astype('uint8'))
cv2.namedWindow('corrected')
cv2.imshow('corrected', Iwarp.astype('uint8'))
cv2.waitKey(0)