matrixfurry.com
The open source OpenXR runtime
1/*
2 * Copyright 2013, Fredrik Hultin.
3 * Copyright 2013, Jakob Bornecrantz.
4 * Copyright 2016 Philipp Zabel
5 * Copyright 2019-2022 Jan Schmidt
6 * Copyright 2023, Collabora, Ltd.
7 * SPDX-License-Identifier: BSL-1.0
8 *
9 */
10/*!
11 * @file
12 * @brief Driver code for Oculus Rift S headsets
13 *
14 * Implementation for the HMD 3dof and 6dof tracking
15 *
16 * @author Jan Schmidt <jan@centricular.com>
17 * @ingroup drv_rift_s
18 */
19#include <stdlib.h>
20#include <string.h>
21#include <stdio.h>
22#include <time.h>
23#include <assert.h>
24
25#include "math/m_api.h"
26#include "math/m_clock_tracking.h"
27#include "math/m_space.h"
28#include "math/m_vec3.h"
29
30#include "os/os_time.h"
31
32#include "util/u_debug.h"
33#include "util/u_device.h"
34#include "util/u_sink.h"
35#include "util/u_trace_marker.h"
36#include "util/u_var.h"
37
38#include "xrt/xrt_config_build.h"
39#include "xrt/xrt_config_drivers.h"
40#include "xrt/xrt_device.h"
41
42#ifdef XRT_BUILD_DRIVER_HANDTRACKING
43#include "../drivers/ht/ht_interface.h"
44#include "../multi_wrapper/multi.h"
45#endif
46
47#include "rift_s.h"
48#include "rift_s_interface.h"
49#include "rift_s_util.h"
50#include "rift_s_tracker.h"
51
52#ifdef XRT_FEATURE_SLAM
53static const bool slam_supported = true;
54#else
55static const bool slam_supported = false;
56#endif
57
58#ifdef XRT_BUILD_DRIVER_HANDTRACKING
59static const bool hand_supported = true;
60#else
61static const bool hand_supported = false;
62#endif
63
64//! Specifies whether the user wants to use a SLAM tracker.
65DEBUG_GET_ONCE_BOOL_OPTION(rift_s_slam, "RIFT_S_SLAM", true)
66
67//! Specifies whether the user wants to use the hand tracker.
68DEBUG_GET_ONCE_BOOL_OPTION(rift_s_handtracking, "RIFT_S_HANDTRACKING", true)
69
70static xrt_result_t
71rift_s_tracker_get_tracked_pose_imu(struct xrt_device *xdev,
72 enum xrt_input_name name,
73 int64_t at_timestamp_ns,
74 struct xrt_space_relation *out_relation);
75
76static void
77rift_s_tracker_switch_method_cb(void *t_ptr)
78{
79 DRV_TRACE_MARKER();
80
81 struct rift_s_tracker *t = t_ptr;
82 t->slam_over_3dof = !t->slam_over_3dof;
83 struct u_var_button *btn = &t->gui.switch_tracker_btn;
84
85 if (t->slam_over_3dof) { // Use SLAM
86 snprintf(btn->label, sizeof(btn->label), "Switch to 3DoF Tracking");
87 } else { // Use 3DoF
88 snprintf(btn->label, sizeof(btn->label), "Switch to SLAM Tracking");
89
90 os_mutex_lock(&t->mutex);
91 m_imu_3dof_reset(&t->fusion.i3dof);
92 t->fusion.i3dof.rot = t->pose.orientation;
93 os_mutex_unlock(&t->mutex);
94 }
95}
96
97XRT_MAYBE_UNUSED void
98rift_s_fill_slam_imu_calibration(struct rift_s_tracker *t, struct rift_s_hmd_config *hmd_config)
99{
100 /* FIXME: Validate these hard coded standard deviations against
101 * some actual at-rest IMU measurements */
102 const double a_bias_std = 0.001;
103 const double a_noise_std = 0.016;
104
105 const double g_bias_std = 0.0001;
106 const double g_noise_std = 0.000282;
107
108 /* we pass already corrected accel and gyro
109 * readings to Basalt, so the transforms and
110 * offsets are just identity / zero matrices */
111 struct t_imu_calibration imu_calib = {
112 .accel =
113 {
114 .transform =
115 {
116 {1.0, 0.0, 0.0},
117 {0.0, 1.0, 0.0},
118 {0.0, 0.0, 1.0},
119 },
120 .offset =
121 {
122 0,
123 },
124 .bias_std = {a_bias_std, a_bias_std, a_bias_std},
125 .noise_std = {a_noise_std, a_noise_std, a_noise_std},
126 },
127 .gyro =
128 {
129 .transform =
130 {
131 {1.0, 0.0, 0.0},
132 {0.0, 1.0, 0.0},
133 {0.0, 0.0, 1.0},
134 },
135 .offset =
136 {
137 0,
138 },
139 .bias_std = {g_bias_std, g_bias_std, g_bias_std},
140 .noise_std = {g_noise_std, g_noise_std, g_noise_std},
141 },
142 };
143
144 struct t_slam_imu_calibration calib = {
145 .base = imu_calib,
146 .frequency = hmd_config->imu_config_info.imu_hz,
147 };
148
149 t->slam_calib.imu = calib;
150}
151
152//! Extended camera calibration for SLAM
153static void
154rift_s_fill_slam_cameras_calibration(struct rift_s_tracker *t, struct rift_s_hmd_config *hmd_config)
155{
156 /* SLAM frames are every 2nd frame of 60Hz camera feed */
157 const int CAMERA_FREQUENCY = 30;
158
159 struct rift_s_camera_calibration_block *camera_calibration = &hmd_config->camera_calibration;
160
161 /* Compute the IMU from cam transform for each cam */
162 struct xrt_pose device_from_imu, imu_from_device;
163 math_pose_from_isometry(&hmd_config->imu_calibration.device_from_imu, &device_from_imu);
164 math_pose_invert(&device_from_imu, &imu_from_device);
165
166 t->slam_calib.cam_count = RIFT_S_CAMERA_COUNT;
167 for (int i = 0; i < RIFT_S_CAMERA_COUNT; i++) {
168 enum rift_s_camera_id cam_id = CAM_IDX_TO_ID[i];
169 struct rift_s_camera_calibration *cam = &camera_calibration->cameras[cam_id];
170
171 struct xrt_pose device_from_cam;
172 math_pose_from_isometry(&cam->device_from_camera, &device_from_cam);
173
174 struct xrt_pose P_imu_cam;
175 math_pose_transform(&imu_from_device, &device_from_cam, &P_imu_cam);
176
177 struct xrt_matrix_4x4 T_imu_cam;
178 math_matrix_4x4_isometry_from_pose(&P_imu_cam, &T_imu_cam);
179
180 RIFT_S_DEBUG("IMU cam%d cam pose %f %f %f orient %f %f %f %f", i, P_imu_cam.position.x,
181 P_imu_cam.position.y, P_imu_cam.position.z, P_imu_cam.orientation.x,
182 P_imu_cam.orientation.y, P_imu_cam.orientation.z, P_imu_cam.orientation.w);
183
184 struct t_slam_camera_calibration calib = {
185 .base = rift_s_get_cam_calib(&hmd_config->camera_calibration, cam_id),
186 .frequency = CAMERA_FREQUENCY,
187 .T_imu_cam = T_imu_cam,
188 };
189 t->slam_calib.cams[i] = calib;
190 }
191}
192
193static void
194rift_s_fill_slam_calibration(struct rift_s_tracker *t, struct rift_s_hmd_config *hmd_config)
195{
196 rift_s_fill_slam_imu_calibration(t, hmd_config);
197 rift_s_fill_slam_cameras_calibration(t, hmd_config);
198}
199
200static struct xrt_slam_sinks *
201rift_s_create_slam_tracker(struct rift_s_tracker *t, struct xrt_frame_context *xfctx)
202{
203 DRV_TRACE_MARKER();
204
205 struct xrt_slam_sinks *sinks = NULL;
206
207#ifdef XRT_FEATURE_SLAM
208 struct t_slam_tracker_config config = {0};
209 t_slam_fill_default_config(&config);
210
211 /* No need to refcount these parameters */
212 config.cam_count = RIFT_S_CAMERA_COUNT;
213 config.slam_calib = &t->slam_calib;
214
215 int create_status = t_slam_create(xfctx, &config, &t->tracking.slam, &sinks);
216 if (create_status != 0) {
217 return NULL;
218 }
219
220 int start_status = t_slam_start(t->tracking.slam);
221 if (start_status != 0) {
222 return NULL;
223 }
224
225 RIFT_S_DEBUG("Rift S SLAM tracker successfully started");
226#endif
227
228 return sinks;
229}
230
231static int
232rift_s_create_hand_tracker(struct rift_s_tracker *t,
233 struct xrt_frame_context *xfctx,
234 struct xrt_hand_masks_sink *masks_sink,
235 struct xrt_slam_sinks **out_sinks,
236 struct xrt_device **out_device)
237{
238 DRV_TRACE_MARKER();
239
240 struct xrt_slam_sinks *sinks = NULL;
241 struct xrt_device *device = NULL;
242
243#ifdef XRT_BUILD_DRIVER_HANDTRACKING
244
245 //!@todo What's a sensible boundary for Rift S?
246 struct t_camera_extra_info extra_camera_info = {
247 0,
248 };
249 extra_camera_info.views[0].boundary_type = HT_IMAGE_BOUNDARY_NONE;
250 extra_camera_info.views[1].boundary_type = HT_IMAGE_BOUNDARY_NONE;
251
252 extra_camera_info.views[0].camera_orientation = CAMERA_ORIENTATION_90;
253 extra_camera_info.views[1].camera_orientation = CAMERA_ORIENTATION_90;
254
255 struct t_hand_tracking_create_info create_info = {.cams_info = extra_camera_info, .masks_sink = masks_sink};
256
257 int create_status = ht_device_create(xfctx, //
258 t->stereo_calib, //
259 create_info, //
260 &sinks, //
261 &device);
262 if (create_status != 0) {
263 return create_status;
264 }
265
266 if (device != NULL) {
267 // Attach tracking override that links hand pose to the SLAM tracked position
268 // The hand poses need to be rotated 90° because of the way we passed
269 // the stereo camera configuration to the hand tracker.
270 struct xrt_pose left_cam_rotated_from_imu;
271 struct xrt_pose cam_rotate = {.orientation = {.x = 1.0, .y = 0.0, .z = 0.0, .w = 0.0},
272 .position = {0, 0, 0}};
273 math_pose_transform(&cam_rotate, &t->left_cam_from_imu, &left_cam_rotated_from_imu);
274
275 device = multi_create_tracking_override(XRT_TRACKING_OVERRIDE_ATTACHED, device, &t->base,
276 XRT_INPUT_GENERIC_TRACKER_POSE, &left_cam_rotated_from_imu);
277 }
278
279 RIFT_S_DEBUG("Rift S HMD hand tracker successfully created");
280#endif
281
282 *out_sinks = sinks;
283 *out_device = device;
284
285 return 0;
286}
287
288void
289rift_s_tracker_add_debug_ui(struct rift_s_tracker *t, void *root)
290{
291 u_var_add_gui_header(root, NULL, "Tracking");
292
293 if (t->tracking.slam_enabled) {
294 t->gui.switch_tracker_btn.cb = rift_s_tracker_switch_method_cb;
295 t->gui.switch_tracker_btn.ptr = t;
296 u_var_add_button(root, &t->gui.switch_tracker_btn, "Switch to 3DoF Tracking");
297 }
298
299 u_var_add_pose(root, &t->pose, "Tracked Pose");
300
301 u_var_add_gui_header(root, NULL, "3DoF Tracking");
302 m_imu_3dof_add_vars(&t->fusion.i3dof, root, "");
303
304 u_var_add_gui_header(root, NULL, "SLAM Tracking");
305 u_var_add_ro_text(root, t->gui.slam_status, "Tracker status");
306
307 u_var_add_gui_header(root, NULL, "Hand Tracking");
308 u_var_add_ro_text(root, t->gui.hand_status, "Tracker status");
309}
310
311/*!
312 * Procedure to setup trackers: 3dof, SLAM and hand tracking.
313 *
314 * Determines which trackers to initialize
315 *
316 * @param xfctx the frame server that will own processing nodes
317 * @param hmd_config HMD configuration and firmware info
318 *
319 * @return initialised tracker on success, NULL if creation fails
320 */
321struct rift_s_tracker *
322rift_s_tracker_create(struct xrt_tracking_origin *origin,
323 struct xrt_frame_context *xfctx,
324 struct rift_s_hmd_config *hmd_config)
325{
326 struct rift_s_tracker *t = U_DEVICE_ALLOCATE(struct rift_s_tracker, U_DEVICE_ALLOC_TRACKING_NONE, 1, 0);
327 if (t == NULL) {
328 return NULL;
329 }
330
331 t->base.tracking_origin = origin;
332 t->base.get_tracked_pose = rift_s_tracker_get_tracked_pose_imu;
333
334 // Pose / state lock
335 int ret = os_mutex_init(&t->mutex);
336 if (ret != 0) {
337 RIFT_S_ERROR("Failed to init mutex!");
338 rift_s_tracker_destroy(t);
339 return NULL;
340 }
341
342 // Compute IMU and camera device poses for get_tracked_pose relations
343 math_pose_from_isometry(&hmd_config->imu_calibration.device_from_imu, &t->device_from_imu);
344
345 struct xrt_pose device_from_left_cam;
346 struct rift_s_camera_calibration *left_cam = &hmd_config->camera_calibration.cameras[RIFT_S_CAMERA_FRONT_LEFT];
347 math_pose_from_isometry(&left_cam->device_from_camera, &device_from_left_cam);
348
349 struct xrt_pose left_cam_from_device;
350 math_pose_invert(&device_from_left_cam, &left_cam_from_device);
351 math_pose_transform(&left_cam_from_device, &t->device_from_imu, &t->left_cam_from_imu);
352
353 // Decide whether to initialize the SLAM tracker
354 bool slam_wanted = debug_get_bool_option_rift_s_slam();
355 bool slam_enabled = slam_supported && slam_wanted;
356
357 // Decide whether to initialize the hand tracker
358 bool hand_wanted = debug_get_bool_option_rift_s_handtracking();
359 bool hand_enabled = hand_supported && hand_wanted;
360
361 t->tracking.slam_enabled = slam_enabled;
362 t->tracking.hand_enabled = hand_enabled;
363
364 t->slam_over_3dof = slam_enabled; // We prefer SLAM over 3dof tracking if possible
365
366 const char *slam_status = t->tracking.slam_enabled ? "Enabled"
367 : !slam_wanted ? "Disabled by the user (envvar set to false)"
368 : !slam_supported ? "Unavailable (not built)"
369 : NULL;
370
371 const char *hand_status = t->tracking.hand_enabled ? "Enabled"
372 : !hand_wanted ? "Disabled by the user (envvar set to false)"
373 : !hand_supported ? "Unavailable (not built)"
374 : NULL;
375
376 assert(slam_status != NULL && hand_status != NULL);
377
378 (void)snprintf(t->gui.slam_status, sizeof(t->gui.slam_status), "%s", slam_status);
379 (void)snprintf(t->gui.hand_status, sizeof(t->gui.hand_status), "%s", hand_status);
380
381 // Initialize 3DoF tracker
382 m_imu_3dof_init(&t->fusion.i3dof, M_IMU_3DOF_USE_GRAVITY_DUR_20MS);
383
384 t->pose.orientation.w = 1.0f; // All other values set to zero by U_DEVICE_ALLOCATE (which calls U_CALLOC)
385
386 // Construct the stereo camera calibration for the front cameras
387 t->stereo_calib = rift_s_create_stereo_camera_calib_rotated(&hmd_config->camera_calibration);
388 rift_s_fill_slam_calibration(t, hmd_config);
389
390 // Initialize the input sinks for the camera to send to
391
392 // Initialize SLAM tracker
393 struct xrt_slam_sinks *slam_sinks = NULL;
394 if (t->tracking.slam_enabled) {
395 slam_sinks = rift_s_create_slam_tracker(t, xfctx);
396 if (slam_sinks == NULL) {
397 RIFT_S_WARN("Unable to setup the SLAM tracker");
398 rift_s_tracker_destroy(t);
399 return NULL;
400 }
401 }
402
403 // Initialize hand tracker
404 struct xrt_slam_sinks *hand_sinks = NULL;
405 struct xrt_device *hand_device = NULL;
406 struct xrt_hand_masks_sink *masks_sink = slam_sinks ? slam_sinks->hand_masks : NULL;
407 if (t->tracking.hand_enabled) {
408 int hand_status = rift_s_create_hand_tracker(t, xfctx, masks_sink, &hand_sinks, &hand_device);
409 if (hand_status != 0 || hand_sinks == NULL || hand_device == NULL) {
410 RIFT_S_WARN("Unable to setup the hand tracker");
411 rift_s_tracker_destroy(t);
412 return NULL;
413 }
414 }
415
416 // Setup sinks depending on tracking configuration
417 struct xrt_slam_sinks entry_sinks = {0};
418 if (slam_enabled && hand_enabled) {
419 struct xrt_frame_sink *entry_cam0_sink = NULL;
420 struct xrt_frame_sink *entry_cam1_sink = NULL;
421
422 u_sink_split_create(xfctx, slam_sinks->cams[0], hand_sinks->cams[0], &entry_cam0_sink);
423 u_sink_split_create(xfctx, slam_sinks->cams[1], hand_sinks->cams[1], &entry_cam1_sink);
424
425 entry_sinks = *slam_sinks;
426 entry_sinks.cams[0] = entry_cam0_sink;
427 entry_sinks.cams[1] = entry_cam1_sink;
428 } else if (slam_enabled) {
429 entry_sinks = *slam_sinks;
430 } else if (hand_enabled) {
431 entry_sinks = *hand_sinks;
432 } else {
433 entry_sinks = (struct xrt_slam_sinks){0};
434 }
435
436 t->slam_sinks = entry_sinks;
437 t->handtracker = hand_device;
438
439 return t;
440}
441
442void
443rift_s_tracker_destroy(struct rift_s_tracker *t)
444{
445 t_stereo_camera_calibration_reference(&t->stereo_calib, NULL);
446
447 m_imu_3dof_close(&t->fusion.i3dof);
448 os_mutex_destroy(&t->mutex);
449}
450
451struct xrt_slam_sinks *
452rift_s_tracker_get_slam_sinks(struct rift_s_tracker *t)
453{
454 return &t->in_slam_sinks;
455}
456
457struct xrt_device *
458rift_s_tracker_get_hand_tracking_device(struct rift_s_tracker *t)
459{
460 return t->handtracker;
461}
462
463void
464rift_s_tracker_clock_update(struct rift_s_tracker *t, uint64_t device_timestamp_ns, timepoint_ns local_timestamp_ns)
465{
466 os_mutex_lock(&t->mutex);
467 time_duration_ns last_hw2mono = t->hw2mono;
468 const float freq = 250.0;
469
470 t->seen_clock_observations++;
471 if (t->seen_clock_observations < 100)
472 goto done;
473
474 m_clock_offset_a2b(freq, device_timestamp_ns, local_timestamp_ns, &t->hw2mono);
475
476 if (!t->have_hw2mono) {
477 time_duration_ns change_ns = last_hw2mono - t->hw2mono;
478 if (change_ns >= -U_TIME_HALF_MS_IN_NS && change_ns <= U_TIME_HALF_MS_IN_NS) {
479 RIFT_S_INFO("HMD device to local clock map stabilised");
480 t->have_hw2mono = true;
481 }
482 }
483done:
484 os_mutex_unlock(&t->mutex);
485}
486
487//! Camera specific logic for clock conversion
488static void
489clock_hw2mono_get(struct rift_s_tracker *t, uint64_t device_ts, timepoint_ns *out)
490{
491 *out = t->hw2mono + device_ts;
492}
493
494void
495rift_s_tracker_imu_update(struct rift_s_tracker *t,
496 uint64_t device_timestamp_ns,
497 const struct xrt_vec3 *accel,
498 const struct xrt_vec3 *gyro)
499{
500 os_mutex_lock(&t->mutex);
501
502 /* Ignore packets before we're ready and clock is stable */
503 if (!t->ready_for_data || !t->have_hw2mono) {
504 os_mutex_unlock(&t->mutex);
505 return;
506 }
507
508 /* Get the smoothed monotonic time estimate for this IMU sample */
509 timepoint_ns local_timestamp_ns;
510
511 clock_hw2mono_get(t, device_timestamp_ns, &local_timestamp_ns);
512
513 if (t->fusion.last_imu_local_timestamp_ns != 0 && local_timestamp_ns < t->fusion.last_imu_local_timestamp_ns) {
514 RIFT_S_WARN("IMU time went backward by %" PRId64 " ns",
515 local_timestamp_ns - t->fusion.last_imu_local_timestamp_ns);
516 } else {
517 m_imu_3dof_update(&t->fusion.i3dof, local_timestamp_ns, accel, gyro);
518 }
519
520 RIFT_S_TRACE("IMU timestamp %" PRIu64 " (dt %f) hw2mono local ts %" PRIu64 " (dt %f) offset %" PRId64,
521 device_timestamp_ns,
522 (double)(device_timestamp_ns - t->fusion.last_imu_timestamp_ns) / 1000000000.0, local_timestamp_ns,
523 (double)(local_timestamp_ns - t->fusion.last_imu_local_timestamp_ns) / 1000000000.0, t->hw2mono);
524
525 t->fusion.last_angular_velocity = *gyro;
526 t->fusion.last_imu_timestamp_ns = device_timestamp_ns;
527 t->fusion.last_imu_local_timestamp_ns = local_timestamp_ns;
528
529 t->pose.orientation = t->fusion.i3dof.rot;
530
531 os_mutex_unlock(&t->mutex);
532
533 if (t->slam_sinks.imu) {
534 /* Push IMU sample to the SLAM tracker */
535 struct xrt_vec3_f64 accel64 = {accel->x, accel->y, accel->z};
536 struct xrt_vec3_f64 gyro64 = {gyro->x, gyro->y, gyro->z};
537 struct xrt_imu_sample sample = {
538 .timestamp_ns = local_timestamp_ns, .accel_m_s2 = accel64, .gyro_rad_secs = gyro64};
539
540 xrt_sink_push_imu(t->slam_sinks.imu, &sample);
541 }
542}
543
544#define UPPER_32BITS(x) ((x)&0xffffffff00000000ULL)
545
546void
547rift_s_tracker_push_slam_frames(struct rift_s_tracker *t,
548 uint64_t frame_ts_ns,
549 struct xrt_frame *frames[RIFT_S_CAMERA_COUNT])
550{
551 timepoint_ns frame_time;
552
553 os_mutex_lock(&t->mutex);
554
555 /* Ignore packets before we're ready */
556 if (!t->ready_for_data) {
557 os_mutex_unlock(&t->mutex);
558 return;
559 }
560
561 if (!t->have_hw2mono) {
562 /* Drop any frames before we have IMU */
563 os_mutex_unlock(&t->mutex);
564 return;
565 }
566
567 /* Ensure the input timestamp is within 32-bits of the IMU
568 * time, because the timestamps are reported and extended to 64-bits
569 * separately and can end up in different epochs */
570 uint64_t adj_frame_ts_ns = frame_ts_ns + t->camera_ts_offset;
571 int64_t frame_to_imu_uS = (adj_frame_ts_ns / 1000 - t->fusion.last_imu_timestamp_ns / 1000);
572
573 if (frame_to_imu_uS < -(int64_t)(1ULL << 31) || frame_to_imu_uS > (int64_t)(1ULL << 31)) {
574 t->camera_ts_offset =
575 (UPPER_32BITS(t->fusion.last_imu_timestamp_ns / 1000) - UPPER_32BITS(frame_ts_ns / 1000)) * 1000;
576 RIFT_S_DEBUG("Applying epoch offset to frame times of %" PRId64 " (frame->imu was %" PRId64 " µS)",
577 t->camera_ts_offset, frame_to_imu_uS);
578 }
579 frame_ts_ns += t->camera_ts_offset;
580
581 clock_hw2mono_get(t, frame_ts_ns, &frame_time);
582
583 if (frame_time < t->last_frame_time) {
584 RIFT_S_WARN("Camera frame time went backward by %" PRId64 " ns", frame_time - t->last_frame_time);
585 os_mutex_unlock(&t->mutex);
586 return;
587 }
588
589 RIFT_S_TRACE("SLAM frame timestamp %" PRIu64 " local %" PRIu64, frame_ts_ns, frame_time);
590
591 t->last_frame_time = frame_time;
592 os_mutex_unlock(&t->mutex);
593
594 for (int i = 0; i < RIFT_S_CAMERA_COUNT; i++) {
595 if (t->slam_sinks.cams[i]) {
596 frames[i]->timestamp = frame_time;
597 xrt_sink_push_frame(t->slam_sinks.cams[i], frames[i]);
598 }
599 }
600}
601
602//! Specific pose correction for Basalt to OpenXR coordinates
603XRT_MAYBE_UNUSED static inline void
604rift_s_tracker_correct_pose_from_basalt(struct xrt_pose *pose)
605{
606 struct xrt_quat q = {0.70710678, 0, 0, -0.70710678};
607 math_quat_rotate(&q, &pose->orientation, &pose->orientation);
608 math_quat_rotate_vec3(&q, &pose->position, &pose->position);
609}
610
611static xrt_result_t
612rift_s_tracker_get_tracked_pose_imu(struct xrt_device *xdev,
613 enum xrt_input_name name,
614 int64_t at_timestamp_ns,
615 struct xrt_space_relation *out_relation)
616{
617 struct rift_s_tracker *tracker = (struct rift_s_tracker *)(xdev);
618 if (name != XRT_INPUT_GENERIC_TRACKER_POSE) {
619 U_LOG_XDEV_UNSUPPORTED_INPUT(&tracker->base, rift_s_log_level, name);
620 return XRT_ERROR_INPUT_UNSUPPORTED;
621 }
622
623 rift_s_tracker_get_tracked_pose(tracker, RIFT_S_TRACKER_POSE_IMU, at_timestamp_ns, out_relation);
624
625 return XRT_SUCCESS;
626}
627
628void
629rift_s_tracker_get_tracked_pose(struct rift_s_tracker *t,
630 enum rift_s_tracker_pose pose,
631 uint64_t at_timestamp_ns,
632 struct xrt_space_relation *out_relation)
633{
634 struct xrt_relation_chain xrc = {0};
635
636 if (pose == RIFT_S_TRACKER_POSE_DEVICE) {
637 m_relation_chain_push_pose(&xrc, &t->device_from_imu);
638 } else if (pose == RIFT_S_TRACKER_POSE_LEFT_CAMERA) {
639 m_relation_chain_push_pose(&xrc, &t->left_cam_from_imu);
640 }
641
642 if (t->tracking.slam_enabled && t->slam_over_3dof) {
643 struct xrt_space_relation imu_relation = XRT_SPACE_RELATION_ZERO;
644
645 // Get the IMU pose from the SLAM tracker
646 xrt_tracked_slam_get_tracked_pose(t->tracking.slam, at_timestamp_ns, &imu_relation);
647#ifdef XRT_FEATURE_SLAM
648 // !todo Correct pose depending on the VIT system in use, this should be done in the system itself.
649 // For now, assume that we are using Basalt.
650 rift_s_tracker_correct_pose_from_basalt(&imu_relation.pose);
651#endif
652 imu_relation.relation_flags = (enum xrt_space_relation_flags)(
653 XRT_SPACE_RELATION_ORIENTATION_VALID_BIT | XRT_SPACE_RELATION_POSITION_VALID_BIT |
654 XRT_SPACE_RELATION_ORIENTATION_TRACKED_BIT | XRT_SPACE_RELATION_POSITION_TRACKED_BIT);
655
656 m_relation_chain_push_relation(&xrc, &imu_relation);
657 } else {
658 struct xrt_space_relation imu_relation = XRT_SPACE_RELATION_ZERO;
659
660 os_mutex_lock(&t->mutex);
661 // TODO: Estimate pose at timestamp at_timestamp_ns
662 math_quat_normalize(&t->pose.orientation);
663 imu_relation.pose = t->pose;
664 imu_relation.angular_velocity = t->fusion.last_angular_velocity;
665 imu_relation.relation_flags = (enum xrt_space_relation_flags)(
666 XRT_SPACE_RELATION_ORIENTATION_VALID_BIT | XRT_SPACE_RELATION_POSITION_VALID_BIT |
667 XRT_SPACE_RELATION_ORIENTATION_TRACKED_BIT);
668
669 m_relation_chain_push_relation(&xrc, &imu_relation);
670
671 os_mutex_unlock(&t->mutex);
672 }
673
674 m_relation_chain_resolve(&xrc, out_relation);
675}
676
677void
678rift_s_tracker_start(struct rift_s_tracker *t)
679{
680 os_mutex_lock(&t->mutex);
681 t->ready_for_data = true;
682 os_mutex_unlock(&t->mutex);
683}