long_screen/engine/
pose_graph.rs

1//! The solve pass's pose graph as a state object: keyframe nodes, odometry and loop edges, the loop-closure error
2//! spread along a chain, the resumable relaxation, and the render pass's per-frame correction.
3//!
4//! The graph lives in memory, as it did in the TypeScript `PoseGraph` it replaces: every node and its adjacency.
5//! It does no I/O. A host that keeps the graph in storage feeds the stored rows back with [`PoseGraph::hydrate_node`]
6//! and [`PoseGraph::hydrate_edge`] (all `node/` rows, then all `edge/` rows, in key order), and stores every
7//! [`Write`] a call returns, in order: one `put` or one `putMany` each, exactly the calls the page made before.
8//! Persisted node rows carry no adjacency; edges are rows of their own (`edge/<a>/<b>/<kind>`), one per direction.
9//!
10//! Node order is insertion order (hydration first, then [`PoseGraph::add`]); a node added again under an id it
11//! already has keeps its place. Relaxation visits nodes in that order, so it is part of the result.
12//!
13//! [`PoseGraph::begin_optimize`] returns an [`Optimizer`] that runs a few Gauss-Seidel sweeps per
14//! [`Optimizer::step`] and does not touch the graph until [`PoseGraph::finish_optimize`] writes the relaxed
15//! positions back. A host that stops between steps drops the optimizer: nothing changes and nothing is written.
16
17use crate::kernels::pose_graph::{Edge as SolverEdge, Graph, Node as SolverNode};
18use std::collections::HashMap;
19use std::fmt;
20
21/// Nodes per `putMany` when a loop closure or a relaxation moves many nodes at once.
22pub const FLUSH_CHUNK: usize = 256;
23/// Relaxation stops after this many sweeps even when nodes still move.
24pub const MAX_SWEEPS: u32 = 200;
25/// Relaxation has converged once no node moves farther than this in one sweep.
26pub const CONVERGED_CHANGE: f64 = 0.04;
27
28#[derive(Clone, Copy, Debug, PartialEq, Eq)]
29pub enum EdgeKind {
30    Odometry,
31    Loop,
32}
33
34impl EdgeKind {
35    pub fn as_str(self) -> &'static str {
36        match self {
37            EdgeKind::Odometry => "odometry",
38            EdgeKind::Loop => "loop",
39        }
40    }
41}
42
43/// One direction of an edge, as stored under `edge/<a>/<other>/<kind>`: the measured offset from `a` to `other`.
44#[derive(Clone, Debug, PartialEq)]
45pub struct PoseEdge {
46    pub other: String,
47    pub dx: f64,
48    pub dy: f64,
49    pub weight: f64,
50    pub kind: EdgeKind,
51}
52
53/// One keyframe's pose, as stored under `node/<id>` (whose value also carries an always-empty `edges` list).
54/// `original_*` is the odometry pose it was added at, `x`/`y` its current corrected pose, and `next` the node
55/// added after it on the same odometry chain.
56#[derive(Clone, Debug, PartialEq)]
57pub struct PoseNode {
58    pub id: String,
59    pub canvas_id: String,
60    pub frame: u32,
61    pub x: f64,
62    pub y: f64,
63    pub original_x: f64,
64    pub original_y: f64,
65    pub pinned: bool,
66    pub next: Option<String>,
67}
68
69/// A row to store.
70#[derive(Clone, Debug, PartialEq)]
71pub enum PoseRow {
72    /// Stored under `node/<id>`.
73    Node(PoseNode),
74    /// Stored under `edge/<from>/<edge.other>/<edge.kind>`.
75    Edge { from: String, edge: PoseEdge },
76}
77
78impl PoseRow {
79    pub fn key(&self) -> String {
80        match self {
81            PoseRow::Node(node) => format!("node/{}", node.id),
82            PoseRow::Edge { from, edge } => {
83                format!("edge/{from}/{}/{}", edge.other, edge.kind.as_str())
84            }
85        }
86    }
87}
88
89/// One storage call: a single `put`, or one `putMany` of a batch.
90#[derive(Clone, Debug, PartialEq)]
91pub enum Write {
92    Put(PoseRow),
93    PutMany(Vec<PoseRow>),
94}
95
96#[derive(Clone, Copy, Debug, PartialEq, Eq)]
97pub enum PoseGraphError {
98    /// `add` was given a previous node the graph does not hold.
99    MissingPrevious,
100    /// `connect` or `unpin` named a node the graph does not hold.
101    MissingNode,
102    /// `correction` named a node the graph does not hold.
103    MissingPose,
104}
105
106impl fmt::Display for PoseGraphError {
107    fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
108        f.write_str(match self {
109            PoseGraphError::MissingPrevious => "Pose graph references a missing previous node.",
110            PoseGraphError::MissingNode => "Pose graph references a missing node.",
111            PoseGraphError::MissingPose => "Missing pose for frame rendering.",
112        })
113    }
114}
115
116impl std::error::Error for PoseGraphError {}
117
118/// What a finished relaxation reports (`graph-summary` stores it next to the loop count).
119#[derive(Clone, Copy, Debug, PartialEq)]
120pub struct OptimizeOutcome {
121    pub residual: f64,
122    pub iterations: u32,
123}
124
125/// `Math.max(lo, Math.min(hi, n))`, NaN included.
126fn clamp(n: f64, lo: f64, hi: f64) -> f64 {
127    let low = if n.is_nan() || hi.is_nan() {
128        f64::NAN
129    } else {
130        n.min(hi)
131    };
132    if low.is_nan() || lo.is_nan() {
133        f64::NAN
134    } else {
135        low.max(lo)
136    }
137}
138
139/// `String(n).padStart(10, '0')`.
140fn pad(n: u32) -> String {
141    format!("{n:0>10}")
142}
143
144/// Splits `node` writes into `putMany` batches of [`FLUSH_CHUNK`]; none for an empty list.
145fn flush(rows: Vec<PoseNode>) -> Vec<Write> {
146    let mut writes = Vec::new();
147    let mut rows = rows.into_iter().map(PoseRow::Node).peekable();
148    while rows.peek().is_some() {
149        writes.push(Write::PutMany(rows.by_ref().take(FLUSH_CHUNK).collect()));
150    }
151    writes
152}
153
154#[derive(Default)]
155pub struct PoseGraph {
156    nodes: Vec<PoseNode>,
157    index: HashMap<String, usize>,
158    /// Keyed by node id, edges in the order they were hydrated or connected. May hold ids no node row exists for
159    /// (an edge row whose node row was never written); those edges are kept and only ever looked up by id.
160    adjacency: HashMap<String, Vec<PoseEdge>>,
161    loops: u32,
162}
163
164impl PoseGraph {
165    pub fn new() -> Self {
166        Self::default()
167    }
168
169    /// Loop edges the graph holds, each counted once.
170    pub fn loops(&self) -> u32 {
171        self.loops
172    }
173
174    pub fn len(&self) -> usize {
175        self.nodes.len()
176    }
177
178    pub fn is_empty(&self) -> bool {
179        self.nodes.is_empty()
180    }
181
182    /// The node id for a canvas and frame: `<canvasId>/<frame padded to 10 digits>`.
183    pub fn node_id(canvas_id: &str, frame: u32) -> String {
184        format!("{canvas_id}/{}", pad(frame))
185    }
186
187    pub fn get(&self, id: &str) -> Option<&PoseNode> {
188        self.index.get(id).map(|&i| &self.nodes[i])
189    }
190
191    fn set(&mut self, node: PoseNode) {
192        match self.index.get(&node.id) {
193            Some(&i) => self.nodes[i] = node,
194            None => {
195                self.index.insert(node.id.clone(), self.nodes.len());
196                self.nodes.push(node);
197            }
198        }
199    }
200
201    /// Takes back one stored `node/` row.
202    pub fn hydrate_node(&mut self, node: PoseNode) {
203        self.set(node);
204    }
205
206    /// Takes back one stored `edge/` row under its full key; the owning node's id is what the key holds between
207    /// `edge/` and `/<other>/<kind>`.
208    pub fn hydrate_edge(&mut self, key: &str, edge: PoseEdge) {
209        let suffix = edge.other.len() + edge.kind.as_str().len() + 2;
210        let from = key
211            .get("edge/".len()..key.len().saturating_sub(suffix))
212            .unwrap_or("")
213            .to_string();
214        if edge.kind == EdgeKind::Loop && from < edge.other {
215            self.loops += 1;
216        }
217        self.adjacency.entry(from).or_default().push(edge);
218    }
219
220    /// Adds the node for `canvas_id` at `frame`, observed at odometry pose `(x, y)`. With a `previous` node on the
221    /// same chain it starts from the correction that node has received so far, links to it with an odometry edge of
222    /// `weight` (a step measured on a thin overlap pulls less) and becomes its `next`; without one it is pinned.
223    pub fn add(
224        &mut self,
225        canvas_id: &str,
226        frame: u32,
227        (x, y): (f64, f64),
228        previous: Option<&str>,
229        weight: f64,
230    ) -> Result<(PoseNode, Vec<Write>), PoseGraphError> {
231        let prev = match previous {
232            Some(id) => Some(self.get(id).ok_or(PoseGraphError::MissingPrevious)?.clone()),
233            None => None,
234        };
235        let id = Self::node_id(canvas_id, frame);
236        let node = PoseNode {
237            id: id.clone(),
238            canvas_id: canvas_id.to_string(),
239            frame,
240            x: x + prev.as_ref().map_or(0.0, |p| p.x - p.original_x),
241            y: y + prev.as_ref().map_or(0.0, |p| p.y - p.original_y),
242            original_x: x,
243            original_y: y,
244            pinned: prev.is_none(),
245            next: None,
246        };
247        self.set(node.clone());
248        let mut writes = vec![Write::Put(PoseRow::Node(node.clone()))];
249        if let Some(mut prev) = prev {
250            writes.extend(self.connect(
251                &prev.id,
252                &id,
253                x - prev.original_x,
254                y - prev.original_y,
255                weight,
256                EdgeKind::Odometry,
257            )?);
258            if prev.id == id {
259                // The previous node was just replaced under its own id: the page stored the replaced node with
260                // its `next` set, and kept the new one in the graph unlinked.
261                prev.next = Some(id);
262            } else {
263                let i = self.index[&prev.id];
264                self.nodes[i].next = Some(id);
265                prev = self.nodes[i].clone();
266            }
267            writes.push(Write::Put(PoseRow::Node(prev)));
268        }
269        Ok((node, writes))
270    }
271
272    /// Links `a` to `b` by the offset `(dx, dy)` measured from `a`, once per pair and kind. A loop edge forward
273    /// along `a`'s own odometry chain also spreads the closure error over the nodes after `a`, in proportion to
274    /// their frame, so relaxation starts near the answer.
275    pub fn connect(
276        &mut self,
277        a_id: &str,
278        b_id: &str,
279        dx: f64,
280        dy: f64,
281        weight: f64,
282        kind: EdgeKind,
283    ) -> Result<Vec<Write>, PoseGraphError> {
284        let (Some(&ai), Some(&bi)) = (self.index.get(a_id), self.index.get(b_id)) else {
285            return Err(PoseGraphError::MissingNode);
286        };
287        if a_id == b_id {
288            return Ok(Vec::new());
289        }
290        if self
291            .adjacency
292            .get(a_id)
293            .is_some_and(|list| list.iter().any(|e| e.other == b_id && e.kind == kind))
294        {
295            return Ok(Vec::new());
296        }
297        let a_edge = PoseEdge {
298            other: b_id.to_string(),
299            dx,
300            dy,
301            weight,
302            kind,
303        };
304        let b_edge = PoseEdge {
305            other: a_id.to_string(),
306            dx: -dx,
307            dy: -dy,
308            weight,
309            kind,
310        };
311        self.adjacency
312            .entry(a_id.to_string())
313            .or_default()
314            .push(a_edge.clone());
315        self.adjacency
316            .entry(b_id.to_string())
317            .or_default()
318            .push(b_edge.clone());
319        let mut writes = vec![Write::PutMany(vec![
320            PoseRow::Edge {
321                from: a_id.to_string(),
322                edge: a_edge,
323            },
324            PoseRow::Edge {
325                from: b_id.to_string(),
326                edge: b_edge,
327            },
328        ])];
329        if kind == EdgeKind::Loop {
330            self.loops += 1;
331            let (a, b) = (&self.nodes[ai], &self.nodes[bi]);
332            if a.canvas_id == b.canvas_id && b.frame > a.frame {
333                // Walk the actual chain (`next` pointers), not every node on the canvas: relocalization can leave
334                // several disjoint chains on one canvas, and only the one the loop edge closes may move.
335                let mut chain = Vec::new();
336                let mut cur = a.next.clone();
337                while let Some(id) = cur {
338                    let Some(&i) = self.index.get(&id) else {
339                        break;
340                    };
341                    chain.push(i);
342                    cur = self.nodes[i].next.clone();
343                }
344                if chain.contains(&bi) {
345                    let (ax, ay, a_frame) = (a.x, a.y, a.frame as f64);
346                    let (cx, cy) = (ax + dx - b.x, ay + dy - b.y);
347                    let span = b.frame as f64 - a_frame;
348                    let mut touched = Vec::new();
349                    for i in chain {
350                        let n = &mut self.nodes[i];
351                        if n.pinned {
352                            continue;
353                        }
354                        let t = clamp((n.frame as f64 - a_frame) / span, 0.0, 1.0);
355                        n.x += cx * t;
356                        n.y += cy * t;
357                        touched.push(n.clone());
358                    }
359                    writes.extend(flush(touched));
360                }
361            }
362        }
363        Ok(writes)
364    }
365
366    /// Lets a node move under relaxation (a relocalized or attached node starts pinned until its loop edge is
367    /// wired) and stores it.
368    pub fn unpin(&mut self, id: &str) -> Result<Vec<Write>, PoseGraphError> {
369        let &i = self.index.get(id).ok_or(PoseGraphError::MissingNode)?;
370        self.nodes[i].pinned = false;
371        Ok(vec![Write::Put(PoseRow::Node(self.nodes[i].clone()))])
372    }
373
374    /// How far relaxation moved the pose at `frame` on the chain through node `id`: that node's correction,
375    /// blended towards its `next` node's by how far `frame` lies between them.
376    pub fn correction(&self, id: &str, frame: f64) -> Result<(f64, f64), PoseGraphError> {
377        let a = self.get(id).ok_or(PoseGraphError::MissingPose)?;
378        let (mut x, mut y) = (a.x - a.original_x, a.y - a.original_y);
379        if let Some(b) = a.next.as_deref().and_then(|next| self.get(next)) {
380            if b.frame > a.frame {
381                let t = clamp(
382                    (frame - a.frame as f64) / (b.frame as f64 - a.frame as f64),
383                    0.0,
384                    1.0,
385                );
386                x = x * (1.0 - t) + (b.x - b.original_x) * t;
387                y = y * (1.0 - t) + (b.y - b.original_y) * t;
388            }
389        }
390        Ok((x, y))
391    }
392
393    /// Starts relaxing the graph, or returns `None` when it holds no loop edge (nothing to relax: the outcome is
394    /// zero residual after zero iterations, and nothing is written).
395    pub fn begin_optimize(&self) -> Option<Optimizer> {
396        if self.loops == 0 {
397            return None;
398        }
399        let mut offsets = Vec::with_capacity(self.nodes.len() + 1);
400        let mut edges = Vec::new();
401        let nodes = self
402            .nodes
403            .iter()
404            .map(|n| {
405                offsets.push(edges.len() as u32);
406                for e in self.adjacency.get(&n.id).map_or(&[][..], Vec::as_slice) {
407                    edges.push(SolverEdge {
408                        other: self.index.get(&e.other).map_or(-1, |&i| i as i32),
409                        dx: e.dx,
410                        dy: e.dy,
411                        weight: e.weight,
412                    });
413                }
414                SolverNode {
415                    x: n.x,
416                    y: n.y,
417                    pinned: n.pinned,
418                }
419            })
420            .collect();
421        offsets.push(edges.len() as u32);
422        Some(Optimizer {
423            graph: Graph {
424                nodes,
425                offsets,
426                edges,
427            },
428            tried_cycle: false,
429            iterations: 0,
430            done: false,
431        })
432    }
433
434    /// Writes a finished relaxation back: every unpinned node with at least one edge takes its relaxed position
435    /// and is stored, in node order, [`FLUSH_CHUNK`] per `putMany`.
436    pub fn finish_optimize(&mut self, optimizer: Optimizer) -> (OptimizeOutcome, Vec<Write>) {
437        let residual = optimizer.graph.residual();
438        let mut moved = Vec::new();
439        for (node, relaxed) in self.nodes.iter_mut().zip(&optimizer.graph.nodes) {
440            if !node.pinned && self.adjacency.get(&node.id).is_some_and(|l| !l.is_empty()) {
441                node.x = relaxed.x;
442                node.y = relaxed.y;
443                moved.push(node.clone());
444            }
445        }
446        (
447            OptimizeOutcome {
448                residual,
449                iterations: optimizer.iterations,
450            },
451            flush(moved),
452        )
453    }
454}
455
456/// A relaxation in progress, over a copy of the graph taken by [`PoseGraph::begin_optimize`].
457pub struct Optimizer {
458    graph: Graph,
459    tried_cycle: bool,
460    iterations: u32,
461    done: bool,
462}
463
464impl Optimizer {
465    /// Runs up to `sweeps` Gauss-Seidel sweeps (at least one). The first step first tries the exact solution of a
466    /// single anchored cycle, which counts as one iteration and finishes at once. Returns whether relaxation is
467    /// done: converged, solved exactly, or out of sweeps.
468    pub fn step(&mut self, sweeps: u32) -> bool {
469        if self.done {
470            return true;
471        }
472        if !self.tried_cycle {
473            self.tried_cycle = true;
474            if self.graph.solve_cycle() {
475                self.iterations = 1;
476                self.done = true;
477                return true;
478            }
479        }
480        for _ in 0..sweeps.max(1) {
481            if self.iterations >= MAX_SWEEPS {
482                self.done = true;
483                break;
484            }
485            let change = self.graph.pass(self.iterations % 2 == 1);
486            self.iterations += 1;
487            if change < CONVERGED_CHANGE || self.iterations >= MAX_SWEEPS {
488                self.done = true;
489                break;
490            }
491        }
492        self.done
493    }
494
495    pub fn iterations(&self) -> u32 {
496        self.iterations
497    }
498
499    pub fn is_done(&self) -> bool {
500        self.done
501    }
502}
503
504#[cfg(test)]
505mod tests {
506    use super::*;
507
508    fn nodes_written(writes: &[Write]) -> Vec<&PoseNode> {
509        writes
510            .iter()
511            .flat_map(|w| match w {
512                Write::Put(r) => std::slice::from_ref(r),
513                Write::PutMany(rs) => rs.as_slice(),
514            })
515            .filter_map(|r| match r {
516                PoseRow::Node(n) => Some(n),
517                PoseRow::Edge { .. } => None,
518            })
519            .collect()
520    }
521
522    fn node(
523        id: &str,
524        canvas: &str,
525        frame: u32,
526        x: f64,
527        pinned: bool,
528        next: Option<&str>,
529    ) -> PoseNode {
530        PoseNode {
531            id: id.into(),
532            canvas_id: canvas.into(),
533            frame,
534            x,
535            y: 0.0,
536            original_x: x,
537            original_y: 0.0,
538            pinned,
539            next: next.map(Into::into),
540        }
541    }
542
543    fn run(graph: &mut PoseGraph, sweeps: u32) -> (OptimizeOutcome, Vec<Write>) {
544        let Some(mut opt) = graph.begin_optimize() else {
545            return (
546                OptimizeOutcome {
547                    residual: 0.0,
548                    iterations: 0,
549                },
550                Vec::new(),
551            );
552        };
553        while !opt.step(sweeps) {}
554        graph.finish_optimize(opt)
555    }
556
557    #[test]
558    fn add_rejects_a_previous_node_it_does_not_hold() {
559        let mut g = PoseGraph::new();
560        assert_eq!(
561            g.add("c", 1, (1.0, 1.0), Some("ghost"), 1.0).unwrap_err(),
562            PoseGraphError::MissingPrevious
563        );
564        assert!(g.is_empty(), "a rejected add leaves no node behind");
565    }
566
567    #[test]
568    fn connect_and_unpin_reject_unknown_nodes_and_correction_an_unknown_pose() {
569        let mut g = PoseGraph::new();
570        g.add("c", 0, (0.0, 0.0), None, 1.0).unwrap();
571        let a = PoseGraph::node_id("c", 0);
572        assert_eq!(
573            g.connect(&a, "ghost", 1.0, 0.0, 1.0, EdgeKind::Loop),
574            Err(PoseGraphError::MissingNode)
575        );
576        assert_eq!(g.unpin("ghost"), Err(PoseGraphError::MissingNode));
577        assert_eq!(g.correction("", 3.0), Err(PoseGraphError::MissingPose));
578        assert_eq!(g.loops(), 0);
579        assert_eq!(
580            PoseGraphError::MissingPrevious.to_string(),
581            "Pose graph references a missing previous node."
582        );
583    }
584
585    #[test]
586    fn add_writes_the_node_then_the_edge_pair_then_the_linked_previous_node() {
587        let mut g = PoseGraph::new();
588        let (first, w0) = g.add("c", 0, (10.0, 5.0), None, 1.0).unwrap();
589        assert!(first.pinned && first.id == "c/0000000000");
590        assert_eq!(w0, vec![Write::Put(PoseRow::Node(first.clone()))]);
591        let (second, w1) = g.add("c", 1, (30.0, 5.0), Some(&first.id), 0.25).unwrap();
592        assert!(!second.pinned);
593        assert_eq!(w1.len(), 3);
594        assert_eq!(w1[0], Write::Put(PoseRow::Node(second.clone())));
595        let Write::PutMany(edges) = &w1[1] else {
596            panic!("edge pair is one putMany")
597        };
598        assert_eq!(
599            edges.iter().map(PoseRow::key).collect::<Vec<_>>(),
600            [
601                "edge/c/0000000000/c/0000000001/odometry",
602                "edge/c/0000000001/c/0000000000/odometry"
603            ]
604        );
605        let Write::Put(PoseRow::Node(linked)) = &w1[2] else {
606            panic!("previous node is one put")
607        };
608        assert_eq!(linked.next.as_deref(), Some(second.id.as_str()));
609        assert_eq!(
610            g.get(&first.id).unwrap().next.as_deref(),
611            Some("c/0000000001")
612        );
613    }
614
615    #[test]
616    fn a_duplicate_edge_and_a_self_edge_write_nothing() {
617        let mut g = PoseGraph::new();
618        let (a, _) = g.add("c", 0, (0.0, 0.0), None, 1.0).unwrap();
619        let (b, _) = g.add("d", 0, (0.0, 0.0), None, 1.0).unwrap();
620        assert_eq!(
621            g.connect(&a.id, &b.id, 1.0, 0.0, 1.0, EdgeKind::Loop)
622                .unwrap()
623                .len(),
624            1
625        );
626        assert!(g
627            .connect(&a.id, &b.id, 9.0, 0.0, 1.0, EdgeKind::Loop)
628            .unwrap()
629            .is_empty());
630        assert!(g
631            .connect(&a.id, &a.id, 9.0, 0.0, 1.0, EdgeKind::Loop)
632            .unwrap()
633            .is_empty());
634        assert_eq!(g.loops(), 1);
635    }
636
637    #[test]
638    fn a_cross_canvas_loop_carrying_offset_plus_shift_converges_to_zero_residual() {
639        let mut g = PoseGraph::new();
640        let (anchor, _) = g.add("F", 0, (60.0, 0.0), None, 1.0).unwrap();
641        let (node, _) = g.add("T", 1, (1085.0, 0.0), None, 1.0).unwrap();
642        g.unpin(&node.id).unwrap();
643        g.connect(&anchor.id, &node.id, 1025.0, 0.0, 5.0, EdgeKind::Loop)
644            .unwrap();
645        let mut prev = node.id.clone();
646        for i in 0..3u32 {
647            let (n, _) = g
648                .add(
649                    "T",
650                    2 + i,
651                    (1085.0 + (i + 1) as f64 * 20.0, 0.0),
652                    Some(&prev),
653                    1.0,
654                )
655                .unwrap();
656            prev = n.id;
657        }
658        let (outcome, _) = run(&mut g, 1);
659        assert!(outcome.residual < 1e-6, "residual {}", outcome.residual);
660    }
661
662    #[test]
663    fn loop_distribution_writes_stay_linear_in_nodes_plus_loops() {
664        let mut g = PoseGraph::new();
665        let mut ids: Vec<String> = Vec::new();
666        for i in 0..=120u32 {
667            let (n, _) = g
668                .add(
669                    "star",
670                    i,
671                    (i as f64 * 10.0, 0.0),
672                    ids.last().map(String::as_str),
673                    1.0,
674                )
675                .unwrap();
676            ids.push(n.id);
677        }
678        let mut calls = 0;
679        for i in 1..=100 {
680            calls += g
681                .connect(&ids[0], &ids[i], i as f64 * 10.0, 0.0, 1.0, EdgeKind::Loop)
682                .unwrap()
683                .len();
684        }
685        assert!(calls < 400, "{calls} storage calls for 100 loop closures");
686    }
687
688    #[test]
689    fn a_loop_closure_leaves_a_disjoint_chain_on_the_same_canvas_alone() {
690        let mut g = PoseGraph::new();
691        let mut prev: Option<String> = None;
692        let mut a = Vec::new();
693        for i in 0..4u32 {
694            let (n, _) = g
695                .add("C", i, (i as f64 * 10.0, 0.0), prev.as_deref(), 1.0)
696                .unwrap();
697            prev = Some(n.id.clone());
698            a.push(n.id);
699        }
700        prev = None;
701        let mut b = Vec::new();
702        for i in 0..3u32 {
703            let (n, _) = g
704                .add(
705                    "C",
706                    100 + i,
707                    (500.0 + i as f64 * 10.0, 0.0),
708                    prev.as_deref(),
709                    1.0,
710                )
711                .unwrap();
712            prev = Some(n.id.clone());
713            b.push(n.id);
714        }
715        g.connect(&a[0], &a[3], 50.0, 0.0, 1.0, EdgeKind::Loop)
716            .unwrap();
717        for (id, x) in b.iter().zip([500.0, 510.0, 520.0]) {
718            assert_eq!(g.get(id).unwrap().x, x);
719        }
720        assert!((g.get(&a[3]).unwrap().x - 50.0).abs() < 1e-9);
721    }
722
723    #[test]
724    fn a_broken_next_chain_treats_an_unreachable_revisit_as_isolated() {
725        let mut g = PoseGraph::new();
726        g.hydrate_node(node("a", "c", 0, 0.0, true, Some("ghost")));
727        g.hydrate_node(node("b", "c", 5, 50.0, false, None));
728        let writes = g.connect("a", "b", 40.0, 0.0, 1.0, EdgeKind::Loop).unwrap();
729        assert_eq!(writes.len(), 1, "only the edge pair");
730        assert_eq!(g.get("b").unwrap().x, 50.0);
731        assert_eq!(g.loops(), 1);
732    }
733
734    #[test]
735    fn a_loop_correction_skips_a_pinned_node_inside_the_chain() {
736        let mut g = PoseGraph::new();
737        g.hydrate_node(node("a0", "c", 0, 0.0, true, Some("a1")));
738        g.hydrate_node(node("a1", "c", 1, 10.0, false, Some("a2")));
739        g.hydrate_node(node("a2", "c", 2, 20.0, true, Some("a3")));
740        g.hydrate_node(node("a3", "c", 3, 30.0, false, None));
741        let writes = g
742            .connect("a0", "a3", 50.0, 0.0, 1.0, EdgeKind::Loop)
743            .unwrap();
744        assert_eq!(g.get("a2").unwrap().x, 20.0);
745        assert!(g.get("a1").unwrap().x.abs() > 1e-9);
746        assert!((g.get("a3").unwrap().x - 50.0).abs() < 1e-9);
747        let written: Vec<_> = nodes_written(&writes)
748            .iter()
749            .map(|n| n.id.as_str())
750            .collect();
751        assert_eq!(written, ["a1", "a3"]);
752    }
753
754    #[test]
755    fn relaxation_skips_a_node_without_edges_and_an_edge_to_a_missing_node() {
756        let mut g = PoseGraph::new();
757        g.hydrate_node(node("a", "c", 0, 0.0, false, None));
758        g.hydrate_node(node("lonely", "d", 0, 3.0, false, None));
759        g.hydrate_edge(
760            "edge/a/ghost/loop",
761            PoseEdge {
762                other: "ghost".into(),
763                dx: 5.0,
764                dy: 0.0,
765                weight: 1.0,
766                kind: EdgeKind::Loop,
767            },
768        );
769        assert_eq!(g.loops(), 1);
770        let (outcome, writes) = run(&mut g, 1);
771        assert!(outcome.residual.is_finite());
772        let written: Vec<_> = nodes_written(&writes)
773            .iter()
774            .map(|n| n.id.as_str())
775            .collect();
776        assert_eq!(written, ["a"], "the edgeless node is never written back");
777        assert_eq!(g.get("lonely").unwrap().x, 3.0);
778    }
779
780    #[test]
781    fn hydration_counts_each_loop_once_and_recovers_the_owner_from_the_key() {
782        let mut g = PoseGraph::new();
783        g.hydrate_node(node("c/1", "c", 1, 0.0, true, None));
784        g.hydrate_node(node("c/2", "c", 2, 0.0, false, None));
785        for (key, other, dx) in [
786            ("edge/c/1/c/2/loop", "c/2", 3.0),
787            ("edge/c/2/c/1/loop", "c/1", -3.0),
788        ] {
789            g.hydrate_edge(
790                key,
791                PoseEdge {
792                    other: other.into(),
793                    dx,
794                    dy: 0.0,
795                    weight: 1.0,
796                    kind: EdgeKind::Loop,
797                },
798            );
799        }
800        assert_eq!(g.loops(), 1);
801        assert!(g
802            .connect("c/1", "c/2", 7.0, 0.0, 1.0, EdgeKind::Loop)
803            .unwrap()
804            .is_empty());
805    }
806
807    #[test]
808    fn no_loops_means_no_relaxation_and_no_writes() {
809        let mut g = PoseGraph::new();
810        let (a, _) = g.add("c", 0, (0.0, 0.0), None, 1.0).unwrap();
811        g.add("c", 1, (10.0, 0.0), Some(&a.id), 1.0).unwrap();
812        assert!(g.begin_optimize().is_none());
813    }
814
815    /// A chain plus a loop that disagrees by far more than the exact cycle solver accepts, so relaxation runs sweeps.
816    fn contested() -> PoseGraph {
817        let mut g = PoseGraph::new();
818        let mut ids: Vec<String> = Vec::new();
819        for i in 0..12u32 {
820            let (n, _) = g
821                .add(
822                    "c",
823                    i,
824                    (i as f64 * 10.0, (i % 3) as f64),
825                    ids.last().map(String::as_str),
826                    1.0,
827                )
828                .unwrap();
829            ids.push(n.id);
830        }
831        g.connect(&ids[0], &ids[11], 400.0, 0.0, 1.0, EdgeKind::Loop)
832            .unwrap();
833        g.connect(&ids[2], &ids[9], 30.0, 9.0, 2.0, EdgeKind::Loop)
834            .unwrap();
835        g
836    }
837
838    #[test]
839    fn an_optimizer_dropped_between_steps_changes_nothing() {
840        let g = contested();
841        let before: Vec<PoseNode> = g.nodes.clone();
842        let mut opt = g.begin_optimize().unwrap();
843        assert!(!opt.step(1), "this graph needs more than one sweep");
844        drop(opt);
845        assert_eq!(g.nodes, before);
846    }
847
848    #[test]
849    fn the_outcome_does_not_depend_on_how_many_sweeps_each_step_runs() {
850        let (one, w_one) = run(&mut contested(), 1);
851        let (many, w_many) = run(&mut contested(), 7);
852        let (all, w_all) = run(&mut contested(), MAX_SWEEPS);
853        assert!(one.iterations > 1);
854        assert_eq!(one, many);
855        assert_eq!(one, all);
856        assert_eq!(w_one, w_many);
857        assert_eq!(w_one, w_all);
858    }
859
860    #[test]
861    fn iterations_count_sweeps_and_stop_at_the_cap() {
862        let mut opt = contested().begin_optimize().unwrap();
863        let mut steps = 0;
864        while !opt.step(1) {
865            steps += 1;
866            assert_eq!(opt.iterations(), steps);
867        }
868        assert!(opt.iterations() <= MAX_SWEEPS);
869        assert!(opt.step(1), "a finished optimizer stays finished");
870    }
871
872    #[test]
873    fn correction_blends_towards_the_next_node_by_frame() {
874        let mut g = PoseGraph::new();
875        g.hydrate_node(PoseNode {
876            x: 4.0,
877            ..node("a", "c", 10, 0.0, false, Some("b"))
878        });
879        g.hydrate_node(PoseNode {
880            x: 108.0,
881            ..node("b", "c", 20, 100.0, false, None)
882        });
883        assert_eq!(g.correction("a", 10.0).unwrap(), (4.0, 0.0));
884        assert_eq!(g.correction("a", 15.0).unwrap(), (6.0, 0.0));
885        assert_eq!(g.correction("a", 40.0).unwrap(), (8.0, 0.0));
886        assert_eq!(g.correction("b", 15.0).unwrap(), (8.0, 0.0));
887    }
888
889    #[test]
890    fn re_adding_an_id_keeps_its_place_in_node_order() {
891        let mut g = PoseGraph::new();
892        g.add("c", 0, (0.0, 0.0), None, 1.0).unwrap();
893        g.add("d", 0, (0.0, 0.0), None, 1.0).unwrap();
894        g.add("c", 0, (5.0, 0.0), None, 1.0).unwrap();
895        let order: Vec<_> = g.nodes.iter().map(|n| (n.id.as_str(), n.x)).collect();
896        assert_eq!(order, [("c/0000000000", 5.0), ("d/0000000000", 0.0)]);
897    }
898}
long_screen/engine/pose_graph.rs

long_screen/engine/
pose_graph.rs
