Lidar offset handling is now done in SangamIO (commit 64020cf), which
transforms lidar data to robot-center coordinates before streaming.
This simplifies VastuSLAM by removing hardware-specific configuration.
Also adds debug visualization support to SVG output:
- DebugMarker struct for custom markers with position, radius, color
- with_waypoints() for path waypoints (orange)
- with_frontiers() for frontier targets (green, labeled)
- with_markers() for arbitrary debug markers
- Dynamic legend sizing based on active entries
Move lidar mounting configuration from mock simulation config to
hardware config section. SangamIO now transforms lidar measurements
from the physical lidar position to robot center before streaming,
ensuring clients receive consistent robot-centered data regardless
of hardware vs simulation.
- Add LidarMountingConfig with transform_to_robot_center() method
- Apply transformation in Delta2DDriver before accumulating points
- Simplify mock simulator to generate robot-centered data directly
- Consolidate mounting parameters in sangamio.toml [device.hardware]
Load VastuConfig from configs/config.toml instead of hardcoded defaults
for map and matcher configuration. Sensor offset is now read from the
config file rather than being hardcoded.
Upgrade benchmark dependencies to latest versions. Criterion 0.8
requires using std::hint::black_box instead of criterion::black_box.
Also includes minor formatting improvements from rustfmt.
Replace time-based movement with MoveDistance and Rotate commands that
use encoder feedback for precise motion control. Commands complete when
target wheel distances are achieved, not after a fixed duration.
- Replace serde_yaml with toml crate for config parsing
- Make serde_yaml optional behind "scenario" feature flag
- Add config.toml with full documentation
- Add scenario_slam example for running test scenarios
- Clean up release profile settings
- Add dhruva proposal document
- Rename DhruvaSLAM references to VastuSLAM throughout CLAUDE.md
- Update module organization to reflect current vastu-slam structure
- Add evaluation module to README.md module table
- Remove obsolete .gitignore and BENCHMARK_LOG.md files
- Minor code formatting fixes in benchmarks
Use par_iter for Level 0 cells (742K cells with Gaussian scoring)
and for each coarse level within-level parallelization.
Performance: 1230µs → 670µs (1.84x speedup, 45% improvement)
on 10-core Apple Silicon.
- Add rayon 1.10 dependency for data parallelism
- Implement search_poses_parallel() with par_iter on outer x-loop
- Add dispatch_search_poses() to choose sequential/parallel mode
- Add use_parallel config flag (default: false for compatibility)
- Add configure_thread_pool(threads, stack_kb) API for embedded
- Update benchmarks to compare sequential vs parallel modes
Performance (10-core Apple Silicon):
- Sequential: 1716 µs
- Parallel: 449 µs
- Speedup: 3.82x
Expected 2-3x speedup on 4-core ARM (Allwinner A33).
Introduce BENCHMARK_LOG.md for tracking performance, accuracy, and
quality metrics across commits. Includes baseline.json with initial
measurements from the simplified configuration commit.
- Rename header from VastuMap to VastuSLAM
- Increase initial grid from 300x300 to 800x800 cells (20m at 2.5cm)
- Move log_odds config from grid to slam section
- Consolidate pose_graph under slam section
- Remove unused sections: submap, motion_filter, query, pathfinding, exploration
- Adjust correlative matcher defaults for better performance
- Add evaluation module with accuracy, quality, and performance metrics
- Add ground truth handling and relations metrics for SLAM evaluation
- Add criterion benchmarks for scan matching, grid operations, and evaluation
- Add integration tests for accuracy, quality, and performance
- Add Serialize/Deserialize derives to CellCounts for evaluation output
Replace old monolithic modules (extraction, features, integration,
loop_closure, matching, vector_map, cbvg) with new streamlined
architecture including grid, slam, pathfinding, submap, and io modules.
Simplify core types and exploration controller.
Major improvements to the Clearance-Based Visibility Graph (CBVG) system:
CBVG Graph:
- Replace grid-based node generation with ray-based placement along lidar scans
- Add angular gap analysis for frontier detection (nodes with uncovered sectors)
- Add clique optimization for merging redundant nodes
- Add path planning with node exclusion for alternative approach angles
- Increase max_nodes to 5000 for large multi-room layouts
- Add configurable max_lidar_range for node creation
Exploration Controller:
- Track CBVG goal nodes that led to collision for each frontier
- Add oscillation detection and frontier suppression
- Implement multi-attempt collision recovery before marking unreachable
- Plan alternative paths by excluding previously failed goal nodes
- Handle collisions during recovery state
Vector Map:
- Add get_path_excluding_nodes() for alternative approach planning
- Integrate CBVG-based frontier detection with configurable parameters
Other:
- Scan module: Include max-range readings for open-space node creation
- Example: Add command-line map file selection
- Temporarily disable release optimizations for debugging
Add pixel_to_world() coordinate conversion to SimulationMap, enabling
the exploration example to randomly select maps from the data/maps
directory and find valid start positions with sufficient clearance.
This makes test runs more varied and robust.
The harness is only used by the exploration example, not as part of
the public API. This change:
- Moves harness from src/harness/ to examples/exploration/harness/
- Removes integration-tests feature flag from Cargo.toml
- Moves sangam-io, svg, image to dev-dependencies
- Removes unused scenario module and path.rs
Example now runs with: cargo run --example exploration --release
Move integration test harness from tests/integration/ to src/harness/
as a feature-gated library module, enabling use by examples and tools.
- Add pub mod harness (behind integration-tests feature)
- Reorganize data files into data/maps/ and data/scenarios/
- Convert integration test to exploration example
- Remove test-only boilerplate (mod.rs, yaml_tests.rs)
SIMD-optimize core math and line fitting functions using f32x4:
- compute_centroid and compute_covariance for 4+ point batches
- fitting_error and max_distance_point in line fitting
- find_best_split_point in split-merge extraction
- New optimize_pose_simd using CorrespondenceSoA for ~30-40% speedup
Add LineSpatialIndex for O(n×k) correspondence search:
- Grid-based spatial hash reduces ICP from O(n×m) to O(n×k)
- Automatic threshold (30+ lines, 8m+ extent) for index use
- Integrated into ICP matcher with brute-force fallback for small maps
- New match_scan_indexed and find_correspondences_indexed methods
Replace the corner-based visibility graph with a new CBVG algorithm
that places navigation nodes along the medial axis of free space.
Key changes:
- Add cbvg module with Voronoi-based medial axis approximation
- Nodes placed at safe distances from walls (configurable clearance)
- Adaptive clearance for narrow passages
- Visibility region tracking for explored area awareness
- Integrate CBVG into VectorMap with nearest_cbvg_node() API
PathFollower changes:
- Remove pure pursuit lookahead logic
- Follow CBVG waypoints sequentially for predictable navigation
Configuration:
- New cbvg config section with min_clearance, voronoi_sample_step, etc.
- Remove old path_planning config
This provides safer robot navigation by routing through corridor
centerlines rather than wall corners.
Refactor exploration to use hierarchical region-based planning instead
of greedy closest-frontier selection. This eliminates ping-pong behavior
and improves coverage efficiency.
Key changes:
- Gap-based frontier detection: frontiers are now viewpoints to observe
unexplored gaps between unconnected wall endpoints, not the endpoints
themselves. Each frontier has a safe viewpoint, look direction, and
estimated unexplored area.
- Region clustering: frontiers grouped using union-find clustering,
enabling systematic area-by-area exploration
- TSP-ordered planning: regions ordered using greedy TSP to minimize
travel distance while maintaining coverage
- Exploration history: spatial grid tracks visited areas to penalize
backtracking and improve region prioritization
- Unreachable frontier recovery: clears unreachable list when robot
moves 1.5m, allowing retry from new positions
New modules: history.rs, planner.rs, region.rs
Updated: Frontier struct (viewpoint/look_direction/endpoint/area)
Enable loading algorithm parameters from YAML files for reproducible
experiments and easier parameter tuning without recompilation.
- Add yaml.rs module with from_yaml_file/to_yaml_file methods
- Add Serialize/Deserialize derives to all config structs
- Add finalize() method to recompute derived fields after deserialization
- Add configs/default.yaml reference configuration with documentation
- Update test harness to load config from VASTU_MAP_CONFIG env var
- Move serde/serde_yaml to main dependencies
Implements a new exploration mode that stores raw scan data and periodically
re-fits lines from accumulated point clouds, eliminating first-scan bias in
line extraction.
New modules:
- scan_store: Retains raw scans with pose tracking and memory management
- point_association: Associates world-frame points with existing map lines
- refitter: Re-fits lines from accumulated points with gap detection/splitting
Key features:
- ExplorationConfig for controlling re-fit intervals and thresholds
- LineFitConfig with max_line_length splitting for robustness
- Line2D point_count field for tracking scan point origins
- Gap-based line splitting during re-fitting
- Range-based weighting using noise model during fitting
- Add exploration controller with frontier-based autonomous exploration
- Add path follower with pure-pursuit algorithm and velocity commands
- Add collision detection with virtual walls and recovery behaviors
- Fix wheel odometry to return robot-frame deltas (not world-frame)
- Add step-by-step simulation API in test harness for exploration tests
- Add odometry module with WheelOdometry for differential drive robots
using arc-based motion integration (ROS REP-103 convention)
- Add preprocessing module with outlier removal and voxel downsampling
for point cloud filtering
- Add path_planning module with visibility graph and Dijkstra's algorithm
for optimal shortest-path planning around obstacles
- Implement get_path() and is_path_clear() on VectorMap using the new
path planner
- Update test harness to simulate encoder ticks and use WheelOdometry
instead of ground-truth-derived relative odometry
- Move approx and serde to dev-dependencies (not needed at runtime)
- Update svg from 0.17 to 0.18
- Update criterion from 0.5 to 0.8
- Remove unused release-with-debug profile
- Add distances_and_projections_into() to LineCollection for single-pass
computation with SIMD, improving cache locality over separate calls
- SIMD-optimize transform_points() in IcpScratchSpace (4 points per iteration)
- Add LineDataRaw struct to group line properties for cleaner correspondence API
- Simplify CorrespondenceSoA::push_inline() to take LineDataRaw instead of 7 params
- Remove simd module (std::simd documentation no longer needed as separate module)
Eliminate per-frame allocations in real-time SLAM pipeline by introducing
reusable scratch buffers:
- RansacScratchSpace: Pre-allocated buffers for RANSAC line extraction
with extract_lines_ransac_with_scratch and hybrid variant
- IcpScratchSpace: Extended with subsampling buffers for multi-resolution
matching; PointToLineIcp now holds internal scratch
- FrontierDetector: Persistent spatial hash grid and output buffers
- ObserveScratchSpace: Buffers for VectorMap observe pipeline
- CorrespondenceSet: In-place retain_by_distance/retain_within_segment
ScanMatcher trait methods changed from &self to &mut self to support
internal scratch space usage.
- Replace custom Float4 wrapper with std::simd::f32x4 (portable_simd)
- Add CorrespondenceSoA with inlined line data for contiguous SIMD loads
- Add optimize_pose_simd() and find_correspondences_soa() functions
- Simplify cargo LLVM flags (explicit SIMD vs auto-vectorization)
- Add rust-toolchain.toml for nightly channel requirement
The SoA layout inlines line normals and endpoints at correspondence
creation time, enabling ~4x faster Gauss-Newton optimization by
avoiding indexed access in the hot loop.
Add TimingBreakdown struct to track per-stage timing (ICP matching,
line extraction, corner detection, association, merging, new features,
loop closure) with microsecond precision. Integration test harness now
collects and prints timing summaries.
Also clean up integration tests by removing unused odometry module and
simplifying path_generator to only include PathSegment (unused PathGenerator
methods removed).
Major improvements to scan matching accuracy and convergence:
- Add Levenberg-Marquardt damping to Gauss-Newton solver for robust convergence
from poor initial guesses (replaces pure Gauss-Newton)
- Add IRLS (Iteratively Reweighted Least Squares) with M-estimators:
Huber (default), Cauchy, Tukey, Geman-McClure for soft outlier rejection
- Add multi-resolution ICP (coarse-to-fine) for better convergence
- Add corner correspondences for point-to-point constraints
- Add DOF-corrected covariance computation for accurate uncertainty
Line extraction improvements:
- Add RANSAC-based line extraction for robustness to outliers
- Add hybrid extraction mode (RANSAC + split-merge)
New modules:
- src/matching/robust.rs: M-estimator cost functions
- src/motion_model.rs: Constant-velocity motion model for pose prediction
- src/extraction/ransac_lines.rs: RANSAC line extraction
Test harness:
- Add fast mode (~22x speedup) for integration tests
- Simplify observation records
Julien Denizot