RustRobotics

July 31, 2026 · View on GitHub

CI codecov crates.io docs.rs Docs License: MIT

RustRobotics is a library-first Rust workspace for robotics algorithms, inspired by PythonRobotics and MathematicalRobotics, and extended with benchmarks, ROS2/Gazebo demos, and a visual showcase.

Run in your browser · Use the Rust crate · Run on a microcontroller · Browse the gallery

RRT tree growth
RRT		 Dynamic Window Approach
Dynamic Window Approach		 Dijkstra motion planner
Dijkstra
EKF localization
EKF Localization		 Particle filter localization
Particle Filter		 Pure pursuit path tracking
Pure Pursuit
EKF SLAM
EKF SLAM		 FastSLAM 1.0
FastSLAM 1.0		 ICP scan matching
ICP Matching
Block-sparse factor graph optimization
Block-sparse Factor Graph Optimization

Every animation above is rendered by the library itself — regenerate them all with ./scripts/generate_gallery_gifs.sh (pure Rust, no gnuplot or system packages).

Why This Repo Is Useful

  • 100+ robotics algorithms across planning, localization, mapping, SLAM, control, aerial navigation, arm motion, and mission behavior.
  • Runnable Rust examples, from headless CI-friendly demos to visualization examples and a ROS2 TurtleBot3 Gazebo navigation stack.
  • Measured comparisons, including Rust vs Python speed checks and MovingAI planner benchmarks with reproducible commands.
  • no_std Kalman filters — the localization stack cross-compiles for bare-metal Cortex-M microcontrollers, something a Python algorithm collection cannot do.
  • Lie groups and factor graphs — reusable SO(2)/SE(2)/SO(3)/SE(3), robust nonlinear least squares, pose graphs, IMU preintegration, and bundle adjustment share one tested mathematical foundation.
  • Real dataset ingestion — standard EuRoC MAV and KITTI odometry layouts, plus lever-arm-corrected IMU input and a connected IMU → bundle-adjustment → state/bias refinement → SE(3) replay pipeline.

Embedded / no_std

The core types and the Kalman-family localizers build without the standard library, so the same EKF you simulate on a workstation runs on a $5 microcontroller (Raspberry Pi Pico 2, STM32, ESP32-C3, ...):

rustup target add thumbv7em-none-eabihf
cargo build -p rust_robotics_core -p rust_robotics_localization \
  --no-default-features --target thumbv7em-none-eabihf

Available in no_std mode (with alloc): EKF, Iterated EKF, UKF, Cubature KF, Square-Root UKF, Information Filter, Complementary Filter, Histogram Filter, and the EKF/CKF Adaptive Filter. Math is routed through pure-Rust libm, no FPU or OS required. The sampling-based localizers (particle filter, MCL, ensemble KF) stay behind the default std feature because they need an entropy source. This is verified on every commit by the embedded-check CI job.

Open the visual gallery: https://rsasaki0109.github.io/rust_robotics/

Quick Start

Try the interactive planners, localization, SLAM, multi-agent formation, and path-tracking controller comparison with no installation:

https://rsasaki0109.github.io/rust_robotics/playground/

Add the latest published umbrella crate:

cargo add rust_robotics --features planning

Then run a headless planner demo from this repository with no GUI dependencies:

git clone https://github.com/rsasaki0109/rust_robotics.git
cd rust_robotics
cargo run -p rust_robotics --example headless_grid_planners --features planning

Interactive grid-planner playground (native egui):

cargo run -p rust_robotics_playground

Open the Localization tab for Particle Filter / EKF driving with arrow keys. Open SLAM to scrub EKF-SLAM / FastSLAM / ICP timelines, or ADMM Formation for the multi-agent horizon-consensus demo. Controller Arena replays Pure Pursuit, Stanley, and LQR Steer under an identical path, initial state, clock, and actuation model, with shareable speed and turn-response settings.

Controller Arena comparing Pure Pursuit, Stanley, and LQR Steer

Build and test the complete workspace:

cargo build --workspace --lib --tests --examples
cargo test --workspace --lib --tests

Use As A Library

Add the latest published umbrella crate from crates.io:

cargo add rust_robotics --features "planning,localization,control"

If you need changes that have not reached crates.io yet:

[dependencies]
rust_robotics = { git = "https://github.com/rsasaki0109/rust_robotics" }

Import the domain crate you need:

