From 4f81967e8a497e2fbfb83f487c14267085d0d8b2 Mon Sep 17 00:00:00 2001
From: Mateo de Mayo <mateo.demayo@collabora.com>
Date: Wed, 20 Jul 2022 12:58:46 -0300
Subject: [PATCH] t/common: Refactor lighthouse builder to prepare for more
 visual trackers

The idea here is to follow a similar approach to how we are doing it in the
WMR driver with the setup_visual_trackers() function.
---
 .../common/target_builder_lighthouse.c        | 420 +++++++++++-------
 1 file changed, 265 insertions(+), 155 deletions(-)

diff --git a/src/xrt/targets/common/target_builder_lighthouse.c b/src/xrt/targets/common/target_builder_lighthouse.c
index 46317cc13..f04845bfc 100644
--- a/src/xrt/targets/common/target_builder_lighthouse.c
+++ b/src/xrt/targets/common/target_builder_lighthouse.c
@@ -8,56 +8,72 @@
  * @ingroup xrt_iface
  */
 
-#include "util/u_debug.h"
+#include "tracking/t_hand_tracking.h"
+
 #include "xrt/xrt_config_drivers.h"
 #include "xrt/xrt_device.h"
 #include "xrt/xrt_prober.h"
 
 #include "util/u_builders.h"
 #include "util/u_config_json.h"
-#include "util/u_system_helpers.h"
+#include "util/u_debug.h"
 #include "util/u_device.h"
+#include "util/u_sink.h"
+#include "util/u_system_helpers.h"
 
 #include "target_builder_interface.h"
 
 #include "vive/vive_config.h"
-#include "xrt/xrt_results.h"
 
-// We compute things using have_{vive,survive} in lighthouse_estimate_system.
+#include "xrt/xrt_frameserver.h"
+#include "xrt/xrt_results.h"
+#include "xrt/xrt_tracking.h"
+
+#include <assert.h>
+
+//! @todo This should not be in static storage. Maybe make this inherit and replace usysd.
+static struct lighthouse_system
+{
+	bool use_libsurvive; //!< Whether we are using survive driver or vive driver
+	bool hand_enabled;   //!< Whether hand tracking is enabled
+} lhs;
 
 #ifdef XRT_BUILD_DRIVER_VIVE
 #include "vive/vive_prober.h"
-static const bool have_vive = true;
-#else
-static const bool have_vive = false;
 #endif
 
 #ifdef XRT_BUILD_DRIVER_SURVIVE
 #include "survive/survive_interface.h"
-static const bool have_survive = true;
-#else
-static const bool have_survive = false;
 #endif
 
 #ifdef XRT_BUILD_DRIVER_HANDTRACKING
-#include "xrt/xrt_frameserver.h"
-#include "util/u_sink.h"
 #include "ht/ht_interface.h"
 #include "ht_ctrl_emu/ht_ctrl_emu_interface.h"
 #include "multi_wrapper/multi.h"
 #include "../../tracking/hand/mercury/hg_interface.h"
 #endif
 
-
-
-#include <assert.h>
-
-
-DEBUG_GET_ONCE_BOOL_OPTION(vive_over_survive, "SETUP_VIVE_OVER_SURVIVE", false)
+DEBUG_GET_ONCE_LOG_OPTION(lh_log, "LH_LOG", U_LOGGING_WARN)
+DEBUG_GET_ONCE_BOOL_OPTION(vive_over_survive, "VIVE_OVER_SURVIVE", false)
+DEBUG_GET_ONCE_BOOL_OPTION(lh_handtracking, "LH_HANDTRACKING", true)
 DEBUG_GET_ONCE_BOOL_OPTION(ht_use_old_rgb, "HT_USE_OLD_RGB", false)
 
