apex_solver/io/
toro.rs

1use crate::io::{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        let mmap = unsafe {
15            Mmap::map(&file).map_err(|e| {
16                IoError::Io(e)
17                    .log_with_source(format!("Failed to memory-map TORO file: {:?}", path_ref))
18            })?
19        };
20        let content = std::str::from_utf8(&mmap).map_err(|e| {
21            IoError::Parse {
22                line: 0,
23                message: format!("Invalid UTF-8: {e}"),
24            }
25            .log()
26        })?;
27
28        Self::parse_content(content)
29    }
30
31    fn write<P: AsRef<Path>>(graph: &Graph, path: P) -> Result<(), IoError> {
32        // TORO only supports SE2
33        if !graph.vertices_se3.is_empty() || !graph.edges_se3.is_empty() {
34            return Err(IoError::UnsupportedFormat(
35                "TORO format only supports SE2 (2D) graphs. Use G2O format for SE3 data."
36                    .to_string(),
37            )
38            .log());
39        }
40
41        let path_ref = path.as_ref();
42        let mut file = fs::File::create(path_ref).map_err(|e| {
43            IoError::Io(e).log_with_source(format!("Failed to create TORO file: {:?}", path_ref))
44        })?;
45
46        // Write SE2 vertices (sorted by ID)
47        let mut vertex_ids: Vec<_> = graph.vertices_se2.keys().collect();
48        vertex_ids.sort();
49
50        for id in vertex_ids {
51            let vertex = &graph.vertices_se2[id];
52            writeln!(
53                file,
54                "VERTEX2 {} {:.17e} {:.17e} {:.17e}",
55                vertex.id,
56                vertex.x(),
57                vertex.y(),
58                vertex.theta()
59            )
60            .map_err(|e| {
61                IoError::Io(e).log_with_source(format!("Failed to write TORO vertex {}", vertex.id))
62            })?;
63        }
64
65        // Write SE2 edges
66        // TORO format: EDGE2 <id1> <id2> <dx> <dy> <dtheta> <i11> <i12> <i22> <i33> <i13> <i23>
67        for edge in &graph.edges_se2 {
68            let meas = &edge.measurement;
69            let info = &edge.information;
70
71            writeln!(
72                file,
73                "EDGE2 {} {} {:.17e} {:.17e} {:.17e} {:.17e} {:.17e} {:.17e} {:.17e} {:.17e} {:.17e}",
74                edge.from,
75                edge.to,
76                meas.x(),
77                meas.y(),
78                meas.angle(),
79                info[(0, 0)], // i11
80                info[(0, 1)], // i12
81                info[(1, 1)], // i22
82                info[(2, 2)], // i33
83                info[(0, 2)], // i13
84                info[(1, 2)]  // i23
85            )
86            .map_err(|e| {
87                IoError::Io(e).log_with_source(format!(
88                    "Failed to write TORO edge {} -> {}",
89                    edge.from, edge.to
90                ))
91            })?;
92        }
93
94        Ok(())
95    }
96}
97
98impl ToroLoader {
99    fn parse_content(content: &str) -> Result<Graph, IoError> {
100        let lines: Vec<&str> = content.lines().collect();
101        let mut graph = Graph::new();
102
103        for (line_num, line) in lines.iter().enumerate() {
104            Self::parse_line(line, line_num + 1, &mut graph)?;
105        }
106
107        Ok(graph)
108    }
109
110    fn parse_line(line: &str, line_num: usize, graph: &mut Graph) -> Result<(), IoError> {
111        let line = line.trim();
112
113        // Skip empty lines and comments
114        if line.is_empty() || line.starts_with('#') {
115            return Ok(());
116        }
117
118        let parts: Vec<&str> = line.split_whitespace().collect();
119        if parts.is_empty() {
120            return Ok(());
121        }
122
123        match parts[0] {
124            "VERTEX2" => {
125                let vertex = Self::parse_vertex2(&parts, line_num)?;
126                let id = vertex.id;
127                if graph.vertices_se2.insert(id, vertex).is_some() {
128                    return Err(IoError::DuplicateVertex { id });
129                }
130            }
131            "EDGE2" => {
132                let edge = Self::parse_edge2(&parts, line_num)?;
133                graph.edges_se2.push(edge);
134            }
135            _ => {
136                // Skip unknown types silently for compatibility
137            }
138        }
139
140        Ok(())
141    }
142
143    fn parse_vertex2(parts: &[&str], line_num: usize) -> Result<VertexSE2, IoError> {
144        if parts.len() < 5 {
145            return Err(IoError::MissingFields { line: line_num });
146        }
147
148        let id = parts[1]
149            .parse::<usize>()
150            .map_err(|_| IoError::InvalidNumber {
151                line: line_num,
152                value: parts[1].to_string(),
153            })?;
154
155        let x = parts[2]
156            .parse::<f64>()
157            .map_err(|_| IoError::InvalidNumber {
158                line: line_num,
159                value: parts[2].to_string(),
160            })?;
161
162        let y = parts[3]
163            .parse::<f64>()
164            .map_err(|_| IoError::InvalidNumber {
165                line: line_num,
166                value: parts[3].to_string(),
167            })?;
168
169        let theta = parts[4]
170            .parse::<f64>()
171            .map_err(|_| IoError::InvalidNumber {
172                line: line_num,
173                value: parts[4].to_string(),
174            })?;
175
176        Ok(VertexSE2::new(id, x, y, theta))
177    }
178
179    fn parse_edge2(parts: &[&str], line_num: usize) -> Result<EdgeSE2, IoError> {
180        if parts.len() < 12 {
181            return Err(IoError::MissingFields { line: line_num });
182        }
183
184        let from = parts[1]
185            .parse::<usize>()
186            .map_err(|_| IoError::InvalidNumber {
187                line: line_num,
188                value: parts[1].to_string(),
189            })?;
190
191        let to = parts[2]
192            .parse::<usize>()
193            .map_err(|_| IoError::InvalidNumber {
194                line: line_num,
195                value: parts[2].to_string(),
196            })?;
197
198        // Parse measurement (dx, dy, dtheta)
199        let dx = parts[3]
200            .parse::<f64>()
201            .map_err(|_| IoError::InvalidNumber {
202                line: line_num,
203                value: parts[3].to_string(),
204            })?;
205        let dy = parts[4]
206            .parse::<f64>()
207            .map_err(|_| IoError::InvalidNumber {
208                line: line_num,
209                value: parts[4].to_string(),
210            })?;
211        let dtheta = parts[5]
212            .parse::<f64>()
213            .map_err(|_| IoError::InvalidNumber {
214                line: line_num,
215                value: parts[5].to_string(),
216            })?;
217
218        // Parse TORO information matrix (I11, I12, I22, I33, I13, I23)
219        let i11 = parts[6]
220            .parse::<f64>()
221            .map_err(|_| IoError::InvalidNumber {
222                line: line_num,
223                value: parts[6].to_string(),
224            })?;
225        let i12 = parts[7]
226            .parse::<f64>()
227            .map_err(|_| IoError::InvalidNumber {
228                line: line_num,
229                value: parts[7].to_string(),
230            })?;
231        let i22 = parts[8]
232            .parse::<f64>()
233            .map_err(|_| IoError::InvalidNumber {
234                line: line_num,
235                value: parts[8].to_string(),
236            })?;
237        let i33 = parts[9]
238            .parse::<f64>()
239            .map_err(|_| IoError::InvalidNumber {
240                line: line_num,
241                value: parts[9].to_string(),
242            })?;
243        let i13 = parts[10]
244            .parse::<f64>()
245            .map_err(|_| IoError::InvalidNumber {
246                line: line_num,
247                value: parts[10].to_string(),
248            })?;
249        let i23 = parts[11]
250            .parse::<f64>()
251            .map_err(|_| IoError::InvalidNumber {
252                line: line_num,
253                value: parts[11].to_string(),
254            })?;
255
256        let information = nalgebra::Matrix3::new(i11, i12, i13, i12, i22, i23, i13, i23, i33);
257
258        Ok(EdgeSE2::new(from, to, dx, dy, dtheta, information))
259    }
260}
apex_solver/io/toro.rs

apex_solver/io/
toro.rs
