The open source OpenXR runtime
d/wmr: Send calibration to SLAM tracker
This makes it possible to use Basalt without having to create a calibration file
for the headset anymore.
+72
-9
+72
-9
src/xrt/drivers/wmr/wmr_hmd.c
+72
-9
src/xrt/drivers/wmr/wmr_hmd.c
···
56
56
#include "../drivers/ht/ht_interface.h"
57
57
#endif
58
58
59
+
// Unsure if these can change nor how to get them if so
60
+
#define CAMERA_FREQUENCY 30 //!< Observed value (OV7251)
61
+
#define IMU_FREQUENCY 1000 //!< Observed value (ICM20602)
62
+
#define IMU_SAMPLES_PER_PACKET 4 //!< There are 4 samples for each USB IMU packet
63
+
#define SLAM_IMU_FREQUENCY IMU_FREQUENCY / IMU_SAMPLES_PER_PACKET
64
+
59
65
//! Specifies whether the user wants to use a SLAM tracker.
60
66
DEBUG_GET_ONCE_BOOL_OPTION(wmr_slam, "WMR_SLAM", true)
61
67
···
1301
1307
return calib;
1302
1308
}
1303
1309
1310
+
//! IMU extrinsics, frequencies, and rpmax
1311
+
XRT_MAYBE_UNUSED static struct t_slam_calib_extras
1312
+
wmr_hmd_create_extra_calib(struct wmr_hmd *wh)
1313
+
{
1314
+
// Compute transform from IMU to HT0 (HT0 space into IMU space)
1315
+
struct wmr_camera_config *ht0 = &wh->config.cameras[0];
1316
+
struct xrt_matrix_3x3 rot_imu_ht0 = wh->config.sensors.accel.rotation;
1317
+
math_matrix_3x3_transpose(&rot_imu_ht0, &rot_imu_ht0); // Row major to col major
1318
+
struct xrt_vec3 tra_imu_ht0 = wh->config.sensors.accel.translation;
1319
+
struct xrt_matrix_4x4 t_imu_ht0;
1320
+
math_matrix_4x4_isometry_from_rt(&rot_imu_ht0, &tra_imu_ht0, &t_imu_ht0);
1321
+
1322
+
// Compute transform from IMU to HT1 (HT1 space into IMU space)
1323
+
struct wmr_camera_config *ht1 = &wh->config.cameras[1];
1324
+
struct xrt_matrix_3x3 ht1_rotation_cm; // HT1 rotation but column major
1325
+
math_matrix_3x3_transpose(&ht1->rotation, &ht1_rotation_cm);
1326
+
struct xrt_matrix_4x4 t_ht1_ht0;
1327
+
math_matrix_4x4_isometry_from_rt(&ht1_rotation_cm, &ht1->translation, &t_ht1_ht0);
1328
+
struct xrt_matrix_4x4 t_ht0_ht1;
1329
+
math_matrix_4x4_isometry_inverse(&t_ht1_ht0, &t_ht0_ht1);
1330
+
struct xrt_matrix_4x4 t_imu_ht1;
1331
+
math_matrix_4x4_multiply(&t_imu_ht0, &t_ht0_ht1, &t_imu_ht1);
1332
+
1333
+
struct t_slam_calib_extras calib = {
1334
+
.imu_frequency = SLAM_IMU_FREQUENCY,
1335
+
.cams =
1336
+
{
1337
+
{
1338
+
.frequency = CAMERA_FREQUENCY,
1339
+
.T_imu_cam = t_imu_ht0,
1340
+
.rpmax = ht0->distortion6KT.params.metric_radius,
1341
+
},
1342
+
{
1343
+
.frequency = CAMERA_FREQUENCY,
1344
+
.T_imu_cam = t_imu_ht1,
1345
+
.rpmax = ht1->distortion6KT.params.metric_radius,
1346
+
},
1347
+
},
1348
+
};
1349
+
return calib;
1350
+
}
1351
+
1304
1352
static void
1305
1353
wmr_hmd_switch_hmd_tracker(void *wh_ptr)
1306
1354
{
···
1322
1370
}
1323
1371
1324
1372
static struct xrt_slam_sinks *
1325
-
wmr_hmd_slam_track(struct wmr_hmd *wh)
1373
+
wmr_hmd_slam_track(struct wmr_hmd *wh,
1374
+
struct t_stereo_camera_calibration *stereo_calib,
1375
+
struct t_imu_calibration *imu_calib,
1376
+
struct t_slam_calib_extras *extra_calib)
1326
1377
{
1327
1378
DRV_TRACE_MARKER();
1328
1379
1329
1380
struct xrt_slam_sinks *sinks = NULL;
1330
1381
1331
1382
#ifdef XRT_FEATURE_SLAM
1332
-
int create_status = t_slam_create(&wh->tracking.xfctx, NULL, &wh->tracking.slam, &sinks);
1383
+
struct t_slam_tracker_config config = {0};
1384
+
t_slam_fill_default_config(&config);
1385
+
config.stereo_calib = stereo_calib; // No need to do refcount here
1386
+
config.imu_calib = imu_calib;
1387
+
config.extra_calib = extra_calib;
1388
+
1389
+
int create_status = t_slam_create(&wh->tracking.xfctx, &config, &wh->tracking.slam, &sinks);
1333
1390
if (create_status != 0) {
1334
1391
return NULL;
1335
1392
}
···
1346
1403
}
1347
1404
1348
1405
static int
1349
-
wmr_hmd_hand_track(struct wmr_hmd *wh, struct xrt_slam_sinks **out_sinks, struct xrt_device **out_device)
1406
+
wmr_hmd_hand_track(struct wmr_hmd *wh,
1407
+
struct t_stereo_camera_calibration *stereo_calib,
1408
+
struct xrt_slam_sinks **out_sinks,
1409
+
struct xrt_device **out_device)
1350
1410
{
1351
1411
DRV_TRACE_MARKER();
1352
1412
···
1354
1414
struct xrt_device *device = NULL;
1355
1415
1356
1416
#ifdef XRT_BUILD_DRIVER_HANDTRACKING
1357
-
struct t_stereo_camera_calibration *calib = wmr_hmd_create_stereo_camera_calib(wh);
1358
1417
1359
1418
int create_status = ht_device_create(&wh->tracking.xfctx, //
1360
-
calib, //
1419
+
stereo_calib, //
1361
1420
HT_OUTPUT_SPACE_CENTER_OF_STEREO_CAMERA, //
1362
1421
HT_ALGORITHM_MERCURY, //
1363
1422
&sinks, //
···
1365
1424
if (create_status != 0) {
1366
1425
return create_status;
1367
1426
}
1368
-
1369
-
t_stereo_camera_calibration_reference(&calib, NULL);
1370
1427
1371
1428
WMR_DEBUG(wh, "WMR HMD hand tracker successfully created");
1372
1429
#endif
···
1475
1532
snprintf(wh->gui.slam_status, sizeof(wh->gui.slam_status), "%s", slam_status);
1476
1533
snprintf(wh->gui.hand_status, sizeof(wh->gui.hand_status), "%s", hand_status);
1477
1534
1535
+
struct t_stereo_camera_calibration *stereo_calib = wmr_hmd_create_stereo_camera_calib(wh);
1536
+
struct t_imu_calibration imu_calib = wmr_hmd_create_imu_calib(wh);
1537
+
struct t_slam_calib_extras extra_calib = wmr_hmd_create_extra_calib(wh);
1538
+
1478
1539
// Initialize 3DoF tracker
1479
1540
m_imu_3dof_init(&wh->fusion.i3dof, M_IMU_3DOF_USE_GRAVITY_DUR_20MS);
1480
1541
1481
1542
// Initialize SLAM tracker
1482
1543
struct xrt_slam_sinks *slam_sinks = NULL;
1483
1544
if (wh->tracking.slam_enabled) {
1484
-
slam_sinks = wmr_hmd_slam_track(wh);
1545
+
slam_sinks = wmr_hmd_slam_track(wh, stereo_calib, &imu_calib, &extra_calib);
1485
1546
if (slam_sinks == NULL) {
1486
1547
WMR_WARN(wh, "Unable to setup the SLAM tracker");
1487
1548
return false;
···
1492
1553
struct xrt_slam_sinks *hand_sinks = NULL;
1493
1554
struct xrt_device *hand_device = NULL;
1494
1555
if (wh->tracking.hand_enabled) {
1495
-
int hand_status = wmr_hmd_hand_track(wh, &hand_sinks, &hand_device);
1556
+
int hand_status = wmr_hmd_hand_track(wh, stereo_calib, &hand_sinks, &hand_device);
1496
1557
if (hand_status != 0 || hand_sinks == NULL || hand_device == NULL) {
1497
1558
WMR_WARN(wh, "Unable to setup the hand tracker");
1498
1559
return false;
1499
1560
}
1500
1561
}
1562
+
1563
+
t_stereo_camera_calibration_reference(&stereo_calib, NULL);
1501
1564
1502
1565
// Setup sinks depending on tracking configuration
1503
1566
struct xrt_slam_sinks entry_sinks = {0};