use rust_robotics::planning::{AStarConfig, AStarPlanner, DWAPlanner};
use rust_robotics::localization::{EKFConfig, EKFLocalizer};
use rust_robotics::control::{PurePursuitController, StanleyController};

Lie-group and optimization primitives are always available from the umbrella crate:

use rust_robotics::core::{se3_exp, se3_log, Vector6};
use rust_robotics::optimization::{RobustKernel, SolverConfig};

let tangent = Vector6::new(1.0, 0.0, 0.0, 0.1, -0.2, 0.3);
let transform = se3_exp(&tangent);
assert!((se3_log(&transform) - tangent).norm() < 1.0e-9);

let _config = SolverConfig::default();
let _loss = RobustKernel::Huber { delta: 1.0 };

What Is Inside

crates/
├── rust_robotics_core/          — Core types, traits, errors, Lie groups
├── rust_robotics_optimization/  — Factor graphs, robust losses, Gauss-Newton/LM
├── rust_robotics_planning/      — Path planning (A*, DWA, RRT, PRM, etc.)
├── rust_robotics_localization/  — Localization (EKF, UKF, PF, Histogram)
├── rust_robotics_control/       — Control & path tracking (Pure Pursuit, LQR, MPC, etc.)
├── rust_robotics_mapping/       — Mapping (NDT, Gaussian Grid, IMLS)
├── rust_robotics_slam/          — SLAM, pose graphs, IMU, BA, geometric ICP
├── rust_robotics_viz/           — Visualization (gnuplot wrapper)
├── ros2_nodes/                  — ROS2 navigation nodes (safe_drive-based)
└── rust_robotics/               — Umbrella crate (feature-gated re-exports)

More Examples

# Regenerate every animated GIF in the gallery (pure Rust, no system deps)
./scripts/generate_gallery_gifs.sh
Show all example commands (headless, benchmarks, SVG/GIF renderers, visualization) 
# Animated GIF renderers (media/gallery/, pure Rust)
cargo run -p rust_robotics --example render_gif_ekf_localization --features "localization,gif"
cargo run -p rust_robotics --example render_gif_particle_filter --features "localization,gif"
cargo run -p rust_robotics --example render_gif_pure_pursuit --features "control,gif"
cargo run -p rust_robotics --example render_gif_dwa --features "planning,gif"
cargo run -p rust_robotics --example render_gif_rrt --features "planning,gif"
cargo run -p rust_robotics --example render_gif_slam --features "slam,gif"
cargo run -p rust_robotics --example render_factor_graph_optimization --features "slam,gif"

