Stabilize recentering with marker pose average

Author: zmerp

alvr/client_openxr/src/extra_extensions/spatial_marker_tracking.rs

@@ -132,7 +132,7 @@ impl QRCodesSpatialContext {
         let new = Instant::now();
 
         if self.snapshot_future.is_none()
-            //&& self.discovery_enabled.value()
+            && self.discovery_enabled.value()
             && new > self.discovery_timeout_deadline
         {
             let snapshot_create_info = sys::SpatialDiscoverySnapshotCreateInfoEXT {
@@ -244,13 +244,12 @@ impl QRCodesSpatialContext {
 
         let mut out_markers = vec![];
         for idx in 0..query_result.entity_id_count_output as usize {
-            alvr_common::error!("Parsing marker");
             if self.entity_states[idx] != sys::SpatialEntityTrackingStateEXT::TRACKING
                 || self.marker_arr[idx].capability != CAPABILITY
                 || self.marker_arr[idx].data.buffer_id == sys::SpatialBufferIdEXT::NULL
                 || self.marker_arr[idx].data.buffer_type != sys::SpatialBufferTypeEXT::STRING
             {
-                alvr_common::error!(
+                alvr_common::debug!(
                     "Parsing marker failed! {:?} {:?} {:?} {:?}",
                     self.entity_states[idx],
                     self.marker_arr[idx].capability,

alvr/common/src/average.rs

@@ -1,3 +1,4 @@
+use glam::Vec2;
 use std::{collections::VecDeque, time::Duration};
 
 pub struct SlidingWindowAverage<T> {
@@ -38,3 +39,40 @@ impl SlidingWindowAverage<Duration> {
         self.history_buffer.iter().sum::<Duration>() / self.history_buffer.len() as u32
     }
 }
+
+impl SlidingWindowAverage<Vec2> {
+    pub fn get_average(&self) -> Vec2 {
+        self.history_buffer.iter().sum::<Vec2>() / self.history_buffer.len() as f32
+    }
+}
+
+pub struct AngleSlidingWindowAverage {
+    history_buffer: VecDeque<Vec2>,
+    max_history_size: usize,
+}
+
+impl AngleSlidingWindowAverage {
+    pub fn new(initial_value: f32, max_history_size: usize) -> Self {
+        Self {
+            history_buffer: [Vec2::from_angle(initial_value)].into_iter().collect(),
+            max_history_size,
+        }
+    }
+
+    pub fn submit_sample(&mut self, sample: f32) {
+        if self.history_buffer.len() >= self.max_history_size {
+            self.history_buffer.pop_front();
+        }
+
+        self.history_buffer.push_back(Vec2::from_angle(sample));
+    }
+
+    pub fn retain(&mut self, count: usize) {
+        self.history_buffer
+            .drain(0..self.history_buffer.len().saturating_sub(count));
+    }
+
+    pub fn get_average(&self) -> f32 {
+        self.history_buffer.iter().sum::<Vec2>().to_angle()
+    }
+}

alvr/server_core/src/tracking/mod.rs

@@ -13,9 +13,10 @@ use crate::{
     input_mapping::ButtonMappingManager,
 };
 use alvr_common::{
-    BODY_CHEST_ID, BODY_HIPS_ID, BODY_LEFT_ELBOW_ID, BODY_LEFT_FOOT_ID, BODY_LEFT_KNEE_ID,
-    BODY_RIGHT_ELBOW_ID, BODY_RIGHT_FOOT_ID, BODY_RIGHT_KNEE_ID, ConnectionError,
-    DEVICE_ID_TO_PATH, DeviceMotion, HAND_LEFT_ID, HAND_RIGHT_ID, HEAD_ID, Pose, ViewParams,
+    AngleSlidingWindowAverage, BODY_CHEST_ID, BODY_HIPS_ID, BODY_LEFT_ELBOW_ID, BODY_LEFT_FOOT_ID,
+    BODY_LEFT_KNEE_ID, BODY_RIGHT_ELBOW_ID, BODY_RIGHT_FOOT_ID, BODY_RIGHT_KNEE_ID,
+    ConnectionError, DEVICE_ID_TO_PATH, DeviceMotion, HAND_LEFT_ID, HAND_RIGHT_ID, HEAD_ID, Pose,
+    SlidingWindowAverage, ViewParams,
     glam::{Quat, Vec2, Vec3},
     parking_lot::Mutex,
 };
@@ -42,6 +43,12 @@ pub enum HandType {
     Right = 1,
 }
 
+struct RecenteringMarker {
+    string: String,
+    average_angle: AngleSlidingWindowAverage,
+    average_position: SlidingWindowAverage<Vec2>,
+}
+
 // todo: Move this struct to Settings and use it for every tracked device
 #[derive(Default)]
 struct MotionConfig {
@@ -55,6 +62,7 @@ pub struct TrackingManager {
     last_head_pose: Pose,             // client's reference space
     inverse_recentering_origin: Pose, // client's reference space
     device_motions_history: HashMap<u64, VecDeque<(Duration, DeviceMotion)>>,
+    recentering_marker: Option<RecenteringMarker>,
     markers: HashMap<String, Pose>,
     hand_skeletons_history: [VecDeque<(Duration, [Pose; 26])>; 2],
     max_history_size: usize,
@@ -66,6 +74,7 @@ impl TrackingManager {
             last_head_pose: Pose::IDENTITY,
             inverse_recentering_origin: Pose::IDENTITY,
             device_motions_history: HashMap::new(),
+            recentering_marker: None,
             markers: HashMap::new(),
             hand_skeletons_history: [VecDeque::new(), VecDeque::new()],
             max_history_size,
@@ -84,35 +93,61 @@ impl TrackingManager {
                         let marker_z_axis = marker_pose.orientation * Vec3::Z;
                         let angle_from_y = Vec3::angle_between(marker_z_axis, Vec3::Y);
 
-                        let orientation = if (angle_from_y - FRAC_PI_2).abs() < FRAC_PI_4 {
+                        let marker_y_angle = if (angle_from_y - FRAC_PI_2).abs() < FRAC_PI_4 {
                             // The marker is vertical
-                            let y_angle = Vec2::new(marker_z_axis.x, marker_z_axis.z)
+                            Vec2::new(marker_z_axis.x, marker_z_axis.z)
                                 .normalize()
-                                .angle_to(Vec2::Y); // (this Y is on the XZ plane -> Z)
-                            Quat::from_rotation_y(y_angle)
+                                .angle_to(Vec2::Y) // (this Y is on the XZ plane -> Z)
                         } else {
                             let marker_x_axis = marker_pose.orientation * Vec3::X;
-                            let y_angle = Vec2::new(marker_x_axis.x, marker_x_axis.z)
+                            Vec2::new(marker_x_axis.x, marker_x_axis.z)
                                 .normalize()
-                                .angle_to(Vec2::X);
-                            Quat::from_rotation_y(y_angle)
+                                .angle_to(Vec2::X)
+                        };
+                        let marker_floor_position =
+                            Vec2::new(marker_pose.position.x, marker_pose.position.z);
+
+                        self.recentering_marker
+                            .take_if(|rm| rm.string != colocation_config.qr_code_string);
+                        let recentering_marker = if let Some(rm) = &mut self.recentering_marker {
+                            rm.average_angle.submit_sample(marker_y_angle);
+                            rm.average_position.submit_sample(marker_floor_position);
+                            rm
+                        } else {
+                            self.recentering_marker.insert(RecenteringMarker {
+                                string: colocation_config.qr_code_string.clone(),
+                                average_angle: AngleSlidingWindowAverage::new(
+                                    marker_y_angle,
+                                    self.max_history_size,
+                                ),
+                                average_position: SlidingWindowAverage::new(
+                                    marker_floor_position,
+                                    self.max_history_size,
+                                ),
+                            })
                         };
 
+                        let average_angle =
+                            Quat::from_rotation_y(recentering_marker.average_angle.get_average());
                         let position = {
-                            let marker_offset_2d = Vec3::new(
+                            let marker_offset_2d = Vec2::new(
                                 colocation_config.floor_offset[0],
-                                0.0,
                                 colocation_config.floor_offset[1],
                             );
-                            let marker_position_2d =
-                                Vec3::new(marker_pose.position.x, 0.0, marker_pose.position.z);
 
-                            marker_position_2d - marker_offset_2d
+                            let offset_2d = recentering_marker.average_position.get_average()
+                                - marker_offset_2d;
+                            Vec3::new(offset_2d.x, 0.0, offset_2d.y)
                         };
+                        alvr_common::debug!(
+                            "Recentering from marker. Angle: {average_angle}, Position: {position}"
+                        );
 
                         Pose {
                             position,
-                            orientation,
+                            orientation: Quat::from_rotation_y(
+                                recentering_marker.average_angle.get_average(),
+                            ),
                         }
                     } else {
                         // In case the marker is not found, abort recentering, we still want to use
@@ -432,13 +467,15 @@ pub fn tracking_loop(
                 &tracking.device_motions,
             );
 
-            tracking_manager_lock.report_markers(tracking.markers);
-            if matches!(
-                headset_config.recentering_mode,
-                RecenteringMode::Stage { .. }
-            ) {
-                tracking_manager_lock.recenter(&headset_config.recentering_mode);
-            };
+            if !tracking.markers.is_empty() {
+                tracking_manager_lock.report_markers(tracking.markers);
+                if matches!(
+                    headset_config.recentering_mode,
+                    RecenteringMode::Stage { .. }
+                ) {
+                    tracking_manager_lock.recenter(&headset_config.recentering_mode);
+                };
+            }
 
             if let Some(skeleton) = tracking.hand_skeletons[0] {
                 tracking_manager_lock.report_hand_skeleton(HandType::Left, timestamp, skeleton);