Calibration: Add functions to access intrinsics (#113)

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 #35, #69

Co-authored-by: Johan von Forstner <[email protected]>

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
@@ -120,3 +122,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 = k4a_module.calibration_get_intrinsics(self._calibration_handle, self.thread_safe, camera)
+        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 = k4a_module.calibration_get_intrinsics(self._calibration_handle, self.thread_safe, camera)
+        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/pyk4a.cpp

@@ -1003,6 +1003,49 @@ static PyObject *calibration_2d_to_3d(PyObject *self, PyObject *args) {
                        target_point3d_mm.xyz.z);
 }
 
+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 PyObject *calibration_get_intrinsics(PyObject *self, PyObject *args) {
+  k4a_calibration_t *calibration_handle;
+  PyObject *capsule;
+  int thread_safe;
+  k4a_calibration_type_t camera;
+  PyThreadState *thread_state;
+
+  PyArg_ParseTuple(args, "OpI", &capsule, &thread_safe, &camera);
+  calibration_handle = (k4a_calibration_t *)PyCapsule_GetPointer(capsule, CAPSULE_CALIBRATION_NAME);
+
+  thread_state = _gil_release(thread_safe);
+
+  k4a_calibration_camera_t calib;
+  if (camera == K4A_CALIBRATION_TYPE_DEPTH) {
+    calib = calibration_handle->depth_camera_calibration;
+  } else if (camera == K4A_CALIBRATION_TYPE_COLOR) {
+    calib = calibration_handle->color_camera_calibration;
+  }
+  _gil_restore(thread_state);
+
+  PyObject *intrinsics = _array_to_list(calib.intrinsics.parameters.v, calib.intrinsics.parameter_count);
+  return Py_BuildValue("N", intrinsics);
+}
+
 static PyObject *playback_open(PyObject *self, PyObject *args) {
   int thread_safe;
   PyThreadState *thread_state;
@@ -1368,6 +1411,8 @@ static PyMethodDef Pyk4aMethods[] = {
     {"calibration_3d_to_3d", calibration_3d_to_3d, METH_VARARGS, "Transforms the coordinates between 2 3D systems"},
     {"calibration_2d_to_3d", calibration_2d_to_3d, METH_VARARGS,
      "Transforms the coordinates between a pixel and a 3D system"},
+    {"calibration_get_intrinsics", calibration_get_intrinsics, METH_VARARGS,
+     "Gets intrinsic parameters from calibration"},
     {"playback_open", playback_open, METH_VARARGS, "Open file for playback"},
     {"playback_close", playback_close, METH_VARARGS, "Close opened playback"},
     {"playback_get_recording_length_usec", playback_get_recording_length_usec, METH_VARARGS, "Return recording length"},