apex_solver/io/
g2o.rs

1use crate::io;
2use memmap2;
3use rayon::prelude::*;
4use std::collections;
5use std::{fs, path};
6
7/// High-performance G2O file loader
8pub struct G2oLoader;
9
10impl io::GraphLoader for G2oLoader {
11    fn load<P: AsRef<path::Path>>(path: P) -> Result<io::Graph, io::ApexSolverIoError> {
12        let file = fs::File::open(path)?;
13        let mmap = unsafe { memmap2::Mmap::map(&file)? };
14        let content = std::str::from_utf8(&mmap).map_err(|e| io::ApexSolverIoError::Parse {
15            line: 0,
16            message: format!("Invalid UTF-8: {e}"),
17        })?;
18
19        Self::parse_content(content)
20    }
21
22    fn write<P: AsRef<path::Path>>(
23        graph: &io::Graph,
24        path: P,
25    ) -> Result<(), io::ApexSolverIoError> {
26        use std::io::Write;
27
28        let mut file = fs::File::create(path)?;
29
30        // Write header comment
31        writeln!(file, "# G2O file written by Apex Solver")?;
32        writeln!(
33            file,
34            "# Timestamp: {}",
35            chrono::Local::now().format("%Y-%m-%d %H:%M:%S")
36        )?;
37        writeln!(
38            file,
39            "# SE2 vertices: {}, SE3 vertices: {}, SE2 edges: {}, SE3 edges: {}",
40            graph.vertices_se2.len(),
41            graph.vertices_se3.len(),
42            graph.edges_se2.len(),
43            graph.edges_se3.len()
44        )?;
45        writeln!(file)?;
46
47        // Write SE2 vertices (sorted by ID)
48        let mut se2_ids: Vec<_> = graph.vertices_se2.keys().collect();
49        se2_ids.sort();
50
51        for id in se2_ids {
52            let vertex = &graph.vertices_se2[id];
53            writeln!(
54                file,
55                "VERTEX_SE2 {} {:.17e} {:.17e} {:.17e}",
56                vertex.id,
57                vertex.x(),
58                vertex.y(),
59                vertex.theta()
60            )?;
61        }
62
63        // Write SE3 vertices (sorted by ID)
64        let mut se3_ids: Vec<_> = graph.vertices_se3.keys().collect();
65        se3_ids.sort();
66
67        for id in se3_ids {
68            let vertex = &graph.vertices_se3[id];
69            let trans = vertex.translation();
70            let quat = vertex.rotation();
71            writeln!(
72                file,
73                "VERTEX_SE3:QUAT {} {:.17e} {:.17e} {:.17e} {:.17e} {:.17e} {:.17e} {:.17e}",
74                vertex.id, trans.x, trans.y, trans.z, quat.i, quat.j, quat.k, quat.w
75            )?;
76        }
77
78        // Write SE2 edges
79        for edge in &graph.edges_se2 {
80            let meas = &edge.measurement;
81            let info = &edge.information;
82
83            // G2O SE2 information matrix order: i11, i12, i22, i33, i13, i23
84            writeln!(
85                file,
86                "EDGE_SE2 {} {} {:.17e} {:.17e} {:.17e} {:.17e} {:.17e} {:.17e} {:.17e} {:.17e} {:.17e}",
87                edge.from,
88                edge.to,
89                meas.x(),
90                meas.y(),
91                meas.angle(),
92                info[(0, 0)],
93                info[(0, 1)],
94                info[(1, 1)],
95                info[(2, 2)],
96                info[(0, 2)],
97                info[(1, 2)]
98            )?;
99        }
100
101        // Write SE3 edges
102        for edge in &graph.edges_se3 {
103            let trans = edge.measurement.translation();
104            let quat = edge.measurement.rotation_quaternion();
105            let info = &edge.information;
106
107            // Write EDGE_SE3:QUAT with full 6x6 upper triangular information matrix (21 values)
108            write!(
109                file,
110                "EDGE_SE3:QUAT {} {} {:.17e} {:.17e} {:.17e} {:.17e} {:.17e} {:.17e} {:.17e}",
111                edge.from, edge.to, trans.x, trans.y, trans.z, quat.i, quat.j, quat.k, quat.w
112            )?;
113
114            // Write upper triangular information matrix (21 values)
115            for i in 0..6 {
116                for j in i..6 {
117                    write!(file, " {:.17e}", info[(i, j)])?;
118                }
119            }
120            writeln!(file)?;
121        }
122
123        Ok(())
124    }
125}
126
127impl G2oLoader {
128    /// Parse G2O content with performance optimizations
129    fn parse_content(content: &str) -> Result<io::Graph, io::ApexSolverIoError> {
130        let lines: Vec<&str> = content.lines().collect();
131        let minimum_lines_for_parallel = 1000;
132
133        // Pre-allocate collections based on estimated size
134        let estimated_vertices = lines.len() / 4;
135        let estimated_edges = estimated_vertices * 3;
136        let mut graph = io::Graph {
137            vertices_se2: collections::HashMap::with_capacity(estimated_vertices),
138            vertices_se3: collections::HashMap::with_capacity(estimated_vertices),
139            edges_se2: Vec::with_capacity(estimated_edges),
140            edges_se3: Vec::with_capacity(estimated_edges),
141        };
142
143        // For large files, use parallel processing
144        if lines.len() > minimum_lines_for_parallel {
145            Self::parse_parallel(&lines, &mut graph)?;
146        } else {
147            Self::parse_sequential(&lines, &mut graph)?;
148        }
149
150        Ok(graph)
151    }
152
153    /// Sequential parsing for smaller files
154    fn parse_sequential(
155        lines: &[&str],
156        graph: &mut io::Graph,
157    ) -> Result<(), io::ApexSolverIoError> {
158        for (line_num, line) in lines.iter().enumerate() {
159            Self::parse_line(line, line_num + 1, graph)?;
160        }
161        Ok(())
162    }
163
164    /// Parallel parsing for larger files
165    fn parse_parallel(lines: &[&str], graph: &mut io::Graph) -> Result<(), io::ApexSolverIoError> {
166        // Collect parse results in parallel
167        let results: Result<Vec<_>, io::ApexSolverIoError> = lines
168            .par_iter()
169            .enumerate()
170            .map(|(line_num, line)| Self::parse_line_to_enum(line, line_num + 1))
171            .collect();
172
173        let parsed_items = results?;
174
175        // Sequential insertion to avoid concurrent modification
176        for item in parsed_items.into_iter().flatten() {
177            match item {
178                ParsedItem::VertexSE2(vertex) => {
179                    let id = vertex.id;
180                    if graph.vertices_se2.insert(id, vertex).is_some() {
181                        return Err(io::ApexSolverIoError::DuplicateVertex { id });
182                    }
183                }
184                ParsedItem::VertexSE3(vertex) => {
185                    let id = vertex.id;
186                    if graph.vertices_se3.insert(id, vertex).is_some() {
187                        return Err(io::ApexSolverIoError::DuplicateVertex { id });
188                    }
189                }
190                ParsedItem::EdgeSE2(edge) => {
191                    graph.edges_se2.push(edge);
192                }
193                ParsedItem::EdgeSE3(edge) => {
194                    graph.edges_se3.push(*edge);
195                }
196            }
197        }
198
199        Ok(())
200    }
201
202    /// Parse a single line (for sequential processing)
203    fn parse_line(
204        line: &str,
205        line_num: usize,
206        graph: &mut io::Graph,
207    ) -> Result<(), io::ApexSolverIoError> {
208        let line = line.trim();
209
210        // Skip empty lines and comments
211        if line.is_empty() || line.starts_with('#') {
212            return Ok(());
213        }
214
215        let parts: Vec<&str> = line.split_whitespace().collect();
216        if parts.is_empty() {
217            return Ok(());
218        }
219
220        match parts[0] {
221            "VERTEX_SE2" => {
222                let vertex = Self::parse_vertex_se2(&parts, line_num)?;
223                let id = vertex.id;
224                if graph.vertices_se2.insert(id, vertex).is_some() {
225                    return Err(io::ApexSolverIoError::DuplicateVertex { id });
226                }
227            }
228            "VERTEX_SE3:QUAT" => {
229                let vertex = Self::parse_vertex_se3(&parts, line_num)?;
230                let id = vertex.id;
231                if graph.vertices_se3.insert(id, vertex).is_some() {
232                    return Err(io::ApexSolverIoError::DuplicateVertex { id });
233                }
234            }
235            "EDGE_SE2" => {
236                let edge = Self::parse_edge_se2(&parts, line_num)?;
237                graph.edges_se2.push(edge);
238            }
239            "EDGE_SE3:QUAT" => {
240                let edge = Self::parse_edge_se3(&parts, line_num)?;
241                graph.edges_se3.push(edge);
242            }
243            _ => {
244                // Skip unknown types silently for compatibility
245            }
246        }
247
248        Ok(())
249    }
250
251    /// Parse a single line to enum (for parallel processing)
252    fn parse_line_to_enum(
253        line: &str,
254        line_num: usize,
255    ) -> Result<Option<ParsedItem>, io::ApexSolverIoError> {
256        let line = line.trim();
257
258        // Skip empty lines and comments
259        if line.is_empty() || line.starts_with('#') {
260            return Ok(None);
261        }
262
263        let parts: Vec<&str> = line.split_whitespace().collect();
264        if parts.is_empty() {
265            return Ok(None);
266        }
267
268        let item = match parts[0] {
269            "VERTEX_SE2" => Some(ParsedItem::VertexSE2(Self::parse_vertex_se2(
270                &parts, line_num,
271            )?)),
272            "VERTEX_SE3:QUAT" => Some(ParsedItem::VertexSE3(Self::parse_vertex_se3(
273                &parts, line_num,
274            )?)),
275            "EDGE_SE2" => Some(ParsedItem::EdgeSE2(Self::parse_edge_se2(&parts, line_num)?)),
276            "EDGE_SE3:QUAT" => Some(ParsedItem::EdgeSE3(Box::new(Self::parse_edge_se3(
277                &parts, line_num,
278            )?))),
279            _ => None, // Skip unknown types
280        };
281
282        Ok(item)
283    }
284
285    /// Parse VERTEX_SE2 line
286    pub fn parse_vertex_se2(
287        parts: &[&str],
288        line_num: usize,
289    ) -> Result<io::VertexSE2, io::ApexSolverIoError> {
290        if parts.len() < 5 {
291            return Err(io::ApexSolverIoError::MissingFields { line: line_num });
292        }
293
294        let id = parts[1]
295            .parse::<usize>()
296            .map_err(|_| io::ApexSolverIoError::InvalidNumber {
297                line: line_num,
298                value: parts[1].to_string(),
299            })?;
300
301        let x = parts[2]
302            .parse::<f64>()
303            .map_err(|_| io::ApexSolverIoError::InvalidNumber {
304                line: line_num,
305                value: parts[2].to_string(),
306            })?;
307
308        let y = parts[3]
309            .parse::<f64>()
310            .map_err(|_| io::ApexSolverIoError::InvalidNumber {
311                line: line_num,
312                value: parts[3].to_string(),
313            })?;
314
315        let theta = parts[4]
316            .parse::<f64>()
317            .map_err(|_| io::ApexSolverIoError::InvalidNumber {
318                line: line_num,
319                value: parts[4].to_string(),
320            })?;
321
322        Ok(io::VertexSE2::new(id, x, y, theta))
323    }
324
325    /// Parse VERTEX_SE3:QUAT line
326    pub fn parse_vertex_se3(
327        parts: &[&str],
328        line_num: usize,
329    ) -> Result<io::VertexSE3, io::ApexSolverIoError> {
330        if parts.len() < 9 {
331            return Err(io::ApexSolverIoError::MissingFields { line: line_num });
332        }
333
334        let id = parts[1]
335            .parse::<usize>()
336            .map_err(|_| io::ApexSolverIoError::InvalidNumber {
337                line: line_num,
338                value: parts[1].to_string(),
339            })?;
340
341        let x = parts[2]
342            .parse::<f64>()
343            .map_err(|_| io::ApexSolverIoError::InvalidNumber {
344                line: line_num,
345                value: parts[2].to_string(),
346            })?;
347
348        let y = parts[3]
349            .parse::<f64>()
350            .map_err(|_| io::ApexSolverIoError::InvalidNumber {
351                line: line_num,
352                value: parts[3].to_string(),
353            })?;
354
355        let z = parts[4]
356            .parse::<f64>()
357            .map_err(|_| io::ApexSolverIoError::InvalidNumber {
358                line: line_num,
359                value: parts[4].to_string(),
360            })?;
361
362        let qx = parts[5]
363            .parse::<f64>()
364            .map_err(|_| io::ApexSolverIoError::InvalidNumber {
365                line: line_num,
366                value: parts[5].to_string(),
367            })?;
368
369        let qy = parts[6]
370            .parse::<f64>()
371            .map_err(|_| io::ApexSolverIoError::InvalidNumber {
372                line: line_num,
373                value: parts[6].to_string(),
374            })?;
375
376        let qz = parts[7]
377            .parse::<f64>()
378            .map_err(|_| io::ApexSolverIoError::InvalidNumber {
379                line: line_num,
380                value: parts[7].to_string(),
381            })?;
382
383        let qw = parts[8]
384            .parse::<f64>()
385            .map_err(|_| io::ApexSolverIoError::InvalidNumber {
386                line: line_num,
387                value: parts[8].to_string(),
388            })?;
389
390        let translation = nalgebra::Vector3::new(x, y, z);
391        let quaternion = nalgebra::Quaternion::new(qw, qx, qy, qz);
392
393        // Validate quaternion normalization
394        let quat_norm = (qw * qw + qx * qx + qy * qy + qz * qz).sqrt();
395        if (quat_norm - 1.0).abs() > 0.01 {
396            return Err(io::ApexSolverIoError::InvalidQuaternion {
397                line: line_num,
398                norm: quat_norm,
399            });
400        }
401
402        // Always normalize for numerical safety
403        let quaternion = quaternion.normalize();
404
405        Ok(io::VertexSE3::from_translation_quaternion(
406            id,
407            translation,
408            quaternion,
409        ))
410    }
411
412    /// Parse EDGE_SE2 line
413    fn parse_edge_se2(
414        parts: &[&str],
415        line_num: usize,
416    ) -> Result<io::EdgeSE2, io::ApexSolverIoError> {
417        if parts.len() < 12 {
418            return Err(io::ApexSolverIoError::MissingFields { line: line_num });
419        }
420
421        let from = parts[1]
422            .parse::<usize>()
423            .map_err(|_| io::ApexSolverIoError::InvalidNumber {
424                line: line_num,
425                value: parts[1].to_string(),
426            })?;
427
428        let to = parts[2]
429            .parse::<usize>()
430            .map_err(|_| io::ApexSolverIoError::InvalidNumber {
431                line: line_num,
432                value: parts[2].to_string(),
433            })?;
434
435        // Parse measurement (dx, dy, dtheta)
436        let dx = parts[3]
437            .parse::<f64>()
438            .map_err(|_| io::ApexSolverIoError::InvalidNumber {
439                line: line_num,
440                value: parts[3].to_string(),
441            })?;
442        let dy = parts[4]
443            .parse::<f64>()
444            .map_err(|_| io::ApexSolverIoError::InvalidNumber {
445                line: line_num,
446                value: parts[4].to_string(),
447            })?;
448        let dtheta = parts[5]
449            .parse::<f64>()
450            .map_err(|_| io::ApexSolverIoError::InvalidNumber {
451                line: line_num,
452                value: parts[5].to_string(),
453            })?;
454
455        // Parse information matrix (upper triangular: i11, i12, i13, i22, i23, i33)
456        let info_values: Result<Vec<f64>, _> =
457            parts[6..12].iter().map(|s| s.parse::<f64>()).collect();
458
459        let info_values = info_values.map_err(|_| io::ApexSolverIoError::Parse {
460            line: line_num,
461            message: "Invalid information matrix values".to_string(),
462        })?;
463
464        let information = nalgebra::Matrix3::new(
465            info_values[0],
466            info_values[1],
467            info_values[2],
468            info_values[1],
469            info_values[3],
470            info_values[4],
471            info_values[2],
472            info_values[4],
473            info_values[5],
474        );
475
476        Ok(io::EdgeSE2::new(from, to, dx, dy, dtheta, information))
477    }
478
479    /// Parse EDGE_SE3:QUAT line (placeholder implementation)
480    fn parse_edge_se3(
481        parts: &[&str],
482        line_num: usize,
483    ) -> Result<io::EdgeSE3, io::ApexSolverIoError> {
484        // EDGE_SE3:QUAT from_id to_id tx ty tz qx qy qz qw [information matrix values]
485        if parts.len() < 10 {
486            return Err(io::ApexSolverIoError::MissingFields { line: line_num });
487        }
488
489        // Parse vertex IDs
490        let from = parts[1]
491            .parse::<usize>()
492            .map_err(|_| io::ApexSolverIoError::InvalidNumber {
493                line: line_num,
494                value: parts[1].to_string(),
495            })?;
496
497        let to = parts[2]
498            .parse::<usize>()
499            .map_err(|_| io::ApexSolverIoError::InvalidNumber {
500                line: line_num,
501                value: parts[2].to_string(),
502            })?;
503
504        // Parse translation (tx, ty, tz)
505        let tx = parts[3]
506            .parse::<f64>()
507            .map_err(|_| io::ApexSolverIoError::InvalidNumber {
508                line: line_num,
509                value: parts[3].to_string(),
510            })?;
511
512        let ty = parts[4]
513            .parse::<f64>()
514            .map_err(|_| io::ApexSolverIoError::InvalidNumber {
515                line: line_num,
516                value: parts[4].to_string(),
517            })?;
518
519        let tz = parts[5]
520            .parse::<f64>()
521            .map_err(|_| io::ApexSolverIoError::InvalidNumber {
522                line: line_num,
523                value: parts[5].to_string(),
524            })?;
525
526        let translation = nalgebra::Vector3::new(tx, ty, tz);
527
528        // Parse rotation quaternion (qx, qy, qz, qw)
529        let qx = parts[6]
530            .parse::<f64>()
531            .map_err(|_| io::ApexSolverIoError::InvalidNumber {
532                line: line_num,
533                value: parts[6].to_string(),
534            })?;
535
536        let qy = parts[7]
537            .parse::<f64>()
538            .map_err(|_| io::ApexSolverIoError::InvalidNumber {
539                line: line_num,
540                value: parts[7].to_string(),
541            })?;
542
543        let qz = parts[8]
544            .parse::<f64>()
545            .map_err(|_| io::ApexSolverIoError::InvalidNumber {
546                line: line_num,
547                value: parts[8].to_string(),
548            })?;
549
550        let qw = parts[9]
551            .parse::<f64>()
552            .map_err(|_| io::ApexSolverIoError::InvalidNumber {
553                line: line_num,
554                value: parts[9].to_string(),
555            })?;
556
557        let rotation =
558            nalgebra::UnitQuaternion::from_quaternion(nalgebra::Quaternion::new(qw, qx, qy, qz));
559
560        // Parse information matrix (upper triangular: i11, i12, i13, i14, i15, i16, i22, i23, i24, i25, i26, i33, i34, i35, i36, i44, i45, i46, i55, i56, i66)
561        let info_values: Result<Vec<f64>, _> =
562            parts[10..31].iter().map(|s| s.parse::<f64>()).collect();
563
564        let info_values = info_values.map_err(|_| io::ApexSolverIoError::Parse {
565            line: line_num,
566            message: "Invalid information matrix values".to_string(),
567        })?;
568
569        let information = nalgebra::Matrix6::new(
570            info_values[0],
571            info_values[1],
572            info_values[2],
573            info_values[3],
574            info_values[4],
575            info_values[5],
576            info_values[1],
577            info_values[6],
578            info_values[7],
579            info_values[8],
580            info_values[9],
581            info_values[10],
582            info_values[2],
583            info_values[7],
584            info_values[11],
585            info_values[12],
586            info_values[13],
587            info_values[14],
588            info_values[3],
589            info_values[8],
590            info_values[12],
591            info_values[15],
592            info_values[16],
593            info_values[17],
594            info_values[4],
595            info_values[9],
596            info_values[13],
597            info_values[16],
598            info_values[18],
599            info_values[19],
600            info_values[5],
601            info_values[10],
602            info_values[14],
603            info_values[17],
604            info_values[19],
605            info_values[20],
606        );
607
608        Ok(io::EdgeSE3::new(
609            from,
610            to,
611            translation,
612            rotation,
613            information,
614        ))
615    }
616}
617
618/// Enum for parsed items (used in parallel processing)
619enum ParsedItem {
620    VertexSE2(io::VertexSE2),
621    VertexSE3(io::VertexSE3),
622    EdgeSE2(io::EdgeSE2),
623    EdgeSE3(Box<io::EdgeSE3>),
624}
625
626#[cfg(test)]
627mod tests {
628    use super::*;
629
630    #[test]
631    fn test_parse_vertex_se2() {
632        let parts = vec!["VERTEX_SE2", "0", "1.0", "2.0", "0.5"];
633        let vertex = G2oLoader::parse_vertex_se2(&parts, 1).unwrap();
634
635        assert_eq!(vertex.id(), 0);
636        assert_eq!(vertex.x(), 1.0);
637        assert_eq!(vertex.y(), 2.0);
638        assert_eq!(vertex.theta(), 0.5);
639    }
640
641    #[test]
642    fn test_parse_vertex_se3() {
643        let parts = vec![
644            "VERTEX_SE3:QUAT",
645            "1",
646            "1.0",
647            "2.0",
648            "3.0",
649            "0.0",
650            "0.0",
651            "0.0",
652            "1.0",
653        ];
654        let vertex = G2oLoader::parse_vertex_se3(&parts, 1).unwrap();
655
656        assert_eq!(vertex.id(), 1);
657        assert_eq!(vertex.translation(), nalgebra::Vector3::new(1.0, 2.0, 3.0));
658        assert!(vertex.rotation().quaternion().w > 0.99); // Should be identity quaternion
659    }
660
661    #[test]
662    fn test_error_handling() {
663        // Test invalid number
664        let parts = vec!["VERTEX_SE2", "invalid", "1.0", "2.0", "0.5"];
665        let result = G2oLoader::parse_vertex_se2(&parts, 1);
666        assert!(matches!(
667            result,
668            Err(io::ApexSolverIoError::InvalidNumber { .. })
669        ));
670
671        // Test missing fields
672        let parts = vec!["VERTEX_SE2", "0"];
673        let result = G2oLoader::parse_vertex_se2(&parts, 1);
674        assert!(matches!(
675            result,
676            Err(io::ApexSolverIoError::MissingFields { .. })
677        ));
678    }
679}
apex_solver/io/g2o.rs

apex_solver/io/
g2o.rs
