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(): args = parse_args() imgsdir = args.imgsdir imgpaths = util.get_imgpaths(imgsdir) print "(Info) Processing {0} images".format(len(imgpaths)) rows, cols = args.patternsize K = calibrate_camera(imgpaths, rows, cols, args.boxdim, SHOW_CB=args.show_cb) print "Computed K:" print K print "Done."
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)