+#define LH_TRACE(...) U_LOG_IFL_T(debug_get_log_option_lh_log(), __VA_ARGS__)
+#define LH_DEBUG(...) U_LOG_IFL_D(debug_get_log_option_lh_log(), __VA_ARGS__)
+#define LH_INFO(...) U_LOG_IFL_I(debug_get_log_option_lh_log(), __VA_ARGS__)
+#define LH_WARN(...) U_LOG_IFL_W(debug_get_log_option_lh_log(), __VA_ARGS__)
+#define LH_ERROR(...) U_LOG_IFL_E(debug_get_log_option_lh_log(), __VA_ARGS__)
+#define LH_ASSERT(predicate, ...)                                                                                      \
+	do {                                                                                                           \
+		bool p = predicate;                                                                                    \
+		if (!p) {                                                                                              \
+			U_LOG(U_LOGGING_ERROR, __VA_ARGS__);                                                           \
+			assert(false && "LH_ASSERT failed: " #predicate);                                              \
+			exit(EXIT_FAILURE);                                                                            \
+		}                                                                                                      \
+	} while (false);
+#define LH_ASSERT_(predicate) LH_ASSERT(predicate, "Assertion failed " #predicate)
 
-static bool use_libsurvive = false;
 
 static const char *driver_list[] = {
 #ifdef XRT_BUILD_DRIVER_SURVIVE
@@ -69,7 +85,6 @@ static const char *driver_list[] = {
 #endif
 };
 
-#ifdef XRT_BUILD_DRIVER_HANDTRACKING
 struct index_camera_finder
 {
 	struct xrt_fs *xfs;
@@ -79,10 +94,31 @@ struct index_camera_finder
 
 /*
  *
- * Helper hand-tracking setup functions.
+ * Helper tracking setup functions.
  *
  */
 
+static uint32_t
+get_selected_mode(struct xrt_fs *xfs)
+{
+	struct xrt_fs_mode *modes = NULL;
+	uint32_t count = 0;
+	xrt_fs_enumerate_modes(xfs, &modes, &count);
+
+	LH_ASSERT(count != 0, "No stream modes found in Index camera");
+
+	uint32_t selected_mode = 0;
+	for (uint32_t i = 0; i < count; i++) {
+		if (modes[i].format == XRT_FORMAT_YUYV422) {
+			selected_mode = i;
+			break;
+		}
+	}
+
+	free(modes);
+	return selected_mode;
+}
+
 static void
 on_video_device(struct xrt_prober *xp,
                 struct xrt_prober_device *pdev,
@@ -102,33 +138,19 @@ on_video_device(struct xrt_prober *xp,
 	}
 }
 
-
 static bool
-create_index_optical_hand_tracker(struct u_system_devices *usysd,
-                                  struct xrt_prober *xp,
-                                  struct t_stereo_camera_calibration *calib,
-                                  struct xrt_device **out_hand_tracker)
+lighthouse_hand_track(struct u_system_devices *usysd,
+                      struct xrt_prober *xp,
+                      struct xrt_pose head_in_left_cam,
+                      struct t_stereo_camera_calibration *stereo_calib,
+                      struct xrt_slam_sinks **out_sinks,
+                      struct xrt_device **out_devices)
 {
-	assert(calib != NULL);
-
-
-	struct index_camera_finder finder = {0};
-	finder.xfctx = &usysd->xfctx;
-
-	xrt_prober_list_video_devices(xp, on_video_device, &finder);
-
-
-	if (finder.xfs == NULL) {
-		U_LOG_W("Couldn't find Index camera at all. Is it plugged in?");
-		return false;
-	}
-
-	bool old_rgb = debug_get_bool_option_ht_use_old_rgb();
-
-
-
+	struct xrt_device *two_hands[2] = {NULL};
 	struct xrt_slam_sinks *sinks = NULL;
-	struct xrt_device *hand_tracker_device = NULL;
+
+#ifdef XRT_BUILD_DRIVER_HANDTRACKING
+	LH_ASSERT_(stereo_calib != NULL);
 
 	struct t_image_boundary_info info;
 	info.views[0].type = HT_IMAGE_BOUNDARY_CIRCLE;
@@ -148,72 +170,40 @@ create_index_optical_hand_tracker(struct u_system_devices *usysd,
 	info.views[0].boundary.circle.normalized_radius = 0.55;
 	info.views[1].boundary.circle.normalized_radius = 0.55;
 
-	ht_device_create(&usysd->xfctx,                                         //
-	                 calib,                                                 //
-	                 HT_OUTPUT_SPACE_LEFT_CAMERA,                           //
-	                 old_rgb ? HT_ALGORITHM_OLD_RGB : HT_ALGORITHM_MERCURY, //
-	                 info,                                                  //
-	                 &sinks,                                                //
-	                 &hand_tracker_device);
+	bool old_rgb = debug_get_bool_option_ht_use_old_rgb();
+	enum t_hand_tracking_algorithm ht_algorithm = old_rgb ? HT_ALGORITHM_OLD_RGB : HT_ALGORITHM_MERCURY;
 
-
-	struct xrt_frame_sink *tmp = NULL;
-
-	u_sink_stereo_sbs_to_slam_sbs_create(&usysd->xfctx, sinks->left, sinks->right, &tmp);
-
-	enum xrt_format fmt = old_rgb ? XRT_FORMAT_R8G8B8 : XRT_FORMAT_L8;
-
-	u_sink_create_format_converter(&usysd->xfctx, fmt, tmp, &tmp);
-
-	// 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(&usysd->xfctx, tmp, &tmp);
-
-	struct xrt_fs_mode *modes = NULL;
-	uint32_t count = 0;
-
-	xrt_fs_enumerate_modes(finder.xfs, &modes, &count);
-
-	if (count == 0) {
-		U_LOG_W("Index camera has no modes?");
-		goto error;
+	struct xrt_device *ht_device = NULL;
+	int create_status = ht_device_create(&usysd->xfctx,               //
+	                                     stereo_calib,                //
+	                                     HT_OUTPUT_SPACE_LEFT_CAMERA, //
+	                                     ht_algorithm,                //
+	                                     info,                        //
+	                                     &sinks,                      //
+	                                     &ht_device);
+	if (create_status != 0) {
+		LH_WARN("Failed to create hand tracking device\n");
+		return false;
 	}
 
-	bool found_mode = false;
-	uint32_t selected_mode = 0;
+	ht_device = multi_create_tracking_override(XRT_TRACKING_OVERRIDE_ATTACHED, ht_device, usysd->base.roles.head,
+	                                           XRT_INPUT_GENERIC_HEAD_POSE, &head_in_left_cam);
 
-	for (; selected_mode < count; selected_mode++) {
-		if (modes[selected_mode].format == XRT_FORMAT_YUYV422) {
-			found_mode = true;
-			break;
-		}
+	int created_devices = cemu_devices_create(usysd->base.roles.head, ht_device, two_hands);
+	if (created_devices != 2) {
+		LH_WARN("Unexpected amount of hand devices created (%d)\n", create_status);
+		xrt_device_destroy(&ht_device);
+		return false;
 	}
 
-	if (!found_mode) {
-		selected_mode = 0;
-	}
-
-	free(modes);
-
-	bool ret = xrt_fs_stream_start(finder.xfs, tmp, XRT_FS_CAPTURE_TYPE_TRACKING, selected_mode);
-
-	if (!ret) {
-		U_LOG_E("Couldn't start camera. Has something else claimed it?");
-		goto error;
-	}
-
-	*out_hand_tracker = hand_tracker_device;
-	return true;
-
-error:
-
-	xrt_frame_context_destroy_nodes(&usysd->xfctx);
-	xrt_device_destroy(&hand_tracker_device);
-	return false;
-}
-
+	LH_INFO("Hand tracker successfully created\n");
 #endif
 
+	*out_sinks = sinks;
+	out_devices[0] = two_hands[0];
+	out_devices[1] = two_hands[1];
+	return true;
+}
 
 /*
  *
@@ -227,21 +217,34 @@ lighthouse_estimate_system(struct xrt_builder *xb,
                            struct xrt_prober *xp,
                            struct xrt_builder_estimate *estimate)
 {
+#ifdef XRT_BUILD_DRIVER_VIVE
+	bool have_vive_drv = true;
+#else
+	bool have_vive_drv = false;
+#endif
+
+#ifdef XRT_BUILD_DRIVER_SURVIVE
+	bool have_survive_drv = true;
+#else
+	bool have_survive_drv = false;
+#endif
 
 	bool vive_over_survive = debug_get_bool_option_vive_over_survive();
-	if (have_survive && have_vive) {
+	if (have_survive_drv && have_vive_drv) {
 		// We have both drivers - default to libsurvive, but if the user asks specifically for vive we'll give
 		// it to them
-		use_libsurvive = !vive_over_survive;
-	} else if (have_survive) {
+		lhs.use_libsurvive = !vive_over_survive;
+	} else if (have_survive_drv) {
 		// We only have libsurvive - don't listen to the env var
 		if (vive_over_survive) {
-			U_LOG_W("Asked for vive, but vive isn't built. Using libsurvive.");
+			LH_WARN("Asked for vive driver, but it isn't built. Using libsurvive.");
 		}
-		use_libsurvive = true;
-	} else {
+		lhs.use_libsurvive = true;
+	} else if (have_vive_drv) {
 		// We only have vive
-		use_libsurvive = false;
+		lhs.use_libsurvive = false;
+	} else {
+		LH_ASSERT_(false);
 	}
 
 	U_ZERO(estimate);
@@ -266,7 +269,7 @@ lighthouse_estimate_system(struct xrt_builder *xb,
 
 	if (have_vive || have_vive_pro || have_index) {
 		estimate->certain.head = true;
-		if (use_libsurvive) {
+		if (lhs.use_libsurvive) {
 			estimate->maybe.dof6 = true;
 			estimate->certain.dof6 = true;
 		}
@@ -305,11 +308,103 @@ lighthouse_estimate_system(struct xrt_builder *xb,
 	estimate->priority = 0;
 
 	xret = xrt_prober_unlock_list(xp, &xpdevs);
-	assert(xret == XRT_SUCCESS);
+	LH_ASSERT_(xret == XRT_SUCCESS);
 
 	return XRT_SUCCESS;
 }
 
+
+static bool
+setup_visual_trackers(struct u_system_devices *usysd,
+                      struct xrt_prober *xp,
+                      struct vive_config *hmd_config,
+                      struct xrt_slam_sinks *out_sinks,
+                      struct xrt_device **out_devices)
+{
+	bool hand_enabled = lhs.hand_enabled;
+
+	struct t_stereo_camera_calibration *stereo_calib = NULL;
+	struct xrt_pose head_in_left_cam;
+	vive_get_stereo_camera_calibration(hmd_config, &stereo_calib, &head_in_left_cam);
+
+	// Initialize hand tracker
+	struct xrt_slam_sinks *hand_sinks = NULL;
+	struct xrt_device *hand_devices[2] = {NULL};
+	if (hand_enabled) {
+		bool success =
+		    lighthouse_hand_track(usysd, xp, head_in_left_cam, stereo_calib, &hand_sinks, hand_devices);
+		if (!success) {
+			LH_WARN("Unable to setup the hand tracker");
+			return false;
+		}
+	}
+
+	t_stereo_camera_calibration_reference(&stereo_calib, NULL);
+
+	// Setup frame graph
+
+	struct xrt_frame_sink *entry_left_sink = NULL;
+	struct xrt_frame_sink *entry_right_sink = NULL;
+	struct xrt_frame_sink *entry_sbs_sink = NULL;
+	bool old_rgb_ht = debug_get_bool_option_ht_use_old_rgb();
+
+	if (hand_enabled) {
+		enum xrt_format fmt = old_rgb_ht ? XRT_FORMAT_R8G8B8 : XRT_FORMAT_L8;
+		entry_left_sink = hand_sinks->left;
+		entry_right_sink = hand_sinks->right;
+		u_sink_stereo_sbs_to_slam_sbs_create(&usysd->xfctx, entry_left_sink, entry_right_sink, &entry_sbs_sink);
+		u_sink_create_format_converter(&usysd->xfctx, fmt, entry_sbs_sink, &entry_sbs_sink);
+	} else {
+		LH_WARN("No visual trackers were set");
+		return false;
+	}
+	//! @todo Using a single slot queue is wrong for SLAM
+	u_sink_simple_queue_create(&usysd->xfctx, entry_sbs_sink, &entry_sbs_sink);
+
+	struct xrt_slam_sinks entry_sinks = {
+	    .left = entry_sbs_sink,
+	    .right = NULL, // v4l2 streams a single SBS frame so we ignore the right sink
+	    .imu = NULL,
+	    .gt = NULL,
+	};
+
+	*out_sinks = entry_sinks;
+	if (hand_enabled) {
+		out_devices[0] = hand_devices[0];
+		out_devices[1] = hand_devices[1];
+	}
+	return true;
+}
+
+
+
+static bool
+stream_data_sources(struct u_system_devices *usysd, struct xrt_prober *xp, struct xrt_slam_sinks sinks)
+{
+	// Open frame server
+	struct index_camera_finder finder = {0};
+	finder.xfctx = &usysd->xfctx;
+	xrt_prober_list_video_devices(xp, on_video_device, &finder);
+	if (finder.xfs == NULL) {
+		LH_WARN("Couldn't find Index camera at all. Is it plugged in?");
+		xrt_frame_context_destroy_nodes(&usysd->xfctx);
+		return false;
+	}
+
+	bool success = false;
+
+	uint32_t mode = get_selected_mode(finder.xfs);
+	success = xrt_fs_stream_start(finder.xfs, sinks.left, XRT_FS_CAPTURE_TYPE_TRACKING, mode);
+
+
+	if (!success) {
+		LH_ERROR("Unable to start data streaming");
+		xrt_frame_context_destroy_nodes(&usysd->xfctx);
+	}
+
+	return success;
+}
+
 static xrt_result_t
 lighthouse_open_system(struct xrt_builder *xb,
                        cJSON *config,
@@ -317,14 +412,29 @@ lighthouse_open_system(struct xrt_builder *xb,
                        struct xrt_system_devices **out_xsysd)
 {
 	struct u_system_devices *usysd = u_system_devices_allocate();
+	xrt_result_t result = XRT_SUCCESS;
 
-	assert(out_xsysd != NULL);
-	assert(*out_xsysd == NULL);
+	if (out_xsysd == NULL || *out_xsysd != NULL) {
+		LH_ERROR("Invalid output system pointer");
+		result = XRT_ERROR_DEVICE_CREATION_FAILED;
+		goto end;
+	}
+
+	// Decide whether to initialize the hand tracker
+	bool hand_wanted = debug_get_bool_option_lh_handtracking();
+#ifdef XRT_BUILD_DRIVER_HANDTRACKING
+	bool hand_supported = true;
+#else
+	bool hand_supported = false;
+#endif
+	bool hand_enabled = hand_supported && hand_wanted;
+
+	lhs.hand_enabled = hand_enabled;
 
 
 	struct vive_config *hmd_config = NULL;
 
-	if (use_libsurvive) {
+	if (lhs.use_libsurvive) {
 #ifdef XRT_BUILD_DRIVER_SURVIVE
 		usysd->base.xdev_count += survive_get_devices(&usysd->base.xdevs[usysd->base.xdev_count], &hmd_config);
 #endif
@@ -332,11 +442,11 @@ lighthouse_open_system(struct xrt_builder *xb,
 #ifdef XRT_BUILD_DRIVER_VIVE
 		struct xrt_prober_device **xpdevs = NULL;
 		size_t xpdev_count = 0;
-		xrt_result_t xret = XRT_SUCCESS;
 
-		xret = xrt_prober_lock_list(xp, &xpdevs, &xpdev_count);
-		if (xret != XRT_SUCCESS) {
-			return xret;
+		result = xrt_prober_lock_list(xp, &xpdevs, &xpdev_count);
+		if (result != XRT_SUCCESS) {
+			LH_ERROR("Unable to lock the prober dev list");
+			goto end;
 		}
 		for (size_t i = 0; i < xpdev_count; i++) {
 			struct xrt_prober_device *device = xpdevs[i];
@@ -373,8 +483,9 @@ lighthouse_open_system(struct xrt_builder *xb,
 	u_device_assign_xdev_roles(usysd->base.xdevs, usysd->base.xdev_count, &head_idx, &left_idx, &right_idx);
 
 	if (head_idx < 0) {
-		// We need to at least create a HMD
-		return XRT_ERROR_DEVICE_CREATION_FAILED;
+		LH_ERROR("Unable to find HMD");
+		result = XRT_ERROR_DEVICE_CREATION_FAILED;
+		goto end;
 	}
 	usysd->base.roles.head = usysd->base.xdevs[head_idx];
 
@@ -390,44 +501,43 @@ lighthouse_open_system(struct xrt_builder *xb,
 		    u_system_devices_get_ht_device(usysd, XRT_INPUT_GENERIC_HAND_TRACKING_RIGHT);
 	}
 
+	bool success = true;
 
-
-#ifdef XRT_BUILD_DRIVER_HANDTRACKING
-	// If we have the camera calibration (in hmd_config), and we have a HMD but no controllers, then try to make a
-	// hand-tracker device.
-	if ((hmd_config != NULL) &&             //
-	    (usysd->base.roles.head != NULL) && //
-	    (usysd->base.roles.left == NULL) && //
-	    (usysd->base.roles.right == NULL)) {
-		struct t_stereo_camera_calibration *cal = NULL;
-
-		struct xrt_pose head_in_left_cam;
-		if (vive_get_stereo_camera_calibration(hmd_config, &cal, &head_in_left_cam)) {
-			struct xrt_device *ht = NULL;
-			create_index_optical_hand_tracker(usysd, xp, cal, &ht);
-			// It's also okay if we couldn't open the hand tracker
-			if (ht != NULL) {
-				struct xrt_device *wrap = multi_create_tracking_override(
-				    XRT_TRACKING_OVERRIDE_ATTACHED, ht, usysd->base.roles.head,
-				    XRT_INPUT_GENERIC_HEAD_POSE, &head_in_left_cam);
-				struct xrt_device *two_hands[2];
-				cemu_devices_create(usysd->base.roles.head, wrap, two_hands);
-
-				usysd->base.roles.left = two_hands[0];
-				usysd->base.roles.right = two_hands[1];
-				usysd->base.roles.hand_tracking.left = two_hands[0];
-				usysd->base.roles.hand_tracking.right = two_hands[1];
-				usysd->base.xdevs[usysd->base.xdev_count++] = two_hands[0];
-				usysd->base.xdevs[usysd->base.xdev_count++] = two_hands[1];
-			}
-			t_stereo_camera_calibration_reference(&cal, NULL);
-		}
+	struct xrt_slam_sinks sinks = {0};
+	struct xrt_device *hand_devices[2] = {NULL};
+	success = setup_visual_trackers(usysd, xp, hmd_config, &sinks, hand_devices);
+	if (!success) {
+		result = XRT_SUCCESS; // We won't have trackers, but creation was otherwise ok
+		goto end;
 	}
-#endif
 
-	*out_xsysd = &usysd->base;
+	if (hand_devices[0] != NULL) {
+		usysd->base.roles.left = hand_devices[0];
+		usysd->base.roles.hand_tracking.left = hand_devices[0];
+		usysd->base.xdevs[usysd->base.xdev_count++] = hand_devices[0];
+	}
 
-	return XRT_SUCCESS;
+	if (hand_devices[1] != NULL) {
+		usysd->base.roles.right = hand_devices[1];
+		usysd->base.roles.hand_tracking.right = hand_devices[1];
+		usysd->base.xdevs[usysd->base.xdev_count++] = hand_devices[1];
+	}
+
+	success = stream_data_sources(usysd, xp, sinks);
+	if (!success) {
+		result = XRT_SUCCESS; // We can continue after freeing trackers
+		goto end;
+	}
+
+end:
+
+	if (result == XRT_SUCCESS) {
+		*out_xsysd = &usysd->base;
+	} else {
+		u_system_devices_destroy(&usysd);
+	}
+
+	return result;
 }
 
 static void