diff --git a/pykinect_azure/k4a/calibration.py b/pykinect_azure/k4a/calibration.py index e84f77e..de1e5dd 100644 --- a/pykinect_azure/k4a/calibration.py +++ b/pykinect_azure/k4a/calibration.py @@ -1,5 +1,7 @@ import ctypes +import numpy as np + from pykinect_azure.k4a import _k4a @@ -10,6 +12,8 @@ def __init__(self, calibration_handle: _k4a.k4a_calibration_t): self._handle = calibration_handle self.color_params = self._handle.color_camera_calibration.intrinsics.parameters.param self.depth_params = self._handle.depth_camera_calibration.intrinsics.parameters.param + self.color_extrinsics = self._handle.color_camera_calibration.extrinsics + self.depth_extrinsics = self._handle.depth_camera_calibration.extrinsics def __del__(self): @@ -47,6 +51,43 @@ def get_matrix(self, camera: _k4a.k4a_calibration_type_t): [0, self.depth_params.fy, self.depth_params.cy], [0, 0, 1]] + def get_extrinsic_matrix(self, camera: _k4a.k4a_calibration_type_t): + if camera == _k4a.K4A_CALIBRATION_TYPE_COLOR: + color_rotation = np.array(list(self.color_extrinsics.rotation)).reshape(3, 3) + color_translation = np.array(list(self.color_extrinsics.translation)) * 1e-3 + color_matrix = np.eye(4) + color_matrix[:3, :3] = color_rotation + color_matrix[:3, 3] = color_translation + return color_matrix.tolist() + + elif camera == _k4a.K4A_CALIBRATION_TYPE_DEPTH: + depth_rotation = np.array(list(self.depth_extrinsics.rotation)).reshape(3, 3) + depth_translation = np.array(list(self.depth_extrinsics.translation)) * 1e-3 + depth_matrix = np.eye(4) + depth_matrix[:3, :3] = depth_rotation + depth_matrix[:3, 3] = depth_translation + return depth_matrix.tolist() + + def get_distortion_parameters(self, camera: _k4a.k4a_calibration_type_t): + if camera == _k4a.K4A_CALIBRATION_TYPE_COLOR: + return { + "k": [self.color_params.k1, self.color_params.k2, self.color_params.k3, self.color_params.k4, + self.color_params.k5, self.color_params.k6], + "p": [self.color_params.p1, self.color_params.p2], + "codx": self.color_params.codx, + "cody": self.color_params.cody, + "metric_radius": self.color_params.metric_radius + } + elif camera == _k4a.K4A_CALIBRATION_TYPE_DEPTH: + return { + "k": [self.depth_params.k1, self.depth_params.k2, self.depth_params.k3, self.depth_params.k4, + self.depth_params.k5, self.depth_params.k6], + "p": [self.depth_params.p1, self.depth_params.p2], + "codx": self.depth_params.codx, + "cody": self.depth_params.cody, + "metric_radius": self.depth_params.metric_radius + } + def is_valid(self): return self._handle diff --git a/pykinect_azure/k4a/image.py b/pykinect_azure/k4a/image.py index e08ffca..22fbf9f 100644 --- a/pykinect_azure/k4a/image.py +++ b/pykinect_azure/k4a/image.py @@ -1,3 +1,5 @@ +import ctypes + import numpy as np import cv2 @@ -35,10 +37,60 @@ def create(image_format,width_pixels,height_pixels,stride_bytes): return Image(handle) + @staticmethod + def create_custom16_from_numpy(arr: np.ndarray): + if arr.dtype != np.uint16: + arr = arr.astype(np.uint16) + assert len(arr.shape) == 2 or len(arr.shape) == 3 + if len(arr.shape) == 3 and arr.shape[2] != 1: + arr = arr[:, :, 0:1] + + height, width = arr.shape[:2] + handle = _k4a.k4a_image_t() + _k4a.VERIFY(_k4a.k4a_image_create_from_buffer(_k4a.K4A_IMAGE_FORMAT_CUSTOM16, width, height, width * 2, arr.ctypes.data_as(ctypes.POINTER(ctypes.c_ubyte)), arr.size, None, None, handle), "Create image failed!") + + return Image(handle) + + @staticmethod + def create_custom16_from_shape(width: int, height: int): + arr = np.zeros((height, width, 1), dtype=np.uint8) + handle = _k4a.k4a_image_t() + _k4a.VERIFY(_k4a.k4a_image_create_from_buffer(_k4a.K4A_IMAGE_FORMAT_CUSTOM16, width, height, width * 2, arr.ctypes.data_as(ctypes.POINTER(ctypes.c_ubyte)), arr.size, None, None, handle), + "Create image failed!") + + return Image(handle) + + @staticmethod + def create_bgra32_from_numpy(arr: np.ndarray): + if arr.dtype != np.uint8: + arr = arr.astype(np.uint8) + assert len(arr.shape) == 2 or len(arr.shape) == 3 + if len(arr.shape) == 3: + if arr.shape[2] > 4: + arr = arr[:, :, 0:4] + elif arr.shape[2] < 4: + _arr = np.zeros((arr.shape[0], arr.shape[1], 4), dtype=np.uint8) + _arr[:, :, 0:arr.shape[2]] = arr + arr = _arr + else: + arr = np.repeat(arr[:, :, np.newaxis], 4, axis=2) + + height, width = arr.shape[:2] + handle = _k4a.k4a_image_t() + _k4a.VERIFY(_k4a.k4a_image_create_from_buffer(_k4a.K4A_IMAGE_FORMAT_COLOR_BGRA32, width, height, width * 4, arr.ctypes.data_as(ctypes.POINTER(ctypes.c_ubyte)), arr.size, None, None, handle), "Create image failed!") + + return Image(handle) + + @staticmethod + def create_bgra32_from_shape(width: int, height: int): + arr = np.zeros((height, width, 4), dtype=np.uint8) + handle = _k4a.k4a_image_t() + _k4a.VERIFY(_k4a.k4a_image_create_from_buffer(_k4a.K4A_IMAGE_FORMAT_COLOR_BGRA32, width, height, width * 4, arr.ctypes.data_as(ctypes.POINTER(ctypes.c_ubyte)), arr.size, None, None, handle), "Create image failed!") + @property def width(self): return self.get_width_pixels() - + @property def height(self): return self.get_height_pixels() @@ -55,6 +107,17 @@ def format(self): def size(self): return self.get_size() + @property + def timestamp_usec(self): + return self.get_timestamp_usec() + + @property + def device_timestamp_usec(self): + return self.get_device_timestamp_usec() + + @property + def system_timestamp_nsec(self): + return self.get_system_timestamp_nsec() def get_buffer(self): if not self._handle: @@ -89,6 +152,15 @@ def get_height_pixels(self): def get_stride_bytes(self): return int(_k4a.k4a_image_get_stride_bytes(self._handle)) + def get_timestamp_usec(self): + return _k4a.k4a_image_get_timestamp_usec(self._handle) + + def get_device_timestamp_usec(self): + return _k4a.k4a_image_get_device_timestamp_usec(self._handle) + + def get_system_timestamp_nsec(self): + return _k4a.k4a_image_get_system_timestamp_nsec(self._handle) + def to_numpy(self): if not self.is_valid(): diff --git a/pykinect_azure/k4a/transformation.py b/pykinect_azure/k4a/transformation.py index afcb5f2..1313b1f 100644 --- a/pykinect_azure/k4a/transformation.py +++ b/pykinect_azure/k4a/transformation.py @@ -95,6 +95,8 @@ def get_custom_bytes_per_pixel(self, custom_image): if custom_image_format == _k4a.K4A_IMAGE_FORMAT_CUSTOM8: return 1 + elif custom_image_format == _k4a.K4A_IMAGE_FORMAT_COLOR_BGRA32: + return 4 else: return 2