t/openvr: Add openvr tracker for recording lighthouse groundtruth

With the SLAM_OPENVR_GROUNDTRUTH_DEVICE we can select a device (HMD, right/left
controller, vive tracker) to use as the groundtruth provider for a SLAM tracker.
This is useful to record euroc datasets with lighthouse groundtruth.

CMakeLists.txt

@@ -189,6 +189,16 @@ foreach(slam_system IN LISTS EXTERNAL_SLAM_SYSTEMS)
 	endif()
 endforeach()
 
+# Find openvr library
+find_library(OPENVR_LIBRARIES NAMES openvr_api)
+if(OPENVR_LIBRARIES)
+	set(OPENVR_FOUND on)
+	message("Found OpenVR: " ${OPENVR_LIBRARIES})
+else()
+	set(OPENVR_FOUND off)
+	message("OpenVR not found")
+endif()
+
 # ILLIXR
 set(ILLIXR_PATH
     ""
@@ -296,6 +306,7 @@ option_with_deps(XRT_HAVE_JPEG "Enable jpeg code (used for some video drivers)"
 option_with_deps(XRT_HAVE_LIBUSB "Enable libusb (used for most drivers)" DEPENDS LIBUSB1_FOUND)
 option_with_deps(XRT_HAVE_LIBUVC "Enable libuvc video driver" DEPENDS LIBUVC_FOUND XRT_HAVE_LIBUSB)
 option_with_deps(XRT_HAVE_REALSENSE "Enable RealSense support" DEPENDS realsense2_FOUND)
+option_with_deps(XRT_HAVE_OPENVR "Enable OpenVR support" DEPENDS OPENVR_FOUND)
 
 # Drivers to build (sorted)
 option_with_deps(XRT_BUILD_DRIVER_ANDROID "Enable Android sensors driver" DEPENDS ANDROID)
@@ -507,6 +518,7 @@ message(STATUS "#    OPENGLES:        ${XRT_HAVE_OPENGLES}")
 message(STATUS "#    OPENGL_GLX:      ${XRT_HAVE_OPENGL_GLX}")
 message(STATUS "#    PERCETTO:        ${XRT_HAVE_PERCETTO}")
 message(STATUS "#    REALSENSE:       ${XRT_HAVE_REALSENSE}")
+message(STATUS "#    OPENVR:          ${XRT_HAVE_OPENVR}")
 message(STATUS "#    SDL2:            ${XRT_HAVE_SDL2}")
 message(STATUS "#    SYSTEM_CJSON:    ${XRT_HAVE_SYSTEM_CJSON}")
 message(STATUS "#    SYSTEMD:         ${XRT_HAVE_SYSTEMD}")

src/xrt/auxiliary/tracking/CMakeLists.txt

@@ -7,6 +7,8 @@ add_library(
 	t_imu_fusion.hpp
 	t_imu.cpp
 	t_imu.h
+	t_openvr_tracker.cpp
+	t_openvr_tracker.h
 	t_tracking.h
 	)
 target_link_libraries(
@@ -53,3 +55,7 @@ if(XRT_FEATURE_SLAM)
 	target_sources(aux_tracking PRIVATE t_tracker_slam.cpp)
 	target_link_libraries(aux_tracking PRIVATE xrt-external-slam)
 endif()
+
+if(XRT_HAVE_OPENVR)
+	target_link_libraries(aux_tracking PRIVATE xrt-external-openvr ${OPENVR_LIBRARIES})
+endif()

src/xrt/auxiliary/tracking/t_openvr_tracker.cpp

@@ -0,0 +1,175 @@
+// Copyright 2023, Collabora, Ltd.
+// SPDX-License-Identifier: BSL-1.0
+/*!
+ * @file
+ * @brief OpenVR tracking source.
+ * @author Mateo de Mayo <mateo.demayo@collabora.com>
+ * @ingroup aux_tracking
+ */
+
+#include "t_openvr_tracker.h"
+
+#include "util/u_logging.h"
+#include "xrt/xrt_config_have.h"
+
+#ifdef XRT_HAVE_OPENVR
+#include <openvr.h>
+#include <cstddef>
+#include <map>
+
+#include "math/m_api.h"
+#include "os/os_threading.h"
+#include "util/u_logging.h"
+#include "util/u_time.h"
+#include "xrt/xrt_defines.h"
+#include "xrt/xrt_tracking.h"
+
+struct openvr_tracker
+{
+	vr::IVRSystem *vr_system;
+	struct os_thread_helper thread;
+	std::map<enum openvr_device, struct xrt_pose_sink *> sinks;
+	double sample_frequency_hz;
+
+	struct xrt_pose_sink *
+	get_sink_for_device_index(uint64_t i)
+	{
+		vr::ETrackedDeviceClass dev_class = vr_system->GetTrackedDeviceClass(i);
+		struct xrt_pose_sink *sink = nullptr;
+		if (dev_class == vr::TrackedDeviceClass_HMD && sinks.count(T_OPENVR_DEVICE_HMD) > 0) {
+			sink = sinks.at(T_OPENVR_DEVICE_HMD);
+		} else if (dev_class == vr::TrackedDeviceClass_Controller &&
+		           vr_system->GetControllerRoleForTrackedDeviceIndex(i) == vr::TrackedControllerRole_LeftHand &&
+		           sinks.count(T_OPENVR_DEVICE_LEFT_CONTROLLER) > 0) {
+			sink = sinks.at(T_OPENVR_DEVICE_LEFT_CONTROLLER);
+		} else if (dev_class == vr::TrackedDeviceClass_Controller &&
+		           vr_system->GetControllerRoleForTrackedDeviceIndex(i) ==
+		               vr::TrackedControllerRole_RightHand &&
+		           sinks.count(T_OPENVR_DEVICE_RIGHT_CONTROLLER) > 0) {
+			sink = sinks.at(T_OPENVR_DEVICE_RIGHT_CONTROLLER);
+		} else if (dev_class == vr::TrackedDeviceClass_GenericTracker &&
+		           sinks.count(T_OPENVR_DEVICE_TRACKER) > 0) {
+			sink = sinks.at(T_OPENVR_DEVICE_TRACKER);
+		}
+		return sink;
+	}
+};
+
+static void *
+tracking_loop(void *ot_ptr)
+{
+	struct openvr_tracker *ovrt = (struct openvr_tracker *)ot_ptr;
+
+	while (os_thread_helper_is_running(&ovrt->thread)) {
+		os_nanosleep(U_TIME_1S_IN_NS / ovrt->sample_frequency_hz);
+
+		// Flush events
+		vr::VREvent_t event;
+		while (ovrt->vr_system->PollNextEvent(&event, sizeof(event))) {
+		}
+
+		timepoint_ns now = os_monotonic_get_ns();
+
+		const uint32_t MAX_DEVS = vr::k_unMaxTrackedDeviceCount;
+		auto origin = vr::ETrackingUniverseOrigin::TrackingUniverseRawAndUncalibrated;
+		vr::TrackedDevicePose_t poses[MAX_DEVS];
+		ovrt->vr_system->GetDeviceToAbsoluteTrackingPose(origin, 0, poses, MAX_DEVS);
+
+		for (uint32_t i = 0; i < MAX_DEVS; i++) {
+			struct xrt_pose_sink *sink_for_i = ovrt->get_sink_for_device_index(i);
+			if (sink_for_i != nullptr && poses[i].bDeviceIsConnected && poses[i].bPoseIsValid) {
+				const auto &m = poses[i].mDeviceToAbsoluteTracking.m;
+				struct xrt_vec3 p = {m[0][3], m[1][3], m[2][3]};
+				struct xrt_matrix_3x3 R = {
+				    m[0][0], m[0][1], m[0][2], //
+				    m[1][0], m[1][1], m[1][2], //
+				    m[2][0], m[2][1], m[2][2], //
+				};
+				struct xrt_quat q = {};
+				math_quat_from_matrix_3x3(&R, &q);
+
+				struct xrt_pose_sample sample = {now, {q, p}};
+				xrt_sink_push_pose(sink_for_i, &sample);
+			}
+		}
+	}
+
+	return nullptr;
+}
+
+extern "C" {
+
+struct openvr_tracker *
+t_openvr_tracker_create(double sample_frequency, enum openvr_device *devs, struct xrt_pose_sink **sinks, int sink_count)
+{
+	struct openvr_tracker *ovrt = new openvr_tracker{};
+	os_thread_helper_init(&ovrt->thread);
+
+	for (int i = 0; i < sink_count; i++) {
+		ovrt->sinks[devs[i]] = sinks[i];
+	}
+	ovrt->sample_frequency_hz = sample_frequency;
+
+	vr::EVRInitError e = vr::VRInitError_None;
+	ovrt->vr_system = vr::VR_Init(&e, vr::VRApplication_Background);
+	if (e != vr::VRInitError_None) {
+		if (e == vr::VRInitError_Init_NoServerForBackgroundApp) {
+			U_LOG_E("Unable to find OpenVR server running. error=%d", e);
+		} else {
+			U_LOG_E("Unable to initialize OpenVR, error=%d", e);
+		}
+		return nullptr;
+	}
+	U_LOG(U_LOGGING_INFO, "OpenVR tracker created");
+	return ovrt;
+}
+
+void
+t_openvr_tracker_start(struct openvr_tracker *ovrt)
+{
+	os_thread_helper_start(&ovrt->thread, tracking_loop, ovrt);
+}
+
+void
+t_openvr_tracker_stop(struct openvr_tracker *ovrt)
+{
+	os_thread_helper_stop_and_wait(&ovrt->thread);
+}
+
+void
+t_openvr_tracker_destroy(struct openvr_tracker *ovrt)
+{
+	if (os_thread_helper_is_running(&ovrt->thread)) {
+		t_openvr_tracker_stop(ovrt);
+	}
+	vr::VR_Shutdown();
+	ovrt->vr_system = nullptr;
+	os_thread_helper_destroy(&ovrt->thread);
+	delete ovrt;
+}
+}
+
+#else
+
+struct openvr_tracker *
+t_openvr_tracker_create(double /*unused*/,
+                        enum openvr_device * /*unused*/,
+                        struct xrt_pose_sink ** /*unused*/,
+                        int /*unused*/)
+{
+	U_LOG_W("OpenVR was not built, unable to initialize lighthouse tracking.");
+	return nullptr;
+}
+
+void
+t_openvr_tracker_start(struct openvr_tracker * /*unused*/)
+{}
+
+void
+t_openvr_tracker_stop(struct openvr_tracker * /*unused*/)
+{}
+
+void
+t_openvr_tracker_destroy(struct openvr_tracker * /*unused*/)
+{}
+#endif

src/xrt/auxiliary/tracking/t_openvr_tracker.h

@@ -0,0 +1,59 @@
+// Copyright 2023, Collabora, Ltd.
+// SPDX-License-Identifier: BSL-1.0
+/*!
+ * @file
+ * @brief OpenVR tracking source.
+ * @author Mateo de Mayo <mateo.demayo@collabora.com>
+ * @ingroup aux_tracking
+ */
+
+#pragma once
+
+#include "xrt/xrt_tracking.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+enum openvr_device
+{
+	T_OPENVR_DEVICE_UNKNOWN = 0,
+	T_OPENVR_DEVICE_HMD,
+	T_OPENVR_DEVICE_LEFT_CONTROLLER,
+	T_OPENVR_DEVICE_RIGHT_CONTROLLER,
+	T_OPENVR_DEVICE_TRACKER,
+};
+
+struct openvr_tracker;
+
+/*!
+ * Creates an OpenVR tracker.
+ *
+ * This creates an OpenVR instance in a separate
+ * thread, and reports the tracking data of each device class `devs[i]` into the
+ * pose sink `sinks[i]` at a rate of `sample_frequency`.
+ *
+ * @param sample_frequency_hz Sample frequency of the tracking data in hertz
+ * @param devs Devices to report tracking data of
+ * @param sinks Where to stream the tracking data of each device in `devs` to
+ * @param sink_count Number of sinks/devices to track
+ * @return struct openvr_tracker* if successfully created, null otherwise.
+ */
+struct openvr_tracker *
+t_openvr_tracker_create(double sample_frequency_hz,
+                        enum openvr_device *devs,
+                        struct xrt_pose_sink **sinks,
+                        int sink_count);
+
+void
+t_openvr_tracker_start(struct openvr_tracker *ovrt);
+
+void
+t_openvr_tracker_stop(struct openvr_tracker *ovrt);
+
+void
+t_openvr_tracker_destroy(struct openvr_tracker *ovrt);
+
+#ifdef __cplusplus
+}
+#endif

src/xrt/auxiliary/tracking/t_tracker_slam.cpp

@@ -25,6 +25,7 @@
 #include "math/m_space.h"
 #include "math/m_vec3.h"
 #include "tracking/t_euroc_recorder.h"
+#include "tracking/t_openvr_tracker.h"
 #include "tracking/t_tracking.h"
 
 #include <slam_tracker.hpp>
@@ -69,6 +70,7 @@ DEBUG_GET_ONCE_LOG_OPTION(slam_log, "SLAM_LOG", U_LOGGING_INFO)
 DEBUG_GET_ONCE_OPTION(slam_config, "SLAM_CONFIG", nullptr)
 DEBUG_GET_ONCE_BOOL_OPTION(slam_ui, "SLAM_UI", false)
 DEBUG_GET_ONCE_BOOL_OPTION(slam_submit_from_start, "SLAM_SUBMIT_FROM_START", false)
+DEBUG_GET_ONCE_NUM_OPTION(slam_openvr_groundtruth_device, "SLAM_OPENVR_GROUNDTRUTH_DEVICE", 0)
 DEBUG_GET_ONCE_NUM_OPTION(slam_prediction_type, "SLAM_PREDICTION_TYPE", long(SLAM_PRED_SP_SO_IA_IL))
 DEBUG_GET_ONCE_BOOL_OPTION(slam_write_csvs, "SLAM_WRITE_CSVS", false)
 DEBUG_GET_ONCE_OPTION(slam_csv_path, "SLAM_CSV_PATH", "evaluation/")
@@ -359,6 +361,7 @@ struct TrackerSlam
 	MatFrame *cv_wrapper;           //!< Wraps a xrt_frame in a cv::Mat to send to the SLAM system
 
 	struct xrt_slam_sinks *euroc_recorder; //!< EuRoC dataset recording sinks
+	struct openvr_tracker *ovr_tracker;    //!< OpenVR lighthouse tracker
 
 	// Used mainly for checking that the timestamps come in order
 	timepoint_ns last_imu_ts;         //!< Last received IMU sample timestamp
@@ -815,8 +818,6 @@ flush_poses(TrackerSlam &t)
 
 		t.slam_rels.push(rel, nts);
 
-		xrt_pose_sample sample{nts, rel.pose};
-		xrt_sink_push_pose(t.euroc_recorder->gt, &sample);
 		gt_ui_push(t, nts, rel.pose);
 		t.slam_traj_writer->push(nts, rel.pose);
 
@@ -1167,6 +1168,7 @@ t_slam_gt_sink_push(struct xrt_pose_sink *sink, xrt_pose_sample *sample)
 	}
 
 	t.gt.trajectory->insert_or_assign(sample->timestamp_ns, sample->pose);
+	xrt_sink_push_pose(t.euroc_recorder->gt, sample);
 }
 
 //! Receive and send IMU samples to the external SLAM system
