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])