def draw_intersection_lines(self, frame, corners):
#Draw lines to intersection (horizon)
for corner in corners.values():
cv2.line(frame, corner, self.infinity_intersection, (150, 0, 0), 1, cv2.LINE_AA)
draw_horizontal_line(frame, self.horizon, (200, 0, 0), 2, cv2.LINE_AA)
draw_vertical_line(frame, self.infinity_intersection[0], (200, 0, 0), 1, cv2.LINE_AA)
def main():
global mode
logging.basicConfig(format='[%(asctime)s] [%(threadName)13s] %(levelname)7s: %(message)s', level=logging.DEBUG)
#Start camera
cap = start_camera()
#Configure windows
configure_windows()
#Setup pattern finder
pattern_finder = PatternFinder(pattern_type, pattern_dims)
pattern_finder.start()
#Setup calibration variables
pattern_found = False
last_calibration_sample_time = None
#Setup coordinate mapper
coordinate_mapper = CoordinateMapper(
checkerboard_distance=0.20,
checkerboard_width=0.279,
checkerboard_height=0.159
)
#Setup display flags
show_coordinate_grid = False
while True:
#Try to get a frame from camera
success, distorted_frame_clean = cap.read()
if not success:
logging.warning("Could not retrieve frame from camera")
continue # Let's just try again
#Make original frame read-only
distorted_frame_clean.flags.writeable = False
image_size = tuple(distorted_frame_clean.shape[0:2][::-1])
calibrator.image_size = image_size
#Create copy that we can draw on
distorted_frame = copy.deepcopy(distorted_frame_clean)
"""
#Draw a grid on distorted frame
draw_grid(
frame=distorted_frame,
x_range=(0, image_size[0], image_size[0] // 10),
y_range=(0, image_size[1], image_size[1] // 10),
color=(0, 0, 0), thickness=1, line_type=cv2.LINE_AA
)
"""
undistorted_frame_clean = None
undistorted_frame = None
if mode == Mode.Calibrated:
#If calibration has been loaded, show the
undistorted_frame_clean = cv2.remap(distorted_frame_clean, calibrator.map_x, calibrator.map_y,
cv2.INTER_LINEAR)
undistorted_frame_clean.flags.writeable = False
undistorted_frame = cv2.remap(distorted_frame, calibrator.map_x, calibrator.map_y, cv2.INTER_LINEAR)
if not pattern_finder.recognition_in_progress:
#Get the results of last recognition
pattern_found = pattern_finder.pattern_found
pattern_points = pattern_finder.pattern_points
#Start a new recognition
#Calculate the corners of cropped image according to crop_scale
crop_corners = (
(int(image_size[0]/2*crop_scale), int(image_size[1]/2*crop_scale)),
(int(image_size[0] - image_size[0]/2*crop_scale), int(image_size[1] - image_size[1]/2*crop_scale))
)
if pattern_points is not None:
pattern_points += crop_corners[0]
#If calibration has been loaded, let's find pattern from calibrated image
if mode == Mode.Calibrated:
target_frame = undistorted_frame
target_frame_clean = undistorted_frame_clean
else:
target_frame = distorted_frame
target_frame_clean = distorted_frame_clean
#Show crop on target frame with a rectangle, and crop the clean frame
cv2.rectangle(target_frame, crop_corners[0], crop_corners[1], (255, 0, 0), 1, cv2.LINE_AA)
cropped_frame = crop_frame(target_frame_clean, crop_corners)
#Start a new pattern Recognition
pattern_finder.start_pattern_recognition(cropped_frame)
if pattern_found:
if mode == Mode.Calibration:
if last_calibration_sample_time is None or time.time() - last_calibration_sample_time > 0.2:
last_calibration_sample_time = time.time()
calibrator.add_sample(pattern_points)
show_ui_taking_sample(distorted_frame)
if calibrator.number_of_samples > 100:
show_ui_calibrating(distorted_frame)
calibrator.calibrate()
calibrator.calculate_new_camera_matrix()
calibrator.generate_maps()
logging.info("Calibration Finished")
logging.info("Calibration error-rate: {}".format(calibrator.accuracy))
mode = Mode.Calibrated
calibrator.save_results("AutoSave")
continue #Needed for initialising "undistorted_frame"
# If view is calibrated, then draw chessboard on undistorted frame, otherwise use the original/distorted one.
if mode == Mode.Calibrated:
target_frame = undistorted_frame
else:
target_frame = distorted_frame
cv2.drawChessboardCorners(target_frame, pattern_dims, pattern_points, pattern_found)
# Find four corners of the board and use then to calculate mapping constants
corners = find_checkerboard_corners(pattern_points, pattern_dims)
# TODO: This should be done somehow differently?
coordinate_mapper.image_dims = (distorted_frame_clean.shape[1], distorted_frame_clean.shape[0])
coordinate_mapper.calculate_constants(corners)
if show_coordinate_grid:
coordinate_mapper.draw_intersection_lines(target_frame, corners)
coordinate_mapper.draw_grid(target_frame)
draw_corners(target_frame, corners)
# Vertical line in the center
draw_vertical_line(target_frame, target_frame.shape[1] // 2, (100, 0, 0), 1,
cv2.LINE_AA)
logging.info("Constants {}".format(coordinate_mapper.constants))
# Display the distorted frame (original with additional lines)
distorted_frame = cv2.resize(distorted_frame, (0, 0), fx=0.5, fy=0.5)
cv2.imshow('distorted', distorted_frame)
if mode == Mode.Calibrated:
undistorted_frame = cv2.resize(undistorted_frame, (0, 0), fx=0.5, fy=0.5)
cv2.imshow('undistorted', undistorted_frame)
# TODO: Why does it work strange?
#if calibrator.roi is not None:
# x, y, w, h = [elem // 2 for elem in calibrator.roi]
# if w != 0 and h != 0:
# cropped = undistorted_frame[y:y + h, x:x + w]
# cv2.imshow('undistorted_cropped', cropped)
key_no = cv2.waitKey(30) & 0xFF
if key_no == 255:
# no key was pressed:
pass
elif key_no == ord('q'):
logging.info("Quitting...")
break
elif key_no == ord('g'):
show_coordinate_grid = not show_coordinate_grid
elif key_no == ord('c'):
logging.info("Calibration started")
calibrator.clear()
mode = Mode.Calibration
elif key_no == ord('s'):
calibrator.save_results("ManualSave")
logging.info("Calibration results saved")
elif key_no == ord('l'):
timestamp = input("Type timestamp to load: ")
try:
calibrator.load_results(timestamp)
except IOError:
logging.exception("Could not load all calibration files.")
else:
mode = Mode.Calibrated
calibrator.calculate_new_camera_matrix()
calibrator.generate_maps()
cv2.setTrackbarPos('alpha', 'undistorted', int(calibrator.alpha * 100))
logging.info("Calibration results loaded")
elif key_no == ord('p'):
calibrator.plot()
else:
print("Press:\n"
"\t'q' to quit\n"
"\t'c' to start calibration\n"
"\t'g' to toggle grid\n"
"\t's' to save calibration results\n"
"\t'l' to load calibration results\n"
)
## End of the main loop ##
cap.release()
cv2.destroyAllWindows()
