bugfix(server_core) Fix judder regression caused by streamer-client p…

…ose desync (#2660)

* Fix judder regression

* Actual fix lol

* Use statistics history size

* Prevent client timestamps from running backwards

* oops

* History maximum once on init

* oops, it deadlocked

* nit

alvr/client_core/src/lib.rs

@@ -242,6 +242,9 @@ impl ClientCoreContext {
             poll_timestamp
         };
 
+        // Guarantee that sent timestamps never go backwards by sending the poll time
+        let reported_timestamp = poll_timestamp;
+
         for (id, motion) in &mut device_motions {
             let velocity_multiplier = *self.connection_context.velocities_multiplier.read();
             motion.linear_velocity *= velocity_multiplier;
@@ -252,7 +255,7 @@ impl ClientCoreContext {
 
                 let mut head_pose_queue = self.connection_context.head_pose_queue.write();
 
-                head_pose_queue.push_back((target_timestamp, motion.pose));
+                head_pose_queue.push_back((reported_timestamp, motion.pose));
 
                 while head_pose_queue.len() > 1024 {
                     head_pose_queue.pop_front();
@@ -299,15 +302,15 @@ impl ClientCoreContext {
         if let Some(sender) = &mut *self.connection_context.tracking_sender.lock() {
             sender
                 .send_header(&Tracking {
-                    target_timestamp,
+                    target_timestamp: reported_timestamp,
                     device_motions,
                     hand_skeletons,
                     face_data,
                 })
                 .ok();
 
             if let Some(stats) = &mut *self.connection_context.statistics_manager.lock() {
-                stats.report_input_acquired(target_timestamp);
+                stats.report_input_acquired(reported_timestamp);
             }
         }
     }

alvr/server_core/src/connection.rs

@@ -1012,7 +1012,8 @@ fn connection_pipeline(
             thread::spawn(|| ())
         };
 
-    *ctx.tracking_manager.write() = TrackingManager::new();
+    *ctx.tracking_manager.write() =
+        TrackingManager::new(initial_settings.connection.statistics_history_size);
     let hand_gesture_manager = Arc::new(Mutex::new(HandGestureManager::new()));
 
     let tracking_receive_thread = thread::spawn({

alvr/server_core/src/lib.rs

@@ -215,7 +215,9 @@ impl ServerCoreContext {
             events_sender,
             statistics_manager: RwLock::new(Some(stats)),
             bitrate_manager: Mutex::new(BitrateManager::new(256, 60.0)),
-            tracking_manager: RwLock::new(TrackingManager::new()),
+            tracking_manager: RwLock::new(TrackingManager::new(
+                initial_settings.connection.statistics_history_size,
+            )),
             decoder_config: Mutex::new(None),
             video_mirror_sender: Mutex::new(None),
             video_recording_file: Mutex::new(None),

alvr/server_core/src/tracking/mod.rs

@@ -35,9 +35,6 @@ use std::{
 };
 
 const DEG_TO_RAD: f32 = PI / 180.0;
-// A small history size is used, we just need to cover the time from packet received to data
-// processed.
-const MAX_HISTORY_SIZE: usize = 16;
 
 #[derive(Debug)]
 pub enum HandType {
@@ -60,16 +57,18 @@ pub struct TrackingManager {
     device_motions_history: HashMap<u64, VecDeque<(Duration, DeviceMotion)>>,
     hand_skeletons_history: [VecDeque<(Duration, [Pose; 26])>; 2],
     last_face_data: FaceData,
+    max_history_size: usize,
 }
 
 impl TrackingManager {
-    pub fn new() -> TrackingManager {
+    pub fn new(max_history_size: usize) -> TrackingManager {
         TrackingManager {
             last_head_pose: Pose::default(),
             inverse_recentering_origin: Pose::default(),
             device_motions_history: HashMap::new(),
             hand_skeletons_history: [VecDeque::new(), VecDeque::new()],
             last_face_data: FaceData::default(),
+            max_history_size,
         }
     }
 
@@ -124,7 +123,7 @@ impl TrackingManager {
     // Performs all kinds of tracking transformations, driven by settings.
     pub fn report_device_motions(
         &mut self,
-        config: &HeadsetConfig,
+        headset_config: &HeadsetConfig,
         timestamp: Duration,
         device_motions: &[(u64, DeviceMotion)],
     ) {
@@ -141,7 +140,7 @@ impl TrackingManager {
             (*BODY_RIGHT_FOOT_ID, MotionConfig::default()),
         ]);
 
-        if let Switch::Enabled(controllers) = &config.controllers {
+        if let Switch::Enabled(controllers) = &headset_config.controllers {
             let t = controllers.left_controller_position_offset;
             let r = controllers.left_controller_rotation_offset;
 
@@ -219,7 +218,7 @@ impl TrackingManager {
             if let Some(motions) = self.device_motions_history.get_mut(&device_id) {
                 motions.push_front((timestamp, motion));
 
-                if motions.len() > MAX_HISTORY_SIZE {
+                if motions.len() > self.max_history_size {
                     motions.pop_back();
                 }
             } else {
@@ -247,7 +246,7 @@ impl TrackingManager {
                     // Note: we are iterating from most recent to oldest
                     for (ts, m) in motions {
                         match ts.cmp(&sample_timestamp) {
-                            Ordering::Equal => return Some(*best_motion_ref),
+                            Ordering::Equal => return Some(*m),
                             Ordering::Greater => {
                                 let diff = ts.saturating_sub(sample_timestamp);
                                 if diff < best_timestamp_diff {
@@ -280,7 +279,7 @@ impl TrackingManager {
 
         skeleton_history.push_back((timestamp, skeleton));
 
-        if skeleton_history.len() > MAX_HISTORY_SIZE {
+        if skeleton_history.len() > self.max_history_size {
             skeleton_history.pop_front();
         }
     }