1use core::fmt;
4use std::collections::BTreeMap;
5
6use nalgebra::{Matrix3, Matrix4, Quaternion, UnitQuaternion, Vector3};
7use rust_robotics_core::Matrix6;
8
9use crate::pose_graph_optimization::{Edge2D, Pose2DNode};
10use crate::pose_graph_optimization_3d::{Edge3D, Pose3DNode};
11
12#[derive(Debug, Clone)]
14pub enum G2oGraph {
15 Se2 {
16 vertices: BTreeMap<usize, Pose2DNode>,
17 edges: Vec<Edge2D>,
18 },
19 Se3 {
20 vertices: BTreeMap<usize, Pose3DNode>,
21 edges: Vec<Edge3D>,
22 },
23}
24
25#[derive(Debug, Clone, PartialEq, Eq)]
27pub struct G2oError {
28 pub line: usize,
29 pub message: String,
30}
31
32#[derive(Debug, Clone, Copy, PartialEq, Eq)]
33enum Dimension {
34 Se2,
35 Se3,
36}
37
38impl fmt::Display for G2oError {
39 fn fmt(&self, formatter: &mut fmt::Formatter<'_>) -> fmt::Result {
40 write!(formatter, "g2o line {}: {}", self.line, self.message)
41 }
42}
43
44impl std::error::Error for G2oError {}
45
46pub fn parse_g2o(input: &str) -> Result<G2oGraph, G2oError> {
49 let mut dimension = None;
50 let mut vertices_2d = BTreeMap::new();
51 let mut edges_2d = Vec::new();
52 let mut vertices_3d = BTreeMap::new();
53 let mut edges_3d = Vec::new();
54
55 for (line_index, raw_line) in input.lines().enumerate() {
56 let line_number = line_index + 1;
57 let line = raw_line.trim();
58 if line.is_empty() || line.starts_with('#') {
59 continue;
60 }
61 let fields = line.split_whitespace().collect::<Vec<_>>();
62 match fields[0] {
63 "VERTEX_SE2" => {
64 require_dimension(&mut dimension, Dimension::Se2, line_number)?;
65 require_len(&fields, 5, line_number)?;
66 let id = parse_usize(fields[1], line_number)?;
67 vertices_2d.insert(
68 id,
69 Pose2DNode::new(
70 parse_f64(fields[2], line_number)?,
71 parse_f64(fields[3], line_number)?,
72 parse_f64(fields[4], line_number)?,
73 ),
74 );
75 }
76 "EDGE_SE2" => {
77 require_dimension(&mut dimension, Dimension::Se2, line_number)?;
78 require_len(&fields, 12, line_number)?;
79 let mut information = Matrix3::zeros();
80 fill_upper_triangle(&mut information, &fields[6..], line_number)?;
81 edges_2d.push(Edge2D {
82 from: parse_usize(fields[1], line_number)?,
83 to: parse_usize(fields[2], line_number)?,
84 measurement: Pose2DNode::new(
85 parse_f64(fields[3], line_number)?,
86 parse_f64(fields[4], line_number)?,
87 parse_f64(fields[5], line_number)?,
88 ),
89 information,
90 });
91 }
92 "VERTEX_SE3:QUAT" => {
93 require_dimension(&mut dimension, Dimension::Se3, line_number)?;
94 require_len(&fields, 9, line_number)?;
95 let id = parse_usize(fields[1], line_number)?;
96 vertices_3d.insert(id, pose_3d_from_fields(&fields[2..], line_number)?);
97 }
98 "EDGE_SE3:QUAT" => {
99 require_dimension(&mut dimension, Dimension::Se3, line_number)?;
100 require_len(&fields, 31, line_number)?;
101 let mut information = Matrix6::zeros();
102 fill_upper_triangle(&mut information, &fields[10..], line_number)?;
103 edges_3d.push(Edge3D {
104 from: parse_usize(fields[1], line_number)?,
105 to: parse_usize(fields[2], line_number)?,
106 measurement: pose_3d_from_fields(&fields[3..10], line_number)?,
107 information,
108 });
109 }
110 tag => {
111 return Err(G2oError {
112 line: line_number,
113 message: format!("unsupported record {tag}"),
114 });
115 }
116 }
117 }
118
119 match dimension {
120 Some(Dimension::Se2) => Ok(G2oGraph::Se2 {
121 vertices: vertices_2d,
122 edges: edges_2d,
123 }),
124 Some(Dimension::Se3) => Ok(G2oGraph::Se3 {
125 vertices: vertices_3d,
126 edges: edges_3d,
127 }),
128 None => Err(G2oError {
129 line: 0,
130 message: "input contains no supported records".into(),
131 }),
132 }
133}
134
135pub fn write_g2o(graph: &G2oGraph) -> String {
138 let mut output = String::new();
139 match graph {
140 G2oGraph::Se2 { vertices, edges } => {
141 for (id, pose) in vertices {
142 output.push_str(&format!(
143 "VERTEX_SE2 {id} {} {} {}\n",
144 pose.x, pose.y, pose.yaw
145 ));
146 }
147 for edge in edges {
148 output.push_str(&format!(
149 "EDGE_SE2 {} {} {} {} {}",
150 edge.from,
151 edge.to,
152 edge.measurement.x,
153 edge.measurement.y,
154 edge.measurement.yaw
155 ));
156 append_upper_triangle(&mut output, &edge.information);
157 output.push('\n');
158 }
159 }
160 G2oGraph::Se3 { vertices, edges } => {
161 for (id, pose) in vertices {
162 append_pose_3d(&mut output, "VERTEX_SE3:QUAT", &id.to_string(), pose);
163 }
164 for edge in edges {
165 append_pose_3d(
166 &mut output,
167 "EDGE_SE3:QUAT",
168 &format!("{} {}", edge.from, edge.to),
169 &edge.measurement,
170 );
171 output.pop();
172 append_upper_triangle(&mut output, &edge.information);
173 output.push('\n');
174 }
175 }
176 }
177 output
178}
179
180fn pose_3d_from_fields(fields: &[&str], line: usize) -> Result<Pose3DNode, G2oError> {
181 let translation = Vector3::new(
182 parse_f64(fields[0], line)?,
183 parse_f64(fields[1], line)?,
184 parse_f64(fields[2], line)?,
185 );
186 let quaternion = UnitQuaternion::new_normalize(Quaternion::new(
187 parse_f64(fields[6], line)?,
188 parse_f64(fields[3], line)?,
189 parse_f64(fields[4], line)?,
190 parse_f64(fields[5], line)?,
191 ));
192 let mut transform = Matrix4::identity();
193 transform
194 .fixed_view_mut::<3, 3>(0, 0)
195 .copy_from(quaternion.to_rotation_matrix().matrix());
196 transform
197 .fixed_view_mut::<3, 1>(0, 3)
198 .copy_from(&translation);
199 Ok(Pose3DNode { transform })
200}
201
202fn append_pose_3d(output: &mut String, tag: &str, ids: &str, pose: &Pose3DNode) {
203 let translation = pose.transform.fixed_view::<3, 1>(0, 3);
204 let rotation =
205 UnitQuaternion::from_matrix(&pose.transform.fixed_view::<3, 3>(0, 0).into_owned());
206 let quaternion = rotation.quaternion();
207 output.push_str(&format!(
208 "{tag} {ids} {} {} {} {} {} {} {}\n",
209 translation.x,
210 translation.y,
211 translation.z,
212 quaternion.i,
213 quaternion.j,
214 quaternion.k,
215 quaternion.w
216 ));
217}
218
219fn append_upper_triangle<const N: usize>(
220 output: &mut String,
221 matrix: &nalgebra::SMatrix<f64, N, N>,
222) {
223 for row in 0..N {
224 for column in row..N {
225 output.push_str(&format!(" {}", matrix[(row, column)]));
226 }
227 }
228}
229
230fn fill_upper_triangle<const N: usize>(
231 matrix: &mut nalgebra::SMatrix<f64, N, N>,
232 fields: &[&str],
233 line: usize,
234) -> Result<(), G2oError> {
235 let mut index = 0;
236 for row in 0..N {
237 for column in row..N {
238 let value = parse_f64(fields[index], line)?;
239 matrix[(row, column)] = value;
240 matrix[(column, row)] = value;
241 index += 1;
242 }
243 }
244 Ok(())
245}
246
247fn require_dimension(
248 current: &mut Option<Dimension>,
249 requested: Dimension,
250 line: usize,
251) -> Result<(), G2oError> {
252 if current.is_some_and(|value| value != requested) {
253 return Err(G2oError {
254 line,
255 message: "mixed SE(2) and SE(3) records are not supported".into(),
256 });
257 }
258 *current = Some(requested);
259 Ok(())
260}
261
262fn require_len(fields: &[&str], expected: usize, line: usize) -> Result<(), G2oError> {
263 if fields.len() == expected {
264 Ok(())
265 } else {
266 Err(G2oError {
267 line,
268 message: format!("expected {expected} fields, found {}", fields.len()),
269 })
270 }
271}
272
273fn parse_f64(field: &str, line: usize) -> Result<f64, G2oError> {
274 field.parse().map_err(|_| G2oError {
275 line,
276 message: format!("invalid number {field}"),
277 })
278}
279
280fn parse_usize(field: &str, line: usize) -> Result<usize, G2oError> {
281 field.parse().map_err(|_| G2oError {
282 line,
283 message: format!("invalid vertex id {field}"),
284 })
285}
286
287#[cfg(test)]
288mod tests {
289 use super::*;
290
291 #[test]
292 fn se2_round_trip_preserves_graph() {
293 let input = "\
294VERTEX_SE2 0 0 0 0
295VERTEX_SE2 1 1 0 0
296EDGE_SE2 0 1 1 0 0 10 0 0 10 0 20
297";
298 let graph = parse_g2o(input).unwrap();
299 let encoded = write_g2o(&graph);
300 let decoded = parse_g2o(&encoded).unwrap();
301 match decoded {
302 G2oGraph::Se2 { vertices, edges } => {
303 assert_eq!(vertices.len(), 2);
304 assert_eq!(edges.len(), 1);
305 assert_eq!(edges[0].information[(2, 2)], 20.0);
306 }
307 _ => panic!("expected SE(2) graph"),
308 }
309 }
310
311 #[test]
312 fn se3_quaternion_round_trip_preserves_pose() {
313 let input = "\
314VERTEX_SE3:QUAT 0 1 2 3 0 0 0 1
315VERTEX_SE3:QUAT 1 2 2 3 0 0 0 1
316EDGE_SE3:QUAT 0 1 1 0 0 0 0 0 1 1 0 0 0 0 0 1 0 0 0 0 1 0 0 0 1 0 0 1 0 1
317";
318 let graph = parse_g2o(input).unwrap();
319 let decoded = parse_g2o(&write_g2o(&graph)).unwrap();
320 match decoded {
321 G2oGraph::Se3 { vertices, edges } => {
322 assert_eq!(vertices.len(), 2);
323 assert_eq!(edges.len(), 1);
324 assert!((vertices[&0].transform[(2, 3)] - 3.0).abs() < 1.0e-12);
325 }
326 _ => panic!("expected SE(3) graph"),
327 }
328 }
329}