The open source OpenXR runtime
d/twrap: Add tracking wrapper driver
+267
+3
CMakeLists.txt
+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
+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
+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
+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
+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)