diff --git a/src/xrt/auxiliary/vive/vive_config.c b/src/xrt/auxiliary/vive/vive_config.c index fe9ce5bf9..e60aa29fc 100644 --- a/src/xrt/auxiliary/vive/vive_config.c +++ b/src/xrt/auxiliary/vive/vive_config.c @@ -356,6 +356,115 @@ vive_get_stereo_camera_calibration(struct vive_config *d, return true; } +struct t_imu_calibration +vive_get_imu_calibration(struct vive_config *d) +{ + + struct xrt_vec3 ab = d->imu.acc_bias; + struct xrt_vec3 as = d->imu.acc_scale; + struct xrt_vec3 gb = d->imu.gyro_bias; + struct xrt_vec3 gs = d->imu.gyro_scale; + + struct t_imu_calibration calib = { + .accel = + { + .transform = {{as.x, 0, 0}, {0, as.y, 0}, {0, 0, as.z}}, + .offset = {-ab.x, -ab.y, -ab.z}, // negative because slam system will add, not subtract + .bias_std = {0.001, 0.001, 0.001}, + .noise_std = {0.016, 0.016, 0.016}, + }, + .gyro = + { + .transform = {{gs.x, 0, 0}, {0, gs.y, 0}, {0, 0, gs.z}}, + .offset = {-gb.x, -gb.y, -gb.z}, // negative because slam system will add, not subtract + .bias_std = {0.0001, 0.0001, 0.0001}, + .noise_std = {0.000282, 0.000282, 0.000282}, + }, + }; + return calib; +} + +//! IMU extrinsics and frequencies +struct t_slam_calib_extras +vive_get_extra_calibration(struct vive_config *d) +{ + VIVE_ERROR(d, "Using default factory extrinsics data for vive driver."); + VIVE_ERROR(d, "The rotations of the sensors in the factory data are off."); + VIVE_ERROR(d, "Use a custom calibration instead whenever possible."); + + struct xrt_pose P_tr_imu = d->imu.trackref; + struct xrt_pose P_tr_cam0 = d->cameras.view[0].trackref; + struct xrt_pose P_tr_cam1 = d->cameras.view[1].trackref; + struct xrt_pose P_imu_tr = {0}; + struct xrt_quat Q_imu_tr = {0}; + struct xrt_quat Q_tr_oxr = {.x = 0, .y = 1, .z = 0, .w = 0}; // TR is X: Left, Y: Up, Z: Forward + struct xrt_quat Q_imu_oxr = {0}; + struct xrt_pose P_imu_imuxr = {0}; + struct xrt_pose P_imuxr_imu = {0}; + struct xrt_quat Q_tr_camslam = {.x = 0, .y = 0, .z = 1, .w = 0}; // Many follow X: Right, Y: Down, Z: Forward + struct xrt_pose P_cam0_tr = {0}; + struct xrt_quat Q_cam0_tr = {0}; + struct xrt_quat Q_cam0_camslam = {0}; + struct xrt_pose P_cam0_cam0slam = {0}; + struct xrt_pose P_cam1_tr = {0}; + struct xrt_quat Q_cam1_tr = {0}; + struct xrt_quat Q_cam1_camslam = {0}; + struct xrt_pose P_cam1_cam1slam = {0}; + struct xrt_pose P_imu_cam0 = {0}; + struct xrt_pose P_imuxr_cam0 = {0}; + struct xrt_pose P_imuxr_cam0slam = {0}; + struct xrt_pose P_imu_cam1 = {0}; + struct xrt_pose P_imuxr_cam1 = {0}; + struct xrt_pose P_imuxr_cam1slam = {0}; + struct xrt_matrix_4x4 T_imu_cam0 = {0}; + struct xrt_matrix_4x4 T_imu_cam1 = {0}; + + // Compute P_imuxr_imu. imuxr is same entity as IMU but with axes like OpenXR + // E.g., for Index IMU has X: down, Y: left, Z: forward + math_pose_invert(&P_tr_imu, &P_imu_tr); + Q_imu_tr = P_imu_tr.orientation; + math_quat_rotate(&Q_imu_tr, &Q_tr_oxr, &Q_imu_oxr); + P_imu_imuxr.orientation = Q_imu_oxr; + math_pose_invert(&P_imu_imuxr, &P_imuxr_imu); + + // Compute P_imuxr_cam0slam. cam0slam is the same entity as cam0 but with axes + // like the SLAM system expects cameras to be. Usually X: Right, Y: Down, Z: Forward. + math_pose_invert(&P_tr_cam0, &P_cam0_tr); + Q_cam0_tr = P_cam0_tr.orientation; + math_quat_rotate(&Q_cam0_tr, &Q_tr_camslam, &Q_cam0_camslam); + P_cam0_cam0slam.orientation = Q_cam0_camslam; + math_pose_transform(&P_imu_tr, &P_tr_cam0, &P_imu_cam0); + math_pose_transform(&P_imuxr_imu, &P_imu_cam0, &P_imuxr_cam0); + math_pose_transform(&P_imuxr_cam0, &P_cam0_cam0slam, &P_imuxr_cam0slam); + + // Compute P_imuxr_cam1slam. Same idea as P_imuxr_cam0slam + math_pose_invert(&P_tr_cam1, &P_cam1_tr); + Q_cam1_tr = P_cam1_tr.orientation; + math_quat_rotate(&Q_cam1_tr, &Q_tr_camslam, &Q_cam1_camslam); + P_cam1_cam1slam.orientation = Q_cam1_camslam; + math_pose_transform(&P_imu_tr, &P_tr_cam1, &P_imu_cam1); + math_pose_transform(&P_imuxr_imu, &P_imu_cam1, &P_imuxr_cam1); + math_pose_transform(&P_imuxr_cam1, &P_cam1_cam1slam, &P_imuxr_cam1slam); + + // Convert to 4x4 SE(3) matrices + math_matrix_4x4_isometry_from_pose(&P_imuxr_cam0slam, &T_imu_cam0); + math_matrix_4x4_isometry_from_pose(&P_imuxr_cam1slam, &T_imu_cam1); + + // Can we avoid hardcoding these? + const int CAMERA_FREQUENCY = 54; + const int IMU_FREQUENCY = 1000; + + struct t_slam_calib_extras calib = { + .imu_frequency = IMU_FREQUENCY, + .cams = + { + {.frequency = CAMERA_FREQUENCY, .T_imu_cam = T_imu_cam0}, + {.frequency = CAMERA_FREQUENCY, .T_imu_cam = T_imu_cam1}, + }, + }; + return calib; +} + static void vive_init_defaults(struct vive_config *d) { diff --git a/src/xrt/auxiliary/vive/vive_config.h b/src/xrt/auxiliary/vive/vive_config.h index e606b2926..4cf05d796 100644 --- a/src/xrt/auxiliary/vive/vive_config.h +++ b/src/xrt/auxiliary/vive/vive_config.h @@ -220,3 +220,9 @@ bool vive_get_stereo_camera_calibration(struct vive_config *d, struct t_stereo_camera_calibration **calibration_ptr_to_ref, struct xrt_pose *out_head_in_left_camera); + +struct t_imu_calibration +vive_get_imu_calibration(struct vive_config *d); + +struct t_slam_calib_extras +vive_get_extra_calibration(struct vive_config *d); diff --git a/src/xrt/targets/common/target_builder_lighthouse.c b/src/xrt/targets/common/target_builder_lighthouse.c index 6043e05f3..08accbb9f 100644 --- a/src/xrt/targets/common/target_builder_lighthouse.c +++ b/src/xrt/targets/common/target_builder_lighthouse.c @@ -151,14 +151,23 @@ on_video_device(struct xrt_prober *xp, } static struct xrt_slam_sinks * -valve_index_slam_track(struct lighthouse_system *lhs) +valve_index_slam_track(struct lighthouse_system *lhs, + struct t_stereo_camera_calibration *stereo_calib, + struct t_imu_calibration *imu_calib, + struct t_slam_calib_extras *extra_calib) { struct xrt_slam_sinks *sinks = NULL; -#ifdef XRT_FEATURE_SLAM struct vive_device *d = (struct vive_device *)lhs->devices->base.roles.head; - int create_status = t_slam_create(&lhs->devices->xfctx, NULL, &d->tracking.slam, &sinks); +#ifdef XRT_FEATURE_SLAM + struct t_slam_tracker_config config = {0}; + t_slam_fill_default_config(&config); + config.stereo_calib = stereo_calib; // Won't hold stereo_calib so no refcount + config.imu_calib = imu_calib; + config.extra_calib = extra_calib; + + int create_status = t_slam_create(&lhs->devices->xfctx, &config, &d->tracking.slam, &sinks); if (create_status != 0) { return NULL; } @@ -371,11 +380,13 @@ valve_index_setup_visual_trackers(struct lighthouse_system *lhs, struct t_stereo_camera_calibration *stereo_calib = NULL; struct xrt_pose head_in_left_cam; vive_get_stereo_camera_calibration(lhs->hmd_config, &stereo_calib, &head_in_left_cam); + struct t_imu_calibration imu_calib = vive_get_imu_calibration(lhs->hmd_config); + struct t_slam_calib_extras extra_calib = vive_get_extra_calibration(lhs->hmd_config); // Initialize SLAM tracker struct xrt_slam_sinks *slam_sinks = NULL; if (slam_enabled) { - slam_sinks = valve_index_slam_track(lhs); + slam_sinks = valve_index_slam_track(lhs, stereo_calib, &imu_calib, &extra_calib); if (slam_sinks == NULL) { lhs->vive_tstatus.slam_enabled = false; slam_enabled = false;