Simpler Multiple cameras

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)

pykinect_azure/__init__.py

@@ -4,4 +4,5 @@
 from .k4arecord import Datablock, Record, Playback
 
 from .k4a._k4atypes import *
-from .k4abt._k4abtTypes import *
+from .k4abt._k4abtTypes import *
+from .k4a._k4a import *

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

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,34 +58,34 @@ 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):
 
 		# Get imu sample
 		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

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)
 

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):