The open source OpenXR runtime
0
fork

Configure Feed

Select the types of activity you want to include in your feed.

d/twrap: Add tracking wrapper driver

+267
+3
CMakeLists.txt
··· 283 283 option_with_deps(XRT_BUILD_DRIVER_DEPTHAI "DepthAI" DEPENDS depthai_FOUND) 284 284 option_with_deps(XRT_BUILD_DRIVER_EUROC "Enable EuRoC dataset driver for SLAM evaluation" DEPENDS XRT_HAVE_OPENCV) 285 285 option_with_deps(XRT_BUILD_DRIVER_HANDTRACKING "Enable Camera Hand Tracking driver" DEPENDS XRT_HAVE_ONNXRUNTIME XRT_HAVE_OPENCV XRT_HAVE_V4L2) 286 + option_with_deps(XRT_BUILD_DRIVER_TWRAP "Enable Tracking Wrapper drivers" ON) # only depends on imu 286 287 option_with_deps(XRT_BUILD_DRIVER_HDK "Enable HDK driver" DEPENDS XRT_HAVE_INTERNAL_HID) 287 288 option_with_deps(XRT_BUILD_DRIVER_HYDRA "Enable Hydra driver" DEPENDS XRT_HAVE_INTERNAL_HID) 288 289 option_with_deps(XRT_BUILD_DRIVER_ILLIXR "Enable ILLIXR driver" DEPENDS ILLIXR_PATH) ··· 356 357 "WMR" 357 358 "EUROC" 358 359 "SIMULAVR" 360 + "TWRAP" 359 361 ) 360 362 361 363 # Package name needs to be known by the native code itself. ··· 518 520 message(STATUS "# DRIVER_REMOTE: ${XRT_BUILD_DRIVER_REMOTE}") 519 521 message(STATUS "# DRIVER_SIMULATED: ${XRT_BUILD_DRIVER_SIMULATED}") 520 522 message(STATUS "# DRIVER_SIMULAVR: ${XRT_BUILD_DRIVER_SIMULAVR}") 523 + message(STATUS "# DRIVER_TWRAP: ${XRT_BUILD_DRIVER_TWRAP}") 521 524 message(STATUS "# DRIVER_SURVIVE: ${XRT_BUILD_DRIVER_SURVIVE}") 522 525 message(STATUS "# DRIVER_ULV2: ${XRT_BUILD_DRIVER_ULV2}") 523 526 message(STATUS "# DRIVER_VF: ${XRT_BUILD_DRIVER_VF}")
+5
src/xrt/drivers/CMakeLists.txt
··· 58 58 list(APPEND ENABLED_HEADSET_DRIVERS simulated) 59 59 endif() 60 60 61 + if(XRT_BUILD_DRIVER_TWRAP) 62 + add_library(drv_twrap STATIC twrap/twrap_slam.c twrap/twrap_interface.h) 63 + target_link_libraries(drv_twrap PRIVATE xrt-interfaces aux_util) 64 + endif() 65 + 61 66 if(XRT_BUILD_DRIVER_QWERTY) 62 67 add_library( 63 68 drv_qwerty STATIC
+21
src/xrt/drivers/twrap/twrap_interface.h
··· 1 + // Copyright 2022, Collabora, Ltd. 2 + // SPDX-License-Identifier: BSL-1.0 3 + /*! 4 + * @file 5 + * @brief Tiny xrt_device exposing SLAM capabilities. 6 + * @author Moses Turner <moses@collabora.com> 7 + * @ingroup drv_twrap 8 + */ 9 + 10 + #pragma once 11 + 12 + #include "xrt/xrt_defines.h" 13 + #include "xrt/xrt_frameserver.h" 14 + #include "xrt/xrt_tracking.h" 15 + 16 + 17 + xrt_result_t 18 + twrap_slam_create_device(struct xrt_frame_context *xfctx, 19 + enum xrt_device_name name, 20 + struct xrt_slam_sinks **out_sinks, 21 + struct xrt_device **out_device);
+232
src/xrt/drivers/twrap/twrap_slam.c
··· 1 + // Copyright 2022, Collabora, Ltd. 2 + // SPDX-License-Identifier: BSL-1.0 3 + /*! 4 + * @file 5 + * @brief Tiny xrt_device exposing SLAM capabilities. 6 + * @author Moses Turner <moses@collabora.com> 7 + * @ingroup drv_twrap 8 + */ 9 + 10 + #include "math/m_vec3.h" 11 + 12 + #include "util/u_sink.h" 13 + #include "xrt/xrt_defines.h" 14 + #include "xrt/xrt_frameserver.h" 15 + #include "xrt/xrt_results.h" 16 + #include "xrt/xrt_tracking.h" 17 + 18 + #include "util/u_var.h" 19 + #include "util/u_misc.h" 20 + #include "util/u_debug.h" 21 + #include "util/u_logging.h" 22 + #include "util/u_trace_marker.h" 23 + #include "math/m_api.h" 24 + 25 + #include "tracking/t_tracking.h" 26 + 27 + #include <assert.h> 28 + 29 + #include "util/u_device.h" 30 + #include "math/m_space.h" 31 + #include "util/u_tracked_imu_3dof.h" 32 + 33 + 34 + /* 35 + * 36 + * Printing functions. 37 + * 38 + */ 39 + 40 + #define SLAM_TRACE(d, ...) U_LOG_IFL_T(d->log_level, __VA_ARGS__) 41 + #define SLAM_DEBUG(d, ...) U_LOG_IFL_D(d->log_level, __VA_ARGS__) 42 + #define SLAM_INFO(d, ...) U_LOG_IFL_I(d->log_level, __VA_ARGS__) 43 + #define SLAM_WARN(d, ...) U_LOG_IFL_W(d->log_level, __VA_ARGS__) 44 + #define SLAM_ERROR(d, ...) U_LOG_IFL_E(d->log_level, __VA_ARGS__) 45 + 46 + DEBUG_GET_ONCE_LOG_OPTION(slam_log, "SLAM_LOG", U_LOGGING_INFO) 47 + 48 + struct slam_device 49 + { 50 + struct xrt_device base; 51 + 52 + enum u_logging_level log_level; 53 + 54 + struct xrt_vec3 pre_rotate_x; 55 + struct xrt_vec3 pre_rotate_z; 56 + 57 + bool pre_rotate; 58 + bool use_3dof; 59 + #ifdef XRT_FEATURE_SLAM 60 + // We do not own this; this gets freed after us when devices on the frame context get freed 61 + struct xrt_tracked_slam *slam; 62 + // Ditto 63 + #endif 64 + struct u_tracked_imu_3dof *dof3; 65 + }; 66 + 67 + static inline struct slam_device * 68 + slam_device(struct xrt_device *xdev) 69 + { 70 + return (struct slam_device *)xdev; 71 + } 72 + 73 + static void 74 + twrap_slam_update_inputs(struct xrt_device *xdev) 75 + { 76 + // Empty 77 + } 78 + 79 + static void 80 + twrap_slam_get_tracked_pose(struct xrt_device *xdev, 81 + enum xrt_input_name name, 82 + uint64_t at_timestamp_ns, 83 + struct xrt_space_relation *out_relation) 84 + { 85 + struct slam_device *dx = slam_device(xdev); 86 + 87 + if (name != XRT_INPUT_GENERIC_TRACKER_POSE) { 88 + SLAM_ERROR(dx, "unknown input name %d", name); 89 + return; 90 + } 91 + #ifdef XRT_FEATURE_SLAM 92 + if (!dx->use_3dof) { 93 + 94 + struct xrt_space_relation basalt_rel = {0}; 95 + 96 + 97 + xrt_tracked_slam_get_tracked_pose(dx->slam, at_timestamp_ns, &basalt_rel); 98 + 99 + int pose_bits = XRT_SPACE_RELATION_ORIENTATION_TRACKED_BIT | XRT_SPACE_RELATION_POSITION_TRACKED_BIT; 100 + bool pose_tracked = basalt_rel.relation_flags & pose_bits; 101 + 102 + if (!pose_tracked) { 103 + U_ZERO(&out_relation->relation_flags); 104 + return; 105 + } 106 + 107 + struct xrt_relation_chain xrc = {0}; 108 + 109 + m_relation_chain_push_relation(&xrc, &basalt_rel); 110 + 111 + 112 + 113 + struct xrt_pose pre = {0}; 114 + math_quat_from_plus_x_z(&dx->pre_rotate_x, &dx->pre_rotate_z, &pre.orientation); 115 + m_relation_chain_push_pose(&xrc, &pre); 116 + 117 + m_relation_chain_resolve(&xrc, out_relation); 118 + return; 119 + } 120 + 121 + #endif 122 + m_relation_history_get(dx->dof3->rh, at_timestamp_ns, out_relation); 123 + } 124 + 125 + static void 126 + twrap_slam_get_view_poses(struct xrt_device *xdev, 127 + const struct xrt_vec3 *default_eye_relation, 128 + uint64_t at_timestamp_ns, 129 + uint32_t view_count, 130 + struct xrt_space_relation *out_head_relation, 131 + struct xrt_fov *out_fovs, 132 + struct xrt_pose *out_poses) 133 + { 134 + assert(false); 135 + } 136 + 137 + static void 138 + twrap_slam_destroy(struct xrt_device *xdev) 139 + { 140 + struct slam_device *dx = slam_device(xdev); 141 + 142 + 143 + u_var_remove_root(dx); 144 + u_device_free(&dx->base); 145 + } 146 + 147 + // Does _NOT_ take ownership or free the xfctx 148 + xrt_result_t 149 + twrap_slam_create_device(struct xrt_frame_context *xfctx, 150 + enum xrt_device_name name, 151 + struct xrt_slam_sinks **out_sinks, 152 + struct xrt_device **out_device) 153 + { 154 + struct slam_device *dx = U_DEVICE_ALLOCATE(struct slam_device, U_DEVICE_ALLOC_TRACKING_NONE, 1, 0); 155 + 156 + 157 + dx->log_level = debug_get_log_option_slam_log(); 158 + 159 + 160 + 161 + dx->base.update_inputs = twrap_slam_update_inputs; 162 + dx->base.get_tracked_pose = twrap_slam_get_tracked_pose; 163 + dx->base.get_view_poses = twrap_slam_get_view_poses; 164 + dx->base.destroy = twrap_slam_destroy; 165 + dx->base.name = name; 166 + dx->base.tracking_origin->type = XRT_TRACKING_TYPE_OTHER; 167 + dx->base.tracking_origin->offset = (struct xrt_pose)XRT_POSE_IDENTITY; 168 + dx->base.inputs[0].name = XRT_INPUT_GENERIC_TRACKER_POSE; 169 + dx->base.orientation_tracking_supported = true; 170 + dx->base.position_tracking_supported = true; 171 + dx->base.device_type = XRT_DEVICE_TYPE_GENERIC_TRACKER; 172 + 173 + 174 + 175 + // Print name. 176 + snprintf(dx->base.str, XRT_DEVICE_NAME_LEN, "Generic Inside-Out Head Tracker"); 177 + snprintf(dx->base.serial, XRT_DEVICE_NAME_LEN, "Generic Inside-Out Head Tracker"); 178 + 179 + #ifdef XRT_FEATURE_SLAM 180 + #ifdef XRT_HAVE_BASALT_SLAM 181 + // Arrived at mostly by trial and error; seeminly does a 90-degree rotation about the X axis. 182 + dx->pre_rotate_x = (struct xrt_vec3){1.0f, 0.0f, 0.0f}; 183 + dx->pre_rotate_z = (struct xrt_vec3){0.0f, 1.0f, 0.0f}; 184 + dx->pre_rotate = true; 185 + #else 186 + #pragma message "World space conversion not implemented for this SLAM system" 187 + #endif 188 + #endif 189 + 190 + // 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 191 + // gui before we link our imu pipeline to it. 192 + u_var_add_root(dx, "Generic Inside-Out Head Tracker", 0); 193 + 194 + u_var_add_vec3_f32(dx, &dx->pre_rotate_x, "pre_rotate_x"); 195 + u_var_add_vec3_f32(dx, &dx->pre_rotate_z, "pre_rotate_z"); 196 + u_var_add_bool(dx, &dx->pre_rotate, "pre_rotate"); 197 + u_var_add_bool(dx, &dx->use_3dof, "Use 3DOF tracking instead of SLAM"); 198 + 199 + // At the end so that it doesn't clutter up the UI 200 + u_tracked_imu_3dof_create(xfctx, &dx->dof3, dx); 201 + 202 + #ifdef XRT_FEATURE_SLAM 203 + int create_status = t_slam_create(xfctx, NULL, &dx->slam, out_sinks); 204 + 205 + if (create_status != 0 || dx->slam == NULL) { 206 + twrap_slam_destroy(&dx->base); 207 + return XRT_ERROR_DEVICE_CREATION_FAILED; 208 + } 209 + 210 + // Create a split sink at out_sink that pushes to the SLAM IMU sink as well as the 3dof IMU sink, then replace 211 + // out_sinks's imu sink with the split sink. 212 + 213 + struct xrt_imu_sink *sink_slam = (*out_sinks)->imu; 214 + 215 + struct xrt_imu_sink *tmp = NULL; 216 + 217 + u_imu_sink_split_create(xfctx, &dx->dof3->sink, sink_slam, &tmp); 218 + u_imu_sink_force_monotonic_create(xfctx, tmp, &tmp); 219 + 220 + (*out_sinks)->imu = tmp; 221 + 222 + int start_status = t_slam_start(dx->slam); 223 + 224 + if (start_status != 0) { 225 + twrap_slam_destroy(&dx->base); 226 + return XRT_ERROR_DEVICE_CREATION_FAILED; 227 + } 228 + #endif 229 + 230 + *out_device = &dx->base; 231 + return XRT_SUCCESS; 232 + }
+6
src/xrt/targets/common/CMakeLists.txt
··· 45 45 target_link_libraries(target_lists PRIVATE drv_arduino) 46 46 endif() 47 47 48 + target_link_libraries(target_lists PRIVATE drv_cemu) 49 + 48 50 if(XRT_BUILD_DRIVER_DAYDREAM) 49 51 target_link_libraries(target_lists PRIVATE drv_daydream) 50 52 endif() ··· 67 69 68 70 if(XRT_BUILD_DRIVER_NS) 69 71 target_link_libraries(target_lists PRIVATE drv_ns) 72 + endif() 73 + 74 + if(XRT_BUILD_DRIVER_TWRAP) 75 + target_link_libraries(target_lists PRIVATE drv_twrap) 70 76 endif() 71 77 72 78 if(XRT_BUILD_DRIVER_ULV2)