apex-io

High-performance file I/O for pose graphs (G2O, TORO) and bundle adjustment (BAL) with SE2/SE3 support.

Overview

This library provides parsers and writers for common SLAM (Simultaneous Localization and Mapping) and Structure-from-Motion file formats. It enables easy loading and saving of pose graph optimization problems with:

Memory-mapped file reading for fast I/O on large files
Parallel parsing with rayon for multi-core acceleration
Type-safe graph representation with separate SE2/SE3 types
Comprehensive error handling with line numbers and context
Optional visualization support via Rerun integration

Target applications include:

Pose graph optimization (2D and 3D SLAM)
Bundle adjustment for Structure-from-Motion
Visual odometry and localization
Robotics trajectory optimization

Supported Formats

Format	Description	Vertex Types	Edge Types	Read	Write
G2O	General Graph Optimization	SE2, SE3	SE2, SE3	✓	✓
TORO	Tree-based netwORk Optimizer	SE2 only	SE2 only	✓	✓
BAL	Bundle Adjustment in the Large	Cameras, Points	Observations	✓	-

G2O Format

The G2O (General Graph Optimization) format is widely used in robotics for pose graph optimization. It supports both 2D (SE2) and 3D (SE3) graphs.

File Structure:

# Comments start with #
VERTEX_SE2 id x y theta
VERTEX_SE3:QUAT id x y z qx qy qz qw
EDGE_SE2 from to dx dy dtheta info_xx info_xy info_yy info_xt info_yt info_tt
EDGE_SE3:QUAT from to dx dy dz dqx dqy dqz dqw [21 information matrix values]

Example (2D):

VERTEX_SE2 0 0.0 0.0 0.0
VERTEX_SE2 1 1.0 0.0 0.0
EDGE_SE2 0 1 1.0 0.0 0.0 500 0 500 0 0 500

TORO Format

The TORO (Tree-based netwORk Optimizer) format is a legacy 2D SLAM format.

File Structure:

VERTEX2 id x y theta
EDGE2 from to dx dy dtheta info_xx info_xy info_yy info_xt info_yt info_tt

Note: TORO only supports SE2 (2D) graphs. Attempting to write SE3 data will result in an error.

BAL Format

The BAL (Bundle Adjustment in the Large) format is used for large-scale bundle adjustment benchmarks in computer vision.

File Structure:

num_cameras num_points num_observations
# Observations block (one per line)
camera_idx point_idx pixel_x pixel_y
...
# Cameras block (9 parameters per camera, one per line)
rotation_x
rotation_y
rotation_z
translation_x
translation_y
translation_z
focal_length
k1
k2
...
# Points block (3 coordinates per point, one per line)
x
y
z
...

Camera Model: Snavely's 9-parameter model from Bundler:

3 parameters: Axis-angle rotation (rx, ry, rz)
3 parameters: Translation (tx, ty, tz)
1 parameter: Focal length (f)
2 parameters: Radial distortion (k1, k2)

Installation

[dependencies]
apex-io = "0.1.0"

For visualization features (Rerun integration):

[dependencies]
apex-io = { version = "0.1.0", features = ["visualization"] }

Data Structures

Graph

The main container for pose graph data:

pub struct Graph {
    pub vertices_se2: HashMap<usize, VertexSE2>,
    pub vertices_se3: HashMap<usize, VertexSE3>,
    pub edges_se2: Vec<EdgeSE2>,
    pub edges_se3: Vec<EdgeSE3>,
}

impl Graph {
    pub fn new() -> Self;
    pub fn vertex_count(&self) -> usize;  // Total SE2 + SE3 vertices
    pub fn edge_count(&self) -> usize;    // Total SE2 + SE3 edges
}

SE2 Types (2D)

VertexSE2 - 2D pose (position + orientation):

pub struct VertexSE2 {
    id: usize,
    pose: SE2,  // From apex-manifolds
}

impl VertexSE2 {
    pub fn new(id: usize, pose: SE2) -> Self;
    pub fn from_vector(id: usize, data: &[f64]) -> Self;
    pub fn id(&self) -> usize;
    pub fn x(&self) -> f64;
    pub fn y(&self) -> f64;
    pub fn theta(&self) -> f64;
}