# Headless (no GUI dependencies)
cargo run -p rust_robotics --example headless_grid_planners --features planning
cargo run -p rust_robotics --example headless_conformal_sipp --no-default-features --features planning
cargo run -p rust_robotics --example headless_traversal_risk_graph --no-default-features --features planning
cargo run -p rust_robotics --example headless_elevation_risk_graph --no-default-features --features planning
cargo run -p rust_robotics --example headless_risk_map_smoothing --no-default-features --features planning
cargo run -p rust_robotics --example headless_clearance_risk_graph --no-default-features --features planning
cargo run -p rust_robotics --example headless_adaptive_costmap_namo --no-default-features --features planning
cargo run -p rust_robotics --example render_traversal_risk_graph_svg --no-default-features --features planning
cargo run -p rust_robotics --example headless_localizers --features localization
cargo run -p rust_robotics --example headless_mppi_double_integrator --no-default-features --features control
cargo run -p rust_robotics --example headless_mppi_constraint_discount --no-default-features --features control
cargo run -p rust_robotics --example benchmark_mppi_unified --no-default-features --features control
cargo run -p rust_robotics --example headless_mppi_terminal_value --no-default-features --features control
cargo run -p rust_robotics --example headless_mppi_value_learning --no-default-features --features control
cargo run -p rust_robotics --example headless_mppi_replay_value_learning --no-default-features --features control
cargo run -p rust_robotics --example render_mppi_value_grid_svg --no-default-features --features control
cargo run -p rust_robotics --example headless_mppi_adaptive_temperature --no-default-features --features control
cargo run -p rust_robotics --example headless_mppi_track_progress --no-default-features --features control
cargo run -p rust_robotics --example render_mppi_track_progress_svg --no-default-features --features control
cargo run -p rust_robotics --example headless_mppi_racing_gate_progress --no-default-features --features control
cargo run -p rust_robotics --example render_mppi_racing_gate_progress_svg --no-default-features --features control
cargo run -p rust_robotics --example benchmark_racing_mppi_3d --no-default-features --features control
cargo run -p rust_robotics --example benchmark_racing_quadrotor --no-default-features --features control
cargo run -p rust_robotics --example benchmark_cbf_safety_filter --no-default-features --features control
cargo run -p rust_robotics --example benchmark_racing_motor --no-default-features --features control
cargo run -p rust_robotics --example benchmark_racing_powertrain --no-default-features --features control
cargo run -p rust_robotics --example benchmark_racing_powertrain_aware --no-default-features --features control
cargo run -p rust_robotics --example benchmark_racing_powertrain_budget --no-default-features --features control
cargo run -p rust_robotics --example benchmark_racing_powertrain_recovery --no-default-features --features control
cargo run -p rust_robotics --example benchmark_racing_powertrain_endurance --no-default-features --features control
cargo run -p rust_robotics --example benchmark_pusher_slider --no-default-features --features control
cargo run -p rust_robotics --example benchmark_pusher_slider_multi --no-default-features --features control
cargo run -p rust_robotics --example benchmark_pusher_slider_two_contact --no-default-features --features control
cargo run -p rust_robotics --example benchmark_admm_formation --no-default-features --features control
cargo run -p rust_robotics --example benchmark_admm_graph_consensus --no-default-features --features control
cargo run -p rust_robotics --example benchmark_admm_horizon_consensus --no-default-features --features control
cargo run -p rust_robotics --example headless_adap_rpf_mppi --no-default-features --features control
cargo run -p rust_robotics --example render_adap_rpf_mppi_svg --no-default-features --features control
cargo run -p rust_robotics --example headless_branchout_multimodal_driving --no-default-features --features planning
cargo run -p rust_robotics --example render_branchout_multimodal_driving_svg --no-default-features --features planning
cargo run -p rust_robotics --example headless_stl_cbs_multi_robot --no-default-features --features planning
cargo run -p rust_robotics --example render_stl_cbs_multi_robot_svg --no-default-features --features planning
cargo run -p rust_robotics --example headless_kinodynamic_stl_cbs --no-default-features --features planning
cargo run -p rust_robotics --example render_kinodynamic_stl_cbs_svg --no-default-features --features planning
cargo run -p rust_robotics --example render_safe_decode_nav_svg --no-default-features --features planning
cargo run -p rust_robotics --example render_frontier_navigator_svg --no-default-features --features planning
cargo run -p rust_robotics --example headless_rigid_body_mip_planning --no-default-features --features planning
cargo run -p rust_robotics --example render_rigid_body_mip_planning_svg --no-default-features --features planning
cargo run -p rust_robotics --example headless_hierarchical_mapf_replanning --no-default-features --features planning
cargo run -p rust_robotics --example render_hierarchical_mapf_replanning_svg --no-default-features --features planning
cargo run -p rust_robotics --example benchmark_hierarchical_mapf_scale --no-default-features --features planning
cargo run -p rust_robotics --example headless_navigation_loop --features "planning,localization,control"
cargo run -p rust_robotics --example headless_mission_recovery --features "planning,localization,control"
cargo run --release -p rust_robotics --example generate_euroc_feature_tracks --no-default-features --features slam -- /datasets/EuRoC/MH_01_easy
cargo run -p rust_robotics --example headless_euroc_vio --no-default-features --features slam
cargo run -p rust_robotics --example render_euroc_vio_svg --no-default-features --features slam
cargo run -p rust_robotics --example benchmark_conformal_sipp --no-default-features --features planning
cargo run -p rust_robotics --example benchmark_conformal_coverage --no-default-features --features planning
cargo run -p rust_robotics --example benchmark_traversal_risk_sweep --no-default-features --features planning

# Visualization (requires gnuplot)
cargo run -p rust_robotics --example a_star --features "planning,viz"
cargo run -p rust_robotics --example jps --features "planning,viz"
cargo run -p rust_robotics --example rear_wheel_feedback --features "control,viz"

dora-rs dataflow example

The workspace also includes a minimal dora-rs planning demo that wraps the existing headless A* planner in a dora node and sends a structured JSON path report to a sink node.

dora run crates/rust_robotics/examples/dora_path_planning_dataflow.yml

