From 194938c405ac3adb0ac58d24e7784427ff0f7bb1 Mon Sep 17 00:00:00 2001 From: Ryan Pavlik Date: Mon, 11 Nov 2019 15:09:04 -0600 Subject: [PATCH] d/psmv: Port positional tracking to use t_imu_fusion with timestamps --- src/xrt/auxiliary/tracking/t_tracker_psmv.cpp | 8 ++--- .../tracking/t_tracker_psmv_fusion.cpp | 31 +++++++++++-------- .../tracking/t_tracker_psmv_fusion.h | 6 ++-- src/xrt/drivers/psmv/psmv_driver.c | 2 +- src/xrt/include/xrt/xrt_tracking.h | 6 ++-- 5 files changed, 29 insertions(+), 24 deletions(-) diff --git a/src/xrt/auxiliary/tracking/t_tracker_psmv.cpp b/src/xrt/auxiliary/tracking/t_tracker_psmv.cpp index 1de25cf32..9c070481c 100644 --- a/src/xrt/auxiliary/tracking/t_tracker_psmv.cpp +++ b/src/xrt/auxiliary/tracking/t_tracker_psmv.cpp @@ -449,7 +449,7 @@ get_pose(TrackerPSMV &t, static void imu_data(TrackerPSMV &t, - time_duration_ns delta_ns, + timepoint_ns timestamp_ns, struct xrt_tracking_sample *sample) { os_thread_helper_lock(&t.oth); @@ -459,7 +459,7 @@ imu_data(TrackerPSMV &t, os_thread_helper_unlock(&t.oth); return; } - t.filter->process_imu_data(delta_ns, sample, NULL); + t.filter->process_imu_data(timestamp_ns, sample, NULL); os_thread_helper_unlock(&t.oth); } @@ -497,11 +497,11 @@ break_apart(TrackerPSMV &t) extern "C" void t_psmv_push_imu(struct xrt_tracked_psmv *xtmv, - time_duration_ns delta_ns, + timepoint_ns timestamp_ns, struct xrt_tracking_sample *sample) { auto &t = *container_of(xtmv, TrackerPSMV, base); - imu_data(t, delta_ns, sample); + imu_data(t, timestamp_ns, sample); } extern "C" void diff --git a/src/xrt/auxiliary/tracking/t_tracker_psmv_fusion.cpp b/src/xrt/auxiliary/tracking/t_tracker_psmv_fusion.cpp index f46203024..6fd0ef7c4 100644 --- a/src/xrt/auxiliary/tracking/t_tracker_psmv_fusion.cpp +++ b/src/xrt/auxiliary/tracking/t_tracker_psmv_fusion.cpp @@ -50,13 +50,13 @@ namespace { clear_position_tracked_flag() override; void - process_imu_data(time_duration_ns delta_ns, + process_imu_data(timepoint_ns timestamp_ns, const struct xrt_tracking_sample *sample, const struct xrt_vec3 *orientation_variance_optional) override; void process_3d_vision_data( - time_duration_ns delta_ns, + timepoint_ns timestamp_ns, const struct xrt_vec3 *position, const struct xrt_vec3 *variance_optional, const struct xrt_vec3 *lever_arm_optional, @@ -78,6 +78,7 @@ namespace { xrt_fusion::SimpleIMUFusion imu; + timepoint_ns filter_time_ns{0}; bool tracked{false}; TrackingInfo orientation_state; TrackingInfo position_state; @@ -108,26 +109,30 @@ namespace { void PSMVFusion::process_imu_data( - time_duration_ns delta_ns, + timepoint_ns timestamp_ns, const struct xrt_tracking_sample *sample, const struct xrt_vec3 *orientation_variance_optional) { - float dt = time_ns_to_s(delta_ns); Eigen::Vector3d variance = Eigen::Vector3d::Constant(0.01); if (orientation_variance_optional) { variance = map_vec3(*orientation_variance_optional) .cast(); } imu.handleAccel(map_vec3(sample->accel_m_s2).cast(), - dt); + timestamp_ns); imu.handleGyro(map_vec3(sample->gyro_rad_secs).cast(), - dt); + timestamp_ns); imu.postCorrect(); //! @todo use better measurements instead of the above "simple //! fusion" - flexkalman::predict(filter_state, process_model, dt); + if (filter_time_ns != 0 && filter_time_ns != timestamp_ns) { + float dt = time_ns_to_s(timestamp_ns - filter_time_ns); + assert(dt > 0); + flexkalman::predict(filter_state, process_model, dt); + } + filter_time_ns = timestamp_ns; auto meas = flexkalman::AbsoluteOrientationMeasurement{ // Must rotate by 180 to align Eigen::Quaterniond( @@ -156,7 +161,7 @@ namespace { void PSMVFusion::process_3d_vision_data( - time_duration_ns delta_ns, + timepoint_ns timestamp_ns, const struct xrt_vec3 *position, const struct xrt_vec3 *variance_optional, const struct xrt_vec3 *lever_arm_optional, @@ -209,12 +214,12 @@ namespace { // Clear to sane values U_ZERO(out_relation); out_relation->pose.orientation.w = 1; - if (!tracked) { + if (!tracked || filter_time_ns == 0) { return; } - auto predicted_state = flexkalman::getPrediction( - filter_state, process_model, - /*! @todo compute dt here */ 0.024); + float dt = time_ns_to_s(when_ns - filter_time_ns); + auto predicted_state = + flexkalman::getPrediction(filter_state, process_model, dt); map_vec3(out_relation->pose.position) = predicted_state.position().cast(); @@ -253,4 +258,4 @@ PSMVFusionInterface::create() auto ret = std::make_unique(); return ret; } -} // namespace xrt_fusion \ No newline at end of file +} // namespace xrt_fusion diff --git a/src/xrt/auxiliary/tracking/t_tracker_psmv_fusion.h b/src/xrt/auxiliary/tracking/t_tracker_psmv_fusion.h index 7ff9c88fc..d81e50f88 100644 --- a/src/xrt/auxiliary/tracking/t_tracker_psmv_fusion.h +++ b/src/xrt/auxiliary/tracking/t_tracker_psmv_fusion.h @@ -33,11 +33,11 @@ public: virtual void process_imu_data( - time_duration_ns delta_ns, + timepoint_ns timestamp_ns, const struct xrt_tracking_sample *sample, const struct xrt_vec3 *orientation_variance_optional) = 0; virtual void - process_3d_vision_data(time_duration_ns delta_ns, + process_3d_vision_data(timepoint_ns timestamp_ns, const struct xrt_vec3 *position, const struct xrt_vec3 *variance_optional, const struct xrt_vec3 *lever_arm_optional, @@ -47,4 +47,4 @@ public: get_prediction(timepoint_ns when_ns, struct xrt_space_relation *out_relation) = 0; }; -} // namespace xrt_fusion \ No newline at end of file +} // namespace xrt_fusion diff --git a/src/xrt/drivers/psmv/psmv_driver.c b/src/xrt/drivers/psmv/psmv_driver.c index 2e2f55f84..f3c38aa69 100644 --- a/src/xrt/drivers/psmv/psmv_driver.c +++ b/src/xrt/drivers/psmv/psmv_driver.c @@ -640,7 +640,7 @@ update_fusion(struct psmv_device *psmv, sample.accel_m_s2 = psmv->read.accel; sample.gyro_rad_secs = psmv->read.gyro; - xrt_tracked_psmv_push_imu(psmv->ball, delta_ns, &sample); + xrt_tracked_psmv_push_imu(psmv->ball, timestamp_ns, &sample); } else { #if 0 diff --git a/src/xrt/include/xrt/xrt_tracking.h b/src/xrt/include/xrt/xrt_tracking.h index a29b7a670..65082d212 100644 --- a/src/xrt/include/xrt/xrt_tracking.h +++ b/src/xrt/include/xrt/xrt_tracking.h @@ -115,7 +115,7 @@ struct xrt_tracked_psmv * Push a IMU sample into the tracking system. */ void (*push_imu)(struct xrt_tracked_psmv *, - time_duration_ns delta_ns, + timepoint_ns timestamp_ns, struct xrt_tracking_sample *sample); /*! @@ -192,10 +192,10 @@ xrt_tracked_psmv_get_tracked_pose(struct xrt_tracked_psmv *psmv, static inline void xrt_tracked_psmv_push_imu(struct xrt_tracked_psmv *psmv, - time_duration_ns delta_ns, + timepoint_ns timestamp_ns, struct xrt_tracking_sample *sample) { - psmv->push_imu(psmv, delta_ns, sample); + psmv->push_imu(psmv, timestamp_ns, sample); } static inline void