def main(): ap = argparse.ArgumentParser() ap.add_argument("-c", "--calib", type = str, required = True, help = "Path to calibration files (camera matrix and distortion coefficient)") ap.add_argument("-i", "--input", type = str, required = True, help = "Path to input files") ap.add_argument("-o", "--output", type = str, default = "", help = "Path to output files (omit if same as input)") ap.add_argument("overlap", type = float, help = "Percentage of overlap in decimal") args = vars(ap.parse_args()) calib_path = args["calib"] if not os.path.isdir(calib_path): print('Invalid calibration path!') return in_path = args["input"] if not os.path.isdir(in_path): print('Invalid input path!') return out_path = args["output"] if out_path == "": out_path = os.path.join(in_path, 'undistort') if not os.path.isdir(out_path): os.mkdir(out_path) else: shutil.rmtree(out_path) overlap = args["overlap"] undistort.undistort(calib_path, in_path, out_path) mosaic(in_path, overlap)
def pipeline(img, s_thresh=(100, 255), sx_thresh=(15, 255)): img = undistort(img) img = np.copy(img) #Convert to HLS color space and separate the V channel hls = cv2.cvtColor(img, cv2.COLOR_RGB2HLS) print(hls) l_channel = hls[:, :, 1] s_channel = hls[:, :, 2] h_channel = hls[:, :, 0] #Sobel x sobelx = cv2.Sobel(l_channel, cv2.CV_64F, 1, 1) # Take the derivative in x abs_sobelx = np.absolute( sobelx ) # Absolute x derivative to accentuate lines away from horizontal scaled_sobel = np.uint8(255 * abs_sobelx / np.max(abs_sobelx)) # Threshold x gradient sxbinary = np.zeros_like(scaled_sobel) sxbinary[(scaled_sobel >= sx_thresh[0]) & (scaled_sobel <= sx_thresh[1])] = 1 # Threshold color channel s_binary = np.zeros_like(s_channel) s_binary[(s_channel >= s_thresh[0]) & (s_channel <= s_thresh[1])] = 1 color_binary = np.dstack( (np.zeros_like(sxbinary), sxbinary, s_binary)) * 255 print(color_binary) combined_binary = np.zeros_like(sxbinary) print(combined_binary) combined_binary[(s_binary == 1) | (sxbinary == 1)] = 1 return combined_binary
def process_image(oimg): uimg = undistort.undistort(oimg) _, _, _, timg = threshold.threshold(uimg) wimg = topview.warp(timg) left_fit, right_fit, roc, dfc, ly, lx, ry, rx = lane_finder.find(wimg) result = draw.draw(uimg, wimg, left_fit, right_fit, roc, dfc, ly, lx, ry, rx, False) return result
def _on_loop(self, policy): """ Logical loop """ self._timer.tick() if self.key == 'q': sys.exit(-1) if self.key == 't': self.training = not self.training self.key = '' if self.key == 's': self._enable_auto_control = False self.key = '' self.training = False self.tele_twist.linear.x = 0 self.tele_twist.angular.z = 0 if self._enable_auto_control: if not self.intention: print('estimate pose + goal....') elif self.intention == 'stop': self.tele_twist.linear.x = 0 self.tele_twist.angular.z = 0 else: if self._mode == 'DLM': intention = Dataset.INTENTION_MAPPING[self.intention] # map intention str => int print('intention: ',intention) if policy.input_frame == 'NORMAL': # 1 cam # convert ros msg -> cv2 img = cv2.resize(undistort(self.bridge.compressed_imgmsg_to_cv2(self.left_img,desired_encoding='bgr8'),FRONT_CAMERA_INFO),(224,224)) pred_control = policy.predict_control(img, intention, self.speed)[0] self.tele_twist.linear.x = pred_control[0]*Dataset.SCALE_VEL self.tele_twist.angular.z = pred_control[1]*Dataset.SCALE_STEER elif policy.input_frame == 'MULTI': # convert ros msg -> cv2 # NOTE: Make sure the left camera is launched by mynteye_2.launch and right is run by mynteye_3.launch left_img = cv2.resize(self.bridge.compressed_imgmsg_to_cv2(self.me3_left,desired_encoding='bgr8'),(224,224)) front_img = cv2.resize(self.bridge.compressed_imgmsg_to_cv2(self.left_img,desired_encoding='bgr8'),(224,224)) right_img = cv2.resize(self.bridge.compressed_imgmsg_to_cv2(self.me2_left,desired_encoding='bgr8'),(224,224)) # stack left,right -> 3channel left_img = np.stack((left_img,)*3,axis=-1) right_img = np.stack((right_img,)*3,axis=-1) pred_control= policy.predict_control([left_img,front_img,right_img],intention,self.speed)[0] self.tele_twist.linear.x = pred_control[0]*Dataset.SCALE_VEL*0.7 self.tele_twist.angular.z = pred_control[1]*Dataset.SCALE_STEER*0.7 # publish to /train/* topic to record data (if in training mode) if self.training: self._publish_as_trn() # self.pub_intention.publish(self.manual_intention) # publish control self.control_pub.publish(self.tele_twist)
def main(): calib_path = raw_input('Enter folder path for calibration parameters:\n') if not os.path.isdir(calib_path): print('Invalid path!') return in_path = raw_input('Enter folder path for input images:\n') if not os.path.isdir(in_path): print('Invalid path!') return out_path = raw_input('Enter folder path for output images:\n') if not os.path.isdir(out_path): os.mkdir(out_path) undistort.undistort(calib_path, in_path, out_path) cluster.cluster(os.path.join(out_path, undistort), out_path) find_contour.find_contour(os.path.join(out_path, cluster), out_path)
def reproject2(depth, depth_intrinsic_mat, depth_intrinsic_undist_mat, rgb_intrinsic_mat, rgb_intrinsic_undist_mat, depth_dist_coeffs, rgb_dist_coeffs, rot_vec, t_vec, img_size): ''' Perform reprojection of depth map. ''' # depth = cv2.undistort(depth, depth_intrinsic_undist_mat, depth_dist_coeffs, None, None) depth = undistort(depth, depth_intrinsic_mat, depth_intrinsic_undist_mat, depth_dist_coeffs) # plt.imshow(depth) # plt.show() xyd_list = depth2xyd_list(depth.copy()).astype(np.float32) # print("!", xyd_list.shape) # xyd_list = drop_zero_depths(xyd_list) # xyd_list = undistort_points(xyd_list, depth_intrinsic_mat, depth_dist_coeffs, depth_intrinsic_undist_mat)#, cv2.Rodrigues(rot_vec)[0], depth_intrinsic_undist_mat) # after = xyd_list2depthmap(xyd_list.copy(), depth.shape) # print("!", after.shape) # plt.subplot(131) # plt.title("after") # plt.imshow(after) # plt.subplot(132) # plt.title("before") # plt.imshow(depth) # plt.subplot(133) # diff = depth - after # print("LOL", np.amax(diff), np.amin(diff)) # plt.imshow(diff / np.amax(diff)) # plt.show() # cv2.imwrite("depth_undist.png",after) xyz3d_list = unproject_to_3d(xyd_list, depth_intrinsic_undist_mat) xyz3d_list = cv2.Rodrigues(rot_vec)[0] @ xyz3d_list + t_vec.reshape( (-1, 1)) xyz3d_list = rgb_intrinsic_undist_mat @ xyz3d_list # xyz3d_list = undistort_points(xyz3d_list, depth_intrinsic_mat, depth_dist_coeffs, depth_intrinsic_undist_mat) #<< xy_list_reprojected = np.array( [xyz3d_list[0] / xyz3d_list[2], xyz3d_list[1] / xyz3d_list[2]]) # xy_list_reprojected = cv2.projectPoints(xyz3d_list, rot_vec, t_vec, rgb_intrinsic_undist_mat, np.array([]))[0][:, 0, :].T#, rgb_dist_coeffs)[0][:, 0, :].T xyd_list_reprojected = np.vstack((xy_list_reprojected, xyz3d_list[2])) print("!!!", xyd_list_reprojected.shape) new_depth = xyd_list2depthmap(xyd_list_reprojected, img_size) print("!!!", new_depth.shape) return new_depth #xyz3d_list
def image_pipeline(image): if (os.path.exists("./param/calibration_param.npz")): data = np.load("./param/calibration_param.npz") mxt = data["mxt"] dist = data["dist"] else: # Camera calibration mxt, dist = calibrate_camera("../camera_cal/calibration*.jpg") # undistortion undist = undistort(image, mxt, dist) h, w = undist.shape[:2] # perspective src = np.float32([(575, 464), (707, 464), (258, 682), (1049, 682)]) dst = np.float32([(450, 0), (w - 450, 0), (450, h), (w - 450, h)]) undist, M, Minv = transform(undist, src, dst) warped = color_grad(undist) leftx, lefty, rightx, righty = find_lane_pixels(warped) left_fit, right_fit = fit_polynomial(leftx, lefty, rightx, righty) left_cur, right_cur = get_curve_real(lefty, righty, left_fit, right_fit) # Create an image to draw the lines on warp_zero = np.zeros_like(warped).astype(np.uint8) color_warp = np.dstack((warp_zero, warp_zero, warp_zero)) # Recast the x and y points into usable format for cv2.fillPoly() ploty = np.linspace(0, 719, num=720) # to cover same y-range as image pts_left = np.array( [np.transpose(np.vstack([helper(left_fit, lefty), lefty]))]) pts_right = np.array([ np.flipud(np.transpose(np.vstack([helper(right_fit, righty), righty]))) ]) pts = np.hstack((pts_left, pts_right)) # Draw the lane onto the warped blank image cv2.fillPoly(color_warp, np.int_([pts]), (0, 255, 0)) # Warp the blank back to original image space using inverse perspective matrix (Minv) newwarp = cv2.warpPerspective(color_warp, Minv, (image.shape[1], image.shape[0])) # Combine the result with the original image result = cv2.addWeighted(image, 1, newwarp, 0.3, 0) return result
def pipelineSingleFrameDetection(image): image = undistort(image) listOfAllBoxes = [] for scale in scales: foundCars, allSquares, listOfBoxes = find_cars(image, scales[scale]['y'][0], scales[scale]['y'][1], scales[scale]['x'][0], scales[scale]['x'][1], scale) listOfAllBoxes += listOfBoxes allFoundCars = draw_boxes(image, listOfAllBoxes) return allFoundCars
def debug_image(oimg): uimg = undistort.undistort(oimg) _, _, _, timg = threshold.threshold(uimg) wimg = topview.warp(timg) #result = np.dstack((timg, timg, timg)) result = np.dstack((wimg, wimg, wimg)) left_fit, right_fit, roc, dfc, ly, lx, ry, rx = lane_finder.find(wimg) #ploty = np.linspace(0, wimg.shape[0]-1, wimg.shape[0]).astype(int) #left_fitx = left_fit[0]*ploty**2 + left_fit[1]*ploty + left_fit[2] #right_fitx = right_fit[0]*ploty**2 + right_fit[1]*ploty + right_fit[2] #result = np.dstack((wimg, wimg, wimg)) result[ly, lx] = [255, 0, 0] result[ry, rx] = [255, 0, 0] #result[ploty, left_fitx.astype(int)] = [255, 255, 0] #result[ploty, right_fitx.astype(int)] = [255, 255, 0] return result
def getFrame(thresh): global frame global gray _, frame = vs.read() frame = cv2.rotate(frame, cv2.ROTATE_180) #!!!! UNTESTEDDDDDD TODO rotation frame = undistort.undistort(frame) gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY) # if thresh parameter is true, use thresholded image if (thresh): #threshold to clean up blooming from light from retroreflector _, frame = cv2.threshold(gray, thresh_min, 255, cv2.THRESH_BINARY) frame = cv2.cvtColor( frame, cv2.COLOR_GRAY2BGR) #aruco detection only works with color return (frame, gray)
def pipeline(image): image = undistort(image) listOfAllBoxes = [] for scale in scales: foundCars, allSquares, listOfBoxes = find_cars(image, scales[scale]['y'][0], scales[scale]['y'][1], scales[scale]['x'][0], scales[scale]['x'][1], scale) listOfAllBoxes += listOfBoxes global heatmap heatmap *= (historyFactor-1) heatmap = add_heat(heatmap, listOfAllBoxes) heatmap /= historyFactor labels = label(apply_threshold(heatmap, threshold)) allFoundCars = draw_labeled_bboxes(image, labels) return allFoundCars
import cv2 from undistort import undistort img = cv2.imread('test_imgs/test3.jpg') img = cv2.cvtColor(img, cv2.COLOR_BGR2RGB) dst = undistort(img) #Visualize distortion cv2.imshow('img', img) cv2.imshow('dst', dst) cv2.waitKey(0) cv2.destroyAllwindows()
import cv2 import os from datetime import datetime import matplotlib.image as mpimg from undistort import undistort from udacityCode import find_cars, draw_boxes pathToTestImages = "../test_images" outputFolder = "../output_images" listTestImages = os.listdir(pathToTestImages) image = cv2.imread("../camera_cal/calibration1.jpg") undistortedImage = undistort(image) cv2.imwrite(outputFolder + "/" + "undist-chessboard.jpg", undistortedImage) #This dict describes the different scales that will be used and also on which subset of the image it will be used. scales = { 0.5: { 'y': (410, 455), 'x': (300, 1280 - 300) }, 0.75: { 'y': (410, 474), 'x': (200, 1280 - 200) }, 1: { 'y': (400, 485), 'x': (0, 1280) }, 1.5: {
plt.xlim(0, 1280) plt.ylim(720, 0) plt.axis("off") # Save visualization if save_image: plt.savefig('./output_images/vis_area.png', bbox_inches='tight') plt.show() if __name__ == '__main__': polyfit = Polyfit() perspective = Perspective() image_file = 'test_images/test2.jpg' image = mpimg.imread(image_file) undistorted = undistort(image) # Create binary outputs abs_thresh, mag_thresh, dir_thresh, hls_thresh, hsv_thresh, combined_output = combined_threshold( undistorted) plt.imshow(combined_output, cmap='gray') warped = perspective.warp(combined_output) plt.imshow(warped, cmap='gray') # Find lanes left_fit, right_fit, vars = polyfit.poly_fit_skip(warped) left_curve_rad, right_curve_rad = polyfit.curvature() position = polyfit.vehicle_position(warped) print('curvature: {:.2f}m'.format((left_curve_rad + right_curve_rad) / 2)) print('vehicle position: {:.2f}m from center'.format(position)) # Visualize lane finding polyfit.visualize_window(warped, vars, save_image=True) polyfit.visualize_area(warped, vars, save_image=True)
def pipeline(image, s_thresh=(170, 255), l_thresh=(220, 255), sx_thresh=(50, 150), previous_lanes=[None], show=False): img_o = np.copy(image) img = dist.undistort(img_o) img_s = saturation(img) binary_s = binary_threshold(img_s, s_thresh) # Only white, non obscured lines img_l = lightness(img) binary_l = binary_threshold(img_l, l_thresh) # obscured lines add_sobel = False img_sobel = sobel(img_l) binary_sobel = binary_threshold(img_sobel, sx_thresh) # Combine the two binary thresholds combined_binary = np.zeros_like(binary_s) #combined_binary[(binary_s == 1) | (binary_sobel == 1)] = 1 combined_binary[(binary_s == 1) | (binary_l == 1)] = 1 if add_sobel: combined_binary[(binary_sobel == 1)] = 1 warped_binary = birdseye(combined_binary, src, dst) if previous_lanes[-1] is not None: lane = sliding_window(warped_binary, previous_lane=previous_lanes[-1]) lane.previous = previous_lanes lane.smooth_fit() else: lane = sliding_window(warped_binary, previous_lane=None) #lane.previous = previous_lanes lane.check_sanity() # Image returned for the video stream if previous_lanes[-1] is not None: if lane.detected and lane.sanity: result_img = cv2.addWeighted( img, 1, lane.warp_lane(Minv, method='incl_previous'), 0.3, 0) else: #Retry print('Reset lane detection') lane = sliding_window(warped_binary, previous_lane=None) lane.previous = previous_lanes lane.check_sanity() if lane.detected and lane.sanity: result_img = cv2.addWeighted(img, 1, lane.warp_lane(Minv), 0.3, 0) else: print( 'Lane could not be detected. Keep the detection from frame before' ) try: #### ToDo: Instead of rolling back; just use an average of the 5 previous detections lane = previous_lanes[-1] lane.previous = previous_lanes result_img = cv2.addWeighted(img, 1, lane.warp_lane(Minv), 0.3, 0) except TypeError: print('No previous lanes available for roll-back.') result_img = img elif lane.detected and lane.sanity: result_img = cv2.addWeighted(img, 1, lane.warp_lane(Minv), 0.3, 0) else: print('Lane was not detected and no previous images are available') result_img = img ### if show: fig, axs = plt.subplots(3, 3, figsize=(12, 8)) #fig.subplots_adjust(hspace = .1, wspace=.1) #plt.subplots_adjust(left=0., right=1, top=0.9, bottom=0.) axs = axs.ravel() axs[0].set_title('Original image') axs[0].imshow(image) axs[1].set_title('Undistorted image') img_e = copy.copy(img) img_e = draw_roi(img_e, SRC) axs[1].imshow(img_e) axs[2].set_title('Result image') undist_with_lane = cv2.addWeighted(img, 1, lane.warp_lane(Minv), 0.3, 0) undist_with_lane = draw_roi(undist_with_lane, SRC) axs[2].imshow(undist_with_lane) axs[3].set_title('Lightness channel') axs[3].imshow(img_l, cmap='Greys_r') axs[4].set_title('Lightness channel thresholded') axs[4].imshow(binary_l, cmap='Greys_r') axs[5].set_title('Sobel of lightness channel') axs[5].imshow(img_sobel, cmap='Greys_r') axs[6].set_title('Saturation channel') # thresholded') #s = image.shape #binary_s_color = np.zeros((s[0], s[1], 3), dtype=np.uint8) axs[6].imshow(img_s, cmap='Greys_r') #axs[6].imshow(binary_s, cmap='Greys_r') axs[7].set_title('Binary result') axs[7].imshow(combined_binary, cmap='Greys_r') axs[8].set_title('Birdseye view') axs[8].imshow(warped_binary, cmap='Greys_r') lane.plot_fit(axs[8]) ''' axs[8].set_title('Lightness channel thresholded')#Saturation channel') #axs[2].imshow(img_s, cmap='Greys_r') axs[8].imshow(binary_l, cmap='Greys_r') axs[3].set_title('Saturation channel thresholded') #s = image.shape #binary_s_color = np.zeros((s[0], s[1], 3), dtype=np.uint8) axs[3].imshow(binary_s, cmap='Greys_r') axs[4].set_title('Sobel of lightness channel') axs[4].imshow(img_sobel, cmap='Greys_r') axs[5].set_title('Sobel thresholded') axs[5].imshow(binary_sobel, cmap='Greys_r') axs[6].set_title('Binary result') axs[6].imshow(combined_binary, cmap='Greys_r') axs[7].set_title('Birdseye view') axs[7].imshow(warped_binary, cmap='Greys_r') lane.plot_fit(axs[7]) ''' for i in range(9): axs[i].axis('off') fig.tight_layout() return lane, result_img
#! /usr/bin/env python import cv2 import numpy as np import sys from undistort import undistort total = 0 for f in sys.argv[1:]: img = cv2.imread(f,0) img = undistort("../share/table_calib_640x480.yml", img) img = cv2.resize(img, (320,240)) #img = cv2.adaptiveThreshold(img,255,cv2.ADAPTIVE_THRESH_MEAN_C,\ # cv2.THRESH_BINARY,15,2) #img = cv2.medianBlur(img,5) cimg = cv2.cvtColor(img,cv2.COLOR_GRAY2BGR) circles = cv2.HoughCircles(img,cv2.HOUGH_GRADIENT,1,20, param1=30,param2=20,minRadius=5,maxRadius=20) if circles == None: #print (f + ": No circles!") #cv2.imshow('detected circles',cimg) #cv2.waitKey(0) pass else: total += len(circles[0,:])
import undistort import pickle import cv2 import matplotlib.image as mpimg import numpy as np import matplotlib.pyplot as plt from matplotlib.path import Path import matplotlib.patches as patches img = mpimg.imread( '../CarND-Advanced-Lane-Lines/test_images/straight_lines1.jpg') uimg = undistort.undistort(img) src = np.float32([ [600, 450], #top left [680, 450], #top right [1100, 720], #bottom right [200, 720] ]) #bottom left dst = np.float32([[250, 0], [950, 0], [950, 720], [250, 720]]) M = cv2.getPerspectiveTransform(src, dst) Minv = cv2.getPerspectiveTransform(dst, src) warped = cv2.warpPerspective(uimg, M, (uimg.shape[1], uimg.shape[0]), flags=cv2.INTER_LINEAR) codes = [ Path.MOVETO,
mag_thresh) from undistort import undistort # Read in the saved objpoints and imgpoints calibration_pickle = pickle.load( open( "calibration_pickle.p", "rb" ) ) cameraMatrix = calibration_pickle["cameraMatrix"] distCoeffs = calibration_pickle["distCoeffs"] # Collect images for undistortion #test_images = glob.glob('test_images/straight_lines*.jpg') test_images = glob.glob('test_images/test*.jpg') for index, filename in enumerate(test_images): print('processing image: ', filename) image = undistort(filename, cameraMatrix, distCoeffs) # Choose a Sobel kernel size ksize = 13 # Choose a larger odd number to smooth gradient measurements # Apply each of the thresholding functions gradx = abs_sobel_thresh(image, orient='x', sobel_kernel=ksize, thresh_min=20, thresh_max=200) grady = abs_sobel_thresh(image, orient='y', sobel_kernel=ksize, thresh_min=50, thresh_max=200) mag_binary = mag_thresh(image, sobel_kernel=ksize, mag_thresh=(60, 130)) dir_binary = dir_threshold(image, sobel_kernel=ksize, thresh=(0.7, 1.3)) c_binary = color_threshold(image, sthreshold=(90,255), vthreshold=(90,255)) combined = np.zeros_like(image[:,:,0]) #combined[((gradx == 1) & (grady == 1) ) | ((mag_binary == 1) & (dir_binary == 1)) ] = 255 # combined[((gradx == 1) & (grady == 1)) | ((mag_binary == 1) & (dir_binary == 1)) | (c_binary == 1) ] = 255 # #combined[((gradx == 1) & (grady == 1)) ] = 1
def cb_lpe_intention(self, msg): self.intention = cv2.resize( undistort(CvBridge().imgmsg_to_cv2(msg, desired_encoding='bgr8')), (224, 224))
def dist_from_center(binary_warped, left_fit, right_fit): # assuming lane center should be at center of image xm_per_pix = 3.7 / 700 # meters per pixel in x dimension y = binary_warped.shape[0] - 1 xl = left_fit[0] * y**2 + left_fit[1] * y + left_fit[2] xr = right_fit[0] * y**2 + right_fit[1] * y + right_fit[2] lane_center = (xl + xr) / 2 img_center = binary_warped.shape[1] / 2 return (lane_center - img_center) * xm_per_pix if __name__ == '__main__': # open an image oimg = mpimg.imread('../CarND-Advanced-Lane-Lines/test_images/test2.jpg') img = undistort.undistort(oimg) _, _, _, img = threshold.threshold(img) binary_warped = topview.warp(img) leftx, lefty, rightx, righty = sliding_window(binary_warped) # Fit a second order polynomial to each left_fit = np.polyfit(lefty, leftx, 2) right_fit = np.polyfit(righty, rightx, 2) left_curverad, right_curverad = roc(binary_warped.shape[0] - 1, left_fit, right_fit) print(left_curverad, 'm', right_curverad, 'm') nonzero = binary_warped.nonzero() nonzeroy = np.array(nonzero[0])