This example requires the dora runtime/CLI to be installed and uses the feature-gated dora support in crates/rust_robotics.

ROS2 Integration

The workspace includes ready-to-use ROS2 navigation nodes built with safe_drive (Rust ROS2 bindings).

  • Path Planner (A*)
  • DWA Local Planner
  • SLAM Node
  • SLAM Corrected Odom (optional)
  • EKF Localizer
  • Waypoint Navigator
                    TurtleBot3 Gazebo
                 /scan  /odom  /cmd_vel
                    |      |       ^
                    v      |       |
               +-----------+       |
               | slam_node | ----- +
               +-----------+    /map
                      |
                      v
             +-------------------+
             | path_planner_node | ---> /planned_path
             +-------------------+             |
                ^           ^                  v
                |           |           +--------------+
         /ekf_odom     /goal_pose ---> | dwa_planner |
                ^                      +--------------+
                |
      +----------------------+
      | ekf_localizer_node   |
      +----------------------+
                ^
                |
      +-------------------------+
      | waypoint_navigator_node |
      +-------------------------+

Demo video: docs/gazebo_demo.mp4

See docs/ros2_integration.md for details.

source /opt/ros/jazzy/setup.bash
export ROS_DOMAIN_ID=42  # optional but recommended if other ROS graphs are already running

cargo build --release --manifest-path ros2_nodes/path_planner_node/Cargo.toml
cargo build --release --manifest-path ros2_nodes/dwa_planner_node/Cargo.toml
cargo build --release --manifest-path ros2_nodes/slam_node/Cargo.toml
cargo build --release --manifest-path ros2_nodes/ekf_localizer_node/Cargo.toml
cargo build --release --manifest-path ros2_nodes/waypoint_navigator_node/Cargo.toml

export TURTLEBOT3_MODEL=burger
./ros2_nodes/launch/run_gazebo_demo.sh

# Multi-goal mission demo
WAYPOINT_NAV_FRAME=relative_start \
WAYPOINT_NAV_WAYPOINTS="0.4,0.0;0.1,0.4" \
  ./ros2_nodes/launch/run_gazebo_mission_demo.sh

run_gazebo_mission_demo.sh defaults to WAYPOINT_NAV_FRAME=relative_start, so the mission waypoints above are interpreted as offsets from the first odom pose observed by waypoint_navigator_node. The wrapper's default mission is a conservative two-waypoint route that was verified in TurtleBot3 world: (0.4, 0.0) -> (0.1, 0.4).

waypoint_navigator_node now includes a simple recovery state machine. If the active waypoint stays outside tolerance without measurable odom progress for WAYPOINT_NAV_STUCK_TIMEOUT seconds, it issues navigation_cancel, waits briefly, rotates in place, backs off, then republishes the active waypoint. The main tuning knobs are WAYPOINT_NAV_MAX_RECOVERY_ATTEMPTS, WAYPOINT_NAV_RECOVERY_ROTATE_SECONDS, WAYPOINT_NAV_RECOVERY_BACKOFF_SECONDS, and WAYPOINT_NAV_RECOVERY_BACKOFF_SPEED.

For observability, navigation_demo.launch.py now also exposes:

  • ENABLE_RVIZ=true ./ros2_nodes/launch/run_gazebo_demo.sh to open RViz with navigation_demo.rviz
  • ENABLE_GAZEBO_GUI=false ./ros2_nodes/launch/run_gazebo_demo.sh for a headless Gazebo server run
  • dynamic TF from the selected nav odom topic to the robot base frame via odom_tf_broadcaster.py
  • /mission_status (std_msgs/String) for mission / recovery state summaries
  • /mission_markers (visualization_msgs/MarkerArray) for the route, active goal, and status text

By default, the current Gazebo demo uses odom as its honest global frame: slam_node publishes /map in the raw odom frame it actually integrates against, path_planner_node republishes /planned_path in that same frame, and waypoint_navigator_node publishes /goal_pose plus /mission_markers in RUST_NAV_GLOBAL_FRAME=odom. If you still want the old RViz alias, you can opt into PUBLISH_MAP_ODOM_TF=true to add a legacy identity map -> odom transform.

If you want the experimental corrected SLAM frame, enable:

ENABLE_SLAM_CORRECTED_FRAME=true ./ros2_nodes/launch/run_gazebo_mission_demo.sh

