apex_solver/io/
toro.rs

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

apex_solver/io/
toro.rs
