Skip to main content

rust_robotics_slam/
g2o.rs

1//! Minimal g2o SE(2)/SE(3) pose-graph text I/O.
2
3use 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/// A supported g2o pose graph.
13#[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/// Error returned for malformed or mixed-dimension g2o input.
26#[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
46/// Parses `VERTEX_SE2`, `EDGE_SE2`, `VERTEX_SE3:QUAT`, and
47/// `EDGE_SE3:QUAT` records.
48pub 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
135/// Serializes a supported graph using standard upper-triangular information
136/// matrix ordering.
137pub 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}