def main():
test_img_files = glob.glob("test_images/*.jpg")
test_images = [read_image_as_rgb(f) for f in test_img_files]
cameraCalibrator = CameraCalibrator()
cameraCalibrator.restore('models/camera_calibration_model')
pipeline = LaneDetectionPipeline(cameraCalibrator)
img = test_images[0]
undistorted = pipeline.get_birds_eye_binary(img)
bet = BirdsEyeViewTransform()
birds_eye_undistorted = bet.get_birds_eye_view(undistorted)
crop = trapezoidal_crop(undistorted)
birds_eye_crop = bet.get_birds_eye_view(crop)
# fig, ax = plt.subplots(2, 2)
# ax[0][0].imshow(undistorted, cmap='gray')
# ax[0][0].set_title("hls and gradient transform")
# ax[0][0].axis("off")
# ax[0][1].imshow(birds_eye_undistorted, cmap='gray')
# ax[0][1].set_title("birds eye")
# ax[0][1].axis("off")
# ax[1][0].imshow(crop, cmap='gray')
# ax[1][0].title("trapeziodal crop")
# ax[1][0].axis("off")
# ax[1][1].imshow(birds_eye_crop, cmap='gray')
# ax[1][1].title("birds eye crop")
# ax[1][1].axis("off")
histogram = np.sum(birds_eye_crop, axis=0)
plt.plot(histogram)
plt.show()
def __init__(self):
self.width = 1280
self.height = 720
self.left_lines = Line()
self.right_lines = Line()
self.s_magnitude_thresh = (175, 255)
self.sobel_kernel = 7
self.m_thresh = (14, 255)
self.d_thresh = (0.0, 0.73)
self.camera_calibrator = CameraCalibrator()
def calibrate_cameras():
global cameras, tfm_pub
greenprint('Step 1. Calibrating multiple cameras.')
cam_calib = CameraCalibrator(cameras)
done = False
while not done:
cam_calib.calibrate(n_obs=CAM_N_OBS, n_avg=CAM_N_AVG)
if cameras.num_cameras == 1:
break
if not cam_calib.calibrated:
redprint('Camera calibration failed.')
cam_calib.reset_calibration()
else:
tfm_pub.add_transforms(cam_calib.get_transforms())
if yes_or_no('Are you happy with the calibration? Check RVIZ.'
):
done = True
else:
yellowprint('Calibrating cameras again.')
cam_calib.reset_calibration()
greenprint('Cameras calibrated.')
from camera_calibration import CameraCalibrator
from image_utils import *
from lane_detection import LaneDetectionPipeline
from lane_finding_histogram import *
from lanes import *
from glob import glob
cameraCalibrator = CameraCalibrator()
cameraCalibrator.restore('models/camera_calibration_model')
pipeline = LaneDetectionPipeline(cameraCalibrator)
test_images_dir = "test_images"
output_images_dir = "output_images"
input_file_paths = glob(test_images_dir + "/*.jpg")
for input_file_path in input_file_paths:
output_file_path = input_file_path.replace(test_images_dir, output_images_dir)
img = read_image_as_rgb(input_file_path)
out_img = pipeline.process(img)
out_img = cv2.cvtColor(out_img, cv2.COLOR_RGB2BGR)
cv2.imwrite(output_file_path, out_img)
pipeline.reset()
from moviepy.editor import VideoFileClip
test_videos_dir = "test_videos"
output_videos_dir = "output_videos"
video_file_paths = glob(test_videos_dir + "/*.mp4")
for video_file_path in video_file_paths:
output_file_path = video_file_path.replace(test_videos_dir, output_videos_dir)
clip = VideoFileClip(video_file_path)
clip_output = clip.fl_image(pipeline.process)
clip_output.write_videofile(output_file_path, audio=False)
def calibrate_cameras ():
global tfm_pub, cameras, tf_listener
tfm_base_kinect = get_robot_kinect_transform()
if yes_or_no('Calibrate again?'):
greenprint("Step 1. Calibrating multiple cameras.")
cam_calib = CameraCalibrator(cameras)
done = False
while not done:
cam_calib.calibrate(CAM_N_OBS, CAM_N_AVG)
if cameras.num_cameras == 1:
break
if not cam_calib.calibrated:
redprint("Camera calibration failed.")
cam_calib.reset_calibration()
else:
tfm_reference_camera = cam_calib.get_transforms()[0]
tfm_reference_camera['child'] = 'camera_link'
tfm_base_reference = {}
tfm_base_reference['child'] = transform['parent']
tfm_base_reference['parent'] = 'base_footprint'
tfm_base_reference['tfm'] = nlg.inv(tfm_reference_camera['tfm'].dot(nlg.inv(tfm_base_kinect)))
tfm_pub.add_transforms([tfm_base_reference])
if yes_or_no("Are you happy with the calibration? Check RVIZ."):
done = True
else:
yellowprint("Calibrating cameras again.")
cam_calib.reset_calibration()
greenprint("Cameras calibrated.")
else:
tfm_pub.load_calibration('cam_pr2')
if yes_or_no('Get PR2 Gripper?'):
# Finding transform between PR2's gripper and the tool_tip
marker_id = 1
camera_id = 1
n_avg = 100
tfms_camera_marker = []
for i in xrange(n_avg):
tfms_camera_marker.append(cameras.get_ar_markers([marker_id], camera=camera_id)[marker_id])
time.sleep(0.03)
tfm_camera_marker = utils.avg_transform(tfms_camera_marker)
calib_file = 'calib_cam_hydra_gripper1'
file_name = osp.join(calib_files_dir, calib_file)
with open(file_name, 'r') as fh: calib_data = cPickle.load(fh)
lr, graph = calib_data['grippers'].items()[0]
gr = gripper.Gripper(lr, graph)
assert 'tool_tip' in gr.mmarkers
gr.tt_calculated = True
tfm_marker_tooltip = gr.get_rel_transform(marker_id, 'tool_tip', 0)
i = 10
tfm_base_gripper = None
while i > 0:
try:
now = rospy.Time.now()
tf_listener.waitForTransform('base_footprint', 'l_gripper_tool_frame', now, rospy.Duration(5.0))
(trans, rot) = tf_listener.lookupTransform('base_footprint', 'l_gripper_tool_frame', rospy.Time(0))
tfm_base_gripper = conversions.trans_rot_to_hmat(trans, rot)
break
except (tf.Exception, tf.LookupException, tf.ConnectivityException):
yellowprint("Failed attempt.")
i -= 1
pass
if tfm_base_gripper is not None:
tfm_gripper_tooltip = {'parent':'l_gripper_tool_frame',
'child':'pr2_lgripper_tooltip',
'tfm':nlg.inv(tfm_base_gripper).dot(tfm_base_kinect).dot(tfm_link_rof).dot(tfm_camera_marker).dot(tfm_marker_tooltip)
}
tfm_pub.add_transforms([tfm_gripper_tooltip])
greenprint("Gripper to marker found.")
else:
redprint("Gripper to marker not found.")
class LaneFinder():
"""Finds laneline inside car camera images"""
def __init__(self):
self.width = 1280
self.height = 720
self.left_lines = Line()
self.right_lines = Line()
self.s_magnitude_thresh = (175, 255)
self.sobel_kernel = 7
self.m_thresh = (14, 255)
self.d_thresh = (0.0, 0.73)
self.camera_calibrator = CameraCalibrator()
def undistort(self, img):
"""Undistort image using camera matrix"""
return self.camera_calibrator.undistort_image(img)
def s_magnitude(self, img):
"""Returns magnitude thresholded binary image of
the S channel in HLS color space
"""
thresh = self.s_magnitude_thresh
hls = cv2.cvtColor(img, cv2.COLOR_BGR2HLS)
s_channel = hls[:, :, 2]
magnitude_binary = np.zeros_like(s_channel)
magnitude_binary[(s_channel >= thresh[0])
& (s_channel <= thresh[1])] = 1
return magnitude_binary
def l_direction(self, img):
""" Apply sobel filter on L(HLS) channel and threshold,
filter pixels direction inside self.d_thresh and
filter pixels magnitude inside self.m_thresh
"""
sobel_kernel = self.sobel_kernel
m_thresh = self.m_thresh
d_thresh = self.d_thresh
hls = cv2.cvtColor(img, cv2.COLOR_BGR2HLS)
l_channel = hls[:, :, 1]
sobelx = cv2.Sobel(l_channel, cv2.CV_64F, 1, 0, ksize=sobel_kernel)
sobely = cv2.Sobel(l_channel, cv2.CV_64F, 0, 1, ksize=sobel_kernel)
magnitude = np.sqrt(np.square(sobelx) + np.square(sobely))
scaled = np.uint8(255 * magnitude / np.max(magnitude))
abs_sobelx = np.absolute(sobelx)
abs_sobely = np.absolute(sobely)
direction = np.arctan2(abs_sobely, abs_sobelx)
binary_output = np.zeros_like(direction)
binary_output[(direction >= d_thresh[0]) & (direction <= d_thresh[1]) &
(scaled >= m_thresh[0]) & (scaled <= m_thresh[1])] = 1
return binary_output
def combined_thresholding(self, img):
"""Returns the combined result of all thresholdings"""
s_mag = self.s_magnitude(img)
l_dir = self.l_direction(img)
combined_binary = np.zeros_like(img[:, :, 1])
combined_binary[(s_mag == 1) | (l_dir == 1)] = 1
return combined_binary
def get_perspective_transform_matrix(self, reverse=False):
matrix_key = 'perspective_transform_mtx'
if reverse:
matrix_key = 'reverse_perspective_transform_mtx'
matrix = getattr(self, matrix_key, None)
if matrix is not None:
return matrix
# no previous stored matrix, calculate one
tls = (563, 470) # top left source point
bls = (220, 700) # bottom left source point
tld = (300, 300) # top left destination
bld = (300, 720) # bottom left destination
src = np.float32([[tls[0], tls[1]], [self.width - tls[0], tls[1]],
[self.width - bls[0], bls[1]], [bls[0], bls[1]]])
dst = np.float32([
[tld[0], tld[1]],
[self.width - tld[0], tld[1]],
[self.width - tld[0], bld[1]],
[bld[0], bld[1]],
])
if reverse:
transform_mtx = cv2.getPerspectiveTransform(dst, src)
else:
transform_mtx = cv2.getPerspectiveTransform(src, dst)
# save matrix for later use
setattr(self, matrix_key, transform_mtx)
return transform_mtx
def perspective_transform(self, img, reverse=False):
"""Transform car camera image into birds eye view"""
transform_mtx = self.get_perspective_transform_matrix(reverse=reverse)
shape = (self.width, self.height)
warped = cv2.warpPerspective(img,
transform_mtx,
shape,
flags=cv2.INTER_LINEAR)
return warped
def histogram_find_lines(self, binary_warped):
"""Find left/right lane line indices from the binary warped
image without knowing previous line positions using the
hostogram/sliding window method
"""
width, height = self.width, self.height
nwindows = 9
window_height = np.int(height / nwindows)
histogram = np.sum(binary_warped[int(height / 2):, :], axis=0)
midpoint = np.int(width / 2)
leftx_base = np.argmax(histogram[:midpoint])
rightx_base = np.argmax(histogram[midpoint:]) + midpoint
nonzero = binary_warped.nonzero()
nonzeroy = np.array(nonzero[0])
nonzerox = np.array(nonzero[1])
leftx_current = leftx_base
rightx_current = rightx_base
left_lane_inds = []
right_lane_inds = []
windows = [] # record search window for visualization
margin = 100 # half window size
minpix = 100 # least pixels to be recognized as found
for window in range(nwindows):
win_y_low = height - (window + 1) * window_height
win_y_high = height - window * window_height
win_xleft_low = leftx_current - margin
win_xleft_high = leftx_current + margin
win_xright_low = rightx_current - margin
win_xright_high = rightx_current + margin
windows.append(
(win_xleft_low, win_y_low, win_xleft_high, win_y_high))
windows.append(
(win_xright_low, win_y_low, win_xright_high, win_y_high))
good_left_inds = ((nonzeroy >= win_y_low) & (nonzeroy < win_y_high)
& (nonzerox >= win_xleft_low) &
(nonzerox < win_xleft_high)).nonzero()[0]
good_right_inds = ((nonzeroy >= win_y_low) &
(nonzeroy < win_y_high) &
(nonzerox >= win_xright_low) &
(nonzerox < win_xright_high)).nonzero()[0]
# Append these indices to the lists
left_lane_inds.append(good_left_inds)
right_lane_inds.append(good_right_inds)
# If you found > minpix pixels, recenter next window
# on their mean position
if len(good_left_inds) > minpix:
leftx_current = np.int(np.mean(nonzerox[good_left_inds]))
if len(good_right_inds) > minpix:
rightx_current = np.int(np.mean(nonzerox[good_right_inds]))
# Concatenate the arrays of indices
left_lane_inds = np.concatenate(left_lane_inds)
right_lane_inds = np.concatenate(right_lane_inds)
leftx = nonzerox[left_lane_inds]
lefty = nonzeroy[left_lane_inds]
rightx = nonzerox[right_lane_inds]
righty = nonzeroy[right_lane_inds]
left_fit = np.polyfit(lefty, leftx, 2)
right_fit = np.polyfit(righty, rightx, 2)
return left_fit, right_fit
def convolution_find_lines(self, binary_warped, left_fit, right_fit):
"""Find line around known previous lines
left_fit, right_fit: previously fitted left, right lines
"""
window_width = 50
hww = 25 # half window width
n_windows = 9
window_height = self.height / n_windows
window_centroids = []
margin = 100 # How much to slide left/right for searching
window = np.ones(window_width)
offset = np.int((window_width + margin) / 2)
nonzero = binary_warped.nonzero()
nonzeroy = np.array(nonzero[0])
nonzerox = np.array(nonzero[1])
left_lane_inds = []
right_lane_inds = []
for level in range(n_windows):
y_min = np.int(level * window_height)
y_max = np.int(y_min + window_height)
layer = binary_warped[y_min:y_max, :]
image_layer = np.sum(layer, axis=0)
conv_signal = np.convolve(window, image_layer)
y_eval = (y_min + y_max) / 2
l_base = np.int(left_fit[0] * y_eval**2 + left_fit[1] * y_eval +
left_fit[2])
l_base = max(0, min(self.width, l_base))
l_center = (np.argmax(
conv_signal[max(0, l_base -
offset):min(self.width, l_base + offset)]) +
l_base - offset - hww)
r_base = np.int(right_fit[0] * y_eval**2 + right_fit[1] * y_eval +
right_fit[2])
r_base = max(0, min(self.width, r_base))
r_center = (np.argmax(
conv_signal[max(0, r_base -
offset):min(self.width, r_base + offset)]) +
r_base - offset - hww)
window_centroids.append((l_center, r_center))
good_left_inds = ((nonzeroy >= y_min) & (nonzeroy <= y_max) &
(nonzerox >= (l_center - hww)) &
(nonzerox <= (l_center + hww))).nonzero()[0]
good_right_inds = ((nonzeroy >= y_min) & (nonzeroy <= y_max) &
(nonzerox >= (r_center - hww)) &
(nonzerox <= (r_center + hww))).nonzero()[0]
left_lane_inds.append(good_left_inds)
right_lane_inds.append(good_right_inds)
left_lane_inds = np.concatenate(left_lane_inds)
right_lane_inds = np.concatenate(right_lane_inds)
leftx = nonzerox[left_lane_inds]
lefty = nonzeroy[left_lane_inds]
rightx = nonzerox[right_lane_inds]
righty = nonzeroy[right_lane_inds]
left_fit = np.polyfit(lefty, leftx, 2)
right_fit = np.polyfit(righty, rightx, 2)
return left_fit, right_fit
def find_lines(self, img):
"""Find lane line"""
prev_left_fit = self.left_lines.best_fit()
prev_right_fit = self.right_lines.best_fit()
if prev_left_fit is None or prev_right_fit is None:
naive_left_fit, naive_right_fit = self.histogram_find_lines(img)
if prev_left_fit is None:
prev_left_fit = naive_left_fit
if prev_right_fit is None:
prev_right_fit = naive_right_fit
new_left_fit, new_right_fit = self.convolution_find_lines(
img, prev_left_fit, prev_right_fit)
# TODO(Olala): sanity check before append
self.left_lines.append_fit(new_left_fit)
self.right_lines.append_fit(new_right_fit)
avg_left_fit = self.left_lines.best_fit()
avg_right_fit = self.right_lines.best_fit()
assert avg_left_fit is not None
assert avg_right_fit is not None
return avg_left_fit, avg_right_fit