From be55dec119fe1c25ea7fedb6b7fa328f99ffc03d Mon Sep 17 00:00:00 2001 From: Mario Kleiner Date: Fri, 31 May 2024 03:51:25 +0100 Subject: [PATCH] d/twrap: Correct axis assignments for poses provided by basalt slam. Testing with a Luxonis Oak-D Pro with basalt slam tracking on a simulated Northstar suggests the returned axis from basalt vit are wrong wrt. to the OpenXR reference frame. Invert assigned y and z axis, similar to what is done for poses returned by basalt slam for other HMD drivers, e.g., the drivers for Rift-S, Valve Index / Vive, WMR and Realsense, in various *_correct_pose_from_basalt() functions. While we are at it, also make sure the dx->pre_rotate enable flag and debug UI checkbox actually has an effect on operation. Signed-off-by: Mario Kleiner Part-of: --- src/xrt/drivers/twrap/twrap_slam.c | 20 +++++++++++++++++--- 1 file changed, 17 insertions(+), 3 deletions(-) diff --git a/src/xrt/drivers/twrap/twrap_slam.c b/src/xrt/drivers/twrap/twrap_slam.c index 3407d5b91..4bd31a100 100644 --- a/src/xrt/drivers/twrap/twrap_slam.c +++ b/src/xrt/drivers/twrap/twrap_slam.c @@ -72,6 +72,16 @@ slam_device(struct xrt_device *xdev) return (struct slam_device *)xdev; } +static struct xrt_pose +twrap_hmd_correct_pose_from_basalt(struct xrt_pose pose) +{ + pose.position.y = -pose.position.y; + pose.position.z = -pose.position.z; + pose.orientation.y = -pose.orientation.y; + pose.orientation.z = -pose.orientation.z; + return pose; +} + static void twrap_slam_get_tracked_pose(struct xrt_device *xdev, enum xrt_input_name name, @@ -100,15 +110,19 @@ twrap_slam_get_tracked_pose(struct xrt_device *xdev, return; } + basalt_rel.pose = twrap_hmd_correct_pose_from_basalt(basalt_rel.pose); + struct xrt_relation_chain xrc = {0}; m_relation_chain_push_relation(&xrc, &basalt_rel); - struct xrt_pose pre = {0}; - math_quat_from_plus_x_z(&dx->pre_rotate_x, &dx->pre_rotate_z, &pre.orientation); - m_relation_chain_push_pose(&xrc, &pre); + if (dx->pre_rotate) { + struct xrt_pose pre = {0}; + math_quat_from_plus_x_z(&dx->pre_rotate_x, &dx->pre_rotate_z, &pre.orientation); + m_relation_chain_push_pose(&xrc, &pre); + } m_relation_chain_resolve(&xrc, out_relation); return;