That switches the mission stack to NAV_ODOM_TOPIC=/slam_odom and NAV_GLOBAL_FRAME=map. In this mode, slam_node publishes /slam_pose plus /slam_odom, the map is integrated in map, and map_odom_tf_broadcaster.py estimates a dynamic map -> odom transform from /slam_odom against the raw odom stream. The corrected pose update is quality-gated: high-error, low-motion, or outlier ICP deltas fall back to pure odom, and medium-quality matches are attenuated instead of applied at the full blend factor.

Corrected mode also exposes two extra observability topics:

  • /slam_diagnostics (std_msgs/String) with per-scan ICP convergence, error distribution, inlier ratio, blend_alpha, gate_reason, and applied correction deltas
  • /slam_ground_truth_status (std_msgs/String) with relative-start Gazebo ground-truth error metrics derived from gz topic -e -t /world/default/dynamic_pose/info --json-output

The ground-truth monitor defaults to the spawned model name (GROUND_TRUTH_ENTITY_NAME=$TURTLEBOT3_MODEL) and compares /ekf_odom plus /slam_odom against the model pose after subtracting the first ground-truth sample. You can override the Gazebo source with GROUND_TRUTH_GZ_POSE_TOPIC if needed.

For a local ROS2/Gazebo regression check, run:

ROS_DOMAIN_ID=89 ENABLE_RVIZ=false ENABLE_GAZEBO_GUI=false ./ros2_nodes/launch/run_navigation_smoke_test.sh

The smoke script launches the mission demo, verifies /map, /planned_path, and typed /mission_markers all match the nav odom frame, checks the dynamic nav odom TF, and waits for mission complete -> goal cleared -> stop command in the navigation logs.

To smoke-test the corrected SLAM frame as well:

ROS_DOMAIN_ID=90 ENABLE_RVIZ=false ENABLE_GAZEBO_GUI=false ENABLE_SLAM_CORRECTED_FRAME=true \
  ./ros2_nodes/launch/run_navigation_smoke_test.sh

In corrected mode, the same script also verifies dynamic map -> odom.

Benchmarks

Rust vs Python Speed Comparison

AlgorithmRust (ms)Python (ms)Speedup
A* (100x100)4.0924.5231x
EKF (1000 steps)0.19103.1543x
RRT (100 runs)0.125.746x
CubicSpline (1000 runs)0.926.97.5x

Any-Angle Planner Comparison (160 MovingAI scenarios)

PlannerPath Quality vs Theta*Speed vs Theta*
Theta*baselinebaseline
Lazy Theta*same (+0.01%)1.7x faster (p=0.025)
A*+optimize_pathsame (+0.27%)2.3x faster

Grid Planner Benchmark (50x50)

cargo bench -p rust_robotics_planning --bench unified_planning_benchmark
cargo bench -p rust_robotics_planning --bench jps_crossover_benchmark

Table of Contents

Localization

Extended Kalman Filter Localization

Gray: GPS measurements, Blue: Ground truth, Green: EKF estimate with 2-sigma covariance ellipse

cargo run -p rust_robotics --example render_gif_ekf_localization --features "localization,gif"

Particle Filter Localization

Yellow: Range landmarks, Light blue: Particles, Blue: Ground truth, Green: Particle filter estimate

cargo run -p rust_robotics --example render_gif_particle_filter --features "localization,gif"

Unscented Kalman Filter Localization

Blue: Ground Truth, Red: UKF Estimate, Black: Dead Reckoning, Green: GPS Observations, Red Ellipse: Uncertainty

Histogram Filter Localization

Grid-based probabilistic localization using RFID landmarks. The algorithm maintains a probability distribution over a 2D grid and updates it based on motion and observations.

Blue: True path, Orange: Dead Reckoning, Green: Histogram Filter estimate, Black: RFID landmarks

Cubature Kalman Filter

Cubature Kalman Filter (CKF) using 3rd-degree spherical-radial cubature rule. Achieves the same accuracy as UKF but 30% faster with zero tuning parameters (no alpha/beta/kappa). Recommended as the default over UKF for typical robotics scenarios.

Ensemble Kalman Filter

Stochastic ensemble-based Kalman filter. Maintains an ensemble of state particles and updates them using the Kalman gain computed from ensemble statistics.

Adaptive Filter

Automatically switches between EKF (fast, linear) and CKF (robust, nonlinear) based on Normalized Innovation Squared (NIS). When innovation exceeds the chi-squared threshold, switches to CKF for better nonlinearity handling.

