apex_solver/io/
mod.rs

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

apex_solver/io/
mod.rs
