apex_io/graph/
mod.rs

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

apex_io/graph/
mod.rs