Complementary Filter

Fuses high-frequency prediction (control/gyro) with low-frequency measurement (position sensor) using a tunable blending factor alpha. Simple, fast, and effective for IMU fusion.

Iterated EKF

Improves EKF accuracy by iterating the update step linearization. Re-linearizes the observation model around the updated state estimate multiple times until convergence.

Information Filter

Dual of the Kalman Filter operating in information space (inverse covariance). Update step is additive in information form, making multi-sensor fusion natural.

Square Root UKF

UKF variant that propagates Cholesky factors instead of full covariance matrices. Improves numerical stability and guarantees positive semi-definiteness.

Monte Carlo Localization

Adaptive Particle Filter with KLD-sampling. Automatically adjusts particle count based on posterior complexity — more particles for multi-modal distributions, fewer after convergence.

Mapping

NDT Map

Gaussian Grid Map

Occupancy grid mapping using Gaussian distribution. Higher probability near obstacles.

Ray Casting Grid Map

Occupancy grid mapping using ray casting. Free space (0.5), Occupied (1.0), Unknown (0.0).

DBSCAN Clustering

Density-based spatial clustering that finds arbitrary-shaped clusters and identifies outliers (noise). No need to specify number of clusters in advance.

Line Extraction

Extracts line segments from 2D scan data using the Split-and-Merge (Iterative End Point Fit) algorithm. Used for feature extraction in indoor environments.

Implicit Moving Least Squares (IMLS)

PCA-based local normal estimation and Gaussian-weighted implicit surface projection for 2D point sets. The API reports signed distance, surface normal, projected point, and local support count.

Occupancy Grid Map

Probabilistic occupancy grid using log-odds representation. Updates cells via Bresenham ray casting — free along rays, occupied at endpoints.

Gaussian Process Regression

GP regression with RBF kernel for terrain/surface mapping from sparse measurements. Provides predictions with uncertainty estimates.

SLAM

Iterative Closest Point (ICP) Matching

Blue: Previous scan, Red: Current scan, Green: Current scan aligned by ICP

cargo run -p rust_robotics --example render_gif_slam --features "slam,gif"

The SLAM crate also provides robust optimizer-backed point-to-line ICP in 2D and point-to-plane ICP in 3D:

FastSLAM 1.0

Particle filter based SLAM (Simultaneous Localization and Mapping). Each particle maintains its own map of landmarks using EKF.

Yellow: True landmarks, Red cross: Estimated landmarks, Light blue: Particles, Blue: True path, Green: FastSLAM estimate

EKF SLAM

Extended Kalman Filter based SLAM. Maintains a joint state vector of robot pose and landmark positions with full covariance matrix.

Yellow: True landmarks, Red cross: Estimated landmarks, Blue: True path, Green: EKF-SLAM estimate

FastSLAM 2.0

Improved particle filter SLAM that incorporates the latest observation into the proposal distribution before sampling, producing better particle diversity than FastSLAM 1.0.

Graph-Based SLAM

Pose graph optimization for SLAM. Constructs a graph of robot poses connected by odometry and observation constraints, then optimizes the graph using iterative methods.

Pose Graph Optimization

Pose graph before and after block-sparse optimization

SE(2) and SE(3) pose graph optimization using the shared Levenberg-Marquardt factor-graph backend. Dense LU and block-sparse preconditioned conjugate-gradient linear solvers are available; SE(3) factors use analytic Jacobians. The module fixes the first pose to remove gauge freedom and supports standard g2o SE2/SE3 quaternion text I/O.

Representative --release results at size 200 on a development workstation:

Problem / solverParametersDense matrixStored blocksIterationsTimeRMSE
Pose graph / dense LU5972.72 MiB29.2 KiB9188.7 ms8.11e-7
Pose graph / block-sparse PCG5972.72 MiB29.2 KiB979.1 ms8.11e-7
BA / dense LU6002.75 MiB14.1 KiB480.9 ms8.42e-13
BA / Schur complement6002.75 MiB14.1 KiB41.86 ms8.42e-13

Reproduce the full 10/50/100/200 sweep:

cargo run --release -p rust_robotics --example benchmark_factor_graph_scaling \
  --no-default-features --features slam

For genuinely large graphs, benchmark_large_pose_graph runs only the block-sparse path and fails if convergence or RMSE acceptance is missed:

