The open source OpenXR runtime
0
fork

Configure Feed

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

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
··· 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};