The open source OpenXR runtime
d/wmr: Use poses instead of isometries for T_imu_cam poses
Now that sensor poses are in WMR space, this is a bit tidier
than creating the 4x4 matrices.
+14
-18
+14
-18
src/xrt/drivers/wmr/wmr_hmd.c
+14
-18
src/xrt/drivers/wmr/wmr_hmd.c
···
1323
1323
XRT_MAYBE_UNUSED static struct t_slam_calib_extras
1324
1324
wmr_hmd_create_extra_calib(struct wmr_hmd *wh)
1325
1325
{
1326
-
// Compute transform from IMU to HT0 (HT0 space into IMU space)
1327
1326
struct wmr_camera_config *ht0 = &wh->config.cameras[0];
1328
-
struct xrt_matrix_3x3 rot_imu_ht0 = wh->config.sensors.accel.rotation;
1329
-
math_matrix_3x3_transpose(&rot_imu_ht0, &rot_imu_ht0); // Row major to col major
1330
-
struct xrt_vec3 tra_imu_ht0 = wh->config.sensors.accel.translation;
1331
-
struct xrt_matrix_4x4 t_imu_ht0;
1332
-
math_matrix_4x4_isometry_from_rt(&rot_imu_ht0, &tra_imu_ht0, &t_imu_ht0);
1333
-
1334
-
// Compute transform from IMU to HT1 (HT1 space into IMU space)
1335
1327
struct wmr_camera_config *ht1 = &wh->config.cameras[1];
1336
-
struct xrt_matrix_3x3 ht1_rotation_cm; // HT1 rotation but column major
1337
-
math_matrix_3x3_transpose(&ht1->rotation, &ht1_rotation_cm);
1338
-
struct xrt_matrix_4x4 t_ht1_ht0;
1339
-
math_matrix_4x4_isometry_from_rt(&ht1_rotation_cm, &ht1->translation, &t_ht1_ht0);
1340
-
struct xrt_matrix_4x4 t_ht0_ht1;
1341
-
math_matrix_4x4_isometry_inverse(&t_ht1_ht0, &t_ht0_ht1);
1342
-
struct xrt_matrix_4x4 t_imu_ht1;
1343
-
math_matrix_4x4_multiply(&t_imu_ht0, &t_ht0_ht1, &t_imu_ht1);
1328
+
1329
+
struct xrt_pose P_imu_ht0 = wh->config.sensors.accel.pose;
1330
+
struct xrt_pose P_ht1_ht0 = ht1->pose;
1331
+
struct xrt_pose P_ht0_ht1;
1332
+
math_pose_invert(&P_ht1_ht0, &P_ht0_ht1);
1333
+
struct xrt_pose P_imu_ht1;
1334
+
math_pose_transform(&P_imu_ht0, &P_ht0_ht1, &P_imu_ht1);
1335
+
1336
+
struct xrt_matrix_4x4 T_imu_ht0;
1337
+
struct xrt_matrix_4x4 T_imu_ht1;
1338
+
math_matrix_4x4_isometry_from_pose(&P_imu_ht0, &T_imu_ht0);
1339
+
math_matrix_4x4_isometry_from_pose(&P_imu_ht1, &T_imu_ht1);
1344
1340
1345
1341
struct t_slam_calib_extras calib = {
1346
1342
.imu_frequency = SLAM_IMU_FREQUENCY,
···
1348
1344
{
1349
1345
{
1350
1346
.frequency = CAMERA_FREQUENCY,
1351
-
.T_imu_cam = t_imu_ht0,
1347
+
.T_imu_cam = T_imu_ht0,
1352
1348
.rpmax = ht0->distortion6KT.params.metric_radius,
1353
1349
},
1354
1350
{
1355
1351
.frequency = CAMERA_FREQUENCY,
1356
-
.T_imu_cam = t_imu_ht1,
1352
+
.T_imu_cam = T_imu_ht1,
1357
1353
.rpmax = ht1->distortion6KT.params.metric_radius,
1358
1354
},
1359
1355
},