Skip to main content

rust_robotics_slam/
dataset.rs

1//! Loaders for the EuRoC MAV and KITTI odometry dataset layouts.
2
3use std::fmt;
4use std::fs;
5use std::io;
6use std::path::{Path, PathBuf};
7
8use nalgebra::{Matrix3, Matrix4, Quaternion, UnitQuaternion, Vector2, Vector3};
9
10/// Errors returned while reading robotics datasets.
11#[derive(Debug)]
12pub enum DatasetError {
13    Io(io::Error),
14    InvalidData(String),
15}
16
17impl fmt::Display for DatasetError {
18    fn fmt(&self, formatter: &mut fmt::Formatter<'_>) -> fmt::Result {
19        match self {
20            Self::Io(error) => error.fmt(formatter),
21            Self::InvalidData(message) => write!(formatter, "invalid dataset: {message}"),
22        }
23    }
24}
25
26impl std::error::Error for DatasetError {}
27
28impl From<io::Error> for DatasetError {
29    fn from(error: io::Error) -> Self {
30        Self::Io(error)
31    }
32}
33
34pub type DatasetResult<T> = Result<T, DatasetError>;
35
36/// Timestamped EuRoC IMU measurement in SI units.
37#[derive(Debug, Clone, Copy, PartialEq)]
38pub struct ImuSample {
39    pub timestamp_ns: i64,
40    pub angular_velocity: Vector3<f64>,
41    pub acceleration: Vector3<f64>,
42}
43
44/// Timestamp and on-disk image path.
45#[derive(Debug, Clone, PartialEq, Eq)]
46pub struct CameraFrame {
47    pub timestamp_ns: i64,
48    pub image_path: PathBuf,
49}
50
51/// EuRoC ground-truth state in the world frame.
52#[derive(Debug, Clone, PartialEq)]
53pub struct GroundTruthState {
54    pub timestamp_ns: i64,
55    pub world_from_body: Matrix4<f64>,
56    pub velocity: Vector3<f64>,
57    pub gyroscope_bias: Vector3<f64>,
58    pub accelerometer_bias: Vector3<f64>,
59}
60
61/// Minimal pinhole metadata from a EuRoC `sensor.yaml`.
62#[derive(Debug, Clone, PartialEq)]
63pub struct EurocCameraCalibration {
64    pub intrinsics: [f64; 4],
65    pub resolution: [usize; 2],
66    /// EuRoC `T_BS`: body-from-camera transform.
67    pub body_from_sensor: Matrix4<f64>,
68}
69
70/// Loaded EuRoC MAV sequence.
71#[derive(Debug, Clone)]
72pub struct EurocDataset {
73    pub root: PathBuf,
74    pub imu: Vec<ImuSample>,
75    /// EuRoC `T_BS`: body-from-IMU transform.
76    pub imu_body_from_sensor: Matrix4<f64>,
77    pub camera_frames: Vec<CameraFrame>,
78    pub ground_truth: Vec<GroundTruthState>,
79    pub camera: EurocCameraCalibration,
80}
81
82/// One initialized landmark in the optional RustRobotics feature sidecar.
83#[derive(Debug, Clone, Copy, PartialEq)]
84pub struct TrackedLandmark {
85    pub id: usize,
86    pub position: Vector3<f64>,
87}
88
89/// One timestamped feature observation in the optional sidecar.
90#[derive(Debug, Clone, Copy, PartialEq)]
91pub struct FeatureTrackObservation {
92    pub timestamp_ns: i64,
93    pub landmark_id: usize,
94    pub pixel: Vector2<f64>,
95}
96
97/// Pre-extracted visual tracks consumed by the offline VIO pipeline.
98#[derive(Debug, Clone, PartialEq)]
99pub struct EurocFeatureTracks {
100    pub landmarks: Vec<TrackedLandmark>,
101    pub observations: Vec<FeatureTrackObservation>,
102}
103
104impl EurocDataset {
105    /// Loads one extracted EuRoC sequence. `root` may point either at the
106    /// sequence directory or directly at its `mav0` directory.
107    pub fn load(root: impl AsRef<Path>) -> DatasetResult<Self> {
108        let root = root.as_ref();
109        let mav0 = if root.join("mav0").is_dir() {
110            root.join("mav0")
111        } else {
112            root.to_path_buf()
113        };
114        let imu = parse_euroc_imu(&mav0.join("imu0/data.csv"))?;
115        let imu_calibration_path = mav0.join("imu0/sensor.yaml");
116        let imu_body_from_sensor = if imu_calibration_path.exists() {
117            parse_euroc_body_from_sensor_yaml(&imu_calibration_path)?
118        } else {
119            Matrix4::identity()
120        };
121        let camera_frames =
122            parse_image_index(&mav0.join("cam0/data.csv"), &mav0.join("cam0/data"))?;
123        let ground_truth_path = mav0.join("state_groundtruth_estimate0/data.csv");
124        let ground_truth = if ground_truth_path.exists() {
125            parse_euroc_ground_truth(&ground_truth_path)?
126        } else {
127            Vec::new()
128        };
129        let camera = parse_euroc_camera_yaml(&mav0.join("cam0/sensor.yaml"))?;
130        ensure_strict_timestamps(imu.iter().map(|sample| sample.timestamp_ns), "EuRoC IMU")?;
131        ensure_strict_timestamps(
132            camera_frames.iter().map(|frame| frame.timestamp_ns),
133            "EuRoC camera",
134        )?;
135        Ok(Self {
136            root: mav0,
137            imu,
138            imu_body_from_sensor,
139            camera_frames,
140            ground_truth,
141            camera,
142        })
143    }
144
145    /// Returns IMU samples in `(start, end]`.
146    pub fn imu_between(&self, start_ns: i64, end_ns: i64) -> &[ImuSample] {
147        let start = self
148            .imu
149            .partition_point(|sample| sample.timestamp_ns <= start_ns);
150        let end = self
151            .imu
152            .partition_point(|sample| sample.timestamp_ns <= end_ns);
153        &self.imu[start..end]
154    }
155
156    /// Loads optional pre-extracted tracks from
157    /// `mav0/rust_robotics/{landmarks,observations}.csv`.
158    pub fn load_feature_tracks(&self) -> DatasetResult<EurocFeatureTracks> {
159        let directory = self.root.join("rust_robotics");
160        let landmarks = data_lines(&directory.join("landmarks.csv"))?
161            .iter()
162            .map(|line| {
163                let fields = parse_fields::<f64>(line, 4, "feature landmark row")?;
164                Ok(TrackedLandmark {
165                    id: fields[0] as usize,
166                    position: Vector3::new(fields[1], fields[2], fields[3]),
167                })
168            })
169            .collect::<DatasetResult<Vec<_>>>()?;
170        let observations = data_lines(&directory.join("observations.csv"))?
171            .iter()
172            .map(|line| {
173                let fields = parse_fields::<f64>(line, 4, "feature observation row")?;
174                Ok(FeatureTrackObservation {
175                    timestamp_ns: fields[0] as i64,
176                    landmark_id: fields[1] as usize,
177                    pixel: Vector2::new(fields[2], fields[3]),
178                })
179            })
180            .collect::<DatasetResult<Vec<_>>>()?;
181        let tracks = EurocFeatureTracks {
182            landmarks,
183            observations,
184        };
185        validate_feature_tracks(&tracks, &self.camera_frames)?;
186        Ok(tracks)
187    }
188
189    /// Writes a generated feature sidecar. Existing files are preserved unless
190    /// `overwrite` is true.
191    pub fn write_feature_tracks(
192        &self,
193        tracks: &EurocFeatureTracks,
194        directory: impl AsRef<Path>,
195        overwrite: bool,
196    ) -> DatasetResult<()> {
197        validate_feature_tracks(tracks, &self.camera_frames)?;
198        let directory = directory.as_ref();
199        let landmarks_path = directory.join("landmarks.csv");
200        let observations_path = directory.join("observations.csv");
201        if !overwrite && (landmarks_path.exists() || observations_path.exists()) {
202            return Err(DatasetError::InvalidData(format!(
203                "feature sidecar already exists at {}; pass overwrite=true to replace it",
204                directory.display()
205            )));
206        }
207        fs::create_dir_all(directory)?;
208        let mut landmarks = String::from("#landmark_id,x,y,z\n");
209        for landmark in &tracks.landmarks {
210            landmarks.push_str(&format!(
211                "{},{:.17},{:.17},{:.17}\n",
212                landmark.id, landmark.position.x, landmark.position.y, landmark.position.z
213            ));
214        }
215        let mut observations = String::from("#timestamp_ns,landmark_id,u,v\n");
216        for observation in &tracks.observations {
217            observations.push_str(&format!(
218                "{},{},{:.17},{:.17}\n",
219                observation.timestamp_ns,
220                observation.landmark_id,
221                observation.pixel.x,
222                observation.pixel.y
223            ));
224        }
225        fs::write(landmarks_path, landmarks)?;
226        fs::write(observations_path, observations)?;
227        Ok(())
228    }
229}
230
231/// KITTI camera projection matrices.
232#[derive(Debug, Clone, PartialEq)]
233pub struct KittiCalibration {
234    pub projections: [Option<nalgebra::SMatrix<f64, 3, 4>>; 4],
235    pub transform_velodyne_to_camera: Option<Matrix4<f64>>,
236}
237
238/// One KITTI odometry frame.
239#[derive(Debug, Clone, PartialEq)]
240pub struct KittiFrame {
241    pub index: usize,
242    pub timestamp_seconds: f64,
243    pub image_paths: [PathBuf; 4],
244    pub velodyne_path: PathBuf,
245    pub ground_truth_pose: Option<Matrix4<f64>>,
246}
247
248/// Loaded KITTI odometry sequence metadata.
249#[derive(Debug, Clone)]
250pub struct KittiOdometryDataset {
251    pub sequence: String,
252    pub calibration: KittiCalibration,
253    pub frames: Vec<KittiFrame>,
254}
255
256impl KittiOdometryDataset {
257    /// Loads a KITTI odometry sequence such as `"00"`.
258    pub fn load(root: impl AsRef<Path>, sequence: &str) -> DatasetResult<Self> {
259        let root = root.as_ref();
260        let directory = root.join("sequences").join(sequence);
261        let times = numeric_lines(&directory.join("times.txt"))?;
262        if times.windows(2).any(|window| window[1] <= window[0]) {
263            return Err(DatasetError::InvalidData(
264                "KITTI timestamps must be strictly increasing".into(),
265            ));
266        }
267        let calibration = parse_kitti_calibration(&directory.join("calib.txt"))?;
268        let pose_path = root.join("poses").join(format!("{sequence}.txt"));
269        let poses = if pose_path.exists() {
270            parse_kitti_poses(&pose_path)?
271        } else {
272            Vec::new()
273        };
274        if !poses.is_empty() && poses.len() != times.len() {
275            return Err(DatasetError::InvalidData(format!(
276                "KITTI pose count {} does not match timestamp count {}",
277                poses.len(),
278                times.len()
279            )));
280        }
281        let frames = times
282            .into_iter()
283            .enumerate()
284            .map(|(index, timestamp_seconds)| {
285                let stem = format!("{index:06}");
286                KittiFrame {
287                    index,
288                    timestamp_seconds,
289                    image_paths: std::array::from_fn(|camera| {
290                        directory
291                            .join(format!("image_{camera}"))
292                            .join(format!("{stem}.png"))
293                    }),
294                    velodyne_path: directory.join("velodyne").join(format!("{stem}.bin")),
295                    ground_truth_pose: poses.get(index).copied(),
296                }
297            })
298            .collect();
299        Ok(Self {
300            sequence: sequence.into(),
301            calibration,
302            frames,
303        })
304    }
305
306    /// Reads `(x, y, z, reflectance)` float32 tuples from a Velodyne scan.
307    pub fn read_velodyne(path: impl AsRef<Path>) -> DatasetResult<Vec<[f32; 4]>> {
308        let bytes = fs::read(path)?;
309        if bytes.len() % 16 != 0 {
310            return Err(DatasetError::InvalidData(
311                "KITTI Velodyne data length is not divisible by 16".into(),
312            ));
313        }
314        Ok(bytes
315            .chunks_exact(16)
316            .map(|chunk| {
317                std::array::from_fn(|index| {
318                    let offset = index * 4;
319                    f32::from_le_bytes(chunk[offset..offset + 4].try_into().expect("four bytes"))
320                })
321            })
322            .collect())
323    }
324}
325
326fn data_lines(path: &Path) -> DatasetResult<Vec<String>> {
327    Ok(fs::read_to_string(path)?
328        .lines()
329        .map(str::trim)
330        .filter(|line| !line.is_empty() && !line.starts_with('#'))
331        .map(str::to_owned)
332        .collect())
333}
334
335fn parse_fields<T>(line: &str, expected: usize, context: &str) -> DatasetResult<Vec<T>>
336where
337    T: std::str::FromStr,
338{
339    let fields = line
340        .split(',')
341        .map(str::trim)
342        .map(|value| {
343            value.parse::<T>().map_err(|_| {
344                DatasetError::InvalidData(format!("{context} contains invalid value `{value}`"))
345            })
346        })
347        .collect::<DatasetResult<Vec<_>>>()?;
348    if fields.len() != expected {
349        return Err(DatasetError::InvalidData(format!(
350            "{context} expected {expected} columns, found {}",
351            fields.len()
352        )));
353    }
354    Ok(fields)
355}
356
357fn parse_euroc_imu(path: &Path) -> DatasetResult<Vec<ImuSample>> {
358    data_lines(path)?
359        .iter()
360        .map(|line| {
361            let fields = parse_fields::<f64>(line, 7, "EuRoC IMU row")?;
362            Ok(ImuSample {
363                timestamp_ns: fields[0] as i64,
364                angular_velocity: Vector3::new(fields[1], fields[2], fields[3]),
365                acceleration: Vector3::new(fields[4], fields[5], fields[6]),
366            })
367        })
368        .collect()
369}
370
371fn parse_image_index(path: &Path, data_directory: &Path) -> DatasetResult<Vec<CameraFrame>> {
372    data_lines(path)?
373        .iter()
374        .map(|line| {
375            let mut fields = line.split(',').map(str::trim);
376            let timestamp = fields
377                .next()
378                .and_then(|value| value.parse::<i64>().ok())
379                .ok_or_else(|| {
380                    DatasetError::InvalidData("invalid EuRoC camera timestamp".into())
381                })?;
382            let filename = fields
383                .next()
384                .ok_or_else(|| DatasetError::InvalidData("missing EuRoC image filename".into()))?;
385            if fields.next().is_some() {
386                return Err(DatasetError::InvalidData(
387                    "EuRoC camera row must have two columns".into(),
388                ));
389            }
390            Ok(CameraFrame {
391                timestamp_ns: timestamp,
392                image_path: data_directory.join(filename),
393            })
394        })
395        .collect()
396}
397
398fn parse_euroc_ground_truth(path: &Path) -> DatasetResult<Vec<GroundTruthState>> {
399    data_lines(path)?
400        .iter()
401        .map(|line| {
402            let fields = parse_fields::<f64>(line, 17, "EuRoC ground-truth row")?;
403            let quaternion = UnitQuaternion::new_normalize(Quaternion::new(
404                fields[4], fields[5], fields[6], fields[7],
405            ));
406            let mut transform = Matrix4::identity();
407            transform
408                .fixed_view_mut::<3, 3>(0, 0)
409                .copy_from(quaternion.to_rotation_matrix().matrix());
410            transform[(0, 3)] = fields[1];
411            transform[(1, 3)] = fields[2];
412            transform[(2, 3)] = fields[3];
413            Ok(GroundTruthState {
414                timestamp_ns: fields[0] as i64,
415                world_from_body: transform,
416                velocity: Vector3::new(fields[8], fields[9], fields[10]),
417                gyroscope_bias: Vector3::new(fields[11], fields[12], fields[13]),
418                accelerometer_bias: Vector3::new(fields[14], fields[15], fields[16]),
419            })
420        })
421        .collect()
422}
423
424fn bracket_values<T>(line: &str, key: &str) -> DatasetResult<Vec<T>>
425where
426    T: std::str::FromStr,
427{
428    let values = line
429        .strip_prefix(key)
430        .and_then(|value| value.trim().strip_prefix('['))
431        .and_then(|value| value.strip_suffix(']'))
432        .ok_or_else(|| DatasetError::InvalidData(format!("missing `{key}` list")))?;
433    values
434        .split(',')
435        .map(str::trim)
436        .map(|value| {
437            value
438                .parse()
439                .map_err(|_| DatasetError::InvalidData(format!("invalid `{key}` value `{value}`")))
440        })
441        .collect()
442}
443
444fn parse_euroc_camera_yaml(path: &Path) -> DatasetResult<EurocCameraCalibration> {
445    let contents = fs::read_to_string(path)?;
446    let intrinsics_line = contents
447        .lines()
448        .map(str::trim)
449        .find(|line| line.starts_with("intrinsics:"))
450        .ok_or_else(|| DatasetError::InvalidData("missing EuRoC intrinsics".into()))?;
451    let resolution_line = contents
452        .lines()
453        .map(str::trim)
454        .find(|line| line.starts_with("resolution:"))
455        .ok_or_else(|| DatasetError::InvalidData("missing EuRoC resolution".into()))?;
456    let intrinsics = bracket_values::<f64>(intrinsics_line, "intrinsics:")?;
457    let resolution = bracket_values::<usize>(resolution_line, "resolution:")?;
458    Ok(EurocCameraCalibration {
459        intrinsics: intrinsics.try_into().map_err(|_| {
460            DatasetError::InvalidData("EuRoC intrinsics must contain four values".into())
461        })?,
462        resolution: resolution.try_into().map_err(|_| {
463            DatasetError::InvalidData("EuRoC resolution must contain two values".into())
464        })?,
465        body_from_sensor: parse_euroc_body_from_sensor(&contents)?,
466    })
467}
468
469fn parse_euroc_body_from_sensor_yaml(path: &Path) -> DatasetResult<Matrix4<f64>> {
470    parse_euroc_body_from_sensor(&fs::read_to_string(path)?)
471}
472
473fn parse_euroc_body_from_sensor(contents: &str) -> DatasetResult<Matrix4<f64>> {
474    let mut in_body_from_sensor = false;
475    for line in contents.lines().map(str::trim) {
476        if line == "T_BS:" {
477            in_body_from_sensor = true;
478        } else if in_body_from_sensor && line.starts_with("data:") {
479            let values = bracket_values::<f64>(line, "data:")?;
480            if values.len() != 16 {
481                return Err(DatasetError::InvalidData(
482                    "EuRoC T_BS must contain 16 values".into(),
483                ));
484            }
485            return Ok(Matrix4::from_row_slice(&values));
486        }
487    }
488    Err(DatasetError::InvalidData("missing EuRoC T_BS".into()))
489}
490
491fn numeric_lines(path: &Path) -> DatasetResult<Vec<f64>> {
492    data_lines(path)?
493        .iter()
494        .map(|line| {
495            line.parse()
496                .map_err(|_| DatasetError::InvalidData(format!("invalid numeric row `{line}`")))
497        })
498        .collect()
499}
500
501fn whitespace_numbers(line: &str, context: &str) -> DatasetResult<Vec<f64>> {
502    line.split_whitespace()
503        .map(|value| {
504            value.parse().map_err(|_| {
505                DatasetError::InvalidData(format!("{context} contains invalid value `{value}`"))
506            })
507        })
508        .collect()
509}
510
511fn matrix_3x4(values: &[f64], context: &str) -> DatasetResult<nalgebra::SMatrix<f64, 3, 4>> {
512    if values.len() != 12 {
513        return Err(DatasetError::InvalidData(format!(
514            "{context} needs 12 values"
515        )));
516    }
517    Ok(nalgebra::SMatrix::<f64, 3, 4>::from_row_slice(values))
518}
519
520fn parse_kitti_calibration(path: &Path) -> DatasetResult<KittiCalibration> {
521    let mut projections = [None, None, None, None];
522    let mut transform_velodyne_to_camera = None;
523    for line in data_lines(path)? {
524        let (key, values) = line.split_once(':').ok_or_else(|| {
525            DatasetError::InvalidData(format!("invalid KITTI calibration row `{line}`"))
526        })?;
527        let values = whitespace_numbers(values, "KITTI calibration")?;
528        if let Some(camera) = key
529            .strip_prefix('P')
530            .and_then(|value| value.parse::<usize>().ok())
531        {
532            if camera < projections.len() {
533                projections[camera] = Some(matrix_3x4(&values, key)?);
534            }
535        } else if matches!(key, "Tr" | "Tr_velo_to_cam") {
536            let matrix = matrix_3x4(&values, key)?;
537            let mut transform = Matrix4::identity();
538            transform.fixed_view_mut::<3, 4>(0, 0).copy_from(&matrix);
539            transform_velodyne_to_camera = Some(transform);
540        }
541    }
542    if projections.iter().all(Option::is_none) {
543        return Err(DatasetError::InvalidData(
544            "KITTI calibration contains no projection matrices".into(),
545        ));
546    }
547    Ok(KittiCalibration {
548        projections,
549        transform_velodyne_to_camera,
550    })
551}
552
553fn parse_kitti_poses(path: &Path) -> DatasetResult<Vec<Matrix4<f64>>> {
554    data_lines(path)?
555        .iter()
556        .map(|line| {
557            let values = whitespace_numbers(line, "KITTI pose")?;
558            let matrix = matrix_3x4(&values, "KITTI pose")?;
559            let mut transform = Matrix4::identity();
560            transform.fixed_view_mut::<3, 4>(0, 0).copy_from(&matrix);
561            let rotation = transform.fixed_view::<3, 3>(0, 0);
562            if (rotation.transpose() * rotation - Matrix3::identity()).norm() > 1.0e-4 {
563                return Err(DatasetError::InvalidData(
564                    "KITTI pose rotation is not orthonormal".into(),
565                ));
566            }
567            Ok(transform)
568        })
569        .collect()
570}
571
572fn ensure_strict_timestamps(
573    timestamps: impl IntoIterator<Item = i64>,
574    context: &str,
575) -> DatasetResult<()> {
576    let mut previous = None;
577    for timestamp in timestamps {
578        if previous.is_some_and(|value| timestamp <= value) {
579            return Err(DatasetError::InvalidData(format!(
580                "{context} timestamps must be strictly increasing"
581            )));
582        }
583        previous = Some(timestamp);
584    }
585    Ok(())
586}
587
588fn validate_feature_tracks(
589    tracks: &EurocFeatureTracks,
590    camera_frames: &[CameraFrame],
591) -> DatasetResult<()> {
592    if tracks.landmarks.is_empty() || tracks.observations.is_empty() {
593        return Err(DatasetError::InvalidData(
594            "feature sidecar must contain landmarks and observations".into(),
595        ));
596    }
597    if tracks
598        .landmarks
599        .iter()
600        .enumerate()
601        .any(|(index, landmark)| landmark.id != index)
602    {
603        return Err(DatasetError::InvalidData(
604            "feature landmark IDs must be contiguous and zero-based".into(),
605        ));
606    }
607    if tracks.observations.iter().any(|observation| {
608        observation.landmark_id >= tracks.landmarks.len()
609            || !observation.pixel.iter().all(|value| value.is_finite())
610    }) {
611        return Err(DatasetError::InvalidData(
612            "feature observation is invalid or references a missing landmark".into(),
613        ));
614    }
615    if tracks.observations.iter().any(|observation| {
616        camera_frames
617            .binary_search_by_key(&observation.timestamp_ns, |frame| frame.timestamp_ns)
618            .is_err()
619    }) {
620        return Err(DatasetError::InvalidData(
621            "feature observation timestamp has no matching camera frame".into(),
622        ));
623    }
624    Ok(())
625}
626
627#[cfg(test)]
628mod tests {
629    use super::*;
630
631    fn fixture(name: &str) -> PathBuf {
632        Path::new(env!("CARGO_MANIFEST_DIR"))
633            .join("tests/fixtures")
634            .join(name)
635    }
636
637    #[test]
638    fn loads_euroc_layout_and_slices_imu() {
639        let dataset = EurocDataset::load(fixture("euroc_mini")).unwrap();
640        assert_eq!(dataset.imu.len(), 5);
641        assert_eq!(dataset.camera_frames.len(), 3);
642        assert_eq!(dataset.ground_truth.len(), 3);
643        assert_eq!(dataset.camera.resolution, [752, 480]);
644        assert_eq!(dataset.camera.body_from_sensor, Matrix4::identity());
645        assert_eq!(dataset.imu_body_from_sensor, Matrix4::identity());
646        assert_eq!(dataset.imu_between(1_000_000_000, 1_010_000_000).len(), 2);
647        assert!(dataset.camera_frames[0]
648            .image_path
649            .ends_with("1000000000.png"));
650        let tracks = dataset.load_feature_tracks().unwrap();
651        assert_eq!(tracks.landmarks.len(), 4);
652        assert_eq!(tracks.observations.len(), 12);
653    }
654
655    #[test]
656    fn parses_euroc_imu_body_from_sensor_transform() {
657        let transform = parse_euroc_body_from_sensor(
658            "sensor_type: imu\nT_BS:\n  data: [0, -1, 0, 0.2, 1, 0, 0, -0.1, 0, 0, 1, 0.3, 0, 0, 0, 1]\n",
659        )
660        .unwrap();
661        assert_eq!(transform[(0, 1)], -1.0);
662        assert_eq!(transform[(1, 0)], 1.0);
663        assert_eq!(transform[(0, 3)], 0.2);
664        assert_eq!(transform[(1, 3)], -0.1);
665        assert_eq!(transform[(2, 3)], 0.3);
666    }
667
668    #[test]
669    fn loads_kitti_layout_and_velodyne_points() {
670        let dataset = KittiOdometryDataset::load(fixture("kitti_mini"), "00").unwrap();
671        assert_eq!(dataset.frames.len(), 3);
672        assert_eq!(dataset.frames[2].ground_truth_pose.unwrap()[(0, 3)], 2.0);
673        let scan_path = std::env::temp_dir().join(format!(
674            "rust_robotics_kitti_scan_{}.bin",
675            std::process::id()
676        ));
677        let bytes = [1.0_f32, 2.0, 3.0, 0.5, -1.0, 0.0, 4.0, 0.25]
678            .into_iter()
679            .flat_map(f32::to_le_bytes)
680            .collect::<Vec<_>>();
681        fs::write(&scan_path, bytes).unwrap();
682        let points = KittiOdometryDataset::read_velodyne(&scan_path).unwrap();
683        fs::remove_file(scan_path).unwrap();
684        assert_eq!(points, vec![[1.0, 2.0, 3.0, 0.5], [-1.0, 0.0, 4.0, 0.25]]);
685    }
686
687    #[test]
688    fn writes_feature_sidecar_without_silent_overwrite() {
689        let dataset = EurocDataset::load(fixture("euroc_mini")).unwrap();
690        let tracks = dataset.load_feature_tracks().unwrap();
691        let directory = std::env::temp_dir().join(format!(
692            "rust_robotics_feature_sidecar_{}",
693            std::process::id()
694        ));
695        if directory.exists() {
696            fs::remove_dir_all(&directory).unwrap();
697        }
698        dataset
699            .write_feature_tracks(&tracks, &directory, false)
700            .unwrap();
701        assert!(directory.join("landmarks.csv").is_file());
702        assert!(dataset
703            .write_feature_tracks(&tracks, &directory, false)
704            .is_err());
705        fs::remove_dir_all(directory).unwrap();
706    }
707}