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

apex_io/
lib.rs
