def plot_overlap(self): current_time = self.start_time_msec + self.index * self.delay_msec self.vidcap.set(cv2.CAP_PROP_POS_MSEC,(current_time)) success, frame_next = self.vidcap.read() if success: frame_next = undistort(frame_next) frame_next = downscale(frame_next, 1000, False, pad_img=False) if len(self.M_list) < self.index: M = homography(self.frame_start, frame_next, draw_matches=False) self.M_list.append(M) else: M = self.M_list[self.index - 1] warped, shift = warp_image(frame_next, M, alpha_channel=False) warped_blue = color_img(warped, [255, 0, 0]) overlapped_img_color, _, _ = merge_images(self.frame_start_red, warped_blue, shift) self.overlapped_img, self.offset_start, self.offset_end = merge_images(self.frame_start, warped, shift) plt.figure() plt.imshow(overlapped_img_color[...,::-1]) plt.show() print msec2string(current_time)
def main_first(self, img):
height = img.shape[0]
warped, _ = utils.perspective(utils.undistort(img, self.objpoints, self.imgpoints))
warped_edge = utils.edge(warped)
left_base, right_base = utils.base(warped_edge)
left_fit, _ = utils.fit_first(warped_edge, left_base)
right_fit, _ = utils.fit_first(warped_edge, right_base)
return left_fit, right_fit
def process_pipeline(frame, keep_state=True):
"""
Apply whole lane detection pipeline to an input color frame.
:param frame: input color frame
:param keep_state: if True, lane-line state is conserved (this permits to average results)
:return: output blend with detected lane overlaid
"""
global line_lt, line_rt, processed_frames
# undistort the image using coefficients found in calibration
undistorted_img = undistort(frame, mtx, dist)
# binarize the frame and highlight lane lines
binarized_img = binarize(undistorted_img)
# perspective transform to obtain bird's eye view
birdeye_img, matrix, inversed_matrix = birdeye(binarized_img,
visualise=False)
# 2 order polynomial curve fit onto lane lines found
if processed_frames > 0 and keep_state and line_lt.detected and line_rt.detected:
find_lane_by_previous_fits(birdeye_img,
line_lt,
line_rt,
visualise=False)
else:
find_lane_by_sliding_windows(birdeye_img,
line_lt,
line_rt,
n_windows=9,
visualise=False)
# compute offset in meter from center of the lane
offset_meter = offset_from_lane_center(line_lt,
line_rt,
frame_width=frame.shape[1])
# draw the surface enclosed by lane lines back onto the original frame
blend_on_road = draw_back_onto_the_road(undistorted_img, inversed_matrix,
line_lt, line_rt, keep_state)
mean_curvature_meter = np.mean(
[line_lt.curvature_meter, line_rt.curvature_meter])
font = cv2.FONT_HERSHEY_SIMPLEX
cv2.putText(blend_on_road,
'Curvature radius: {:.02f}m'.format(mean_curvature_meter),
(60, 60), font, 1, (255, 255, 255), 2)
cv2.putText(blend_on_road,
'Offset from center: {:.02f}m'.format(offset_meter), (60, 90),
font, 1, (255, 255, 255), 2)
processed_frames += 1
return blend_on_road
def save_and_continue(self): if self.index > 0: self.save() self.start_time_msec = self.start_time_msec + self.index * self.delay_msec self.index = 0 self.M_list = [] self.vidcap.set(cv2.CAP_PROP_POS_MSEC,(self.start_time_msec)) success, self.frame_start = self.vidcap.read() self.frame_start = undistort(self.frame_start) self.frame_start = downscale(self.frame_start, 1000, False, pad_img=False) self.frame_start_red = color_img(self.frame_start, [0, 0, 255])
def main_rest(self, img, prev_left_fit, prev_right_fit):
height = img.shape[0]
width = img.shape[1]
warped, M = utils.perspective(utils.undistort(img, self.objpoints, self.imgpoints))
warped_edge = utils.edge(warped)
left_base, right_base = utils.base(warped_edge)
left_fit, left_fit_m = utils.fit_rest(warped_edge, prev_left_fit)
right_fit, right_fit_m = utils.fit_rest(warped_edge, prev_right_fit)
curv = utils.curvature(left_fit_m, right_fit_m, height)
shif = utils.shift(left_fit_m, right_fit_m, height, width)
return left_fit, right_fit, curv, shif, M
def update(self):
# Get a frame from the video source
ret, frame = self.vid.get_frame()
if ret:
if _CALIBRATE:
frame = undistort(frame)
frame = cv2.resize(frame, self.resize_to_fit)
photo = PIL.ImageTk.PhotoImage(image=PIL.Image.fromarray(frame))
self.canvas_vid.create_image(0, 0, image=photo, anchor=Tkinter.NW)
self.canvas_vid.image = photo
self.window.after(self.delay, self.update)
def snapshot(self):
# Get a frame from the video source
ret, frame = self.vid.get_frame()
if ret:
if _CALIBRATE:
frame = undistort(frame)
img_path = "./snapshots/frame-" + time.strftime(
"%d-%m-%Y-%H-%M-%S") + ".jpg"
cv2.imwrite(img_path, cv2.cvtColor(frame, cv2.COLOR_RGB2BGR))
display_image(img_path,
self.frame_2_1,
1,
0,
resize_to=self.resize_to_fit)
btn_snapshot = Tkinter.Button(self.frame_2_1,
text="Segment",
width=10,
command=self.segment_image)
btn_snapshot.grid(row=2, column=0)
self.image_path = img_path
def __init__(self, video_path, start_time, fps, json_path, save_dir): self.index = 0 self.M_list = [] self.start_time_msec = string2msec(start_time) self.delay_msec = int(1000 * (1 / fps)) self.vidcap = cv2.VideoCapture(video_path) self.vidcap.set(cv2.CAP_PROP_POS_MSEC,(self.start_time_msec)) success, self.frame_start = self.vidcap.read() self.frame_start = undistort(self.frame_start) self.frame_start = downscale(self.frame_start, 1000, False, pad_img=False) self.frame_start_red = color_img(self.frame_start, [0, 0, 255]) self.json_path = json_path if not os.path.exists(json_path): os.mknod(json_path) json_data = dict() json_data['fps'] = fps json_data['video_path'] = video_path json_data['save_dir'] = save_dir json_data['sections'] = [] with open(json_path, 'w') as f: json.dump(json_data, f) self.save_dir = save_dir
arrayCounter = 0
arrayCurve = np.zeros([noOfArrayValues])
myVals = []
utils.initializeTrackers(intialTracbarVals)
while True:
success, img = cap.read()
#img = cv2.imread('test3.jpg')
if cameraFeed == False:
img = cv2.resize(img, (frameWidth, frameHeight), None)
imgWarpPoints = img.copy()
imgFinal = img.copy()
imgCanny = img.copy()
imgUndis = utils.undistort(img)
imgThres, imgCanny, imgColor = utils.thresholding(imgUndis)
src = utils.valTrackbars()
imgWarp = utils.perspective_warp(imgThres,
dst_size=(frameWidth, frameHeight),
src=src)
imgWarpPoints = utils.drawPoints(imgWarpPoints, src)
imgSliding, curves, lanes, ploty = utils.sliding_window(imgWarp,
draw_windows=True)
try:
curverad = utils.get_curve(imgFinal, curves[0], curves[1])
lane_curve = np.mean([curverad[0], curverad[1]])
imgFinal = utils.draw_lanes(img,
curves[0],
curves[1],
def start_single_record(counter, cap, cam, controller, listener, q, left_coord,
left_coeff, right_coord, right_coeff):
directory_rr = "./single_data/{}/R/raw".format(counter)
directory_lr = "./single_data/{}/L/raw".format(counter)
directory_ru = "./single_data/{}/R/undistorted".format(counter)
directory_lu = "./single_data/{}/L/undistorted".format(counter)
directory_leap_info = "./single_data/{}/leap_motion/tracking_data".format(
counter)
directory_rgb = "./single_data/{}/rgb".format(counter)
directory_z = "./single_data/{}/depth/z".format(counter)
directory_ir = "./single_data/{}/depth/ir".format(counter)
list_img_rr = []
list_img_ru = []
list_img_lr = []
list_img_lu = []
list_json = []
list_img_rgb = []
list_img_z = []
list_img_ir = []
record_if_valid = False
frame_counter = 0
if not cap:
print("error rgb cam")
exit(-1)
error_queue = False
cam.startCapture()
while True:
# print(frame_counter)
if (cv2.waitKey(1) == ord('s') and record_if_valid and frame_counter > args.n_min_frames) \
or error_queue:
# print(error_queue)
break
frame = controller.frame()
# controllo di validità per inizio registrazione (OPZIONALE)
# inizia a registrare i frame solo se leap motion rileva correttamente la mano
# print(self.listener.recording)
if utils.hand_is_valid(frame) and not record_if_valid:
print('\nhand is valid -> ready to start')
record_if_valid = True
# print(self.listener.recording)
print("start gesture")
listener.setRecording(True)
# print(self.listener.recording)
if record_if_valid:
print("\rrecord valid -> showing {}".format(frame_counter), end="")
utils.draw_ui(text="recording - press S to stop",
circle=True,
thickness=-1)
# RGB CAM
# get rgb image
# print(1)
ret, img_rgb = cap.read()
# print(2)
# resize dim img rgb
if not ret:
print("\nrgb cam not working")
exit(-1)
# cv2.imshow('img_rgb', img_rgb)
# cv2.waitKey(1)
# Leap Motion
if frame.is_valid:
image_l = frame.images[0]
image_r = frame.images[1]
# print(3)
else:
print("\rframe {} not valid".format(frame_counter), end="")
continue
if image_l.is_valid and image_r.is_valid:
# print(4)
raw_img_l = utils.get_raw_image(image_l)
raw_img_r = utils.get_raw_image(image_r)
# undistorted images
undistorted_left = utils.undistort(image_l, left_coord,
left_coeff, 400, 400)
undistorted_right = utils.undistort(image_r, right_coord,
right_coeff, 400, 400)
# print(5)
# show images
# previous position cv2.imshow()
# cv2.imshow('img_leap', undistorted_right)
# json
json_obj = utils.frame2json_struct(frame)
# print(6)
# PICOFLEXX
# imgs == (z, ir)
ret_pico, imgs = utils.get_images_from_picoflexx(q)
# print("ret_pico, z, ir", ret_pico, imgs[0], imgs[1])
if not ret_pico:
print("pico image not valid")
error_queue = True
continue
cv2.moveWindow('img_rgb', -700, 325)
cv2.moveWindow('img_leap', -1150, 400)
cv2.moveWindow('img_ir', -1500, 600)
cv2.imshow('img_leap', undistorted_right)
cv2.imshow('img_rgb', img_rgb)
cv2.imshow('img_ir', imgs[1])
# print(7)
list_img_rr.append(raw_img_r.copy())
list_img_ru.append(undistorted_right.copy())
list_img_lr.append(raw_img_l.copy())
list_img_lu.append(undistorted_left.copy())
list_img_rgb.append(img_rgb.copy())
list_json.append(json_obj)
list_img_z.append(imgs[0].copy())
list_img_ir.append(imgs[1].copy())
# list_img_z.append(z.copy())
# list_img_ir.append(ir.copy())
frame_counter += 1
# print(8)
else:
print('image not valid')
else:
print("\rerror in getting valid leap motion frame", end="")
# print(self.listener.recording)
listener.setRecording(False)
cam.stopCapture()
cap.release()
#write single record
print("saving record")
utils.draw_ui(text="Saving session...")
utils.save_single_record(list_img_rr, list_img_ru, list_img_lr,
list_img_lu, list_json, list_img_rgb, list_img_z,
list_img_ir, directory_rr, directory_ru,
directory_lr, directory_lu, directory_leap_info,
directory_rgb, directory_z, directory_ir)
def record(self):
list_img_rr = []
list_img_ru = []
list_img_lr = []
list_img_lu = []
list_json = []
list_img_rgb = []
list_img_z = []
list_img_ir = []
record_if_valid = False
frame_counter = 0
# print("ready to go")
# open rgb camera
# cap = cv2.VideoCapture(1)
#
# cap.set(cv2.CAP_PROP_FRAME_WIDTH, 1280.0)
# cap.set(cv2.CAP_PROP_FRAME_HEIGHT, 720.0)
# print(cap.get(cv2.CAP_PROP_FRAME_WIDTH), cap.get(cv2.CAP_PROP_FRAME_HEIGHT))
if not self.cap:
print("error rgb cam")
exit(-1)
error_queue = False
self.cam.startCapture()
while True:
# print(frame_counter)
if (cv2.waitKey(1) == ord('s') and record_if_valid and frame_counter > args.n_min_frames)\
or error_queue:
# print(error_queue)
break
frame = self.controller.frame()
# controllo di validità per inizio registrazione (OPZIONALE)
# inizia a registrare i frame solo se leap motion rileva correttamente la mano
# print(self.listener.recording)
if utils.hand_is_valid(frame) and not record_if_valid:
print('\nhand is valid -> ready to start')
record_if_valid = True
# print(self.listener.recording)
print("start gesture")
self.listener.setRecording(True)
# print(self.listener.recording)
if record_if_valid:
print("\rrecord valid -> showing {}".format(frame_counter),
end="")
utils.draw_ui(text="recording - press S to stop",
circle=True,
thickness=-1)
# RGB CAM
# get rgb image
# print(1)
ret, img_rgb = self.cap.read()
# print(2)
# resize dim img rgb
if not ret:
print("\nrgb cam not working")
exit(-1)
# cv2.imshow('img_rgb', img_rgb)
# cv2.waitKey(1)
# Leap Motion
if frame.is_valid:
image_l = frame.images[0]
image_r = frame.images[1]
# print(3)
else:
print("\rframe {} not valid".format(frame_counter), end="")
continue
if image_l.is_valid and image_r.is_valid:
# print(4)
raw_img_l = utils.get_raw_image(image_l)
raw_img_r = utils.get_raw_image(image_r)
# undistorted images
undistorted_left = utils.undistort(image_l,
self.left_coord,
self.left_coeff, 400,
400)
undistorted_right = utils.undistort(
image_r, self.right_coord, self.right_coeff, 400, 400)
# print(5)
# show images
# previous position cv2.imshow()
# cv2.imshow('img_leap', undistorted_right)
# json
json_obj = utils.frame2json_struct(frame)
# print(6)
# PICOFLEXX
#imgs == (z, ir)
ret_pico, imgs = utils.get_images_from_picoflexx(self.q)
# print("ret_pico, z, ir", ret_pico, imgs[0], imgs[1])
if not ret_pico:
print("pico image not valid")
error_queue = True
continue
cv2.moveWindow('img_rgb', -700, 325)
cv2.moveWindow('img_leap', -1150, 400)
cv2.moveWindow('img_ir', -1500, 600)
cv2.imshow('img_leap', undistorted_right)
cv2.imshow('img_rgb', img_rgb)
cv2.imshow('img_ir', imgs[1])
# print(7)
list_img_rr.append(raw_img_r.copy())
list_img_ru.append(undistorted_right.copy())
list_img_lr.append(raw_img_l.copy())
list_img_lu.append(undistorted_left.copy())
list_img_rgb.append(img_rgb.copy())
list_json.append(json_obj)
list_img_z.append(imgs[0].copy())
list_img_ir.append(imgs[1].copy())
# list_img_z.append(z.copy())
# list_img_ir.append(ir.copy())
frame_counter += 1
# print(8)
else:
print('image not valid')
else:
print("\rerror in getting valid leap motion frame", end="")
# print(self.listener.recording)
self.listener.setRecording(False)
self.cam.stopCapture()
# print(self.listener.recording)
# release rgb camera
# cap.release()
print('\nrecord {}/2 of g{} completed'.format(self.record_number,
self.gesture_id))
record_if_valid = False
# scrittura su disco
# if not args.on_disk:
if self.rewrite:
rewrite = True
else:
rewrite = False
return utils.GestureData(self.gesture_id,
list_img_rr,
list_img_ru,
list_img_lr,
list_img_lu,
list_json,
list_img_rgb,
list_img_z,
list_img_ir,
self.directory_rr,
self.directory_ru,
self.directory_lr,
self.directory_lu,
self.directory_leap_info,
self.directory_rgb,
self.directory_z,
self.directory_ir,
rewrite=rewrite)
def calibrate_with_ChArUco_board(result_filepath_no_ext,
calibImages,
calib_result_format,
dictionary,
squareL,
markerL,
tb,
lr,
pixels_per_mm,
isUndistort=False,
isPrintResult=False,
undistort_res_dirpath=None):
"""Caribrate the camera using images of specified ChArUco board."""
board, _ = utils.get_A4_board(dictionary, squareL, markerL, tb, lr,
pixels_per_mm)
# detect checker board intersection of ChArUco
allCharucoCorners = []
allCharucoIds = []
charucoCorners, charucoIds = [0, 0]
decimator = 0
num_images_to_use = 0
# critetion for sub pixel corner detection
criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 100,
0.00001)
decimation_interval = 2 # 1 means not applied
for calImg in calibImages:
calImg = cv2.cvtColor(calImg, cv2.COLOR_BGR2GRAY) # convert to gray
# find ArUco markers
corners, ids, rejectedImgPoints = \
aruco.detectMarkers(calImg, dictionary)
# find ChArUco corners
if len(corners) > 0:
# sub pixel detection
# corners = cv2.cornerSubPix(
# calImg,
# corners,
# winSize=(3,3),
# zeroZone=(-1,-1),
# criteria=criteria)
res2 = aruco.interpolateCornersCharuco(corners, ids, calImg, board)
if (res2[1] is not None) and\
(res2[2] is not None) and\
(len(res2[1]) > 5) and\
(decimator % decimation_interval == 0):
allCharucoCorners.append(res2[1])
allCharucoIds.append(res2[2])
num_images_to_use += 1
decimator += 1
aruco.drawDetectedMarkers(calImg, corners, ids)
print("\n Use " + str(num_images_to_use) + " images for this calibration.")
try:
imgSize = calibImages[0].shape[:2]
calib_params = aruco.calibrateCameraCharucoExtended(
allCharucoCorners, allCharucoIds, board, imgSize, None, None)
except Exception as e:
print("Failed to calibrate..., ", e.args)
return -1
if isPrintResult:
utils.show_calibration_params(calib_params)
_, camMat, distCoef, rvecs, tvecs, stdIn, stdEx, peojErr = calib_params
save_param_list = [camMat, distCoef, rvecs, tvecs, stdIn, stdEx]
# save the camera parameters
if calib_result_format == 'json':
cam_param_path = result_filepath_no_ext + '.json'
with open(cam_param_path, mode='w') as f:
data = {
"camera_matrix": cameraMatrix.tolist(),
"dist_coeff": distCoeffs.tolist(),
"rvecs": rvecs,
"tvecs": tvecs
}
json.dump(data, f, sort_keys=True, indent=4)
elif calib_result_format == 'pkl':
cam_param_path = result_filepath_no_ext + '.pkl'
with open(cam_param_path, mode='wb') as f:
pickle.dump(save_param_list, f, protocol=-1)
print("Saved " + cam_param_path)
if isUndistort:
if undistort_res_dirpath is None:
print("Error: Please specify save path for undistort images.")
return -1
with open(cam_param_path, 'rb') as f:
camera_params = pickle.load(f)
cam_mat, dist_coeffs, rvecs, tvecs, stdIn, stdEx = camera_params
utils.undistort(cam_mat, dist_coeffs, calibImages,
undistort_res_dirpath)