spatialrust_sync/
frame_graph.rs

1//! Calibrated frame graph with parent→child isometries.
2
3use std::collections::{HashMap, HashSet, VecDeque};
4
5use spatialrust_core::FrameId;
6use spatialrust_math::Isometry3;
7
8use crate::{SyncError, SyncResult};
9
10/// One directed edge: `child_T_parent` transform.
11#[derive(Clone, Debug, PartialEq)]
12pub struct FrameEdge {
13    /// Parent frame.
14    pub parent: FrameId,
15    /// Child frame.
16    pub child: FrameId,
17    /// Transform that maps parent coordinates into child coordinates.
18    pub child_t_parent: Isometry3<f32>,
19}
20
21/// Directed calibrated frame graph.
22#[derive(Clone, Debug, Default, PartialEq)]
23pub struct FrameGraph {
24    /// Adjacency: parent → (child, child_T_parent).
25    edges: HashMap<String, Vec<(String, Isometry3<f32>)>>,
26    /// Reverse adjacency for lookups towards parents.
27    reverse: HashMap<String, Vec<(String, Isometry3<f32>)>>,
28}
29
30impl FrameGraph {
31    /// Creates an empty frame graph.
32    #[must_use]
33    pub fn new() -> Self {
34        Self::default()
35    }
36
37    /// Inserts or replaces a parent→child edge.
38    pub fn insert_edge(&mut self, edge: FrameEdge) -> SyncResult<()> {
39        if edge.parent.0 == edge.child.0 {
40            return Err(SyncError::InvalidConfiguration(
41                "frame edge parent and child must differ".into(),
42            ));
43        }
44        self.edges
45            .entry(edge.parent.0.clone())
46            .or_default()
47            .retain(|(child, _)| child != &edge.child.0);
48        self.edges
49            .entry(edge.parent.0.clone())
50            .or_default()
51            .push((edge.child.0.clone(), edge.child_t_parent));
52
53        let parent_t_child = edge.child_t_parent.inverse();
54        self.reverse
55            .entry(edge.child.0.clone())
56            .or_default()
57            .retain(|(parent, _)| parent != &edge.parent.0);
58        self.reverse.entry(edge.child.0.clone()).or_default().push((edge.parent.0, parent_t_child));
59        Ok(())
60    }
61
62    /// Looks up a transform that maps `from` coordinates into `to` coordinates.
63    pub fn lookup(&self, from: &FrameId, to: &FrameId) -> SyncResult<Isometry3<f32>> {
64        if from == to {
65            return Ok(Isometry3::identity());
66        }
67        let mut queue = VecDeque::from([(from.0.clone(), Isometry3::identity())]);
68        let mut visited = HashSet::from([from.0.clone()]);
69        while let Some((node, acc)) = queue.pop_front() {
70            for (next, edge) in self.neighbors(&node) {
71                if !visited.insert(next.clone()) {
72                    continue;
73                }
74                let composed = edge.compose(acc);
75                if next == to.0 {
76                    return Ok(composed);
77                }
78                queue.push_back((next, composed));
79            }
80        }
81        Err(SyncError::NoTransformPath { from: from.0.clone(), to: to.0.clone() })
82    }
83
84    fn neighbors(&self, node: &str) -> Vec<(String, Isometry3<f32>)> {
85        let mut out = Vec::new();
86        if let Some(forward) = self.edges.get(node) {
87            out.extend(forward.iter().cloned());
88        }
89        if let Some(back) = self.reverse.get(node) {
90            out.extend(back.iter().cloned());
91        }
92        out
93    }
94}
95
96#[cfg(test)]
97mod tests {
98    use super::{FrameEdge, FrameGraph};
99    use spatialrust_core::FrameId;
100    use spatialrust_math::{Isometry3, Quat, TransformPoint, Vec3};
101
102    #[test]
103    fn composes_chain_base_to_lidar() {
104        let mut graph = FrameGraph::new();
105        graph
106            .insert_edge(FrameEdge {
107                parent: FrameId::new("base"),
108                child: FrameId::new("sensor"),
109                child_t_parent: Isometry3::new(
110                    Quat::new(0.0, 0.0, 0.0, 1.0),
111                    Vec3::new(1.0, 0.0, 0.0),
112                ),
113            })
114            .unwrap();
115        graph
116            .insert_edge(FrameEdge {
117                parent: FrameId::new("sensor"),
118                child: FrameId::new("lidar"),
119                child_t_parent: Isometry3::new(
120                    Quat::new(0.0, 0.0, 0.0, 1.0),
121                    Vec3::new(0.0, 2.0, 0.0),
122                ),
123            })
124            .unwrap();
125        let t = graph.lookup(&FrameId::new("base"), &FrameId::new("lidar")).unwrap();
126        let p = t.transform_point(Vec3::new(0.0, 0.0, 0.0));
127        assert!((p.x - 1.0).abs() < 1e-5);
128        assert!((p.y - 2.0).abs() < 1e-5);
129    }
130}
spatialrust_sync/frame_graph.rs

spatialrust_sync/
frame_graph.rs
