Calibration: Add functions to access intrinsics

Camera matrix and distortion coefficients in OpenCV-compatible format.
These values are already specific to the selected camera resolution (in contrast to those accessed through calibration_raw).

related to etiennedub#35, etiennedub#69

pyk4a/calibration.py

@@ -1,6 +1,8 @@
 from enum import IntEnum
 from typing import Optional, Tuple
 
+import numpy as np
+
 import k4a_module
 
 from .config import ColorResolution, DepthMode
@@ -18,23 +20,38 @@ class CalibrationType(IntEnum):
 
 class Calibration:
     def __init__(
-        self, handle: object, depth_mode: DepthMode, color_resolution: ColorResolution, thread_safe: bool = True
+        self,
+        handle: object,
+        depth_mode: DepthMode,
+        color_resolution: ColorResolution,
+        color_intrinsics,
+        depth_intrinsics,
+        thread_safe: bool = True,
     ):
         self._calibration_handle = handle
         self._transformation_handle: Optional[object] = None
         self.thread_safe = thread_safe
         self._depth_mode = depth_mode
         self._color_resolution = color_resolution
         self._raw: Optional[str] = None
+        self._color_intrinsics = color_intrinsics
+        self._depth_intrinsics = depth_intrinsics
 
     @classmethod
     def from_raw(
         cls, value: str, depth_mode: DepthMode, color_resolution: ColorResolution, thread_safe: bool = True
     ) -> "Calibration":
-        res, handle = k4a_module.calibration_get_from_raw(thread_safe, value, depth_mode, color_resolution)
+        res, handle, color_intrinsics, depth_intrinsics = k4a_module.calibration_get_from_raw(
+            thread_safe, value, depth_mode, color_resolution
+        )
         _verify_error(res)
         return Calibration(
-            handle=handle, depth_mode=depth_mode, color_resolution=color_resolution, thread_safe=thread_safe
+            handle=handle,
+            depth_mode=depth_mode,
+            color_resolution=color_resolution,
+            thread_safe=thread_safe,
+            color_intrinsics=color_intrinsics,
+            depth_intrinsics=depth_intrinsics,
         )
 
     @property
@@ -120,3 +137,28 @@ def transformation_handle(self) -> object:
                 raise K4AException("Cannot create transformation handle")
             self._transformation_handle = handle
         return self._transformation_handle
+
+    def get_camera_matrix(self, camera: CalibrationType) -> np.ndarray:
+        """
+        Get the camera matrix (in OpenCV compatible format) for the color or depth camera
+        """
+        if camera not in [CalibrationType.COLOR, CalibrationType.DEPTH]:
+            raise ValueError("Camera matrix only available for color and depth cameras.")
+        params = self._color_intrinsics if camera == CalibrationType.COLOR else self._depth_intrinsics
+        if len(params) != 14:
+            raise ValueError("Unknown camera calibration type")
+
+        cx, cy, fx, fy = params[:4]
+        return np.array([[fx, 0, cx], [0, fy, cy], [0, 0, 1]])
+
+    def get_distortion_coefficients(self, camera: CalibrationType) -> np.ndarray:
+        """
+        Get the distortion coefficients (in OpenCV compatible format) for the color or depth camera
+        """
+        if camera not in [CalibrationType.COLOR, CalibrationType.DEPTH]:
+            raise ValueError("Distortion coefficients only available for color and depth cameras.")
+        params = self._color_intrinsics if camera == CalibrationType.COLOR else self._depth_intrinsics
+        if len(params) != 14:
+            raise ValueError("Unknown camera calibration type")
+
+        return np.array([params[4], params[5], params[13], params[12], *params[6:10]])

pyk4a/playback.py

@@ -112,13 +112,18 @@ def calibration_raw(self, value: str):
     def calibration(self) -> Calibration:
         self._validate_is_open()
         if self._calibration is None:
-            res, handle = k4a_module.playback_get_calibration(self._handle, self.thread_safe)
+            print(k4a_module.playback_get_calibration(self._handle, self.thread_safe))
+            res, handle, color_intrinsics, depth_intrinsics = k4a_module.playback_get_calibration(
+                self._handle, self.thread_safe
+            )
             _verify_error(res)
             self._calibration = Calibration(
                 handle=handle,
                 depth_mode=self.configuration["depth_mode"],
                 color_resolution=self.configuration["color_resolution"],
                 thread_safe=self.thread_safe,
+                color_intrinsics=color_intrinsics,
+                depth_intrinsics=depth_intrinsics,
             )
         return self._calibration
 

