apex_io/
toro.rs

1use crate::{EdgeSE2, Graph, GraphLoader, IoError, VertexSE2};
2use memmap2::Mmap;
3use std::{fs, io::Write, path::Path};
4
5/// TORO format loader
6pub struct ToroLoader;
7
8impl GraphLoader for ToroLoader {
9    fn load<P: AsRef<Path>>(path: P) -> Result<Graph, IoError> {
10        let path_ref = path.as_ref();
11        let file = fs::File::open(path_ref).map_err(|e| {
12            IoError::Io(e).log_with_source(format!("Failed to open TORO file: {:?}", path_ref))
13        })?;
14        // SAFETY: The file is opened read-only and the handle remains valid for the
15        // lifetime of `mmap`. No other thread modifies the file during this scope.
16        let mmap = unsafe {
17            Mmap::map(&file).map_err(|e| {
18                IoError::Io(e)
19                    .log_with_source(format!("Failed to memory-map TORO file: {:?}", path_ref))
20            })?
21        };
22        let content = std::str::from_utf8(&mmap).map_err(|e| {
23            IoError::Parse {
24                line: 0,
25                message: format!("Invalid UTF-8: {e}"),
26            }
27            .log()
28        })?;
29
30        Self::parse_content(content)
31    }
32
33    fn write<P: AsRef<Path>>(graph: &Graph, path: P) -> Result<(), IoError> {
34        // TORO only supports SE2
35        if !graph.vertices_se3.is_empty() || !graph.edges_se3.is_empty() {
36            return Err(IoError::UnsupportedFormat(
37                "TORO format only supports SE2 (2D) graphs. Use G2O format for SE3 data."
38                    .to_string(),
39            )
40            .log());
41        }
42
43        let path_ref = path.as_ref();
44        let mut file = fs::File::create(path_ref).map_err(|e| {
45            IoError::Io(e).log_with_source(format!("Failed to create TORO file: {:?}", path_ref))
46        })?;
47
48        // Write SE2 vertices (sorted by ID)
49        let mut vertex_ids: Vec<_> = graph.vertices_se2.keys().collect();
50        vertex_ids.sort();
51
52        for id in vertex_ids {
53            let vertex = &graph.vertices_se2[id];
54            writeln!(
55                file,
56                "VERTEX2 {} {:.17e} {:.17e} {:.17e}",
57                vertex.id,
58                vertex.x(),
59                vertex.y(),
60                vertex.theta()
61            )
62            .map_err(|e| {
63                IoError::Io(e).log_with_source(format!("Failed to write TORO vertex {}", vertex.id))
64            })?;
65        }
66
67        // Write SE2 edges
68        // TORO format: EDGE2 <id1> <id2> <dx> <dy> <dtheta> <i11> <i12> <i22> <i33> <i13> <i23>
69        for edge in &graph.edges_se2 {
70            let meas = &edge.measurement;
71            let info = &edge.information;
72
73            writeln!(
74                file,
75                "EDGE2 {} {} {:.17e} {:.17e} {:.17e} {:.17e} {:.17e} {:.17e} {:.17e} {:.17e} {:.17e}",
76                edge.from,
77                edge.to,
78                meas.x(),
79                meas.y(),
80                meas.angle(),
81                info[(0, 0)], // i11
82                info[(0, 1)], // i12
83                info[(1, 1)], // i22
84                info[(2, 2)], // i33
85                info[(0, 2)], // i13
86                info[(1, 2)]  // i23
87            )
88            .map_err(|e| {
89                IoError::Io(e).log_with_source(format!(
90                    "Failed to write TORO edge {} -> {}",
91                    edge.from, edge.to
92                ))
93            })?;
94        }
95
96        Ok(())
97    }
98}
99
100impl ToroLoader {
101    fn parse_content(content: &str) -> Result<Graph, IoError> {
102        let lines: Vec<&str> = content.lines().collect();
103        let mut graph = Graph::new();
104
105        for (line_num, line) in lines.iter().enumerate() {
106            Self::parse_line(line, line_num + 1, &mut graph)?;
107        }
108
109        Ok(graph)
110    }
111
112    fn parse_line(line: &str, line_num: usize, graph: &mut Graph) -> Result<(), IoError> {
113        let line = line.trim();
114
115        // Skip empty lines and comments
116        if line.is_empty() || line.starts_with('#') {
117            return Ok(());
118        }
119
120        let parts: Vec<&str> = line.split_whitespace().collect();
121        if parts.is_empty() {
122            return Ok(());
123        }
124
125        match parts[0] {
126            "VERTEX2" => {
127                let vertex = Self::parse_vertex2(&parts, line_num)?;
128                let id = vertex.id;
129                if graph.vertices_se2.insert(id, vertex).is_some() {
130                    return Err(IoError::DuplicateVertex { id });
131                }
132            }
133            "EDGE2" => {
134                let edge = Self::parse_edge2(&parts, line_num)?;
135                graph.edges_se2.push(edge);
136            }
137            _ => {
138                // Skip unknown types silently for compatibility
139            }
140        }
141
142        Ok(())
143    }
144
145    fn parse_vertex2(parts: &[&str], line_num: usize) -> Result<VertexSE2, IoError> {
146        if parts.len() < 5 {
147            return Err(IoError::MissingFields { line: line_num });
148        }
149
150        let id = parts[1]
151            .parse::<usize>()
152            .map_err(|_| IoError::InvalidNumber {
153                line: line_num,
154                value: parts[1].to_string(),
155            })?;
156
157        let x = parts[2]
158            .parse::<f64>()
159            .map_err(|_| IoError::InvalidNumber {
160                line: line_num,
161                value: parts[2].to_string(),
162            })?;
163
164        let y = parts[3]
165            .parse::<f64>()
166            .map_err(|_| IoError::InvalidNumber {
167                line: line_num,
168                value: parts[3].to_string(),
169            })?;
170
171        let theta = parts[4]
172            .parse::<f64>()
173            .map_err(|_| IoError::InvalidNumber {
174                line: line_num,
175                value: parts[4].to_string(),
176            })?;
177
178        Ok(VertexSE2::new(id, x, y, theta))
179    }
180
181    fn parse_edge2(parts: &[&str], line_num: usize) -> Result<EdgeSE2, IoError> {
182        if parts.len() < 12 {
183            return Err(IoError::MissingFields { line: line_num });
184        }
185
186        let from = parts[1]
187            .parse::<usize>()
188            .map_err(|_| IoError::InvalidNumber {
189                line: line_num,
190                value: parts[1].to_string(),
191            })?;
192
193        let to = parts[2]
194            .parse::<usize>()
195            .map_err(|_| IoError::InvalidNumber {
196                line: line_num,
197                value: parts[2].to_string(),
198            })?;
199
200        // Parse measurement (dx, dy, dtheta)
201        let dx = parts[3]
202            .parse::<f64>()
203            .map_err(|_| IoError::InvalidNumber {
204                line: line_num,
205                value: parts[3].to_string(),
206            })?;
207        let dy = parts[4]
208            .parse::<f64>()
209            .map_err(|_| IoError::InvalidNumber {
210                line: line_num,
211                value: parts[4].to_string(),
212            })?;
213        let dtheta = parts[5]
214            .parse::<f64>()
215            .map_err(|_| IoError::InvalidNumber {
216                line: line_num,
217                value: parts[5].to_string(),
218            })?;
219
220        // Parse TORO information matrix (I11, I12, I22, I33, I13, I23)
221        let i11 = parts[6]
222            .parse::<f64>()
223            .map_err(|_| IoError::InvalidNumber {
224                line: line_num,
225                value: parts[6].to_string(),
226            })?;
227        let i12 = parts[7]
228            .parse::<f64>()
229            .map_err(|_| IoError::InvalidNumber {
230                line: line_num,
231                value: parts[7].to_string(),
232            })?;
233        let i22 = parts[8]
234            .parse::<f64>()
235            .map_err(|_| IoError::InvalidNumber {
236                line: line_num,
237                value: parts[8].to_string(),
238            })?;
239        let i33 = parts[9]
240            .parse::<f64>()
241            .map_err(|_| IoError::InvalidNumber {
242                line: line_num,
243                value: parts[9].to_string(),
244            })?;
245        let i13 = parts[10]
246            .parse::<f64>()
247            .map_err(|_| IoError::InvalidNumber {
248                line: line_num,
249                value: parts[10].to_string(),
250            })?;
251        let i23 = parts[11]
252            .parse::<f64>()
253            .map_err(|_| IoError::InvalidNumber {
254                line: line_num,
255                value: parts[11].to_string(),
256            })?;
257
258        let information = nalgebra::Matrix3::new(i11, i12, i13, i12, i22, i23, i13, i23, i33);
259
260        Ok(EdgeSE2::new(from, to, dx, dy, dtheta, information))
261    }
262}
263
264#[cfg(test)]
265#[allow(clippy::unwrap_used)]
266mod tests {
267    use super::*;
268    use crate::{EdgeSE3, VertexSE3};
269    use nalgebra::{Matrix3, UnitQuaternion, Vector3};
270    use std::io::Write;
271    use tempfile::NamedTempFile;
272
273    type TestResult = Result<(), Box<dyn std::error::Error>>;
274
275    fn write_toro_content(content: &str) -> Result<NamedTempFile, Box<dyn std::error::Error>> {
276        let mut f = NamedTempFile::new()?;
277        write!(f, "{}", content)?;
278        f.flush()?;
279        Ok(f)
280    }
281
282    #[test]
283    fn test_parse_vertex2_and_edge2() -> TestResult {
284        let content = "VERTEX2 0 1.0 2.0 0.5\n\
285                       VERTEX2 1 3.0 4.0 1.0\n\
286                       EDGE2 0 1 0.5 0.3 0.1 500.0 0.0 500.0 200.0 0.0 0.0\n";
287        let f = write_toro_content(content)?;
288        let graph = ToroLoader::load(f.path())?;
289        assert_eq!(graph.vertices_se2.len(), 2);
290        assert_eq!(graph.edges_se2.len(), 1);
291        let v0 = &graph.vertices_se2[&0];
292        assert!((v0.x() - 1.0).abs() < 1e-10);
293        assert!((v0.y() - 2.0).abs() < 1e-10);
294        let e = &graph.edges_se2[0];
295        assert_eq!(e.from, 0);
296        assert_eq!(e.to, 1);
297        assert!((e.information[(0, 0)] - 500.0).abs() < 1e-10);
298        Ok(())
299    }
300
301    #[test]
302    fn test_write_and_reload_round_trip() -> TestResult {
303        let mut graph = Graph::new();
304        graph
305            .vertices_se2
306            .insert(0, VertexSE2::new(0, 1.0, 2.0, 0.5));
307        graph
308            .vertices_se2
309            .insert(1, VertexSE2::new(1, 3.0, 4.0, 1.0));
310        let info = Matrix3::new(500.0, 0.0, 0.0, 0.0, 500.0, 0.0, 0.0, 0.0, 200.0);
311        graph
312            .edges_se2
313            .push(EdgeSE2::new(0, 1, 0.5, 0.3, 0.1, info));
314
315        let f = NamedTempFile::new()?;
316        ToroLoader::write(&graph, f.path())?;
317        let loaded = ToroLoader::load(f.path())?;
318
319        assert_eq!(loaded.vertices_se2.len(), 2);
320        assert_eq!(loaded.edges_se2.len(), 1);
321        let v0 = &loaded.vertices_se2[&0];
322        assert!((v0.x() - 1.0).abs() < 1e-10);
323        assert!((v0.y() - 2.0).abs() < 1e-10);
324        let e = &loaded.edges_se2[0];
325        assert_eq!(e.from, 0);
326        assert_eq!(e.to, 1);
327        assert!((e.information[(0, 0)] - 500.0).abs() < 1e-10);
328        Ok(())
329    }
330
331    #[test]
332    fn test_write_rejects_se3_vertices() -> TestResult {
333        let mut graph = Graph::new();
334        graph.vertices_se3.insert(
335            0,
336            VertexSE3::new(0, Vector3::zeros(), UnitQuaternion::identity()),
337        );
338        let f = NamedTempFile::new()?;
339        let result = ToroLoader::write(&graph, f.path());
340        assert!(
341            matches!(result, Err(IoError::UnsupportedFormat(_))),
342            "should reject graph with SE3 vertices"
343        );
344        Ok(())
345    }
346
347    #[test]
348    fn test_write_rejects_se3_edges() -> TestResult {
349        let mut graph = Graph::new();
350        graph.edges_se3.push(EdgeSE3::new(
351            0,
352            1,
353            Vector3::zeros(),
354            UnitQuaternion::identity(),
355            nalgebra::Matrix6::identity(),
356        ));
357        let f = NamedTempFile::new()?;
358        let result = ToroLoader::write(&graph, f.path());
359        assert!(
360            matches!(result, Err(IoError::UnsupportedFormat(_))),
361            "should reject graph with SE3 edges"
362        );
363        Ok(())
364    }
365
366    #[test]
367    fn test_duplicate_vertex_returns_error() -> TestResult {
368        let content = "VERTEX2 5 1.0 2.0 0.0\nVERTEX2 5 3.0 4.0 0.0\n";
369        let f = write_toro_content(content)?;
370        let result = ToroLoader::load(f.path());
371        assert!(
372            matches!(result, Err(IoError::DuplicateVertex { id: 5 })),
373            "duplicate vertex ID should return DuplicateVertex error"
374        );
375        Ok(())
376    }
377
378    #[test]
379    fn test_parse_missing_vertex_fields() -> TestResult {
380        // VERTEX2 needs 5 fields: VERTEX2 id x y theta
381        let content = "VERTEX2 0 1.0\n"; // only 3 fields
382        let f = write_toro_content(content)?;
383        let result = ToroLoader::load(f.path());
384        assert!(result.is_err(), "VERTEX2 with too few fields should fail");
385        Ok(())
386    }
387
388    #[test]
389    fn test_parse_missing_edge_fields() -> TestResult {
390        // EDGE2 needs 12 fields
391        let content = "EDGE2 0 1 0.5 0.3\n"; // only 5 fields
392        let f = write_toro_content(content)?;
393        let result = ToroLoader::load(f.path());
394        assert!(result.is_err(), "EDGE2 with too few fields should fail");
395        Ok(())
396    }
397
398    #[test]
399    fn test_comment_and_empty_lines_ignored() -> TestResult {
400        let content = "# this is a comment\n\
401                       VERTEX2 0 1.0 2.0 0.0\n\
402                       \n\
403                       VERTEX2 1 2.0 3.0 0.0\n";
404        let f = write_toro_content(content)?;
405        let graph = ToroLoader::load(f.path())?;
406        assert_eq!(
407            graph.vertices_se2.len(),
408            2,
409            "comments and blank lines should be ignored"
410        );
411        Ok(())
412    }
413
414    #[test]
415    fn test_unknown_token_ignored() -> TestResult {
416        let content = "UNKNOWN_TOKEN 1 2 3\nVERTEX2 0 0.0 0.0 0.0\n";
417        let f = write_toro_content(content)?;
418        let graph = ToroLoader::load(f.path())?;
419        assert_eq!(
420            graph.vertices_se2.len(),
421            1,
422            "unknown token lines should be silently skipped"
423        );
424        Ok(())
425    }
426
427    #[test]
428    fn test_load_nonexistent_file() {
429        let result = ToroLoader::load("/no/such/file.graph");
430        assert!(result.is_err(), "loading missing file should return Err");
431    }
432
433    #[test]
434    fn test_write_empty_graph() -> TestResult {
435        let graph = Graph::new();
436        let f = NamedTempFile::new()?;
437        ToroLoader::write(&graph, f.path())?;
438        let loaded = ToroLoader::load(f.path())?;
439        assert_eq!(loaded.vertices_se2.len(), 0);
440        assert_eq!(loaded.edges_se2.len(), 0);
441        Ok(())
442    }
443
444    #[test]
445    fn test_parse_vertex2_invalid_number() -> TestResult {
446        let content = "VERTEX2 0 bad 2.0 0.0\n"; // bad x value
447        let f = write_toro_content(content)?;
448        let result = ToroLoader::load(f.path());
449        assert!(result.is_err(), "invalid number in VERTEX2 should fail");
450        Ok(())
451    }
452
453    // -------------------------------------------------------------------------
454    // parse_vertex2 additional error paths
455    // -------------------------------------------------------------------------
456
457    #[test]
458    fn test_parse_vertex2_invalid_id() -> TestResult {
459        let content = "VERTEX2 bad 1.0 2.0 0.0\n";
460        let f = write_toro_content(content)?;
461        let result = ToroLoader::load(f.path());
462        assert!(
463            matches!(result, Err(IoError::InvalidNumber { .. })),
464            "invalid id in VERTEX2 should return InvalidNumber"
465        );
466        Ok(())
467    }
468
469    #[test]
470    fn test_parse_vertex2_invalid_y() -> TestResult {
471        let content = "VERTEX2 0 1.0 bad 0.0\n";
472        let f = write_toro_content(content)?;
473        let result = ToroLoader::load(f.path());
474        assert!(
475            matches!(result, Err(IoError::InvalidNumber { .. })),
476            "invalid y in VERTEX2 should return InvalidNumber"
477        );
478        Ok(())
479    }
480
481    #[test]
482    fn test_parse_vertex2_invalid_theta() -> TestResult {
483        let content = "VERTEX2 0 1.0 2.0 bad\n";
484        let f = write_toro_content(content)?;
485        let result = ToroLoader::load(f.path());
486        assert!(
487            matches!(result, Err(IoError::InvalidNumber { .. })),
488            "invalid theta in VERTEX2 should return InvalidNumber"
489        );
490        Ok(())
491    }
492
493    // -------------------------------------------------------------------------
494    // parse_edge2 error paths
495    // -------------------------------------------------------------------------
496
497    #[test]
498    fn test_parse_edge2_invalid_from() -> TestResult {
499        let content = "EDGE2 bad 1 0.5 0.3 0.1 500.0 0.0 500.0 200.0 0.0 0.0\n";
500        let f = write_toro_content(content)?;
501        let result = ToroLoader::load(f.path());
502        assert!(
503            matches!(result, Err(IoError::InvalidNumber { .. })),
504            "invalid from-id in EDGE2 should return InvalidNumber"
505        );
506        Ok(())
507    }
508
509    #[test]
510    fn test_parse_edge2_invalid_to() -> TestResult {
511        let content = "EDGE2 0 bad 0.5 0.3 0.1 500.0 0.0 500.0 200.0 0.0 0.0\n";
512        let f = write_toro_content(content)?;
513        let result = ToroLoader::load(f.path());
514        assert!(
515            matches!(result, Err(IoError::InvalidNumber { .. })),
516            "invalid to-id in EDGE2 should return InvalidNumber"
517        );
518        Ok(())
519    }
520
521    #[test]
522    fn test_parse_edge2_invalid_dx() -> TestResult {
523        let content = "EDGE2 0 1 bad 0.3 0.1 500.0 0.0 500.0 200.0 0.0 0.0\n";
524        let f = write_toro_content(content)?;
525        let result = ToroLoader::load(f.path());
526        assert!(
527            matches!(result, Err(IoError::InvalidNumber { .. })),
528            "invalid dx in EDGE2 should return InvalidNumber"
529        );
530        Ok(())
531    }
532
533    #[test]
534    fn test_parse_edge2_invalid_dy() -> TestResult {
535        let content = "EDGE2 0 1 0.5 bad 0.1 500.0 0.0 500.0 200.0 0.0 0.0\n";
536        let f = write_toro_content(content)?;
537        let result = ToroLoader::load(f.path());
538        assert!(
539            matches!(result, Err(IoError::InvalidNumber { .. })),
540            "invalid dy in EDGE2 should return InvalidNumber"
541        );
542        Ok(())
543    }
544
545    #[test]
546    fn test_parse_edge2_invalid_dtheta() -> TestResult {
547        let content = "EDGE2 0 1 0.5 0.3 bad 500.0 0.0 500.0 200.0 0.0 0.0\n";
548        let f = write_toro_content(content)?;
549        let result = ToroLoader::load(f.path());
550        assert!(
551            matches!(result, Err(IoError::InvalidNumber { .. })),
552            "invalid dtheta in EDGE2 should return InvalidNumber"
553        );
554        Ok(())
555    }
556
557    #[test]
558    fn test_parse_edge2_invalid_i11() -> TestResult {
559        let content = "EDGE2 0 1 0.5 0.3 0.1 bad 0.0 500.0 200.0 0.0 0.0\n";
560        let f = write_toro_content(content)?;
561        let result = ToroLoader::load(f.path());
562        assert!(
563            matches!(result, Err(IoError::InvalidNumber { .. })),
564            "invalid i11 in EDGE2 should return InvalidNumber"
565        );
566        Ok(())
567    }
568
569    #[test]
570    fn test_parse_edge2_invalid_i12() -> TestResult {
571        let content = "EDGE2 0 1 0.5 0.3 0.1 500.0 bad 500.0 200.0 0.0 0.0\n";
572        let f = write_toro_content(content)?;
573        let result = ToroLoader::load(f.path());
574        assert!(
575            matches!(result, Err(IoError::InvalidNumber { .. })),
576            "invalid i12 in EDGE2 should return InvalidNumber"
577        );
578        Ok(())
579    }
580
581    #[test]
582    fn test_parse_edge2_invalid_i22() -> TestResult {
583        let content = "EDGE2 0 1 0.5 0.3 0.1 500.0 0.0 bad 200.0 0.0 0.0\n";
584        let f = write_toro_content(content)?;
585        let result = ToroLoader::load(f.path());
586        assert!(
587            matches!(result, Err(IoError::InvalidNumber { .. })),
588            "invalid i22 in EDGE2 should return InvalidNumber"
589        );
590        Ok(())
591    }
592
593    #[test]
594    fn test_parse_edge2_invalid_i33() -> TestResult {
595        let content = "EDGE2 0 1 0.5 0.3 0.1 500.0 0.0 500.0 bad 0.0 0.0\n";
596        let f = write_toro_content(content)?;
597        let result = ToroLoader::load(f.path());
598        assert!(
599            matches!(result, Err(IoError::InvalidNumber { .. })),
600            "invalid i33 in EDGE2 should return InvalidNumber"
601        );
602        Ok(())
603    }
604
605    #[test]
606    fn test_parse_edge2_invalid_i13() -> TestResult {
607        let content = "EDGE2 0 1 0.5 0.3 0.1 500.0 0.0 500.0 200.0 bad 0.0\n";
608        let f = write_toro_content(content)?;
609        let result = ToroLoader::load(f.path());
610        assert!(
611            matches!(result, Err(IoError::InvalidNumber { .. })),
612            "invalid i13 in EDGE2 should return InvalidNumber"
613        );
614        Ok(())
615    }
616
617    #[test]
618    fn test_parse_edge2_invalid_i23() -> TestResult {
619        let content = "EDGE2 0 1 0.5 0.3 0.1 500.0 0.0 500.0 200.0 0.0 bad\n";
620        let f = write_toro_content(content)?;
621        let result = ToroLoader::load(f.path());
622        assert!(
623            matches!(result, Err(IoError::InvalidNumber { .. })),
624            "invalid i23 in EDGE2 should return InvalidNumber"
625        );
626        Ok(())
627    }
628
629    // -------------------------------------------------------------------------
630    // Round-trip fidelity
631    // -------------------------------------------------------------------------
632
633    #[test]
634    fn test_edge_measurement_all_components_round_trip() -> TestResult {
635        let mut graph = Graph::new();
636        graph
637            .vertices_se2
638            .insert(0, VertexSE2::new(0, 0.0, 0.0, 0.0));
639        graph
640            .vertices_se2
641            .insert(1, VertexSE2::new(1, 1.0, 0.0, 0.0));
642        let info = Matrix3::identity();
643        graph
644            .edges_se2
645            .push(EdgeSE2::new(0, 1, 1.5, 2.5, 0.7, info));
646
647        let f = NamedTempFile::new()?;
648        ToroLoader::write(&graph, f.path())?;
649        let loaded = ToroLoader::load(f.path())?;
650
651        let e = &loaded.edges_se2[0];
652        assert!((e.measurement.x() - 1.5).abs() < 1e-10, "dx mismatch");
653        assert!((e.measurement.y() - 2.5).abs() < 1e-10, "dy mismatch");
654        assert!(
655            (e.measurement.angle() - 0.7).abs() < 1e-10,
656            "dtheta mismatch"
657        );
658        Ok(())
659    }
660
661    #[test]
662    fn test_off_diagonal_info_matrix_round_trip() -> TestResult {
663        let mut graph = Graph::new();
664        graph
665            .vertices_se2
666            .insert(0, VertexSE2::new(0, 0.0, 0.0, 0.0));
667        graph
668            .vertices_se2
669            .insert(1, VertexSE2::new(1, 1.0, 0.0, 0.0));
670        // Symmetric matrix with off-diagonal entries
671        let info = Matrix3::new(500.0, 10.0, 5.0, 10.0, 400.0, 3.0, 5.0, 3.0, 200.0);
672        graph
673            .edges_se2
674            .push(EdgeSE2::new(0, 1, 1.0, 0.0, 0.0, info));
675
676        let f = NamedTempFile::new()?;
677        ToroLoader::write(&graph, f.path())?;
678        let loaded = ToroLoader::load(f.path())?;
679
680        let e = &loaded.edges_se2[0];
681        assert!((e.information[(0, 0)] - 500.0).abs() < 1e-10, "i11");
682        assert!((e.information[(0, 1)] - 10.0).abs() < 1e-10, "i12");
683        assert!((e.information[(1, 1)] - 400.0).abs() < 1e-10, "i22");
684        assert!((e.information[(2, 2)] - 200.0).abs() < 1e-10, "i33");
685        assert!((e.information[(0, 2)] - 5.0).abs() < 1e-10, "i13");
686        assert!((e.information[(1, 2)] - 3.0).abs() < 1e-10, "i23");
687        Ok(())
688    }
689
690    #[test]
691    fn test_multiple_edges_round_trip() -> TestResult {
692        let mut graph = Graph::new();
693        for i in 0..4usize {
694            graph
695                .vertices_se2
696                .insert(i, VertexSE2::new(i, i as f64, 0.0, 0.0));
697        }
698        let info = Matrix3::identity();
699        for i in 0..3usize {
700            graph
701                .edges_se2
702                .push(EdgeSE2::new(i, i + 1, 1.0, 0.0, 0.0, info));
703        }
704
705        let f = NamedTempFile::new()?;
706        ToroLoader::write(&graph, f.path())?;
707        let loaded = ToroLoader::load(f.path())?;
708
709        assert_eq!(loaded.vertices_se2.len(), 4);
710        assert_eq!(loaded.edges_se2.len(), 3);
711        Ok(())
712    }
713
714    #[test]
715    fn test_vertex_theta_preserved_round_trip() -> TestResult {
716        let mut graph = Graph::new();
717        graph
718            .vertices_se2
719            .insert(0, VertexSE2::new(0, 1.0, 2.0, std::f64::consts::PI / 4.0));
720
721        let f = NamedTempFile::new()?;
722        ToroLoader::write(&graph, f.path())?;
723        let loaded = ToroLoader::load(f.path())?;
724
725        let v = &loaded.vertices_se2[&0];
726        assert!(
727            (v.theta() - std::f64::consts::PI / 4.0).abs() < 1e-10,
728            "theta not preserved"
729        );
730        Ok(())
731    }
732
733    #[test]
734    fn test_load_invalid_utf8_returns_err() {
735        let mut f = NamedTempFile::new().unwrap();
736        f.write_all(&[0xFF, 0xFE, 0x80, 0x00, 0xAB]).unwrap();
737        let result = ToroLoader::load(f.path());
738        assert!(result.is_err());
739    }
740
741    #[test]
742    fn test_write_to_nonexistent_dir_returns_err() {
743        let mut graph = Graph::new();
744        graph
745            .vertices_se2
746            .insert(0, VertexSE2::new(0, 0.0, 0.0, 0.0));
747        let dir = tempfile::tempdir().unwrap();
748        let path = dir.path().join("nested").join("deep").join("output.toro");
749        let result = ToroLoader::write(&graph, &path);
750        assert!(result.is_err());
751    }
752}
apex_io/toro.rs

apex_io/
toro.rs
