apex_solver/io/
mod.rs

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

apex_solver/io/
mod.rs
