d/ht: support DepthAI cameras and Mercury hand tracking

src/xrt/drivers/CMakeLists.txt

@@ -228,9 +228,16 @@ if(XRT_BUILD_DRIVER_HANDTRACKING)
 			aux_gstreamer
 			ONNXRuntime::ONNXRuntime
 			${OpenCV_LIBRARIES}
+			t_ht_mercury
 			t_ht_old_rgb
 			hand_async
 		)
+
+		if(XRT_BUILD_DRIVER_DEPTHAI)
+			target_sources(drv_ht PRIVATE ht/ht_prober.c)
+			target_link_libraries(drv_ht PRIVATE drv_depthai)
+		endif()
+
 	target_include_directories(drv_ht PRIVATE ${OpenCV_INCLUDE_DIRS} ${EIGEN3_INCLUDE_DIR})
 	list(APPEND ENABLED_DRIVERS ht)
 

src/xrt/drivers/ht/ht_driver.c

@@ -8,11 +8,8 @@
  * @ingroup drv_ht
  */
 
-#include "gstreamer/gst_pipeline.h"
-#include "gstreamer/gst_sink.h"
 #include "ht_interface.h"
 
-#include "../depthai/depthai_interface.h"
 
 #include "util/u_var.h"
 #include "xrt/xrt_defines.h"
@@ -20,35 +17,29 @@
 #include "xrt/xrt_frameserver.h"
 #include "xrt/xrt_prober.h"
 
-#include "os/os_time.h"
-#include "os/os_threading.h"
-
-#include "math/m_api.h"
-
 #include "util/u_device.h"
-#include "util/u_frame.h"
-#include "util/u_sink.h"
-#include "util/u_format.h"
 #include "util/u_logging.h"
-#include "util/u_time.h"
 #include "util/u_trace_marker.h"
-#include "util/u_time.h"
-#include "util/u_json.h"
 #include "util/u_config_json.h"
 #include "util/u_debug.h"
+#include "util/u_sink.h"
 
-
-// #include "tracking/t_frame_cv_mat_wrapper.hpp"
-// #include "tracking/t_calibration_opencv.hpp"
 #include "tracking/t_hand_tracking.h"
 
 // Save me, Obi-Wan!
+
 #include "../../tracking/hand/old_rgb/rgb_interface.h"
+#include "../../tracking/hand/mercury/hg_interface.h"
+
+#ifdef XRT_BUILD_DRIVER_DEPTHAI
+#include "../depthai/depthai_interface.h"
+#endif
 
 
 #include <cjson/cJSON.h>
 
 DEBUG_GET_ONCE_LOG_OPTION(ht_log, "HT_LOG", U_LOGGING_WARN)
+DEBUG_GET_ONCE_BOOL_OPTION(ht_use_old_rgb, "HT_USE_OLD_RGB", false)
 
 
 #define HT_TRACE(htd, ...) U_LOG_XDEV_IFL_T(&htd->base, htd->log_level, __VA_ARGS__)
@@ -63,18 +54,8 @@ struct ht_device
 {
 	struct xrt_device base;
 
-	struct xrt_tracking_origin tracking_origin; // probably cargo-culted
-
-	enum xrt_format desired_format;
-
 	struct xrt_frame_context xfctx;
 
-	struct xrt_fs *xfs;
-
-	struct xrt_fs_mode mode;
-
-	struct xrt_prober *prober;
-
 	struct t_hand_tracking_sync *sync;
 	struct t_hand_tracking_async *async;
 
@@ -157,27 +138,6 @@ userConfigSetDefaults(struct ht_device *htd)
 }
 #endif
 
-static void
-on_video_device(struct xrt_prober *xp,
-                struct xrt_prober_device *pdev,
-                const char *product,
-                const char *manufacturer,
-                const char *serial,
-                void *ptr)
-{
-	// Stolen from gui_scene_record
-
-	struct ht_device *htd = (struct ht_device *)ptr;
-
-	// Hardcoded for the Index.
-	if (product != NULL && manufacturer != NULL) {
-		if ((strcmp(product, "3D Camera") == 0) && (strcmp(manufacturer, "Etron Technology, Inc.") == 0)) {
-			xrt_prober_open_video_device(xp, pdev, &htd->xfctx, &htd->xfs);
-			return;
-		}
-	}
-}
-
 /*!
  * xrt_device function implementations
  */
