apex_solver/io/
toro.rs

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

apex_solver/io/
toro.rs
