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    pub fn estimate<M: ScanMatcher>(
85        &self,
86        episode: &MemoryEpisode,
87        topic: &TopicId,
88        matcher: &M,
89    ) -> MappingResult<ScanOdometryResult> {
90        let scans: Vec<&StampedRecord> = episode
91            .records()
92            .iter()
93            .filter(|record| &record.topic == topic)
94            .take(self.config.max_scans)
95            .collect();
96        if scans.is_empty() {
97            return Err(MappingError::Missing(format!("scan topic `{}`", topic.as_str())));
98        }
99        let truncated =
100            episode.records().iter().filter(|record| &record.topic == topic).count() > scans.len();
101        let frame_id = scans[0].record.metadata().frame_id.clone();
102        let clock = scans[0].stamp.clock.clone();
103        let domain = scans[0].stamp.domain;
104        for (index, scan) in scans.iter().enumerate() {
105            if scan.record.metadata().frame_id != frame_id {
106                return Err(MappingError::InvalidConfiguration(format!(
107                    "scan {index} frame `{}` differs from `{}`",
108                    scan.record.metadata().frame_id.0,
109                    frame_id.0
110                )));
111            }
112            if scan.stamp.clock != clock || scan.stamp.domain != domain {
113                return Err(MappingError::InvalidConfiguration(
114                    "scan timestamps must share one clock and domain".into(),
115                ));
116            }
117            if scan.record.cloud().len() < self.config.min_points {
118                return Err(MappingError::InvalidConfiguration(format!(
119                    "scan {index} has {} points, minimum is {}",
120                    scan.record.cloud().len(),
121                    self.config.min_points
122                )));
123            }
124            if let Some(previous) = index.checked_sub(1).and_then(|value| scans.get(value)) {
125                if scan.stamp.as_nanos() < previous.stamp.as_nanos() {
126                    return Err(MappingError::InvalidConfiguration(
127                        "scan timestamps must be non-decreasing".into(),
128                    ));
129                }
130            }
131        }
132
133        let root = node_id(topic, 0);
134        let mut graph = PoseGraph::new();
135        graph.upsert_node(
136            root.clone(),
137            StampedPose::new(
138                scans[0].stamp.clone(),
139                Pose3::new(Isometry3::new(
140                    Quat::<f32>::identity(),
141                    spatialrust_math::Vec3::new(0.0, 0.0, 0.0),
142                )),
143            ),
144        );
145        let mut motions = Vec::with_capacity(scans.len().saturating_sub(1));
146        for index in 1..scans.len() {
147            let previous = scans[index - 1];
148            let current = scans[index];
149            let to_t_from = matcher.match_scans(previous.record.cloud(), current.record.cloud())?;
150            let previous_id = node_id(topic, index - 1);
151            let current_id = node_id(topic, index);
152            let previous_pose = graph
153                .nodes()
154                .get(&previous_id.0)
155                .ok_or_else(|| MappingError::Missing(format!("pose node `{}`", previous_id.0)))?
156                .pose
157                .isometry;
158            let current_pose = to_t_from.compose(previous_pose);
159            let current_sample = StampedPose::new(current.stamp.clone(), Pose3::new(current_pose));
160            graph.upsert_node(current_id.clone(), current_sample);
161            graph.add_edge(PoseGraphEdge {
162                from: previous_id,
163                to: current_id,
164                to_t_from,
165                loop_closure: false,
166            })?;
167            motions.push(DeltaMotion {
168                from: previous.stamp.clone(),
169                to: current.stamp.clone(),
170                to_t_from,
171            });
172        }
173        graph.localize_from_root(&root)?;
174
175        let mut trajectory = Trajectory::new();
176        for index in 0..scans.len() {
177            let id = node_id(topic, index);
178            let pose = graph
179                .nodes()
180                .get(&id.0)
181                .cloned()
182                .ok_or_else(|| MappingError::Missing(format!("pose node `{}`", id.0)))?;
183            trajectory.push(pose)?;
184        }
185
186        Ok(ScanOdometryResult {
187            topic: topic.clone(),
188            frame_id,
189            trajectory,
190            pose_graph: graph,
191            motions,
192            truncated,
193        })
194    }
195}
196
197/// Output of one bounded scan odometry run.
198#[derive(Clone, Debug, PartialEq)]
199pub struct ScanOdometryResult {
200    /// Selected topic.
201    pub topic: TopicId,
202    /// Common source frame of all selected scans.
203    pub frame_id: FrameId,
204    /// Deterministic timestamped trajectory.
205    pub trajectory: Trajectory,
206    /// Relative pose graph containing sequential edges.
207    pub pose_graph: PoseGraph,
208    /// Sequential motions in timestamp order.
209    pub motions: Vec<DeltaMotion>,
210    /// Whether the topic contained more scans than the configured prefix.
211    pub truncated: bool,
212}
213
214fn node_id(topic: &TopicId, index: usize) -> PoseNodeId {
215    PoseNodeId::new(format!("{}#{index}", topic.as_str()))
216}
217
218#[cfg(feature = "scan-icp")]
219/// ICP-backed scan matcher for sequential point-cloud odometry.
220#[derive(Clone, Copy, Debug, PartialEq)]
221pub struct IcpScanMatcher {
222    registration: spatialrust_registration::IcpRegistration,
223}
224
225#[cfg(feature = "scan-icp")]
226impl IcpScanMatcher {
227    /// Creates an ICP scan matcher.
228    #[must_use]
229    pub const fn new(config: spatialrust_registration::IcpConfig) -> Self {
230        Self { registration: spatialrust_registration::IcpRegistration::new(config) }
231    }
232
233    /// Returns the underlying ICP configuration.
234    #[must_use]
235    pub const fn config(&self) -> spatialrust_registration::IcpConfig {
236        self.registration.config()
237    }
238}
239
240#[cfg(feature = "scan-icp")]
241impl ScanMatcher for IcpScanMatcher {
242    fn match_scans(
243        &self,
244        previous: &PointCloud,
245        current: &PointCloud,
246    ) -> MappingResult<Isometry3<f32>> {
247        use spatialrust_registration::PointCloudRegistration;
248
249        Ok(self.registration.align(previous, current)?.transform)
250    }
251}
252
253#[cfg(test)]
254mod tests {
255    use super::{ScanMatcher, ScanOdometry, ScanOdometryConfig};
256    use spatialrust_core::{
257        FrameId, PointBuffer, PointBufferSet, PointCloud, SpatialMetadata, StandardSchemas,
258        Timestamp,
259    };
260    use spatialrust_math::{Isometry3, Quat, Vec3};
261    use spatialrust_records::{SchemaVersion, SpatialRecord};
262    use spatialrust_sync::{ClockDomain, MemoryEpisode, StampedRecord, StampedTime, TopicId};
263
264    #[derive(Clone, Copy)]
265    struct ShiftMatcher;
266
267    impl ScanMatcher for ShiftMatcher {
268        fn match_scans(
269            &self,
270            _previous: &PointCloud,
271            _current: &PointCloud,
272        ) -> super::MappingResult<Isometry3<f32>> {
273            Ok(Isometry3::new(Quat::<f32>::identity(), Vec3::new(1.0, 0.0, 0.0)))
274        }
275    }
276
277    fn scan(topic: &str, stamp: u64, frame: &str) -> StampedRecord {
278        let mut buffers = PointBufferSet::new();
279        buffers.insert("x", PointBuffer::from_f32(vec![0.0, 1.0, 0.0]));
280        buffers.insert("y", PointBuffer::from_f32(vec![0.0, 0.0, 1.0]));
281        buffers.insert("z", PointBuffer::from_f32(vec![0.0, 0.0, 0.0]));
282        let cloud = PointCloud::try_from_parts(
283            StandardSchemas::point_xyz(),
284            buffers,
285            SpatialMetadata::new(frame, Timestamp::from_nanos(stamp)),
286        )
287        .unwrap();
288        let record =
289            SpatialRecord::try_from_cloud("scan", SchemaVersion::new(1, 0), cloud).unwrap();
290        StampedRecord::new(
291            topic,
292            StampedTime::exact("ros2", ClockDomain::External, Timestamp::from_nanos(stamp)),
293            record,
294        )
295    }
296
297    #[test]
298    fn builds_trajectory_and_pose_graph_from_bounded_topic_prefix() {
299        let topic = TopicId::new("/lidar");
300        let episode = MemoryEpisode::from_records(vec![
301            scan(topic.as_str(), 20, "lidar"),
302            scan(topic.as_str(), 10, "lidar"),
303            scan(topic.as_str(), 30, "lidar"),
304        ]);
305        let odometry = ScanOdometry::try_new(ScanOdometryConfig::new(2, 3)).unwrap();
306        let result = odometry.estimate(&episode, &topic, &ShiftMatcher).unwrap();
307        assert_eq!(result.trajectory.samples().len(), 2);
308        assert_eq!(result.motions.len(), 1);
309        assert!(result.truncated);
310        assert_eq!(result.frame_id, FrameId::new("lidar"));
311        assert!((result.trajectory.samples()[1].pose.isometry.translation().x - 1.0).abs() < 1e-5);
312        assert_eq!(result.pose_graph.edges().len(), 1);
313    }
314
315    #[test]
316    fn rejects_mixed_frames() {
317        let topic = TopicId::new("/lidar");
318        let episode = MemoryEpisode::from_records(vec![
319            scan(topic.as_str(), 1, "front"),
320            scan(topic.as_str(), 2, "rear"),
321        ]);
322        let odometry = ScanOdometry::try_new(ScanOdometryConfig::default()).unwrap();
323        let error = odometry.estimate(&episode, &topic, &ShiftMatcher).unwrap_err();
324        assert!(error.to_string().contains("differs"));
325    }
326
327    #[cfg(feature = "scan-icp")]
328    #[test]
329    fn icp_matcher_estimates_previous_to_current_motion() {
330        use super::IcpScanMatcher;
331        use spatialrust_core::PointCloudBuilder;
332        use spatialrust_registration::{transform_point_cloud, IcpConfig};
333
334        let mut builder = PointCloudBuilder::new(StandardSchemas::point_xyz());
335        for x in 0..6 {
336            for y in 0..6 {
337                for z in 0..3 {
338                    builder
339                        .push_point([x as f32 * 0.05, y as f32 * 0.05, z as f32 * 0.05])
340                        .unwrap();
341                }
342            }
343        }
344        let previous = builder.build().unwrap();
345        let expected = Isometry3::new(Quat::<f32>::identity(), Vec3::new(0.02, -0.01, 0.0));
346        let current = transform_point_cloud(&previous, expected).unwrap();
347        let matcher = IcpScanMatcher::new(IcpConfig {
348            max_correspondence_distance: 0.1,
349            max_iterations: 30,
350            ..IcpConfig::default()
351        });
352        let estimated = matcher.match_scans(&previous, &current).unwrap();
353        assert!((estimated.translation().x - expected.translation().x).abs() < 5e-3);
354        assert!((estimated.translation().y - expected.translation().y).abs() < 5e-3);
355    }
356}
spatialrust_mapping/scan.rs

spatialrust_mapping/
scan.rs
