Add 3d => 2d conversion function (#114)

pyk4a/calibration.py

@@ -112,6 +112,43 @@ def convert_2d_to_3d(
             target_camera = source_camera
         return self._convert_2d_to_3d(coordinates, depth, source_camera, target_camera)
 
+    def _convert_3d_to_2d(
+        self,
+        source_point_3d: Tuple[float, float, float],
+        source_camera: CalibrationType,
+        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.
+        """
+        res, valid, target_px_2d = k4a_module.calibration_3d_to_2d(
+            self._calibration_handle, self.thread_safe, source_point_3d, source_camera, target_camera,
+        )
+
+        _verify_error(res)
+        if valid == 0:
+            raise ValueError(f"Coordinates {source_point_3d} are not valid in the calibration model")
+
+        return target_px_2d
+
+    def convert_3d_to_2d(
+        self,
+        coordinates: Tuple[float, float, float],
+        source_camera: CalibrationType,
+        target_camera: Optional[CalibrationType] = None,
+    ):
+        """
+            Transform a 3d point to a 2d pixel of the target coordinate system.
+        """
+        if target_camera is None:
+            target_camera = source_camera
+        return self._convert_3d_to_2d(coordinates, source_camera, target_camera)
+
     @property
     def transformation_handle(self) -> object:
         if not self._transformation_handle:

pyk4a/pyk4a.cpp

@@ -981,6 +981,48 @@ extern "C" {
         return Py_BuildValue("II(fff)", res, valid, target_point3d_mm.xyz.x, target_point3d_mm.xyz.y, target_point3d_mm.xyz.z);
     }
 
+    static PyObject* calibration_3d_to_2d(PyObject* self, PyObject *args){
+        k4a_calibration_t* calibration_handle;
+        PyObject *capsule;
+        int thread_safe;
+        PyThreadState *thread_state;
+        float source_point_x;
+        float source_point_y;
+        float source_point_z;
+        int valid;
+        k4a_calibration_type_t source_camera;
+        k4a_calibration_type_t target_camera;
+        k4a_result_t res;
+        k4a_float2_t target_point2d;
+
+        PyArg_ParseTuple(args, "Op(fff)II",
+                &capsule,
+                &thread_safe,
+                &source_point_x,
+                &source_point_y,
+                &source_point_z,
+                &source_camera,
+                &target_camera);
+        calibration_handle = (k4a_calibration_t*)PyCapsule_GetPointer(capsule, CAPSULE_CALIBRATION_NAME);
+
+        thread_state = _gil_release(thread_safe);
+
+        const k4a_float3_t source_point3d_mm = {source_point_x, source_point_y, source_point_z};
+
+
+        res = k4a_calibration_3d_to_2d (calibration_handle,
+                                        &source_point3d_mm,
+                                        source_camera,
+                                        target_camera,
+                                        &target_point2d,
+                                        &valid);
+        _gil_restore(thread_state);
+        if (res == K4A_RESULT_FAILED ) {
+            return Py_BuildValue("IIN", res, valid, Py_None);
+        }
+        // Return object...
+        return Py_BuildValue("II(ff)", res, valid, target_point2d.xy.x, target_point2d.xy.y);
+    }
 
     static PyObject* playback_open(PyObject* self, PyObject *args) {
         int thread_safe;
@@ -1247,6 +1289,7 @@ extern "C" {
         {"transformation_depth_image_to_point_cloud", transformation_depth_image_to_point_cloud, METH_VARARGS, "Transforms the depth map to a point cloud."},
         {"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_3d_to_2d", calibration_3d_to_2d, METH_VARARGS, "Transform a 3D point of a source coordinate system into a 2D pixel coordinate of the target camera"},
         {"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"},

tests/unit/test_calibration.py

@@ -45,3 +45,15 @@ def test_2d_to_3d_with_target_camera(self, calibration: Calibration):
         point3d = -122.29087829589844, -45.17741394042969, 243.19528198242188
         converted = calibration.convert_2d_to_3d(point, depth, CalibrationType.COLOR, CalibrationType.DEPTH)
         assert np.allclose(converted, point3d)
+
+    def test_3d_to_2d_without_target_camera(self, calibration: Calibration):
+        point3d = -154.5365753173828, -26.12171173095703, 250.0
+        point = 250.0, 300.0
+        converted = calibration.convert_3d_to_2d(point3d, CalibrationType.COLOR)
+        assert np.allclose(converted, point)
+
+    def test_3d_to_2d_with_target_camera(self, calibration: Calibration):
+        point3d = -122.29087829589844, -45.17741394042969, 243.19528198242188
+        point = 250.0, 300.0
+        converted = calibration.convert_3d_to_2d(point3d, CalibrationType.DEPTH, CalibrationType.COLOR)
+        assert np.allclose(converted, point)