Version 1.4.0 (#170)

* Feature/playback get imu sample (#150) * feat(playback-imu-sample): add "get_next_imu_sample" to playback * fix(playback-imu-sample): PR changes, add correct indentation, add import in playback.py * add info related to conda and DLL not found error * update black version (fix click bug) (#168) * update black version (fix click bug) * black reformat * fix typo Co-authored-by: Louis-Philippe Asselin <[email protected]> Co-authored-by: annStein <[email protected]>
etiennedub · May 27, 2022 · 2791f6e · 2791f6e
1 parent 905d77b
commit 2791f6e
Show file tree

Hide file tree

Showing 15 changed files with 164 additions and 56 deletions.
diff --git a/README.md b/README.md
@@ -34,6 +34,8 @@ pip install pyk4a
 
 In most cases `pip install pyk4a` is enough to install this package.
 
+When using an anaconda environment, you need to set the environment variable `CONDA_DLL_SEARCH_MODIFICATION_ENABLE=1` https://github.com/conda/conda/issues/10897
+
 Because of the numerous issues received from Windows users, the installer ([setup.py](setup.py)) automatically detects the kinect SDK path.
 
 When the installer is not able to find the path, the following snippet can help.

diff --git a/example/threads.py b/example/threads.py
@@ -93,10 +93,16 @@ def draw(results: Dict[int, Dict[bool, int]]):
     plt.ylabel("Operations Count")
     plt.xlabel("CPU Workers count")
     plt.plot(
-        results.keys(), [result[True] for result in results.values()], "r", label="Thread safe",
+        results.keys(),
+        [result[True] for result in results.values()],
+        "r",
+        label="Thread safe",
     )
     plt.plot(
-        results.keys(), [result[False] for result in results.values()], "g", label="Non thread safe",
+        results.keys(),
+        [result[False] for result in results.values()],
+        "g",
+        label="Non thread safe",
     )
     plt.legend()
 
@@ -105,7 +111,8 @@ def draw(results: Dict[int, Dict[bool, int]]):
     plt.ylabel("Difference, %")
     plt.xlabel("CPU Workers count")
     plt.plot(
-        results.keys(), [float(result[False] - result[True]) / result[True] * 100 for result in results.values()],
+        results.keys(),
+        [float(result[False] - result[True]) / result[True] * 100 for result in results.values()],
     )
     xmin, xmax, ymin, ymax = plt.axis()
     if ymin > 0:

diff --git a/example/viewer_point_cloud.py b/example/viewer_point_cloud.py
@@ -36,7 +36,11 @@ def main():
     fig = plt.figure()
     ax = fig.add_subplot(111, projection="3d")
     ax.scatter(
-        points[:, 0], points[:, 1], points[:, 2], s=1, c=colors / 255,
+        points[:, 0],
+        points[:, 1],
+        points[:, 2],
+        s=1,
+        c=colors / 255,
     )
     ax.set_xlabel("x")
     ax.set_ylabel("y")

diff --git a/example/viewer_transformation.py b/example/viewer_transformation.py
@@ -6,7 +6,12 @@
 
 
 def main():
-    k4a = PyK4A(Config(color_resolution=pyk4a.ColorResolution.RES_720P, depth_mode=pyk4a.DepthMode.NFOV_UNBINNED,))
+    k4a = PyK4A(
+        Config(
+            color_resolution=pyk4a.ColorResolution.RES_720P,
+            depth_mode=pyk4a.DepthMode.NFOV_UNBINNED,
+        )
+    )
     k4a.start()
 
     while True:

diff --git a/pyk4a/calibration.py b/pyk4a/calibration.py
@@ -53,15 +53,19 @@ def _convert_3d_to_3d(
         target_camera: CalibrationType,
     ) -> Tuple[float, float, float]:
         """
-            Transform a 3d point of a source coordinate system into a 3d
-            point of the target coordinate system.
-            :param source_point_3d  The 3D coordinates in millimeters representing a point in source_camera.
-            :param source_camera    The current camera.
-            :param target_camera    The target camera.
-            :return                 The 3D coordinates in millimeters representing a point in target camera.
+        Transform a 3d point of a source coordinate system into a 3d
+        point of the target coordinate system.
+        :param source_point_3d  The 3D coordinates in millimeters representing a point in source_camera.
+        :param source_camera    The current camera.
+        :param target_camera    The target camera.
+        :return                 The 3D coordinates in millimeters representing a point in target camera.
         """
         res, target_point_3d = k4a_module.calibration_3d_to_3d(
-            self._calibration_handle, self.thread_safe, source_point_3d, source_camera, target_camera,
+            self._calibration_handle,
+            self.thread_safe,
+            source_point_3d,
+            source_camera,
+            target_camera,
         )
 
         _verify_error(res)
@@ -81,16 +85,21 @@ def _convert_2d_to_3d(
         target_camera: CalibrationType,
     ) -> Tuple[float, float, float]:
         """
-            Transform a 3d point of a source coordinate system into a 3d
-            point of the target coordinate system.
-            :param source_pixel_2d    The 2D coordinates in px of source_camera color_image.
-            :param source_depth       Depth in mm
-            :param source_camera      The current camera.
-            :param target_camera      The target camera.
-            :return                   The 3D coordinates in mm representing a point in target camera.
+        Transform a 3d point of a source coordinate system into a 3d
+        point of the target coordinate system.
+        :param source_pixel_2d    The 2D coordinates in px of source_camera color_image.
+        :param source_depth       Depth in mm
+        :param source_camera      The current camera.
+        :param target_camera      The target camera.
+        :return                   The 3D coordinates in mm representing a point in target camera.
         """
         res, valid, target_point_3d = k4a_module.calibration_2d_to_3d(
-            self._calibration_handle, self.thread_safe, source_pixel_2d, source_depth, source_camera, target_camera,
+            self._calibration_handle,
+            self.thread_safe,
+            source_pixel_2d,
+            source_depth,
+            source_camera,
+            target_camera,
         )
 
         _verify_error(res)
@@ -107,7 +116,7 @@ def convert_2d_to_3d(
         target_camera: Optional[CalibrationType] = None,
     ):
         """
-            Transform a 2d pixel to a 3d point of the target coordinate system.
+        Transform a 2d pixel to a 3d point of the target coordinate system.
         """
         if target_camera is None:
             target_camera = source_camera
@@ -120,15 +129,19 @@ def _convert_3d_to_2d(
         target_camera: CalibrationType,
     ) -> Tuple[float, float]:
         """
-            Transform a 3d point of a source coordinate system into a 3d
-            point of the target coordinate system.
-            :param source_point_3d    The 3D coordinates in mm of source_camera.
-            :param source_camera      The current camera.
-            :param target_camera      The target camera.
-            :return                   The 3D coordinates in mm representing a point in target camera.
+        Transform a 3d point of a source coordinate system into a 3d
+        point of the target coordinate system.
+        :param source_point_3d    The 3D coordinates in mm of source_camera.
+        :param source_camera      The current camera.
+        :param target_camera      The target camera.
+        :return                   The 3D coordinates in mm representing a point in target camera.
         """
         res, valid, target_px_2d = k4a_module.calibration_3d_to_2d(
-            self._calibration_handle, self.thread_safe, source_point_3d, source_camera, target_camera,
+            self._calibration_handle,
+            self.thread_safe,
+            source_point_3d,
+            source_camera,
+            target_camera,
         )
 
         _verify_error(res)
@@ -144,7 +157,7 @@ def convert_3d_to_2d(
         target_camera: Optional[CalibrationType] = None,
     ):
         """
-            Transform a 3d point to a 2d pixel of the target coordinate system.
+        Transform a 3d point to a 2d pixel of the target coordinate system.
         """
         if target_camera is None:
             target_camera = source_camera

diff --git a/pyk4a/capture.py b/pyk4a/capture.py
@@ -138,15 +138,21 @@ def transformed_depth(self) -> Optional[np.ndarray]:
     def depth_point_cloud(self) -> Optional[np.ndarray]:
         if self._depth_point_cloud is None and self.depth is not None:
             self._depth_point_cloud = depth_image_to_point_cloud(
-                self._depth, self._calibration, self.thread_safe, calibration_type_depth=True,
+                self._depth,
+                self._calibration,
+                self.thread_safe,
+                calibration_type_depth=True,
             )
         return self._depth_point_cloud
 
     @property
     def transformed_depth_point_cloud(self) -> Optional[np.ndarray]:
         if self._transformed_depth_point_cloud is None and self.transformed_depth is not None:
             self._transformed_depth_point_cloud = depth_image_to_point_cloud(
-                self.transformed_depth, self._calibration, self.thread_safe, calibration_type_depth=False,
+                self.transformed_depth,
+                self._calibration,
+                self.thread_safe,
+                calibration_type_depth=False,
             )
         return self._transformed_depth_point_cloud
 

diff --git a/pyk4a/playback.py b/pyk4a/playback.py
@@ -14,6 +14,7 @@
 from .config import FPS, ColorResolution, DepthMode, ImageFormat, WiredSyncMode
 from .errors import K4AException, _verify_error
 from .module import k4a_module
+from .pyk4a import ImuSample
 from .results import Result, StreamResult
 
 
@@ -62,7 +63,7 @@ def __exit__(self):
     @property
     def path(self) -> Path:
         """
-            Record file path
+        Record file path
         """
         return self._path
 
@@ -93,7 +94,7 @@ def configuration(self) -> Configuration:
     @property
     def length(self) -> int:
         """
-            Record length in usec
+        Record length in usec
         """
         if self._length is None:
             self._validate_is_open()
@@ -130,7 +131,7 @@ def calibration(self) -> Calibration:
 
     def open(self) -> None:
         """
-            Open record file
+        Open record file
         """
         if self._handle:
             raise K4AException("Playback already opened")
@@ -142,15 +143,15 @@ def open(self) -> None:
 
     def close(self):
         """
-            Close record file
+        Close record file
         """
         self._validate_is_open()
         k4a_module.playback_close(self._handle, self.thread_safe)
         self._handle = None
 
     def seek(self, offset: int, origin: SeekOrigin = SeekOrigin.BEGIN) -> None:
         """
-            Seek playback pointer to specified offset
+        Seek playback pointer to specified offset
         """
         self._validate_is_open()
         result = k4a_module.playback_seek_timestamp(self._handle, self.thread_safe, offset, int(origin))
@@ -178,6 +179,12 @@ def get_previouse_capture(self):
             thread_safe=self.thread_safe,
         )
 
+    def get_next_imu_sample(self) -> Optional["ImuSample"]:
+        self._validate_is_open()
+        result, imu_sample = k4a_module.playback_get_next_imu_sample(self._handle, self.thread_safe)
+        self._verify_stream_error(result)
+        return imu_sample
+
     def _validate_is_open(self):
         if not self._handle:
             raise K4AException("Playback not opened.")

diff --git a/pyk4a/pyk4a.cpp b/pyk4a/pyk4a.cpp
@@ -1312,6 +1312,31 @@ static PyObject *playback_get_previous_capture(PyObject *self, PyObject *args) {
   return Py_BuildValue("IN", result, capsule_capture);
 }
 
+static PyObject *playback_get_next_imu_sample(PyObject *self, PyObject *args) {
+  int thread_safe;
+  PyThreadState *thread_state;
+  PyObject *capsule;
+  k4a_playback_t *playback_handle;
+  k4a_imu_sample_t imu_sample;
+  k4a_stream_result_t result;
+
+  PyArg_ParseTuple(args, "Op", &capsule, &thread_safe);
+  playback_handle = (k4a_playback_t *)PyCapsule_GetPointer(capsule, CAPSULE_PLAYBACK_NAME);
+
+  thread_state = _gil_release(thread_safe);
+  result = k4a_playback_get_next_imu_sample(*playback_handle, &imu_sample);
+  _gil_restore(thread_state);
+
+  if (result != K4A_STREAM_RESULT_SUCCEEDED) {
+    return Py_BuildValue("IN", result, Py_None);
+  }
+  return Py_BuildValue("I{s:f,s:(fff),s:L,s:(fff),s:L}", result, "temperature", imu_sample.temperature, "acc_sample",
+                       imu_sample.acc_sample.xyz.x, imu_sample.acc_sample.xyz.y, imu_sample.acc_sample.xyz.z,
+                       "acc_timestamp", imu_sample.acc_timestamp_usec, "gyro_sample", imu_sample.gyro_sample.xyz.x,
+                       imu_sample.gyro_sample.xyz.y, imu_sample.gyro_sample.xyz.z, "gyro_timestamp",
+                       imu_sample.gyro_timestamp_usec);
+}
+
 static PyObject *record_create(PyObject *self, PyObject *args) {
   k4a_device_t *device_handle = NULL;
   PyObject *device_capsule;
@@ -1471,6 +1496,7 @@ static PyMethodDef Pyk4aMethods[] = {
     {"playback_get_next_capture", playback_get_next_capture, METH_VARARGS, "Get next capture from playback"},
     {"playback_get_previous_capture", playback_get_previous_capture, METH_VARARGS,
      "Get previous capture from playback"},
+    {"playback_get_next_imu_sample", playback_get_next_imu_sample, METH_VARARGS, "Get next imu sample from playback"},
     {"color_image_get_exposure_usec", color_image_get_exposure_usec, METH_VARARGS,
      "Get color image exposure in microseconds"},
     {"color_image_get_white_balance", color_image_get_white_balance, METH_VARARGS, "Get color image white balance"},

diff --git a/pyk4a/pyk4a.py b/pyk4a/pyk4a.py
@@ -105,7 +105,10 @@ def _stop_imu(self):
         res = k4a_module.device_stop_imu(self._device_handle, self.thread_safe)
         _verify_error(res)
 
-    def get_capture(self, timeout=TIMEOUT_WAIT_INFINITE,) -> "PyK4ACapture":
+    def get_capture(
+        self,
+        timeout=TIMEOUT_WAIT_INFINITE,
+    ) -> "PyK4ACapture":
         """
         Fetch a capture from the device and return a PyK4ACapture object. Images are
         lazily fetched.

diff --git a/pyk4a/record.py b/pyk4a/record.py
@@ -26,7 +26,7 @@ def __del__(self):
             self.close()
 
     def create(self) -> None:
-        """ Create record file """
+        """Create record file"""
         if self.created:
             raise K4AException(f"Record already created {self._path}")
         device_handle = self._device._device_handle if self._device else None
@@ -38,13 +38,13 @@ def create(self) -> None:
         self._handle = handle
 
     def close(self):
-        """ Close record """
+        """Close record"""
         self._validate_is_created()
         k4a_module.record_close(self._handle, self.thread_safe)
         self._handle = None
 
     def write_header(self):
-        """ Write MKV header """
+        """Write MKV header"""
         self._validate_is_created()
         if self.header_written:
             raise K4AException(f"Header already written {self._path}")
@@ -54,7 +54,7 @@ def write_header(self):
         self._header_written = True
 
     def write_capture(self, capture: PyK4ACapture):
-        """ Write capture to file (send to queue) """
+        """Write capture to file (send to queue)"""
         self._validate_is_created()
         if not self.header_written:
             self.write_header()
@@ -64,7 +64,7 @@ def write_capture(self, capture: PyK4ACapture):
         self._captures_count += 1
 
     def flush(self):
-        """ Flush queue"""
+        """Flush queue"""
         self._validate_is_created()
         result: Result = k4a_module.record_flush(self._handle, self.thread_safe)
         if result != Result.Success:

diff --git a/pyk4a/transformation.py b/pyk4a/transformation.py
@@ -12,19 +12,31 @@ def depth_image_to_color_camera(depth: np.ndarray, calibration: Calibration, thr
     Return empty result if transformation failed
     """
     return k4a_module.transformation_depth_image_to_color_camera(
-        calibration.transformation_handle, thread_safe, depth, calibration.color_resolution,
+        calibration.transformation_handle,
+        thread_safe,
+        depth,
+        calibration.color_resolution,
     )
 
 
 def depth_image_to_color_camera_custom(
-    depth: np.ndarray, custom: np.ndarray, calibration: Calibration, thread_safe: bool, interp_nearest: bool = True,
+    depth: np.ndarray,
+    custom: np.ndarray,
+    calibration: Calibration,
+    thread_safe: bool,
+    interp_nearest: bool = True,
 ) -> Optional[np.ndarray]:
     """
     Transforms depth image and custom image to color_image space
     Return empty result if transformation failed
     """
     return k4a_module.transformation_depth_image_to_color_camera_custom(
-        calibration.transformation_handle, thread_safe, depth, custom, calibration.color_resolution, interp_nearest,
+        calibration.transformation_handle,
+        thread_safe,
+        depth,
+        custom,
+        calibration.color_resolution,
+        interp_nearest,
     )
 
 
@@ -36,7 +48,10 @@ def depth_image_to_point_cloud(
     Return empty result if transformation failed
     """
     return k4a_module.transformation_depth_image_to_point_cloud(
-        calibration.transformation_handle, thread_safe, depth, calibration_type_depth,
+        calibration.transformation_handle,
+        thread_safe,
+        depth,
+        calibration_type_depth,
     )