🦀 Apex Solver

A high-performance Rust-based nonlinear least squares optimization library designed for computer vision applications including bundle adjustment, SLAM, and pose graph optimization. Built with focus on zero-cost abstractions, memory safety, and mathematical correctness.

Apex Solver is a comprehensive optimization library that bridges the gap between theoretical robotics and practical implementation. It provides manifold-aware optimization for Lie groups commonly used in computer vision, multiple optimization algorithms with unified interfaces, flexible linear algebra backends supporting both sparse Cholesky and QR decompositions, and industry-standard file format support for seamless integration with existing workflows.

Key Features (v1.0.0)

• 📷 Bundle Adjustment with Camera Intrinsic Optimization: Simultaneous optimization of camera poses, 3D landmarks, and camera intrinsics (8 camera models: Pinhole, RadialTangential, FOV, UnifiedCamera, ExtendedUnified, DoubleSphere, KannalaBrandt, Orthographic)
• 🔧 Explicit & Implicit Schur Complement Solvers: Memory-efficient matrix-free PCG for large-scale problems (10,000+ cameras) alongside traditional explicit formulation
• 🛡️ 15 Robust Loss Functions: Comprehensive outlier rejection (Huber, Cauchy, Tukey, Welsch, Barron, and more)
• 📐 Manifold-Aware: Full Lie group support (SE2, SE3, SO2, SO3) with analytic Jacobians
• 🚀 Three Optimization Algorithms: Levenberg-Marquardt, Gauss-Newton, and Dog Leg with unified interface
• 📌 Prior Factors & Fixed Variables: Anchor poses with known values and constrain specific parameter indices
• 📊 Uncertainty Quantification: Covariance estimation for both Cholesky and QR solvers
• 🎨 Real-time Visualization: Integrated Rerun support for live debugging of optimization progress
• 📝 G2O I/O: Read and write G2O format files for seamless integration with SLAM ecosystems
• ⚡ High Performance: Sparse linear algebra with persistent symbolic factorization
• ✅ Production-Grade: Comprehensive error handling, structured tracing, integration test suite

🚀 Quick Start

use std::collections::HashMap;
use apex_solver::core::problem::Problem;
use apex_solver::factors::{BetweenFactor, PriorFactor};
use apex_solver::io::{G2oLoader, GraphLoader};
use apex_solver::linalg::LinearSolverType;
use apex_solver::manifold::ManifoldType;
use apex_solver::optimizer::levenberg_marquardt::{LevenbergMarquardt, LevenbergMarquardtConfig};
use nalgebra::dvector;

fn main() -> Result<(), Box<dyn std::error::Error>> {
    // Load pose graph from G2O file
    let graph = G2oLoader::load("data/odometry/sphere2500.g2o")?;
    
    // Create optimization problem
    let mut problem = Problem::new();
    let mut initial_values = HashMap::new();
    
    // Add SE3 poses as variables
    for (&id, vertex) in &graph.vertices_se3 {
        let var_name = format!("x{}", id);
        let quat = vertex.pose.rotation_quaternion();
        let trans = vertex.pose.translation();
        let se3_data = dvector![trans.x, trans.y, trans.z, quat.w, quat.i, quat.j, quat.k];
        initial_values.insert(var_name, (ManifoldType::SE3, se3_data));
    }
    
    // Add between factors (relative pose constraints)
    for edge in &graph.edges_se3 {
        let factor = BetweenFactor::new(edge.measurement.clone());
        problem.add_residual_block(
            &[&format!("x{}", edge.from), &format!("x{}", edge.to)],
            Box::new(factor),
            None,  // Optional: add HuberLoss for robustness
        );
    }
    
    // Configure and run optimizer
    let config = LevenbergMarquardtConfig::new()
        .with_linear_solver_type(LinearSolverType::SparseCholesky)
        .with_max_iterations(100)
        .with_cost_tolerance(1e-6)
        .with_compute_covariances(true);  // Enable uncertainty estimation
    
    let mut solver = LevenbergMarquardt::with_config(config);
    let result = solver.optimize(&problem, &initial_values)?;
    
    println!("Status: {:?}", result.status);
    println!("Initial cost: {:.3e}", result.initial_cost);
    println!("Final cost: {:.3e}", result.final_cost);
    println!("Iterations: {}", result.iterations);
    
    Ok(())
}

