def main(serial):
# Read camera parameters
image_width, image_height, camera_matrix, dist_coeffs = utils.get_camera_params(serial)
# Set up webcam
cap = utils.get_video_cap(serial, image_width, image_height)
# Set up aruco dict
aruco_dict = cv2.aruco.Dictionary_get(utils.get_marker_dict_id())
while True:
if cv2.waitKey(1) == 27: # Esc key
break
image = None
while image is None:
_, image = cap.read()
# Undistort image and detect markers
image = cv2.undistort(image, camera_matrix, dist_coeffs)
corners, ids, _ = cv2.aruco.detectMarkers(image, aruco_dict)
# Show detections
image_copy = image.copy()
if ids is not None:
cv2.aruco.drawDetectedMarkers(image_copy, corners, ids)
cv2.imshow('out', image_copy)
cap.release()
cv2.destroyAllWindows()
def __init__(self, serial):
image_width, image_height, camera_matrix, dist_coeffs = utils.get_camera_params(serial)
self._map_x, self._map_y = cv2.initUndistortRectifyMap(camera_matrix, dist_coeffs, None, camera_matrix, (image_width, image_height), cv2.CV_32FC1)
self._cap = utils.get_video_cap(serial, image_width, image_height)
self._queue = Queue(maxsize=1)
self._thread = Thread(target=self._worker)
self._thread.start()
def main(args):
# Read camera parameters
camera_params_file_path = utils.get_camera_params_file_path(
args.camera_name)
image_width, image_height, camera_matrix, dist_coeffs = utils.get_camera_params(
camera_params_file_path)
# Set up webcam
cap = utils.get_video_cap(image_width, image_height, args.camera_id)
# Set up aruco dict
params = utils.get_marker_parameters()
aruco_dict = cv2.aruco.Dictionary_get(params['dict_id'])
# Enable corner refinement
#detector_params = cv2.aruco.DetectorParameters_create()
#detector_params.cornerRefinementMethod = cv2.aruco.CORNER_REFINE_SUBPIX
while True:
if cv2.waitKey(1) == 27: # Esc key
break
_, image = cap.read()
if image is None:
continue
# Undistort image and detect markers
image = cv2.undistort(image, camera_matrix, dist_coeffs)
#corners, ids, _ = cv2.aruco.detectMarkers(image, aruco_dict, parameters=detector_params)
corners, ids, _ = cv2.aruco.detectMarkers(image, aruco_dict)
# Show detections
image_copy = image.copy()
if ids is not None:
cv2.aruco.drawDetectedMarkers(image_copy, corners, ids)
cv2.imshow('out', image_copy)
cap.release()
cv2.destroyAllWindows()
def main(args):
# Read camera parameters
camera_params_file_path = utils.get_camera_params_file_path(
args.camera_name)
image_width, image_height, camera_matrix, dist_coeffs = utils.get_camera_params(
camera_params_file_path)
# Set up webcam
cap = utils.get_video_cap(image_width, image_height, args.camera_id)
# Board and marker params
boards = [{
'name': 'robots',
'corner_offset_mm': 36
}, {
'name': 'cubes',
'corner_offset_mm': 12
}]
marker_params = utils.get_marker_parameters()
room_length_mm = 1000 * args.room_length
room_width_mm = 1000 * args.room_width
room_length_pixels = int(room_length_mm * marker_params['pixels_per_mm'])
room_width_pixels = int(room_width_mm * marker_params['pixels_per_mm'])
# Set up aruco dicts
for board in boards:
aruco_dict = cv2.aruco.Dictionary_get(marker_params['dict_id'])
aruco_dict.bytesList = aruco_dict.bytesList[utils.get_marker_ids(
'corners_{}'.format(board['name']))]
board['board_dict'] = aruco_dict
aruco_dict = cv2.aruco.Dictionary_get(marker_params['dict_id'])
aruco_dict.bytesList = aruco_dict.bytesList[utils.get_marker_ids(
board['name'])]
board['marker_dict'] = aruco_dict
# Board warping
for board in boards:
corner_offset_pixels = board['corner_offset_mm'] * marker_params[
'pixels_per_mm']
board['src_points'] = None
board['dst_points'] = [
[-corner_offset_pixels, -corner_offset_pixels],
[room_length_pixels + corner_offset_pixels, -corner_offset_pixels],
[
room_length_pixels + corner_offset_pixels,
room_width_pixels + corner_offset_pixels
],
[-corner_offset_pixels, room_width_pixels + corner_offset_pixels]
]
# Enable corner refinement
detector_params = cv2.aruco.DetectorParameters_create()
detector_params.cornerRefinementMethod = cv2.aruco.CORNER_REFINE_SUBPIX
# Set up server
address = ('localhost', 6000)
listener = Listener(address, authkey=b'secret password')
conn = None
def process_image(image):
# Undistort image
image = cv2.undistort(image, camera_matrix, dist_coeffs)
data = {}
for board in boards:
board_name = board['name']
data[board_name] = {}
if board['src_points'] is None:
# Detect board markers (assumes board won't move since this is only done once)
#board_corners, board_indices, _ = cv2.aruco.detectMarkers(image, board['board_dict'], parameters=detector_params)
board_corners, board_indices, _ = cv2.aruco.detectMarkers(
image, board['board_dict'])
# Show detections
if args.debug:
image_copy = image.copy()
if board_indices is not None:
cv2.aruco.drawDetectedMarkers(image_copy,
board_corners,
board_indices)
cv2.imshow('{}_board_corners'.format(board_name),
image_copy)
# Ensure board was found
if board_indices is None or len(board_indices) < 4:
data[
board_name] = None # None rather than {} to signify board was not detected
continue
board['src_points'] = []
for marker_index, corner in sorted(
zip(board_indices, board_corners)):
board['src_points'].append(
corner.squeeze(0).mean(axis=0).tolist())
else:
# Warp the board
M = cv2.getPerspectiveTransform(
np.asarray(board['src_points'], dtype=np.float32),
np.asarray(board['dst_points'], dtype=np.float32))
warped_image = cv2.warpPerspective(
image, M, (room_length_pixels, room_width_pixels))
# Detect markers in warped image
corners, indices, _ = cv2.aruco.detectMarkers(
warped_image,
board['marker_dict'],
parameters=detector_params)
# Show detections
if args.debug:
image_copy = warped_image.copy()
if indices is not None:
cv2.aruco.drawDetectedMarkers(image_copy, corners,
indices)
image_copy = cv2.resize(image_copy,
(int(image_copy.shape[1] / 2),
int(image_copy.shape[0] / 2)))
cv2.imshow(board_name, image_copy)
if indices is None:
continue
# Compute poses
board_data = {}
for marker_index, corner in zip(indices, corners):
marker_index = marker_index.item()
marker_corners = corner.squeeze(0)
marker_center = marker_corners.mean(axis=0)
# Compute heading estimates for each of the four marker corners
diffs = [c - marker_center for c in marker_corners]
angles = np.array(
[np.arctan2(-diff[1], diff[0]) for diff in diffs])
angles = angles + np.radians([-135, -45, 45, 135])
angles = np.mod(angles + np.pi, 2 * np.pi) - np.pi
# If multiple markers are detected on same cube, use the marker on top (which should have the lowest angle_std)
angle_std = angles.std()
if board_name == 'cubes' and marker_index in board_data and angle_std > board_data[
marker_index]['angle_std']:
continue
# Compute position and heading
position = [
(marker_center[0] / marker_params['pixels_per_mm'] -
room_length_mm / 2) / 1000,
(room_width_mm / 2 -
(marker_center[1] / marker_params['pixels_per_mm'])) /
1000
]
heading = angles.mean()
marker_data = {'position': position, 'heading': heading}
if board_name == 'cubes':
marker_data['angle_std'] = angle_std
board_data[marker_index] = marker_data
data[board_name] = board_data
return data
while True:
if cv2.waitKey(1) == 27: # Esc key
break
if conn is None:
print('Waiting for connection...')
conn = listener.accept()
print('Connected!')
_, image = cap.read()
if image is None:
continue
data = process_image(image)
try:
conn.send(data)
except:
conn = None
cap.release()
cv2.destroyAllWindows()
def main(args):
# Set up webcam
image_width = args.image_width
image_height = args.image_height
cap = utils.get_video_cap(args.serial, image_width, image_height)
# Set up aruco dict and board
aruco_dict = cv2.aruco.Dictionary_get(utils.get_marker_dict_id())
board_params = utils.get_charuco_board_params()
board = cv2.aruco.CharucoBoard_create(board_params['squares_x'],
board_params['squares_y'],
board_params['square_length'],
board_params['marker_length'],
aruco_dict)
# Enable corner refinement
detector_params = cv2.aruco.DetectorParameters_create()
detector_params.cornerRefinementMethod = cv2.aruco.CORNER_REFINE_SUBPIX
# Capture images
all_corners = []
all_ids = []
all_imgs = []
while True:
_, image = cap.read()
if image is None:
continue
# Detect markers
corners, ids, rejected = cv2.aruco.detectMarkers(
image, aruco_dict, parameters=detector_params)
# Refine markers
corners, ids, _, _ = cv2.aruco.refineDetectedMarkers(
image, board, corners, ids, rejected)
# Interpolate corners
if ids is not None:
_, curr_charuco_corners, curr_charuco_ids = cv2.aruco.interpolateCornersCharuco(
corners, ids, image, board)
# Draw results
image_copy = image.copy()
if ids is not None:
cv2.aruco.drawDetectedMarkers(image_copy, corners)
if curr_charuco_corners is not None:
cv2.aruco.drawDetectedCornersCharuco(image_copy,
curr_charuco_corners,
curr_charuco_ids)
# Display and wait for keyboard input
cv2.putText(
image_copy,
"Press 'c' to add current frame. 'ESC' to finish and calibrate",
(10, 20), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (255, 0, 0), 2)
cv2.imshow('out', image_copy)
key = cv2.waitKey(10) & 0xFF
if key == 27:
break
if key == ord('c') and ids is not None and len(ids) > 4:
print('Frame captured')
all_corners.append(corners)
all_ids.append(ids)
all_imgs.append(image)
cap.release()
cv2.destroyAllWindows()
if len(all_imgs) < 1:
print('Not enough captures for calibration')
sys.exit()
# Aruco calibration
all_corners_concatenated = []
all_ids_concatenated = []
marker_counter_per_frame = []
for corners, ids in zip(all_corners, all_ids):
marker_counter_per_frame.append(len(corners))
all_corners_concatenated.extend(corners)
all_ids_concatenated.extend(ids)
all_corners_concatenated = np.asarray(all_corners_concatenated)
all_ids_concatenated = np.asarray(all_ids_concatenated)
marker_counter_per_frame = np.asarray(marker_counter_per_frame)
rep_error_aruco, camera_matrix, dist_coeffs, _, _ = cv2.aruco.calibrateCameraAruco(
all_corners_concatenated, all_ids_concatenated,
marker_counter_per_frame, board, (image_width, image_height), None,
None)
# Charuco calibration using previous camera params
all_charuco_corners = []
all_charuco_ids = []
for corners, ids, image in zip(all_corners, all_ids, all_imgs):
_, curr_charuco_corners, curr_charuco_ids = cv2.aruco.interpolateCornersCharuco(
corners,
ids,
image,
board,
cameraMatrix=camera_matrix,
distCoeffs=dist_coeffs)
all_charuco_corners.append(curr_charuco_corners)
all_charuco_ids.append(curr_charuco_ids)
if len(all_charuco_corners) < 4:
print('Not enough corners for calibration')
sys.exit()
rep_error, camera_matrix, dist_coeffs, _, _ = cv2.aruco.calibrateCameraCharuco(
all_charuco_corners, all_charuco_ids, board,
(image_width, image_height), camera_matrix, dist_coeffs)
print('Rep Error:', rep_error)
print('Rep Error Aruco:', rep_error_aruco)
# Save camera params
camera_params_file_path = Path('camera_params') / '{}.yml'.format(
args.serial)
camera_params_dir = camera_params_file_path.parent
if not camera_params_dir.exists():
camera_params_dir.mkdir()
fs = cv2.FileStorage(str(camera_params_file_path), cv2.FILE_STORAGE_WRITE)
fs.write('image_width', image_width)
fs.write('image_height', image_height)
fs.write('camera_matrix', camera_matrix)
fs.write('distortion_coefficients', dist_coeffs)
fs.write('avg_reprojection_error', rep_error)
fs.release()
print('Calibration saved to ', camera_params_file_path)
# Show interpolated charuco corners
for image, ids, charuco_corners, charuco_ids in zip(
all_imgs, all_ids, all_charuco_corners, all_charuco_ids):
image_copy = image.copy()
if ids is not None:
cv2.aruco.drawDetectedCornersCharuco(image_copy, charuco_corners,
charuco_ids)
cv2.imshow('out', image_copy)
key = cv2.waitKey(0)
if key == 27: # Esc key
break