EdgeSE2 - 2D constraint between vertices:

pub struct EdgeSE2 {
    pub from: usize,
    pub to: usize,
    pub measurement: SE2,
    pub information: Matrix3<f64>,  // 3x3 information matrix
}

SE3 Types (3D)

VertexSE3 - 3D pose (position + orientation):

pub struct VertexSE3 {
    id: usize,
    pose: SE3,  // From apex-manifolds
}

impl VertexSE3 {
    pub fn new(id: usize, pose: SE3) -> Self;
    pub fn from_vector(id: usize, data: &[f64]) -> Self;
    pub fn from_translation_quaternion(id: usize, t: Vector3, q: UnitQuaternion) -> Self;
    pub fn id(&self) -> usize;
    pub fn translation(&self) -> Vector3<f64>;
    pub fn rotation(&self) -> UnitQuaternion<f64>;
    pub fn x(&self) -> f64;
    pub fn y(&self) -> f64;
    pub fn z(&self) -> f64;
}

EdgeSE3 - 3D constraint between vertices:

pub struct EdgeSE3 {
    pub from: usize,
    pub to: usize,
    pub measurement: SE3,
    pub information: Matrix6<f64>,  // 6x6 information matrix
}

BAL Types

BalDataset - Complete bundle adjustment problem:

pub struct BalDataset {
    pub cameras: Vec<BalCamera>,
    pub points: Vec<BalPoint>,
    pub observations: Vec<BalObservation>,
}

BalCamera - Snavely's 9-parameter camera model:

pub struct BalCamera {
    pub rotation: Vector3<f64>,     // Axis-angle (rx, ry, rz)
    pub translation: Vector3<f64>,  // (tx, ty, tz)
    pub focal_length: f64,
    pub k1: f64,                    // Radial distortion coefficient
    pub k2: f64,
}

impl BalCamera {
    pub fn clamped_focal_length(&self, min: f64, max: f64) -> f64;
}

BalPoint - 3D landmark:

pub struct BalPoint {
    pub position: Vector3<f64>,
}

BalObservation - 2D image observation:

pub struct BalObservation {
    pub camera_index: usize,
    pub point_index: usize,
    pub x: f64,  // Pixel x-coordinate
    pub y: f64,  // Pixel y-coordinate
}

API Reference

GraphLoader Trait

All pose graph loaders implement this trait:

pub trait GraphLoader {
    fn load<P: AsRef<Path>>(path: P) -> Result<Graph, IoError>;
    fn write<P: AsRef<Path>>(graph: &Graph, path: P) -> Result<(), IoError>;
}

Loaders

Loader	Formats	Description
`G2oLoader`	`.g2o`	G2O format with SE2/SE3 support
`ToroLoader`	`.graph`	TORO format (SE2 only)
`BalLoader`	`.txt`	BAL bundle adjustment format

Convenience Functions

/// Auto-detect format from file extension
pub fn load_graph<P: AsRef<Path>>(path: P) -> Result<Graph, IoError>;

Supported extensions:

.g2o → G2oLoader
.graph → ToroLoader

Error Handling

The IoError enum provides detailed error information:

Variant	Description
`Io(io::Error)`	Underlying I/O error
`Parse { line, message }`	Parse error with line number
`UnsupportedVertexType(String)`	Unknown vertex type in file
`UnsupportedEdgeType(String)`	Unknown edge type in file
`InvalidNumber { line, value }`	Failed to parse number
`MissingFields { line }`	Insufficient fields on line
`DuplicateVertex { id }`	Vertex ID already exists
`InvalidQuaternion { line, norm }`	Quaternion not unit length
`UnsupportedFormat(String)`	File extension not recognized
`FileCreationFailed { path, reason }`	Could not create output file

All errors include context via .log() and .log_with_source() methods for tracing integration.

Usage Examples

Loading a G2O File

use apex_io::{G2oLoader, GraphLoader};

let graph = G2oLoader::load("data/sphere2500.g2o")?;
println!("Loaded {} vertices and {} edges", 
    graph.vertex_count(), graph.edge_count());

Auto-detect Format

use apex_io::load_graph;

// Automatically detects format from extension
let graph = load_graph("data/M3500.g2o")?;

Working with SE3 Vertices and Edges