Result:

Status: CostToleranceReached
Initial cost: 1.280e+05
Final cost: 2.130e+01
Iterations: 5

🏗️ Architecture

The library is organized into five core modules:

apex-solver/
├── src/
│   ├── core/           # Problem formulation, factors, residuals
│   ├── factors/        # Factor implementations (projection, between, prior)
│   ├── optimizer/      # LM, GN, Dog Leg algorithms
│   ├── linalg/         # Cholesky, QR, Explicit/Implicit Schur
│   ├── manifold/       # SE2, SE3, SO2, SO3, Rn
│   ├── observers/      # Optimization observers and callbacks
│   └── io/             # G2O, TORO, TUM file formats
├── bin/                # Executable binaries
├── benches/            # Benchmarks (Rust + C++ comparisons)
├── examples/           # Example programs
├── tests/              # Integration tests
└── doc/                # Extended documentation

Core Modules:

• core/: Optimization problem definitions, residual blocks, robust loss functions, and variable management
• optimizer/: Three optimization algorithms (Levenberg-Marquardt with adaptive damping, Gauss-Newton, Dog Leg trust region) with real-time visualization support
• linalg/: Linear algebra backends including sparse Cholesky decomposition, QR factorization, explicit Schur complement, and implicit Schur complement (matrix-free PCG)
• manifold/: Lie group implementations (SE2/SE3 for rigid transformations, SO2/SO3 for rotations, Rn for Euclidean space) with analytic Jacobians
• io/: File format parsers for G2O, TORO, and TUM trajectory formats

📊 Performance Benchmarks

Hardware: Apple Mac Mini M4, 64GB RAM
Methodology: Average over multiple runs

Pose Graph Optimization

Performance comparison across 6 optimization libraries on standard pose graph datasets. All benchmarks use Levenberg-Marquardt algorithm with consistent parameters (max_iterations=100, cost_tolerance=1e-4).

Metrics: Wall-clock time (ms), iterations, initial/final cost, convergence status

2D Datasets (SE2)

3D Datasets (SE3)

Key Observations:

• apex-solver: 100% convergence rate (8/8 datasets), most reliable Rust solver
• Ceres/g2o: 100% convergence but often slower (especially g2o)
• GTSAM: Fast when it converges, but diverged on torus3D (87.5% rate)
• factrs: Fast on 2D but panics on large 3D problems (62.5% rate)
• tiny-solver: Convergence issues on several datasets (75% rate)

Bundle Adjustment (Self-Calibration)

Large-scale bundle adjustment benchmarks optimizing camera poses, 3D landmarks, and camera intrinsics simultaneously. Tests self-calibration capability on real-world structure-from-motion datasets from the Bundle Adjustment in the Large (BAL) collection.

Key Results:

• Apex-Iterative: 100% convergence rate (4/4 datasets), handles up to 5M observations efficiently
• Superior scalability: Only solver alongside g2o to complete Venice dataset; Ceres and GTSAM timeout after 10 minutes
• Best accuracy on largest dataset: Achieves 0.458 RMSE on Venice (5M observations) in only 2 iterations
• Speed advantage: 61x faster than Ceres on Dubrovnik, 4x faster on Trafalgar (where Ceres converged)

📊 Examples

Example 1: Basic Pose Graph Optimization

use std::collections::HashMap;
use apex_solver::core::problem::Problem;
use apex_solver::factors::BetweenFactor;
use apex_solver::io::{G2oLoader, GraphLoader};
use apex_solver::manifold::ManifoldType;
use apex_solver::optimizer::levenberg_marquardt::{LevenbergMarquardt, LevenbergMarquardtConfig};
use nalgebra::dvector;

