def get_corners(camera, camera_frame, intrinsic, charuco_board):
ir_frame = camera_frame[camera][(rs.stream.infrared, 1)]
depth_frame = post_process_depth_frame(
camera_frame[camera][rs.stream.depth])
depth_intrinsics = intrinsic[camera][rs.stream.depth]
list_found_corners, IDs = hf.cv_find_chessboard(depth_frame, ir_frame,
charuco_board)
validPoints = [False] * charuco_board.chessboardCorners.shape[0]
corners3D = [IDs, None, None, validPoints]
points3D = np.zeros((3, charuco_board.chessboardCorners.shape[0]))
if list_found_corners:
found_corners = list_found_corners[0]
for index in range(len(found_corners)):
theID = IDs[0][index][0]
corner = found_corners[index].flatten()
depth = hf.get_depth_at_pixel(depth_frame, corner[0], corner[1])
if depth != 0 and depth is not None:
validPoints[theID] = True
[X, Y, Z] = hf.convert_depth_pixel_to_metric_coordinate(
depth, corner[0], corner[1], depth_intrinsics)
points3D[0, theID] = X
points3D[1, theID] = Y
points3D[2, theID] = Z
corners3D = IDs, found_corners, points3D, validPoints
return corners3D
def get_chessboard_corners_in3d(self):
"""
Searches the chessboard corners in the infrared images of
every connected device and uses the information in the
corresponding depth image to calculate the 3d
coordinates of the chessboard corners in the coordinate system of
the camera
Returns:
-----------
corners3D : dict
keys: str
Serial number of the device
values: [success, points3D, validDepths]
success: bool
Indicates wether the operation was successfull
points3d: array
(3,N) matrix with the coordinates of the chessboard corners
in the coordinate system of the camera. N is the number of corners
in the chessboard. May contain points with invalid depth values
validDephts: [bool]*
Sequence with length N indicating which point in points3D has a valid depth value
"""
corners3D = {}
for (serial, frameset) in self.frames.items():
depth_frame = post_process_depth_frame(frameset[rs.stream.depth])
infrared_frame = frameset[(rs.stream.infrared, 1)]
depth_intrinsics = self.intrinsic[serial][rs.stream.depth]
list_found_corners, IDs = cv_find_chessboard(
depth_frame, infrared_frame, self.charuco_params)
corners3D[serial] = [IDs, None, None, None]
if list_found_corners:
found_corners = list_found_corners[0]
points3D = np.zeros(
(3, self.charuco_params.chessboardCorners.shape[0]))
validPoints = [
False
] * self.charuco_params.chessboardCorners.shape[0]
for index in range(len(found_corners)):
theID = IDs[0][index][0]
corner = found_corners[index].flatten()
depth = get_depth_at_pixel(depth_frame, corner[0],
corner[1])
if depth != 0 and depth is not None:
validPoints[theID] = True
[X, Y, Z] = convert_depth_pixel_to_metric_coordinate(
depth, corner[0], corner[1], depth_intrinsics)
points3D[0, theID] = X
points3D[1, theID] = Y
points3D[2, theID] = Z
corners3D[serial] = IDs, found_corners, points3D, validPoints
return corners3D
def get_transformation_matrix(frames_dict, camera_set, intrinsic,
charuco_board):
from_camera = np.transpose(np.array([0, 0, 0]))
to_camera = np.transpose(np.array([0, 0, 0]))
for frame_count in range(len(frames_dict)):
camera_frame = frames_dict[frame_count]
corners3D = {}
for camera in camera_set:
ir_frame = camera_frame[camera][(rs.stream.infrared, 1)]
depth_frame = post_process_depth_frame(
camera_frame[camera][rs.stream.depth])
depth_intrinsics = intrinsic[camera][rs.stream.depth]
list_found_corners, IDs = cv_find_chessboard(
depth_frame, ir_frame, charuco_board)
validPoints = [False] * charuco_board.chessboardCorners.shape[0]
corners3D[camera] = [IDs, None, None, validPoints]
points3D = np.zeros((3, charuco_board.chessboardCorners.shape[0]))
if list_found_corners:
found_corners = list_found_corners[0]
for index in range(len(found_corners)):
theID = IDs[0][index][0]
corner = found_corners[index].flatten()
depth = get_depth_at_pixel(depth_frame, corner[0],
corner[1])
if depth != 0 and depth is not None:
validPoints[theID] = True
[X, Y, Z] = convert_depth_pixel_to_metric_coordinate(
depth, corner[0], corner[1], depth_intrinsics)
points3D[0, theID] = X
points3D[1, theID] = Y
points3D[2, theID] = Z
corners3D[camera] = IDs, found_corners, points3D, validPoints
for corner in range(len(charuco_board.nearestMarkerCorners)):
if corners3D[camera_set[0]][3][corner] and corners3D[
camera_set[1]][3][corner]:
to_camera = np.vstack(
(to_camera, np.array(corners3D[camera_set[0]][2][:,
corner])))
from_camera = np.vstack(
(from_camera,
np.array(corners3D[camera_set[1]][2][:, corner])))
if np.size(from_camera) > 30:
rotation, translation, rmsd = calculate_transformation_kabsch(
np.transpose(to_camera[1:, :]), np.transpose(from_camera[1:, :]))
return Transformation(rotation, translation), rmsd
else:
return 0, np.inf
def get_charuco_points(camera_frame, transform, intrinsic, charuco_board):
ir_frame = camera_frame[(rs.stream.infrared, 1)]
depth_frame = post_process_depth_frame(camera_frame[rs.stream.depth])
depth_intrinsics = intrinsic[rs.stream.depth]
list_found_corners, IDs = cv_find_chessboard(depth_frame, ir_frame,
charuco_board)
board_points = np.array([0, 0, 0])
if list_found_corners:
found_corners = list_found_corners[0]
for idx in range(len(found_corners)):
corner = found_corners[idx].flatten()
depth = get_depth_at_pixel(depth_frame, corner[0], corner[1])
coords = convert_depth_pixel_to_metric_coordinate(
depth, corner[0], corner[1], depth_intrinsics)
board_points = np.vstack((board_points, coords))
return transform.apply_transformation(np.transpose(
board_points[1:, :]))
else:
return np.array([])
def get_charuco_points_ID(camera_frame, transform, intrinsic, charuco_board):
ir_frame = camera_frame[(rs.stream.infrared, 1)]
depth_frame = post_process_depth_frame(camera_frame[rs.stream.depth])
depth_intrinsics = intrinsic[rs.stream.depth]
list_found_corners, IDs = cv_find_chessboard(depth_frame, ir_frame,
charuco_board)
board_points = np.array(
np.zeros((3, charuco_board.chessboardCorners.shape[0])))
validPoints = np.array([False] * charuco_board.chessboardCorners.shape[0])
if list_found_corners:
found_corners = list_found_corners[0]
for idx in range(len(found_corners)):
corner = found_corners[idx].flatten()
ID = IDs[0][idx][0]
validPoints[ID] = True
depth = get_depth_at_pixel(depth_frame, corner[0], corner[1])
coords = convert_depth_pixel_to_metric_coordinate(
depth, corner[0], corner[1], depth_intrinsics)
board_points[:, ID] = coords
return transform.apply_transformation(board_points), validPoints
else:
return np.array([]), np.array([False] *
charuco_board.chessboardCorners.shape[0])