@@ -219,13 +179,14 @@ ht_device_destroy(struct xrt_device *xdev)
 	u_device_free(&htd->base);
 }
 
-struct xrt_device *
+static struct ht_device *
-ht_device_create(struct xrt_prober *xp, struct t_stereo_camera_calibration *calib)
+ht_device_create_common(struct t_stereo_camera_calibration *calib,
+                        struct xrt_frame_context *xfctx,
+                        struct t_hand_tracking_sync *sync)
 {
 	XRT_TRACE_MARKER();
-	assert(calib != NULL);
 
-	enum u_device_alloc_flags flags = U_DEVICE_ALLOC_NO_FLAGS;
+	enum u_device_alloc_flags flags = U_DEVICE_ALLOC_NO_FLAGS | U_DEVICE_ALLOC_TRACKING_NONE;
 
 	//! @todo 2 hands hardcoded
 	int num_hands = 2;
@@ -233,29 +194,17 @@ ht_device_create(struct xrt_prober *xp, struct t_stereo_camera_calibration *cali
 	// Allocate device
 	struct ht_device *htd = U_DEVICE_ALLOCATE(struct ht_device, flags, num_hands, 0);
 
-	// Setup logging first. We like logging.
+	// Setup logging first
 	htd->log_level = debug_get_log_option_ht_log();
 
-	// Set defaults - most people won't have a config json and it won't get past here.
+	htd->xfctx.nodes = xfctx->nodes;
-	htd->desired_format = XRT_FORMAT_YUYV422;
 
-	htd->prober = xp;
-	htd->xfs = NULL;
-
-	xrt_prober_list_video_devices(htd->prober, on_video_device, htd);
-
-	if (htd->xfs == NULL) {
-		return NULL;
-	}
-
-	htd->base.tracking_origin = &htd->tracking_origin;
 	htd->base.tracking_origin->type = XRT_TRACKING_TYPE_RGB;
 	htd->base.tracking_origin->offset.position.x = 0.0f;
 	htd->base.tracking_origin->offset.position.y = 0.0f;
 	htd->base.tracking_origin->offset.position.z = 0.0f;
 	htd->base.tracking_origin->offset.orientation.w = 1.0f;
 
-
 	htd->base.update_inputs = ht_device_update_inputs;
 	htd->base.get_hand_tracking = ht_device_get_hand_tracking;
 	htd->base.destroy = ht_device_destroy;
@@ -273,34 +222,91 @@ ht_device_create(struct xrt_prober *xp, struct t_stereo_camera_calibration *cali
 	htd->base.position_tracking_supported = true;
 	htd->base.hand_tracking_supported = true;
 
-	htd->sync = t_hand_tracking_sync_old_rgb_create(calib);
+	htd->sync = sync;
-	htd->async = t_hand_tracking_async_default_create(&htd->xfctx, htd->sync);
+
+	htd->async = t_hand_tracking_async_default_create(&htd->xfctx, sync);
+	return htd;
+}
+
+struct index_camera_finder
+{
+	struct xrt_fs *xfs;
+	struct xrt_frame_context xfctx;
+	bool found;
+};
+
+static void
+on_video_device(struct xrt_prober *xp,
+                struct xrt_prober_device *pdev,
+                const char *product,
+                const char *manufacturer,
+                const char *serial,
+                void *ptr)
+{
+	struct index_camera_finder *finder = (struct index_camera_finder *)ptr;
+
+	// Hardcoded for the Index.
+	if (product != NULL && manufacturer != NULL) {
+		if ((strcmp(product, "3D Camera") == 0) && (strcmp(manufacturer, "Etron Technology, Inc.") == 0)) {
+			xrt_prober_open_video_device(xp, pdev, &finder->xfctx, &finder->xfs);
+			return;
+		}
+	}
+}
+
+struct xrt_device *
+ht_device_create_index(struct xrt_prober *xp, struct t_stereo_camera_calibration *calib)
+{
+	XRT_TRACE_MARKER();
+	assert(calib != NULL);
+
+
+	struct index_camera_finder finder = {0};
+
+	xrt_prober_list_video_devices(xp, on_video_device, &finder);
+
+
+	if (finder.xfs == NULL) {
+		return NULL;
+	}
+
+	bool use_old_rgb = debug_get_bool_option_ht_use_old_rgb();
+
+	struct t_hand_tracking_sync *sync;
+
+	if (use_old_rgb) {
+		sync = t_hand_tracking_sync_old_rgb_create(calib);
+	} else {
+		sync = t_hand_tracking_sync_mercury_create(calib, MERCURY_OUTPUT_SPACE_LEFT_CAMERA);
+	}
+
+	struct ht_device *htd = ht_device_create_common(calib, &finder.xfctx, sync);
 
 	struct xrt_frame_sink *tmp = NULL;
 
 	u_sink_stereo_sbs_to_slam_sbs_create(&htd->xfctx, &htd->async->left, &htd->async->right, &tmp);
 
-	// Converts images (we'd expect YUV422 or MJPEG) to R8G8B8. Can take a long time, especially on unoptimized
+	if (use_old_rgb) {
-	// builds. If it's really slow, triple-check that you built Monado with optimizations!
-	//!@todo
 		u_sink_create_format_converter(&htd->xfctx, XRT_FORMAT_R8G8B8, tmp, &tmp);
+	} else {
+		u_sink_create_format_converter(&htd->xfctx, XRT_FORMAT_L8, tmp, &tmp);
+	}
 
-	// This puts u_sink_create_to_r8g8b8_or_l8 on its own thread, so that nothing gets backed up if it runs slower
+	// This puts the format converter on its own thread, so that nothing gets backed up if it runs slower
 	// than the native camera framerate.
 	u_sink_simple_queue_create(&htd->xfctx, tmp, &tmp);
 
-	struct xrt_fs_mode *modes;
+	struct xrt_fs_mode *modes = NULL;
 	uint32_t count;
 
-	xrt_fs_enumerate_modes(htd->xfs, &modes, &count);
+	xrt_fs_enumerate_modes(finder.xfs, &modes, &count);
 
-	// Index should only have XRT_FORMAT_YUYV422 or XRT_FORMAT_MJPEG.
 
 	bool found_mode = false;
 	uint32_t selected_mode = 0;
 
 	for (; selected_mode < count; selected_mode++) {
-		if (modes[selected_mode].format == htd->desired_format) {
+		if (modes[selected_mode].format == XRT_FORMAT_YUYV422) {
 			found_mode = true;
 			break;
 		}
@@ -313,9 +319,49 @@ ht_device_create(struct xrt_prober *xp, struct t_stereo_camera_calibration *cali
 
 	free(modes);
 
-	xrt_fs_stream_start(htd->xfs, tmp, XRT_FS_CAPTURE_TYPE_TRACKING, selected_mode);
+	xrt_fs_stream_start(finder.xfs, tmp, XRT_FS_CAPTURE_TYPE_TRACKING, selected_mode);
 
 	HT_DEBUG(htd, "Hand Tracker initialized!");
 
 	return &htd->base;
 }
+
+#ifdef XRT_BUILD_DRIVER_DEPTHAI
+struct xrt_device *
+ht_device_create_depthai_ov9282()
+{
+	XRT_TRACE_MARKER();
+
+	struct xrt_frame_context xfctx = {0};
+
+	struct xrt_fs *xfs = depthai_fs_stereo_grayscale(&xfctx);
+
+	if (xfs == NULL) {
+		return NULL;
+	}
+
+	struct t_stereo_camera_calibration *calib = NULL;
+
+	depthai_fs_get_stereo_calibration(xfs, &calib);
+
+	assert(calib != NULL);
+
+	struct t_hand_tracking_sync *sync;
+
+	sync = t_hand_tracking_sync_mercury_create(calib, MERCURY_OUTPUT_SPACE_LEFT_CAMERA);
+
+	struct ht_device *htd = ht_device_create_common(calib, &xfctx, sync);
+
+	struct xrt_slam_sinks tmp;
+
+	t_stereo_camera_calibration_reference(&calib, NULL);
+
+	u_sink_force_genlock_create(&htd->xfctx, &htd->async->left, &htd->async->right, &tmp.left, &tmp.right);
+
+	xrt_fs_slam_stream_start(xfs, &tmp);
+
+	HT_DEBUG(htd, "Hand Tracker initialized!");
+
+	return &htd->base;
+}
+#endif

src/xrt/drivers/ht/ht_interface.h

@@ -11,6 +11,7 @@
 #pragma once
 
 #include "xrt/xrt_device.h"
+#include "xrt/xrt_config_drivers.h"
 
 #include "tracking/t_tracking.h"
 #include "xrt/xrt_prober.h"
@@ -32,7 +33,15 @@ extern "C" {
  * @ingroup drv_ht
  */
 struct xrt_device *
-ht_device_create(struct xrt_prober *xp, struct t_stereo_camera_calibration *calib);
+ht_device_create_index(struct xrt_prober *xp, struct t_stereo_camera_calibration *calib);
+
+#ifdef XRT_BUILD_DRIVER_DEPTHAI
+struct xrt_device *
+ht_device_create_depthai_ov9282(void);
+
+struct xrt_auto_prober *
+ht_create_auto_prober();
+#endif
 
 /*!
  * @dir drivers/ht

src/xrt/drivers/ht/ht_prober.c

@@ -0,0 +1,57 @@
+// Copyright 2021, Collabora, Ltd.
+// SPDX-License-Identifier: BSL-1.0
+/*!
+ * @file
+ * @brief Hack for making it easy to run our tracking with DepthAI cameras.
+ * @author Moses Turner <moses@collabora.com>
+ * @ingroup drv_ht
+ */
+
+#include "ht_interface.h"
+
+#include "util/u_debug.h"
+
+
+DEBUG_GET_ONCE_BOOL_OPTION(ht_use_depthai, "HT_USE_DEPTHAI", false)
+
+static void
+ht_prober_destroy(struct xrt_auto_prober *p)
+{
+	free(p);
+}
+
+static int
+ht_prober_autoprobe(struct xrt_auto_prober *xap,
+                    cJSON *attached_data,
+                    bool no_hmds,
+                    struct xrt_prober *xp,
+                    struct xrt_device **out_xdevs)
+{
+	if (!debug_get_bool_option_ht_use_depthai()) {
+		return 0;
+	}
+
+	struct xrt_device *ht = ht_device_create_depthai_ov9282();
+
+	if (ht == NULL) {
+		return 0;
+	}
+
+	int out_idx = 0;
+
+	out_xdevs[out_idx++] = ht;
+	return out_idx;
+}
+
+
+
+struct xrt_auto_prober *
+ht_create_auto_prober()
+{
+	struct xrt_auto_prober *xap = U_TYPED_CALLOC(struct xrt_auto_prober);
+	xap->name = "ht_depthai";
+	xap->destroy = ht_prober_destroy;
+	xap->lelo_dallas_autoprobe = ht_prober_autoprobe;
+
+	return xap;
+}

src/xrt/drivers/survive/survive_driver.c

@@ -1411,7 +1411,7 @@ survive_device_autoprobe(struct xrt_auto_prober *xap,
 		struct xrt_pose head_in_left_cam;
 		vive_get_stereo_camera_calibration(&ss->hmd->hmd.config, &cal, &head_in_left_cam);
 
-		struct xrt_device *ht = ht_device_create(xp, cal);
+		struct xrt_device *ht = ht_device_create_index(xp, cal);
 		if (ht != NULL) { // Returns NULL if there's a problem and the hand tracker can't start. By no means a
 			          // fatal error.
 			struct xrt_device *wrap =

src/xrt/drivers/vive/vive_prober.c

@@ -269,7 +269,7 @@ init_valve_index(struct xrt_prober *xp,
 		// vive_get_stereo_camera_calibration(&ss->hmd->hmd.config, &cal, &head_in_left_cam);
 		vive_get_stereo_camera_calibration(&d->config, &cal, &head_in_left_cam);
 
-		struct xrt_device *ht = ht_device_create(xp, cal);
+		struct xrt_device *ht = ht_device_create_index(xp, cal);
 		if (ht != NULL) { // Returns NULL if there's a problem and the hand tracker can't start. By no means a
 			          // fatal error.
 			struct xrt_device *wrap =

src/xrt/targets/common/target_lists.c

@@ -83,6 +83,12 @@
 #include "euroc/euroc_interface.h"
 #endif
 
+#ifdef XRT_BUILD_DRIVER_HANDTRACKING
+#ifdef XRT_BUILD_DRIVER_DEPTHAI
+#include "ht/ht_interface.h"
+#endif
+#endif
+
 
 /*!
  * Each entry should be a vendor ID (VID), product ID (PID), a "found" function,
@@ -197,6 +203,11 @@ xrt_auto_prober_creator target_auto_list[] = {
     dummy_create_auto_prober,
 #endif
 
+#ifdef XRT_BUILD_DRIVER_HANDTRACKING
+#ifdef XRT_BUILD_DRIVER_DEPTHAI
+    ht_create_auto_prober,
+#endif
+#endif
     NULL, // Terminate
 };