// Load pose graph
let graph = G2oLoader::load("data/odometry/sphere2500.g2o")?;

// Build optimization problem
let mut problem = Problem::new();
let mut initial_values = HashMap::new();

// Add SE3 poses as variables
for (&id, vertex) in &graph.vertices_se3 {
    let quat = vertex.pose.rotation_quaternion();
    let trans = vertex.pose.translation();
    initial_values.insert(
        format!("x{}", id),
        (ManifoldType::SE3, dvector![trans.x, trans.y, trans.z, quat.w, quat.i, quat.j, quat.k])
    );
}

// Add between factors
for edge in &graph.edges_se3 {
    problem.add_residual_block(
        &[&format!("x{}", edge.from), &format!("x{}", edge.to)],
        Box::new(BetweenFactor::new(edge.measurement.clone())),
        None,
    );
}

// Configure and solve
let config = LevenbergMarquardtConfig::new()
    .with_max_iterations(100)
    .with_cost_tolerance(1e-6);

let mut solver = LevenbergMarquardt::with_config(config);
let result = solver.optimize(&problem, &initial_values)?;

println!("Optimized {} poses in {} iterations", 
    result.parameters.len(), result.iterations);

Example 2: Custom Factor Implementation

Create custom factors by implementing the Factor trait:

use apex_solver::factors::Factor;
use nalgebra::{DMatrix, DVector};

#[derive(Debug, Clone)]
pub struct MyRangeFactor {
    pub measurement: f64,
    pub information: f64,
}

impl Factor for MyRangeFactor {
    fn linearize(
        &self,
        params: &[DVector<f64>],
        compute_jacobian: bool,
    ) -> (DVector<f64>, Option<DMatrix<f64>>) {
        // Extract 2D point parameters [x, y]
        let x = params[0][0];
        let y = params[0][1];

        // Compute predicted measurement
        let predicted_distance = (x * x + y * y).sqrt();

        // Compute residual: measurement - prediction
        let residual = DVector::from_vec(vec![
            self.information.sqrt() * (self.measurement - predicted_distance)
        ]);

        // Compute analytic Jacobian
        let jacobian = if compute_jacobian {
            if predicted_distance > 1e-8 {
                let scale = -self.information.sqrt() / predicted_distance;
                Some(DMatrix::from_row_slice(1, 2, &[scale * x, scale * y]))
            } else {
                Some(DMatrix::zeros(1, 2))
            }
        } else {
            None
        };

        (residual, jacobian)
    }

    fn get_dimension(&self) -> usize {
        1  // One scalar residual
    }
}

// Use in optimization
problem.add_residual_block(
    &["point"],
    Box::new(MyRangeFactor { measurement: 5.0, information: 1.0 }),
    None
);

Example 3: Self-Calibration Bundle Adjustment

Optimize camera poses, 3D landmarks, AND camera intrinsics simultaneously using ProjectionFactor<CameraModel, OptConfig>:

use std::collections::HashMap;
use apex_solver::core::problem::Problem;
use apex_solver::factors::ProjectionFactor;
use apex_solver::factors::camera::{BALPinholeCameraStrict, SelfCalibration};
use apex_solver::linalg::LinearSolverType;
use apex_solver::manifold::ManifoldType;
use apex_solver::manifold::se3::SE3;
use apex_solver::optimizer::levenberg_marquardt::{LevenbergMarquardt, LevenbergMarquardtConfig};
use nalgebra::{DVector, Matrix2xX, Vector2};

