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 toro;
22
23// Re-exports
24pub use bal::{BalCamera, BalDataset, BalLoader, BalObservation, BalPoint};
25pub use g2o::G2oLoader;
26pub use toro::ToroLoader;
27
28/// Errors that can occur during graph file parsing
29#[derive(Error, Debug)]
30pub enum IoError {
31    #[error("IO error: {0}")]
32    Io(#[from] io::Error),
33
34    #[error("Parse error at line {line}: {message}")]
35    Parse { line: usize, message: String },
36
37    #[error("Unsupported vertex type: {0}")]
38    UnsupportedVertexType(String),
39
40    #[error("Unsupported edge type: {0}")]
41    UnsupportedEdgeType(String),
42
43    #[error("Invalid number format at line {line}: {value}")]
44    InvalidNumber { line: usize, value: String },
45
46    #[error("Missing required fields at line {line}")]
47    MissingFields { line: usize },
48
49    #[error("Duplicate vertex ID: {id}")]
50    DuplicateVertex { id: usize },
51
52    #[error("Invalid quaternion at line {line}: norm = {norm:.6}, expected ~1.0")]
53    InvalidQuaternion { line: usize, norm: f64 },
54
55    #[error("Unsupported file format: {0}")]
56    UnsupportedFormat(String),
57
58    #[error("Failed to create file '{path}': {reason}")]
59    FileCreationFailed { path: String, reason: String },
60}
61
62impl IoError {
63    /// Log the error using tracing::error and return self for chaining
64    #[must_use]
65    pub fn log(self) -> Self {
66        error!("{}", self);
67        self
68    }
69
70    /// Log the error with source error information using tracing::error and return self for chaining
71    #[must_use]
72    pub fn log_with_source<E: std::fmt::Debug>(self, source_error: E) -> Self {
73        error!("{} | Source: {:?}", self, source_error);
74        self
75    }
76}
77
78#[derive(Clone, PartialEq)]
79pub struct VertexSE2 {
80    pub id: usize,
81    pub pose: SE2,
82}
83impl VertexSE2 {
84    pub fn new(id: usize, x: f64, y: f64, theta: f64) -> Self {
85        Self {
86            id,
87            pose: SE2::from_xy_angle(x, y, theta),
88        }
89    }
90
91    pub fn from_vector(id: usize, vector: Vector3<f64>) -> Self {
92        Self {
93            id,
94            pose: SE2::from_xy_angle(vector[0], vector[1], vector[2]),
95        }
96    }
97
98    pub fn id(&self) -> usize {
99        self.id
100    }
101
102    pub fn x(&self) -> f64 {
103        self.pose.x()
104    }
105
106    pub fn y(&self) -> f64 {
107        self.pose.y()
108    }
109
110    pub fn theta(&self) -> f64 {
111        self.pose.angle()
112    }
113}
114
115impl Display for VertexSE2 {
116    fn fmt(&self, f: &mut Formatter<'_>) -> fmt::Result {
117        write!(f, "VertexSE2 [ id: {}, pose: {} ]", self.id, self.pose)
118    }
119}
120
121impl VertexSE2 {
122    /// Convert to Rerun 2D position with scaling
123    ///
124    /// **Note:** Requires the `visualization` feature to be enabled.
125    ///
126    /// # Arguments
127    /// * `scale` - Scale factor to apply to position
128    ///
129    /// # Returns
130    /// 2D position array [x, y] compatible with Rerun Points2D
131    pub fn to_rerun_position_2d(&self, scale: f32) -> [f32; 2] {
132        [(self.x() as f32) * scale, (self.y() as f32) * scale]
133    }
134
135    /// Convert to Rerun 3D position with scaling and specified height
136    ///
137    /// **Note:** Requires the `visualization` feature to be enabled.
138    ///
139    /// # Arguments
140    /// * `scale` - Scale factor to apply to X and Y
141    /// * `height` - Z coordinate for the 2D point in 3D space
142    ///
143    /// # Returns
144    /// 3D position compatible with Rerun Transform3D or Points3D
145    #[cfg(feature = "visualization")]
146    pub fn to_rerun_position_3d(&self, scale: f32, height: f32) -> Vec3 {
147        Vec3::new((self.x() as f32) * scale, (self.y() as f32) * scale, height)
148    }
149}
150
151/// SE3 vertex with ID (x, y, z, qx, qy, qz, qw)
152#[derive(Clone, PartialEq)]
153pub struct VertexSE3 {
154    pub id: usize,
155    pub pose: SE3,
156}
157
158impl VertexSE3 {
159    pub fn new(id: usize, translation: Vector3<f64>, rotation: UnitQuaternion<f64>) -> Self {
160        Self {
161            id,
162            pose: SE3::new(translation, rotation),
163        }
164    }
165
166    pub fn from_vector(id: usize, vector: [f64; 7]) -> Self {
167        let translation = Vector3::from([vector[0], vector[1], vector[2]]);
168        let rotation = UnitQuaternion::from_quaternion(Quaternion::from([
169            vector[3], vector[4], vector[5], vector[6],
170        ]));
171        Self::new(id, translation, rotation)
172    }
173
174    pub fn from_translation_quaternion(
175        id: usize,
176        translation: Vector3<f64>,
177        quaternion: Quaternion<f64>,
178    ) -> Self {
179        Self {
180            id,
181            pose: SE3::from_translation_quaternion(translation, quaternion),
182        }
183    }
184
185    pub fn id(&self) -> usize {
186        self.id
187    }
188
189    pub fn translation(&self) -> Vector3<f64> {
190        self.pose.translation()
191    }
192
193    pub fn rotation(&self) -> UnitQuaternion<f64> {
194        self.pose.rotation_quaternion()
195    }
196
197    pub fn x(&self) -> f64 {
198        self.pose.x()
199    }
200
201    pub fn y(&self) -> f64 {
202        self.pose.y()
203    }
204
205    pub fn z(&self) -> f64 {
206        self.pose.z()
207    }
208}
209
210impl Display for VertexSE3 {
211    fn fmt(&self, f: &mut Formatter<'_>) -> fmt::Result {
212        write!(f, "VertexSE3 [ id: {}, pose: {} ]", self.id, self.pose)
213    }
214}
215
216impl VertexSE3 {
217    /// Convert to Rerun 3D transform components (position and rotation) with scaling
218    ///
219    /// **Note:** Requires the `visualization` feature to be enabled.
220    ///
221    /// # Arguments
222    /// * `scale` - Scale factor to apply to position
223    ///
224    /// # Returns
225    /// Tuple of (position, rotation) compatible with Rerun Transform3D
226    #[cfg(feature = "visualization")]
227    pub fn to_rerun_transform(&self, scale: f32) -> (Vec3, Quat) {
228        // Extract translation and convert to glam Vec3
229        let trans = self.translation();
230        let position = Vec3::new(trans.x as f32, trans.y as f32, trans.z as f32) * scale;
231
232        // Extract rotation quaternion and convert to glam Quat
233        let rot = self.rotation();
234        let nq = rot.as_ref();
235        let rotation = Quat::from_xyzw(nq.i as f32, nq.j as f32, nq.k as f32, nq.w as f32);
236
237        (position, rotation)
238    }
239}
240
241/// 2D edge constraint between two SE2 vertices
242#[derive(Clone, PartialEq)]
243pub struct EdgeSE2 {
244    pub from: usize,
245    pub to: usize,
246    pub measurement: SE2,          // Relative transformation
247    pub information: Matrix3<f64>, // 3x3 information matrix
248}
249
250impl EdgeSE2 {
251    pub fn new(
252        from: usize,
253        to: usize,
254        dx: f64,
255        dy: f64,
256        dtheta: f64,
257        information: Matrix3<f64>,
258    ) -> Self {
259        Self {
260            from,
261            to,
262            measurement: SE2::from_xy_angle(dx, dy, dtheta),
263            information,
264        }
265    }
266}
267
268impl Display for EdgeSE2 {
269    fn fmt(&self, f: &mut Formatter<'_>) -> fmt::Result {
270        write!(
271            f,
272            "EdgeSE2 [ from: {}, to: {}, measurement: {}, information: {} ]",
273            self.from, self.to, self.measurement, self.information
274        )
275    }
276}
277
278/// 3D edge constraint between two SE3 vertices
279#[derive(Clone, PartialEq)]
280pub struct EdgeSE3 {
281    pub from: usize,
282    pub to: usize,
283    pub measurement: SE3,          // Relative transformation
284    pub information: Matrix6<f64>, // 6x6 information matrix
285}
286
287impl EdgeSE3 {
288    pub fn new(
289        from: usize,
290        to: usize,
291        translation: Vector3<f64>,
292        rotation: UnitQuaternion<f64>,
293        information: Matrix6<f64>,
294    ) -> Self {
295        Self {
296            from,
297            to,
298            measurement: SE3::new(translation, rotation),
299            information,
300        }
301    }
302}
303
304impl Display for EdgeSE3 {
305    fn fmt(&self, f: &mut Formatter<'_>) -> fmt::Result {
306        write!(
307            f,
308            "EdgeSE3 [ from: {}, to: {}, measurement: {}, information: {} ]",
309            self.from, self.to, self.measurement, self.information
310        )
311    }
312}
313
314/// Main graph structure containing vertices and edges
315#[derive(Clone)]
316pub struct Graph {
317    pub vertices_se2: collections::HashMap<usize, VertexSE2>,
318    pub vertices_se3: collections::HashMap<usize, VertexSE3>,
319    pub edges_se2: Vec<EdgeSE2>,
320    pub edges_se3: Vec<EdgeSE3>,
321}
322
323impl Graph {
324    pub fn new() -> Self {
325        Self {
326            vertices_se2: collections::HashMap::new(),
327            vertices_se3: collections::HashMap::new(),
328            edges_se2: Vec::new(),
329            edges_se3: Vec::new(),
330        }
331    }
332
333    pub fn vertex_count(&self) -> usize {
334        self.vertices_se2.len() + self.vertices_se3.len()
335    }
336
337    pub fn edge_count(&self) -> usize {
338        self.edges_se2.len() + self.edges_se3.len()
339    }
340
341    // Note: The from_optimized_variables() method has been moved to apex-solver
342    // as it depends on VariableEnum from the core module.
343}
344
345impl Default for Graph {
346    fn default() -> Self {
347        Self::new()
348    }
349}
350
351impl Display for Graph {
352    fn fmt(&self, f: &mut Formatter<'_>) -> fmt::Result {
353        write!(
354            f,
355            "Graph [[ vertices_se2: {} (count: {}), vertices_se3: {} (count: {}), edges_se2: {} (count: {}), edges_se3: {} (count: {}) ]]",
356            self.vertices_se2
357                .values()
358                .map(|v| format!("{}", v))
359                .collect::<Vec<_>>()
360                .join(", "),
361            self.vertices_se2.len(),
362            self.vertices_se3
363                .values()
364                .map(|v| format!("{}", v))
365                .collect::<Vec<_>>()
366                .join(", "),
367            self.vertices_se3.len(),
368            self.edges_se2
369                .iter()
370                .map(|e| format!("{}", e))
371                .collect::<Vec<_>>()
372                .join(", "),
373            self.edges_se2.len(),
374            self.edges_se3
375                .iter()
376                .map(|e| format!("{}", e))
377                .collect::<Vec<_>>()
378                .join(", "),
379            self.edges_se3.len()
380        )
381    }
382}
383
384/// Trait for graph file loaders and writers
385pub trait GraphLoader {
386    /// Load a graph from a file
387    fn load<P: AsRef<Path>>(path: P) -> Result<Graph, IoError>;
388
389    /// Write a graph to a file
390    fn write<P: AsRef<Path>>(graph: &Graph, path: P) -> Result<(), IoError>;
391}
392
393/// Convenience function to load any supported format based on file extension
394pub fn load_graph<P: AsRef<Path>>(path: P) -> Result<Graph, IoError> {
395    let path_ref = path.as_ref();
396    let extension = path_ref
397        .extension()
398        .and_then(|ext| ext.to_str())
399        .ok_or_else(|| {
400            IoError::UnsupportedFormat("No file extension".to_string())
401                .log_with_source(format!("File path: {:?}", path_ref))
402        })?;
403
404    match extension.to_lowercase().as_str() {
405        "g2o" => G2oLoader::load(path),
406        "graph" => ToroLoader::load(path),
407        _ => Err(
408            IoError::UnsupportedFormat(format!("Unsupported extension: {extension}"))
409                .log_with_source(format!("File path: {:?}", path_ref)),
410        ),
411    }
412}
413
414#[cfg(test)]
415mod tests {
416    use super::*;
417    use std::{error, io::Write};
418    use tempfile::NamedTempFile;
419
420    #[test]
421    fn test_load_simple_graph() -> Result<(), IoError> {
422        let mut temp_file = NamedTempFile::new().map_err(|e| {
423            IoError::FileCreationFailed {
424                path: "temp_file".to_string(),
425                reason: e.to_string(),
426            }
427            .log()
428        })?;
429        writeln!(temp_file, "VERTEX_SE2 0 0.0 0.0 0.0")?;
430        writeln!(temp_file, "VERTEX_SE2 1 1.0 0.0 0.0")?;
431        writeln!(temp_file, "# This is a comment")?;
432        writeln!(temp_file)?; // Empty line
433        writeln!(temp_file, "VERTEX_SE3:QUAT 2 0.0 0.0 0.0 0.0 0.0 0.0 1.0")?;
434
435        let graph = G2oLoader::load(temp_file.path())?;
436
437        assert_eq!(graph.vertices_se2.len(), 2);
438        assert_eq!(graph.vertices_se3.len(), 1);
439        assert!(graph.vertices_se2.contains_key(&0));
440        assert!(graph.vertices_se2.contains_key(&1));
441        assert!(graph.vertices_se3.contains_key(&2));
442
443        Ok(())
444    }
445
446    #[test]
447    fn test_load_m3500() -> Result<(), Box<dyn error::Error>> {
448        let graph = G2oLoader::load("../../data/odometry/M3500.g2o")?;
449        assert!(!graph.vertices_se2.is_empty());
450        Ok(())
451    }
452
453    #[test]
454    fn test_load_sphere2500() -> Result<(), Box<dyn error::Error>> {
455        let graph = G2oLoader::load("../../data/odometry/sphere2500.g2o")?;
456        assert!(!graph.vertices_se3.is_empty());
457        Ok(())
458    }
459
460    #[test]
461    fn test_duplicate_vertex_error() -> Result<(), io::Error> {
462        let mut temp_file = NamedTempFile::new()?;
463        writeln!(temp_file, "VERTEX_SE2 0 0.0 0.0 0.0")?;
464        writeln!(temp_file, "VERTEX_SE2 0 1.0 0.0 0.0")?; // Duplicate ID
465
466        let result = G2oLoader::load(temp_file.path());
467        assert!(matches!(result, Err(IoError::DuplicateVertex { id: 0 })));
468
469        Ok(())
470    }
471
472    #[test]
473    fn test_toro_loader() -> Result<(), IoError> {
474        let mut temp_file = NamedTempFile::new().map_err(|e| {
475            IoError::FileCreationFailed {
476                path: "temp_file".to_string(),
477                reason: e.to_string(),
478            }
479            .log()
480        })?;
481        writeln!(temp_file, "VERTEX2 0 0.0 0.0 0.0")?;
482        writeln!(temp_file, "VERTEX2 1 1.0 0.0 0.0")?;
483
484        let graph = ToroLoader::load(temp_file.path()).map_err(|e| e.log())?;
485        assert_eq!(graph.vertices_se2.len(), 2);
486
487        Ok(())
488    }
489
490    #[test]
491    #[cfg(feature = "visualization")]
492    fn test_se3_to_rerun() {
493        let vertex = VertexSE3::new(0, Vector3::new(1.0, 2.0, 3.0), UnitQuaternion::identity());
494
495        let (pos, rot) = vertex.to_rerun_transform(0.1);
496
497        assert!((pos.x - 0.1).abs() < 1e-6);
498        assert!((pos.y - 0.2).abs() < 1e-6);
499        assert!((pos.z - 0.3).abs() < 1e-6);
500        assert!((rot.w - 1.0).abs() < 1e-6);
501    }
502
503    #[test]
504    fn test_se2_to_rerun_2d() {
505        let vertex = VertexSE2::new(0, 10.0, 20.0, 0.5);
506
507        let pos = vertex.to_rerun_position_2d(0.1);
508
509        assert!((pos[0] - 1.0).abs() < 1e-6);
510        assert!((pos[1] - 2.0).abs() < 1e-6);
511    }
512
513    #[test]
514    #[cfg(feature = "visualization")]
515    fn test_se2_to_rerun_3d() {
516        let vertex = VertexSE2::new(0, 10.0, 20.0, 0.5);
517
518        let pos = vertex.to_rerun_position_3d(0.1, 5.0);
519
520        assert!((pos.x - 1.0).abs() < 1e-6);
521        assert!((pos.y - 2.0).abs() < 1e-6);
522        assert!((pos.z - 5.0).abs() < 1e-6);
523    }
524}
apex_io/lib.rs

apex_io/
lib.rs
