apex_io/
toro.rs

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

apex_io/
toro.rs
