From d3bde043bddd965df58450013236c1e4d26bb996 Mon Sep 17 00:00:00 2001 From: Mateo de Mayo Date: Mon, 6 Dec 2021 08:56:56 -0300 Subject: [PATCH] d/wmr: Forward IMU samples to wmr_source --- src/xrt/drivers/wmr/wmr_hmd.c | 34 +++++++++++++++++++++----------- src/xrt/drivers/wmr/wmr_source.c | 31 +++++++++++++++++++++++++++++ src/xrt/drivers/wmr/wmr_source.h | 5 +++++ 3 files changed, 59 insertions(+), 11 deletions(-) diff --git a/src/xrt/drivers/wmr/wmr_hmd.c b/src/xrt/drivers/wmr/wmr_hmd.c index 0125fa285..bf860ddf6 100644 --- a/src/xrt/drivers/wmr/wmr_hmd.c +++ b/src/xrt/drivers/wmr/wmr_hmd.c @@ -272,32 +272,44 @@ hololens_sensors_read_packets(struct wmr_hmd *wh) struct xrt_vec3 raw_gyro[4]; struct xrt_vec3 raw_accel[4]; + struct xrt_vec3 calib_gyro[4]; + struct xrt_vec3 calib_accel[4]; for (int i = 0; i < 4; i++) { - struct xrt_vec3 raw_sample, sample; - vec3_from_hololens_gyro(wh->packet.gyro, i, &raw_sample); - math_matrix_3x3_transform_vec3(&wh->config.sensors.gyro.mix_matrix, &raw_sample, &sample); - math_vec3_accum(&wh->config.sensors.gyro.bias_offsets, &sample); - math_quat_rotate_vec3(&wh->gyro_to_centerline.orientation, &sample, &raw_gyro[i]); + struct xrt_vec3 *rg = &raw_gyro[i]; + struct xrt_vec3 *cg = &calib_gyro[i]; + vec3_from_hololens_gyro(wh->packet.gyro, i, rg); + math_matrix_3x3_transform_vec3(&wh->config.sensors.gyro.mix_matrix, rg, cg); + math_vec3_accum(&wh->config.sensors.gyro.bias_offsets, cg); + math_quat_rotate_vec3(&wh->gyro_to_centerline.orientation, cg, cg); - vec3_from_hololens_accel(wh->packet.accel, i, &raw_sample); - math_matrix_3x3_transform_vec3(&wh->config.sensors.accel.mix_matrix, &raw_sample, &sample); - math_vec3_accum(&wh->config.sensors.accel.bias_offsets, &sample); - math_quat_rotate_vec3(&wh->accel_to_centerline.orientation, &sample, &raw_accel[i]); + struct xrt_vec3 *ra = &raw_accel[i]; + struct xrt_vec3 *ca = &calib_accel[i]; + vec3_from_hololens_accel(wh->packet.accel, i, ra); + math_matrix_3x3_transform_vec3(&wh->config.sensors.accel.mix_matrix, ra, ca); + math_vec3_accum(&wh->config.sensors.accel.bias_offsets, ca); + math_quat_rotate_vec3(&wh->accel_to_centerline.orientation, ca, ca); } + // Fusion tracking os_mutex_lock(&wh->fusion.mutex); for (int i = 0; i < 4; i++) { m_imu_3dof_update( // &wh->fusion.i3dof, // wh->packet.gyro_timestamp[i] * WMR_MS_HOLOLENS_NS_PER_TICK, // - &raw_accel[i], // - &raw_gyro[i]); // + &calib_accel[i], // + &calib_gyro[i]); // } wh->fusion.last_imu_timestamp_ns = now_ns; wh->fusion.last_angular_velocity = raw_gyro[3]; os_mutex_unlock(&wh->fusion.mutex); + // SLAM tracking + //! @todo For now we are using raw_samples here because the centerline fix + //! in the calibrated samples breaks SLAM systems. Handling of coordinate + //! systems needs to be revisited. + wmr_source_push_imu_packet(wh->slam.source, wh->packet.gyro_timestamp, raw_accel, raw_gyro); + break; } case WMR_MS_HOLOLENS_MSG_BT_IFACE: // diff --git a/src/xrt/drivers/wmr/wmr_source.c b/src/xrt/drivers/wmr/wmr_source.c index 154f6f6f8..e11be8e39 100644 --- a/src/xrt/drivers/wmr/wmr_source.c +++ b/src/xrt/drivers/wmr/wmr_source.c @@ -304,3 +304,34 @@ wmr_source_create(struct xrt_frame_context *xfctx, struct xrt_prober_device *dev return xfs; } + +void +wmr_source_push_imu_packet(struct xrt_fs *xfs, uint64_t ts[4], struct xrt_vec3 accels[4], struct xrt_vec3 gyros[4]) +{ + struct wmr_source *ws = wmr_source_from_xfs(xfs); + + if (ws->average_imus) { + struct xrt_vec3 a = {0, 0, 0}; + struct xrt_vec3 g = {0, 0, 0}; + for (int i = 0; i < 4; i++) { + math_vec3_accum(&accels[i], &a); + math_vec3_accum(&gyros[i], &g); + } + math_vec3_scalar_mul(1.0f / 4, &a); + math_vec3_scalar_mul(1.0f / 4, &g); + + timepoint_ns t = (ts[3] + ts[0]) / 2 * WMR_MS_HOLOLENS_NS_PER_TICK; + struct xrt_vec3_f64 accel = {a.x, a.y, a.z}; + struct xrt_vec3_f64 gyro = {g.x, g.y, g.z}; + struct xrt_imu_sample sample = {.timestamp_ns = t, .accel_m_s2 = accel, .gyro_rad_secs = gyro}; + xrt_sink_push_imu(&ws->imu_sink, &sample); + } else { + for (int i = 0; i < 4; i++) { + timepoint_ns t = ts[i] * WMR_MS_HOLOLENS_NS_PER_TICK; + struct xrt_vec3_f64 accel = {accels[i].x, accels[i].y, accels[i].z}; + struct xrt_vec3_f64 gyro = {gyros[i].x, gyros[i].y, gyros[i].z}; + struct xrt_imu_sample sample = {.timestamp_ns = t, .accel_m_s2 = accel, .gyro_rad_secs = gyro}; + xrt_sink_push_imu(&ws->imu_sink, &sample); + } + } +} diff --git a/src/xrt/drivers/wmr/wmr_source.h b/src/xrt/drivers/wmr/wmr_source.h index bcc91bca3..3cabb5ebf 100644 --- a/src/xrt/drivers/wmr/wmr_source.h +++ b/src/xrt/drivers/wmr/wmr_source.h @@ -29,6 +29,11 @@ extern "C" { struct xrt_fs * wmr_source_create(struct xrt_frame_context *xfctx, struct xrt_prober_device *dev_holo, struct wmr_hmd_config cfg); +//! @todo IMU data should be generated from within the data source, but right +//! now we need this function because it is being generated from wmr_hmd +void +wmr_source_push_imu_packet(struct xrt_fs *xfs, uint64_t ts[4], struct xrt_vec3 accels[4], struct xrt_vec3 gyros[4]); + /*! * @} */