d/psmv: Port positional tracking to use t_imu_fusion with timestamps

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

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<double>();
 		}
 		imu.handleAccel(map_vec3(sample->accel_m_s2).cast<double>(),
-		                dt);
+		                timestamp_ns);
 		imu.handleGyro(map_vec3(sample->gyro_rad_secs).cast<double>(),
-		               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(
+		float dt = time_ns_to_s(when_ns - filter_time_ns);
-		    filter_state, process_model,
+		auto predicted_state =
-		    /*! @todo compute dt here */ 0.024);
+		    flexkalman::getPrediction(filter_state, process_model, dt);
 
 		map_vec3(out_relation->pose.position) =
 		    predicted_state.position().cast<float>();
@@ -253,4 +258,4 @@ PSMVFusionInterface::create()
 	auto ret = std::make_unique<PSMVFusion>();
 	return ret;
 }
-} // namespace xrt_fusion
+} // namespace xrt_fusion

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
+} // namespace xrt_fusion

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

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