Skip to main content

vibe/cosmic/
ar.rs

1//! Augmented Reality spatial anchors and VIO frame hierarchy (OCS §5).
2//!
3//! Reference: OCS Specification v2.2.0 §5.
4
5use crate::cosmic::transforms::Geodetic;
6use crate::value::Value;
7use std::collections::BTreeMap;
8
9/// AR frame hierarchy level (OCS §5.1).
10#[derive(Debug, Clone, Copy, PartialEq, Eq, Hash)]
11pub enum ArFrameLevel {
12    /// L_6.0 — Local geodetic anchor (GPS/RTK/VPS).
13    L60GeodeticAnchor,
14    /// L_6.1 — VIO/SLAM metric world root.
15    L61VioWorldRoot,
16    /// L_6.2 — Head/device pose frame (6-DOF, 1000 Hz).
17    L62HeadPose,
18    /// L_6.3 — Eye & display frustum frame.
19    L63EyeFrustum,
20}
21
22impl ArFrameLevel {
23    /// `str` form.
24    pub fn as_str(&self) -> &'static str {
25        match self {
26            Self::L60GeodeticAnchor => "L_6.0",
27            Self::L61VioWorldRoot => "L_6.1",
28            Self::L62HeadPose => "L_6.2",
29            Self::L63EyeFrustum => "L_6.3",
30        }
31    }
32}
33
34/// A persistent spatial anchor — world lock (OCS §5.2).
35#[derive(Debug, Clone, PartialEq)]
36pub struct SpatialAnchor {
37    pub anchor_id: String,
38    pub parent_frame: String,
39    pub geodetic_anchor: [f64; 3],  // [lat, lon, alt]
40    pub enu_offset: [f32; 3],       // Local [East, North, Up] in meters
41    pub orientation_quat: [f32; 4], // [w, x, y, z]
42    pub confidence_radius_mm: f32,
43    pub vps_mesh_signature: Vec<u8>,
44}
45
46impl SpatialAnchor {
47    /// Create a new spatial anchor at a geodetic position.
48    pub fn new(anchor_id: &str, geodetic: [f64; 3]) -> Self {
49        Self {
50            anchor_id: anchor_id.into(),
51            parent_frame: "urn:omni:v1:physical:observable:standard:earth:wgs84".into(),
52            geodetic_anchor: geodetic,
53            enu_offset: [0.0, 0.0, 0.0],
54            orientation_quat: [1.0, 0.0, 0.0, 0.0], // Identity quaternion
55            confidence_radius_mm: 10.0,
56            vps_mesh_signature: Vec::new(),
57        }
58    }
59
60    /// Set ENU offset from the geodetic anchor.
61    pub fn with_enu_offset(mut self, east: f32, north: f32, up: f32) -> Self {
62        self.enu_offset = [east, north, up];
63        self
64    }
65
66    /// Set orientation quaternion [w, x, y, z].
67    pub fn with_orientation(mut self, w: f32, x: f32, y: f32, z: f32) -> Self {
68        self.orientation_quat = [w, x, y, z];
69        self
70    }
71
72    /// Set confidence radius in mm.
73    pub fn with_confidence(mut self, radius_mm: f32) -> Self {
74        self.confidence_radius_mm = radius_mm;
75        self
76    }
77
78    /// Get the geodetic position of this anchor.
79    pub fn geodetic(&self) -> Geodetic {
80        Geodetic {
81            lat_deg: self.geodetic_anchor[0],
82            lon_deg: self.geodetic_anchor[1],
83            alt_m: self.geodetic_anchor[2],
84        }
85    }
86
87    /// Check if this anchor is sub-millimeter precision (OCS-T05).
88    pub fn is_submillimeter(&self) -> bool {
89        self.confidence_radius_mm < 1.0
90    }
91
92    /// Verify the quaternion is normalized.
93    pub fn is_quaternion_valid(&self) -> bool {
94        let [w, x, y, z] = self.orientation_quat;
95        let norm = (w * w + x * x + y * y + z * z).sqrt();
96        (norm - 1.0).abs() < 1e-5
97    }
98
99    /// Converts to value.
100    pub fn to_value(&self) -> Value {
101        let mut rec = BTreeMap::new();
102        rec.insert("anchor_id".into(), Value::String(self.anchor_id.clone()));
103        rec.insert(
104            "parent_frame".into(),
105            Value::String(self.parent_frame.clone()),
106        );
107        rec.insert(
108            "geodetic_anchor".into(),
109            Value::List(
110                self.geodetic_anchor
111                    .iter()
112                    .map(|v| Value::F64(*v))
113                    .collect(),
114            ),
115        );
116        rec.insert(
117            "enu_offset".into(),
118            Value::List(
119                self.enu_offset
120                    .iter()
121                    .map(|v| Value::F64(*v as f64))
122                    .collect(),
123            ),
124        );
125        rec.insert(
126            "orientation_quat".into(),
127            Value::List(
128                self.orientation_quat
129                    .iter()
130                    .map(|v| Value::F64(*v as f64))
131                    .collect(),
132            ),
133        );
134        rec.insert(
135            "confidence_radius_mm".into(),
136            Value::F64(self.confidence_radius_mm as f64),
137        );
138        Value::Record(rec)
139    }
140}
141
142/// Head pose — 6-DOF tracking transform (OCS §5.1, L_6.2).
143#[derive(Debug, Clone, Copy, PartialEq)]
144pub struct HeadPose {
145    /// Position [x, y, z] in VIO world frame (meters)
146    pub position: [f32; 3],
147    /// Orientation quaternion [w, x, y, z]
148    pub orientation: [f32; 4],
149    /// Timestamp (monotonic nanos)
150    pub timestamp_ns: u64,
151}
152
153impl HeadPose {
154    /// Identity quaternion/transform.
155    pub fn identity() -> Self {
156        Self {
157            position: [0.0, 0.0, 0.0],
158            orientation: [1.0, 0.0, 0.0, 0.0],
159            timestamp_ns: 0,
160        }
161    }
162
163    /// Apply a translation to the head pose.
164    pub fn translated(&self, dx: f32, dy: f32, dz: f32) -> Self {
165        Self {
166            position: [
167                self.position[0] + dx,
168                self.position[1] + dy,
169                self.position[2] + dz,
170            ],
171            orientation: self.orientation,
172            timestamp_ns: self.timestamp_ns,
173        }
174    }
175}
176
177/// VIO/SLAM metric world root (OCS §5.1, L_6.1).
178#[derive(Debug, Clone)]
179pub struct VioWorldRoot {
180    /// The geodetic anchor where VIO was initialized
181    pub init_anchor: SpatialAnchor,
182    /// Head pose history (most recent first)
183    pub pose_history: Vec<HeadPose>,
184    /// Whether the world root is stable (has converged)
185    pub is_stable: bool,
186}
187
188impl VioWorldRoot {
189    /// Creates a new `VioWorldRoot`.
190    pub fn new(anchor: SpatialAnchor) -> Self {
191        Self {
192            init_anchor: anchor,
193            pose_history: Vec::new(),
194            is_stable: false,
195        }
196    }
197
198    /// Push a new head pose into the history.
199    pub fn update_pose(&mut self, pose: HeadPose) {
200        self.pose_history.push(pose);
201        // Mark as stable after 30 poses (~30ms at 1000Hz)
202        if self.pose_history.len() >= 30 {
203            self.is_stable = true;
204        }
205    }
206
207    /// Get the current (most recent) head pose.
208    pub fn current_pose(&self) -> Option<&HeadPose> {
209        self.pose_history.last()
210    }
211
212    /// Check anchor stability under camera translation (OCS-T05).
213    /// Returns true if the anchor maintains sub-millimeter stability.
214    pub fn check_stability(&self) -> bool {
215        if self.pose_history.len() < 2 {
216            return false;
217        }
218        // Check that the init anchor has sub-mm confidence
219        if !self.init_anchor.is_submillimeter() {
220            return false;
221        }
222        // Check that recent poses haven't jumped dramatically
223        let recent = &self.pose_history;
224        if recent.len() < 2 {
225            return false;
226        }
227        let last = recent[recent.len() - 1];
228        let prev = recent[recent.len() - 2];
229        let dx = last.position[0] - prev.position[0];
230        let dy = last.position[1] - prev.position[1];
231        let dz = last.position[2] - prev.position[2];
232        let drift = (dx * dx + dy * dy + dz * dz).sqrt();
233        // Drift between frames should be < 0.5mm (OCS-T05)
234        drift < 0.0005
235    }
236}
237
238#[cfg(test)]
239mod tests {
240    use super::*;
241
242    #[test]
243    fn spatial_anchor_default() {
244        let a = SpatialAnchor::new("anchor-1", [37.8080, -122.4177, 10.0]);
245        assert_eq!(a.anchor_id, "anchor-1");
246        assert_eq!(a.enu_offset, [0.0, 0.0, 0.0]);
247        assert!(a.is_quaternion_valid()); // Identity quaternion
248    }
249
250    #[test]
251    fn spatial_anchor_with_offset() {
252        let a = SpatialAnchor::new("anchor-1", [37.8, -122.4, 10.0]).with_enu_offset(5.0, 3.0, 1.0);
253        assert_eq!(a.enu_offset, [5.0, 3.0, 1.0]);
254    }
255
256    #[test]
257    fn spatial_anchor_submillimeter() {
258        let a = SpatialAnchor::new("precise", [37.8, -122.4, 10.0]).with_confidence(0.5);
259        assert!(a.is_submillimeter());
260    }
261
262    #[test]
263    fn spatial_anchor_not_submillimeter() {
264        let a = SpatialAnchor::new("rough", [37.8, -122.4, 10.0]).with_confidence(50.0);
265        assert!(!a.is_submillimeter());
266    }
267
268    #[test]
269    fn spatial_anchor_geodetic() {
270        let a = SpatialAnchor::new("a", [37.8, -122.4, 10.0]);
271        let g = a.geodetic();
272        assert_eq!(g.lat_deg, 37.8);
273        assert_eq!(g.lon_deg, -122.4);
274    }
275
276    #[test]
277    fn spatial_anchor_to_value() {
278        let a = SpatialAnchor::new("test", [37.8, -122.4, 10.0]);
279        let v = a.to_value();
280        match v {
281            Value::Record(r) => {
282                assert_eq!(r.get("anchor_id"), Some(&Value::String("test".into())));
283                assert!(r.contains_key("geodetic_anchor"));
284            }
285            _ => panic!("expected Record"),
286        }
287    }
288
289    #[test]
290    fn head_pose_identity() {
291        let p = HeadPose::identity();
292        assert_eq!(p.position, [0.0, 0.0, 0.0]);
293        assert_eq!(p.orientation, [1.0, 0.0, 0.0, 0.0]);
294    }
295
296    #[test]
297    fn head_pose_translated() {
298        let p = HeadPose::identity();
299        let p2 = p.translated(1.0, 2.0, 3.0);
300        assert_eq!(p2.position, [1.0, 2.0, 3.0]);
301    }
302
303    #[test]
304    fn vio_world_root_stability() {
305        let anchor = SpatialAnchor::new("root", [37.8, -122.4, 10.0]).with_confidence(0.3);
306        let mut root = VioWorldRoot::new(anchor);
307        // Add 30 stable poses
308        for i in 0..30 {
309            root.update_pose(HeadPose {
310                position: [i as f32 * 0.0001, 0.0, 0.0], // Very small movements
311                orientation: [1.0, 0.0, 0.0, 0.0],
312                timestamp_ns: i as u64 * 1_000_000,
313            });
314        }
315        assert!(root.is_stable);
316        assert!(root.check_stability());
317    }
318
319    #[test]
320    fn vio_world_root_unstable_with_drift() {
321        let anchor = SpatialAnchor::new("root", [37.8, -122.4, 10.0]).with_confidence(0.3);
322        let mut root = VioWorldRoot::new(anchor);
323        root.update_pose(HeadPose::identity());
324        // Large drift between frames
325        root.update_pose(HeadPose {
326            position: [10.0, 0.0, 0.0], // 10 meter jump!
327            orientation: [1.0, 0.0, 0.0, 0.0],
328            timestamp_ns: 1_000_000,
329        });
330        assert!(!root.check_stability());
331    }
332
333    #[test]
334    fn ar_frame_level_names() {
335        assert_eq!(ArFrameLevel::L60GeodeticAnchor.as_str(), "L_6.0");
336        assert_eq!(ArFrameLevel::L63EyeFrustum.as_str(), "L_6.3");
337    }
338
339    #[test]
340    fn quaternion_validity_check() {
341        let mut a = SpatialAnchor::new("a", [0.0; 3]);
342        a.orientation_quat = [2.0, 0.0, 0.0, 0.0]; // Not normalized
343        assert!(!a.is_quaternion_valid());
344    }
345}