matrixfurry.com
The open source OpenXR runtime
d/wmr: Optionally average IMU samples for 3DoF tracker
Similar to how this was being done for SLAM.
It's significantly less jittery and still has a good response.
Smarter filters might benefit from raw measurements so the functionality can
be enabled again with a checkbox.
+1
-1
src/xrt/drivers/depthai/depthai_driver.cpp
+1
-1
src/xrt/drivers/depthai/depthai_driver.cpp
···
397
397
.count();
398
398
}
399
399
400
-
// Look at wmr_source_push_imu_packet - that's where these averaging shenanigans come from ;)
400
+
// Look at the WMR driver - that's where these averaging shenanigans come from ;)
401
401
static void
402
402
depthai_do_one_imu_frame(struct depthai_fs *depthai)
403
403
{
+79
-16
src/xrt/drivers/wmr/wmr_hmd.c
+79
-16
src/xrt/drivers/wmr/wmr_hmd.c
···
1
1
// Copyright 2018, Philipp Zabel.
2
2
// Copyright 2020-2021, N Madsen.
3
-
// Copyright 2020-2021, Collabora, Ltd.
3
+
// Copyright 2020-2022, Collabora, Ltd.
4
4
// SPDX-License-Identifier: BSL-1.0
5
5
/*!
6
6
* @file
···
8
8
* @author Philipp Zabel <philipp.zabel@gmail.com>
9
9
* @author nima01 <nima_zero_one@protonmail.com>
10
10
* @author Jakob Bornecrantz <jakob@collabora.com>
11
+
* @author Mateo de Mayo <mateo.demayo@collabora.com>
11
12
* @ingroup drv_wmr
12
13
*/
13
14
···
61
62
#define CAMERA_FREQUENCY 30 //!< Observed value (OV7251)
62
63
#define IMU_FREQUENCY 1000 //!< Observed value (ICM20602)
63
64
#define IMU_SAMPLES_PER_PACKET 4 //!< There are 4 samples for each USB IMU packet
64
-
#define SLAM_IMU_FREQUENCY IMU_FREQUENCY / IMU_SAMPLES_PER_PACKET
65
65
66
66
//! Specifies whether the user wants to use a SLAM tracker.
67
67
DEBUG_GET_ONCE_BOOL_OPTION(wmr_slam, "WMR_SLAM", true)
···
304
304
}
305
305
306
306
static void
307
-
hololens_handle_sensors(struct wmr_hmd *wh, const unsigned char *buffer, int size)
307
+
hololens_handle_sensors_avg(struct wmr_hmd *wh, const unsigned char *buffer, int size)
308
308
{
309
309
DRV_TRACE_MARKER();
310
310
···
313
313
314
314
hololens_sensors_decode_packet(wh, &wh->packet, buffer, size);
315
315
316
-
struct xrt_vec3 raw_gyro[4];
317
-
struct xrt_vec3 raw_accel[4];
318
-
struct xrt_vec3 calib_gyro[4];
319
-
struct xrt_vec3 calib_accel[4];
316
+
// Use a single averaged sample from all the samples in the packet
317
+
struct xrt_vec3 avg_raw_accel = XRT_VEC3_ZERO;
318
+
struct xrt_vec3 avg_raw_gyro = XRT_VEC3_ZERO;
319
+
for (int i = 0; i < IMU_SAMPLES_PER_PACKET; i++) {
320
+
struct xrt_vec3 a = XRT_VEC3_ZERO;
321
+
struct xrt_vec3 g = XRT_VEC3_ZERO;
322
+
vec3_from_hololens_accel(wh->packet.accel, i, &a);
323
+
vec3_from_hololens_gyro(wh->packet.gyro, i, &g);
324
+
math_vec3_accum(&a, &avg_raw_accel);
325
+
math_vec3_accum(&g, &avg_raw_gyro);
326
+
}
327
+
math_vec3_scalar_mul(1.0f / IMU_SAMPLES_PER_PACKET, &avg_raw_accel);
328
+
math_vec3_scalar_mul(1.0f / IMU_SAMPLES_PER_PACKET, &avg_raw_gyro);
320
329
321
-
for (int i = 0; i < 4; i++) {
330
+
// Calibrate averaged sample
331
+
struct xrt_vec3 avg_calib_accel = XRT_VEC3_ZERO;
332
+
struct xrt_vec3 avg_calib_gyro = XRT_VEC3_ZERO;
333
+
math_matrix_3x3_transform_vec3(&wh->config.sensors.accel.mix_matrix, &avg_raw_accel, &avg_calib_accel);
334
+
math_matrix_3x3_transform_vec3(&wh->config.sensors.gyro.mix_matrix, &avg_raw_gyro, &avg_calib_gyro);
335
+
math_vec3_accum(&wh->config.sensors.accel.bias_offsets, &avg_calib_accel);
336
+
math_vec3_accum(&wh->config.sensors.gyro.bias_offsets, &avg_calib_gyro);
337
+
math_quat_rotate_vec3(&wh->P_oxr_acc.orientation, &avg_calib_accel, &avg_calib_accel);
338
+
math_quat_rotate_vec3(&wh->P_oxr_gyr.orientation, &avg_calib_gyro, &avg_calib_gyro);
339
+
340
+
// Fusion tracking
341
+
os_mutex_lock(&wh->fusion.mutex);
342
+
timepoint_ns t = wh->packet.gyro_timestamp[IMU_SAMPLES_PER_PACKET - 1] * WMR_MS_HOLOLENS_NS_PER_TICK;
343
+
m_imu_3dof_update(&wh->fusion.i3dof, t, &avg_calib_accel, &avg_calib_gyro);
344
+
wh->fusion.last_imu_timestamp_ns = now_ns;
345
+
wh->fusion.last_angular_velocity = avg_calib_gyro;
346
+
os_mutex_unlock(&wh->fusion.mutex);
347
+
348
+
// SLAM tracking
349
+
wmr_source_push_imu_packet(wh->tracking.source, t, avg_raw_accel, avg_raw_gyro);
350
+
}
351
+
352
+
static void
353
+
hololens_handle_sensors_all(struct wmr_hmd *wh, const unsigned char *buffer, int size)
354
+
{
355
+
DRV_TRACE_MARKER();
356
+
357
+
// Get the timing as close to reading the packet as possible.
358
+
uint64_t now_ns = os_monotonic_get_ns();
359
+
360
+
hololens_sensors_decode_packet(wh, &wh->packet, buffer, size);
361
+
362
+
struct xrt_vec3 raw_gyro[IMU_SAMPLES_PER_PACKET];
363
+
struct xrt_vec3 raw_accel[IMU_SAMPLES_PER_PACKET];
364
+
struct xrt_vec3 calib_gyro[IMU_SAMPLES_PER_PACKET];
365
+
struct xrt_vec3 calib_accel[IMU_SAMPLES_PER_PACKET];
366
+
367
+
for (int i = 0; i < IMU_SAMPLES_PER_PACKET; i++) {
322
368
struct xrt_vec3 *rg = &raw_gyro[i];
323
369
struct xrt_vec3 *cg = &calib_gyro[i];
324
370
vec3_from_hololens_gyro(wh->packet.gyro, i, rg);
···
336
382
337
383
// Fusion tracking
338
384
os_mutex_lock(&wh->fusion.mutex);
339
-
for (int i = 0; i < 4; i++) {
385
+
for (int i = 0; i < IMU_SAMPLES_PER_PACKET; i++) {
340
386
m_imu_3dof_update( //
341
387
&wh->fusion.i3dof, //
342
388
wh->packet.gyro_timestamp[i] * WMR_MS_HOLOLENS_NS_PER_TICK, //
···
348
394
os_mutex_unlock(&wh->fusion.mutex);
349
395
350
396
// SLAM tracking
351
-
//! @todo For now we are using raw_samples here because the centerline fix
352
-
//! in the calibrated samples breaks SLAM systems. Handling of coordinate
353
-
//! systems needs to be revisited.
354
-
wmr_source_push_imu_packet(wh->tracking.source, wh->packet.gyro_timestamp, raw_accel, raw_gyro);
397
+
for (int i = 0; i < IMU_SAMPLES_PER_PACKET; i++) {
398
+
timepoint_ns t = wh->packet.gyro_timestamp[i] * WMR_MS_HOLOLENS_NS_PER_TICK;
399
+
wmr_source_push_imu_packet(wh->tracking.source, t, raw_accel[i], raw_gyro[i]);
400
+
}
401
+
}
402
+
403
+
static void
404
+
hololens_handle_sensors(struct wmr_hmd *wh, const unsigned char *buffer, int size)
405
+
{
406
+
if (wh->average_imus) {
407
+
// Less overhead and jitter.
408
+
hololens_handle_sensors_avg(wh, buffer, size);
409
+
} else {
410
+
// More sophisticated fusion algorithms might work better with raw data.
411
+
hololens_handle_sensors_all(wh, buffer, size);
412
+
}
355
413
}
356
414
357
415
static bool
···
1338
1396
math_matrix_4x4_isometry_from_pose(&P_imu_ht0, &T_imu_ht0);
1339
1397
math_matrix_4x4_isometry_from_pose(&P_imu_ht1, &T_imu_ht1);
1340
1398
1399
+
//! @note This might change during runtime but the calibration data will be already submitted
1400
+
double imu_frequency = wh->average_imus ? IMU_FREQUENCY / IMU_SAMPLES_PER_PACKET : IMU_FREQUENCY;
1401
+
1341
1402
struct t_slam_calib_extras calib = {
1342
-
.imu_frequency = SLAM_IMU_FREQUENCY,
1403
+
.imu_frequency = imu_frequency,
1343
1404
.cams =
1344
1405
{
1345
1406
{
···
1461
1522
}
1462
1523
u_var_add_pose(wh, &wh->pose, "Tracked Pose");
1463
1524
u_var_add_pose(wh, &wh->offset, "Pose Offset");
1525
+
u_var_add_bool(wh, &wh->average_imus, "Average IMU samples");
1464
1526
1465
1527
u_var_add_gui_header(wh, NULL, "3DoF Tracking");
1466
1528
m_imu_3dof_add_vars(&wh->fusion.i3dof, wh, "");
···
1732
1794
return;
1733
1795
}
1734
1796
1735
-
wh->pose = (struct xrt_pose){{0, 0, 0, 1}, {0, 0, 0}};
1736
-
wh->offset = (struct xrt_pose){{0, 0, 0, 1}, {0, 0, 0}};
1797
+
wh->pose = (struct xrt_pose)XRT_POSE_IDENTITY;
1798
+
wh->offset = (struct xrt_pose)XRT_POSE_IDENTITY;
1799
+
wh->average_imus = true;
1737
1800
1738
1801
/* Now that we have the config loaded, iterate the map of known headsets and see if we have
1739
1802
* an entry for this specific headset (otherwise the generic entry will be used)
+3
src/xrt/drivers/wmr/wmr_hmd.h
+3
src/xrt/drivers/wmr/wmr_hmd.h
+5
-33
src/xrt/drivers/wmr/wmr_source.c
+5
-33
src/xrt/drivers/wmr/wmr_source.c
···
74
74
75
75
bool is_running; //!< Whether the device is streaming
76
76
bool first_imu_received; //!< Don't send frames until first IMU sample
77
-
bool average_imus; //!< Average 4 IMU samples before sending them to the sinks
78
77
time_duration_ns hw2mono; //!< Estimated offset from IMU to monotonic clock
79
78
time_duration_ns cam_hw2mono; //!< Cache for hw2mono used in last left frame
80
79
};
···
308
307
309
308
struct wmr_source *ws = U_TYPED_CALLOC(struct wmr_source);
310
309
ws->log_level = debug_get_log_option_wmr_log();
311
-
ws->average_imus = true;
312
310
313
311
// Setup xrt_fs
314
312
struct xrt_fs *xfs = &ws->xfs;
···
341
339
m_ff_vec3_f32_alloc(&ws->accel_ff, 1000);
342
340
u_var_add_root(ws, WMR_SOURCE_STR, false);
343
341
u_var_add_log_level(ws, &ws->log_level, "Log Level");
344
-
u_var_add_bool(ws, &ws->average_imus, "Average IMU samples");
345
342
u_var_add_ro_ff_vec3_f32(ws, ws->gyro_ff, "Gyroscope");
346
343
u_var_add_ro_ff_vec3_f32(ws, ws->accel_ff, "Accelerometer");
347
344
u_var_add_sink_debug(ws, &ws->ui_left_sink, "Left Camera");
···
359
356
}
360
357
361
358
void
362
-
wmr_source_push_imu_packet(struct xrt_fs *xfs,
363
-
const uint64_t ts[4],
364
-
struct xrt_vec3 accels[4],
365
-
struct xrt_vec3 gyros[4])
359
+
wmr_source_push_imu_packet(struct xrt_fs *xfs, timepoint_ns t, struct xrt_vec3 accel, struct xrt_vec3 gyro)
366
360
{
367
361
DRV_TRACE_MARKER();
368
-
369
362
struct wmr_source *ws = wmr_source_from_xfs(xfs);
370
-
371
-
if (ws->average_imus) {
372
-
struct xrt_vec3 a = {0, 0, 0};
373
-
struct xrt_vec3 g = {0, 0, 0};
374
-
for (int i = 0; i < 4; i++) {
375
-
math_vec3_accum(&accels[i], &a);
376
-
math_vec3_accum(&gyros[i], &g);
377
-
}
378
-
math_vec3_scalar_mul(1.0f / 4, &a);
379
-
math_vec3_scalar_mul(1.0f / 4, &g);
380
-
381
-
timepoint_ns t = (ts[3] + ts[0]) / 2 * WMR_MS_HOLOLENS_NS_PER_TICK;
382
-
struct xrt_vec3_f64 accel = {a.x, a.y, a.z};
383
-
struct xrt_vec3_f64 gyro = {g.x, g.y, g.z};
384
-
struct xrt_imu_sample sample = {.timestamp_ns = t, .accel_m_s2 = accel, .gyro_rad_secs = gyro};
385
-
xrt_sink_push_imu(&ws->imu_sink, &sample);
386
-
} else {
387
-
for (int i = 0; i < 4; i++) {
388
-
timepoint_ns t = ts[i] * WMR_MS_HOLOLENS_NS_PER_TICK;
389
-
struct xrt_vec3_f64 accel = {accels[i].x, accels[i].y, accels[i].z};
390
-
struct xrt_vec3_f64 gyro = {gyros[i].x, gyros[i].y, gyros[i].z};
391
-
struct xrt_imu_sample sample = {.timestamp_ns = t, .accel_m_s2 = accel, .gyro_rad_secs = gyro};
392
-
xrt_sink_push_imu(&ws->imu_sink, &sample);
393
-
}
394
-
}
363
+
struct xrt_vec3_f64 accel_f64 = {accel.x, accel.y, accel.z};
364
+
struct xrt_vec3_f64 gyro_f64 = {gyro.x, gyro.y, gyro.z};
365
+
struct xrt_imu_sample sample = {.timestamp_ns = t, .accel_m_s2 = accel_f64, .gyro_rad_secs = gyro_f64};
366
+
xrt_sink_push_imu(&ws->imu_sink, &sample);
395
367
}
+4
-4
src/xrt/drivers/wmr/wmr_source.h
+4
-4
src/xrt/drivers/wmr/wmr_source.h
···
12
12
#include "wmr_config.h"
13
13
#include "xrt/xrt_frameserver.h"
14
14
#include "xrt/xrt_prober.h"
15
+
#include "xrt/xrt_tracking.h"
15
16
16
17
/*!
17
18
* WMR video/IMU data sources
···
31
32
32
33
//! @todo IMU data should be generated from within the data source, but right
33
34
//! now we need this function because it is being generated from wmr_hmd
35
+
//! @todo Should this method receive raw or calibrated samples? Currently
36
+
//! receiving raw because Basalt can calibrate them, but other systems can't.
34
37
void
35
-
wmr_source_push_imu_packet(struct xrt_fs *xfs,
36
-
const uint64_t ts[4],
37
-
struct xrt_vec3 accels[4],
38
-
struct xrt_vec3 gyros[4]);
38
+
wmr_source_push_imu_packet(struct xrt_fs *xfs, timepoint_ns t, struct xrt_vec3 accel, struct xrt_vec3 gyro);
39
39
40
40
/*!
41
41
* @}