diff --git a/exampleColorImage_multicam.py b/exampleColorImage_multicam.py new file mode 100644 index 0000000..b64b052 --- /dev/null +++ b/exampleColorImage_multicam.py @@ -0,0 +1,97 @@ +# -*- coding: utf-8 -*- +""" +Created on Tue Sep 17 13:29:09 2024 + +@author: 28731 +""" + +import pykinect_azure as pykinect +import cv2 + +# Start single camera +def start_camera(device_info): + + device = device_info['device'] + device.start_cameras(device_info['config']) + print( + f"Successfully started camera for device {device_info['index']} ({device_info['type']})") + +# Close all the devices +def close_devices(devices): + + for device_info in devices: + device_info['device'].close() + + +if __name__ == "__main__": + + # Initialize the library, if the library is not found, add the library path as argument + pykinect.initialize_libraries() + + # A list to store the devices + devices = [] + + # The number of your devices + num_devices = pykinect.k4a_device_get_installed_count() + + # Modify camera configuration and start devices + for i in range(num_devices): + device = pykinect.Device(i) + device_config, device_type = device.device_configinit() + devices.append({ + 'device': device, + 'type': device_type, + 'config': device_config, + 'index': i, + 'rgb_image': None}) + + cv2.namedWindow(f'Color Image_{i}',cv2.WINDOW_NORMAL) + + # Start cameras + master_devices = [d for d in devices if d['type'] == 'Master'] + sub_devices = [d for d in devices if d['type'] == 'Sub'] + stan_devices = [d for d in devices if d['type'] == 'Standalone'] + + for device_info in stan_devices: + start_camera(device_info) + for device_info in sub_devices: + start_camera(device_info) + # Finally open the master camera + for device_info in master_devices: + start_camera(device_info) + + if len(master_devices) == 1 and len(sub_devices) == 0: + close_devices() + raise Exception( + "NO Sub device detected but detected Master device, please check the sync cable!") + + elif len(master_devices) > 1: + close_devices() + raise Exception( + "The Master device cannot be more than one, please check the sync cable!") + + elif len(master_devices) == 0 and len(sub_devices) != 0: + close_devices() + raise Exception( + "NO Master device detected but detected Sub device, please check the sync cable!") + + while True: + + for device_info in devices: + device = device_info['device'] + capture = device.update() + ret_color, color_image = capture.get_color_image() + if not ret_color: + continue + + device_info['rgb_image'] = color_image + + for i in range(num_devices): + # Plot the image + cv2.imshow(f"Color Image_{i}",devices[i]['rgb_image']) + + # Press q key to stop + if cv2.waitKey(1) == ord('q'): + break + + close_devices(devices) diff --git a/pykinect_azure/__init__.py b/pykinect_azure/__init__.py index c98318f..09db88f 100644 --- a/pykinect_azure/__init__.py +++ b/pykinect_azure/__init__.py @@ -4,4 +4,5 @@ from .k4arecord import Datablock, Record, Playback from .k4a._k4atypes import * -from .k4abt._k4abtTypes import * \ No newline at end of file +from .k4abt._k4abtTypes import * +from .k4a._k4a import * \ No newline at end of file diff --git a/pykinect_azure/k4a/calibration.py b/pykinect_azure/k4a/calibration.py index e84f77e..a788d9f 100644 --- a/pykinect_azure/k4a/calibration.py +++ b/pykinect_azure/k4a/calibration.py @@ -1,7 +1,7 @@ import ctypes from pykinect_azure.k4a import _k4a - +import numpy as np class Calibration: @@ -37,16 +37,17 @@ def __str__(self): ) return message - def get_matrix(self, camera: _k4a.k4a_calibration_type_t): - if camera == _k4a.K4A_CALIBRATION_TYPE_COLOR: - return [[self.color_params.fx, 0, self.color_params.cx], + def get_matrix(self): + return np.array([[self.color_params.fx, 0, self.color_params.cx], [0, self.color_params.fy, self.color_params.cy], - [0, 0, 1]] - elif camera == _k4a.K4A_CALIBRATION_TYPE_DEPTH: - return [[self.depth_params.fx, 0, self.depth_params.cx], - [0, self.depth_params.fy, self.depth_params.cy], - [0, 0, 1]] - + [0, 0, 1]]) + + def get_distortion_coefficients(self) -> np.ndarray: + """ + Get the distortion coefficients (in OpenCV compatible format) for the color or depth camera + """ + return np.array([self.color_params.k1, self.color_params.k2, self.color_params.p1, self.color_params.p2, self.color_params.k3]) + def is_valid(self): return self._handle @@ -121,3 +122,11 @@ def convert_color_2d_to_depth_2d(self, source_point2d: _k4a.k4a_float2_t, valid), "Failed to convert from Color 2D to Depth 2D") return target_point2d + + def get_extrinsic_parameters(self): + + extrinsic = self._handle.extrinsics[_k4a.K4A_CALIBRATION_TYPE_COLOR][_k4a.K4A_CALIBRATION_TYPE_DEPTH] + rotation = np.array(extrinsic.rotation).reshape((3, 3)) + translation = np.array(extrinsic.translation).reshape((3, 1)) + + return rotation, translation diff --git a/pykinect_azure/k4a/device.py b/pykinect_azure/k4a/device.py index 2d261f2..2a54362 100644 --- a/pykinect_azure/k4a/device.py +++ b/pykinect_azure/k4a/device.py @@ -1,5 +1,5 @@ import ctypes - +from ctypes import byref from pykinect_azure.k4a import _k4a from pykinect_azure.k4a.capture import Capture from pykinect_azure.k4a.imu_sample import ImuSample @@ -25,10 +25,10 @@ def is_valid(self): return self._handle def is_capture_initialized(self): - return Device.capture + return self.capture def is_imu_sample_initialized(self): - return Device.imu_sample + return self.imu_sample def handle(self): return self._handle @@ -58,15 +58,15 @@ def update(self, timeout_in_ms=K4A_WAIT_INFINITE): capture_handle = self.get_capture(timeout_in_ms) if self.is_capture_initialized(): - Device.capture._handle = capture_handle + self.capture._handle = capture_handle else : - Device.capture = Capture(capture_handle, Device.calibration) + self.capture = Capture(capture_handle, self.calibration) # Write capture if recording if self.recording: - self.record.write_capture(Device.capture.handle()) + self.record.write_capture(self.capture.handle()) - return Device.capture + return self.capture def update_imu(self, timeout_in_ms=K4A_WAIT_INFINITE): @@ -74,18 +74,18 @@ def update_imu(self, timeout_in_ms=K4A_WAIT_INFINITE): imu_sample = self.get_imu_sample(timeout_in_ms) if self.is_imu_sample_initialized(): - Device.imu_sample._struct = imu_sample - Device.imu_sample.parse_data() + self.imu_sample._struct = imu_sample + self.imu_sample.parse_data() else : - Device.imu_sample = ImuSample(imu_sample) + self.imu_sample = ImuSample(imu_sample) - return Device.imu_sample + return self.imu_sample def get_capture(self, timeout_in_ms=_k4a.K4A_WAIT_INFINITE): # Release current handle if self.is_capture_initialized(): - Device.capture.release_handle() + self.capture.release_handle() capture_handle = _k4a.k4a_capture_t() _k4a.VERIFY(_k4a.k4a_device_get_capture(self._handle, capture_handle, timeout_in_ms),"Get capture failed!") @@ -101,7 +101,7 @@ def get_imu_sample(self, timeout_in_ms=_k4a.K4A_WAIT_INFINITE): return imu_sample def start_cameras(self, device_config): - Device.calibration = self.get_calibration(device_config.depth_mode, device_config.color_resolution) + self.calibration = self.get_calibration(device_config.depth_mode, device_config.color_resolution) _k4a.VERIFY(_k4a.k4a_device_start_cameras(self._handle, device_config.handle()),"Start K4A cameras failed!") @@ -129,8 +129,8 @@ def get_serialnum(self): return serial_number.value.decode("utf-8") - def get_calibration(self, depth_mode, color_resolution): - + def get_calibration(self, depth_mode, color_resolution): + calibration_handle = _k4a.k4a_calibration_t() _k4a.VERIFY(_k4a.k4a_device_get_calibration(self._handle,depth_mode,color_resolution,calibration_handle),"Get calibration failed!") @@ -145,10 +145,43 @@ def get_version(self): return version + def device_configinit(self): + # Automatically determine master and Sub devices and config + device_config = Configuration() + device_config.color_format = _k4a.K4A_IMAGE_FORMAT_COLOR_BGRA32 + device_config.color_resolution = _k4a.K4A_COLOR_RESOLUTION_1080P + device_config.depth_mode = _k4a.K4A_DEPTH_MODE_NFOV_2X2BINNED + device_config.calibration_type = _k4a.K4A_CALIBRATION_TYPE_COLOR + device_config.synchronized_images_only = True + + sync_in_connected = ctypes.c_bool() + sync_out_connected = ctypes.c_bool() + + if _k4a.K4A_RESULT_SUCCEEDED == _k4a.k4a_device_get_sync_jack(self.handle(), byref(sync_in_connected), byref(sync_out_connected)): + if sync_in_connected and not sync_out_connected: + device_config.wired_sync_mode = _k4a.K4A_WIRED_SYNC_MODE_SUBORDINATE + device_config.subordinate_delay_off_master_usec = 160 + device_type = "Sub" + elif not sync_in_connected and sync_out_connected: + device_config.wired_sync_mode = _k4a.K4A_WIRED_SYNC_MODE_MASTER + device_type = "Master" + elif sync_in_connected and sync_out_connected: + device_config.wired_sync_mode = _k4a.K4A_WIRED_SYNC_MODE_SUBORDINATE + device_config.subordinate_delay_off_master_usec = 160 + device_type = "Sub" + else: + device_config.wired_sync_mode = _k4a.K4A_WIRED_SYNC_MODE_STANDALONE + device_type = "Standalone" + else: + device_config.wired_sync_mode = _k4a.K4A_WIRED_SYNC_MODE_STANDALONE + device_type = "Standalone" + + return device_config, device_type + @staticmethod def open(index=0): device_handle = _k4a.k4a_device_t() - + _k4a.VERIFY(_k4a.k4a_device_open(index, device_handle),"Open K4A Device failed!") return device_handle diff --git a/pykinect_azure/k4abt/tracker.py b/pykinect_azure/k4abt/tracker.py index 7471b65..4cc6274 100644 --- a/pykinect_azure/k4abt/tracker.py +++ b/pykinect_azure/k4abt/tracker.py @@ -31,12 +31,12 @@ def destroy(self): _k4abt.k4abt_tracker_destroy(self._handle) self._handle = None - def update(self, capture=None, timeout_in_ms=K4A_WAIT_INFINITE): + def update(self, device, capture=None, timeout_in_ms=K4A_WAIT_INFINITE): # Add capture to the body tracker processing queue if capture: self.enqueue_capture(capture.handle(), timeout_in_ms) else: - self.enqueue_capture(Device.capture.handle(), timeout_in_ms) + self.enqueue_capture(device.capture.handle(), timeout_in_ms) return self.pop_result(timeout_in_ms) diff --git a/pykinect_azure/pykinect.py b/pykinect_azure/pykinect.py index e45f806..ab3c687 100644 --- a/pykinect_azure/pykinect.py +++ b/pykinect_azure/pykinect.py @@ -50,11 +50,11 @@ def start_device(device_index=0, config=default_configuration, record=False, rec return device -def start_body_tracker(model_type=_k4abt.K4ABT_DEFAULT_MODEL, calibration=None): +def start_body_tracker(device, model_type=_k4abt.K4ABT_DEFAULT_MODEL, calibration=None): if calibration: return Tracker(calibration, model_type) else: - return Tracker(Device.calibration, model_type) + return Tracker(device.calibration, model_type) def start_playback(filepath):