PosesParametersDense equivalentStored blocksIterationsTimeRMSE
1,0002,99768.5 MiB141.0 KiB80.64 s2.20e-3
5,00014,9971.68 GiB706.3 KiB94.54 s1.13e-3
10,00029,9976.70 GiB1.38 MiB98.62 s3.52e-4
cargo run --release -p rust_robotics --example benchmark_large_pose_graph \
  --no-default-features --features slam

EuRoC / KITTI Dataset Replay

EuRoC IMU, bundle adjustment, and SE3 pose graph pipeline

The SLAM crate loads official EuRoC MAV and KITTI odometry directory layouts without imposing an image decoder on the library. A companion CLI decodes EuRoC PNGs, tracks Shi-Tomasi corners with forward/backward pyramidal Lucas-Kanade, and triangulates them from the IMU-predicted metric trajectory. The replay then jointly optimizes cameras and landmarks with Schur elimination then refines navigation states and time-varying IMU biases before fusing metric IMU edges with the visual closure in an SE(3) pose graph. EuRoC imu0 T_BS extrinsics include centripetal and tangential lever-arm correction.

cargo run --release -p rust_robotics \
  --example generate_euroc_feature_tracks \
  --no-default-features --features slam -- /datasets/EuRoC/MH_01_easy

cargo run -p rust_robotics --example headless_euroc_vio \
  --no-default-features --features slam

IMU Preintegration

Bias-aware accelerometer/gyroscope preintegration with SO(3) updates, 9-state error covariance propagation, 9x6 bias Jacobians, navigation-state prediction, and an optimizer-ready IMU factor with analytic state and bias Jacobians.

Bundle Adjustment

Pinhole projection, analytic camera/landmark reprojection Jacobians, robust losses, configurable gauge anchoring, and joint camera/landmark optimization with landmark Schur elimination enabled by default. Camera poses use world-from-camera SE(3) matrices.

Correlative Scan Matching

Brute-force correlative scan matcher that searches over a discretized pose space. More robust to initial pose errors than ICP — useful as a SLAM front-end.

Path Planning

A* Algorithm

Blue: Start, Red: Goal, Green: Path, Gray: Obstacles

cargo run -p rust_robotics --example a_star --features "planning,viz"

Theta* Algorithm

Any-angle path planning algorithm. Unlike A* which restricts movement to grid edges, Theta* allows paths at any angle by checking line-of-sight between nodes.

cargo run -p rust_robotics --example theta_star --features "planning,viz"

Lazy Theta*

Lazy Theta* defers line-of-sight checks until node expansion, reducing redundant visibility tests. Achieves the same path quality as Theta* while being 1.7x faster (p=0.025 on 160 MovingAI scenarios).

Enhanced Lazy Theta*

Extends Lazy Theta* with wider parent selection at expansion time. Uses 2-ring neighborhood search and ancestor chain walks to find better any-angle shortcuts. Achieves near-optimal paths (+0.11% vs visibility-graph optimal on 50x50 grids).

Anya (Optimal Any-Angle)

Optimal any-angle pathfinding using visibility-graph Dijkstra on all free cells. Guarantees the shortest any-angle path. Used as the optimality baseline for evaluating Theta* variants.

Path Smoothing

Post-processing pipeline for grid-based paths: greedy LOS shortcutting followed by iterative waypoint relaxation. Transforms grid-constrained A* paths into near-optimal any-angle paths. A*+optimize_path achieves 2.3x speedup over Theta* with equivalent path quality.

Jump Point Search (JPS)

Optimized pathfinding algorithm for uniform-cost grids. Reduces the number of nodes to explore by identifying and jumping to key "jump points" instead of examining all neighbors.

cargo run -p rust_robotics --example jps --features "planning,viz"

Bezier Path Planning

Blue: Start, Red: Goal, Green: Path

Cubic Spline

Black: Control points, Green: Path

Dynamic Window Approach

Black: Obstacles, Blue cross: Goal, Green: Traveled trajectory, Light green: Best predicted trajectory

cargo run -p rust_robotics --example render_gif_dwa --features "planning,gif"

D* Lite

Blue: Start, Red: Goal, Green: Path, Black: Obstacles

D* Lite is an incremental heuristic search algorithm for path planning in dynamic environments. It's particularly efficient for replanning when the environment changes.

Dijkstra Algorithm

Informed RRT*

Blue: Start, Red: Goal, Green: Path, Black: Tree