fn main() -> Result<(), Box<dyn std::error::Error>> {
    let mut problem = Problem::new();
    let mut initial_values = HashMap::new();

    // Add camera poses (SE3) and per-camera intrinsics
    for (i, cam) in cameras.iter().enumerate() {
        // SE3 pose: [tx, ty, tz, qw, qi, qj, qk]
        let pose = SE3::from_translation_so3(cam.translation, cam.rotation);
        initial_values.insert(
            format!("pose_{:04}", i),
            (ManifoldType::SE3, DVector::from(pose))
        );
        
        // Per-camera intrinsics: [focal, k1, k2]
        initial_values.insert(
            format!("intr_{:04}", i),
            (ManifoldType::RN, DVector::from_vec(vec![cam.focal, cam.k1, cam.k2]))
        );
    }

    // Add 3D landmarks
    for (j, point) in landmarks.iter().enumerate() {
        initial_values.insert(
            format!("pt_{:05}", j),
            (ManifoldType::RN, DVector::from_vec(vec![point.x, point.y, point.z]))
        );
    }

    // Add projection factors with SelfCalibration optimization type
    for obs in &observations {
        let camera = BALPinholeCameraStrict::new(obs.focal, obs.k1, obs.k2);
        let measurements = Matrix2xX::from_columns(&[Vector2::new(obs.u, obs.v)]);
        
        // ProjectionFactor<CameraModel, OptConfig> with compile-time optimization config
        let factor: ProjectionFactor<BALPinholeCameraStrict, SelfCalibration> =
            ProjectionFactor::new(measurements, camera);
        
        problem.add_residual_block(
            &[&format!("pose_{:04}", obs.cam_id), 
              &format!("pt_{:05}", obs.pt_id),
              &format!("intr_{:04}", obs.cam_id)],
            Box::new(factor),
            None,  // Or Some(Box::new(HuberLoss::new(1.0)?)) for robustness
        );
    }

    // Fix first camera for gauge freedom
    for dof in 0..6 {
        problem.fix_variable("pose_0000", dof);
    }

    // Configure with Schur complement solver (best for BA)
    let config = LevenbergMarquardtConfig::for_bundle_adjustment()
        .with_max_iterations(50)
        .with_cost_tolerance(1e-6);

    let mut solver = LevenbergMarquardt::with_config(config);
    let result = solver.optimize(&problem, &initial_values)?;

    println!("Status: {:?}", result.status);
    println!("Final RMSE: {:.3} pixels", (result.final_cost / observations.len() as f64).sqrt());

    Ok(())
}

Optimization Types (compile-time configuration):

• SelfCalibration: Optimize pose + landmarks + intrinsics (shown above)
• BundleAdjustment: Optimize pose + landmarks (fixed intrinsics)
• OnlyPose: Visual odometry with fixed landmarks and intrinsics
• OnlyLandmarks: Triangulation with known poses
• OnlyIntrinsics: Camera calibration with known structure

🧮 Technical Implementation

Manifold Operations

Apex Solver implements mathematically rigorous Lie group operations following the manif library conventions. All manifold types provide plus() (retraction), minus() (inverse retraction), compose() (group composition), inverse() (group inverse), and analytic Jacobians for efficient optimization.

Supported Manifolds:

Camera Projection Models

Apex Solver supports 11 camera projection models for bundle adjustment with analytic Jacobians:

All models support compile-time optimization configuration (pose-only, landmark-only, intrinsics-only, or any combination).

Robust Loss Functions

15 robust loss functions for handling outliers in optimization:

• L2Loss: Standard least squares (no outliers)
• L1Loss: Linear growth (light outliers)
• HuberLoss: Quadratic near zero, linear after threshold (moderate outliers)
• CauchyLoss: Logarithmic growth (heavy outliers)
• FairLoss, GemanMcClureLoss, WelschLoss, TukeyBiweightLoss, AndrewsWaveLoss: Various robustness profiles
• RamsayEaLoss: Asymmetric outliers
• TrimmedMeanLoss: Ignores worst residuals
• LpNormLoss: Generalized Lp norm
• BarronGeneralLoss, AdaptiveBarronLoss: Adaptive robustness
• TDistributionLoss: Statistical outliers

Usage:

use apex_solver::core::loss_functions::HuberLoss;