pyk4a/pyk4a.cpp

@@ -36,6 +36,25 @@ extern "C" {
             PyEval_RestoreThread(thread_state);
         }
     }
+
+    static PyObject* _array_to_list(float *array, size_t length) {
+        size_t i;
+        PyObject *result = NULL, *value = NULL;
+
+        result = PyList_New(length);
+        if (result) {
+            for (i = 0; i < length; ++i) {
+                value = PyFloat_FromDouble(array[i]);
+                if (value) {
+                    PyList_SET_ITEM(result, i, value);
+                } else {
+                    Py_CLEAR(result);
+                    break;
+                }
+            }
+        }
+        return result;
+    }
 
     static void capsule_cleanup_device(PyObject *capsule) {
         k4a_device_t* device_handle;
@@ -354,7 +373,7 @@ extern "C" {
         calibration_handle = (k4a_calibration_t*) malloc(sizeof(k4a_calibration_t));
         if (calibration_handle == NULL) {
             fprintf(stderr, "Cannot allocate memory");
-            return Py_BuildValue("IN", K4A_RESULT_FAILED, Py_None);
+            return Py_BuildValue("INNN", K4A_RESULT_FAILED, Py_None, Py_None, Py_None);
         }
 
         thread_state = _gil_release(thread_safe);
@@ -363,12 +382,14 @@ extern "C" {
                 color_resolution, calibration_handle);
         if (result == K4A_RESULT_FAILED) {
             _gil_restore(thread_state);
-            return Py_BuildValue("IN", result, Py_None);
+            return Py_BuildValue("INNN", K4A_RESULT_FAILED, Py_None, Py_None, Py_None);
         }
         _gil_restore(thread_state);
         PyObject *calibration_capsule = PyCapsule_New(calibration_handle, CAPSULE_CALIBRATION_NAME, capsule_cleanup_calibration);
+        PyObject *color_intrinsics = _array_to_list(calibration_handle->color_camera_calibration.intrinsics.parameters.v, calibration_handle->color_camera_calibration.intrinsics.parameter_count);
+        PyObject *depth_intrinsics = _array_to_list(calibration_handle->depth_camera_calibration.intrinsics.parameters.v, calibration_handle->depth_camera_calibration.intrinsics.parameter_count);
 
-        return Py_BuildValue("IN", K4A_RESULT_SUCCEEDED, calibration_capsule);
+        return Py_BuildValue("INNN", result, calibration_capsule, color_intrinsics, depth_intrinsics);
     }
 
     static PyObject* device_get_calibration(PyObject* self, PyObject* args){
@@ -386,7 +407,7 @@ extern "C" {
         k4a_calibration_t* calibration_handle = (k4a_calibration_t*) malloc(sizeof(k4a_calibration_t));
         if (calibration_handle == NULL) {
             fprintf(stderr, "Cannot allocate memory");
-            return Py_BuildValue("IN", K4A_RESULT_FAILED, Py_None);
+            return Py_BuildValue("INNN", K4A_RESULT_FAILED, Py_None, Py_None, Py_None);
         }
 
         thread_state = _gil_release(thread_safe);
@@ -395,13 +416,14 @@ extern "C" {
 
         if (result != K4A_RESULT_SUCCEEDED) {
             free(calibration_handle);
-            return Py_BuildValue("IN", K4A_RESULT_FAILED, Py_None);
+            return Py_BuildValue("INNN", K4A_RESULT_FAILED, Py_None, Py_None, Py_None);
         }
 
         PyObject *calibration_capsule = PyCapsule_New(calibration_handle, CAPSULE_CALIBRATION_NAME, capsule_cleanup_calibration);
+        PyObject *color_intrinsics = _array_to_list(calibration_handle->color_camera_calibration.intrinsics.parameters.v, calibration_handle->color_camera_calibration.intrinsics.parameter_count);
+        PyObject *depth_intrinsics = _array_to_list(calibration_handle->depth_camera_calibration.intrinsics.parameters.v, calibration_handle->depth_camera_calibration.intrinsics.parameter_count);
 
-        return Py_BuildValue("IN", result, calibration_capsule);
-
+        return Py_BuildValue("INNN", result, calibration_capsule, color_intrinsics, depth_intrinsics);
     }
 
     static PyObject* device_get_raw_calibration(PyObject* self, PyObject* args){
@@ -1106,7 +1128,7 @@ extern "C" {
         calibration_handle = (k4a_calibration_t*) malloc(sizeof(k4a_calibration_t));
         if (calibration_handle == NULL) {
             fprintf(stderr, "Cannot allocate memory");
-            return Py_BuildValue("IN", K4A_RESULT_FAILED, Py_None);
+            return Py_BuildValue("INNN", K4A_RESULT_FAILED, Py_None, Py_None, Py_None);
         }
 
         thread_state = _gil_release(thread_safe);
@@ -1115,12 +1137,14 @@ extern "C" {
 
         if (result == K4A_RESULT_FAILED ) {
             free(calibration_handle);
-            return Py_BuildValue("IN", result, Py_None);
+            return Py_BuildValue("INNN", result, Py_None, Py_None, Py_None);
         }
 
         PyObject *calibration_capsule = PyCapsule_New(calibration_handle, CAPSULE_CALIBRATION_NAME, capsule_cleanup_calibration);
+        PyObject *color_intrinsics = _array_to_list(calibration_handle->color_camera_calibration.intrinsics.parameters.v, calibration_handle->color_camera_calibration.intrinsics.parameter_count);
+        PyObject *depth_intrinsics = _array_to_list(calibration_handle->depth_camera_calibration.intrinsics.parameters.v, calibration_handle->depth_camera_calibration.intrinsics.parameter_count);
 
-        return Py_BuildValue("IN", result, calibration_capsule);
+        return Py_BuildValue("INNN", result, calibration_capsule, color_intrinsics, depth_intrinsics);
     }
 
     static PyObject* playback_get_record_configuration(PyObject* self, PyObject *args) {

pyk4a/pyk4a.py

@@ -278,7 +278,7 @@ def reset_color_control_to_default(self):
     def calibration(self) -> Calibration:
         self._validate_is_opened()
         if not self._calibration:
-            res, calibration_handle = k4a_module.device_get_calibration(
+            res, calibration_handle, color_intrinsics, depth_intrinsics = k4a_module.device_get_calibration(
                 self._device_handle, self.thread_safe, self._config.depth_mode, self._config.color_resolution
             )
             _verify_error(res)
@@ -287,6 +287,8 @@ def calibration(self) -> Calibration:
                 depth_mode=self._config.depth_mode,
                 color_resolution=self._config.color_resolution,
                 thread_safe=self.thread_safe,
+                color_intrinsics=color_intrinsics,
+                depth_intrinsics=depth_intrinsics,
             )
         return self._calibration
 

tests/plugins/device.py

@@ -237,10 +237,12 @@ def device_get_imu_sample(
                 (36.6, (0.1, 9.8, 0.005), int(time.time() * 1e6), (0.1, 0.2, 0.3), int(time.time() * 1e6)),
             )
 
-        def device_get_calibration(self, depth_mode: int, color_resolution: int) -> Tuple[int, Optional[object]]:
+        def device_get_calibration(
+            self, depth_mode: int, color_resolution: int
+        ) -> Tuple[int, Optional[object], list, list]:
             assert self._opened is True
             calibration = Calibration.from_raw(calibration_raw, DepthMode.NFOV_UNBINNED, ColorResolution.RES_720P)
-            return Result.Success.value, calibration._calibration_handle
+            return Result.Success.value, calibration._calibration_handle, [], []
 
         def device_get_raw_calibration(self) -> Optional[str]:
             assert self._opened is True

tests/plugins/playback.py

@@ -85,9 +85,9 @@ def playback_seek_timestamp(self, offset: int, origin: int) -> int:
             self._position = position
             return StreamResult.Success.value
 
-        def playback_get_calibration(self) -> Tuple[int, object]:
+        def playback_get_calibration(self) -> Tuple[int, object, list, list]:
             assert self._opened
-            return StreamResult.Success.value, CalibrationHandle()
+            return StreamResult.Success.value, CalibrationHandle(), [], []
 
         def playback_get_record_configuration(self) -> Tuple[int, Tuple[int, ...]]:
             assert self._opened