mirror of
https://gitlab.freedesktop.org/monado/monado.git
synced 2024-12-29 11:06:18 +00:00
d/vive: Generate SLAM calibration automatically from factory data
Use factory data for SLAM calibration. Sensor rotations are off in most calib files we saw (or at least we can't figure something better) so this won't work very well. It's still necessary to properly calibrate the headset for good results but at least now something works without that step.
This commit is contained in:
Mateo de Mayo
parent
0ebc7e042f
commit
c3757792d7
|
|
@ -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)
|
||||
{
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
Loading…
Reference in a new issue