Model Predictive Trajectory Generator

Green: Path

Potential Field Algorithm

Blue: Start, Red: Goal, Green: Path, Gray: Obstacles

Quintic Polynomials

Blue: Start, Red: Goal, Green: Path

Rapidly-Exploring Random Trees (RRT)

Sampling-based path planning algorithm that builds a tree by randomly sampling the configuration space.

Green: Start and tree, Blue cross: Goal, Red: Found path, Black: Obstacles

cargo run -p rust_robotics --example render_gif_rrt --features "planning,gif"

RRT*

Optimized version of RRT that rewires the tree to find shorter paths. Asymptotically optimal.

Blue: Start, Red: Goal, Green: Path, Gray: Tree

Reeds-Shepp Path

Blue: Start, Red: Goal, Green: Path

Dubins Path

Shortest path for non-holonomic vehicles with bounded turning radius. Computes optimal paths composed of circular arcs and straight segments (6 types: LSL, RSR, LSR, RSL, RLR, LRL).

PRM (Probabilistic Road-Map)

Sampling-based path planning using random samples and k-nearest neighbor connections.

Blue: Start, Red: Goal, Green: Path, Gray: Samples and edges, Black: Obstacles

Voronoi Road-Map

Path planning using Voronoi diagram vertices as waypoints. Provides paths that maximize clearance from obstacles.

Blue: Start, Red: Goal, Green: Path, Cyan: Voronoi vertices, Black: Obstacles

Frenet Optimal Trajectory

Optimal trajectory planning in Frenet coordinate frame. Widely used in autonomous driving for lane keeping and obstacle avoidance.

Gray: Reference path, Green: Optimal trajectory, Black: Obstacles, Red: Vehicle

State Lattice Planner

Lattice-based motion planning that searches over a pre-computed set of motion primitives. Generates smooth, dynamically feasible trajectories by connecting state lattice primitives.

cargo run -p rust_robotics --example state_lattice --features "planning,viz"

Path Tracking

LQR Steer Control

Black: Planned path, Green: Tracked path

Move to Pose

Green: Path, Red: Start and Goal

Pure Pursuit

Gray: Planned course, Green: Tracked path, Red: Vehicle

cargo run -p rust_robotics --example render_gif_pure_pursuit --features "control,gif"

Stanley Control

Black: Planned path, Green: Tracked path

Rear Wheel Feedback Control

Path tracking using rear wheel feedback steering control. Combines heading error and lateral error with path curvature feedforward.

Blue: Reference path, Red: Vehicle trajectory, Green: Waypoints

cargo run -p rust_robotics --example rear_wheel_feedback --features "control,viz"

MPC (Model Predictive Control)

Model Predictive Control for path tracking using linearized bicycle model. Predicts future states and optimizes control inputs over a horizon.

Gray: Reference path, Blue: Tracked trajectory, Green: Prediction horizon, Red: Vehicle

Inverted Pendulum

LQR Control

Cart-pendulum animation showing LQR control stabilization. Blue: Cart, Black: Pendulum. Multiple frames overlaid to show time progression from initial angle to stabilized state.

Arm Navigation

Two Joint Arm Control

Two joint arm to a point control simulation using inverse kinematics.

Black: Arm links, Red: Joints (shoulder, elbow, end effector), Green: Target position

Aerial Navigation

3D Grid A*

Bounded 3D voxel-grid planning for aerial robots. The planner supports 6-connected or 26-connected motion and returns a collision-free waypoint sequence.

Drone 3D Trajectory Following

Closed-loop quadrotor waypoint tracking with quintic segments, PD thrust/attitude control, and Euler-integrated rigid-body dynamics.

Drone Minimum-Snap Trajectory

Seventh-order minimum-snap segment generation for drone waypoint loops. The module exposes piecewise segment generation, desired-state sampling, and a direct path into the existing quadrotor tracker.

Mission Planning

Behavior Tree

Behavior tree runtime for mission-level decision making with sequence, selector, condition, and action nodes backed by a shared blackboard.

State Machine

Finite state machine for robot behavior management with states, transitions, guards, and actions

Community

Contributions are welcome, especially focused algorithm ports, deterministic benchmarks, reproducible ROS2/Gazebo issues, and visual examples that make the library easier to evaluate.

Mathematical foundations and demonstrations adapted from third-party projects retain their original attribution in THIRD_PARTY_NOTICES.md.