diff --git a/src/xrt/auxiliary/tracking/t_tracker_slam.cpp b/src/xrt/auxiliary/tracking/t_tracker_slam.cpp index 172aae808..2c92d0e3e 100644 --- a/src/xrt/auxiliary/tracking/t_tracker_slam.cpp +++ b/src/xrt/auxiliary/tracking/t_tracker_slam.cpp @@ -174,11 +174,16 @@ extern "C" void t_slam_imu_sink_push(struct xrt_imu_sink *sink, struct xrt_imu_sample *s) { auto &t = *container_of(sink, TrackerSlam, imu_sink); + + timepoint_ns ts = s->timestamp_ns; + xrt_vec3_f64 a = s->accel_m_s2; + xrt_vec3_f64 w = s->gyro_rad_secs; + //! @todo There are many conversions like these between xrt and //! slam_tracker.hpp types. Implement a casting mechanism to avoid copies. - imu_sample sample{s->timestamp, s->ax, s->ay, s->az, s->wx, s->wy, s->wz}; + imu_sample sample{ts, a.x, a.y, a.z, w.x, w.y, w.z}; t.slam->push_imu_sample(sample); - SLAM_TRACE("imu t=%ld a=[%f,%f,%f] w=[%f,%f,%f]", s->timestamp, s->ax, s->ay, s->az, s->wx, s->wy, s->wz); + SLAM_TRACE("imu t=%ld a=[%f,%f,%f] w=[%f,%f,%f]", ts, a.x, a.y, a.z, w.x, w.y, w.z); } /*! diff --git a/src/xrt/drivers/euroc/euroc_player.cpp b/src/xrt/drivers/euroc/euroc_player.cpp index 19d759169..206c01ce0 100644 --- a/src/xrt/drivers/euroc/euroc_player.cpp +++ b/src/xrt/drivers/euroc/euroc_player.cpp @@ -159,7 +159,7 @@ euroc_player_preload_imu_data(struct euroc_player *ep, set_base_ts = false; } - xrt_imu_sample sample{timestamp, v[3], v[4], v[5], v[0], v[1], v[2]}; + xrt_imu_sample sample{timestamp, {v[3], v[4], v[5]}, {v[0], v[1], v[2]}}; samples->push_back(sample); } return true; @@ -220,7 +220,7 @@ euroc_player_user_skip(struct euroc_player *ep) { timepoint_ns skip_first_ns = ep->playback.skip_first_s * 1000 * 1000 * 1000; - while (ep->imus->at(ep->imu_seq).timestamp < skip_first_ns) { + while (ep->imus->at(ep->imu_seq).timestamp_ns < skip_first_ns) { ep->imu_seq++; } @@ -343,7 +343,7 @@ euroc_player_is_imu_next(struct euroc_player *ep) } bool more_imgs = ep->img_seq < ep->left_imgs->size(); - timepoint_ns imu_ts = more_imus ? ep->imus->at(ep->imu_seq).timestamp : INT64_MAX; + timepoint_ns imu_ts = more_imus ? ep->imus->at(ep->imu_seq).timestamp_ns : INT64_MAX; timepoint_ns img_ts = more_imgs ? ep->left_imgs->at(ep->img_seq).first : INT64_MAX; return imu_ts < img_ts; } @@ -356,7 +356,7 @@ euroc_player_push_next_sample(struct euroc_player *ep) // Push next IMU sample if (euroc_player_is_imu_next(ep)) { xrt_imu_sample sample = ep->imus->at(ep->imu_seq++); - sample.timestamp = euroc_player_mapped_ts(ep, sample.timestamp); + sample.timestamp_ns = euroc_player_mapped_ts(ep, sample.timestamp_ns); xrt_sink_push_imu(ep->in_sinks.imu, &sample); return; } @@ -502,15 +502,18 @@ receive_imu_sample(struct xrt_imu_sink *sink, struct xrt_imu_sample *s) { struct euroc_player *ep = container_of(sink, struct euroc_player, imu_sink); + timepoint_ns ts = s->timestamp_ns; + xrt_vec3_f64 a = s->accel_m_s2; + xrt_vec3_f64 w = s->gyro_rad_secs; + // UI log - const xrt_vec3 gyro{(float)s->wx, (float)s->wy, (float)s->wz}; - const xrt_vec3 accel{(float)s->ax, (float)s->ay, (float)s->az}; - m_ff_vec3_f32_push(ep->gyro_ff, &gyro, s->timestamp); - m_ff_vec3_f32_push(ep->accel_ff, &accel, s->timestamp); + const xrt_vec3 gyro{(float)w.x, (float)w.y, (float)w.z}; + const xrt_vec3 accel{(float)a.x, (float)a.y, (float)a.z}; + m_ff_vec3_f32_push(ep->gyro_ff, &gyro, ts); + m_ff_vec3_f32_push(ep->accel_ff, &accel, ts); // Trace log - EUROC_TRACE(ep, "imu t=%ld ax=%f ay=%f az=%f wx=%f wy=%f wz=%f", s->timestamp, s->ax, s->ay, s->az, s->wx, - s->wy, s->wz); + EUROC_TRACE(ep, "imu t=%ld ax=%f ay=%f az=%f wx=%f wy=%f wz=%f", ts, a.x, a.y, a.z, w.x, w.y, w.z); if (ep->out_sinks.imu) { xrt_sink_push_imu(ep->out_sinks.imu, s); } diff --git a/src/xrt/drivers/realsense/rs_hdev.c b/src/xrt/drivers/realsense/rs_hdev.c index b0cd18fa8..6c3bf552e 100644 --- a/src/xrt/drivers/realsense/rs_hdev.c +++ b/src/xrt/drivers/realsense/rs_hdev.c @@ -547,7 +547,7 @@ partial_imu_sample_push(struct rs_hdev *rs, timepoint_ns ts, struct xrt_vec3 val if (should_submit) { struct xrt_vec3 gyro = rs->partial_imu_sample.gyro; struct xrt_vec3 accel = rs->partial_imu_sample.accel; - struct xrt_imu_sample sample = {ts, accel.x, accel.y, accel.z, gyro.x, gyro.y, gyro.z}; + struct xrt_imu_sample sample = {ts, {accel.x, accel.y, accel.z}, {gyro.x, gyro.y, gyro.z}}; RS_TRACE("imu t=%ld a=%f,%f,%f w=%f,%f,%f", ts, accel.x, accel.y, accel.z, gyro.x, gyro.y, gyro.z); xrt_sink_push_imu(rs->in_sinks.imu, &sample); @@ -862,7 +862,12 @@ static void receive_imu_sample(struct xrt_imu_sink *sink, struct xrt_imu_sample *s) { struct rs_hdev *rs = container_of(sink, struct rs_hdev, imu_sink); - RS_TRACE("imu t=%ld a=(%f %f %f) w=(%f %f %f)", s->timestamp, s->ax, s->ay, s->az, s->wx, s->wy, s->wz); + + timepoint_ns ts = s->timestamp_ns; + struct xrt_vec3_f64 a = s->accel_m_s2; + struct xrt_vec3_f64 w = s->gyro_rad_secs; + RS_TRACE("imu t=%ld a=(%f %f %f) w=(%f %f %f)", ts, a.x, a.y, a.z, w.x, w.y, w.z); + if (rs->out_sinks.imu) { xrt_sink_push_imu(rs->out_sinks.imu, s); } diff --git a/src/xrt/include/xrt/xrt_tracking.h b/src/xrt/include/xrt/xrt_tracking.h index b9a0c902d..2887eef4c 100644 --- a/src/xrt/include/xrt/xrt_tracking.h +++ b/src/xrt/include/xrt/xrt_tracking.h @@ -137,8 +137,9 @@ struct xrt_tracking_sample */ struct xrt_imu_sample { - timepoint_ns timestamp; - double ax, ay, az, wx, wy, wz; + timepoint_ns timestamp_ns; + struct xrt_vec3_f64 accel_m_s2; + struct xrt_vec3_f64 gyro_rad_secs; }; /*!