@@ -1203,6 +1205,9 @@ t_slam_receive_imu(struct xrt_imu_sink *sink, struct xrt_imu_sample *s)
 static void
 receive_frame(TrackerSlam &t, struct xrt_frame *frame, int cam_index)
 {
+	if (cam_index == 1) {
+		flush_poses(t); // Useful to flush SLAM poses when no openxr app is open
+	}
 	SLAM_DASSERT(t.last_cam_ts[0] != INT64_MIN || cam_index == 0, "First frame was not a cam0 frame");
 
 	// Construct and send the image sample
@@ -1248,6 +1253,9 @@ extern "C" void
 t_slam_node_break_apart(struct xrt_frame_node *node)
 {
 	auto &t = *container_of(node, TrackerSlam, node);
+	if (t.ovr_tracker != NULL) {
+		t_openvr_tracker_stop(t.ovr_tracker);
+	}
 	t.slam->finalize();
 	t.slam->stop();
 	os_thread_helper_stop_and_wait(&t.oth);
@@ -1260,6 +1268,9 @@ t_slam_node_destroy(struct xrt_frame_node *node)
 	auto t_ptr = container_of(node, TrackerSlam, node);
 	auto &t = *t_ptr; // Needed by SLAM_DEBUG
 	SLAM_DEBUG("Destroying SLAM tracker");
+	if (t.ovr_tracker != NULL) {
+		t_openvr_tracker_destroy(t.ovr_tracker);
+	}
 	os_thread_helper_destroy(&t_ptr->oth);
 	delete t.gt.trajectory;
 	delete t.slam_times_writer;
@@ -1308,6 +1319,7 @@ t_slam_fill_default_config(struct t_slam_tracker_config *config)
 	config->slam_config = debug_get_option_slam_config();
 	config->slam_ui = debug_get_bool_option_slam_ui();
 	config->submit_from_start = debug_get_bool_option_slam_submit_from_start();
+	config->openvr_groundtruth_device = int(debug_get_num_option_slam_openvr_groundtruth_device());
 	config->prediction = t_slam_prediction_type(debug_get_num_option_slam_prediction_type());
 	config->write_csvs = debug_get_bool_option_slam_write_csvs();
 	config->csv_path = debug_get_option_slam_csv_path();
@@ -1455,6 +1467,16 @@ t_slam_create(struct xrt_frame_context *xfctx,
 
 	setup_ui(t);
 
+	// Setup OpenVR groundtruth tracker
+	if (config->openvr_groundtruth_device > 0) {
+		enum openvr_device dev_class = openvr_device(config->openvr_groundtruth_device);
+		const double freq = 1000.0f;
+		t.ovr_tracker = t_openvr_tracker_create(freq, &dev_class, &t.sinks.gt, 1);
+		if (t.ovr_tracker != NULL) {
+			t_openvr_tracker_start(t.ovr_tracker);
+		}
+	}
+
 	*out_xts = &t.base;
 	*out_sink = &t.sinks;
 

src/xrt/auxiliary/tracking/t_tracking.h

@@ -649,6 +649,7 @@ struct t_slam_tracker_config
 	int cam_count;                  //!< Number of cameras in use
 	bool slam_ui;                   //!< Whether to open the external UI of the external SLAM system
 	bool submit_from_start;         //!< Whether to submit data to the SLAM tracker without user action
+	int openvr_groundtruth_device;  //!< If >0, use lighthouse as groundtruth, see @ref enum openvr_device
 	enum t_slam_prediction_type prediction; //!< Which level of prediction to use
 	bool write_csvs;                        //!< Whether to enable CSV writers from the start for later analysis
 	const char *csv_path;                   //!< Path to write CSVs to

src/xrt/drivers/euroc/euroc_player.cpp

@@ -915,7 +915,7 @@ euroc_player_setup_gui(struct euroc_player *ep)
 	u_var_add_ro_ff_vec3_f32(ep, ep->gyro_ff, "Gyroscope");
 	u_var_add_ro_ff_vec3_f32(ep, ep->accel_ff, "Accelerometer");
 	for (int i = 0; i < ep->dataset.cam_count; i++) {
-		char label[] = "Camera NNNN";
+		char label[] = "Camera NNNNNNNNNN";
 		(void)snprintf(label, sizeof(label), "Camera %d", i);
 		u_var_add_sink_debug(ep, &ep->ui_cam_sinks[i], label);
 	}

src/xrt/include/xrt/xrt_config_have.h.cmake_in

@@ -30,6 +30,7 @@
 #cmakedefine XRT_HAVE_OPENCV
 #cmakedefine XRT_HAVE_OPENGL
 #cmakedefine XRT_HAVE_OPENGLES
+#cmakedefine XRT_HAVE_OPENVR
 #cmakedefine XRT_HAVE_PERCETTO
 #cmakedefine XRT_HAVE_SDL2
 #cmakedefine XRT_HAVE_SYSTEMD