From b14612e48a8526242054b0cc81ab4be6fe2ee813 Mon Sep 17 00:00:00 2001 From: Simon Zeni Date: Wed, 17 Jan 2024 13:34:49 -0500 Subject: [PATCH] d/*: pose correction for VIT system --- src/xrt/drivers/euroc/euroc_device.c | 27 +------------------------ src/xrt/drivers/realsense/rs_hdev.c | 7 ++++--- src/xrt/drivers/rift_s/rift_s_tracker.c | 7 ++++--- src/xrt/drivers/twrap/twrap_slam.c | 7 +++---- src/xrt/drivers/vive/vive_device.c | 6 +++++- src/xrt/drivers/wmr/wmr_hmd.c | 4 +++- 6 files changed, 20 insertions(+), 38 deletions(-) diff --git a/src/xrt/drivers/euroc/euroc_device.c b/src/xrt/drivers/euroc/euroc_device.c index e5c39d743..88006a621 100644 --- a/src/xrt/drivers/euroc/euroc_device.c +++ b/src/xrt/drivers/euroc/euroc_device.c @@ -17,6 +17,7 @@ #include "xrt/xrt_prober.h" #include "xrt/xrt_tracking.h" #include "xrt/xrt_config_have.h" +#include "xrt/xrt_config_build.h" #include "euroc_driver.h" @@ -101,26 +102,6 @@ euroc_device(struct xrt_device *xdev) return (struct euroc_device *)xdev; } -//! Corrections specific for original euroc datasets and Kimera. -//! If your datasets comes from a different camera you should probably -//! use a different pose correction function. -XRT_MAYBE_UNUSED static inline struct xrt_pose -euroc_device_correct_pose_from_kimera(struct xrt_pose pose) -{ - //! @todo Implement proper pose corrections for the original euroc datasets - //! @todo Allow to use different pose corrections depending on the device used to record - return pose; -} - -//! Similar to `euroc_device_correct_pose_from_kimera` but for Basalt. -XRT_MAYBE_UNUSED static inline struct xrt_pose -euroc_device_correct_pose_from_basalt(struct xrt_pose pose) -{ - //! @todo Implement proper pose corrections for the original euroc datasets - //! @todo Allow to use different pose corrections depending on the device used to record - return pose; -} - static void euroc_device_get_tracked_pose(struct xrt_device *xdev, enum xrt_input_name name, @@ -136,13 +117,7 @@ euroc_device_get_tracked_pose(struct xrt_device *xdev, int pose_bits = XRT_SPACE_RELATION_ORIENTATION_TRACKED_BIT | XRT_SPACE_RELATION_POSITION_TRACKED_BIT; bool pose_tracked = out_relation->relation_flags & pose_bits; if (pose_tracked) { -#if defined(XRT_HAVE_KIMERA) - ed->pose = euroc_device_correct_pose_from_kimera(out_relation->pose); -#elif defined(XRT_HAVE_BASALT) - ed->pose = euroc_device_correct_pose_from_basalt(out_relation->pose); -#else ed->pose = out_relation->pose; -#endif } } diff --git a/src/xrt/drivers/realsense/rs_hdev.c b/src/xrt/drivers/realsense/rs_hdev.c index 78248505d..74095b2f3 100644 --- a/src/xrt/drivers/realsense/rs_hdev.c +++ b/src/xrt/drivers/realsense/rs_hdev.c @@ -18,6 +18,7 @@ #include "xrt/xrt_frame.h" #include "xrt/xrt_prober.h" #include "xrt/xrt_config_have.h" +#include "xrt/xrt_config_build.h" #include "math/m_filter_fifo.h" #include "math/m_space.h" @@ -288,9 +289,9 @@ rs_hdev_get_tracked_pose(struct xrt_device *xdev, bool pose_tracked = out_relation->relation_flags & pose_bits; if (pose_tracked) { -#if defined(XRT_HAVE_KIMERA) - rh->pose = rs_hdev_correct_pose_from_kimera(out_relation->pose); -#elif defined(XRT_HAVE_BASALT) +#ifdef XRT_FEATURE_SLAM + // !todo Correct pose depending on the VIT system in use, this should be done in the system itself. + // For now, assume that we are using Basalt. rh->pose = rs_hdev_correct_pose_from_basalt(out_relation->pose); #else rh->pose = out_relation->pose; diff --git a/src/xrt/drivers/rift_s/rift_s_tracker.c b/src/xrt/drivers/rift_s/rift_s_tracker.c index b40147f12..d19ee28aa 100644 --- a/src/xrt/drivers/rift_s/rift_s_tracker.c +++ b/src/xrt/drivers/rift_s/rift_s_tracker.c @@ -36,6 +36,7 @@ #include "util/u_trace_marker.h" #include "util/u_var.h" +#include "xrt/xrt_config_build.h" #include "xrt/xrt_config_drivers.h" #include "xrt/xrt_device.h" @@ -635,11 +636,11 @@ rift_s_tracker_get_tracked_pose(struct rift_s_tracker *t, // Get the IMU pose from the SLAM tracker xrt_tracked_slam_get_tracked_pose(t->tracking.slam, at_timestamp_ns, &imu_relation); - -#if defined(XRT_HAVE_BASALT) +#ifdef XRT_FEATURE_SLAM + // !todo Correct pose depending on the VIT system in use, this should be done in the system itself. + // For now, assume that we are using Basalt. rift_s_tracker_correct_pose_from_basalt(&imu_relation.pose); #endif - imu_relation.relation_flags = (enum xrt_space_relation_flags)( XRT_SPACE_RELATION_ORIENTATION_VALID_BIT | XRT_SPACE_RELATION_POSITION_VALID_BIT | XRT_SPACE_RELATION_ORIENTATION_TRACKED_BIT | XRT_SPACE_RELATION_POSITION_TRACKED_BIT); diff --git a/src/xrt/drivers/twrap/twrap_slam.c b/src/xrt/drivers/twrap/twrap_slam.c index 3257459f3..3407d5b91 100644 --- a/src/xrt/drivers/twrap/twrap_slam.c +++ b/src/xrt/drivers/twrap/twrap_slam.c @@ -14,6 +14,8 @@ #include "xrt/xrt_frameserver.h" #include "xrt/xrt_results.h" #include "xrt/xrt_tracking.h" +#include "xrt/xrt_config_have.h" +#include "xrt/xrt_config_build.h" #include "util/u_var.h" #include "util/u_misc.h" @@ -171,14 +173,11 @@ twrap_slam_create_device(struct xrt_frame_context *xfctx, snprintf(dx->base.serial, XRT_DEVICE_NAME_LEN, "Generic Inside-Out Head Tracker"); #ifdef XRT_FEATURE_SLAM -#ifdef XRT_HAVE_BASALT + // !todo Correct pose depending on the VIT system in use, this should be done in the system itself. // Arrived at mostly by trial and error; seeminly does a 90-degree rotation about the X axis. dx->pre_rotate_x = (struct xrt_vec3){1.0f, 0.0f, 0.0f}; dx->pre_rotate_z = (struct xrt_vec3){0.0f, 1.0f, 0.0f}; dx->pre_rotate = true; -#else -#pragma message "World space conversion not implemented for this SLAM system" -#endif #endif // note: we can't put this at the very end; we need u_tracked_imu_3dof, and that needs to be put on the debug diff --git a/src/xrt/drivers/vive/vive_device.c b/src/xrt/drivers/vive/vive_device.c index c14d2bd7e..13c184de2 100644 --- a/src/xrt/drivers/vive/vive_device.c +++ b/src/xrt/drivers/vive/vive_device.c @@ -34,6 +34,8 @@ #include "vive_protocol.h" #include "vive_source.h" #include "xrt/xrt_tracking.h" +#include "xrt/xrt_config_have.h" +#include "xrt/xrt_config_build.h" // Used to scale the IMU range from config. #define VIVE_IMU_RANGE_CONVERSION_VALUE (32768.0) @@ -156,7 +158,9 @@ vive_device_get_slam_tracked_pose(struct xrt_device *xdev, bool pose_tracked = out_relation->relation_flags & pose_bits; if (pose_tracked) { -#if defined(XRT_HAVE_BASALT) +#ifdef XRT_FEATURE_SLAM + // !todo Correct pose depending on the VIT system in use, this should be done in the system itself. + // For now, assume that we are using Basalt. d->pose = vive_device_correct_pose_from_basalt(out_relation->pose); #else d->pose = out_relation->pose; diff --git a/src/xrt/drivers/wmr/wmr_hmd.c b/src/xrt/drivers/wmr/wmr_hmd.c index cc0a188c3..b78c2e024 100644 --- a/src/xrt/drivers/wmr/wmr_hmd.c +++ b/src/xrt/drivers/wmr/wmr_hmd.c @@ -1145,7 +1145,9 @@ wmr_hmd_get_slam_tracked_pose(struct xrt_device *xdev, bool pose_tracked = out_relation->relation_flags & pose_bits; if (pose_tracked) { -#if defined(XRT_HAVE_BASALT) +#ifdef XRT_FEATURE_SLAM + // !todo Correct pose depending on the VIT system in use, this should be done in the system itself. + // For now, assume that we are using Basalt. wh->pose = wmr_hmd_correct_pose_from_basalt(out_relation->pose); #else wh->pose = out_relation->pose;