let loss = HuberLoss::new(1.345);  // 95% efficiency threshold
problem.add_residual_block(Box::new(factor), Some(Box::new(loss)));

Optimization Algorithms

Levenberg-Marquardt (Recommended)

• Adaptive damping between gradient descent and Gauss-Newton
• Robust convergence from poor initial estimates
• Supports covariance estimation for uncertainty quantification
• 9 comprehensive termination criteria (gradient norm, cost change, trust region radius, etc.)

Gauss-Newton

• Fast convergence near solution
• Minimal memory requirements
• Best for well-initialized problems

Dog Leg Trust Region

• Combines steepest descent and Gauss-Newton
• Global convergence guarantees
• Adaptive trust region management

Linear Algebra Backends

Four sparse linear solvers for different use cases:

• Sparse Cholesky: Direct factorization of J^T J + λI - fast, moderate memory, best for well-conditioned problems
• Sparse QR: QR factorization of Jacobian - robust for rank-deficient systems, slightly slower
• Explicit Schur Complement: Constructs reduced camera matrix S = B - E C⁻¹ Eᵀ explicitly in memory - most accurate for bundle adjustment, moderate memory usage
• Implicit Schur Complement: Matrix-free PCG solver computing only S·x products - memory-efficient for large-scale problems (10,000+ cameras), highly scalable

Configure via LinearSolverType in optimizer config:

config.with_linear_solver_type(LinearSolverType::ExplicitSchur)  // For bundle adjustment
config.with_linear_solver_type(LinearSolverType::ImplicitSchur)  // For very large BA

🎨 Interactive Visualization

Real-time optimization debugging with integrated Rerun visualization using the observer pattern:

use apex_solver::optimizer::levenberg_marquardt::{LevenbergMarquardt, LevenbergMarquardtConfig};

let config = LevenbergMarquardtConfig::new()
    .with_max_iterations(100);

let mut solver = LevenbergMarquardt::with_config(config);

// Add Rerun visualization observer (requires `visualization` feature)
#[cfg(feature = "visualization")]
{
    use apex_solver::observers::RerunObserver;
    solver.add_observer(RerunObserver::new(true)?);  // true = spawn viewer
}

let result = solver.optimize(&problem, &initial_values)?;

Visualized Metrics:

• Time series: Cost, gradient norm, damping (λ), step quality (ρ), step norm
• Matrix visualizations: Hessian heat map, gradient vector
• 3D poses: SE3 camera frusta, SE2 2D points

Run Examples:

# Enable visualization feature and run
cargo run --release --bin optimize_3d_graph -- --dataset sphere2500 --with-visualizer
cargo run --release --bin optimize_2d_graph -- --dataset intel --with-visualizer

Zero overhead when disabled (feature-gated).

🧠 Learning Resources

Computer Vision Background

• Multiple View Geometry (Hartley & Zisserman) - Mathematical foundations
• Visual SLAM algorithms (Durrant-Whyte & Bailey) - Probabilistic robotics
• g2o documentation - Reference C++ implementation

Lie Group Theory

• A micro Lie theory (Solà et al.) - Practical introduction
• manif library - C++ reference we follow
• State Estimation for Robotics (Barfoot) - SO(3) and SE(3)

Optimization Theory

• Numerical Optimization (Nocedal & Wright) - Standard reference
• Trust Region Methods - Dog Leg theory
• Ceres Solver Tutorial - Practical guide

🙏 Acknowledgments

Apex Solver draws inspiration and reference implementations from:

• Ceres Solver - Google's C++ optimization library
• g2o - General framework for graph optimization
• GTSAM - Georgia Tech Smoothing and Mapping library
• tiny-solver - Lightweight nonlinear least squares solver
• factrs - Rust factor graph optimization library
• faer - High-performance linear algebra library for Rust
• manif - C++ Lie theory library (for manifold conventions)
• nalgebra - Geometry and linear algebra primitives

📜 License

Licensed under the Apache License, Version 2.0. See LICENSE for details.