use apex_io::{G2oLoader, GraphLoader};

let graph = G2oLoader::load("sphere2500.g2o")?;

// Iterate over SE3 vertices
for (id, vertex) in &graph.vertices_se3 {
    println!("Vertex {}: position ({:.2}, {:.2}, {:.2})", 
        id, vertex.x(), vertex.y(), vertex.z());
}

// Iterate over SE3 edges
for edge in &graph.edges_se3 {
    println!("Edge {} -> {}", edge.from, edge.to);
}

Working with SE2 Vertices and Edges

use apex_io::{G2oLoader, GraphLoader};

let graph = G2oLoader::load("intel.g2o")?;

// Iterate over SE2 vertices
for (id, vertex) in &graph.vertices_se2 {
    println!("Vertex {}: ({:.2}, {:.2}, {:.2} rad)", 
        id, vertex.x(), vertex.y(), vertex.theta());
}

// Iterate over SE2 edges
for edge in &graph.edges_se2 {
    println!("Edge {} -> {}: measurement = ({:.2}, {:.2}, {:.2})",
        edge.from, edge.to,
        edge.measurement.x(), edge.measurement.y(), edge.measurement.theta());
}

Writing a Graph

use apex_io::{G2oLoader, GraphLoader};

let graph = G2oLoader::load("input.g2o")?;

// Write to G2O format
G2oLoader::write(&graph, "output.g2o")?;

// Write to TORO format (SE2 only)
ToroLoader::write(&graph, "output.graph")?;

Loading BAL Dataset

use apex_io::BalLoader;

let dataset = BalLoader::load("problem-1778-993923-pre.txt")?;
println!("Loaded {} cameras, {} points, {} observations",
    dataset.cameras.len(),
    dataset.points.len(),
    dataset.observations.len());

// Access camera parameters
for (i, cam) in dataset.cameras.iter().enumerate() {
    println!("Camera {}: focal={:.1}, k1={:.4}, k2={:.4}",
        i, cam.focal_length, cam.k1, cam.k2);
}

// Access observations
for obs in &dataset.observations {
    println!("Camera {} sees point {} at pixel ({:.1}, {:.1})",
        obs.camera_index, obs.point_index, obs.x, obs.y);
}

Performance

Memory Mapping

All loaders use memory-mapped file I/O via memmap2 for efficient reading of large files. This avoids loading the entire file into memory and leverages the operating system's page cache.

Parallel Parsing

For files with more than 1000 lines, parsing is automatically parallelized using rayon. This significantly speeds up loading of large pose graphs and BAL datasets.

Pre-allocation

Collections are pre-allocated based on estimated file size to minimize memory reallocations during parsing.

Visualization Feature

Enable the visualization feature for Rerun integration:

apex-io = { version = "0.1.0", features = ["visualization"] }

This adds helper methods for converting vertices to Rerun types:

// SE2 vertices
let pos_2d: [f32; 2] = vertex_se2.to_rerun_position_2d(scale);
let pos_3d: Vec3 = vertex_se2.to_rerun_position_3d(scale, height);

// SE3 vertices
let (position, rotation): (Vec3, Quat) = vertex_se3.to_rerun_transform(scale);

Dependencies

Dependency	Purpose
`apex-manifolds`	Lie group types (SE2, SE3)
`nalgebra`	Linear algebra (vectors, matrices)
`memmap2`	Memory-mapped file I/O
`rayon`	Parallel parsing
`thiserror`	Error handling
`tracing`	Structured logging
`serde`, `serde_json`	Serialization support
`chrono`	Timestamps in file headers
`rerun`	Visualization (optional)

References

License

Apache-2.0

apex-io 0.1.0

apex-io

Overview

Supported Formats

G2O Format

TORO Format

BAL Format

Installation

Data Structures

Graph

SE2 Types (2D)

SE3 Types (3D)

BAL Types

API Reference

GraphLoader Trait

Loaders

Convenience Functions

Error Handling

Usage Examples

Loading a G2O File

Auto-detect Format

Working with SE3 Vertices and Edges

Working with SE2 Vertices and Edges

Writing a Graph

Loading BAL Dataset

Performance

Memory Mapping

Parallel Parsing

Pre-allocation

Visualization Feature

Dependencies

References

License