The open source OpenXR runtime
0
fork

Configure Feed

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

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