apex_io/
lib.rs

1use nalgebra::{Matrix3, Matrix6, Quaternion, UnitQuaternion, Vector3};
2
3#[cfg(feature = "visualization")]
4use rerun::external::glam::{Quat, Vec3};
5
6use std::{
7    collections, fmt,
8    fmt::{Display, Formatter},
9    io,
10    path::Path,
11};
12use thiserror::Error;
13use tracing::error;
14
15// Import manifold types from apex-manifolds crate
16use apex_manifolds::{se2::SE2, se3::SE3};
17
18// Module declarations
19pub mod asl;
20pub mod bal;
21pub mod g2o;
22pub mod logger;
23pub mod toro;
24pub mod utils;
25
26pub mod rosbag;
27
28#[cfg(feature = "dds")]
29pub use rosbag::ros2::dds;
30
31pub use logger::init_logger;
32pub use utils::{DatasetRegistry, ensure_ba_dataset, ensure_odometry_dataset};
33
34/// Default base directory for odometry (pose graph) datasets relative to the workspace root.
35pub const ODOMETRY_DATA_DIR: &str = "data/odometry";
36
37/// Directory for 2D odometry datasets (`data/odometry/2d`).
38pub const ODOMETRY_DATA_DIR_2D: &str = "data/odometry/2d";
39
40/// Directory for 3D odometry datasets (`data/odometry/3d`).
41pub const ODOMETRY_DATA_DIR_3D: &str = "data/odometry/3d";
42
43/// Default directory for bundle adjustment datasets relative to the workspace root.
44pub const BUNDLE_ADJUSTMENT_DATA_DIR: &str = "data/bundle_adjustment";
45
46// Re-exports
47pub use asl::error::AslError;
48pub use asl::{AslDataset, AslReader, AslStream};
49pub use bal::{BalCamera, BalDataset, BalLoader, BalObservation, BalPoint};
50pub use g2o::G2oLoader;
51pub use toro::ToroLoader;
52
53/// Errors that can occur during graph file parsing
54#[derive(Error, Debug)]
55pub enum IoError {
56    #[error("IO error: {0}")]
57    Io(#[from] io::Error),
58
59    #[error("Parse error at line {line}: {message}")]
60    Parse { line: usize, message: String },
61
62    #[error("Unsupported vertex type: {0}")]
63    UnsupportedVertexType(String),
64
65    #[error("Unsupported edge type: {0}")]
66    UnsupportedEdgeType(String),
67
68    #[error("Invalid number format at line {line}: {value}")]
69    InvalidNumber { line: usize, value: String },
70
71    #[error("Missing required fields at line {line}")]
72    MissingFields { line: usize },
73
74    #[error("Duplicate vertex ID: {id}")]
75    DuplicateVertex { id: usize },
76
77    #[error("Invalid quaternion at line {line}: norm = {norm:.6}, expected ~1.0")]
78    InvalidQuaternion { line: usize, norm: f64 },
79
80    #[error("Unsupported file format: {0}")]
81    UnsupportedFormat(String),
82
83    #[error("Failed to create file '{path}': {reason}")]
84    FileCreationFailed { path: String, reason: String },
85}
86
87impl IoError {
88    /// Log the error using tracing::error and return self for chaining
89    pub fn log(self) -> Self {
90        error!("{}", self);
91        self
92    }
93
94    /// Log the error with source error information using tracing::error and return self for chaining
95    pub fn log_with_source<E: std::fmt::Debug>(self, source_error: E) -> Self {
96        error!("{} | Source: {:?}", self, source_error);
97        self
98    }
99}
100
101#[derive(Clone, PartialEq)]
102pub struct VertexSE2 {
103    pub id: usize,
104    pub pose: SE2,
105}
106impl VertexSE2 {
107    pub fn new(id: usize, x: f64, y: f64, theta: f64) -> Self {
108        Self {
109            id,
110            pose: SE2::from_xy_angle(x, y, theta),
111        }
112    }
113
114    pub fn from_vector(id: usize, vector: Vector3<f64>) -> Self {
115        Self {
116            id,
117            pose: SE2::from_xy_angle(vector[0], vector[1], vector[2]),
118        }
119    }
120
121    pub fn id(&self) -> usize {
122        self.id
123    }
124
125    pub fn x(&self) -> f64 {
126        self.pose.x()
127    }
128
129    pub fn y(&self) -> f64 {
130        self.pose.y()
131    }
132
133    pub fn theta(&self) -> f64 {
134        self.pose.angle()
135    }
136}
137
138impl Display for VertexSE2 {
139    fn fmt(&self, f: &mut Formatter<'_>) -> fmt::Result {
140        write!(f, "VertexSE2 [ id: {}, pose: {} ]", self.id, self.pose)
141    }
142}
143
144impl VertexSE2 {
145    /// Convert to Rerun 2D position with scaling
146    ///
147    /// **Note:** Requires the `visualization` feature to be enabled.
148    ///
149    /// # Arguments
150    /// * `scale` - Scale factor to apply to position
151    ///
152    /// # Returns
153    /// 2D position array [x, y] compatible with Rerun Points2D
154    pub fn to_rerun_position_2d(&self, scale: f32) -> [f32; 2] {
155        [(self.x() as f32) * scale, (self.y() as f32) * scale]
156    }
157
158    /// Convert to Rerun 3D position with scaling and specified height
159    ///
160    /// **Note:** Requires the `visualization` feature to be enabled.
161    ///
162    /// # Arguments
163    /// * `scale` - Scale factor to apply to X and Y
164    /// * `height` - Z coordinate for the 2D point in 3D space
165    ///
166    /// # Returns
167    /// 3D position compatible with Rerun Transform3D or Points3D
168    #[cfg(feature = "visualization")]
169    pub fn to_rerun_position_3d(&self, scale: f32, height: f32) -> Vec3 {
170        Vec3::new((self.x() as f32) * scale, (self.y() as f32) * scale, height)
171    }
172}
173
174/// SE3 vertex with ID (x, y, z, qx, qy, qz, qw)
175#[derive(Clone, PartialEq)]
176pub struct VertexSE3 {
177    pub id: usize,
178    pub pose: SE3,
179}
180
181impl VertexSE3 {
182    pub fn new(id: usize, translation: Vector3<f64>, rotation: UnitQuaternion<f64>) -> Self {
183        Self {
184            id,
185            pose: SE3::new(translation, rotation),
186        }
187    }
188
189    pub fn from_vector(id: usize, vector: [f64; 7]) -> Self {
190        let translation = Vector3::from([vector[0], vector[1], vector[2]]);
191        let rotation = UnitQuaternion::from_quaternion(Quaternion::from([
192            vector[3], vector[4], vector[5], vector[6],
193        ]));
194        Self::new(id, translation, rotation)
195    }
196
197    pub fn from_translation_quaternion(
198        id: usize,
199        translation: Vector3<f64>,
200        quaternion: Quaternion<f64>,
201    ) -> Self {
202        Self {
203            id,
204            pose: SE3::from_translation_quaternion(translation, quaternion),
205        }
206    }
207
208    pub fn id(&self) -> usize {
209        self.id
210    }
211
212    pub fn translation(&self) -> Vector3<f64> {
213        self.pose.translation()
214    }
215
216    pub fn rotation(&self) -> UnitQuaternion<f64> {
217        self.pose.rotation_quaternion()
218    }
219
220    pub fn x(&self) -> f64 {
221        self.pose.x()
222    }
223
224    pub fn y(&self) -> f64 {
225        self.pose.y()
226    }
227
228    pub fn z(&self) -> f64 {
229        self.pose.z()
230    }
231}
232
233impl Display for VertexSE3 {
234    fn fmt(&self, f: &mut Formatter<'_>) -> fmt::Result {
235        write!(f, "VertexSE3 [ id: {}, pose: {} ]", self.id, self.pose)
236    }
237}
238
239impl VertexSE3 {
240    /// Convert to Rerun 3D transform components (position and rotation) with scaling
241    ///
242    /// **Note:** Requires the `visualization` feature to be enabled.
243    ///
244    /// # Arguments
245    /// * `scale` - Scale factor to apply to position
246    ///
247    /// # Returns
248    /// Tuple of (position, rotation) compatible with Rerun Transform3D
249    #[cfg(feature = "visualization")]
250    pub fn to_rerun_transform(&self, scale: f32) -> (Vec3, Quat) {
251        // Extract translation and convert to glam Vec3
252        let trans = self.translation();
253        let position = Vec3::new(trans.x as f32, trans.y as f32, trans.z as f32) * scale;
254
255        // Extract rotation quaternion and convert to glam Quat
256        let rot = self.rotation();
257        let nq = rot.as_ref();
258        let rotation = Quat::from_xyzw(nq.i as f32, nq.j as f32, nq.k as f32, nq.w as f32);
259
260        (position, rotation)
261    }
262}
263
264/// 2D edge constraint between two SE2 vertices
265#[derive(Clone, PartialEq)]
266pub struct EdgeSE2 {
267    pub from: usize,
268    pub to: usize,
269    pub measurement: SE2,          // Relative transformation
270    pub information: Matrix3<f64>, // 3x3 information matrix
271}
272
273impl EdgeSE2 {
274    pub fn new(
275        from: usize,
276        to: usize,
277        dx: f64,
278        dy: f64,
279        dtheta: f64,
280        information: Matrix3<f64>,
281    ) -> Self {
282        Self {
283            from,
284            to,
285            measurement: SE2::from_xy_angle(dx, dy, dtheta),
286            information,
287        }
288    }
289}
290
291impl Display for EdgeSE2 {
292    fn fmt(&self, f: &mut Formatter<'_>) -> fmt::Result {
293        write!(
294            f,
295            "EdgeSE2 [ from: {}, to: {}, measurement: {}, information: {} ]",
296            self.from, self.to, self.measurement, self.information
297        )
298    }
299}
300
301/// 3D edge constraint between two SE3 vertices
302#[derive(Clone, PartialEq)]
303pub struct EdgeSE3 {
304    pub from: usize,
305    pub to: usize,
306    pub measurement: SE3,          // Relative transformation
307    pub information: Matrix6<f64>, // 6x6 information matrix
308}
309
310impl EdgeSE3 {
311    pub fn new(
312        from: usize,
313        to: usize,
314        translation: Vector3<f64>,
315        rotation: UnitQuaternion<f64>,
316        information: Matrix6<f64>,
317    ) -> Self {
318        Self {
319            from,
320            to,
321            measurement: SE3::new(translation, rotation),
322            information,
323        }
324    }
325}
326
327impl Display for EdgeSE3 {
328    fn fmt(&self, f: &mut Formatter<'_>) -> fmt::Result {
329        write!(
330            f,
331            "EdgeSE3 [ from: {}, to: {}, measurement: {}, information: {} ]",
332            self.from, self.to, self.measurement, self.information
333        )
334    }
335}
336
337/// Main graph structure containing vertices and edges
338#[derive(Clone)]
339pub struct Graph {
340    pub vertices_se2: collections::HashMap<usize, VertexSE2>,
341    pub vertices_se3: collections::HashMap<usize, VertexSE3>,
342    pub edges_se2: Vec<EdgeSE2>,
343    pub edges_se3: Vec<EdgeSE3>,
344}
345
346impl Graph {
347    pub fn new() -> Self {
348        Self {
349            vertices_se2: collections::HashMap::new(),
350            vertices_se3: collections::HashMap::new(),
351            edges_se2: Vec::new(),
352            edges_se3: Vec::new(),
353        }
354    }
355
356    pub fn vertex_count(&self) -> usize {
357        self.vertices_se2.len() + self.vertices_se3.len()
358    }
359
360    pub fn edge_count(&self) -> usize {
361        self.edges_se2.len() + self.edges_se3.len()
362    }
363
364    // Note: The from_optimized_variables() method has been moved to apex-solver
365    // as it depends on VariableEnum from the core module.
366}
367
368impl Default for Graph {
369    fn default() -> Self {
370        Self::new()
371    }
372}
373
374impl Display for Graph {
375    fn fmt(&self, f: &mut Formatter<'_>) -> fmt::Result {
376        write!(
377            f,
378            "Graph [[ vertices_se2: {} (count: {}), vertices_se3: {} (count: {}), edges_se2: {} (count: {}), edges_se3: {} (count: {}) ]]",
379            self.vertices_se2
380                .values()
381                .map(|v| format!("{}", v))
382                .collect::<Vec<_>>()
383                .join(", "),
384            self.vertices_se2.len(),
385            self.vertices_se3
386                .values()
387                .map(|v| format!("{}", v))
388                .collect::<Vec<_>>()
389                .join(", "),
390            self.vertices_se3.len(),
391            self.edges_se2
392                .iter()
393                .map(|e| format!("{}", e))
394                .collect::<Vec<_>>()
395                .join(", "),
396            self.edges_se2.len(),
397            self.edges_se3
398                .iter()
399                .map(|e| format!("{}", e))
400                .collect::<Vec<_>>()
401                .join(", "),
402            self.edges_se3.len()
403        )
404    }
405}
406
407/// Trait for graph file loaders and writers
408pub trait GraphLoader {
409    /// Load a graph from a file
410    fn load<P: AsRef<Path>>(path: P) -> Result<Graph, IoError>;
411
412    /// Write a graph to a file
413    fn write<P: AsRef<Path>>(graph: &Graph, path: P) -> Result<(), IoError>;
414}
415
416/// Convenience function to load any supported format based on file extension
417pub fn load_graph<P: AsRef<Path>>(path: P) -> Result<Graph, IoError> {
418    let path_ref = path.as_ref();
419    let extension = path_ref
420        .extension()
421        .and_then(|ext| ext.to_str())
422        .ok_or_else(|| {
423            IoError::UnsupportedFormat("No file extension".to_string())
424                .log_with_source(format!("File path: {:?}", path_ref))
425        })?;
426
427    match extension.to_lowercase().as_str() {
428        "g2o" => G2oLoader::load(path),
429        "graph" => ToroLoader::load(path),
430        _ => Err(
431            IoError::UnsupportedFormat(format!("Unsupported extension: {extension}"))
432                .log_with_source(format!("File path: {:?}", path_ref)),
433        ),
434    }
435}
436
437#[cfg(test)]
438mod tests {
439    use super::*;
440    use nalgebra::{Matrix3, Matrix6, Quaternion, UnitQuaternion, Vector3};
441    use std::{error, io::Write};
442    use tempfile::NamedTempFile;
443
444    #[test]
445    fn test_load_simple_graph() -> Result<(), IoError> {
446        let mut temp_file = NamedTempFile::new().map_err(|e| {
447            IoError::FileCreationFailed {
448                path: "temp_file".to_string(),
449                reason: e.to_string(),
450            }
451            .log()
452        })?;
453        writeln!(temp_file, "VERTEX_SE2 0 0.0 0.0 0.0")?;
454        writeln!(temp_file, "VERTEX_SE2 1 1.0 0.0 0.0")?;
455        writeln!(temp_file, "# This is a comment")?;
456        writeln!(temp_file)?; // Empty line
457        writeln!(temp_file, "VERTEX_SE3:QUAT 2 0.0 0.0 0.0 0.0 0.0 0.0 1.0")?;
458
459        let graph = G2oLoader::load(temp_file.path())?;
460
461        assert_eq!(graph.vertices_se2.len(), 2);
462        assert_eq!(graph.vertices_se3.len(), 1);
463        assert!(graph.vertices_se2.contains_key(&0));
464        assert!(graph.vertices_se2.contains_key(&1));
465        assert!(graph.vertices_se3.contains_key(&2));
466
467        Ok(())
468    }
469
470    #[test]
471    fn test_load_m3500() -> Result<(), Box<dyn error::Error>> {
472        let path = utils::ensure_odometry_dataset("M3500")?;
473        let graph = G2oLoader::load(&path)?;
474        assert!(!graph.vertices_se2.is_empty());
475        Ok(())
476    }
477
478    #[test]
479    fn test_load_sphere2500() -> Result<(), Box<dyn error::Error>> {
480        let path = utils::ensure_odometry_dataset("sphere2500")?;
481        let graph = G2oLoader::load(&path)?;
482        assert!(!graph.vertices_se3.is_empty());
483        Ok(())
484    }
485
486    #[test]
487    fn test_duplicate_vertex_error() -> Result<(), io::Error> {
488        let mut temp_file = NamedTempFile::new()?;
489        writeln!(temp_file, "VERTEX_SE2 0 0.0 0.0 0.0")?;
490        writeln!(temp_file, "VERTEX_SE2 0 1.0 0.0 0.0")?; // Duplicate ID
491
492        let result = G2oLoader::load(temp_file.path());
493        assert!(matches!(result, Err(IoError::DuplicateVertex { id: 0 })));
494
495        Ok(())
496    }
497
498    #[test]
499    fn test_toro_loader() -> Result<(), IoError> {
500        let mut temp_file = NamedTempFile::new().map_err(|e| {
501            IoError::FileCreationFailed {
502                path: "temp_file".to_string(),
503                reason: e.to_string(),
504            }
505            .log()
506        })?;
507        writeln!(temp_file, "VERTEX2 0 0.0 0.0 0.0")?;
508        writeln!(temp_file, "VERTEX2 1 1.0 0.0 0.0")?;
509
510        let graph = ToroLoader::load(temp_file.path()).map_err(|e| e.log())?;
511        assert_eq!(graph.vertices_se2.len(), 2);
512
513        Ok(())
514    }
515
516    #[test]
517    #[cfg(feature = "visualization")]
518    fn test_se3_to_rerun() {
519        let vertex = VertexSE3::new(0, Vector3::new(1.0, 2.0, 3.0), UnitQuaternion::identity());
520
521        let (pos, rot) = vertex.to_rerun_transform(0.1);
522
523        assert!((pos.x - 0.1).abs() < 1e-6);
524        assert!((pos.y - 0.2).abs() < 1e-6);
525        assert!((pos.z - 0.3).abs() < 1e-6);
526        assert!((rot.w - 1.0).abs() < 1e-6);
527    }
528
529    #[test]
530    fn test_se2_to_rerun_2d() {
531        let vertex = VertexSE2::new(0, 10.0, 20.0, 0.5);
532
533        let pos = vertex.to_rerun_position_2d(0.1);
534
535        assert!((pos[0] - 1.0).abs() < 1e-6);
536        assert!((pos[1] - 2.0).abs() < 1e-6);
537    }
538
539    #[test]
540    #[cfg(feature = "visualization")]
541    fn test_se2_to_rerun_3d() {
542        let vertex = VertexSE2::new(0, 10.0, 20.0, 0.5);
543
544        let pos = vertex.to_rerun_position_3d(0.1, 5.0);
545
546        assert!((pos.x - 1.0).abs() < 1e-6);
547        assert!((pos.y - 2.0).abs() < 1e-6);
548        assert!((pos.z - 5.0).abs() < 1e-6);
549    }
550
551    // -------------------------------------------------------------------------
552    // VertexSE2 / VertexSE3 constructors
553    // -------------------------------------------------------------------------
554
555    #[test]
556    fn test_vertex_se2_from_vector() {
557        let v = VertexSE2::from_vector(5, Vector3::new(1.0, 2.0, 0.5));
558        assert_eq!(v.id(), 5);
559        assert!((v.x() - 1.0).abs() < 1e-12);
560        assert!((v.y() - 2.0).abs() < 1e-12);
561        assert!((v.theta() - 0.5).abs() < 1e-12);
562    }
563
564    #[test]
565    fn test_vertex_se3_from_vector() {
566        // [tx, ty, tz, qx, qy, qz, qw]
567        let arr = [1.0f64, 2.0, 3.0, 0.0, 0.0, 0.0, 1.0];
568        let v = VertexSE3::from_vector(7, arr);
569        assert_eq!(v.id(), 7);
570        assert!((v.x() - 1.0).abs() < 1e-10);
571        assert!((v.y() - 2.0).abs() < 1e-10);
572        assert!((v.z() - 3.0).abs() < 1e-10);
573    }
574
575    #[test]
576    fn test_vertex_se3_from_translation_quaternion() {
577        let trans = Vector3::new(1.0, 2.0, 3.0);
578        // nalgebra Quaternion::new(w, i, j, k)
579        let quat = Quaternion::new(1.0, 0.0, 0.0, 0.0);
580        let v = VertexSE3::from_translation_quaternion(3, trans, quat);
581        assert_eq!(v.id(), 3);
582        assert!((v.x() - 1.0).abs() < 1e-10);
583        assert!((v.y() - 2.0).abs() < 1e-10);
584        assert!((v.z() - 3.0).abs() < 1e-10);
585    }
586
587    // -------------------------------------------------------------------------
588    // Display implementations
589    // -------------------------------------------------------------------------
590
591    #[test]
592    fn test_vertex_se2_display() {
593        let v = VertexSE2::new(0, 1.0, 2.0, 0.5);
594        let s = format!("{v}");
595        assert!(
596            s.contains("VertexSE2"),
597            "Display should contain 'VertexSE2': {s}"
598        );
599        assert!(s.contains('0'), "Display should contain id: {s}");
600    }
601
602    #[test]
603    fn test_vertex_se3_display() {
604        let v = VertexSE3::new(1, Vector3::new(1.0, 2.0, 3.0), UnitQuaternion::identity());
605        let s = format!("{v}");
606        assert!(
607            s.contains("VertexSE3"),
608            "Display should contain 'VertexSE3': {s}"
609        );
610        assert!(s.contains('1'), "Display should contain id: {s}");
611    }
612
613    #[test]
614    fn test_edge_se2_display() {
615        let e = EdgeSE2::new(0, 1, 1.0, 0.0, 0.0, Matrix3::identity());
616        let s = format!("{e}");
617        assert!(
618            s.contains("EdgeSE2"),
619            "Display should contain 'EdgeSE2': {s}"
620        );
621    }
622
623    #[test]
624    fn test_edge_se3_display() {
625        let e = EdgeSE3::new(
626            0,
627            1,
628            Vector3::zeros(),
629            UnitQuaternion::identity(),
630            Matrix6::identity(),
631        );
632        let s = format!("{e}");
633        assert!(
634            s.contains("EdgeSE3"),
635            "Display should contain 'EdgeSE3': {s}"
636        );
637    }
638
639    #[test]
640    fn test_graph_display() {
641        let mut g = Graph::new();
642        g.vertices_se2.insert(0, VertexSE2::new(0, 0.0, 0.0, 0.0));
643        let s = format!("{g}");
644        assert!(s.contains("Graph"), "Display should contain 'Graph': {s}");
645        assert!(s.contains("count: 1"), "Display should show count: {s}");
646    }
647
648    // -------------------------------------------------------------------------
649    // Graph helpers
650    // -------------------------------------------------------------------------
651
652    #[test]
653    fn test_graph_default_is_empty() {
654        let g = Graph::default();
655        assert_eq!(g.vertex_count(), 0);
656        assert_eq!(g.edge_count(), 0);
657    }
658
659    #[test]
660    fn test_graph_vertex_and_edge_counts() {
661        let mut g = Graph::new();
662        g.vertices_se2.insert(0, VertexSE2::new(0, 0.0, 0.0, 0.0));
663        g.vertices_se3.insert(
664            1,
665            VertexSE3::new(1, Vector3::zeros(), UnitQuaternion::identity()),
666        );
667        g.edges_se2
668            .push(EdgeSE2::new(0, 1, 0.0, 0.0, 0.0, Matrix3::identity()));
669        assert_eq!(g.vertex_count(), 2);
670        assert_eq!(g.edge_count(), 1);
671    }
672
673    // -------------------------------------------------------------------------
674    // load_graph() dispatch
675    // -------------------------------------------------------------------------
676
677    #[test]
678    fn test_load_graph_unsupported_extension() {
679        let result = load_graph("fake_path.xyz");
680        assert!(
681            matches!(result, Err(IoError::UnsupportedFormat(_))),
682            "unknown extension should return UnsupportedFormat"
683        );
684    }
685
686    #[test]
687    fn test_load_graph_no_extension() {
688        let result = load_graph("/tmp/no_extension_file");
689        assert!(
690            matches!(result, Err(IoError::UnsupportedFormat(_))),
691            "path with no extension should return UnsupportedFormat"
692        );
693    }
694
695    #[test]
696    fn test_load_graph_toro_extension() -> Result<(), Box<dyn error::Error>> {
697        let mut f = NamedTempFile::new()?;
698        writeln!(f, "VERTEX2 0 0.0 0.0 0.0")?;
699        writeln!(f, "VERTEX2 1 1.0 0.0 0.0")?;
700        f.flush()?;
701        // Rename temp file path to have .graph extension
702        let toro_path = f.path().with_extension("graph");
703        std::fs::copy(f.path(), &toro_path)?;
704        let graph = load_graph(&toro_path)?;
705        std::fs::remove_file(&toro_path)?;
706        assert_eq!(graph.vertices_se2.len(), 2);
707        Ok(())
708    }
709
710    #[test]
711    fn test_io_error_log_returns_self() {
712        let err = IoError::UnsupportedFormat("xyz".to_string());
713        let returned = err.log();
714        assert!(matches!(returned, IoError::UnsupportedFormat(_)));
715    }
716
717    #[test]
718    fn test_io_error_log_with_source() {
719        let err = IoError::UnsupportedFormat("abc".to_string());
720        let source = std::io::Error::other("source");
721        let returned = err.log_with_source(source);
722        assert!(matches!(returned, IoError::UnsupportedFormat(_)));
723    }
724
725    #[test]
726    fn test_vertex_se2_theta() {
727        use std::f64::consts::PI;
728        let v = VertexSE2::new(0, 1.0, 2.0, PI / 4.0);
729        assert!((v.theta() - PI / 4.0).abs() < 1e-10);
730    }
731
732    #[test]
733    fn test_edge_se3_new() {
734        let t = Vector3::new(1.0, 2.0, 3.0);
735        let r = UnitQuaternion::identity();
736        let info = Matrix6::identity();
737        let e = EdgeSE3::new(0, 1, t, r, info);
738        assert_eq!(e.from, 0);
739        assert_eq!(e.to, 1);
740    }
741
742    #[test]
743    fn test_vertex_se3_new() {
744        let t = Vector3::new(1.0, 2.0, 3.0);
745        let r = UnitQuaternion::identity();
746        let v = VertexSE3::new(5, t, r);
747        assert_eq!(v.id, 5);
748        assert!((v.translation() - t).norm() < 1e-10);
749    }
750}
apex_io/lib.rs

apex_io/
lib.rs
