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 find_chessboard_boundary_for_depth_image(self):
        boundary = {}

        for (serial, frameset) in self.frames.items():

            depth_frame = post_process_depth_frame(frameset[rs.stream.depth])
            infrared_frame = frameset[(rs.stream.infrared, 1)]
            found_corners, points2D = cv_find_chessboard(
                depth_frame, infrared_frame, self.charuco_params)
            boundary[serial] = [
                np.floor(np.amin(points2D[0, :])).astype(int),
                np.floor(np.amax(points2D[0, :])).astype(int),
                np.floor(np.amin(points2D[1, :])).astype(int),
                np.floor(np.amax(points2D[1, :])).astype(int)
            ]

        return boundary
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])