spatialrust_mapping/
scan.rs

1//! Bounded scan-sequence odometry over synchronized spatial records.
2
3use spatialrust_core::{FrameId, PointCloud};
4use spatialrust_math::{Isometry3, Pose3, Quat};
5use spatialrust_sync::{MemoryEpisode, StampedRecord, TopicId};
6
7use crate::{
8    DeltaMotion, MappingError, MappingResult, PoseGraph, PoseGraphEdge, PoseNodeId, StampedPose,
9    Trajectory,
10};
11
12/// Contract for an estimator that maps a previous scan into the current scan.
13pub trait ScanMatcher {
14    /// Estimates `current_T_previous` from two same-frame scans.
15    fn match_scans(
16        &self,
17        previous: &PointCloud,
18        current: &PointCloud,
19    ) -> MappingResult<Isometry3<f32>>;
20}
21
22/// Bounds and validation settings for scan-sequence odometry.
23#[derive(Clone, Copy, Debug, PartialEq, Eq)]
24pub struct ScanOdometryConfig {
25    /// Maximum number of scans retained from the selected topic prefix.
26    pub max_scans: usize,
27    /// Minimum points required in every scan passed to the matcher.
28    pub min_points: usize,
29}
30
31impl ScanOdometryConfig {
32    /// Creates scan odometry limits.
33    #[must_use]
34    pub const fn new(max_scans: usize, min_points: usize) -> Self {
35        Self { max_scans, min_points }
36    }
37
38    fn validate(self) -> MappingResult<Self> {
39        if self.max_scans == 0 {
40            return Err(MappingError::InvalidConfiguration(
41                "scan odometry max_scans must be greater than zero".into(),
42            ));
43        }
44        if self.min_points == 0 {
45            return Err(MappingError::InvalidConfiguration(
46                "scan odometry min_points must be greater than zero".into(),
47            ));
48        }
49        Ok(self)
50    }
51}
52
53impl Default for ScanOdometryConfig {
54    fn default() -> Self {
55        Self::new(1_024, 3)
56    }
57}
58
59/// Deterministic scan odometry runner over one topic in a memory episode.
60#[derive(Clone, Copy, Debug, PartialEq, Eq)]
61pub struct ScanOdometry {
62    config: ScanOdometryConfig,
63}
64
65impl ScanOdometry {
66    /// Creates a scan odometry runner with validated limits.
67    pub fn try_new(config: ScanOdometryConfig) -> MappingResult<Self> {
68        Ok(Self { config: config.validate()? })
69    }
70
71    /// Returns the configured scan limits.
72    #[must_use]
73    pub const fn config(&self) -> ScanOdometryConfig {
74        self.config
75    }
76
77    /// Estimates a deterministic trajectory and relative pose graph.
78    ///
79    /// The episode is already timestamp/topic ordered. Only the selected
80    /// topic is borrowed, and at most `max_scans` records are inspected. Every
81    /// selected record must use the same frame and clock domain. The first
82    /// scan is the graph root; each matcher result becomes the edge
83    /// `current_T_previous` and is composed using the pose-graph convention.
84    /// Stored trajectory isometries are `current_T_first_scan`; explicitly
85    /// invert them when transforming a current cloud into the first scan frame.
86    pub fn estimate<M: ScanMatcher>(
87        &self,
88        episode: &MemoryEpisode,
89        topic: &TopicId,
90        matcher: &M,
91    ) -> MappingResult<ScanOdometryResult> {
92        let scans: Vec<&StampedRecord> = episode
93            .records()
94            .iter()
95            .filter(|record| &record.topic == topic)
96            .take(self.config.max_scans)
97            .collect();
98        if scans.is_empty() {
99            return Err(MappingError::Missing(format!("scan topic `{}`", topic.as_str())));
100        }
101        let truncated =
102            episode.records().iter().filter(|record| &record.topic == topic).count() > scans.len();
103        let frame_id = scans[0].record.metadata().frame_id.clone();
104        let clock = scans[0].stamp.clock.clone();
105        let domain = scans[0].stamp.domain;
106        for (index, scan) in scans.iter().enumerate() {
107            if scan.record.metadata().frame_id != frame_id {
108                return Err(MappingError::InvalidConfiguration(format!(
109                    "scan {index} frame `{}` differs from `{}`",
110                    scan.record.metadata().frame_id.0,
111                    frame_id.0
112                )));
113            }
114            if scan.stamp.clock != clock || scan.stamp.domain != domain {
115                return Err(MappingError::InvalidConfiguration(
116                    "scan timestamps must share one clock and domain".into(),
117                ));
118            }
119            if scan.record.cloud().len() < self.config.min_points {
120                return Err(MappingError::InvalidConfiguration(format!(
121                    "scan {index} has {} points, minimum is {}",
122                    scan.record.cloud().len(),
123                    self.config.min_points
124                )));
125            }
126            if let Some(previous) = index.checked_sub(1).and_then(|value| scans.get(value)) {
127                if scan.stamp.as_nanos() < previous.stamp.as_nanos() {
128                    return Err(MappingError::InvalidConfiguration(
129                        "scan timestamps must be non-decreasing".into(),
130                    ));
131                }
132            }
133        }
134
135        let root = node_id(topic, 0);
136        let mut graph = PoseGraph::new();
137        graph.upsert_node(
138            root.clone(),
139            StampedPose::new(
140                scans[0].stamp.clone(),
141                Pose3::new(Isometry3::new(
142                    Quat::<f32>::identity(),
143                    spatialrust_math::Vec3::new(0.0, 0.0, 0.0),
144                )),
145            ),
146        );
147        let mut motions = Vec::with_capacity(scans.len().saturating_sub(1));
148        for index in 1..scans.len() {
149            let previous = scans[index - 1];
150            let current = scans[index];
151            let to_t_from = matcher.match_scans(previous.record.cloud(), current.record.cloud())?;
152            let previous_id = node_id(topic, index - 1);
153            let current_id = node_id(topic, index);
154            let previous_pose = graph
155                .nodes()
156                .get(&previous_id.0)
157                .ok_or_else(|| MappingError::Missing(format!("pose node `{}`", previous_id.0)))?
158                .pose
159                .isometry;
160            let current_pose = to_t_from.compose(previous_pose);
161            let current_sample = StampedPose::new(current.stamp.clone(), Pose3::new(current_pose));
162            graph.upsert_node(current_id.clone(), current_sample);
163            graph.add_edge(PoseGraphEdge {
164                from: previous_id,
165                to: current_id,
166                to_t_from,
167                loop_closure: false,
168            })?;
169            motions.push(DeltaMotion {
170                from: previous.stamp.clone(),
171                to: current.stamp.clone(),
172                to_t_from,
173            });
174        }
175        graph.localize_from_root(&root)?;
176
177        let mut trajectory = Trajectory::new();
178        for index in 0..scans.len() {
179            let id = node_id(topic, index);
180            let pose = graph
181                .nodes()
182                .get(&id.0)
183                .cloned()
184                .ok_or_else(|| MappingError::Missing(format!("pose node `{}`", id.0)))?;
185            trajectory.push(pose)?;
186        }
187
188        Ok(ScanOdometryResult {
189            topic: topic.clone(),
190            frame_id,
191            trajectory,
192            pose_graph: graph,
193            motions,
194            truncated,
195        })
196    }
197}
198
199/// Output of one bounded scan odometry run.
200#[derive(Clone, Debug, PartialEq)]
201pub struct ScanOdometryResult {
202    /// Selected topic.
203    pub topic: TopicId,
204    /// Common source frame of all selected scans.
205    pub frame_id: FrameId,
206    /// Deterministic timestamped trajectory.
207    pub trajectory: Trajectory,
208    /// Relative pose graph containing sequential edges.
209    pub pose_graph: PoseGraph,
210    /// Sequential motions in timestamp order.
211    pub motions: Vec<DeltaMotion>,
212    /// Whether the topic contained more scans than the configured prefix.
213    pub truncated: bool,
214}
215
216fn node_id(topic: &TopicId, index: usize) -> PoseNodeId {
217    PoseNodeId::new(format!("{}#{index}", topic.as_str()))
218}
219
220#[cfg(feature = "scan-icp")]
221/// ICP-backed scan matcher for sequential point-cloud odometry.
222#[derive(Clone, Copy, Debug, PartialEq)]
223pub struct IcpScanMatcher {
224    registration: spatialrust_registration::IcpRegistration,
225}
226
227#[cfg(feature = "scan-icp")]
228impl IcpScanMatcher {
229    /// Creates an ICP scan matcher.
230    #[must_use]
231    pub const fn new(config: spatialrust_registration::IcpConfig) -> Self {
232        Self { registration: spatialrust_registration::IcpRegistration::new(config) }
233    }
234
235    /// Returns the underlying ICP configuration.
236    #[must_use]
237    pub const fn config(&self) -> spatialrust_registration::IcpConfig {
238        self.registration.config()
239    }
240}
241
242#[cfg(feature = "scan-icp")]
243impl ScanMatcher for IcpScanMatcher {
244    fn match_scans(
245        &self,
246        previous: &PointCloud,
247        current: &PointCloud,
248    ) -> MappingResult<Isometry3<f32>> {
249        use spatialrust_registration::PointCloudRegistration;
250
251        Ok(self.registration.align(previous, current)?.transform)
252    }
253}
254
255#[cfg(test)]
256mod tests {
257    use super::{ScanMatcher, ScanOdometry, ScanOdometryConfig};
258    use spatialrust_core::{
259        FrameId, PointBuffer, PointBufferSet, PointCloud, SpatialMetadata, StandardSchemas,
260        Timestamp,
261    };
262    use spatialrust_math::{Isometry3, Quat, Vec3};
263    use spatialrust_records::{SchemaVersion, SpatialRecord};
264    use spatialrust_sync::{ClockDomain, MemoryEpisode, StampedRecord, StampedTime, TopicId};
265
266    #[derive(Clone, Copy)]
267    struct ShiftMatcher;
268
269    impl ScanMatcher for ShiftMatcher {
270        fn match_scans(
271            &self,
272            _previous: &PointCloud,
273            _current: &PointCloud,
274        ) -> super::MappingResult<Isometry3<f32>> {
275            Ok(Isometry3::new(Quat::<f32>::identity(), Vec3::new(1.0, 0.0, 0.0)))
276        }
277    }
278
279    fn scan(topic: &str, stamp: u64, frame: &str) -> StampedRecord {
280        let mut buffers = PointBufferSet::new();
281        buffers.insert("x", PointBuffer::from_f32(vec![0.0, 1.0, 0.0]));
282        buffers.insert("y", PointBuffer::from_f32(vec![0.0, 0.0, 1.0]));
283        buffers.insert("z", PointBuffer::from_f32(vec![0.0, 0.0, 0.0]));
284        let cloud = PointCloud::try_from_parts(
285            StandardSchemas::point_xyz(),
286            buffers,
287            SpatialMetadata::new(frame, Timestamp::from_nanos(stamp)),
288        )
289        .unwrap();
290        let record =
291            SpatialRecord::try_from_cloud("scan", SchemaVersion::new(1, 0), cloud).unwrap();
292        StampedRecord::new(
293            topic,
294            StampedTime::exact("ros2", ClockDomain::External, Timestamp::from_nanos(stamp)),
295            record,
296        )
297    }
298
299    #[test]
300    fn builds_trajectory_and_pose_graph_from_bounded_topic_prefix() {
301        let topic = TopicId::new("/lidar");
302        let episode = MemoryEpisode::from_records(vec![
303            scan(topic.as_str(), 20, "lidar"),
304            scan(topic.as_str(), 10, "lidar"),
305            scan(topic.as_str(), 30, "lidar"),
306        ]);
307        let odometry = ScanOdometry::try_new(ScanOdometryConfig::new(2, 3)).unwrap();
308        let result = odometry.estimate(&episode, &topic, &ShiftMatcher).unwrap();
309        assert_eq!(result.trajectory.samples().len(), 2);
310        assert_eq!(result.motions.len(), 1);
311        assert!(result.truncated);
312        assert_eq!(result.frame_id, FrameId::new("lidar"));
313        assert!((result.trajectory.samples()[1].pose.isometry.translation().x - 1.0).abs() < 1e-5);
314        assert_eq!(result.pose_graph.edges().len(), 1);
315    }
316
317    #[test]
318    fn scan_graph_preserves_current_from_first_rotation_direction() {
319        use spatialrust_math::TransformPoint;
320
321        struct QuarterTurnMatcher;
322        impl ScanMatcher for QuarterTurnMatcher {
323            fn match_scans(
324                &self,
325                _previous: &PointCloud,
326                _current: &PointCloud,
327            ) -> super::MappingResult<Isometry3<f32>> {
328                Ok(Isometry3::new(
329                    Quat::from_axis_angle(Vec3::new(0.0, 0.0, 1.0), std::f32::consts::FRAC_PI_2),
330                    Vec3::new(1.0, 0.0, 0.0),
331                ))
332            }
333        }
334
335        let topic = TopicId::new("/lidar");
336        let episode = MemoryEpisode::from_records(vec![
337            scan(topic.as_str(), 10, "lidar"),
338            scan(topic.as_str(), 20, "lidar"),
339            scan(topic.as_str(), 30, "lidar"),
340        ]);
341        let result = ScanOdometry::try_new(ScanOdometryConfig::new(3, 3))
342            .unwrap()
343            .estimate(&episode, &topic, &QuarterTurnMatcher)
344            .unwrap();
345        let first = result.trajectory.samples()[1].pose.isometry;
346        let basis = first.transform_point(Vec3::new(1.0, 0.0, 0.0));
347        assert!((basis.x - 1.0).abs() < 1e-5 && (basis.y - 1.0).abs() < 1e-5);
348        let second = result.trajectory.samples()[2].pose.isometry;
349        let origin = second.transform_point(Vec3::new(0.0, 0.0, 0.0));
350        assert!((origin.x - 1.0).abs() < 1e-5 && (origin.y - 1.0).abs() < 1e-5);
351        assert_eq!(result.trajectory.samples()[2].stamp.as_nanos(), 30);
352    }
353
354    #[test]
355    fn rejects_mixed_frames() {
356        let topic = TopicId::new("/lidar");
357        let episode = MemoryEpisode::from_records(vec![
358            scan(topic.as_str(), 1, "front"),
359            scan(topic.as_str(), 2, "rear"),
360        ]);
361        let odometry = ScanOdometry::try_new(ScanOdometryConfig::default()).unwrap();
362        let error = odometry.estimate(&episode, &topic, &ShiftMatcher).unwrap_err();
363        assert!(error.to_string().contains("differs"));
364    }
365
366    #[cfg(feature = "scan-icp")]
367    #[test]
368    fn icp_matcher_estimates_previous_to_current_motion() {
369        use super::IcpScanMatcher;
370        use spatialrust_core::PointCloudBuilder;
371        use spatialrust_registration::{transform_point_cloud, IcpConfig};
372
373        let mut builder = PointCloudBuilder::new(StandardSchemas::point_xyz());
374        for x in 0..6 {
375            for y in 0..6 {
376                for z in 0..3 {
377                    builder
378                        .push_point([x as f32 * 0.05, y as f32 * 0.05, z as f32 * 0.05])
379                        .unwrap();
380                }
381            }
382        }
383        let previous = builder.build().unwrap();
384        let expected = Isometry3::new(Quat::<f32>::identity(), Vec3::new(0.02, -0.01, 0.0));
385        let current = transform_point_cloud(&previous, expected).unwrap();
386        let matcher = IcpScanMatcher::new(IcpConfig {
387            max_correspondence_distance: 0.1,
388            max_iterations: 30,
389            ..IcpConfig::default()
390        });
391        let estimated = matcher.match_scans(&previous, &current).unwrap();
392        assert!((estimated.translation().x - expected.translation().x).abs() < 5e-3);
393        assert!((estimated.translation().y - expected.translation().y).abs() < 5e-3);
394    }
395}
spatialrust_mapping/scan.rs

spatialrust_mapping/
scan.rs
