spatialrust_mapping/
pose_graph.rs

1//! Pose graph nodes, relative edges, and loop-closure candidates.
2
3use std::collections::{HashMap, VecDeque};
4
5use spatialrust_math::Isometry3;
6
7use crate::{MappingError, MappingResult, StampedPose};
8
9/// Stable pose-graph node identifier.
10#[derive(Clone, Debug, PartialEq, Eq, Hash)]
11pub struct PoseNodeId(pub String);
12
13impl PoseNodeId {
14    /// Creates a node id.
15    #[must_use]
16    pub fn new(value: impl Into<String>) -> Self {
17        Self(value.into())
18    }
19}
20
21impl From<&str> for PoseNodeId {
22    fn from(value: &str) -> Self {
23        Self(value.to_owned())
24    }
25}
26
27/// Relative pose constraint between two nodes: `to_T_from`.
28#[derive(Clone, Debug, PartialEq)]
29pub struct PoseGraphEdge {
30    /// Source node.
31    pub from: PoseNodeId,
32    /// Target node.
33    pub to: PoseNodeId,
34    /// Transform mapping `from` coordinates into `to` coordinates.
35    pub to_t_from: Isometry3<f32>,
36    /// Whether this edge is a loop closure.
37    pub loop_closure: bool,
38}
39
40/// Pose graph with absolute node estimates and relative constraints.
41#[derive(Clone, Debug, Default, PartialEq)]
42pub struct PoseGraph {
43    nodes: HashMap<String, StampedPose>,
44    edges: Vec<PoseGraphEdge>,
45}
46
47impl PoseGraph {
48    /// Creates an empty pose graph.
49    #[must_use]
50    pub fn new() -> Self {
51        Self::default()
52    }
53
54    /// Inserts or replaces a node estimate.
55    pub fn upsert_node(&mut self, id: impl Into<PoseNodeId>, pose: StampedPose) {
56        self.nodes.insert(id.into().0, pose);
57    }
58
59    /// Adds a relative constraint.
60    pub fn add_edge(&mut self, edge: PoseGraphEdge) -> MappingResult<()> {
61        if !self.nodes.contains_key(&edge.from.0) || !self.nodes.contains_key(&edge.to.0) {
62            return Err(MappingError::Missing("pose graph endpoint".into()));
63        }
64        self.edges.push(edge);
65        Ok(())
66    }
67
68    /// Returns node estimates.
69    #[must_use]
70    pub fn nodes(&self) -> &HashMap<String, StampedPose> {
71        &self.nodes
72    }
73
74    /// Returns relative edges.
75    #[must_use]
76    pub fn edges(&self) -> &[PoseGraphEdge] {
77        &self.edges
78    }
79
80    /// Propagates complete poses through the directed non-loop tree rooted at `root`.
81    ///
82    /// Stored isometries use `node_T_anchor`, with
83    /// `to_T_anchor = to_T_from * from_T_anchor`. The original root and all stamps
84    /// remain fixed; disconnected components and loop-closure edges are untouched.
85    /// Callers needing `anchor_T_node` explicitly invert the resulting isometry.
86    ///
87    /// The reachable non-loop component must be a tree: cycles, duplicate edges
88    /// and multiple incoming paths return [`MappingError::Graph`]. Non-finite
89    /// reachable transforms or composed results also fail. Every failure leaves
90    /// the graph unchanged. Work and temporary storage are O(nodes + edges).
91    pub fn localize_from_root(&mut self, root: &PoseNodeId) -> MappingResult<()> {
92        let root_pose = self
93            .nodes
94            .get(&root.0)
95            .cloned()
96            .ok_or_else(|| MappingError::Missing(format!("root node `{}`", root.0)))?;
97        if !finite_isometry(root_pose.pose.isometry) {
98            return Err(MappingError::Graph("non-finite root pose".into()));
99        }
100
101        let mut outgoing: HashMap<&str, Vec<&PoseGraphEdge>> = HashMap::new();
102        for edge in self.edges.iter().filter(|edge| !edge.loop_closure) {
103            outgoing.entry(edge.from.0.as_str()).or_default().push(edge);
104        }
105        let mut localized = HashMap::new();
106        localized.insert(root.0.as_str(), root_pose.pose.isometry);
107        let mut pending = VecDeque::from([root.0.as_str()]);
108        while let Some(from) = pending.pop_front() {
109            let Some(edges) = outgoing.get(from) else {
110                continue;
111            };
112            let from_pose = localized[from];
113            for edge in edges {
114                let to = edge.to.0.as_str();
115                if localized.contains_key(to) {
116                    return Err(MappingError::Graph(format!(
117                        "reachable non-loop constraints must form a tree; repeated node `{to}`"
118                    )));
119                }
120                if !finite_isometry(edge.to_t_from) {
121                    return Err(MappingError::Graph(format!("non-finite constraint to `{to}`")));
122                }
123                let predicted = edge.to_t_from.compose(from_pose);
124                if !finite_isometry(predicted) {
125                    return Err(MappingError::Graph(format!("non-finite localized pose `{to}`")));
126                }
127                localized.insert(to, predicted);
128                pending.push_back(to);
129            }
130        }
131
132        // Validation completes before the first mutation. add_edge guarantees
133        // every endpoint exists, and this traversal never removes a node.
134        for (id, isometry) in localized {
135            if id != root.0 {
136                let target = self.nodes.get_mut(id).expect("validated pose graph endpoint");
137                target.pose = spatialrust_math::Pose3::new(isometry);
138            }
139        }
140        Ok(())
141    }
142
143    /// Suggests loop closures for nodes within `max_distance` translation of each other.
144    pub fn loop_closure_candidates(&self, max_distance: f32) -> Vec<(PoseNodeId, PoseNodeId)> {
145        let mut ids: Vec<_> = self.nodes.keys().cloned().collect();
146        ids.sort();
147        let mut out = Vec::new();
148        for i in 0..ids.len() {
149            for j in (i + 1)..ids.len() {
150                let a = &self.nodes[&ids[i]];
151                let b = &self.nodes[&ids[j]];
152                let delta =
153                    (a.pose.isometry.translation() - b.pose.isometry.translation()).length();
154                if delta <= max_distance {
155                    out.push((PoseNodeId(ids[i].clone()), PoseNodeId(ids[j].clone())));
156                }
157            }
158        }
159        out
160    }
161}
162
163fn finite_isometry(pose: Isometry3<f32>) -> bool {
164    let rotation = pose.rotation();
165    let translation = pose.translation();
166    [rotation.x, rotation.y, rotation.z, rotation.w, translation.x, translation.y, translation.z]
167        .iter()
168        .all(|value| value.is_finite())
169}
170
171#[cfg(test)]
172mod tests {
173    use super::{PoseGraph, PoseGraphEdge, PoseNodeId};
174    use crate::StampedPose;
175    use spatialrust_core::Timestamp;
176    use spatialrust_math::{Isometry3, Pose3, Quat, Vec3};
177    use spatialrust_sync::{ClockDomain, StampedTime};
178
179    fn stamped(x: f32, nanos: u64) -> StampedPose {
180        StampedPose::new(
181            StampedTime::exact("host", ClockDomain::HostSteady, Timestamp::from_nanos(nanos)),
182            Pose3::new(Isometry3::new(Quat::new(0.0, 0.0, 0.0, 1.0), Vec3::new(x, 0.0, 0.0))),
183        )
184    }
185
186    #[test]
187    fn localizes_child_from_relative_edge() {
188        let mut graph = PoseGraph::new();
189        graph.upsert_node("a", stamped(0.0, 0));
190        graph.upsert_node("b", stamped(0.0, 1));
191        graph
192            .add_edge(PoseGraphEdge {
193                from: PoseNodeId::new("a"),
194                to: PoseNodeId::new("b"),
195                to_t_from: Isometry3::new(Quat::new(0.0, 0.0, 0.0, 1.0), Vec3::new(2.0, 0.0, 0.0)),
196                loop_closure: false,
197            })
198            .unwrap();
199        graph.localize_from_root(&PoseNodeId::new("a")).unwrap();
200        assert!((graph.nodes()["b"].pose.isometry.translation().x - 2.0).abs() < 1e-4);
201        let loops = graph.loop_closure_candidates(0.1);
202        assert!(loops.is_empty());
203    }
204}
spatialrust_mapping/pose_graph.rs

spatialrust_mapping/
pose_graph.rs
