From 4270670fc7da5f3eb43c3b60673000ba6a0258df Mon Sep 17 00:00:00 2001 From: codetiger Date: Tue, 30 Dec 2025 21:11:51 +0530 Subject: [PATCH] Refactor vastu-map with new modular architecture 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. --- .gitignore | 2 +- vastu-map/Cargo.toml | 19 +- vastu-map/configs/config.yaml | 121 + vastu-map/configs/default.yaml | 268 --- .../scenarios/drift_continuous_curve.yaml | 43 - vastu-map/data/scenarios/drift_long_path.yaml | 57 - .../data/scenarios/icp_sparse_corridor.yaml | 27 - .../data/scenarios/loop_closure_test.yaml | 47 - .../scenarios/medium_room_exploration.yaml | 42 - .../data/scenarios/medium_room_obstacles.yaml | 52 - .../data/scenarios/medium_room_perimeter.yaml | 46 - .../data/scenarios/pure_rotation_test.yaml | 33 - .../scenarios/simple_room_straight_line.yaml | 20 - .../data/scenarios/static_scan_test.yaml | 24 - .../data/scenarios/straight_line_test.yaml | 19 - vastu-map/docs/proposal.md | 594 +++++ .../examples/exploration/harness/adapters.rs | 28 - .../examples/exploration/harness/harness.rs | 545 ----- .../examples/exploration/harness/metrics.rs | 325 --- vastu-map/examples/exploration/harness/mod.rs | 18 - .../exploration/harness/visualization.rs | 816 ------- vastu-map/examples/exploration/main.rs | 502 ---- vastu-map/examples/explore_mock.rs | 726 ++++++ vastu-map/examples/explore_simple.rs | 305 +++ vastu-map/examples/scan_match_test.rs | 671 ++++++ vastu-map/{data => }/maps/corridor.pgm | 0 vastu-map/{data => }/maps/corridor.yaml | 0 vastu-map/{data => }/maps/large_room.pgm | 0 vastu-map/{data => }/maps/large_room.yaml | 0 vastu-map/{data => }/maps/medium_room.pgm | 0 vastu-map/{data => }/maps/medium_room.yaml | 0 vastu-map/{data => }/maps/simple_room.pgm | 0 vastu-map/{data => }/maps/simple_room.yaml | 0 vastu-map/scenarios/rotation_360.yaml | 23 + vastu-map/scenarios/square_path.yaml | 94 + vastu-map/scenarios/straight_line.yaml | 22 + vastu-map/src/config.rs | 907 +++++--- vastu-map/src/core/README.md | 92 - vastu-map/src/core/bounds.rs | 330 --- vastu-map/src/core/cell.rs | 270 +++ vastu-map/src/core/math.rs | 457 ---- vastu-map/src/core/mod.rs | 94 +- vastu-map/src/core/point.rs | 474 ++-- vastu-map/src/core/pose.rs | 403 ++-- vastu-map/src/core/scan.rs | 652 ------ vastu-map/src/core/sensors.rs | 368 +++ vastu-map/src/core/simd.rs | 604 +++++ vastu-map/src/exploration/collision.rs | 215 -- vastu-map/src/exploration/config.rs | 187 +- vastu-map/src/exploration/controller.rs | 2020 +++++++---------- vastu-map/src/exploration/history.rs | 264 --- vastu-map/src/exploration/mod.rs | 97 +- vastu-map/src/exploration/path_follower.rs | 374 --- vastu-map/src/exploration/planner.rs | 580 ----- vastu-map/src/exploration/region.rs | 424 ---- vastu-map/src/exploration/state.rs | 182 ++ vastu-map/src/explore/config.rs | 125 + vastu-map/src/explore/explorer.rs | 428 ++++ vastu-map/src/explore/mod.rs | 40 + vastu-map/src/explore/source.rs | 101 + vastu-map/src/explore/submap_explorer.rs | 548 +++++ vastu-map/src/extraction/README.md | 98 - vastu-map/src/extraction/corner_detection.rs | 784 ------- vastu-map/src/extraction/line_fitting.rs | 1054 --------- vastu-map/src/extraction/mod.rs | 174 -- vastu-map/src/extraction/ransac_lines.rs | 1117 --------- .../src/extraction/split_merge/algorithm.rs | 742 ------ .../src/extraction/split_merge/config.rs | 88 - .../src/extraction/split_merge/helpers.rs | 470 ---- vastu-map/src/extraction/split_merge/mod.rs | 38 - vastu-map/src/extraction/traits.rs | 122 - vastu-map/src/features/README.md | 108 - vastu-map/src/features/corner.rs | 341 --- vastu-map/src/features/descriptors.rs | 564 ----- vastu-map/src/features/feature_set.rs | 354 --- vastu-map/src/features/line.rs | 586 ----- vastu-map/src/features/line_collection.rs | 996 -------- vastu-map/src/features/mod.rs | 70 - vastu-map/src/features/spatial_index.rs | 482 ---- vastu-map/src/grid/bumper_update.rs | 267 +++ vastu-map/src/grid/cliff_update.rs | 208 ++ vastu-map/src/grid/config.rs | 205 ++ vastu-map/src/grid/lidar_update.rs | 284 +++ vastu-map/src/grid/mod.rs | 19 + vastu-map/src/grid/raycaster.rs | 309 +++ vastu-map/src/grid/storage.rs | 1095 +++++++++ vastu-map/src/integration/README.md | 140 -- vastu-map/src/integration/association.rs | 544 ----- vastu-map/src/integration/merger.rs | 1017 --------- vastu-map/src/integration/mod.rs | 64 - .../src/integration/point_association.rs | 544 ----- vastu-map/src/integration/refitter.rs | 706 ------ vastu-map/src/integration/scan_store.rs | 907 -------- vastu-map/src/integration/spatial_index.rs | 406 ---- vastu-map/src/io/mod.rs | 60 + vastu-map/src/io/pgm.rs | 140 ++ vastu-map/src/io/scenario.rs | 313 +++ vastu-map/src/io/svg.rs | 553 +++++ vastu-map/src/io/vastu_format.rs | 320 +++ vastu-map/src/lib.rs | 798 +++---- vastu-map/src/loop_closure/README.md | 175 -- vastu-map/src/loop_closure/config.rs | 76 - vastu-map/src/loop_closure/detector.rs | 502 ---- vastu-map/src/loop_closure/mod.rs | 48 - vastu-map/src/loop_closure/traits.rs | 77 - vastu-map/src/loop_closure/types.rs | 97 - vastu-map/src/matching/README.md | 182 -- vastu-map/src/matching/correspondence.rs | 1107 --------- vastu-map/src/matching/gauss_newton.rs | 1233 ---------- vastu-map/src/matching/icp/config.rs | 428 ---- vastu-map/src/matching/icp/matcher.rs | 1324 ----------- vastu-map/src/matching/icp/mod.rs | 30 - vastu-map/src/matching/mod.rs | 92 - vastu-map/src/matching/nearest_neighbor.rs | 542 ----- vastu-map/src/matching/ransac.rs | 853 ------- vastu-map/src/matching/robust.rs | 300 --- vastu-map/src/matching/scratch.rs | 962 -------- vastu-map/src/matching/traits.rs | 150 -- vastu-map/src/motion_model.rs | 290 --- vastu-map/src/odometry.rs | 405 ---- vastu-map/src/pathfinding/astar.rs | 822 +++++++ vastu-map/src/pathfinding/mod.rs | 38 + vastu-map/src/pathfinding/smoothing.rs | 400 ++++ vastu-map/src/query/README.md | 153 -- vastu-map/src/query/cbvg/clearance.rs | 302 --- vastu-map/src/query/cbvg/config.rs | 227 -- vastu-map/src/query/cbvg/dijkstra.rs | 360 --- vastu-map/src/query/cbvg/graph.rs | 1923 ---------------- vastu-map/src/query/cbvg/mod.rs | 83 - vastu-map/src/query/cbvg/node.rs | 167 -- vastu-map/src/query/cbvg/visibility.rs | 597 ----- vastu-map/src/query/cbvg/voronoi.rs | 467 ---- vastu-map/src/query/frontier.rs | 1615 ++++++------- vastu-map/src/query/mod.rs | 71 +- vastu-map/src/query/occupancy.rs | 315 --- vastu-map/src/query/path_planning.rs | 138 -- vastu-map/src/query/raycast.rs | 218 -- vastu-map/src/query/traversability.rs | 911 ++++++++ vastu-map/src/slam/config.rs | 264 +++ vastu-map/src/slam/loop_closure/descriptor.rs | 282 +++ vastu-map/src/slam/loop_closure/detector.rs | 507 +++++ vastu-map/src/slam/loop_closure/graph.rs | 562 +++++ vastu-map/src/slam/loop_closure/mod.rs | 14 + vastu-map/src/slam/matcher.rs | 1012 +++++++++ vastu-map/src/slam/mod.rs | 36 + vastu-map/src/slam/types.rs | 109 + vastu-map/src/submap/config.rs | 216 ++ vastu-map/src/submap/graph.rs | 557 +++++ vastu-map/src/submap/manager.rs | 624 +++++ vastu-map/src/submap/matching.rs | 352 +++ vastu-map/src/submap/mod.rs | 47 + vastu-map/src/submap/types.rs | 375 +++ vastu-map/src/vector_map/README.md | 179 -- vastu-map/src/vector_map/config.rs | 490 ---- vastu-map/src/vector_map/line_store.rs | 291 --- vastu-map/src/vector_map/map.rs | 1394 ------------ vastu-map/src/vector_map/mod.rs | 39 - vastu-map/src/vector_map/yaml.rs | 461 ---- vastu-map/tools/.gitignore | 2 - vastu-map/tools/floorplan_converter.py | 503 ---- vastu-map/tools/houseexpo_converter.py | 390 ---- vastu-map/tools/requirements.txt | 11 - 162 files changed, 19094 insertions(+), 40115 deletions(-) create mode 100644 vastu-map/configs/config.yaml delete mode 100644 vastu-map/configs/default.yaml delete mode 100644 vastu-map/data/scenarios/drift_continuous_curve.yaml delete mode 100644 vastu-map/data/scenarios/drift_long_path.yaml delete mode 100644 vastu-map/data/scenarios/icp_sparse_corridor.yaml delete mode 100644 vastu-map/data/scenarios/loop_closure_test.yaml delete mode 100644 vastu-map/data/scenarios/medium_room_exploration.yaml delete mode 100644 vastu-map/data/scenarios/medium_room_obstacles.yaml delete mode 100644 vastu-map/data/scenarios/medium_room_perimeter.yaml delete mode 100644 vastu-map/data/scenarios/pure_rotation_test.yaml delete mode 100644 vastu-map/data/scenarios/simple_room_straight_line.yaml delete mode 100644 vastu-map/data/scenarios/static_scan_test.yaml delete mode 100644 vastu-map/data/scenarios/straight_line_test.yaml create mode 100644 vastu-map/docs/proposal.md delete mode 100644 vastu-map/examples/exploration/harness/adapters.rs delete mode 100644 vastu-map/examples/exploration/harness/harness.rs delete mode 100644 vastu-map/examples/exploration/harness/metrics.rs delete mode 100644 vastu-map/examples/exploration/harness/mod.rs delete mode 100644 vastu-map/examples/exploration/harness/visualization.rs delete mode 100644 vastu-map/examples/exploration/main.rs create mode 100644 vastu-map/examples/explore_mock.rs create mode 100644 vastu-map/examples/explore_simple.rs create mode 100644 vastu-map/examples/scan_match_test.rs rename vastu-map/{data => }/maps/corridor.pgm (100%) rename vastu-map/{data => }/maps/corridor.yaml (100%) rename vastu-map/{data => }/maps/large_room.pgm (100%) rename vastu-map/{data => }/maps/large_room.yaml (100%) rename vastu-map/{data => }/maps/medium_room.pgm (100%) rename vastu-map/{data => }/maps/medium_room.yaml (100%) rename vastu-map/{data => }/maps/simple_room.pgm (100%) rename vastu-map/{data => }/maps/simple_room.yaml (100%) create mode 100644 vastu-map/scenarios/rotation_360.yaml create mode 100644 vastu-map/scenarios/square_path.yaml create mode 100644 vastu-map/scenarios/straight_line.yaml delete mode 100644 vastu-map/src/core/README.md delete mode 100644 vastu-map/src/core/bounds.rs create mode 100644 vastu-map/src/core/cell.rs delete mode 100644 vastu-map/src/core/math.rs delete mode 100644 vastu-map/src/core/scan.rs create mode 100644 vastu-map/src/core/sensors.rs create mode 100644 vastu-map/src/core/simd.rs delete mode 100644 vastu-map/src/exploration/collision.rs delete mode 100644 vastu-map/src/exploration/history.rs delete mode 100644 vastu-map/src/exploration/path_follower.rs delete mode 100644 vastu-map/src/exploration/planner.rs delete mode 100644 vastu-map/src/exploration/region.rs create mode 100644 vastu-map/src/exploration/state.rs create mode 100644 vastu-map/src/explore/config.rs create mode 100644 vastu-map/src/explore/explorer.rs create mode 100644 vastu-map/src/explore/mod.rs create mode 100644 vastu-map/src/explore/source.rs create mode 100644 vastu-map/src/explore/submap_explorer.rs delete mode 100644 vastu-map/src/extraction/README.md delete mode 100644 vastu-map/src/extraction/corner_detection.rs delete mode 100644 vastu-map/src/extraction/line_fitting.rs delete mode 100644 vastu-map/src/extraction/mod.rs delete mode 100644 vastu-map/src/extraction/ransac_lines.rs delete mode 100644 vastu-map/src/extraction/split_merge/algorithm.rs delete mode 100644 vastu-map/src/extraction/split_merge/config.rs delete mode 100644 vastu-map/src/extraction/split_merge/helpers.rs delete mode 100644 vastu-map/src/extraction/split_merge/mod.rs delete mode 100644 vastu-map/src/extraction/traits.rs delete mode 100644 vastu-map/src/features/README.md delete mode 100644 vastu-map/src/features/corner.rs delete mode 100644 vastu-map/src/features/descriptors.rs delete mode 100644 vastu-map/src/features/feature_set.rs delete mode 100644 vastu-map/src/features/line.rs delete mode 100644 vastu-map/src/features/line_collection.rs delete mode 100644 vastu-map/src/features/mod.rs delete mode 100644 vastu-map/src/features/spatial_index.rs create mode 100644 vastu-map/src/grid/bumper_update.rs create mode 100644 vastu-map/src/grid/cliff_update.rs create mode 100644 vastu-map/src/grid/config.rs create mode 100644 vastu-map/src/grid/lidar_update.rs create mode 100644 vastu-map/src/grid/mod.rs create mode 100644 vastu-map/src/grid/raycaster.rs create mode 100644 vastu-map/src/grid/storage.rs delete mode 100644 vastu-map/src/integration/README.md delete mode 100644 vastu-map/src/integration/association.rs delete mode 100644 vastu-map/src/integration/merger.rs delete mode 100644 vastu-map/src/integration/mod.rs delete mode 100644 vastu-map/src/integration/point_association.rs delete mode 100644 vastu-map/src/integration/refitter.rs delete mode 100644 vastu-map/src/integration/scan_store.rs delete mode 100644 vastu-map/src/integration/spatial_index.rs create mode 100644 vastu-map/src/io/mod.rs create mode 100644 vastu-map/src/io/pgm.rs create mode 100644 vastu-map/src/io/scenario.rs create mode 100644 vastu-map/src/io/svg.rs create mode 100644 vastu-map/src/io/vastu_format.rs delete mode 100644 vastu-map/src/loop_closure/README.md delete mode 100644 vastu-map/src/loop_closure/config.rs delete mode 100644 vastu-map/src/loop_closure/detector.rs delete mode 100644 vastu-map/src/loop_closure/mod.rs delete mode 100644 vastu-map/src/loop_closure/traits.rs delete mode 100644 vastu-map/src/loop_closure/types.rs delete mode 100644 vastu-map/src/matching/README.md delete mode 100644 vastu-map/src/matching/correspondence.rs delete mode 100644 vastu-map/src/matching/gauss_newton.rs delete mode 100644 vastu-map/src/matching/icp/config.rs delete mode 100644 vastu-map/src/matching/icp/matcher.rs delete mode 100644 vastu-map/src/matching/icp/mod.rs delete mode 100644 vastu-map/src/matching/mod.rs delete mode 100644 vastu-map/src/matching/nearest_neighbor.rs delete mode 100644 vastu-map/src/matching/ransac.rs delete mode 100644 vastu-map/src/matching/robust.rs delete mode 100644 vastu-map/src/matching/scratch.rs delete mode 100644 vastu-map/src/matching/traits.rs delete mode 100644 vastu-map/src/motion_model.rs delete mode 100644 vastu-map/src/odometry.rs create mode 100644 vastu-map/src/pathfinding/astar.rs create mode 100644 vastu-map/src/pathfinding/mod.rs create mode 100644 vastu-map/src/pathfinding/smoothing.rs delete mode 100644 vastu-map/src/query/README.md delete mode 100644 vastu-map/src/query/cbvg/clearance.rs delete mode 100644 vastu-map/src/query/cbvg/config.rs delete mode 100644 vastu-map/src/query/cbvg/dijkstra.rs delete mode 100644 vastu-map/src/query/cbvg/graph.rs delete mode 100644 vastu-map/src/query/cbvg/mod.rs delete mode 100644 vastu-map/src/query/cbvg/node.rs delete mode 100644 vastu-map/src/query/cbvg/visibility.rs delete mode 100644 vastu-map/src/query/cbvg/voronoi.rs delete mode 100644 vastu-map/src/query/occupancy.rs delete mode 100644 vastu-map/src/query/path_planning.rs delete mode 100644 vastu-map/src/query/raycast.rs create mode 100644 vastu-map/src/query/traversability.rs create mode 100644 vastu-map/src/slam/config.rs create mode 100644 vastu-map/src/slam/loop_closure/descriptor.rs create mode 100644 vastu-map/src/slam/loop_closure/detector.rs create mode 100644 vastu-map/src/slam/loop_closure/graph.rs create mode 100644 vastu-map/src/slam/loop_closure/mod.rs create mode 100644 vastu-map/src/slam/matcher.rs create mode 100644 vastu-map/src/slam/mod.rs create mode 100644 vastu-map/src/slam/types.rs create mode 100644 vastu-map/src/submap/config.rs create mode 100644 vastu-map/src/submap/graph.rs create mode 100644 vastu-map/src/submap/manager.rs create mode 100644 vastu-map/src/submap/matching.rs create mode 100644 vastu-map/src/submap/mod.rs create mode 100644 vastu-map/src/submap/types.rs delete mode 100644 vastu-map/src/vector_map/README.md delete mode 100644 vastu-map/src/vector_map/config.rs delete mode 100644 vastu-map/src/vector_map/line_store.rs delete mode 100644 vastu-map/src/vector_map/map.rs delete mode 100644 vastu-map/src/vector_map/mod.rs delete mode 100644 vastu-map/src/vector_map/yaml.rs delete mode 100644 vastu-map/tools/.gitignore delete mode 100644 vastu-map/tools/floorplan_converter.py delete mode 100644 vastu-map/tools/houseexpo_converter.py delete mode 100644 vastu-map/tools/requirements.txt diff --git a/.gitignore b/.gitignore index 5b7a131..ec28544 100644 --- a/.gitignore +++ b/.gitignore @@ -57,4 +57,4 @@ analysis/ *.log -results/ +output/ diff --git a/vastu-map/Cargo.toml b/vastu-map/Cargo.toml index b149843..3382dfe 100644 --- a/vastu-map/Cargo.toml +++ b/vastu-map/Cargo.toml @@ -1,35 +1,30 @@ [package] name = "vastu-map" -version = "0.1.0" +version = "0.2.0" edition = "2024" -description = "Feature-based 2D SLAM library with SIMD-friendly data layouts" +description = "Occupancy grid mapping library for robot exploration and navigation" license = "MIT" -[features] -default = [] +# Note: This crate requires nightly Rust for portable_simd feature [dependencies] -rstar = "0.12" # R-tree spatial index log = "0.4" # Logging -rand = "0.9" # RANSAC randomness serde = { version = "1.0", features = ["derive"] } # Config serialization serde_yaml = "0.9" # YAML config files [dev-dependencies] -criterion = "0.8" # Benchmarks env_logger = "0.11" # Test logging approx = "0.5" # Approximate comparisons (tests only) # Simulation dependencies (examples only) sangam-io = { path = "../sangam-io", features = ["mock"] } -svg = "0.18" image = { version = "0.25", default-features = false, features = ["png", "pnm"] } +clap = { version = "4.5", features = ["derive"] } +rand = "0.9" [[example]] -name = "exploration" +name = "explore_mock" +path = "examples/explore_mock.rs" [profile.release] opt-level = 3 lto = true -# codegen-units = 1 # Better optimization, slower compile -# panic = "abort" # Smaller binary, no unwinding overhead -# strip = true # Strip debug symbols for smaller binary diff --git a/vastu-map/configs/config.yaml b/vastu-map/configs/config.yaml new file mode 100644 index 0000000..fe59c6f --- /dev/null +++ b/vastu-map/configs/config.yaml @@ -0,0 +1,121 @@ +# VastuMap Occupancy Grid Configuration +# Units: meters (m), radians (rad), cells (n) + +# ============================================================================ +# Grid Configuration +# ============================================================================ +grid: + # Cell resolution in meters per cell + resolution: 0.025 # 2.5cm cells + + # Initial grid dimensions (cells) + initial_width: 300 + initial_height: 300 + + # Grid origin mode: "center" or "corner" + origin_mode: center + + # Custom origin offset (only used if origin_mode: custom) + origin_x: 0.0 + origin_y: 0.0 + + # Enable automatic grid expansion + auto_expand: true + +# ============================================================================ +# Sensor Configuration +# ============================================================================ +sensor: + # LiDAR configuration + lidar: + # Sensor offset from robot center (x forward, y left) + offset_x: -0.110 + offset_y: 0.0 + + # Range limits + min_range: 0.15 + max_range: 8.0 + + # Robot geometry + robot: + radius: 0.17 # CRL-200S physical radius + + # Cliff sensor positions (robot frame) + cliff_sensors: + left_side: [0.12, 0.10] + left_front: [0.15, 0.05] + right_front: [0.15, -0.05] + right_side: [0.12, -0.10] + + # Bumper zones (angles from center in radians) + bumper: + left_angle: 0.5 # ~30 degrees + right_angle: -0.5 + +# ============================================================================ +# Query Configuration +# ============================================================================ +query: + # Frontier detection + frontier: + min_frontier_size: 5 # Minimum cells for valid frontier + use_8_connectivity: true # Use 8-connected grid (vs 4-connected) + + # Traversability checking + traversability: + safety_margin: 0.05 # Extra clearance around robot (m) + +# ============================================================================ +# Pathfinding Configuration +# ============================================================================ +pathfinding: + # A* algorithm settings + astar: + allow_diagonal: true # Enable 8-directional movement + diagonal_cost: 1.414 # sqrt(2) cost for diagonal moves + max_iterations: 100000 # Max nodes to expand + clearance_weight: 0.0 # Penalty for paths near obstacles (0=disabled) + + # Path smoothing + smoothing: + enabled: true + los_step_size: 0.05 # Line-of-sight check step (m) + max_iterations: 100 # Midpoint smoothing iterations + +# ============================================================================ +# Exploration Configuration +# ============================================================================ +exploration: + # Frontier selection + min_frontier_size: 5 + frontier_reached_threshold: 0.3 # Distance to consider frontier reached (m) + waypoint_threshold: 0.1 # Distance for waypoint reached (m) + + # Planning + max_plan_distance: 5.0 # Maximum planning distance (m) + replan_interval: 1.0 # Seconds between replans when blocked + + # Failure handling + max_consecutive_failures: 5 + + # Path preferences + path_clearance_weight: 0.0 + smooth_paths: true + + # Recovery behavior + recovery: + turn_angle: 0.785 # 45 degrees in radians + backup_distance: 0.1 # Backup distance (m) + +# ============================================================================ +# Map Persistence +# ============================================================================ +persistence: + # Output format: "vastu" (binary), "pgm" (ROS), "both" + output_format: both + + # Output directory (relative to working directory) + output_dir: ./output + + # Auto-save interval during exploration (seconds, 0=disabled) + auto_save_interval: 30 diff --git a/vastu-map/configs/default.yaml b/vastu-map/configs/default.yaml deleted file mode 100644 index ab7ffe6..0000000 --- a/vastu-map/configs/default.yaml +++ /dev/null @@ -1,268 +0,0 @@ -# VastuMap YAML Configuration -# All values must be specified (no preset inheritance) -# Units: meters (m), radians (rad), counts (n) - -# ============================================================================ -# Line Extraction (Split-Merge Algorithm) -# ============================================================================ -extraction: - # Maximum perpendicular distance for splitting (m) - split_threshold: 0.05 - # Minimum points to form a valid line - min_points: 5 - # Minimum line length to keep (m) - min_length: 0.05 - # Maximum gap between consecutive points (m) - max_point_gap: 0.10 - # Merge threshold for collinear segments (m) - merge_threshold: 0.05 - # Coefficient for range-adaptive threshold scaling - adaptive_range_scale: 0.03 - -# ============================================================================ -# RANSAC Line Extraction (for hybrid mode) -# ============================================================================ -ransac_extraction: - # Number of RANSAC iterations per line - iterations: 100 - # Inlier distance threshold (m) - inlier_threshold: 0.02 - # Minimum inliers for valid line - min_inliers: 5 - # Minimum line length (m) - min_line_length: 0.1 - # Maximum gap between inliers (m) - prevents connecting walls across doorways - max_point_gap: 0.10 - # Maximum lines to extract (0 = unlimited) - max_lines: 0 - # Random seed (0 = random) - seed: 0 - -# Whether to use RANSAC + Split-Merge hybrid extraction -use_hybrid_extraction: true - -# ============================================================================ -# Corner Detection -# ============================================================================ -corner: - # Minimum angle between lines to form corner (rad) - min_angle: 0.524 # 30 degrees - # Maximum angle between lines to form corner (rad) - max_angle: 2.618 # 150 degrees - # Maximum endpoint distance for corner detection (m) - max_endpoint_distance: 0.05 - # Curvature detection window size (points on each side) - curvature_window_size: 3 - # Minimum curvature angle for corner candidate (rad) - curvature_threshold: 0.785 # 45 degrees - # Merge distance for deduplicating corners (m) - merge_distance: 0.10 - # Non-maximum suppression radius (m) - nms_radius: 0.15 - -# ============================================================================ -# ICP Scan Matching -# ============================================================================ -matching: - # Maximum ICP iterations - max_iterations: 30 - # Convergence threshold for pose change - convergence_threshold: 0.0001 - # Maximum correspondence distance (m) - max_correspondence_distance: 0.5 - - # Robust cost function for outlier handling - # Options: None, Huber, Cauchy, Tukey, GemanMcClure - robust_cost: - type: Huber - delta: 0.03 - - # Use Iteratively Reweighted Least Squares - use_irls: true - # Minimum correspondences required - min_correspondences: 10 - # Minimum match confidence to accept - min_confidence: 0.3 - # Use RANSAC for initial pose estimate - use_ransac_init: true - # Use batch (SIMD) correspondence search - use_batch_search: true - - # Lidar noise model - noise_model: - sigma_base: 0.01 - sigma_range: 0.001 - - # Coarse search when confidence is low - use_coarse_search: true - coarse_search_confidence_threshold: 0.5 - - # Multi-resolution ICP (coarse-to-fine) - multi_resolution: - enabled: true - levels: 3 - subsample_factor: 2 - iterations_per_level: [3, 5, 10] - -# ============================================================================ -# Line Association (Map Integration) -# ============================================================================ -association: - # Maximum perpendicular distance for association (m) - max_perpendicular_distance: 0.15 - # Maximum angular difference (rad) - max_angle_difference: 0.2 - # Minimum overlap ratio for valid association - min_overlap_ratio: 0.3 - # Maximum gap between line endpoints (m) - max_endpoint_gap: 0.3 - # Weight for distance in quality scoring - distance_weight: 10.0 - # Weight for angle in quality scoring - angle_weight: 5.0 - -# ============================================================================ -# Line Merging -# ============================================================================ -merger: - # Weight given to existing map line (0-1) - map_weight: 0.8 - # Whether to extend line endpoints - extend_endpoints: true - # Maximum extension per observation (m) - max_extension: 0.5 - # Minimum extension to apply (m) - min_extension: 0.05 - -# ============================================================================ -# Frontier Detection (Gap-Based) -# ============================================================================ -frontier: - # Maximum endpoint distance for "connected" (m) - connection_distance: 0.3 - # Minimum distance from robot for valid frontier (m) - min_distance_from_robot: 1.0 - # Safety margin: viewpoint offset from wall (m) - safety_margin: 0.4 - # Gap probe distance: how far to probe for unexplored gaps (m) - gap_probe_distance: 2.0 - # Robot radius for viewpoint validation (m) - robot_radius: 0.16 - # Maximum iterations for viewpoint adjustment - max_viewpoint_iterations: 5 - # Viewpoint adjustment step size (m) - viewpoint_step: 0.1 - # Minimum gap width for safe robot passage (m) - # Gaps narrower than this are ignored (robot can't fit) - min_gap_width: 0.4 - # Maximum gap width to consider as a single gap (m) - # Allow gaps across rooms (up to 10m) - max_gap_width: 10.0 - # CBVG-based frontier detection parameters - # Minimum uncovered angle for a node to be a frontier (radians) - # 1.75 rad = 100 degrees - min_frontier_angle: 1.75 - # Maximum distance to check for walls when detecting frontiers (m) - max_wall_distance: 0.5 - -# ============================================================================ -# Occupancy Queries -# ============================================================================ -occupancy: - # Distance threshold for "occupied" (m) - obstacle_distance: 0.1 - # Distance threshold for "known" (m) - known_distance: 2.0 - -# ============================================================================ -# Clearance-Based Visibility Graph (CBVG) - Path Planning -# ============================================================================ -cbvg: - # Minimum clearance from walls in open areas (m) - min_clearance: 0.3 - # Minimum clearance in narrow passages (m) - # Used when corridor is too narrow for full clearance - min_clearance_narrow: 0.18 - # Sample resolution along Voronoi edges (m) - voronoi_sample_step: 0.1 - # Maximum edge length between nodes (m) - # Increased from 0.8 to 1.5 for better graph connectivity during exploration - max_edge_length: 1.5 - # Interval for placing coverage nodes in open areas (m) - # Ensures nodes exist even in areas without medial axis - coverage_node_interval: 0.35 - # Distance threshold for merging nearby nodes (m) - node_merge_distance: 0.15 - # Maximum nodes in the graph - max_nodes: 500 - # Clearance sample step for edge validation (m) - edge_clearance_step: 0.05 - # Wall sampling step for Voronoi approximation (m) - wall_sample_step: 0.1 - # Minimum clique size for merging during optimization - # Cliques (mutually visible node groups) smaller than this are kept separate - min_clique_size_for_merge: 3 - # Number of lidar scans between clique optimization runs - # Set to 1 to optimize after every scan for best graph quality - optimize_interval: 1 - # Maximum range for CBVG node creation (m) - # Nodes are only placed up to this distance from robot - # (wall detection still uses full lidar range) - max_lidar_range: 1.0 - -# ============================================================================ -# Loop Closure Detection -# ============================================================================ -loop_closure: - # Minimum travel distance before checking loop closure (m) - min_travel_distance: 5.0 - # Maximum descriptor distance for candidates - max_descriptor_distance: 0.5 - # Minimum ICP confidence to accept closure - min_verification_confidence: 0.6 - # Distance between keyframes (m) - keyframe_interval: 2.0 - # Radius for computing descriptors (m) - descriptor_radius: 2.0 - # Maximum candidates to verify per keyframe - max_candidates_to_verify: 3 - # Minimum lines for valid keyframe - min_lines_for_keyframe: 3 - # Minimum corners for valid keyframe - min_corners_for_keyframe: 2 - - # ICP config for loop closure verification (nested) - icp_config: - max_iterations: 30 - convergence_threshold: 0.0001 - max_correspondence_distance: 0.5 - robust_cost: - type: Huber - delta: 0.03 - use_irls: true - min_correspondences: 10 - min_confidence: 0.3 - use_ransac_init: true - use_batch_search: true - noise_model: - sigma_base: 0.01 - sigma_range: 0.001 - use_coarse_search: true - coarse_search_confidence_threshold: 0.5 - multi_resolution: - enabled: true - levels: 3 - subsample_factor: 2 - iterations_per_level: [3, 5, 10] - -# ============================================================================ -# Global Settings -# ============================================================================ -# Minimum match confidence to use scan matching (vs. odometry) -min_match_confidence: 0.3 - -# Whether to update map with new observations -mapping_enabled: true - -# Whether loop closure detection is enabled -loop_closure_enabled: true diff --git a/vastu-map/data/scenarios/drift_continuous_curve.yaml b/vastu-map/data/scenarios/drift_continuous_curve.yaml deleted file mode 100644 index b9e7a6f..0000000 --- a/vastu-map/data/scenarios/drift_continuous_curve.yaml +++ /dev/null @@ -1,43 +0,0 @@ -# Continuous Curve Drift Test -# -# Tests ICP behavior during continuous curved motion. -# Arc motion stresses ICP because the robot is constantly rotating -# while also translating, making scan-to-map matching more challenging. -# -# Expected behavior: -# - ICP should maintain convergence during curves -# - Average iterations may be higher than straight-line motion -# - No convergence failures -# -# Map: 8m x 8m medium room (using existing map) -# Path: Series of arcs creating an S-curve pattern - -name: "drift_continuous_curve" -map: "../maps/medium_room.yaml" - -start_pose: - x: 4.0 - y: 2.0 - theta: 1.5708 # Facing +Y - -path: - # Arc right (left wheel travels more) - - {left: 1.0, right: 0.7, speed: 0.4} - - # Arc left (right wheel travels more) - - {left: 0.7, right: 1.0, speed: 0.4} - - # Arc right - - {left: 1.0, right: 0.7, speed: 0.4} - - # Arc left - - {left: 0.7, right: 1.0, speed: 0.4} - - # Tighter arc right - - {left: 0.8, right: 0.4, speed: 0.4} - - # Tighter arc left - - {left: 0.4, right: 0.8, speed: 0.4} - - # Final straight segment - - {left: 0.5, right: 0.5, speed: 0.4} diff --git a/vastu-map/data/scenarios/drift_long_path.yaml b/vastu-map/data/scenarios/drift_long_path.yaml deleted file mode 100644 index 3ecea4c..0000000 --- a/vastu-map/data/scenarios/drift_long_path.yaml +++ /dev/null @@ -1,57 +0,0 @@ -# Long Path Drift Test -# -# Tests cumulative drift over extended motion (20m total path). -# In a large room, the robot makes a zigzag pattern to cover distance -# while staying within bounds. -# -# Expected behavior: -# - Position error should grow slowly (< 0.75% of distance traveled) -# - After 20m, error should be < 15cm -# - ICP should maintain convergence throughout -# -# Map: 12m x 12m large empty room -# Path: Zigzag covering ~20m total distance - -name: "drift_long_path" -map: "../maps/large_room.yaml" - -start_pose: - x: 2.0 - y: 6.0 - theta: 0.0 # Facing +X - -path: - # Leg 1: 4m forward (now at x=6, y=6) - - {left: 4.0, right: 4.0, speed: 0.5} - - # Turn 90 degrees CCW (now facing +Y) - - {left: -0.183, right: 0.183, speed: 0.4} - - # Leg 2: 4m forward (now at x=6, y=10) - - {left: 4.0, right: 4.0, speed: 0.5} - - # Turn 90 degrees CW (now facing +X) - - {left: 0.183, right: -0.183, speed: 0.4} - - # Leg 3: 4m forward (now at x=10, y=10) - - {left: 4.0, right: 4.0, speed: 0.5} - - # Turn 90 degrees CW (now facing -Y) - - {left: 0.183, right: -0.183, speed: 0.4} - - # Leg 4: 2m forward (now at x=10, y=8) - - {left: 2.0, right: 2.0, speed: 0.5} - - # Turn 90 degrees CW (now facing -X) - - {left: 0.183, right: -0.183, speed: 0.4} - - # Leg 5: 4m forward (now at x=6, y=8) - - {left: 4.0, right: 4.0, speed: 0.5} - - # Turn 90 degrees CCW (now facing -Y) - - {left: -0.183, right: 0.183, speed: 0.4} - - # Leg 6: 2m forward (final position at x=6, y=6) - - {left: 2.0, right: 2.0, speed: 0.5} - - # Total: 4+4+4+2+4+2 = 20m of travel diff --git a/vastu-map/data/scenarios/icp_sparse_corridor.yaml b/vastu-map/data/scenarios/icp_sparse_corridor.yaml deleted file mode 100644 index 10971e4..0000000 --- a/vastu-map/data/scenarios/icp_sparse_corridor.yaml +++ /dev/null @@ -1,27 +0,0 @@ -# ICP Sparse Corridor Test -# -# Tests ICP behavior with only parallel walls visible (degenerate geometry). -# In a narrow corridor, the robot can see both walls but rotation is -# underconstrained because all features are parallel lines. -# -# Expected behavior: -# - ICP should detect high condition number (degenerate case) -# - Position along corridor may drift -# - Rotation estimation may be unstable -# -# Map: 1m x 10m corridor -# Path: Move 3m down the corridor - -name: "icp_sparse_corridor" -map: "../maps/corridor.yaml" - -start_pose: - x: 0.5 # Center of 1m-wide corridor - y: 1.0 # Near one end - theta: 1.5708 # Facing +Y (down the corridor) - -path: - # Move 3m down the corridor in 1m segments - - {left: 1.0, right: 1.0, speed: 0.4} - - {left: 1.0, right: 1.0, speed: 0.4} - - {left: 1.0, right: 1.0, speed: 0.4} diff --git a/vastu-map/data/scenarios/loop_closure_test.yaml b/vastu-map/data/scenarios/loop_closure_test.yaml deleted file mode 100644 index e58ca41..0000000 --- a/vastu-map/data/scenarios/loop_closure_test.yaml +++ /dev/null @@ -1,47 +0,0 @@ -# Loop Closure Test (Square Trajectory) -# -# Robot traces a square path and returns to starting position. -# This tests cumulative drift and loop closure accuracy. -# -# Starting at (3.5, 2.5) in 8m x 8m room, trace a 2m square: -# - Move 2m forward -# - Turn 90 degrees CCW -# - Repeat 4 times to return to start -# -# Note: Start position chosen to avoid obstacles: -# - Table at (2, 2), Pillar at (6, 2), L-shape at (5, 5), Small box at (2, 6) -# -# Expected: Final pose within 15cm/5 degrees of starting pose - -name: "loop_closure_square" -map: "../maps/medium_room.yaml" - -start_pose: - x: 3.5 - y: 2.5 - theta: 0.0 # Facing right (+X) - -path: - # Leg 1: Move forward 2m (now at x=5.5, y=2.5) - - {left: 2.0, right: 2.0, speed: 0.4} - - # Turn 90 degrees CCW (now facing +Y) - - {left: -0.183, right: 0.183, speed: 0.4} - - # Leg 2: Move forward 2m (now at x=5.5, y=4.5) - - {left: 2.0, right: 2.0, speed: 0.4} - - # Turn 90 degrees CCW (now facing -X) - - {left: -0.183, right: 0.183, speed: 0.4} - - # Leg 3: Move forward 2m (now at x=3.5, y=4.5) - - {left: 2.0, right: 2.0, speed: 0.4} - - # Turn 90 degrees CCW (now facing -Y) - - {left: -0.183, right: 0.183, speed: 0.4} - - # Leg 4: Move forward 2m (back to x=3.5, y=2.5) - - {left: 2.0, right: 2.0, speed: 0.4} - - # Final turn to restore original orientation (facing +X) - - {left: -0.183, right: 0.183, speed: 0.4} diff --git a/vastu-map/data/scenarios/medium_room_exploration.yaml b/vastu-map/data/scenarios/medium_room_exploration.yaml deleted file mode 100644 index ad2a4c0..0000000 --- a/vastu-map/data/scenarios/medium_room_exploration.yaml +++ /dev/null @@ -1,42 +0,0 @@ -# Medium Room Exploration Test -# -# Starting from center (4m, 4m), explore the room with rotation -# and forward motion to discover all walls and obstacles. -# -# Map: 8m x 8m room with 4 obstacles -# Wheel base: 0.233m - -name: "medium_room_exploration" -map: "../maps/medium_room.yaml" - -start_pose: - x: 4.0 - y: 4.0 - theta: 0.0 - -path: - # Initial 360 degree rotation to scan surroundings - # 360 degrees = 2*pi * 0.233 / 2 = 0.732m per wheel - - {left: -0.732, right: 0.732, speed: 0.4} - - # Move forward 1m - - {left: 1.0, right: 1.0, speed: 0.4} - - # Turn right 90 degrees and move - # 90 degrees = pi/2 * 0.233 / 2 = 0.183m - - {left: 0.183, right: -0.183, speed: 0.4} - - {left: 1.5, right: 1.5, speed: 0.4} - - # Turn right 90 degrees and move back - - {left: 0.183, right: -0.183, speed: 0.4} - - {left: 1.0, right: 1.0, speed: 0.4} - - # Another rotation to scan - - {left: -0.732, right: 0.732, speed: 0.4} - - # Explore other direction - - {left: 0.183, right: -0.183, speed: 0.4} - - {left: 2.0, right: 2.0, speed: 0.4} - - # Final rotation - - {left: -0.732, right: 0.732, speed: 0.4} diff --git a/vastu-map/data/scenarios/medium_room_obstacles.yaml b/vastu-map/data/scenarios/medium_room_obstacles.yaml deleted file mode 100644 index 1fe79e8..0000000 --- a/vastu-map/data/scenarios/medium_room_obstacles.yaml +++ /dev/null @@ -1,52 +0,0 @@ -# Medium Room Obstacle Navigation Test -# -# Navigate around obstacles to test obstacle detection. -# Visit areas near each obstacle. -# -# Obstacles: -# - Table at (2m, 2m) -# - Pillar at (6m, 2m) -# - L-shape at (5m, 5m) -# - Small box at (2m, 6m) - -name: "medium_room_obstacles" -map: "../maps/medium_room.yaml" - -start_pose: - x: 4.0 - y: 4.0 - theta: 0.0 - -path: - # Scan from center - - {left: -0.732, right: 0.732, speed: 0.4} - - # Move toward table area - - {left: -0.366, right: 0.366, speed: 0.4} # Turn 180 degrees - - {left: 1.5, right: 1.5, speed: 0.4} # Move toward (2.5, 4) - - {left: -0.183, right: 0.183, speed: 0.4} # Turn left 90 degrees - - {left: 1.0, right: 1.0, speed: 0.4} # Move toward table (2.5, 3) - - # Scan near table - - {left: -0.732, right: 0.732, speed: 0.4} - - # Navigate to pillar area - - {left: 0.183, right: -0.183, speed: 0.4} # Turn right 90 degrees - - {left: 3.5, right: 3.5, speed: 0.4} # Move across to pillar side (6, 3) - - # Scan near pillar - - {left: -0.732, right: 0.732, speed: 0.4} - - # Move up toward L-shape - - {left: -0.183, right: 0.183, speed: 0.4} # Turn left 90 degrees - - {left: 2.0, right: 2.0, speed: 0.4} # Move up to (6, 5) - - # Scan near L-shape - - {left: -0.732, right: 0.732, speed: 0.4} - - # Navigate to small box area - - {left: 0.366, right: -0.366, speed: 0.4} # Turn right 180 degrees - - {left: 2.5, right: 2.5, speed: 0.4} # Move toward small box (3.5, 5) - - # Final scan - - {left: -0.732, right: 0.732, speed: 0.4} diff --git a/vastu-map/data/scenarios/medium_room_perimeter.yaml b/vastu-map/data/scenarios/medium_room_perimeter.yaml deleted file mode 100644 index a2994e1..0000000 --- a/vastu-map/data/scenarios/medium_room_perimeter.yaml +++ /dev/null @@ -1,46 +0,0 @@ -# Medium Room Perimeter Test -# -# Starting near bottom-left, trace along the perimeter of the room -# to detect all outer walls. -# -# Map: 8m x 8m room -# Path: Rectangle around the room (keeping 1m from walls) - -name: "medium_room_perimeter" -map: "../maps/medium_room.yaml" - -start_pose: - x: 1.0 - y: 1.0 - theta: 0.0 # Facing right (+X) - -path: - # Initial scan - - {left: -0.732, right: 0.732, speed: 0.4} - - # Side 1: Move right along bottom (6m) - - {left: 6.0, right: 6.0, speed: 0.5} - - # Turn left 90 degrees (CCW) - - {left: -0.183, right: 0.183, speed: 0.4} - - # Side 2: Move up along right side (6m) - - {left: 6.0, right: 6.0, speed: 0.5} - - # Turn left 90 degrees - - {left: -0.183, right: 0.183, speed: 0.4} - - # Side 3: Move left along top (6m) - - {left: 6.0, right: 6.0, speed: 0.5} - - # Turn left 90 degrees - - {left: -0.183, right: 0.183, speed: 0.4} - - # Side 4: Move down along left side (6m) - - {left: 6.0, right: 6.0, speed: 0.5} - - # Turn left 90 degrees (back to original heading) - - {left: -0.183, right: 0.183, speed: 0.4} - - # Final scan - - {left: -0.732, right: 0.732, speed: 0.4} diff --git a/vastu-map/data/scenarios/pure_rotation_test.yaml b/vastu-map/data/scenarios/pure_rotation_test.yaml deleted file mode 100644 index d005cac..0000000 --- a/vastu-map/data/scenarios/pure_rotation_test.yaml +++ /dev/null @@ -1,33 +0,0 @@ -# Pure Rotation Test (360 degrees) -# -# Robot rotates in place without translation. -# This tests orientation estimation accuracy in isolation. -# -# Starting in center of room, perform 4 consecutive 90-degree turns -# to complete a full 360-degree rotation. -# -# Expected: Final orientation within 5 degrees of starting orientation (0 rad) - -name: "pure_rotation_360" -map: "../maps/medium_room.yaml" - -start_pose: - x: 4.0 - y: 4.0 - theta: 0.0 # Facing right (+X) - -path: - # Each 90-degree turn requires opposite wheel motions - # turn_dist = wheel_base * theta / 2 = 0.233 * (pi/2) / 2 = 0.183m - - # 90 degrees CCW (turn left) - - {left: -0.183, right: 0.183, speed: 0.4} - - # Another 90 degrees CCW (now at 180 degrees) - - {left: -0.183, right: 0.183, speed: 0.4} - - # Another 90 degrees CCW (now at 270 degrees) - - {left: -0.183, right: 0.183, speed: 0.4} - - # Final 90 degrees CCW (back to 0 degrees) - - {left: -0.183, right: 0.183, speed: 0.4} diff --git a/vastu-map/data/scenarios/simple_room_straight_line.yaml b/vastu-map/data/scenarios/simple_room_straight_line.yaml deleted file mode 100644 index a280739..0000000 --- a/vastu-map/data/scenarios/simple_room_straight_line.yaml +++ /dev/null @@ -1,20 +0,0 @@ -# Simple Room Straight Line Test -# -# Robot moves straight forward in a simple square room (no obstacles). -# This isolates the feature extraction issue from obstacle interference. -# -# Room: 3m x 3m simple square room -# Starting in center, facing +X (theta=0) -# Move 0.5m forward in a straight line - -name: "simple_room_straight_line" -map: "../maps/simple_room.yaml" - -start_pose: - x: 1.5 - y: 1.5 - theta: 0.0 # Facing right (+X) - -path: - # Move forward 0.5m (both wheels equal distance = straight line) - - {left: 0.5, right: 0.5, speed: 0.4} diff --git a/vastu-map/data/scenarios/static_scan_test.yaml b/vastu-map/data/scenarios/static_scan_test.yaml deleted file mode 100644 index 2e6814c..0000000 --- a/vastu-map/data/scenarios/static_scan_test.yaml +++ /dev/null @@ -1,24 +0,0 @@ -# Static Scan Test -# -# Robot stays stationary for 5 seconds while continuously scanning. -# Uses the stop command for clean static observation. -# -# At 5Hz lidar rate, this produces ~25 scans. -# Expected: Position error < 10cm, orientation error < 5 degrees -# -# Use this to verify: -# - Feature extraction consistency -# - ICP convergence behavior -# - Map building accuracy without motion - -name: "static_scan_5s" -map: "../maps/medium_room.yaml" - -start_pose: - x: 4.0 - y: 4.0 - theta: 0.0 # Facing right (+X), center of 8m x 8m room - -path: - # Stay stationary for 5 seconds - - {stop: 5.0} diff --git a/vastu-map/data/scenarios/straight_line_test.yaml b/vastu-map/data/scenarios/straight_line_test.yaml deleted file mode 100644 index 15c5ef9..0000000 --- a/vastu-map/data/scenarios/straight_line_test.yaml +++ /dev/null @@ -1,19 +0,0 @@ -# Simple Straight Line Test -# -# Robot moves straight forward only, no rotation. -# This isolates any orientation issues in line extraction. -# -# Starting in center of room, facing +X (theta=0) -# Move 1m forward in a straight line - -name: "straight_line_test" -map: "../maps/medium_room.yaml" - -start_pose: - x: 2.0 - y: 4.0 - theta: 0.0 # Facing right (+X) - -path: - # Move forward 1m (both wheels equal distance = straight line) - - {left: 1.0, right: 1.0, speed: 0.4} diff --git a/vastu-map/docs/proposal.md b/vastu-map/docs/proposal.md new file mode 100644 index 0000000..c67691d --- /dev/null +++ b/vastu-map/docs/proposal.md @@ -0,0 +1,594 @@ +# Vastu-Map Accuracy Improvement Proposal + +## Overview + +This document outlines remaining architectural and algorithmic gaps between Vastu-Map and Google Cartographer, with proposed solutions to achieve comparable accuracy for 2D SLAM on embedded platforms. + +**Goal**: Improve scan-to-map matching accuracy while maintaining exploration-friendly occupancy grid representation. + +**Constraints**: +- ARM Cortex-A7 target (Allwinner A33) +- No external optimizer dependencies (Ceres, g2o) +- Real-time operation at 5Hz lidar rate +- Memory budget: <50MB for map storage + +--- + +## Completed Items + +The following features from the original proposal have been fully implemented: + +### Submap Architecture (Gap 1 - COMPLETED) + +Full submap architecture with state machine lifecycle: +- `Submap` struct with origin, local grid, raw scan storage +- `SubmapManager` with Active → Filling → Finalized state machine +- Overlap scan handling during submap transitions +- Lazy global grid recomposition with dirty flag caching +- Raw scan storage for pose graph adjustment + +**Location**: `src/submap/` (types.rs, manager.rs) + +### Loop Closure Detection (COMPLETED) + +LiDAR-IRIS descriptor-based loop closure detection: +- Descriptor computation and matching +- Pose graph structure with odometry and loop edges + +**Location**: `src/slam/loop_closure/` + +### SIMD Optimizations (PARTIAL - Foundation Complete) + +Basic SIMD infrastructure using `std::simd`: +- `f32x4` batch point transformations +- SoA grid storage with `u8x16` vectorization +- ARM NEON / x86 SSE portable implementation + +**Location**: `src/core/simd.rs`, `src/grid/storage.rs` + +### Gauss-Newton Refinement (PARTIAL) + +Scan matching with Cartographer-style prior constraints: +- Prior weights for translation (500.0) and rotation (20.0) +- Fixed LM damping (λ=1e-3) + +**Location**: `src/slam/matcher.rs` + +--- + +## Remaining Gaps (Priority Order) + +### Priority 1: Motion Filter + +**Current State**: +Every lidar scan triggers scan matching and grid update. + +**Problem**: +- Stationary robot accumulates redundant observations +- Wasted computation on identical poses +- Submap size varies with robot speed + +**Cartographer's Approach**: +Motion filter gates scan insertion: +- `max_time_seconds`: 5s (insert at least every 5s) +- `max_distance_meters`: 0.2m +- `max_angle_radians`: 0.17 rad (~10°) + +**Proposed Solution**: + +```rust +pub struct MotionFilter { + last_pose: Pose2D, + last_time: Instant, + config: MotionFilterConfig, +} + +pub struct MotionFilterConfig { + pub max_time: Duration, // 5s + pub max_distance: f32, // 0.2m + pub max_angle: f32, // 0.17 rad +} + +impl MotionFilter { + pub fn should_process(&mut self, pose: Pose2D, time: Instant) -> bool { + let dt = time.duration_since(self.last_time); + let dist = (pose.x - self.last_pose.x).hypot(pose.y - self.last_pose.y); + let angle = (pose.theta - self.last_pose.theta).abs(); + + let should = dt > self.config.max_time + || dist > self.config.max_distance + || angle > self.config.max_angle; + + if should { + self.last_pose = pose; + self.last_time = time; + } + + should + } +} +``` + +**Implementation Effort**: Low (1-2 hours) +**Impact**: Reduces CPU usage, consistent submap sizes + +--- + +### Priority 2: Log-Odds Probability Cells + +**Current State**: +Cells use a `confidence: u8` counter with priority-based type overwriting: +```rust +pub struct Cell { + pub cell_type: CellType, // Unknown/Floor/Wall/Cliff/Bump + pub confidence: u8, // 0-255, saturates + pub observation_count: u8, + pub swept: bool, +} +``` + +**Problem**: +- Counter saturates quickly, losing uncertainty information +- No principled way to combine conflicting observations +- Priority system prevents dynamic obstacle handling + +**Cartographer's Approach**: +Proper Bayesian update using log-odds representation: +``` +P(occupied | observations) ∝ P(observations | occupied) × P(occupied) +``` + +In log-odds space, multiplication becomes addition: +``` +L(x) = log(P(x) / (1 - P(x))) +L_new = L_old + L_observation +``` + +**Proposed Solution**: + +```rust +pub struct Cell { + /// Log-odds occupancy (fixed-point: actual = value / 100) + /// Range: [-200, 200] maps to probability [0.12, 0.88] + pub log_odds: i16, + + /// Semantic type (Wall, Floor, Cliff, Bump) + pub cell_type: CellType, + + /// Last observation timestamp (for decay) + pub last_seen: u16, +} + +impl Cell { + // Observation log-odds (tunable parameters) + const L_HIT: i16 = 70; // P=0.67 → confident occupied + const L_MISS: i16 = -30; // P=0.43 → slight evidence of free + const L_MIN: i16 = -200; // P=0.12 → very likely free + const L_MAX: i16 = 200; // P=0.88 → very likely occupied + + pub fn apply_hit(&mut self) { + self.log_odds = (self.log_odds.saturating_add(Self::L_HIT)) + .clamp(Self::L_MIN, Self::L_MAX); + self.cell_type = CellType::Wall; + } + + pub fn apply_miss(&mut self) { + self.log_odds = (self.log_odds.saturating_add(Self::L_MISS)) + .clamp(Self::L_MIN, Self::L_MAX); + if self.log_odds < 0 { + self.cell_type = CellType::Floor; + } + } + + pub fn probability(&self) -> f32 { + let l = self.log_odds as f32 / 100.0; + 1.0 / (1.0 + (-l).exp()) + } + + pub fn is_occupied(&self) -> bool { + self.log_odds > 50 // P > 0.62 + } + + pub fn is_free(&self) -> bool { + self.log_odds < -50 // P < 0.38 + } +} +``` + +**Migration Path**: +1. Add `log_odds: i16` to GridStorage (SoA layout already supports this) +2. Update `apply_observation()` to use log-odds arithmetic +3. Keep `cell_type` for semantic information (Cliff, Bump sensors) +4. Update scan matching scoring to use probability values + +**Implementation Effort**: Medium (4-6 hours) +**Impact**: Better uncertainty handling, dynamic obstacle support, thinner walls + +--- + +### Priority 3: Adaptive Levenberg-Marquardt + +**Current State**: +Gauss-Newton with fixed LM damping (λ=1e-3) in both scan matching and pose graph: +```rust +// src/slam/matcher.rs line 273 +let damping = self.config.gn_damping; // Fixed 1e-3 +``` + +**Problem**: +- Fixed damping may be too aggressive (slow convergence) or too weak (overshoot) +- No step acceptance/rejection logic +- Convergence not guaranteed + +**Cartographer's Approach**: +Ceres Solver with trust-region method (automatic damping adjustment). + +**Proposed Solution** (without Ceres dependency): + +```rust +pub struct AdaptiveLM { + lambda: f64, + lambda_factor: f64, + min_lambda: f64, + max_lambda: f64, +} + +impl AdaptiveLM { + pub fn new() -> Self { + Self { + lambda: 1e-3, + lambda_factor: 10.0, + min_lambda: 1e-7, + max_lambda: 1e7, + } + } + + pub fn step( + &mut self, + current_cost: f64, + jacobian: &Matrix3xN, + residuals: &VectorN, + ) -> (Vector3, StepResult) { + let jtj = jacobian.transpose() * jacobian; + let jtr = jacobian.transpose() * residuals; + + loop { + // Add damping to diagonal + let mut damped = jtj.clone(); + damped[(0,0)] += self.lambda; + damped[(1,1)] += self.lambda; + damped[(2,2)] += self.lambda; + + // Solve for step + let delta = solve_3x3(&damped, &(-jtr)); + + // Predict cost reduction using linear model + let predicted_reduction = -delta.dot(&jtr) + - 0.5 * delta.dot(&(jtj * &delta)); + + if predicted_reduction > 0.0 { + // Step looks promising + return (delta, StepResult::TryStep { + predicted_reduction, + lambda_on_success: (self.lambda / self.lambda_factor) + .max(self.min_lambda), + lambda_on_failure: (self.lambda * self.lambda_factor) + .min(self.max_lambda), + }); + } + + // Predicted to increase cost, increase damping + self.lambda = (self.lambda * self.lambda_factor).min(self.max_lambda); + + if self.lambda >= self.max_lambda { + return (Vector3::zeros(), StepResult::Converged); + } + } + } + + pub fn accept_step(&mut self, actual_reduction: f64, predicted_reduction: f64) { + let rho = actual_reduction / predicted_reduction; + + if rho > 0.25 { + // Good step, decrease damping + self.lambda = (self.lambda / self.lambda_factor).max(self.min_lambda); + } else if rho < 0.1 { + // Bad step, increase damping + self.lambda = (self.lambda * self.lambda_factor).min(self.max_lambda); + } + // Otherwise keep lambda unchanged + } +} +``` + +**Implementation Effort**: Medium (3-4 hours) +**Impact**: Better convergence, fewer iterations, more robust matching + +--- + +### Priority 4: Branch-and-Bound Search + +**Current State**: +Correlative scan matching uses 2-level coarse-to-fine search: +- Coarse: resolution × 2.0 factor over 30cm window +- Fine: Full resolution around best coarse candidate + +Evaluates ~2,000-5,000 poses per match. + +**Problem**: +- Cannot efficiently handle larger search windows (>30cm) +- Poor odometry requires 50cm+ windows +- Computational cost scales linearly with window size + +**Cartographer's Approach**: +Branch-and-bound with precomputed multi-resolution grids: +- 4 resolution levels (1×, 4×, 16×, 64× cell size) +- Each coarse cell stores max probability of all finer cells +- DFS search with pruning: if upper_bound ≤ best_score, skip subtree + +**Proposed Solution**: + +``` +Precomputed Grid Hierarchy: + +Level 0 (2.5cm): [0.8][0.2][0.9][0.1][0.7][0.3][0.8][0.2] + \ / \ / \ / \ / +Level 1 (10cm): [0.8] [0.9] [0.7] [0.8] + \ / \ / +Level 2 (40cm): [0.9] [0.8] + \ / +Level 3 (160cm): [0.9] + +Upper bound: max of children guarantees no better solution exists below +``` + +```rust +pub struct PrecomputedGrids { + /// Resolution levels: [2.5cm, 10cm, 40cm, 160cm] + levels: [Grid; 4], +} + +impl PrecomputedGrids { + pub fn from_submap(grid: &LocalGrid) -> Self { + let mut levels = [Grid::default(); 4]; + levels[0] = grid.probability_grid(); + + for i in 1..4 { + levels[i] = Self::downsample_max(&levels[i-1], 4); + } + + Self { levels } + } + + fn downsample_max(grid: &Grid, factor: usize) -> Grid { + // Each output cell = max of factor×factor input cells + let mut out = Grid::new(grid.width / factor, grid.height / factor); + for y in 0..out.height { + for x in 0..out.width { + let mut max_val = 0.0f32; + for dy in 0..factor { + for dx in 0..factor { + max_val = max_val.max( + grid.get(x * factor + dx, y * factor + dy) + ); + } + } + out.set(x, y, max_val); + } + } + out + } +} + +pub fn branch_and_bound_match( + scan: &[Point2D], + grids: &PrecomputedGrids, + search_window: SearchWindow, +) -> Option<(Pose2D, f32)> { + let mut best = (Pose2D::identity(), 0.0f32); + let mut stack = Vec::with_capacity(1024); + + // Start with coarsest level covering entire window + stack.push(SearchNode { + level: 3, + x_range: search_window.x_range, + y_range: search_window.y_range, + theta_range: search_window.theta_range, + }); + + while let Some(node) = stack.pop() { + // Compute upper bound at this resolution + let upper_bound = score_scan_upper_bound( + scan, + &grids.levels[node.level], + node.center_pose(), + ); + + // Prune if cannot beat current best + if upper_bound <= best.1 { + continue; + } + + if node.level == 0 { + // Finest level: compute exact score + let score = score_scan_exact(scan, &grids.levels[0], node.center_pose()); + if score > best.1 { + best = (node.center_pose(), score); + } + } else { + // Subdivide into 8 children (2×2×2 in x,y,theta) + for child in node.subdivide() { + stack.push(child); + } + } + } + + (best.1 > MIN_MATCH_SCORE).then_some(best) +} +``` + +**Complexity**: +- Brute force: O(W² × A) where W=window size, A=angular steps +- Branch-and-bound: O(log(W) × k) where k=matched poses (typically <100) + +**Implementation Effort**: High (8-12 hours) +**Impact**: 10× speedup for large search windows, enables 50cm+ windows + +--- + +### Priority 5: Background Pose Graph Optimization + +**Current State**: +Pose graph optimization runs synchronously when loop closure is detected. + +**Problem**: +- Blocks scan processing during optimization (100-500ms) +- Cannot continuously refine poses as new constraints arrive +- Single-shot optimization may not converge fully + +**Cartographer's Approach**: +- Background thread runs optimization continuously +- Triggered every N nodes (configurable) +- Results applied incrementally to trajectory +- Multiple iterations for better convergence + +**Proposed Solution**: + +``` +┌─────────────────────────────────────────────────────────┐ +│ SLAM Thread │ +│ ┌──────────┐ ┌──────────┐ ┌──────────────────┐ │ +│ │ Scan │───▶│ Match │───▶│ Insert to Submap │ │ +│ │ Receive │ │ (Local) │ │ + Add Graph Node │ │ +│ └──────────┘ └──────────┘ └────────┬─────────┘ │ +│ │ │ +│ ▼ │ +│ ┌────────────────────────┐ │ +│ │ Loop Closure Detector │ │ +│ │ (descriptor matching) │ │ +│ └───────────┬────────────┘ │ +│ │ │ +└──────────────────────────────────────────┼──────────────┘ + │ trigger + ▼ +┌──────────────────────────────────────────────────────────┐ +│ Background Optimizer Thread │ +│ ┌─────────────┐ ┌─────────────┐ ┌─────────────┐ │ +│ │ Copy Graph │───▶│ Optimize │───▶│ Publish │ │ +│ │ Snapshot │ │ (50 iters) │ │ Corrections │ │ +│ └─────────────┘ └─────────────┘ └─────────────┘ │ +└──────────────────────────────────────────────────────────┘ +``` + +```rust +pub struct BackgroundOptimizer { + graph: Arc>, + config: OptimizationConfig, +} + +pub struct OptimizationConfig { + pub trigger_every_n_nodes: usize, // 20 + pub max_iterations: usize, // 50 + pub convergence_threshold: f64, // 1e-5 + pub huber_scale: f64, // 0.1 (10cm) + pub odometry_weight: f64, // 100.0 + pub loop_closure_weight: f64, // 50.0 +} + +impl BackgroundOptimizer { + pub fn run(&self, trigger_rx: Receiver<()>, result_tx: Sender) { + while trigger_rx.recv().is_ok() { + // Snapshot current graph + let graph_snapshot = self.graph.read().clone(); + + // Run optimization (doesn't block SLAM thread) + let optimized = self.optimize(&graph_snapshot); + + // Compute corrections + let corrections = self.compute_corrections(&graph_snapshot, &optimized); + + // Send back to SLAM thread + let _ = result_tx.send(corrections); + } + } + + fn optimize(&self, graph: &PoseGraph) -> Vec { + let mut poses = graph.poses.clone(); + + for iteration in 0..self.config.max_iterations { + let (h, b) = self.build_linear_system(graph, &poses); + let delta = self.solve_sparse(&h, &b); + + // Apply update + let max_update = self.apply_update(&mut poses, &delta); + + if max_update < self.config.convergence_threshold { + break; + } + } + + poses + } +} +``` + +**Implementation Effort**: Medium-High (6-8 hours) +**Impact**: Non-blocking optimization, better convergence + +**Note**: For embedded ARM target, background threading may not be critical. Synchronous optimization at 100-500ms is acceptable if triggered only on loop closure events (rare). + +--- + +## Updated Implementation Roadmap + +``` +Phase 1: Quick Wins (Immediate) +├── Motion filter ← PRIORITY 1 (1-2 hours) +└── Adaptive LM refinement ← PRIORITY 3 (3-4 hours) + +Phase 2: Accuracy Improvement +└── Log-odds probability cells ← PRIORITY 2 (4-6 hours) + +Phase 3: Performance +├── Branch-and-bound matcher ← PRIORITY 4 (8-12 hours) +├── Precomputed grid hierarchy +└── SIMD for B&B scoring + +Phase 4: Optional (if needed) +└── Background optimizer thread ← PRIORITY 5 (6-8 hours) + (may not be needed for embedded target) +``` + +--- + +## Memory Budget Analysis (Updated) + +| Component | Current | After Phase 2 | Notes | +|-----------|---------|---------------|-------| +| Global Grid (800×800) | 2.5 MB | 4 MB | +log_odds (i16) | +| Submaps (10 active) | 2 MB | 2 MB | Already implemented | +| Raw Scans (1000) | 2 MB | 2 MB | Already implemented | +| Precomputed Grids | - | 3 MB | Phase 3 | +| Pose Graph | 0.5 MB | 0.5 MB | Already implemented | +| **Total** | ~7 MB | ~12 MB | Within 50MB budget | + +--- + +## Expected Accuracy Improvements + +| Metric | Current | After Phase 1 | After Phase 2 | Method | +|--------|---------|---------------|---------------|--------| +| Drift (per 10m) | ~4cm | ~3cm | ~2cm | Adaptive LM | +| Loop closure error | ~2cm | ~2cm | ~1cm | Already good | +| Wall thickness | 5-8cm | 5-8cm | 2-3cm | Log-odds | +| CPU per match | 15ms | 12ms | 12ms | Motion filter | +| Ghost artifacts | Rare | Rare | Very rare | Already good | + +--- + +## References + +1. Hess, W., et al. "Real-Time Loop Closure in 2D LIDAR SLAM." IEEE ICRA 2016. +2. Cartographer ROS Documentation: https://google-cartographer-ros.readthedocs.io/ +3. Olson, E. "Real-Time Correlative Scan Matching." IEEE ICRA 2009. +4. Konolige, K. "Improved Occupancy Grids for Map Building." Autonomous Robots 1997. diff --git a/vastu-map/examples/exploration/harness/adapters.rs b/vastu-map/examples/exploration/harness/adapters.rs deleted file mode 100644 index a6a7a8b..0000000 --- a/vastu-map/examples/exploration/harness/adapters.rs +++ /dev/null @@ -1,28 +0,0 @@ -//! Type conversions between sangam-io and vastu-map -//! -//! sangam-io's lidar simulator produces `Vec<(f32, f32, u8)>` which maps -//! directly to vastu-map's `PolarScan.points` format. - -use vastu_map::core::{PointCloud2D, PolarScan}; - -/// Convert lidar scan data from sangam-io format to vastu-map PointCloud2D. -/// -/// Uses full 360° lidar data for better line extraction quality. -/// -/// # Arguments -/// * `lidar_data` - Raw lidar data from LidarSimulator::generate_scan() -/// * `min_quality` - Minimum quality value to include (0-255) -/// * `min_range` - Minimum valid range in meters -/// * `max_range` - Maximum valid range in meters -/// -/// # Returns -/// Cartesian point cloud in robot frame, filtered by quality and range. -pub fn lidar_to_point_cloud( - lidar_data: Vec<(f32, f32, u8)>, - min_quality: u8, - min_range: f32, - max_range: f32, -) -> PointCloud2D { - let polar = PolarScan { points: lidar_data }; - polar.to_cartesian(min_quality, min_range, max_range) -} diff --git a/vastu-map/examples/exploration/harness/harness.rs b/vastu-map/examples/exploration/harness/harness.rs deleted file mode 100644 index 78bb764..0000000 --- a/vastu-map/examples/exploration/harness/harness.rs +++ /dev/null @@ -1,545 +0,0 @@ -//! Core test harness for exploration example -//! -//! Provides synchronous simulation using sangam-io's mock device components -//! directly (without spawning daemon threads). - -use sangam_io::devices::mock::config::{LidarConfig, RobotConfig}; -use sangam_io::devices::mock::lidar_sim::LidarSimulator; -use sangam_io::devices::mock::map_loader::SimulationMap; -use sangam_io::devices::mock::noise::NoiseGenerator; -use sangam_io::devices::mock::physics::PhysicsState; - -use vastu_map::config::ExplorationConfig; -use vastu_map::core::Pose2D; -use vastu_map::core::math::normalize_angle; -use vastu_map::integration::{ScanStore, ScanStoreConfig}; -use vastu_map::odometry::WheelOdometry; -use vastu_map::query::ClearanceVisibilityGraph; -use vastu_map::{Map, Path as PlannedPath, VectorMap, VectorMapConfig}; - -use super::adapters::lidar_to_point_cloud; -use super::metrics::{ConvergenceStats, TestMetrics, TimingStats}; -use super::visualization::Visualizer; - -/// Test harness configuration. -#[derive(Clone)] -pub struct HarnessConfig { - /// Path to map YAML file - pub map_file: String, - /// Starting X position in meters - pub start_x: f32, - /// Starting Y position in meters - pub start_y: f32, - /// Starting orientation in radians - pub start_theta: f32, - /// Robot configuration - pub robot: RobotConfig, - /// Lidar configuration - pub lidar: LidarConfig, - /// SLAM configuration - pub slam: VectorMapConfig, - /// Random seed (0 = random) - pub random_seed: u64, - /// Minimum lidar quality to include - pub min_quality: u8, - /// Minimum lidar range in meters - pub min_range: f32, - /// Maximum lidar range in meters - pub max_range: f32, - /// Simulation timestep in seconds - pub dt: f32, - /// Lidar scan interval (in simulation steps) - pub lidar_interval: usize, - /// Fast mode: skip intermediate physics, run only at lidar rate (~22x speedup) - pub fast_mode: bool, -} - -impl Default for HarnessConfig { - fn default() -> Self { - // Use lidar config with zero offsets for test accuracy - // The defaults match the physical robot but cause systematic errors: - // - angle_offset=0.2182 rad (~12.5°) rotates the scan - // - mounting_x=-0.110m shifts scan origin 11cm behind robot center - // For testing we want the lidar at robot center with no angular offset. - let mut lidar = LidarConfig::default(); - lidar.angle_offset = 0.0; - lidar.mounting_x = 0.0; - lidar.mounting_y = 0.0; - lidar.optical_offset = 0.0; - - Self { - map_file: String::new(), - start_x: 2.5, - start_y: 2.5, - start_theta: 0.0, - robot: RobotConfig::default(), - lidar, - slam: VectorMapConfig::default(), - random_seed: 42, // Fixed seed for reproducibility - min_quality: 50, - min_range: 0.15, - max_range: 8.0, - dt: 1.0 / 110.0, // 110Hz simulation - lidar_interval: 22, // ~5Hz lidar (110/22 = 5) - fast_mode: true, // Skip physics substeps for faster tests - } - } -} - -/// Per-observation record for trajectory visualization. -#[derive(Clone, Debug)] -pub struct ObservationRecord { - /// Ground truth pose from physics. - pub truth_pose: Pose2D, - /// Estimated pose from SLAM. - pub slam_pose: Pose2D, - /// Match confidence (0.0-1.0). - pub confidence: f32, - /// Drift magnitude (translation error in meters). - pub drift_translation: f32, -} - -/// Trajectory history for visualization. -#[derive(Clone, Debug, Default)] -pub struct TrajectoryHistory { - /// All observation records in chronological order. - pub observations: Vec, -} - -impl TrajectoryHistory { - /// Create a new empty trajectory history. - pub fn new() -> Self { - Self::default() - } - - /// Record an observation. - pub fn record(&mut self, truth_pose: Pose2D, slam_pose: Pose2D, confidence: f32) { - let drift_translation = - ((truth_pose.x - slam_pose.x).powi(2) + (truth_pose.y - slam_pose.y).powi(2)).sqrt(); - - self.observations.push(ObservationRecord { - truth_pose, - slam_pose, - confidence, - drift_translation, - }); - } -} - -/// Main test harness for running integration tests. -pub struct TestHarness { - config: HarnessConfig, - map: SimulationMap, - physics: PhysicsState, - lidar_sim: LidarSimulator, - slam: VectorMap, - visualizer: Option, - sim_time: f32, - tick_count: usize, - observations: usize, - last_slam_pose: Pose2D, - /// Ground truth pose from last scan (for computing odometry) - last_physics_pose: Pose2D, - /// Last lidar scan in robot frame (for visualization) - last_scan_robot_frame: Option, - /// ICP convergence statistics across all observations - convergence_stats: ConvergenceStats, - /// Trajectory history for enhanced visualization - trajectory: TrajectoryHistory, - /// Algorithm timing statistics across all observations - timing_stats: TimingStats, - /// Wheel odometry tracker using vastu-map's odometry module - wheel_odometry: WheelOdometry, - /// Simulated left encoder ticks (accumulated) - left_encoder_ticks: i32, - /// Simulated right encoder ticks (accumulated) - right_encoder_ticks: i32, - /// Scan storage for visualization (stores ALL scans) - scan_store: ScanStore, -} - -impl TestHarness { - /// Create a new test harness. - pub fn new(config: HarnessConfig) -> Result> { - // Load map - let map = SimulationMap::load(&config.map_file)?; - - // Initialize physics at start pose - let physics = PhysicsState::new( - config.start_x, - config.start_y, - config.start_theta, - &config.robot, - ); - - // Create noise generator with fixed seed - let noise = NoiseGenerator::new(config.random_seed); - - // Create lidar simulator - let lidar_sim = LidarSimulator::new(&config.lidar, noise); - - // Create SLAM map - let slam = VectorMap::new(config.slam.clone()); - - let start_pose = Pose2D::new(config.start_x, config.start_y, config.start_theta); - - // Create wheel odometry tracker with robot configuration - let wheel_odometry = - WheelOdometry::new(config.robot.wheel_base, config.robot.ticks_per_meter); - - // Create scan store for visualization (stores all scans) - let scan_store = ScanStore::new(ScanStoreConfig::default()); - - Ok(Self { - config, - map, - physics, - lidar_sim, - slam, - visualizer: None, - sim_time: 0.0, - tick_count: 0, - observations: 0, - last_slam_pose: start_pose, - last_physics_pose: start_pose, - last_scan_robot_frame: None, - convergence_stats: ConvergenceStats::new(), - trajectory: TrajectoryHistory::new(), - timing_stats: TimingStats::new(), - wheel_odometry, - left_encoder_ticks: 0, - right_encoder_ticks: 0, - scan_store, - }) - } - - /// Enable visualization output. - pub fn with_visualization(mut self, output_path: &str) -> Self { - self.visualizer = Some(Visualizer::new(output_path)); - self - } - - /// Enable exploration mode on the SLAM map. - /// - /// In exploration mode, VectorMap stores raw scan data and periodically - /// re-fits lines from accumulated point clouds, eliminating first-scan bias. - /// - /// # Arguments - /// * `config` - Exploration configuration (uses default if None) - /// - /// # Example - /// ```rust,ignore - /// let harness = TestHarness::new(HarnessConfig::simple_room())? - /// .with_exploration_mode(None); // Use default config - /// ``` - pub fn with_exploration_mode(mut self, config: Option) -> Self { - let exploration_config = config.unwrap_or_default(); - self.slam.enable_exploration_mode(exploration_config); - self - } - - /// Process a lidar scan and feed to SLAM. - fn process_lidar_scan(&mut self) { - // Generate lidar scan - let scan_data = self.lidar_sim.generate_scan( - &self.map, - self.physics.x(), - self.physics.y(), - self.physics.theta(), - ); - - // Convert to point cloud - let cloud = lidar_to_point_cloud( - scan_data, - self.config.min_quality, - self.config.min_range, - self.config.max_range, - ); - - // Skip if scan is too sparse - if cloud.len() < 10 { - return; - } - - // Store the robot-frame scan for visualization - self.last_scan_robot_frame = Some(cloud.clone()); - - // Get ground truth pose for metrics/visualization - let current_truth = Pose2D::new(self.physics.x(), self.physics.y(), self.physics.theta()); - - // Compute relative odometry from ground truth physics poses - // This mimics perfect wheel encoders that measure actual motion. - // Note: We also update WheelOdometry for cumulative tracking, but use - // ground-truth-derived odometry for SLAM to avoid commanded vs actual mismatch. - let odometry = if self.observations == 0 { - // First observation - use absolute pose as initial guess - let _ = self - .wheel_odometry - .update(self.left_encoder_ticks, self.right_encoder_ticks); - current_truth - } else { - // Update wheel odometry for tracking (though we use ground truth for SLAM) - let _ = self - .wheel_odometry - .update(self.left_encoder_ticks, self.right_encoder_ticks); - // Compute relative pose from physics (like perfect encoders) - self.compute_relative_odometry(current_truth) - }; - - // Feed to SLAM - let result = self.slam.observe(&cloud, odometry); - self.last_slam_pose = result.pose; - self.last_physics_pose = current_truth; // Update for metrics computation - self.observations += 1; - - // Store scan in scan store for visualization (stores ALL scans) - self.scan_store.add_scan( - odometry, - result.pose, - &cloud, - result.features_extracted, - result.confidence, - ); - - // Record ICP convergence stats - self.convergence_stats - .record(result.icp_iterations, result.icp_converged); - - // Record timing stats - self.timing_stats.record(&result.timing); - - // Record to trajectory history for visualization - // Note: We no longer store scans here since ScanStore handles it - self.trajectory - .record(current_truth, result.pose, result.confidence); - - log::debug!( - "Observation {}: truth=({:.2}, {:.2}, {:.1}°), slam=({:.2}, {:.2}, {:.1}°), conf={:.2}, icp_iters={}", - self.observations, - current_truth.x, - current_truth.y, - current_truth.theta.to_degrees(), - result.pose.x, - result.pose.y, - result.pose.theta.to_degrees(), - result.confidence, - result.icp_iterations - ); - } - - /// Compute relative odometry between consecutive ground truth poses. - /// - /// Uses physics poses (not SLAM poses) to mimic real-world encoders that - /// measure actual wheel motion independent of SLAM corrections. - fn compute_relative_odometry(&self, current_truth: Pose2D) -> Pose2D { - // Compute delta from previous physics pose (like real encoders) - let dx = current_truth.x - self.last_physics_pose.x; - let dy = current_truth.y - self.last_physics_pose.y; - let dtheta = normalize_angle(current_truth.theta - self.last_physics_pose.theta); - - // Transform to robot frame using previous physics pose orientation - let cos_th = self.last_physics_pose.theta.cos(); - let sin_th = self.last_physics_pose.theta.sin(); - let dx_robot = dx * cos_th + dy * sin_th; - let dy_robot = -dx * sin_th + dy * cos_th; - - Pose2D::new(dx_robot, dy_robot, dtheta) - } - - /// Simulate encoder ticks from commanded wheel velocities. - /// - /// Computes individual wheel velocities from linear/angular velocity commands - /// and integrates them over the timestep to produce encoder tick counts. - /// This mimics how real wheel encoders would accumulate ticks. - fn simulate_encoder_ticks(&mut self, dt: f32, linear_vel: f32, angular_vel: f32) { - // Compute individual wheel velocities from (v, ω) commands - // Using differential drive kinematics: - // v_left = v - ω * wheel_base / 2 - // v_right = v + ω * wheel_base / 2 - let (left_vel, right_vel) = - self.physics - .wheel_velocities(linear_vel, angular_vel, self.config.robot.wheel_base); - - // Integrate wheel velocities to get distance traveled per wheel - let left_distance = left_vel * dt; - let right_distance = right_vel * dt; - - // Convert distance to encoder ticks - let left_delta_ticks = (left_distance * self.config.robot.ticks_per_meter) as i32; - let right_delta_ticks = (right_distance * self.config.robot.ticks_per_meter) as i32; - - // Accumulate encoder ticks - self.left_encoder_ticks += left_delta_ticks; - self.right_encoder_ticks += right_delta_ticks; - } - - /// Finalize the test with CBVG visualization. - /// - /// Generates the SVG visualization with the clearance-based visibility - /// graph and planned path, then prints a timing summary. - /// - /// # Arguments - /// * `graph` - Optional CBVG to visualize - /// * `path` - Optional planned path to visualize - pub fn finalize_with_graph( - &mut self, - graph: Option<&ClearanceVisibilityGraph>, - path: Option<&PlannedPath>, - ) { - let final_pose = self.slam.current_pose(); - let ground_truth_pose = - Pose2D::new(self.physics.x(), self.physics.y(), self.physics.theta()); - - // Compute metrics using the simulation map, including convergence stats - let metrics = TestMetrics::compute( - &self.slam, - &self.map, - final_pose, - ground_truth_pose, - self.convergence_stats.clone(), - ); - - // Generate enhanced visualization with graph if enabled - if let Some(ref viz) = self.visualizer { - viz.render_with_path_planning( - &self.slam, - &self.map, - final_pose, - ground_truth_pose, - &self.scan_store, - &self.trajectory, - &metrics, - graph, - path, - ); - } - - // Print timing summary - self.timing_stats.print_summary(); - } - - /// Get current SLAM map reference. - pub fn slam(&self) -> &VectorMap { - &self.slam - } - - /// Get mutable reference to SLAM map (for adding virtual walls). - pub fn slam_mut(&mut self) -> &mut VectorMap { - &mut self.slam - } - - /// Get current physics (ground truth) pose. - pub fn physics_pose(&self) -> Pose2D { - Pose2D::new(self.physics.x(), self.physics.y(), self.physics.theta()) - } - - /// Get configuration. - pub fn config(&self) -> &HarnessConfig { - &self.config - } - - /// Run a single simulation step and return collision status. - /// - /// This is used for interactive exploration tests where the controller - /// needs to react to sensor feedback at each step. - /// - /// # Arguments - /// * `linear_vel` - Linear velocity command (m/s) - /// * `angular_vel` - Angular velocity command (rad/s) - /// - /// # Returns - /// StepResult containing collision status and updated poses. - pub fn step(&mut self, linear_vel: f32, angular_vel: f32) -> StepResult { - let dt = if self.config.fast_mode { - self.config.dt * self.config.lidar_interval as f32 - } else { - self.config.dt - }; - - // Simulate encoder ticks - self.simulate_encoder_ticks(dt, linear_vel, angular_vel); - - // Update physics - self.physics - .update(dt, linear_vel, angular_vel, &self.map, &self.config.robot); - - self.sim_time += dt; - self.tick_count += 1; - - // Check bumper sensors using the physics simulation - let (left_bumper, right_bumper) = self.check_bumpers(); - - StepResult { - left_bumper, - right_bumper, - physics_pose: Pose2D::new(self.physics.x(), self.physics.y(), self.physics.theta()), - } - } - - /// Check bumper sensors based on current position. - /// - /// Uses simple distance-based collision detection against the map. - fn check_bumpers(&self) -> (bool, bool) { - use std::f32::consts::FRAC_PI_4; - - let x = self.physics.x(); - let y = self.physics.y(); - let theta = self.physics.theta(); - let radius = self.config.robot.robot_radius; - - // Check points on left and right front bumper zones - let check_distance = radius + 0.02; // Slightly past robot surface - - // Left bumper zone (front-left quadrant) - let left_angle = theta + FRAC_PI_4; - let left_x = x + check_distance * left_angle.cos(); - let left_y = y + check_distance * left_angle.sin(); - let left_triggered = self.map.is_occupied(left_x, left_y); - - // Right bumper zone (front-right quadrant) - let right_angle = theta - FRAC_PI_4; - let right_x = x + check_distance * right_angle.cos(); - let right_y = y + check_distance * right_angle.sin(); - let right_triggered = self.map.is_occupied(right_x, right_y); - - // Also check center front - let center_x = x + check_distance * theta.cos(); - let center_y = y + check_distance * theta.sin(); - let center_triggered = self.map.is_occupied(center_x, center_y); - - ( - left_triggered || center_triggered, - right_triggered || center_triggered, - ) - } - - /// Process a lidar scan and feed to SLAM (public version). - /// - /// Call this at lidar intervals during exploration. - pub fn process_lidar(&mut self) { - self.process_lidar_scan(); - } - - /// Get current simulation time. - pub fn sim_time(&self) -> f32 { - self.sim_time - } -} - -/// Result of a single simulation step. -#[derive(Clone, Debug)] -pub struct StepResult { - /// Whether the left bumper is triggered. - pub left_bumper: bool, - /// Whether the right bumper is triggered. - pub right_bumper: bool, - /// Current physics (ground truth) pose. - pub physics_pose: Pose2D, -} - -impl StepResult { - /// Check if any bumper is triggered. - pub fn any_bumper(&self) -> bool { - self.left_bumper || self.right_bumper - } -} diff --git a/vastu-map/examples/exploration/harness/metrics.rs b/vastu-map/examples/exploration/harness/metrics.rs deleted file mode 100644 index 77c0694..0000000 --- a/vastu-map/examples/exploration/harness/metrics.rs +++ /dev/null @@ -1,325 +0,0 @@ -//! Quantitative validation metrics -//! -//! Computes error metrics comparing SLAM output against ground truth: -//! - Position and orientation error -//! - Wall accuracy (how close detected lines are to actual walls in map) -//! - Timing statistics for algorithm performance - -use sangam_io::devices::mock::map_loader::SimulationMap; - -use vastu_map::TimingBreakdown; -use vastu_map::VectorMap; -use vastu_map::core::Pose2D; -use vastu_map::features::Line2D; - -/// Statistics collected during ICP matching over a test run. -#[derive(Debug, Clone, Default)] -pub struct ConvergenceStats { - /// Total ICP iterations across all observations - pub total_iterations: usize, - /// Number of observations processed - pub observation_count: usize, - /// Maximum ICP iterations in any single observation - pub max_iterations: usize, - /// Number of observations where ICP failed to converge - pub convergence_failures: usize, -} - -impl ConvergenceStats { - /// Create a new empty stats tracker. - pub fn new() -> Self { - Self::default() - } - - /// Record an observation result. - pub fn record(&mut self, iterations: usize, converged: bool) { - self.total_iterations += iterations; - self.observation_count += 1; - if iterations > self.max_iterations { - self.max_iterations = iterations; - } - if !converged && iterations > 0 { - self.convergence_failures += 1; - } - } - - /// Get average ICP iterations per observation. - pub fn avg_iterations(&self) -> f32 { - if self.observation_count == 0 { - 0.0 - } else { - self.total_iterations as f32 / self.observation_count as f32 - } - } -} - -/// Single stage timing accumulator. -#[derive(Debug, Clone, Default)] -struct StageTiming { - total_us: u64, - min_us: u64, - max_us: u64, - count: usize, -} - -impl StageTiming { - fn new() -> Self { - Self { - total_us: 0, - min_us: u64::MAX, - max_us: 0, - count: 0, - } - } - - fn record(&mut self, us: u64) { - self.total_us += us; - self.min_us = self.min_us.min(us); - self.max_us = self.max_us.max(us); - self.count += 1; - } - - fn avg_us(&self) -> f64 { - if self.count == 0 { - 0.0 - } else { - self.total_us as f64 / self.count as f64 - } - } - - fn min_display(&self) -> u64 { - if self.min_us == u64::MAX { - 0 - } else { - self.min_us - } - } -} - -/// Accumulated timing statistics across all observations. -#[derive(Debug, Clone, Default)] -pub struct TimingStats { - icp_matching: StageTiming, - line_extraction: StageTiming, - corner_detection: StageTiming, - association: StageTiming, - merging: StageTiming, - new_features: StageTiming, - loop_closure: StageTiming, - total: StageTiming, -} - -impl TimingStats { - /// Create a new empty timing stats tracker. - pub fn new() -> Self { - Self { - icp_matching: StageTiming::new(), - line_extraction: StageTiming::new(), - corner_detection: StageTiming::new(), - association: StageTiming::new(), - merging: StageTiming::new(), - new_features: StageTiming::new(), - loop_closure: StageTiming::new(), - total: StageTiming::new(), - } - } - - /// Record timing from a single observation. - pub fn record(&mut self, timing: &TimingBreakdown) { - self.icp_matching.record(timing.icp_matching_us); - self.line_extraction.record(timing.line_extraction_us); - self.corner_detection.record(timing.corner_detection_us); - self.association.record(timing.association_us); - self.merging.record(timing.merging_us); - self.new_features.record(timing.new_features_us); - self.loop_closure.record(timing.loop_closure_us); - self.total.record(timing.total_us); - } - - /// Get number of observations recorded. - fn observation_count(&self) -> usize { - self.total.count - } - - /// Print a formatted timing summary. - pub fn print_summary(&self) { - let count = self.observation_count(); - if count == 0 { - println!("No timing data collected."); - return; - } - - println!("\n=== Timing Summary ({} observations) ===", count); - println!( - "{:<24} | {:>10} | {:>10} | {:>10} | {:>10}", - "Stage", "Total", "Avg", "Min", "Max" - ); - println!("{}", "-".repeat(73)); - - self.print_stage("ICP Matching", &self.icp_matching); - self.print_stage("Line Extraction", &self.line_extraction); - self.print_stage("Corner Detection", &self.corner_detection); - self.print_stage("Association", &self.association); - self.print_stage("Merging", &self.merging); - self.print_stage("New Features", &self.new_features); - self.print_stage("Loop Closure", &self.loop_closure); - - println!("{}", "-".repeat(73)); - self.print_stage("Total per Observe", &self.total); - } - - fn print_stage(&self, name: &str, stage: &StageTiming) { - println!( - "{:<24} | {:>10} | {:>10} | {:>10} | {:>10}", - name, - Self::format_us(stage.total_us), - Self::format_us(stage.avg_us() as u64), - Self::format_us(stage.min_display()), - Self::format_us(stage.max_us), - ); - } - - fn format_us(us: u64) -> String { - if us >= 1_000_000 { - format!("{:.2} s", us as f64 / 1_000_000.0) - } else if us >= 1_000 { - format!("{:.2} ms", us as f64 / 1_000.0) - } else { - format!("{} µs", us) - } - } -} - -/// Test metrics results. -#[derive(Debug, Clone)] -pub struct TestMetrics { - /// Position error in meters (Euclidean distance) - pub position_error: f32, - /// Orientation error in radians - pub orientation_error: f32, - /// Percentage of detected line points that are near actual walls (0.0-1.0) - pub wall_accuracy: f32, - /// Number of detected walls/features - pub detected_walls: usize, - /// ICP convergence statistics - pub convergence: ConvergenceStats, -} - -impl TestMetrics { - /// Compute all metrics comparing SLAM output to ground truth map. - pub fn compute( - slam: &VectorMap, - map: &SimulationMap, - final_pose: Pose2D, - truth_pose: Pose2D, - convergence: ConvergenceStats, - ) -> Self { - // Position error - let position_error = - ((final_pose.x - truth_pose.x).powi(2) + (final_pose.y - truth_pose.y).powi(2)).sqrt(); - - // Orientation error (normalized to [-PI, PI]) - let orientation_error = angle_diff(final_pose.theta, truth_pose.theta).abs(); - - // Get detected lines - let detected_lines = slam.lines(); - let detected_walls = detected_lines.len(); - - // Compute wall accuracy using the simulation map - let wall_accuracy = compute_wall_accuracy(detected_lines, map); - - Self { - position_error, - orientation_error, - wall_accuracy, - detected_walls, - convergence, - } - } -} - -/// Compute angle difference normalized to [-PI, PI]. -fn angle_diff(a: f32, b: f32) -> f32 { - let diff = a - b; - if diff > std::f32::consts::PI { - diff - 2.0 * std::f32::consts::PI - } else if diff < -std::f32::consts::PI { - diff + 2.0 * std::f32::consts::PI - } else { - diff - } -} - -/// Maximum distance (meters) from line point to wall to count as valid. -const WALL_DISTANCE_THRESHOLD: f32 = 0.15; // 15cm - -/// Number of sample points along each line. -const SAMPLES_PER_LINE: usize = 20; - -/// Compute wall accuracy by checking if detected lines lie on occupied pixels. -/// -/// Returns accuracy: fraction of sample points that are within threshold of a wall. -fn compute_wall_accuracy(detected: &[Line2D], map: &SimulationMap) -> f32 { - if detected.is_empty() { - return 0.0; - } - - let mut near_wall_count = 0; - let mut total_samples = 0; - - let resolution = map.resolution(); - - for line in detected { - // Sample points along the line - for i in 0..SAMPLES_PER_LINE { - let t = i as f32 / (SAMPLES_PER_LINE - 1) as f32; - let x = line.start.x + t * (line.end.x - line.start.x); - let y = line.start.y + t * (line.end.y - line.start.y); - - // Find distance to nearest occupied cell - let dist = distance_to_nearest_wall(x, y, map, resolution); - total_samples += 1; - - if dist <= WALL_DISTANCE_THRESHOLD { - near_wall_count += 1; - } - } - } - - near_wall_count as f32 / total_samples as f32 -} - -/// Find distance from point to nearest occupied cell in map. -/// -/// Searches in a spiral pattern outward from the point. -fn distance_to_nearest_wall(x: f32, y: f32, map: &SimulationMap, resolution: f32) -> f32 { - // Check if point itself is on a wall - if map.is_occupied(x, y) { - return 0.0; - } - - // Search outward in a grid pattern - let max_search = (WALL_DISTANCE_THRESHOLD * 2.0 / resolution) as i32; - - for radius in 1..=max_search { - for dx in -radius..=radius { - for dy in -radius..=radius { - // Only check cells at this radius (perimeter) - if dx.abs() != radius && dy.abs() != radius { - continue; - } - - let check_x = x + dx as f32 * resolution; - let check_y = y + dy as f32 * resolution; - - if map.is_occupied(check_x, check_y) { - let dist = ((check_x - x).powi(2) + (check_y - y).powi(2)).sqrt(); - return dist; - } - } - } - } - - // No wall found within search radius - WALL_DISTANCE_THRESHOLD * 2.0 -} diff --git a/vastu-map/examples/exploration/harness/mod.rs b/vastu-map/examples/exploration/harness/mod.rs deleted file mode 100644 index d5203fe..0000000 --- a/vastu-map/examples/exploration/harness/mod.rs +++ /dev/null @@ -1,18 +0,0 @@ -//! Test harness for the exploration example -//! -//! This module provides a simulation environment for testing -//! vastu-map's SLAM algorithms using sangam-io's mock device. -//! -//! # Features -//! -//! - Synchronous simulation with physics, lidar, and SLAM -//! - Ground truth tracking for accuracy metrics -//! - SVG visualization output - -mod adapters; -mod harness; -mod metrics; -mod visualization; - -// Re-exports -pub use harness::{HarnessConfig, TestHarness}; diff --git a/vastu-map/examples/exploration/harness/visualization.rs b/vastu-map/examples/exploration/harness/visualization.rs deleted file mode 100644 index 60bfdc4..0000000 --- a/vastu-map/examples/exploration/harness/visualization.rs +++ /dev/null @@ -1,816 +0,0 @@ -//! SVG visualization for manual inspection -//! -//! Generates SVG files showing: -//! - Map walls from PGM (gray background) -//! - Detected walls (teal solid lines) -//! - Robot trajectories (ground truth blue, estimated with confidence heatmap) -//! - Drift vectors at observation points -//! - Scan history at key points -//! - Ground truth pose (blue marker) -//! - Estimated pose (orange marker) -//! - Metrics overlay - -use sangam_io::devices::mock::map_loader::SimulationMap; -use svg::Document; -use svg::node::element::{ - Circle, Definitions, Group, Line, Marker, Path, Polyline, Rectangle, Text, -}; - -use vastu_map::Path as PlannedPath; -use vastu_map::VectorMap; -use vastu_map::core::{Point2D, Pose2D}; -use vastu_map::integration::ScanStore; -use vastu_map::query::{ClearanceVisibilityGraph, NodeType}; - -use super::harness::TrajectoryHistory; -use super::metrics::TestMetrics; - -/// Colorblind-friendly color palette (Okabe-Ito). -mod colors { - /// Ground truth trajectory - blue - pub const TRUTH_PATH: &str = "#0072B2"; - /// Estimated trajectory default - orange (used when not showing confidence) - pub const ESTIMATED_PATH: &str = "#E69F00"; - /// Drift vectors - vermillion - pub const DRIFT_VECTOR: &str = "#D55E00"; - /// Detected walls - teal - pub const DETECTED_WALLS: &str = "#009E73"; - /// Map walls - gray - pub const MAP_WALLS: &str = "#BBBBBB"; - /// Lidar scan - sky blue - pub const LIDAR_SCAN: &str = "#56B4E9"; - /// Lidar scan history - lighter, semi-transparent - pub const LIDAR_SCAN_HISTORY: &str = "#56B4E9"; - /// Visibility graph nodes - purple - pub const GRAPH_NODE: &str = "#CC79A7"; - /// Visibility graph edges - light purple - pub const GRAPH_EDGE: &str = "#CC79A7"; - /// Planned path - green - pub const PLANNED_PATH: &str = "#009E73"; - /// Start node - green - pub const START_NODE: &str = "#00FF00"; - /// Goal node - red - pub const GOAL_NODE: &str = "#FF0000"; -} - -/// Interval for displaying drift vectors (every Nth observation). -const DRIFT_VECTOR_INTERVAL: usize = 5; - -/// Opacity for historical scans. -const SCAN_HISTORY_OPACITY: f32 = 0.3; - -/// Visualization generator. -pub struct Visualizer { - output_path: String, - scale: f32, - margin: f32, -} - -impl Visualizer { - /// Create a new visualizer. - /// - /// # Arguments - /// * `output_path` - Path for the output SVG file - pub fn new(output_path: &str) -> Self { - Self { - output_path: output_path.to_string(), - scale: 100.0, // 100 pixels per meter - margin: 50.0, - } - } - - /// Convert confidence value (0.0-1.0) to a color. - /// 0.0 = red, 0.5 = yellow, 1.0 = green - fn confidence_to_color(confidence: f32) -> String { - let conf = confidence.clamp(0.0, 1.0); - let r = if conf < 0.5 { - 255 - } else { - ((1.0 - conf) * 2.0 * 255.0) as u8 - }; - let g = if conf > 0.5 { - 255 - } else { - (conf * 2.0 * 255.0) as u8 - }; - format!("#{:02X}{:02X}00", r, g) - } - - /// Create SVG marker definitions for arrows. - fn create_marker_definitions(&self) -> Definitions { - let arrow_marker = Marker::new() - .set("id", "drift-arrow") - .set("markerWidth", 6) - .set("markerHeight", 6) - .set("refX", 5) - .set("refY", 3) - .set("orient", "auto") - .add( - Path::new() - .set("d", "M0,0 L6,3 L0,6 Z") - .set("fill", colors::DRIFT_VECTOR), - ); - - Definitions::new().add(arrow_marker) - } - - /// Render ground truth path (solid blue) and estimated path with confidence heatmap. - fn render_trajectories(&self, trajectory: &TrajectoryHistory, svg_height: f32) -> Group { - let mut group = Group::new().set("id", "trajectories"); - - if trajectory.observations.len() < 2 { - return group; - } - - // Ground truth path - solid blue polyline - let truth_points: String = trajectory - .observations - .iter() - .map(|o| { - let (x, y) = self.transform_point(o.truth_pose.position(), 0.0, 0.0, svg_height); - format!("{},{}", x, y) - }) - .collect::>() - .join(" "); - - let truth_path = Polyline::new() - .set("points", truth_points) - .set("fill", "none") - .set("stroke", colors::TRUTH_PATH) - .set("stroke-width", 2.5) - .set("stroke-linecap", "round") - .set("stroke-linejoin", "round"); - - group = group.add(truth_path); - - // Estimated path - individual line segments colored by confidence - for i in 1..trajectory.observations.len() { - let prev = &trajectory.observations[i - 1]; - let curr = &trajectory.observations[i]; - - // Use the current observation's confidence for the segment color - let color = Self::confidence_to_color(curr.confidence); - - let (x1, y1) = self.transform_point(prev.slam_pose.position(), 0.0, 0.0, svg_height); - let (x2, y2) = self.transform_point(curr.slam_pose.position(), 0.0, 0.0, svg_height); - - let segment = Line::new() - .set("x1", x1) - .set("y1", y1) - .set("x2", x2) - .set("y2", y2) - .set("stroke", color) - .set("stroke-width", 2.5) - .set("stroke-linecap", "round"); - - group = group.add(segment); - } - - group - } - - /// Render drift vectors at every N observations. - fn render_drift_vectors(&self, trajectory: &TrajectoryHistory, svg_height: f32) -> Group { - let mut group = Group::new().set("id", "drift_vectors"); - - for (i, obs) in trajectory.observations.iter().enumerate() { - // Show drift at fixed intervals and at the last observation - if i % DRIFT_VECTOR_INTERVAL != 0 && i != trajectory.observations.len() - 1 { - continue; - } - - // Skip if drift is negligible - if obs.drift_translation < 0.02 { - continue; - } - - let (x1, y1) = self.transform_point(obs.truth_pose.position(), 0.0, 0.0, svg_height); - let (x2, y2) = self.transform_point(obs.slam_pose.position(), 0.0, 0.0, svg_height); - - // Drift vector line with arrowhead - let line = Line::new() - .set("x1", x1) - .set("y1", y1) - .set("x2", x2) - .set("y2", y2) - .set("stroke", colors::DRIFT_VECTOR) - .set("stroke-width", 1.5) - .set("marker-end", "url(#drift-arrow)"); - - group = group.add(line); - - // Small circle at ground truth position - let circle = Circle::new() - .set("cx", x1) - .set("cy", y1) - .set("r", 3.0) - .set("fill", colors::DRIFT_VECTOR) - .set("opacity", 0.6); - - group = group.add(circle); - } - - group - } - - /// Render all scans from the scan store. - fn render_all_scans(&self, scan_store: &ScanStore, svg_height: f32) -> Group { - let mut group = Group::new().set("id", "scan_history"); - - // Skip the last scan as it will be rendered at full opacity separately - let scan_count = scan_store.len(); - for (idx, scan) in scan_store.iter().enumerate() { - // Skip the last scan (rendered separately at full opacity) - if idx == scan_count - 1 { - continue; - } - - // Get world-frame points directly from stored scan - let world_points = scan.world_points(); - - for point in world_points { - let (sx, sy) = self.transform_point(point, 0.0, 0.0, svg_height); - - let circle = Circle::new() - .set("cx", sx) - .set("cy", sy) - .set("r", 2.0) - .set("fill", colors::LIDAR_SCAN_HISTORY) - .set("opacity", SCAN_HISTORY_OPACITY); - - group = group.add(circle); - } - } - - group - } - - /// Render world-frame points (for final scan). - fn render_world_points(&self, points: &[Point2D], svg_height: f32) -> Group { - let mut group = Group::new().set("id", "lidar_scan"); - - for point in points { - let (sx, sy) = self.transform_point(*point, 0.0, 0.0, svg_height); - - let circle = Circle::new() - .set("cx", sx) - .set("cy", sy) - .set("r", 3.0) - .set("fill", colors::LIDAR_SCAN) - .set("stroke", colors::TRUTH_PATH) - .set("stroke-width", 0.5) - .set("opacity", 0.8); - - group = group.add(circle); - } - - group - } - - /// Render enhanced legend with all visual elements. - fn render_enhanced_legend(&self, _svg_height: f32) -> Group { - let mut group = Group::new().set("id", "enhanced_legend"); - - let legend_x = 10.0; - let mut legend_y = 20.0; - let item_height = 16.0; - let icon_width = 20.0; - - let items = vec![ - (colors::MAP_WALLS, "Map Walls", "rect"), - (colors::DETECTED_WALLS, "Detected Walls", "line"), - (colors::LIDAR_SCAN, "Lidar Scan", "circle"), - (colors::TRUTH_PATH, "Ground Truth Path", "line"), - ("gradient", "Estimated Path (Confidence)", "gradient"), - (colors::DRIFT_VECTOR, "Drift Vector", "arrow"), - (colors::GRAPH_NODE, "Graph Nodes", "circle"), - (colors::PLANNED_PATH, "Planned Path", "line"), - ]; - - for (color, label, icon_type) in items { - // Draw icon based on type - match icon_type { - "rect" => { - let rect = Rectangle::new() - .set("x", legend_x) - .set("y", legend_y - 8.0) - .set("width", icon_width) - .set("height", 10.0) - .set("fill", color); - group = group.add(rect); - } - "line" => { - let line = Line::new() - .set("x1", legend_x) - .set("y1", legend_y - 3.0) - .set("x2", legend_x + icon_width) - .set("y2", legend_y - 3.0) - .set("stroke", color) - .set("stroke-width", 2.5); - group = group.add(line); - } - "circle" => { - let circle = Circle::new() - .set("cx", legend_x + icon_width / 2.0) - .set("cy", legend_y - 3.0) - .set("r", 4.0) - .set("fill", color); - group = group.add(circle); - } - "gradient" => { - // Show green-yellow-red gradient - let gradient_steps = 5; - let step_width = icon_width / gradient_steps as f32; - for i in 0..gradient_steps { - let conf = i as f32 / (gradient_steps - 1) as f32; - let grad_color = Self::confidence_to_color(conf); - let rect = Rectangle::new() - .set("x", legend_x + i as f32 * step_width) - .set("y", legend_y - 8.0) - .set("width", step_width + 0.5) - .set("height", 10.0) - .set("fill", grad_color); - group = group.add(rect); - } - } - "arrow" => { - let line = Line::new() - .set("x1", legend_x) - .set("y1", legend_y - 3.0) - .set("x2", legend_x + icon_width) - .set("y2", legend_y - 3.0) - .set("stroke", color) - .set("stroke-width", 1.5) - .set("marker-end", "url(#drift-arrow)"); - group = group.add(line); - } - _ => {} - } - - // Draw label - let text = Text::new(label) - .set("x", legend_x + icon_width + 8.0) - .set("y", legend_y) - .set("font-size", 11) - .set("font-family", "sans-serif") - .set("fill", "black"); - group = group.add(text); - - legend_y += item_height; - } - - group - } - - /// Render metrics overlay in top-right corner. - fn render_metrics_overlay(&self, metrics: &TestMetrics, width: i32) -> Group { - let mut group = Group::new().set("id", "metrics_overlay"); - - let box_width = 210.0; - let box_height = 110.0; - let box_x = width as f32 - box_width - 10.0; - let box_y = 10.0; - - // Semi-transparent background - let bg = Rectangle::new() - .set("x", box_x) - .set("y", box_y) - .set("width", box_width) - .set("height", box_height) - .set("fill", "white") - .set("fill-opacity", 0.9) - .set("stroke", "#ccc") - .set("stroke-width", 1) - .set("rx", 5); - group = group.add(bg); - - // Title - let title = Text::new("Test Metrics") - .set("x", box_x + 10.0) - .set("y", box_y + 18.0) - .set("font-size", 12) - .set("font-family", "sans-serif") - .set("font-weight", "bold") - .set("fill", "black"); - group = group.add(title); - - let mut text_y = box_y + 35.0; - let line_height = 15.0; - - let lines = vec![ - format!("Position Error: {:.3} m", metrics.position_error), - format!( - "Orientation Error: {:.1}°", - metrics.orientation_error.to_degrees() - ), - format!("Wall Accuracy: {:.0}%", metrics.wall_accuracy * 100.0), - format!("Lines Detected: {}", metrics.detected_walls), - format!("Avg ICP Iters: {:.1}", metrics.convergence.avg_iterations()), - ]; - - for line in lines { - let text = Text::new(line) - .set("x", box_x + 10.0) - .set("y", text_y) - .set("font-size", 11) - .set("font-family", "monospace") - .set("fill", "black"); - group = group.add(text); - text_y += line_height; - } - - group - } - - /// Render map walls by sampling the occupancy grid. - fn render_map_walls(&self, map: &SimulationMap, svg_height: f32) -> Group { - let mut group = Group::new().set("id", "map_walls"); - - let resolution = map.resolution(); - let sample_step = resolution * 2.0; // Sample every 2 pixels for efficiency - - let map_width_m = map.width() as f32 * resolution; - let map_height_m = map.height() as f32 * resolution; - - // Sample the map and draw small rectangles for occupied cells - let mut x = 0.0; - while x < map_width_m { - let mut y = 0.0; - while y < map_height_m { - if map.is_occupied(x, y) { - let (sx, sy) = self.transform_point(Point2D::new(x, y), 0.0, 0.0, svg_height); - let rect_size = sample_step * self.scale; - - let rect = Rectangle::new() - .set("x", sx - rect_size / 2.0) - .set("y", sy - rect_size / 2.0) - .set("width", rect_size) - .set("height", rect_size) - .set("fill", colors::MAP_WALLS) - .set("stroke", "none"); - - group = group.add(rect); - } - y += sample_step; - } - x += sample_step; - } - - group - } - - fn render_detected(&self, slam: &VectorMap, svg_height: f32) -> Group { - let mut group = Group::new().set("id", "detected"); - - for line in slam.lines() { - let svg_line = self.create_line( - line.start, - line.end, - 0.0, - 0.0, - svg_height, - colors::DETECTED_WALLS, - 2.0, - None, - ); - group = group.add(svg_line); - } - - group - } - - fn render_poses(&self, estimated: Pose2D, truth: Pose2D, svg_height: f32) -> Group { - let mut group = Group::new().set("id", "poses"); - - // Ground truth pose (blue) - group = - group.add(self.create_robot_marker(truth, 0.0, 0.0, svg_height, colors::TRUTH_PATH)); - - // Estimated pose (orange) - group = group.add(self.create_robot_marker( - estimated, - 0.0, - 0.0, - svg_height, - colors::ESTIMATED_PATH, - )); - - group - } - - fn transform_point( - &self, - p: Point2D, - origin_x: f32, - origin_y: f32, - svg_height: f32, - ) -> (f32, f32) { - // Transform world coordinates to SVG coordinates - // SVG has Y increasing downward, world has Y increasing upward - let x = (p.x - origin_x) * self.scale + self.margin; - let y = svg_height - ((p.y - origin_y) * self.scale + self.margin); - (x, y) - } - - fn create_line( - &self, - start: Point2D, - end: Point2D, - origin_x: f32, - origin_y: f32, - svg_height: f32, - color: &str, - width: f32, - dash: Option<&str>, - ) -> Line { - let (x1, y1) = self.transform_point(start, origin_x, origin_y, svg_height); - let (x2, y2) = self.transform_point(end, origin_x, origin_y, svg_height); - - let mut line = Line::new() - .set("x1", x1) - .set("y1", y1) - .set("x2", x2) - .set("y2", y2) - .set("stroke", color) - .set("stroke-width", width); - - if let Some(pattern) = dash { - line = line.set("stroke-dasharray", pattern); - } - - line - } - - fn create_robot_marker( - &self, - pose: Pose2D, - origin_x: f32, - origin_y: f32, - svg_height: f32, - color: &str, - ) -> Group { - let (cx, cy) = self.transform_point(pose.position(), origin_x, origin_y, svg_height); - - // Robot body (circle) - let radius = 10.0; - let body = Circle::new() - .set("cx", cx) - .set("cy", cy) - .set("r", radius) - .set("fill", color) - .set("opacity", 0.7); - - // Direction indicator (line) - let dir_length = 15.0; - // Note: In SVG, Y increases downward, so we negate the sin component - let dir_x = cx + dir_length * pose.theta.cos(); - let dir_y = cy - dir_length * pose.theta.sin(); - - let direction = Line::new() - .set("x1", cx) - .set("y1", cy) - .set("x2", dir_x) - .set("y2", dir_y) - .set("stroke", color) - .set("stroke-width", 3); - - Group::new().add(body).add(direction) - } - - /// Render a clearance-based visibility graph showing nodes and edges. - /// - /// # Arguments - /// * `graph` - The CBVG to render - /// * `svg_height` - SVG canvas height for coordinate transformation - /// * `show_edges` - Whether to show edges (can be noisy for large graphs) - pub fn render_cbvg( - &self, - graph: &ClearanceVisibilityGraph, - svg_height: f32, - show_edges: bool, - ) -> Group { - let mut group = Group::new().set("id", "cbvg"); - - let nodes = graph.nodes(); - - // Render edges first (so nodes appear on top) - if show_edges { - let edge_group = self.render_cbvg_edges(graph, svg_height); - group = group.add(edge_group); - } - - // Render nodes - for node in nodes { - let (sx, sy) = self.transform_point(node.position, 0.0, 0.0, svg_height); - - // Choose color and size based on node type - let (color, radius, opacity) = match node.node_type { - NodeType::Robot => (colors::START_NODE, 6.0, 0.9), - NodeType::Goal => (colors::GOAL_NODE, 6.0, 0.9), - NodeType::Junction => ("#9966FF", 5.0, 0.8), // Purple for junctions - NodeType::MedialAxis => { - if node.is_narrow_passage { - ("#FF9900", 4.0, 0.7) // Orange for narrow passage nodes - } else { - (colors::GRAPH_NODE, 4.0, 0.7) // Default color for medial axis - } - } - }; - - let circle = Circle::new() - .set("cx", sx) - .set("cy", sy) - .set("r", radius) - .set("fill", color) - .set("opacity", opacity); - - group = group.add(circle); - } - - group - } - - /// Render CBVG edges. - fn render_cbvg_edges(&self, graph: &ClearanceVisibilityGraph, svg_height: f32) -> Group { - let mut group = Group::new().set("id", "cbvg_edges"); - - let nodes = graph.nodes(); - let edges = graph.edges(); - - for (from_idx, neighbors) in edges.iter().enumerate() { - for (to_idx, _dist) in neighbors { - // Only draw each edge once (when from_idx < to_idx) - if from_idx < *to_idx { - let from_node = nodes[from_idx].position; - let to_node = nodes[*to_idx].position; - - let line = self.create_line( - from_node, - to_node, - 0.0, - 0.0, - svg_height, - colors::GRAPH_EDGE, - 0.5, - Some("2,2"), // Dashed line for edges - ); - - group = group.add(line.set("opacity", 0.3)); - } - } - } - - group - } - - /// Render a planned path. - /// - /// # Arguments - /// * `path` - The planned path to render - /// * `svg_height` - SVG canvas height for coordinate transformation - pub fn render_planned_path(&self, path: &PlannedPath, svg_height: f32) -> Group { - let mut group = Group::new().set("id", "planned_path"); - - if path.points.len() < 2 { - return group; - } - - // Render path as connected line segments - for i in 1..path.points.len() { - let from = path.points[i - 1]; - let to = path.points[i]; - - let line = self.create_line( - from, - to, - 0.0, - 0.0, - svg_height, - colors::PLANNED_PATH, - 3.0, - None, - ); - - group = group.add(line); - } - - // Render waypoint markers - for (i, &point) in path.points.iter().enumerate() { - let (sx, sy) = self.transform_point(point, 0.0, 0.0, svg_height); - - // First and last points are start/goal, middle points are waypoints - let (color, radius) = if i == 0 { - (colors::START_NODE, 5.0) - } else if i == path.points.len() - 1 { - (colors::GOAL_NODE, 5.0) - } else { - (colors::PLANNED_PATH, 3.0) - }; - - let circle = Circle::new() - .set("cx", sx) - .set("cy", sy) - .set("r", radius) - .set("fill", color) - .set("stroke", "white") - .set("stroke-width", 1.0); - - group = group.add(circle); - } - - group - } - - /// Render full visualization with clearance-based visibility graph. - /// - /// Extended version of `render_full` that also shows the CBVG - /// and planned path for path planning debugging. - #[allow(clippy::too_many_arguments)] - pub fn render_with_path_planning( - &self, - slam: &VectorMap, - map: &SimulationMap, - final_pose: Pose2D, - truth_pose: Pose2D, - scan_store: &ScanStore, - trajectory: &TrajectoryHistory, - metrics: &TestMetrics, - graph: Option<&ClearanceVisibilityGraph>, - path: Option<&PlannedPath>, - ) { - // Compute bounds from map - let map_width_m = map.width() as f32 * map.resolution(); - let map_height_m = map.height() as f32 * map.resolution(); - - let width = (map_width_m * self.scale + 2.0 * self.margin) as i32; - let height = (map_height_m * self.scale + 2.0 * self.margin) as i32; - - // Create document with arrow marker definition - let mut doc = Document::new() - .set("width", width) - .set("height", height) - .set("viewBox", (0, 0, width, height)); - - // Add marker definitions for arrows - let defs = self.create_marker_definitions(); - doc = doc.add(defs); - - // Layer 0: Background - doc = doc.add( - Rectangle::new() - .set("x", 0) - .set("y", 0) - .set("width", width) - .set("height", height) - .set("fill", "white"), - ); - - // Layer 1: Map walls (gray) - doc = doc.add(self.render_map_walls(map, height as f32)); - - // Layer 2: All scans from scan store (semi-transparent) - doc = doc.add(self.render_all_scans(scan_store, height as f32)); - - // Layer 3: Final lidar scan (full opacity) - use last scan from store - if let Some(last_scan) = scan_store.latest() { - let world_points = last_scan.world_points(); - doc = doc.add(self.render_world_points(&world_points, height as f32)); - } - - // Layer 4: Detected walls (teal) - doc = doc.add(self.render_detected(slam, height as f32)); - - // Layer 5: Clearance-based visibility graph (if provided) - if let Some(g) = graph { - doc = doc.add(self.render_cbvg(g, height as f32, true)); - } - - // Layer 6: Planned path (if provided) - if let Some(p) = path { - doc = doc.add(self.render_planned_path(p, height as f32)); - } - - // Layer 7: Trajectories with confidence heatmap - doc = doc.add(self.render_trajectories(trajectory, height as f32)); - - // Layer 8: Drift vectors - doc = doc.add(self.render_drift_vectors(trajectory, height as f32)); - - // Layer 9: Final poses - doc = doc.add(self.render_poses(final_pose, truth_pose, height as f32)); - - // Layer 10: Enhanced legend - doc = doc.add(self.render_enhanced_legend(height as f32)); - - // Layer 11: Metrics overlay - doc = doc.add(self.render_metrics_overlay(metrics, width)); - - // Ensure output directory exists - if let Some(parent) = std::path::Path::new(&self.output_path).parent() { - std::fs::create_dir_all(parent).ok(); - } - - // Save - svg::save(&self.output_path, &doc).expect("Failed to save SVG"); - } -} diff --git a/vastu-map/examples/exploration/main.rs b/vastu-map/examples/exploration/main.rs deleted file mode 100644 index 08f7886..0000000 --- a/vastu-map/examples/exploration/main.rs +++ /dev/null @@ -1,502 +0,0 @@ -//! Autonomous exploration example -//! -//! Demonstrates the complete exploration pipeline using vastu_map's public APIs: -//! 1. Frontier detection -//! 2. Path planning to frontiers -//! 3. Path following with collision detection -//! 4. Virtual wall insertion on unexpected collisions -//! 5. Exploration completion when no frontiers remain -//! -//! # Running -//! -//! ```bash -//! # With a specific map file: -//! cargo run --example exploration --release -- path/to/map.yaml -//! -//! # With random map selection: -//! cargo run --example exploration --release -//! ``` -//! -//! Output SVG is saved to `results/exploration_test.svg` - -mod harness; - -use harness::{HarnessConfig, TestHarness}; -use rand::seq::IndexedRandom; -use sangam_io::devices::mock::map_loader::SimulationMap; -use vastu_map::Map; -use vastu_map::Path as PlannedPath; -use vastu_map::config::ExplorationConfig as MapExplorationConfig; -use vastu_map::core::{Point2D, Pose2D}; -use vastu_map::exploration::{ - CollisionEvent, CollisionType, ExplorationConfig, ExplorationController, ExplorationState, - VelocityCommand, -}; -use vastu_map::integration::CoplanarMergeConfig; -use vastu_map::query::cbvg::ClearanceVisibilityGraph; - -/// Maximum simulation time in seconds (no limit). -const MAX_TIME: f32 = f32::MAX; - -/// Directory containing available maps. -const MAPS_DIR: &str = concat!(env!("CARGO_MANIFEST_DIR"), "/data/maps"); - -/// Select a random map file from the maps directory. -/// -/// # Returns -/// The full path to a randomly selected .yaml map file. -/// -/// # Panics -/// If no map files are found in the directory. -fn select_random_map() -> String { - let maps_path = std::path::Path::new(MAPS_DIR); - - // Collect all .yaml files in the maps directory - let map_files: Vec<_> = std::fs::read_dir(maps_path) - .expect("Failed to read maps directory") - .filter_map(|entry| { - let entry = entry.ok()?; - let path = entry.path(); - if path.extension().is_some_and(|ext| ext == "yaml") { - Some(path) - } else { - None - } - }) - .collect(); - - if map_files.is_empty() { - panic!("No map files found in {}", MAPS_DIR); - } - - // Pick a random map - let mut rng = rand::rng(); - let selected = map_files.choose(&mut rng).unwrap(); - let map_name = selected.file_stem().unwrap().to_string_lossy(); - - println!("Selected random map: {}", map_name); - println!(" (from {} available maps)", map_files.len()); - - selected.to_string_lossy().into_owned() -} - -/// Find a random reachable (free) position in the map with sufficient clearance. -/// -/// # Arguments -/// * `map_file` - Path to the map YAML file -/// * `robot_radius` - Robot radius in meters for clearance checking -/// -/// # Returns -/// A tuple (x, y) of world coordinates for a valid start position. -/// -/// # Panics -/// If no valid position can be found after sampling. -fn find_random_start_position(map_file: &str, robot_radius: f32) -> (f32, f32) { - let map = SimulationMap::load(map_file).expect("Failed to load map for position sampling"); - - // Collect all free pixels with sufficient clearance - let mut valid_positions = Vec::new(); - - // Calculate clearance in pixels (add some margin) - let clearance_pixels = ((robot_radius * 1.5) / map.resolution()).ceil() as i32; - - for py in 0..map.height() { - for px in 0..map.width() { - let (x, y) = map.pixel_to_world(px, py); - - // Check if this position is free - if map.is_occupied(x, y) { - continue; - } - - // Check clearance around the robot - let mut has_clearance = true; - 'clearance: for dy in -clearance_pixels..=clearance_pixels { - for dx in -clearance_pixels..=clearance_pixels { - let check_x = x + dx as f32 * map.resolution(); - let check_y = y + dy as f32 * map.resolution(); - if map.is_occupied(check_x, check_y) { - has_clearance = false; - break 'clearance; - } - } - } - - if has_clearance { - valid_positions.push((x, y)); - } - } - } - - if valid_positions.is_empty() { - panic!("No valid start positions found in map with sufficient clearance"); - } - - // Pick a random position - let mut rng = rand::rng(); - let (x, y) = *valid_positions.choose(&mut rng).unwrap(); - - println!("Selected random start position: ({:.2}, {:.2})", x, y); - println!( - " (from {} valid positions with {:.2}m clearance)", - valid_positions.len(), - robot_radius * 1.5 - ); - - (x, y) -} - -/// Result of exploration. -#[derive(Debug)] -pub struct ExplorationResult { - /// Whether exploration completed (no frontiers remain). - pub completed: bool, - /// Total simulation time. - pub sim_time: f32, - /// Number of exploration cycles. - pub cycles: usize, - /// Number of collisions handled. - pub collisions: usize, - /// Number of virtual walls added. - pub virtual_walls: usize, - /// Map line count before optimization. - pub map_lines_before: usize, - /// Final map line count (after optimization). - pub map_lines: usize, - /// Number of lines merged during optimization. - pub lines_merged: usize, - /// Initial frontier count. - pub initial_frontiers: usize, - /// Final frontier count (should be 0 on success). - pub remaining_frontiers: usize, -} - -fn main() { - env_logger::init(); - - // Ensure results directory exists - std::fs::create_dir_all("results").ok(); - - // Check for command-line argument for map file - let args: Vec = std::env::args().collect(); - let map_file = if args.len() > 1 { - let provided_map = &args[1]; - // Check if file exists - if !std::path::Path::new(provided_map).exists() { - eprintln!("Error: Map file not found: {}", provided_map); - std::process::exit(1); - } - println!("Using provided map: {}", provided_map); - provided_map.clone() - } else { - // Select a random map from available maps - select_random_map() - }; - - // Get default robot radius for clearance checking - let default_config = HarnessConfig::default(); - let robot_radius = default_config.robot.robot_radius; - - // Find a random reachable start position - let (start_x, start_y) = find_random_start_position(&map_file, robot_radius); - - // Setup harness with map file and random starting position - let harness_config = HarnessConfig { - map_file, - start_x, - start_y, - ..Default::default() - }; - - // Enable VectorMap exploration mode for point-cloud-based line re-fitting - // This will re-fit lines every 10 observations using accumulated scan data - let map_exploration_config = MapExplorationConfig::default() - .with_refit_interval(10) - .with_gap_threshold(0.3); - - let mut harness = TestHarness::new(harness_config) - .expect("Failed to create harness") - .with_exploration_mode(Some(map_exploration_config)) - .with_visualization("results/exploration_test.svg"); - - // Create robot exploration config (for path following) - let exploration_config = ExplorationConfig::default() - .with_max_linear_speed(0.3) - .with_max_angular_speed(1.0) - .with_backoff_distance(0.15) - .with_robot_radius(0.15); - - // Run exploration - let result = run_exploration_loop(&mut harness, exploration_config); - - // Print results - println!("\n=== Exploration Results ==="); - println!("Completed: {}", result.completed); - println!("Simulation time: {:.1}s", result.sim_time); - println!("Cycles: {}", result.cycles); - println!("Collisions handled: {}", result.collisions); - println!("Virtual walls added: {}", result.virtual_walls); - println!( - "Map lines: {} before optimization -> {} after ({} merged)", - result.map_lines_before, result.map_lines, result.lines_merged - ); - println!( - "Frontiers: {} initial -> {} remaining", - result.initial_frontiers, result.remaining_frontiers - ); - println!("\nVisualization saved to: results/exploration_test.svg"); -} - -/// Run the exploration loop using the ExplorationController with region-based TSP. -/// -/// This uses a continuous control loop where the controller is called every step, -/// properly integrating with its state machine for path following and collision handling. -fn run_exploration_loop(harness: &mut TestHarness, config: ExplorationConfig) -> ExplorationResult { - let mut cycles = 0; - let mut collisions = 0; - let mut virtual_walls = 0; - - // Variables to capture the last CBVG and path for visualization - let mut last_cbvg: Option = None; - let mut last_planned_path: Option = None; - - println!("Starting exploration with region-based TSP planning..."); - - // Create the exploration controller with region-based algorithm - let mut controller = ExplorationController::new(config.clone()); - - // Phase 1: Initial map update (longer for initial observation) - wait_for_map_update(harness, 2.0); - - let initial_frontiers = harness.slam().frontiers().len(); - println!("Initial frontiers: {}", initial_frontiers); - - // Start exploration - controller.start(); - - let mut step_count = 0; - let mut last_target: Option = None; - let mut last_cycle_print = 0; - let mut pending_collision: Option = None; - const MAX_STEPS: usize = usize::MAX; - const LIDAR_INTERVAL: usize = 5; // Process lidar every N steps - - while step_count < MAX_STEPS && harness.sim_time() < MAX_TIME { - step_count += 1; - - // Get current pose - let physics_pose = harness.physics_pose(); - let current_pose = Pose2D::new(physics_pose.x, physics_pose.y, physics_pose.theta); - - // Update controller with any pending collision - let step = controller.update(harness.slam(), current_pose, pending_collision.take()); - - // Check completion states - match &step.state { - ExplorationState::Complete => { - println!(" Exploration complete - no frontiers remain!"); - break; - } - ExplorationState::Failed { reason } => { - println!(" Exploration failed - {}", reason); - break; - } - ExplorationState::Idle => { - break; - } - ExplorationState::SelectingFrontier => { - // Step simulation forward while selecting - harness.step(0.0, 0.0); - // Process lidar while selecting - if step_count % LIDAR_INTERVAL == 0 { - harness.process_lidar(); - } - } - ExplorationState::FollowingPath { - target_frontier, - path, - .. - } => { - // Track cycles (new target = new cycle) - let is_new_target = last_target - .map(|last| last.distance(*target_frontier) > 0.3) - .unwrap_or(true); - - if is_new_target { - cycles += 1; - last_target = Some(*target_frontier); - - let robot_pos = current_pose.position(); - println!( - " Cycle {}: Path to ({:.2}, {:.2}), dist: {:.2}m, path: {:.2}m, {} waypoints", - cycles, - target_frontier.x, - target_frontier.y, - robot_pos.distance(*target_frontier), - path.length, - path.points.len() - ); - - // Capture CBVG for visualization using visibility-based building - // This ensures nodes are only in lidar-scanned areas - last_cbvg = Some(harness.slam().build_cbvg()); - last_planned_path = Some(path.clone()); - } - - // Execute velocity command from controller (or stop if none) - let vel = step.velocity.unwrap_or(VelocityCommand::new(0.0, 0.0)); - let step_result = harness.step(vel.linear, vel.angular); - - // Check for collision - if step_result.any_bumper() { - collisions += 1; - - let collision_type = match (step_result.left_bumper, step_result.right_bumper) { - (true, true) => CollisionType::BumperBoth, - (true, false) => CollisionType::BumperLeft, - (false, true) => CollisionType::BumperRight, - _ => CollisionType::BumperBoth, - }; - - let theta = step_result.physics_pose.theta; - let radius = harness.config().robot.robot_radius; - let point = Point2D::new( - step_result.physics_pose.x + (radius + 0.02) * theta.cos(), - step_result.physics_pose.y + (radius + 0.02) * theta.sin(), - ); - - // Create collision event for controller - pending_collision = Some(CollisionEvent::new(collision_type, point, theta)); - - // Also add virtual wall directly to map - let wall = pending_collision - .as_ref() - .unwrap() - .to_virtual_wall(config.virtual_wall_length); - harness.slam_mut().add_line(wall.line); - virtual_walls += 1; - - println!( - " Collision at ({:.2}, {:.2}) - added virtual wall", - point.x, point.y - ); - } - - // Process lidar periodically - if step_count % LIDAR_INTERVAL == 0 { - harness.process_lidar(); - } - - // Log progress periodically - if cycles > last_cycle_print && step_count % 100 == 0 { - let end_pos = harness.physics_pose().position(); - let current_frontiers = harness.slam().frontiers().len(); - println!( - " Position: ({:.2}, {:.2}), frontiers: {}, unreachable: {}", - end_pos.x, - end_pos.y, - current_frontiers, - controller.unreachable_frontier_count() - ); - last_cycle_print = cycles; - } - } - ExplorationState::RecoveringFromCollision { .. } => { - // Execute backoff velocity (or stop if none) - let vel = step.velocity.unwrap_or(VelocityCommand::new(0.0, 0.0)); - let step_result = harness.step(vel.linear, vel.angular); - - // Check for collision during recovery (defensive - should be rare) - if step_result.any_bumper() { - collisions += 1; - - let collision_type = match (step_result.left_bumper, step_result.right_bumper) { - (true, true) => CollisionType::BumperBoth, - (true, false) => CollisionType::BumperLeft, - (false, true) => CollisionType::BumperRight, - _ => CollisionType::BumperBoth, - }; - - let theta = step_result.physics_pose.theta; - let radius = harness.config().robot.robot_radius; - let point = Point2D::new( - step_result.physics_pose.x + (radius + 0.02) * theta.cos(), - step_result.physics_pose.y + (radius + 0.02) * theta.sin(), - ); - - // Create collision event for controller - pending_collision = Some(CollisionEvent::new(collision_type, point, theta)); - - // Also add virtual wall directly to map - let wall = pending_collision - .as_ref() - .unwrap() - .to_virtual_wall(config.virtual_wall_length); - harness.slam_mut().add_line(wall.line); - virtual_walls += 1; - - println!( - " Recovery collision at ({:.2}, {:.2}) - added virtual wall", - point.x, point.y - ); - } - } - } - } - - // Final log - let end_pos = harness.physics_pose().position(); - let current_frontiers = harness.slam().frontiers().len(); - println!( - " Final position: ({:.2}, {:.2}), frontiers: {}, unreachable: {}", - end_pos.x, - end_pos.y, - current_frontiers, - controller.unreachable_frontier_count() - ); - - // Get line count before optimization - let map_lines_before = harness.slam().lines().len(); - - // Optimize coplanar lines with tight thresholds - let coplanar_config = CoplanarMergeConfig::default() - .with_max_angle_diff(3.0 * std::f32::consts::PI / 180.0) - .with_max_perpendicular_dist(0.05); - - let lines_merged = harness.slam_mut().optimize_lines(&coplanar_config); - - println!( - "\n Line optimization: {} lines -> {} lines ({} merged)", - map_lines_before, - harness.slam().lines().len(), - lines_merged - ); - - // Finalize to generate visualization with CBVG - harness.finalize_with_graph(last_cbvg.as_ref(), last_planned_path.as_ref()); - - ExplorationResult { - completed: harness.slam().frontiers().is_empty(), - sim_time: harness.sim_time(), - cycles, - collisions, - virtual_walls, - map_lines_before, - map_lines: harness.slam().lines().len(), - lines_merged, - initial_frontiers, - remaining_frontiers: harness.slam().frontiers().len(), - } -} - -/// Wait for map update by running lidar scans for specified duration. -fn wait_for_map_update(harness: &mut TestHarness, duration: f32) { - let start_time = harness.sim_time(); - - while harness.sim_time() - start_time < duration { - // Stop the robot - harness.step(0.0, 0.0); - // Process lidar - harness.process_lidar(); - } -} diff --git a/vastu-map/examples/explore_mock.rs b/vastu-map/examples/explore_mock.rs new file mode 100644 index 0000000..36d2c29 --- /dev/null +++ b/vastu-map/examples/explore_mock.rs @@ -0,0 +1,726 @@ +//! Mock exploration example with encoder odometry and scan matching +//! +//! Demonstrates autonomous frontier-based exploration using a simulated robot +//! with realistic encoder drift and scan-to-map matching for correction. +//! +//! Usage: +//! cargo run --example explore_mock -- --map simple_room +//! cargo run --example explore_mock # Uses random map +//! +//! Enable debug logging to see drift metrics: +//! RUST_LOG=info cargo run --example explore_mock + +use clap::Parser; +use rand::prelude::*; +use std::f32::consts::{PI, TAU}; +use std::path::Path; +use std::time::Instant; + +// SangamIO mock simulation components +use sangam_io::devices::mock::{ + config::{EncoderConfig, LidarConfig, RobotConfig}, + encoder_sim::EncoderSimulator, + lidar_sim::LidarSimulator, + map_loader::SimulationMap, + noise::NoiseGenerator, + physics::PhysicsState, +}; + +// Vastu-map components +use vastu_map::{ + LidarScan, OccupancyGridMap, Pose2D, VastuConfig, + exploration::ExplorationController, + io::{export_ros_map, save_vastu}, + slam::{CorrelativeMatcher, CorrelativeMatcherConfig}, +}; + +/// Mock exploration example +#[derive(Parser, Debug)] +#[command(author, version, about, long_about = None)] +struct Args { + /// Map name to load from maps/ folder (e.g., "simple_room") + /// If not specified, picks a random map + #[arg(short, long)] + map: Option, + + /// Configuration file path + #[arg(short, long, default_value = "configs/config.yaml")] + config: String, + + /// Output directory for saved maps + #[arg(short, long, default_value = "./output")] + output: String, + + /// Maximum simulation steps (0 = unlimited) + #[arg(long, default_value = "10000")] + max_steps: usize, + + /// Simulation speed factor (1.0 = realtime) + #[arg(long, default_value = "10.0")] + speed: f32, + + /// Show progress every N steps + #[arg(long, default_value = "100")] + progress_interval: usize, +} + +fn main() { + env_logger::init(); + let args = Args::parse(); + + // List available maps + let maps_dir = Path::new("maps"); + let available_maps = list_available_maps(maps_dir); + + if available_maps.is_empty() { + eprintln!("No maps found in maps/ folder"); + std::process::exit(1); + } + + // Select map + let map_name = args.map.unwrap_or_else(|| { + let mut rng = rand::rng(); + let selected = available_maps.choose(&mut rng).unwrap().clone(); + println!("Randomly selected map: {}", selected); + selected + }); + + let map_yaml = maps_dir.join(format!("{}.yaml", map_name)); + if !map_yaml.exists() { + eprintln!("Map not found: {}", map_yaml.display()); + eprintln!("Available maps: {:?}", available_maps); + std::process::exit(1); + } + + // Load configuration + let config_path = Path::new(&args.config); + let vastu_config = if config_path.exists() { + VastuConfig::load(config_path).unwrap_or_else(|e| { + eprintln!("Warning: Failed to load config: {}, using defaults", e); + VastuConfig::default() + }) + } else { + println!("Config not found, using defaults"); + VastuConfig::default() + }; + + // Load simulation map + println!("Loading map: {}", map_yaml.display()); + let sim_map = SimulationMap::load(&map_yaml).expect("Failed to load simulation map"); + println!( + "Map size: {}x{} pixels, resolution: {}m/px", + sim_map.width(), + sim_map.height(), + sim_map.resolution() + ); + + // Find starting position (search for free space in grid bounds) + let start_pose = find_free_start_position(&sim_map, &vastu_config); + println!( + "Starting at: ({:.2}, {:.2}, {:.1}°)", + start_pose.x, + start_pose.y, + start_pose.theta.to_degrees() + ); + + // Create output directory + std::fs::create_dir_all(&args.output).ok(); + + // Run exploration + let result = run_exploration( + &sim_map, + start_pose, + &vastu_config, + args.max_steps, + args.speed, + args.progress_interval, + ); + + // Save map + let output_base = Path::new(&args.output).join(&map_name); + + println!("\nSaving maps..."); + save_vastu(result.map.storage(), &output_base.with_extension("vastu")) + .expect("Failed to save .vastu file"); + println!(" Saved: {}.vastu", output_base.display()); + + export_ros_map(result.map.storage(), &output_base).expect("Failed to save ROS map"); + println!( + " Saved: {}.pgm + {}.yaml", + output_base.display(), + output_base.display() + ); + + // Print summary + println!("\n=== Exploration Summary ==="); + println!("Status: {:?}", result.status); + println!("Steps: {}", result.steps); + println!("Time: {:.1}s", result.elapsed_secs); + println!( + "Distance traveled: {:.2}m", + result.progress.distance_traveled + ); + println!("Frontiers explored: {}", result.progress.frontiers_explored); + + let map_stats = result.map.coverage_stats(); + println!( + "Cells explored: {} floor, {} wall, {} unknown", + map_stats.floor_cells, map_stats.wall_cells, map_stats.unknown_cells + ); + println!("Coverage: {:.2}m²", map_stats.explored_area_m2); + + // Drift and scan matching statistics + println!("\n=== Scan Matching Stats ==="); + let ds = &result.drift_stats; + let total_matches = ds.successful_matches + ds.failed_matches; + let success_rate = if total_matches > 0 { + 100.0 * ds.successful_matches as f32 / total_matches as f32 + } else { + 0.0 + }; + println!( + "Matches: {} successful, {} failed ({:.1}% success)", + ds.successful_matches, ds.failed_matches, success_rate + ); + println!( + "Total drift corrected: {:.3}m translation, {:.2}° rotation", + ds.total_translation_drift, + ds.total_rotation_drift.to_degrees() + ); + println!( + "Max drift error: {:.3}m translation, {:.2}° rotation", + ds.max_translation_error, + ds.max_rotation_error.to_degrees() + ); +} + +/// List available map names in directory +fn list_available_maps(dir: &Path) -> Vec { + let mut maps = Vec::new(); + if let Ok(entries) = std::fs::read_dir(dir) { + for entry in entries.flatten() { + let path = entry.path(); + if path.extension().is_some_and(|e| e == "yaml") { + if let Some(stem) = path.file_stem() { + maps.push(stem.to_string_lossy().to_string()); + } + } + } + } + maps.sort(); + maps +} + +/// Find a free starting position in the map +/// Constrains to the overlapping region of simulation map and vastu-map grid +fn find_free_start_position(map: &SimulationMap, vastu_config: &VastuConfig) -> Pose2D { + let mut rng = rand::rng(); + let sim_origin = map.origin(); + let sim_width_m = map.width() as f32 * map.resolution(); + let sim_height_m = map.height() as f32 * map.resolution(); + + // Vastu-map grid bounds (centered) + let grid_size_x = vastu_config.grid.initial_width as f32 * vastu_config.grid.resolution; + let grid_size_y = vastu_config.grid.initial_height as f32 * vastu_config.grid.resolution; + let grid_min_x = -grid_size_x / 2.0; + let grid_min_y = -grid_size_y / 2.0; + let grid_max_x = grid_size_x / 2.0; + let grid_max_y = grid_size_y / 2.0; + + // Find overlap region + let min_x = sim_origin.0.max(grid_min_x) + 0.5; + let min_y = sim_origin.1.max(grid_min_y) + 0.5; + let max_x = (sim_origin.0 + sim_width_m).min(grid_max_x) - 0.5; + let max_y = (sim_origin.1 + sim_height_m).min(grid_max_y) - 0.5; + + // Try random positions within overlap region + for _ in 0..1000 { + let x = min_x + rng.random::() * (max_x - min_x); + let y = min_y + rng.random::() * (max_y - min_y); + + // Check if position is free (not occupied, with margin) + if !map.is_occupied(x, y) + && !map.is_occupied(x + 0.2, y) + && !map.is_occupied(x - 0.2, y) + && !map.is_occupied(x, y + 0.2) + && !map.is_occupied(x, y - 0.2) + { + let theta = rng.random::() * TAU; + return Pose2D::new(x, y, theta); + } + } + + // Fallback: scan for any free position in overlap + for py in 0..map.height() { + for px in 0..map.width() { + let (x, y) = map.pixel_to_world(px, py); + if x >= min_x && x <= max_x && y >= min_y && y <= max_y && !map.is_occupied(x, y) { + return Pose2D::new(x, y, 0.0); + } + } + } + + // Last resort: center of overlap + Pose2D::new((min_x + max_x) / 2.0, (min_y + max_y) / 2.0, 0.0) +} + +/// Exploration result +struct ExplorationResult { + map: OccupancyGridMap, + status: ExplorationStatus, + steps: usize, + elapsed_secs: f32, + progress: vastu_map::exploration::ExplorationProgress, + drift_stats: DriftStats, +} + +/// Drift statistics for encoder vs corrected pose +#[derive(Debug, Default)] +struct DriftStats { + /// Total translation drift (meters) + total_translation_drift: f32, + /// Total rotation drift (radians) + total_rotation_drift: f32, + /// Maximum translation error observed + max_translation_error: f32, + /// Maximum rotation error observed + max_rotation_error: f32, + /// Number of successful scan matches + successful_matches: usize, + /// Number of failed scan matches + failed_matches: usize, +} + +#[derive(Debug)] +enum ExplorationStatus { + Complete, + MaxStepsReached, + #[allow(dead_code)] + Failed(String), +} + +/// Simple wheel odometry tracker +/// +/// Computes pose from encoder ticks using differential drive kinematics. +struct WheelOdometry { + wheel_base: f32, + ticks_per_meter: f32, + last_left: i32, + last_right: i32, + pose: Pose2D, + initialized: bool, +} + +impl WheelOdometry { + fn new(wheel_base: f32, ticks_per_meter: f32, initial_pose: Pose2D) -> Self { + Self { + wheel_base, + ticks_per_meter, + last_left: 0, + last_right: 0, + pose: initial_pose, + initialized: false, + } + } + + /// Update odometry from encoder ticks, returns pose delta + fn update(&mut self, left_ticks: u16, right_ticks: u16) -> Pose2D { + let left = left_ticks as i32; + let right = right_ticks as i32; + + if !self.initialized { + self.last_left = left; + self.last_right = right; + self.initialized = true; + return Pose2D::new(0.0, 0.0, 0.0); + } + + // Handle wrapping (u16 encoder ticks wrap at 65535) + let delta_left = (left.wrapping_sub(self.last_left) as i16) as i32; + let delta_right = (right.wrapping_sub(self.last_right) as i16) as i32; + + self.last_left = left; + self.last_right = right; + + // Convert to meters + let left_dist = delta_left as f32 / self.ticks_per_meter; + let right_dist = delta_right as f32 / self.ticks_per_meter; + + // Differential drive kinematics + let delta_theta = (right_dist - left_dist) / self.wheel_base; + let delta_linear = (left_dist + right_dist) / 2.0; + + // Robot-frame delta + let (dx, dy) = if delta_theta.abs() < 1e-6 { + (delta_linear, 0.0) + } else { + let radius = delta_linear / delta_theta; + ( + radius * delta_theta.sin(), + radius * (1.0 - delta_theta.cos()), + ) + }; + + // Transform to world frame + let (sin_t, cos_t) = self.pose.theta.sin_cos(); + let dx_world = dx * cos_t - dy * sin_t; + let dy_world = dx * sin_t + dy * cos_t; + + // Update pose + self.pose = Pose2D::new( + self.pose.x + dx_world, + self.pose.y + dy_world, + normalize_angle(self.pose.theta + delta_theta), + ); + + Pose2D::new(dx, dy, delta_theta) + } + + fn pose(&self) -> Pose2D { + self.pose + } + + /// Reset odometry to a corrected pose (after scan matching) + fn reset_to(&mut self, pose: Pose2D) { + self.pose = pose; + } +} + +/// Run the exploration simulation +fn run_exploration( + sim_map: &SimulationMap, + start_pose: Pose2D, + config: &VastuConfig, + max_steps: usize, + speed_factor: f32, + progress_interval: usize, +) -> ExplorationResult { + // Create occupancy grid map + let map_config = config.to_map_config(); + let mut map = OccupancyGridMap::new(map_config); + + // Create exploration controller + let explore_config = config.to_exploration_config(); + let mut explorer = ExplorationController::new(explore_config); + + // Create physics simulation (ground truth) + let robot_config = RobotConfig::default(); + let mut physics = + PhysicsState::new(start_pose.x, start_pose.y, start_pose.theta, &robot_config); + + // Create encoder simulator (with noise/slip for realistic odometry) + let encoder_config = EncoderConfig::default(); + let encoder_noise = NoiseGenerator::new(42); + let mut encoder = + EncoderSimulator::new(&encoder_config, robot_config.ticks_per_meter, encoder_noise); + + // Create wheel odometry tracker + let mut odometry = WheelOdometry::new( + robot_config.wheel_base, + robot_config.ticks_per_meter, + start_pose, + ); + + // Create lidar simulator + let lidar_config = LidarConfig::default(); + + // Create scan-to-map matcher with larger search window for encoder drift + // Also set sensor offset to match lidar mounting position for accurate alignment + let matcher_config = CorrelativeMatcherConfig { + search_x: 0.5, // ±0.5m (default: 0.3m) + search_y: 0.5, // ±0.5m + search_theta: 1.0, // ±1.0 rad (~57°) - handles large rotation errors + sensor_offset: (lidar_config.mounting_x, lidar_config.mounting_y), + ..CorrelativeMatcherConfig::default() + }; + let matcher = CorrelativeMatcher::new(matcher_config); + let lidar_noise = NoiseGenerator::new(43); // Different seed for lidar + let mut lidar = LidarSimulator::new(&lidar_config, lidar_noise); + + // Simulation parameters + let dt = 0.05; // 20Hz physics + let lidar_interval = 0.2; // 5Hz lidar + let mut time_since_lidar = lidar_interval; // Trigger immediately + let _ = speed_factor; + + // Velocity state + let mut linear_vel = 0.0f32; + let mut angular_vel = 0.0f32; + + // Corrected pose (from scan matching) + let mut corrected_pose = start_pose; + + // Drift statistics + let mut drift_stats = DriftStats::default(); + + // Start exploration + explorer.start(); + let start_time = Instant::now(); + + for step in 0..max_steps.max(1) { + // Update encoder simulation based on wheel velocities + let (left_vel, right_vel) = + physics.wheel_velocities(linear_vel, angular_vel, robot_config.wheel_base); + let (left_ticks, right_ticks) = encoder.update(left_vel, right_vel, dt); + + // Update wheel odometry from encoder ticks + odometry.update(left_ticks, right_ticks); + let odom_pose = odometry.pose(); + + // Generate lidar scan periodically + time_since_lidar += dt; + if time_since_lidar >= lidar_interval { + time_since_lidar = 0.0; + + // Generate scan at ground truth position (what sensor actually sees) + let gt_pose = Pose2D::new(physics.x(), physics.y(), physics.theta()); + let scan_data = lidar.generate_scan(sim_map, gt_pose.x, gt_pose.y, gt_pose.theta); + + if !scan_data.is_empty() { + let ranges: Vec = scan_data.iter().map(|(_, d, _)| *d).collect(); + // Include lidar angle_offset so map observation matches ray-cast direction + let angles: Vec = scan_data + .iter() + .map(|(a, _, _)| a + lidar_config.angle_offset) + .collect(); + + let scan = LidarScan::new( + ranges, + angles, + lidar_config.min_range, + lidar_config.max_range, + ); + + // Use encoder odometry as initial guess for scan matching + let match_result = matcher.match_scan(&scan, odom_pose, map.storage()); + + // Check map has enough structure for reliable matching + let map_stats = map.coverage_stats(); + let has_enough_structure = map_stats.wall_cells > 100; + + // Accept match if converged with reasonable score + // Lower thresholds for closed-loop SLAM where map quality depends on matching + let min_hit_ratio = if has_enough_structure { 0.25 } else { 0.35 }; + let accept_match = + match_result.converged && match_result.hit_ratio() > min_hit_ratio; + + // Use matched pose if successful, otherwise use odometry + if accept_match { + corrected_pose = match_result.pose; + drift_stats.successful_matches += 1; + + // Reset odometry to corrected pose to prevent drift accumulation + odometry.reset_to(corrected_pose); + + // Track drift: difference between encoder and corrected pose + let dx = odom_pose.x - corrected_pose.x; + let dy = odom_pose.y - corrected_pose.y; + let trans_error = (dx * dx + dy * dy).sqrt(); + let rot_error = (odom_pose.theta - corrected_pose.theta).abs(); + + drift_stats.total_translation_drift += trans_error; + drift_stats.total_rotation_drift += rot_error; + drift_stats.max_translation_error = + drift_stats.max_translation_error.max(trans_error); + drift_stats.max_rotation_error = drift_stats.max_rotation_error.max(rot_error); + + log::debug!( + "[ScanMatch] SUCCESS: odom=({:.2},{:.2},{:.1}°) → corrected=({:.2},{:.2},{:.1}°), error={:.3}m, hits={:.0}%", + odom_pose.x, + odom_pose.y, + odom_pose.theta.to_degrees(), + corrected_pose.x, + corrected_pose.y, + corrected_pose.theta.to_degrees(), + trans_error, + match_result.hit_ratio() * 100.0 + ); + } else { + // Match failed or not trusted - use odometry pose + corrected_pose = odom_pose; + drift_stats.failed_matches += 1; + + log::warn!( + "[ScanMatch] FAILED: using odom pose ({:.2},{:.2},{:.1}°), score={:.2}", + odom_pose.x, + odom_pose.y, + odom_pose.theta.to_degrees(), + match_result.score + ); + } + + // Update map with estimated pose (closed-loop SLAM) + map.observe_lidar(&scan, corrected_pose); + } + } + + // Update exploration controller with corrected pose + let command = explorer.update(corrected_pose, map.storage()); + + // Convert command to velocities + // Note: We use corrected_pose for control, but physics uses ground truth internally + match command { + vastu_map::exploration::ExplorationCommand::MoveTo { target, max_speed } => { + let (lin, ang) = compute_velocity_to_target( + corrected_pose.x, + corrected_pose.y, + corrected_pose.theta, + target.x, + target.y, + max_speed, + 2.0, // max angular speed + ); + linear_vel = lin; + angular_vel = ang; + } + vastu_map::exploration::ExplorationCommand::Rotate { + target_heading, + max_angular_speed, + } => { + linear_vel = 0.0; + angular_vel = compute_angular_velocity( + corrected_pose.theta, + target_heading, + max_angular_speed, + ); + } + vastu_map::exploration::ExplorationCommand::Stop => { + linear_vel = 0.0; + angular_vel = 0.0; + } + vastu_map::exploration::ExplorationCommand::ExplorationComplete => { + println!("\nExploration complete!"); + return ExplorationResult { + map, + status: ExplorationStatus::Complete, + steps: step, + elapsed_secs: start_time.elapsed().as_secs_f32(), + progress: explorer.progress(), + drift_stats, + }; + } + vastu_map::exploration::ExplorationCommand::ExplorationFailed { reason } => { + println!("\nExploration failed: {}", reason); + return ExplorationResult { + map, + status: ExplorationStatus::Failed(reason), + steps: step, + elapsed_secs: start_time.elapsed().as_secs_f32(), + progress: explorer.progress(), + drift_stats, + }; + } + vastu_map::exploration::ExplorationCommand::None => { + // Continue with current velocities + } + } + + // Update physics and check for collision + let collision = physics.update(dt, linear_vel, angular_vel, sim_map, &robot_config); + if collision { + // Trigger obstacle detection event for the exploration controller + explorer.handle_event(vastu_map::exploration::ExplorationEvent::ObstacleDetected); + } + + // Progress output - also show when exploration changes state + let progress = explorer.progress(); + if progress_interval > 0 && step % progress_interval == 0 && step > 0 { + // Compare ground truth vs corrected pose for drift visualization + let gt_pose = Pose2D::new(physics.x(), physics.y(), physics.theta()); + let pos_error = ((gt_pose.x - corrected_pose.x).powi(2) + + (gt_pose.y - corrected_pose.y).powi(2)) + .sqrt(); + + println!( + "[Step {}] State: {}, Frontiers: {}, Dist: {:.1}m, Matches: {}/{}, PosErr: {:.3}m", + step, + progress.state, + progress.frontiers_explored, + progress.distance_traveled, + drift_stats.successful_matches, + drift_stats.successful_matches + drift_stats.failed_matches, + pos_error + ); + } + + // Debug: show completion or state changes + if matches!( + command, + vastu_map::exploration::ExplorationCommand::ExplorationComplete + ) { + println!("[Step {}] Exploration complete!", step); + } + } + + ExplorationResult { + map, + status: ExplorationStatus::MaxStepsReached, + steps: max_steps, + elapsed_secs: start_time.elapsed().as_secs_f32(), + progress: explorer.progress(), + drift_stats, + } +} + +/// Compute velocity command to move towards target +/// +/// Uses proportional control with concurrent linear + angular movement +/// for smooth path following. +fn compute_velocity_to_target( + x: f32, + y: f32, + theta: f32, + target_x: f32, + target_y: f32, + max_linear: f32, + max_angular: f32, +) -> (f32, f32) { + let dx = target_x - x; + let dy = target_y - y; + let distance = (dx * dx + dy * dy).sqrt(); + + if distance < 0.05 { + return (0.0, 0.0); // Close enough + } + + // Angle to target + let target_angle = dy.atan2(dx); + let angle_error = normalize_angle(target_angle - theta); + let abs_error = angle_error.abs(); + + // Proportional angular velocity (gain = 1.5) + let angular_vel = (angle_error * 1.5).clamp(-max_angular, max_angular); + + // Only pure rotation if angle is very large (>45°, ~0.78 rad) + if abs_error > 0.78 { + return (0.0, angular_vel); + } + + // Move forward with concurrent angular correction + // Scale linear velocity based on angle error (slower when turning more) + let angle_factor = 1.0 - (abs_error / 0.78).min(0.8); + let linear_vel = max_linear.min(distance) * angle_factor; + + (linear_vel, angular_vel) +} + +/// Compute angular velocity to rotate to target heading +fn compute_angular_velocity(current: f32, target: f32, max_speed: f32) -> f32 { + let error = normalize_angle(target - current); + if error.abs() < 0.05 { + 0.0 + } else { + (error * 2.0).clamp(-max_speed, max_speed) + } +} + +/// Normalize angle to [-PI, PI) +fn normalize_angle(angle: f32) -> f32 { + let mut a = angle % TAU; + if a > PI { + a -= TAU; + } else if a < -PI { + a += TAU; + } + a +} diff --git a/vastu-map/examples/explore_simple.rs b/vastu-map/examples/explore_simple.rs new file mode 100644 index 0000000..a719ff6 --- /dev/null +++ b/vastu-map/examples/explore_simple.rs @@ -0,0 +1,305 @@ +//! Simple exploration example using VastuExplorer API. +//! +//! This example demonstrates the high-level exploration API that handles +//! all SLAM, scan matching, and navigation internally. +//! +//! Usage: +//! cargo run --example explore_simple -- --map simple_room +//! cargo run --example explore_simple # Uses random map + +use clap::Parser; +use rand::prelude::*; +use std::f32::consts::TAU; +use std::path::Path; + +// SangamIO mock simulation components +use sangam_io::devices::mock::{ + config::{LidarConfig, RobotConfig}, + lidar_sim::LidarSimulator, + map_loader::SimulationMap, + noise::NoiseGenerator, + physics::PhysicsState, +}; + +// Vastu-map components +use vastu_map::{ + LidarScan, Pose2D, VastuConfig, + explore::{ExploreResult, SensorSource, VastuExplorer}, + io::{export_ros_map, save_vastu}, +}; + +/// Simple exploration example +#[derive(Parser, Debug)] +#[command(author, version, about, long_about = None)] +struct Args { + /// Configuration file path + #[arg(short, long, default_value = "configs/config.yaml")] + config: String, + + /// Map name to load from maps/ folder (e.g., "simple_room") + #[arg(short, long)] + map: Option, + + /// Output directory for saved maps + #[arg(short, long, default_value = "./output")] + output: String, +} + +/// Mock sensor source that wraps the SangamIO simulation. +struct MockSensorSource<'a> { + sim_map: &'a SimulationMap, + physics: PhysicsState, + lidar: LidarSimulator, + lidar_config: LidarConfig, + robot_config: RobotConfig, + latest_scan: Option, + scan_counter: u32, + /// Current commanded velocities + linear_vel: f32, + angular_vel: f32, +} + +impl<'a> MockSensorSource<'a> { + fn new(sim_map: &'a SimulationMap, start_pose: Pose2D) -> Self { + let robot_config = RobotConfig::default(); + let lidar_config = LidarConfig::default(); + let noise = NoiseGenerator::new(42); + let lidar = LidarSimulator::new(&lidar_config, noise); + let physics = + PhysicsState::new(start_pose.x, start_pose.y, start_pose.theta, &robot_config); + + Self { + sim_map, + physics, + lidar, + lidar_config, + robot_config, + latest_scan: None, + scan_counter: 0, + linear_vel: 0.0, + angular_vel: 0.0, + } + } + + fn update_physics(&mut self, dt: f32, linear: f32, angular: f32) { + self.physics + .update(dt, linear, angular, self.sim_map, &self.robot_config); + } +} + +impl SensorSource for MockSensorSource<'_> { + fn get_pose(&self) -> Pose2D { + Pose2D::new(self.physics.x(), self.physics.y(), self.physics.theta()) + } + + fn get_lidar_scan(&self) -> Option { + self.latest_scan.clone() + } + + fn send_velocity(&mut self, linear: f32, angular: f32) { + // Store commanded velocities (physics updated in poll) + self.linear_vel = linear; + self.angular_vel = angular; + } + + fn is_connected(&self) -> bool { + true + } + + fn poll(&mut self) { + // Update physics with current velocities (10Hz control loop) + let dt = 0.1; + self.update_physics(dt, self.linear_vel, self.angular_vel); + + // Generate lidar scan every 2nd poll (5Hz lidar at 10Hz control) + // Start at 1 so first poll generates a scan + self.scan_counter += 1; + if self.scan_counter == 1 || self.scan_counter % 2 == 0 { + let scan_data = self.lidar.generate_scan( + self.sim_map, + self.physics.x(), + self.physics.y(), + self.physics.theta(), + ); + + if !scan_data.is_empty() { + let ranges: Vec = scan_data.iter().map(|(_, d, _)| *d).collect(); + let angles: Vec = scan_data.iter().map(|(a, _, _)| *a).collect(); + + self.latest_scan = Some(LidarScan::new( + ranges, + angles, + self.lidar_config.min_range, + self.lidar_config.max_range, + )); + } + } else { + self.latest_scan = None; + } + } +} + +fn main() { + env_logger::init(); + let args = Args::parse(); + + // Load configuration + let config_path = Path::new(&args.config); + let vastu_config = if config_path.exists() { + VastuConfig::load(config_path).unwrap_or_else(|e| { + eprintln!("Warning: Failed to load config: {}, using defaults", e); + VastuConfig::default() + }) + } else { + println!( + "Config not found at {}, using defaults", + config_path.display() + ); + VastuConfig::default() + }; + + // List available maps + let maps_dir = Path::new("maps"); + let available_maps = list_available_maps(maps_dir); + + if available_maps.is_empty() { + eprintln!("No maps found in maps/ folder"); + std::process::exit(1); + } + + // Select map + let map_name = args.map.unwrap_or_else(|| { + let mut rng = rand::rng(); + let selected = available_maps.choose(&mut rng).unwrap().clone(); + println!("Randomly selected map: {}", selected); + selected + }); + + let map_yaml = maps_dir.join(format!("{}.yaml", map_name)); + if !map_yaml.exists() { + eprintln!("Map not found: {}", map_yaml.display()); + eprintln!("Available maps: {:?}", available_maps); + std::process::exit(1); + } + + // Load simulation map + println!("Loading map: {}", map_yaml.display()); + let sim_map = SimulationMap::load(&map_yaml).expect("Failed to load simulation map"); + println!( + "Map size: {}x{} pixels, resolution: {}m/px", + sim_map.width(), + sim_map.height(), + sim_map.resolution() + ); + + // Find starting position + let start_pose = find_free_start_position(&sim_map); + println!( + "Starting at: ({:.2}, {:.2}, {:.1}°)", + start_pose.x, + start_pose.y, + start_pose.theta.to_degrees() + ); + + // Create output directory + std::fs::create_dir_all(&args.output).ok(); + + // Create explorer with loaded configuration + let explorer_config = vastu_config.to_explorer_config(); + let explorer = VastuExplorer::new(explorer_config); + + // Create sensor source + let mut source = MockSensorSource::new(&sim_map, start_pose); + + // Run exploration - the library handles everything! + println!("\nStarting exploration..."); + match explorer.explore(&mut source) { + ExploreResult::Complete(map) => { + println!("\nExploration complete!"); + + // Save maps + let output_base = Path::new(&args.output).join(&map_name); + + save_vastu(map.storage(), &output_base.with_extension("vastu")) + .expect("Failed to save .vastu file"); + println!("Saved: {}.vastu", output_base.display()); + + export_ros_map(map.storage(), &output_base).expect("Failed to save ROS map"); + println!( + "Saved: {}.pgm + {}.yaml", + output_base.display(), + output_base.display() + ); + + let stats = map.coverage_stats(); + println!("\n=== Map Statistics ==="); + println!("Floor cells: {}", stats.floor_cells); + println!("Wall cells: {}", stats.wall_cells); + println!("Coverage: {:.2}m²", stats.explored_area_m2); + } + ExploreResult::Failed { map, reason } => { + println!("\nExploration failed: {}", reason); + + // Save partial map + let output_base = Path::new(&args.output).join(format!("{}_partial", map_name)); + save_vastu(map.storage(), &output_base.with_extension("vastu")).ok(); + println!("Saved partial map: {}.vastu", output_base.display()); + } + } +} + +/// List available map names in directory. +fn list_available_maps(dir: &Path) -> Vec { + let mut maps = Vec::new(); + if let Ok(entries) = std::fs::read_dir(dir) { + for entry in entries.flatten() { + let path = entry.path(); + if path.extension().is_some_and(|e| e == "yaml") { + if let Some(stem) = path.file_stem() { + maps.push(stem.to_string_lossy().to_string()); + } + } + } + } + maps.sort(); + maps +} + +/// Find a free starting position in the map. +fn find_free_start_position(map: &SimulationMap) -> Pose2D { + let mut rng = rand::rng(); + let sim_origin = map.origin(); + let sim_width_m = map.width() as f32 * map.resolution(); + let sim_height_m = map.height() as f32 * map.resolution(); + + // Search in center region + let margin = 0.5; + let min_x = sim_origin.0 + margin; + let min_y = sim_origin.1 + margin; + let max_x = sim_origin.0 + sim_width_m - margin; + let max_y = sim_origin.1 + sim_height_m - margin; + + // Try random positions + for _ in 0..1000 { + let x = min_x + rng.random::() * (max_x - min_x); + let y = min_y + rng.random::() * (max_y - min_y); + + // Check if position is free with margin + if !map.is_occupied(x, y) + && !map.is_occupied(x + 0.2, y) + && !map.is_occupied(x - 0.2, y) + && !map.is_occupied(x, y + 0.2) + && !map.is_occupied(x, y - 0.2) + { + let theta = rng.random::() * TAU; + return Pose2D::new(x, y, theta); + } + } + + // Fallback: center of map + Pose2D::new( + sim_origin.0 + sim_width_m / 2.0, + sim_origin.1 + sim_height_m / 2.0, + 0.0, + ) +} diff --git a/vastu-map/examples/scan_match_test.rs b/vastu-map/examples/scan_match_test.rs new file mode 100644 index 0000000..7ade2b2 --- /dev/null +++ b/vastu-map/examples/scan_match_test.rs @@ -0,0 +1,671 @@ +//! Scan Matching Accuracy Test +//! +//! Controlled tests to measure scan-to-map matching accuracy using YAML scenario files. +//! +//! Scenarios define: +//! - Which map to use +//! - Starting pose (optional) +//! - Sequence of movement commands (move/stay) +//! - SVG output configuration +//! +//! Usage: +//! cargo run --example scan_match_test -- --scenario scenarios/straight_line.yaml +//! cargo run --example scan_match_test -- -s scenarios/rotation_360.yaml +//! cargo run --example scan_match_test -- -s scenarios/square_path.yaml -o ./my_output + +use clap::Parser; +use std::f32::consts::{PI, TAU}; +use std::path::Path; + +// SangamIO mock simulation components +use sangam_io::devices::mock::{ + config::{EncoderConfig, LidarConfig, RobotConfig}, + encoder_sim::EncoderSimulator, + lidar_sim::LidarSimulator, + map_loader::SimulationMap, + noise::NoiseGenerator, + physics::PhysicsState, +}; + +// Vastu-map components +use vastu_map::{ + LidarScan, OccupancyGridMap, Pose2D, VastuConfig, + io::{Command, Scenario, SvgConfig, SvgVisualizer, markers_by_distance, markers_by_time}, + slam::{CorrelativeMatcher, CorrelativeMatcherConfig}, +}; + +/// Scan matching accuracy test using YAML scenario files +#[derive(Parser, Debug)] +#[command(author, version, about, long_about = None)] +struct Args { + /// Scenario YAML file defining the test + #[arg(short = 's', long)] + scenario: String, + + /// Configuration file path + #[arg(short, long, default_value = "configs/config.yaml")] + config: String, + + /// Output directory for SVG audit file + #[arg(short, long, default_value = "./output")] + output: String, +} + +/// Per-scan metrics +#[derive(Clone, Debug)] +struct ScanMetrics { + /// Ground truth pose + gt_pose: Pose2D, + /// Encoder odometry pose + odom_pose: Pose2D, + /// Scan-matched corrected pose + corrected_pose: Pose2D, + /// Match score (0-1) + match_score: f32, + /// Hit ratio (0-1) + hit_ratio: f32, + /// Whether match converged + converged: bool, +} + +/// Test results +struct TestResults { + test_name: String, + start_pose: Pose2D, + gt_final: Pose2D, + odom_final: Pose2D, + corrected_final: Pose2D, + scans: Vec, + wall_cells: usize, + map: OccupancyGridMap, +} + +/// Simple wheel odometry tracker +struct WheelOdometry { + wheel_base: f32, + ticks_per_meter: f32, + last_left: i32, + last_right: i32, + pose: Pose2D, + initialized: bool, +} + +impl WheelOdometry { + fn new(wheel_base: f32, ticks_per_meter: f32, initial_pose: Pose2D) -> Self { + Self { + wheel_base, + ticks_per_meter, + last_left: 0, + last_right: 0, + pose: initial_pose, + initialized: false, + } + } + + fn update(&mut self, left_ticks: u16, right_ticks: u16) { + let left = left_ticks as i32; + let right = right_ticks as i32; + + if !self.initialized { + self.last_left = left; + self.last_right = right; + self.initialized = true; + return; + } + + let delta_left = (left.wrapping_sub(self.last_left) as i16) as i32; + let delta_right = (right.wrapping_sub(self.last_right) as i16) as i32; + + self.last_left = left; + self.last_right = right; + + let left_dist = delta_left as f32 / self.ticks_per_meter; + let right_dist = delta_right as f32 / self.ticks_per_meter; + + let delta_theta = (right_dist - left_dist) / self.wheel_base; + let delta_linear = (left_dist + right_dist) / 2.0; + + let (dx, dy) = if delta_theta.abs() < 1e-6 { + (delta_linear, 0.0) + } else { + let radius = delta_linear / delta_theta; + ( + radius * delta_theta.sin(), + radius * (1.0 - delta_theta.cos()), + ) + }; + + let (sin_t, cos_t) = self.pose.theta.sin_cos(); + let dx_world = dx * cos_t - dy * sin_t; + let dy_world = dx * sin_t + dy * cos_t; + + self.pose = Pose2D::new( + self.pose.x + dx_world, + self.pose.y + dy_world, + normalize_angle(self.pose.theta + delta_theta), + ); + } + + fn pose(&self) -> Pose2D { + self.pose + } + + fn reset_to(&mut self, pose: Pose2D) { + self.pose = pose; + } +} + +fn main() { + env_logger::init(); + let args = Args::parse(); + + // Load configuration + let config_path = Path::new(&args.config); + let vastu_config = if config_path.exists() { + VastuConfig::load(config_path).unwrap_or_else(|e| { + eprintln!("Warning: Failed to load config: {}, using defaults", e); + VastuConfig::default() + }) + } else { + println!( + "Config not found at {}, using defaults", + config_path.display() + ); + VastuConfig::default() + }; + + // Create output directory + let output_dir = Path::new(&args.output); + std::fs::create_dir_all(output_dir).ok(); + + // Load scenario + let scenario_file = Path::new(&args.scenario); + if !scenario_file.exists() { + eprintln!("Scenario file not found: {}", scenario_file.display()); + std::process::exit(1); + } + + println!("Loading scenario: {}", scenario_file.display()); + let scenario = Scenario::load(scenario_file).unwrap_or_else(|e| { + eprintln!("Failed to load scenario: {}", e); + std::process::exit(1); + }); + + // Resolve and load map from scenario + let maps_dir = Path::new("maps"); + let map_yaml = scenario.resolve_map_path(maps_dir); + if !map_yaml.exists() { + eprintln!("Map not found: {}", map_yaml.display()); + std::process::exit(1); + } + + println!("Loading map: {}", map_yaml.display()); + let sim_map = SimulationMap::load(&map_yaml).expect("Failed to load simulation map"); + println!( + "Map size: {}x{} pixels, resolution: {}m/px", + sim_map.width(), + sim_map.height(), + sim_map.resolution() + ); + + // Run the scenario test + let results = run_scenario_test(&sim_map, &scenario, &vastu_config); + print_results(&results); + save_output(&results, output_dir, &scenario); +} + +/// Find a free starting position with good clearance +fn find_start_position(sim_map: &SimulationMap) -> Pose2D { + let sim_origin = sim_map.origin(); + let sim_width_m = sim_map.width() as f32 * sim_map.resolution(); + let sim_height_m = sim_map.height() as f32 * sim_map.resolution(); + + // Search for position with good clearance + let clearance = 1.0; // 1m from walls + for py in 0..sim_map.height() { + for px in 0..sim_map.width() { + let (x, y) = sim_map.pixel_to_world(px, py); + + // Check bounds + if x < sim_origin.0 + clearance + || x > sim_origin.0 + sim_width_m - clearance + || y < sim_origin.1 + clearance + || y > sim_origin.1 + sim_height_m - clearance + { + continue; + } + + // Check clearance in all directions + let mut clear = true; + for angle in 0..8 { + let a = angle as f32 * PI / 4.0; + for dist in [0.3, 0.6, 0.9] { + if sim_map.is_occupied(x + dist * a.cos(), y + dist * a.sin()) { + clear = false; + break; + } + } + if !clear { + break; + } + } + + if clear { + return Pose2D::new(x, y, 0.0); + } + } + } + + // Fallback + Pose2D::new( + sim_origin.0 + sim_width_m / 2.0, + sim_origin.1 + sim_height_m / 2.0, + 0.0, + ) +} + +/// Run a scenario-based test from YAML specification +fn run_scenario_test( + sim_map: &SimulationMap, + scenario: &Scenario, + vastu_config: &VastuConfig, +) -> TestResults { + println!("\n=== Running Scenario: {} ===", scenario.name); + if !scenario.description.is_empty() { + println!("{}", scenario.description); + } + + // Determine start pose + let start_pose = if let Some(ref sp) = scenario.start_pose { + Pose2D::new(sp.x, sp.y, sp.theta) + } else { + find_start_position(sim_map) + }; + + println!( + "Start position: ({:.2}, {:.2}, {:.1}°)", + start_pose.x, + start_pose.y, + start_pose.theta.to_degrees() + ); + + // Setup simulation + let map_config = vastu_config.to_map_config(); + let mut map = OccupancyGridMap::new(map_config); + + let robot_config = RobotConfig::default(); + let mut physics = + PhysicsState::new(start_pose.x, start_pose.y, start_pose.theta, &robot_config); + + let encoder_config = EncoderConfig::default(); + let encoder_noise = NoiseGenerator::new(42); + let mut encoder = + EncoderSimulator::new(&encoder_config, robot_config.ticks_per_meter, encoder_noise); + + let mut pure_odometry = WheelOdometry::new( + robot_config.wheel_base, + robot_config.ticks_per_meter, + start_pose, + ); + let mut corrected_odometry = WheelOdometry::new( + robot_config.wheel_base, + robot_config.ticks_per_meter, + start_pose, + ); + + let lidar_config = LidarConfig::default(); + + let matcher_config = CorrelativeMatcherConfig { + sensor_offset: (lidar_config.mounting_x, lidar_config.mounting_y), + ..CorrelativeMatcherConfig::default() + }; + let matcher = CorrelativeMatcher::new(matcher_config); + let lidar_noise = NoiseGenerator::new(43); + let mut lidar = LidarSimulator::new(&lidar_config, lidar_noise); + + // Simulation parameters + let dt = 0.05; // 20Hz physics + let lidar_interval = 0.2; // 5Hz lidar + + let mut time_since_lidar = lidar_interval; + let mut corrected_pose = start_pose; + let mut scans: Vec = Vec::new(); + + // Phase 1: Seed the map + if scenario.seed.rotation { + println!( + "Seeding map with {} rotational scans...", + scenario.seed.scans + ); + for i in 0..scenario.seed.scans { + let angle = i as f32 * TAU / scenario.seed.scans as f32; + let seed_pose = Pose2D::new(start_pose.x, start_pose.y, angle); + let scan_data = lidar.generate_scan(sim_map, seed_pose.x, seed_pose.y, seed_pose.theta); + if !scan_data.is_empty() { + let ranges: Vec = scan_data.iter().map(|(_, d, _)| *d).collect(); + let angles: Vec = scan_data + .iter() + .map(|(a, _, _)| a + lidar_config.angle_offset) + .collect(); + let scan = LidarScan::new( + ranges, + angles, + lidar_config.min_range, + lidar_config.max_range, + ); + map.observe_lidar(&scan, seed_pose); + } + } + } + + let map_stats = map.coverage_stats(); + println!( + "Map seeded: {} wall cells, {} floor cells", + map_stats.wall_cells, map_stats.floor_cells + ); + + // Phase 2: Execute scenario commands + println!( + "Executing {} commands (total: {:.1}s)...", + scenario.commands.len(), + scenario.total_duration_secs() + ); + + for (cmd_idx, command) in scenario.commands.iter().enumerate() { + let (linear_vel, angular_vel) = command.velocities(); + let duration_secs = command.duration_secs(); + let steps = (duration_secs / dt) as usize; + + let cmd_type = match command { + Command::Move { .. } => "move", + Command::Stay { .. } => "stay", + }; + println!( + " Command {}: {} (v={:.2}m/s, w={:.2}rad/s) for {:.1}s", + cmd_idx + 1, + cmd_type, + linear_vel, + angular_vel, + duration_secs + ); + + for _ in 0..steps { + // Update encoders + let (left_vel, right_vel) = + physics.wheel_velocities(linear_vel, angular_vel, robot_config.wheel_base); + let (left_ticks, right_ticks) = encoder.update(left_vel, right_vel, dt); + + pure_odometry.update(left_ticks, right_ticks); + corrected_odometry.update(left_ticks, right_ticks); + + // Update physics + let collision = physics.update(dt, linear_vel, angular_vel, sim_map, &robot_config); + if collision { + println!("Warning: Collision detected, stopping command early"); + break; + } + + // Lidar scan + time_since_lidar += dt; + if time_since_lidar >= lidar_interval { + time_since_lidar = 0.0; + + let gt_pose = Pose2D::new(physics.x(), physics.y(), physics.theta()); + let pure_odom_pose = pure_odometry.pose(); + let corrected_odom_pose = corrected_odometry.pose(); + + let scan_data = lidar.generate_scan(sim_map, gt_pose.x, gt_pose.y, gt_pose.theta); + if !scan_data.is_empty() { + let ranges: Vec = scan_data.iter().map(|(_, d, _)| *d).collect(); + let angles: Vec = scan_data + .iter() + .map(|(a, _, _)| a + lidar_config.angle_offset) + .collect(); + let scan = LidarScan::new( + ranges, + angles, + lidar_config.min_range, + lidar_config.max_range, + ); + + let match_result = + matcher.match_scan(&scan, corrected_odom_pose, map.storage()); + + let match_dist = ((match_result.pose.x - corrected_odom_pose.x).powi(2) + + (match_result.pose.y - corrected_odom_pose.y).powi(2)) + .sqrt(); + + let valid_match = match_result.converged + && match_result.hit_ratio() > 0.3 + && match_dist < 0.5; + + if valid_match { + corrected_pose = match_result.pose; + corrected_odometry.reset_to(corrected_pose); + } else { + corrected_pose = corrected_odom_pose; + } + + map.observe_lidar(&scan, corrected_pose); + + scans.push(ScanMetrics { + gt_pose, + odom_pose: pure_odom_pose, + corrected_pose, + match_score: match_result.score, + hit_ratio: match_result.hit_ratio(), + converged: match_result.converged, + }); + } + } + } + } + + let map_stats = map.coverage_stats(); + + // Use the last scan's poses for fair comparison + // (corrected_pose is from the last scan, so odom should be too) + let (odom_final, corrected_final, gt_final) = if let Some(last_scan) = scans.last() { + ( + last_scan.odom_pose, + last_scan.corrected_pose, + last_scan.gt_pose, + ) + } else { + // Fallback if no scans + ( + pure_odometry.pose(), + corrected_pose, + Pose2D::new(physics.x(), physics.y(), physics.theta()), + ) + }; + + TestResults { + test_name: scenario.name.clone(), + start_pose, + gt_final, + odom_final, + corrected_final, + scans, + wall_cells: map_stats.wall_cells, + map, + } +} + +/// Generate SVG audit file +fn save_output(results: &TestResults, output_dir: &Path, scenario: &Scenario) { + println!("\nGenerating SVG audit file..."); + + // Collect trajectories from scan metrics + let gt_poses: Vec = results.scans.iter().map(|s| s.gt_pose).collect(); + let est_poses: Vec = results.scans.iter().map(|s| s.corrected_pose).collect(); + let odom_poses: Vec = results.scans.iter().map(|s| s.odom_pose).collect(); + + // Calculate markers based on scenario config + let marker_indices = if let Some(interval_m) = scenario.svg_output.marker_interval_m { + markers_by_distance(>_poses, interval_m) + } else if let Some(interval_s) = scenario.svg_output.marker_interval_s { + markers_by_time(gt_poses.len(), scenario.total_duration_secs(), interval_s) + } else { + markers_by_distance(>_poses, 0.5) + }; + + let svg_config = SvgConfig { + scale: scenario.svg_output.scale, + trajectory_width: scenario.svg_output.trajectory_width, + ..SvgConfig::default() + }; + + let visualizer = SvgVisualizer::new(results.map.storage().clone(), svg_config) + .with_title(&scenario.name) + .with_ground_truth(gt_poses, marker_indices.clone()) + .with_estimated(est_poses, marker_indices.clone()) + .with_odometry(odom_poses, marker_indices); + + let svg_filename = scenario.svg_filename(); + let svg_path = output_dir.join(&svg_filename); + + if let Err(e) = visualizer.save(&svg_path) { + eprintln!(" Failed to save SVG: {}", e); + } else { + println!(" Saved: {}", svg_path.display()); + } +} + +/// Print test results +fn print_results(results: &TestResults) { + println!("\n=== {} Results ===", results.test_name); + println!("Scans collected: {}", results.scans.len()); + println!("Map wall cells: {}", results.wall_cells); + + // Pose comparison + println!("\nPose Trajectory:"); + println!( + " Start: ({:7.3}, {:7.3}, {:7.2}°)", + results.start_pose.x, + results.start_pose.y, + results.start_pose.theta.to_degrees() + ); + println!( + " GT Final: ({:7.3}, {:7.3}, {:7.2}°)", + results.gt_final.x, + results.gt_final.y, + results.gt_final.theta.to_degrees() + ); + println!( + " Odom Final:({:7.3}, {:7.3}, {:7.2}°)", + results.odom_final.x, + results.odom_final.y, + results.odom_final.theta.to_degrees() + ); + println!( + " Corrected: ({:7.3}, {:7.3}, {:7.2}°)", + results.corrected_final.x, + results.corrected_final.y, + results.corrected_final.theta.to_degrees() + ); + + // Calculate errors + let odom_pos_error = ((results.odom_final.x - results.gt_final.x).powi(2) + + (results.odom_final.y - results.gt_final.y).powi(2)) + .sqrt(); + let odom_ang_error = normalize_angle(results.odom_final.theta - results.gt_final.theta).abs(); + + let corr_pos_error = ((results.corrected_final.x - results.gt_final.x).powi(2) + + (results.corrected_final.y - results.gt_final.y).powi(2)) + .sqrt(); + let corr_ang_error = + normalize_angle(results.corrected_final.theta - results.gt_final.theta).abs(); + + println!("\nFinal Errors (vs Ground Truth):"); + println!( + " Encoder: position = {:6.4}m, heading = {:6.2}°", + odom_pos_error, + odom_ang_error.to_degrees() + ); + println!( + " Corrected: position = {:6.4}m, heading = {:6.2}°", + corr_pos_error, + corr_ang_error.to_degrees() + ); + + // Improvement ratio + if odom_pos_error > 0.0 { + let pos_improvement = (odom_pos_error - corr_pos_error) / odom_pos_error * 100.0; + println!("\n Position improvement: {:+.1}%", pos_improvement); + } + if odom_ang_error > 0.0 { + let ang_improvement = (odom_ang_error - corr_ang_error) / odom_ang_error * 100.0; + println!(" Heading improvement: {:+.1}%", ang_improvement); + } + + // Scan matching statistics + let converged_count = results.scans.iter().filter(|s| s.converged).count(); + let avg_score: f32 = if !results.scans.is_empty() { + results.scans.iter().map(|s| s.match_score).sum::() / results.scans.len() as f32 + } else { + 0.0 + }; + let avg_hit_ratio: f32 = if !results.scans.is_empty() { + results.scans.iter().map(|s| s.hit_ratio).sum::() / results.scans.len() as f32 + } else { + 0.0 + }; + + println!("\nScan Matching Statistics:"); + println!( + " Success rate: {}/{} ({:.1}%)", + converged_count, + results.scans.len(), + if results.scans.is_empty() { + 0.0 + } else { + 100.0 * converged_count as f32 / results.scans.len() as f32 + } + ); + println!(" Avg score: {:.3}", avg_score); + println!(" Avg hit ratio:{:.1}%", avg_hit_ratio * 100.0); + + // Per-scan error analysis + if !results.scans.is_empty() { + let mut odom_errors: Vec = Vec::new(); + let mut corr_errors: Vec = Vec::new(); + + for scan in &results.scans { + let odom_err = ((scan.odom_pose.x - scan.gt_pose.x).powi(2) + + (scan.odom_pose.y - scan.gt_pose.y).powi(2)) + .sqrt(); + let corr_err = ((scan.corrected_pose.x - scan.gt_pose.x).powi(2) + + (scan.corrected_pose.y - scan.gt_pose.y).powi(2)) + .sqrt(); + odom_errors.push(odom_err); + corr_errors.push(corr_err); + } + + let odom_max = odom_errors.iter().cloned().fold(0.0f32, f32::max); + let corr_max = corr_errors.iter().cloned().fold(0.0f32, f32::max); + let odom_avg = odom_errors.iter().sum::() / odom_errors.len() as f32; + let corr_avg = corr_errors.iter().sum::() / corr_errors.len() as f32; + + println!("\nPer-Scan Position Error:"); + println!( + " Encoder: avg = {:6.4}m, max = {:6.4}m", + odom_avg, odom_max + ); + println!( + " Corrected: avg = {:6.4}m, max = {:6.4}m", + corr_avg, corr_max + ); + } +} + +/// Normalize angle to [-PI, PI) +fn normalize_angle(angle: f32) -> f32 { + let mut a = angle % TAU; + if a > PI { + a -= TAU; + } else if a < -PI { + a += TAU; + } + a +} diff --git a/vastu-map/data/maps/corridor.pgm b/vastu-map/maps/corridor.pgm similarity index 100% rename from vastu-map/data/maps/corridor.pgm rename to vastu-map/maps/corridor.pgm diff --git a/vastu-map/data/maps/corridor.yaml b/vastu-map/maps/corridor.yaml similarity index 100% rename from vastu-map/data/maps/corridor.yaml rename to vastu-map/maps/corridor.yaml diff --git a/vastu-map/data/maps/large_room.pgm b/vastu-map/maps/large_room.pgm similarity index 100% rename from vastu-map/data/maps/large_room.pgm rename to vastu-map/maps/large_room.pgm diff --git a/vastu-map/data/maps/large_room.yaml b/vastu-map/maps/large_room.yaml similarity index 100% rename from vastu-map/data/maps/large_room.yaml rename to vastu-map/maps/large_room.yaml diff --git a/vastu-map/data/maps/medium_room.pgm b/vastu-map/maps/medium_room.pgm similarity index 100% rename from vastu-map/data/maps/medium_room.pgm rename to vastu-map/maps/medium_room.pgm diff --git a/vastu-map/data/maps/medium_room.yaml b/vastu-map/maps/medium_room.yaml similarity index 100% rename from vastu-map/data/maps/medium_room.yaml rename to vastu-map/maps/medium_room.yaml diff --git a/vastu-map/data/maps/simple_room.pgm b/vastu-map/maps/simple_room.pgm similarity index 100% rename from vastu-map/data/maps/simple_room.pgm rename to vastu-map/maps/simple_room.pgm diff --git a/vastu-map/data/maps/simple_room.yaml b/vastu-map/maps/simple_room.yaml similarity index 100% rename from vastu-map/data/maps/simple_room.yaml rename to vastu-map/maps/simple_room.yaml diff --git a/vastu-map/scenarios/rotation_360.yaml b/vastu-map/scenarios/rotation_360.yaml new file mode 100644 index 0000000..4ec6392 --- /dev/null +++ b/vastu-map/scenarios/rotation_360.yaml @@ -0,0 +1,23 @@ +# 360 Degree Rotation Test Scenario +# Tests scan matching during pure rotation + +name: "360 Rotation Test" +description: "Rotate in place at 0.5 rad/s for full 360 degrees" + +map: "medium_room" + +seed: + scans: 20 + rotation: true + +commands: + # Full 360 rotation: 2*PI / 0.5 rad/s = ~12.57 seconds + - type: move + linear_velocity: 0.0 + angular_velocity: 0.5 + duration_ms: 12566 + +svg_output: + scale: 50.0 + marker_interval_s: 1.0 + trajectory_width: 2.0 diff --git a/vastu-map/scenarios/square_path.yaml b/vastu-map/scenarios/square_path.yaml new file mode 100644 index 0000000..00452ef --- /dev/null +++ b/vastu-map/scenarios/square_path.yaml @@ -0,0 +1,94 @@ +# Square Path Navigation Scenario +# Tests scan matching during combined linear and angular motion + +name: "Square Path" +description: "Navigate a 1m x 1m square path with 90-degree turns" + +map: "medium_room" + +start_pose: + x: 4.0 + y: 4.0 + theta: 0.0 + +seed: + scans: 20 + rotation: true + +commands: + # Side 1: Move forward 1m + - type: move + linear_velocity: 0.2 + angular_velocity: 0.0 + duration_ms: 5000 + + # Pause + - type: stay + duration_ms: 500 + + # Turn 90 degrees CW (PI/2 at 0.5 rad/s = ~3.14s) + - type: move + linear_velocity: 0.0 + angular_velocity: -0.5 + duration_ms: 3142 + + # Pause + - type: stay + duration_ms: 500 + + # Side 2: Move forward 1m + - type: move + linear_velocity: 0.2 + angular_velocity: 0.0 + duration_ms: 5000 + + # Pause + - type: stay + duration_ms: 500 + + # Turn 90 degrees CW + - type: move + linear_velocity: 0.0 + angular_velocity: -0.5 + duration_ms: 3142 + + # Pause + - type: stay + duration_ms: 500 + + # Side 3: Move forward 1m + - type: move + linear_velocity: 0.2 + angular_velocity: 0.0 + duration_ms: 5000 + + # Pause + - type: stay + duration_ms: 500 + + # Turn 90 degrees CW + - type: move + linear_velocity: 0.0 + angular_velocity: -0.5 + duration_ms: 3142 + + # Pause + - type: stay + duration_ms: 500 + + # Side 4: Move forward 1m (back to start) + - type: move + linear_velocity: 0.2 + angular_velocity: 0.0 + duration_ms: 5000 + + # Final turn to original heading + - type: move + linear_velocity: 0.0 + angular_velocity: -0.5 + duration_ms: 3142 + +svg_output: + scale: 75.0 + marker_interval_m: 0.25 + trajectory_width: 2.5 diff --git a/vastu-map/scenarios/straight_line.yaml b/vastu-map/scenarios/straight_line.yaml new file mode 100644 index 0000000..c78786b --- /dev/null +++ b/vastu-map/scenarios/straight_line.yaml @@ -0,0 +1,22 @@ +# Straight Line Test Scenario +# Tests scan matching during forward motion + +name: "Straight Line Test" +description: "Move forward at 0.2 m/s for 10 seconds (2m total)" + +map: "medium_room" + +seed: + scans: 20 + rotation: true + +commands: + - type: move + linear_velocity: 0.2 + angular_velocity: 0.0 + duration_ms: 10000 + +svg_output: + scale: 50.0 + marker_interval_m: 0.5 + trajectory_width: 2.0 diff --git a/vastu-map/src/config.rs b/vastu-map/src/config.rs index fc354ef..cd5c33f 100644 --- a/vastu-map/src/config.rs +++ b/vastu-map/src/config.rs @@ -1,327 +1,672 @@ -//! Configuration for VectorMap SLAM. +//! Unified configuration loading for VastuMap. //! -//! All configuration parameters have sensible defaults for indoor robot navigation. +//! Loads all configuration from a single YAML file. +use crate::core::WorldPoint; +use crate::exploration::ExplorationConfig; +use crate::explore::ExplorerConfig; +use crate::grid::{GridConfig, MapConfig, SensorConfig}; +use crate::pathfinding::{AStarConfig, SmoothingConfig}; +use crate::query::{FrontierDetector, RobotFootprint}; use serde::{Deserialize, Serialize}; +use std::path::Path; -/// Lidar measurement noise model. -/// -/// Models measurement uncertainty as a function of range: -/// σ(r) = σ_base + σ_range × r² -/// -/// This allows weighting correspondences by their measurement quality. -/// Close-range measurements are more reliable than far-range ones. -#[derive(Clone, Copy, Debug, Serialize, Deserialize)] -pub struct LidarNoiseModel { - /// Base measurement standard deviation (meters). - /// Minimum uncertainty even at zero range. - /// Default: 0.01m (1cm) - pub sigma_base: f32, +/// Full VastuMap configuration loaded from YAML +#[derive(Clone, Debug, Serialize, Deserialize, Default)] +pub struct VastuConfig { + /// Grid settings + #[serde(default)] + pub grid: GridSection, - /// Range-dependent noise coefficient. - /// Uncertainty grows quadratically with range. - /// Default: 0.001 (1mm per m² of range) - pub sigma_range: f32, + /// Sensor settings + #[serde(default)] + pub sensor: SensorSection, + + /// Query settings + #[serde(default)] + pub query: QuerySection, + + /// Pathfinding settings + #[serde(default)] + pub pathfinding: PathfindingSection, + + /// Exploration settings + #[serde(default)] + pub exploration: ExplorationSection, + + /// Persistence settings + #[serde(default)] + pub persistence: PersistenceSection, } -impl Default for LidarNoiseModel { - fn default() -> Self { - Self { - sigma_base: 0.01, // 1cm base uncertainty - sigma_range: 0.001, // 1mm per m² +impl VastuConfig { + /// Load configuration from a YAML file + pub fn load(path: &Path) -> Result { + let contents = + std::fs::read_to_string(path).map_err(|e| ConfigLoadError::Io(e.to_string()))?; + Self::from_yaml(&contents) + } + + /// Load from default config path (configs/config.yaml) + pub fn load_default() -> Result { + let path = Path::new("configs/config.yaml"); + if path.exists() { + Self::load(path) + } else { + Ok(Self::default()) + } + } + + /// Parse from YAML string + pub fn from_yaml(yaml: &str) -> Result { + serde_yaml::from_str(yaml).map_err(|e| ConfigLoadError::Parse(e.to_string())) + } + + /// Convert to MapConfig for OccupancyGridMap + pub fn to_map_config(&self) -> MapConfig { + MapConfig { + grid: self.grid.to_grid_config(), + sensor: self.sensor.to_sensor_config(), + } + } + + /// Get robot footprint + pub fn robot_footprint(&self) -> RobotFootprint { + RobotFootprint::new( + self.sensor.robot.radius, + self.query.traversability.safety_margin, + ) + } + + /// Convert to ExplorationConfig + pub fn to_exploration_config(&self) -> ExplorationConfig { + ExplorationConfig { + footprint: self.robot_footprint(), + min_frontier_size: self.exploration.min_frontier_size, + frontier_reached_threshold: self.exploration.frontier_reached_threshold, + waypoint_threshold: self.exploration.waypoint_threshold, + max_plan_distance: self.exploration.max_plan_distance, + replan_interval: self.exploration.replan_interval, + max_consecutive_failures: self.exploration.max_consecutive_failures, + path_clearance_weight: self.exploration.path_clearance_weight, + smooth_paths: self.exploration.smooth_paths, + recovery_turn_angle: self.exploration.recovery.turn_angle, + recovery_backup_distance: self.exploration.recovery.backup_distance, + } + } + + /// Convert to AStarConfig + pub fn to_astar_config(&self) -> AStarConfig { + AStarConfig { + footprint: self.robot_footprint(), + allow_diagonal: self.pathfinding.astar.allow_diagonal, + diagonal_cost: self.pathfinding.astar.diagonal_cost, + max_iterations: self.pathfinding.astar.max_iterations, + clearance_weight: self.pathfinding.astar.clearance_weight, + exploration_mode: false, // Default to safe mode + } + } + + /// Convert to SmoothingConfig + pub fn to_smoothing_config(&self) -> SmoothingConfig { + SmoothingConfig { + footprint: self.robot_footprint(), + los_step_size: self.pathfinding.smoothing.los_step_size, + max_iterations: self.pathfinding.smoothing.max_iterations, + } + } + + /// Convert to ExplorerConfig for VastuExplorer + pub fn to_explorer_config(&self) -> ExplorerConfig { + ExplorerConfig { + grid: self.grid.to_grid_config(), + sensor: self.sensor.to_sensor_config(), + exploration: self.to_exploration_config(), + ..ExplorerConfig::default() + } + } + + /// Create frontier detector with config settings + pub fn frontier_detector(&self) -> FrontierDetector { + FrontierDetector { + min_frontier_size: self.query.frontier.min_frontier_size, + use_8_connectivity: self.query.frontier.use_8_connectivity, } } } -impl LidarNoiseModel { - /// Create a noise model with custom parameters. - pub fn new(sigma_base: f32, sigma_range: f32) -> Self { - Self { - sigma_base, - sigma_range, - } - } - - /// Compute measurement standard deviation at a given range. - #[inline] - pub fn sigma(&self, range: f32) -> f32 { - self.sigma_base + self.sigma_range * range * range - } - - /// Compute weight for a measurement at given range. - /// Weight = 1 / σ² - #[inline] - pub fn weight(&self, range: f32) -> f32 { - let sigma = self.sigma(range); - 1.0 / (sigma * sigma) - } - - /// Compute weights for a batch of ranges. - pub fn weights(&self, ranges: &[f32]) -> Vec { - ranges.iter().map(|&r| self.weight(r)).collect() - } - - /// Compute weights for a batch of ranges into a pre-allocated buffer (zero-allocation). - /// - /// # Panics - /// Panics if `weights.len() != ranges.len()`. - /// - /// # Example - /// ```rust,ignore - /// let model = LidarNoiseModel::default(); - /// let ranges = vec![1.0, 2.0, 3.0]; - /// let mut weights = vec![0.0; ranges.len()]; - /// - /// model.compute_weights_into(&ranges, &mut weights); - /// ``` - #[inline] - pub fn compute_weights_into(&self, ranges: &[f32], weights: &mut [f32]) { - debug_assert_eq!( - ranges.len(), - weights.len(), - "weights buffer must match ranges length" - ); - for (r, w) in ranges.iter().zip(weights.iter_mut()) { - *w = self.weight(*r); - } - } - - /// Compute weights for a PointCloud2D based on point distances from origin. - /// - /// This is useful for weighting correspondences during scan matching. - /// - /// # Arguments - /// * `xs` - X coordinates of points - /// * `ys` - Y coordinates of points - /// * `weights` - Output buffer for weights (must match points length) - /// - /// # Example - /// ```rust,ignore - /// let model = LidarNoiseModel::default(); - /// let cloud = PointCloud2D::from_points(&points); - /// let mut weights = vec![0.0; cloud.len()]; - /// - /// model.compute_weights_for_cloud(&cloud.xs, &cloud.ys, &mut weights); - /// ``` - #[inline] - pub fn compute_weights_for_cloud(&self, xs: &[f32], ys: &[f32], weights: &mut [f32]) { - debug_assert_eq!(xs.len(), ys.len(), "xs and ys must have same length"); - debug_assert_eq!( - xs.len(), - weights.len(), - "weights buffer must match points length" - ); - - for i in 0..xs.len() { - let range = (xs[i] * xs[i] + ys[i] * ys[i]).sqrt(); - weights[i] = self.weight(range); - } - } -} - -/// Configuration for exploration mode with point-cloud-based line re-fitting. -/// -/// When exploration mode is enabled, VectorMap stores raw scan data and -/// periodically re-fits lines from accumulated point clouds. This eliminates -/// first-scan bias in line extraction. -/// -/// # Example -/// ```rust,ignore -/// use vastu_map::config::ExplorationConfig; -/// -/// let config = ExplorationConfig::default() -/// .with_refit_interval(10) -/// .with_gap_threshold(0.3); -/// -/// map.enable_exploration_mode(config); -/// ``` +/// Grid configuration section #[derive(Clone, Debug, Serialize, Deserialize)] -pub struct ExplorationConfig { - /// Re-fit lines every N observations. - /// Default: 10 - pub refit_interval: usize, +pub struct GridSection { + /// Cell resolution (meters) + #[serde(default = "default_resolution")] + pub resolution: f32, - /// Minimum points per line to trigger re-fitting. - /// Lines with fewer associated points keep their current geometry. - /// Default: 10 - pub min_points_per_line: usize, + /// Initial grid width (cells) + #[serde(default = "default_grid_size")] + pub initial_width: usize, - /// Maximum perpendicular distance for point-to-line association (meters). - /// Points farther than this from any line are not associated. - /// Default: 0.10m (10cm) - pub max_point_distance: f32, + /// Initial grid height (cells) + #[serde(default = "default_grid_size")] + pub initial_height: usize, - /// Gap threshold for splitting lines (meters). - /// If consecutive points along a line have a gap larger than this, - /// the line is split into multiple segments. - /// Default: 0.30m (30cm) - pub gap_threshold: f32, + /// Origin mode: "center" or "corner" + #[serde(default = "default_origin_mode")] + pub origin_mode: String, - /// Noise model for range-based weighting during re-fitting. - /// Closer points get higher weight. - pub noise_model: LidarNoiseModel, + /// Origin X (for custom mode) + #[serde(default)] + pub origin_x: f32, + + /// Origin Y (for custom mode) + #[serde(default)] + pub origin_y: f32, + + /// Auto expand grid + #[serde(default = "default_true")] + pub auto_expand: bool, } -impl Default for ExplorationConfig { +impl Default for GridSection { fn default() -> Self { Self { - refit_interval: 10, - min_points_per_line: 10, - max_point_distance: 0.10, - gap_threshold: 0.30, - noise_model: LidarNoiseModel::default(), + resolution: 0.025, + initial_width: 800, + initial_height: 800, + origin_mode: "center".to_string(), + origin_x: 0.0, + origin_y: 0.0, + auto_expand: true, } } } -impl ExplorationConfig { - /// Create a new configuration with default values. - pub fn new() -> Self { - Self::default() - } +impl GridSection { + /// Convert to GridConfig + pub fn to_grid_config(&self) -> GridConfig { + let origin = if self.origin_mode == "corner" { + Some(WorldPoint::new(self.origin_x, self.origin_y)) + } else { + None // Centered + }; - /// Set the re-fit interval (every N observations). - pub fn with_refit_interval(mut self, interval: usize) -> Self { - self.refit_interval = interval; - self + GridConfig { + resolution: self.resolution, + initial_width: self.initial_width, + initial_height: self.initial_height, + origin, + auto_expand: self.auto_expand, + max_width: self.initial_width * 2, + max_height: self.initial_height * 2, + } } +} - /// Set the minimum points per line for re-fitting. - pub fn with_min_points(mut self, min_points: usize) -> Self { - self.min_points_per_line = min_points; - self - } +/// Sensor configuration section +#[derive(Clone, Debug, Serialize, Deserialize, Default)] +pub struct SensorSection { + /// LiDAR settings + #[serde(default)] + pub lidar: LidarSettings, - /// Set the maximum point-to-line distance for association. - pub fn with_max_point_distance(mut self, distance: f32) -> Self { - self.max_point_distance = distance; - self - } + /// Robot geometry + #[serde(default)] + pub robot: RobotSettings, - /// Set the gap threshold for line splitting. - pub fn with_gap_threshold(mut self, gap: f32) -> Self { - self.gap_threshold = gap; - self - } + /// Cliff sensor positions + #[serde(default)] + pub cliff_sensors: CliffSensorSettings, - /// Set the noise model for range-based weighting. - pub fn with_noise_model(mut self, model: LidarNoiseModel) -> Self { - self.noise_model = model; - self + /// Bumper settings + #[serde(default)] + pub bumper: BumperSettings, +} + +impl SensorSection { + /// Convert to SensorConfig + pub fn to_sensor_config(&self) -> SensorConfig { + SensorConfig { + robot_radius: self.robot.radius, + lidar_offset: WorldPoint::new(self.lidar.offset_x, self.lidar.offset_y), + confidence_threshold: 3, + max_lidar_range: self.lidar.max_range, + min_lidar_range: self.lidar.min_range, + } } } +/// LiDAR settings +#[derive(Clone, Debug, Serialize, Deserialize)] +pub struct LidarSettings { + /// Offset X from robot center + #[serde(default = "default_lidar_offset_x")] + pub offset_x: f32, + + /// Offset Y from robot center + #[serde(default)] + pub offset_y: f32, + + /// Minimum valid range + #[serde(default = "default_min_range")] + pub min_range: f32, + + /// Maximum valid range + #[serde(default = "default_max_range")] + pub max_range: f32, +} + +impl Default for LidarSettings { + fn default() -> Self { + Self { + offset_x: -0.110, + offset_y: 0.0, + min_range: 0.15, + max_range: 8.0, + } + } +} + +/// Robot geometry settings +#[derive(Clone, Debug, Serialize, Deserialize)] +pub struct RobotSettings { + /// Robot radius (meters) + #[serde(default = "default_robot_radius")] + pub radius: f32, +} + +impl Default for RobotSettings { + fn default() -> Self { + Self { radius: 0.17 } + } +} + +/// Cliff sensor positions +#[derive(Clone, Debug, Serialize, Deserialize)] +pub struct CliffSensorSettings { + /// Left side sensor position [x, y] + pub left_side: [f32; 2], + /// Left front sensor position [x, y] + pub left_front: [f32; 2], + /// Right front sensor position [x, y] + pub right_front: [f32; 2], + /// Right side sensor position [x, y] + pub right_side: [f32; 2], +} + +impl Default for CliffSensorSettings { + fn default() -> Self { + Self { + left_side: [0.12, 0.10], + left_front: [0.15, 0.05], + right_front: [0.15, -0.05], + right_side: [0.12, -0.10], + } + } +} + +/// Bumper settings +#[derive(Clone, Debug, Serialize, Deserialize)] +pub struct BumperSettings { + /// Left bumper angle (radians from forward) + #[serde(default = "default_left_bumper_angle")] + pub left_angle: f32, + + /// Right bumper angle (radians from forward) + #[serde(default = "default_right_bumper_angle")] + pub right_angle: f32, +} + +impl Default for BumperSettings { + fn default() -> Self { + Self { + left_angle: 0.5, + right_angle: -0.5, + } + } +} + +/// Query settings section +#[derive(Clone, Debug, Serialize, Deserialize, Default)] +pub struct QuerySection { + /// Frontier detection settings + #[serde(default)] + pub frontier: FrontierSettings, + + /// Traversability checking settings + #[serde(default)] + pub traversability: TraversabilitySettings, +} + +/// Frontier detection settings +#[derive(Clone, Debug, Serialize, Deserialize)] +pub struct FrontierSettings { + /// Minimum cells for valid frontier + #[serde(default = "default_min_frontier_size")] + pub min_frontier_size: usize, + + /// Use 8-connected grid (vs 4-connected) + #[serde(default = "default_true")] + pub use_8_connectivity: bool, +} + +impl Default for FrontierSettings { + fn default() -> Self { + Self { + min_frontier_size: 5, + use_8_connectivity: true, + } + } +} + +/// Traversability settings +#[derive(Clone, Debug, Serialize, Deserialize)] +pub struct TraversabilitySettings { + /// Extra clearance around robot (meters) + #[serde(default = "default_safety_margin")] + pub safety_margin: f32, +} + +impl Default for TraversabilitySettings { + fn default() -> Self { + Self { + safety_margin: 0.05, + } + } +} + +/// Pathfinding settings section +#[derive(Clone, Debug, Serialize, Deserialize, Default)] +pub struct PathfindingSection { + /// A* algorithm settings + #[serde(default)] + pub astar: AStarSettings, + + /// Path smoothing settings + #[serde(default)] + pub smoothing: SmoothingSettings, +} + +/// A* algorithm settings +#[derive(Clone, Debug, Serialize, Deserialize)] +pub struct AStarSettings { + /// Enable 8-directional movement + #[serde(default = "default_true")] + pub allow_diagonal: bool, + + /// Cost multiplier for diagonal moves (sqrt(2)) + #[serde(default = "default_diagonal_cost")] + pub diagonal_cost: f32, + + /// Maximum nodes to expand + #[serde(default = "default_max_iterations")] + pub max_iterations: usize, + + /// Penalty for paths near obstacles (0=disabled) + #[serde(default)] + pub clearance_weight: f32, +} + +impl Default for AStarSettings { + fn default() -> Self { + Self { + allow_diagonal: true, + diagonal_cost: std::f32::consts::SQRT_2, + max_iterations: 100_000, + clearance_weight: 0.0, + } + } +} + +/// Path smoothing settings +#[derive(Clone, Debug, Serialize, Deserialize)] +pub struct SmoothingSettings { + /// Enable path smoothing + #[serde(default = "default_true")] + pub enabled: bool, + + /// Line-of-sight check step (meters) + #[serde(default = "default_los_step")] + pub los_step_size: f32, + + /// Midpoint smoothing iterations + #[serde(default = "default_smooth_iterations")] + pub max_iterations: usize, +} + +impl Default for SmoothingSettings { + fn default() -> Self { + Self { + enabled: true, + los_step_size: 0.05, + max_iterations: 100, + } + } +} + +/// Exploration settings section +#[derive(Clone, Debug, Serialize, Deserialize)] +pub struct ExplorationSection { + /// Minimum cells for valid frontier + #[serde(default = "default_min_frontier_size")] + pub min_frontier_size: usize, + + /// Distance to consider frontier reached (meters) + #[serde(default = "default_frontier_threshold")] + pub frontier_reached_threshold: f32, + + /// Distance for waypoint reached (meters) + #[serde(default = "default_waypoint_threshold")] + pub waypoint_threshold: f32, + + /// Maximum planning distance (meters) + #[serde(default = "default_max_plan_distance")] + pub max_plan_distance: f32, + + /// Seconds between replans when blocked + #[serde(default = "default_replan_interval")] + pub replan_interval: f32, + + /// Max consecutive planning failures before giving up + #[serde(default = "default_max_failures")] + pub max_consecutive_failures: usize, + + /// Penalty for paths near obstacles + #[serde(default)] + pub path_clearance_weight: f32, + + /// Enable path smoothing + #[serde(default = "default_true")] + pub smooth_paths: bool, + + /// Recovery behavior settings + #[serde(default)] + pub recovery: RecoverySettings, +} + +impl Default for ExplorationSection { + fn default() -> Self { + Self { + min_frontier_size: 5, + frontier_reached_threshold: 0.3, + waypoint_threshold: 0.1, + max_plan_distance: 5.0, + replan_interval: 1.0, + max_consecutive_failures: 5, + path_clearance_weight: 0.0, + smooth_paths: true, + recovery: RecoverySettings::default(), + } + } +} + +/// Recovery behavior settings +#[derive(Clone, Debug, Serialize, Deserialize)] +pub struct RecoverySettings { + /// Turn angle for recovery (radians) + #[serde(default = "default_turn_angle")] + pub turn_angle: f32, + + /// Backup distance for recovery (meters) + #[serde(default = "default_backup_distance")] + pub backup_distance: f32, +} + +impl Default for RecoverySettings { + fn default() -> Self { + Self { + turn_angle: std::f32::consts::FRAC_PI_4, + backup_distance: 0.1, + } + } +} + +/// Persistence settings section +#[derive(Clone, Debug, Serialize, Deserialize)] +pub struct PersistenceSection { + /// Output format: "vastu", "pgm", or "both" + #[serde(default = "default_output_format")] + pub output_format: String, + + /// Output directory path + #[serde(default = "default_output_dir")] + pub output_dir: String, + + /// Auto-save interval (seconds, 0=disabled) + #[serde(default)] + pub auto_save_interval: u32, +} + +impl Default for PersistenceSection { + fn default() -> Self { + Self { + output_format: "both".to_string(), + output_dir: "./output".to_string(), + auto_save_interval: 30, + } + } +} + +/// Config load error +#[derive(Debug, Clone)] +pub enum ConfigLoadError { + /// I/O error + Io(String), + /// Parse error + Parse(String), +} + +impl std::fmt::Display for ConfigLoadError { + fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result { + match self { + ConfigLoadError::Io(msg) => write!(f, "IO error: {}", msg), + ConfigLoadError::Parse(msg) => write!(f, "Parse error: {}", msg), + } + } +} + +impl std::error::Error for ConfigLoadError {} + +// Default value functions for serde +fn default_resolution() -> f32 { + 0.025 +} +fn default_grid_size() -> usize { + 800 +} +fn default_origin_mode() -> String { + "center".to_string() +} +fn default_true() -> bool { + true +} +fn default_lidar_offset_x() -> f32 { + -0.110 +} +fn default_min_range() -> f32 { + 0.15 +} +fn default_max_range() -> f32 { + 8.0 +} +fn default_robot_radius() -> f32 { + 0.17 +} +fn default_left_bumper_angle() -> f32 { + 0.5 +} +fn default_right_bumper_angle() -> f32 { + -0.5 +} +fn default_min_frontier_size() -> usize { + 5 +} +fn default_safety_margin() -> f32 { + 0.05 +} +fn default_diagonal_cost() -> f32 { + std::f32::consts::SQRT_2 +} +fn default_max_iterations() -> usize { + 100_000 +} +fn default_los_step() -> f32 { + 0.05 +} +fn default_smooth_iterations() -> usize { + 100 +} +fn default_frontier_threshold() -> f32 { + 0.3 +} +fn default_waypoint_threshold() -> f32 { + 0.1 +} +fn default_max_plan_distance() -> f32 { + 5.0 +} +fn default_replan_interval() -> f32 { + 1.0 +} +fn default_max_failures() -> usize { + 5 +} +fn default_turn_angle() -> f32 { + std::f32::consts::FRAC_PI_4 +} +fn default_backup_distance() -> f32 { + 0.1 +} +fn default_output_format() -> String { + "both".to_string() +} +fn default_output_dir() -> String { + "./output".to_string() +} + #[cfg(test)] mod tests { use super::*; #[test] - fn test_lidar_noise_model_default() { - let model = LidarNoiseModel::default(); - assert_eq!(model.sigma_base, 0.01); - assert_eq!(model.sigma_range, 0.001); + fn test_default_config() { + let config = VastuConfig::default(); + assert_eq!(config.grid.resolution, 0.025); + assert_eq!(config.sensor.robot.radius, 0.17); } #[test] - fn test_lidar_noise_model_sigma() { - let model = LidarNoiseModel::default(); - - // At 0m range, sigma = sigma_base = 0.01 - assert!((model.sigma(0.0) - 0.01).abs() < 1e-6); - - // At 1m range, sigma = 0.01 + 0.001 * 1 = 0.011 - assert!((model.sigma(1.0) - 0.011).abs() < 1e-6); - - // At 5m range, sigma = 0.01 + 0.001 * 25 = 0.035 - assert!((model.sigma(5.0) - 0.035).abs() < 1e-6); + fn test_yaml_roundtrip() { + let config = VastuConfig::default(); + let yaml = serde_yaml::to_string(&config).unwrap(); + let parsed: VastuConfig = serde_yaml::from_str(&yaml).unwrap(); + assert_eq!(parsed.grid.resolution, config.grid.resolution); } #[test] - fn test_lidar_noise_model_weight() { - let model = LidarNoiseModel::default(); - - // Weight at 0m = 1 / (0.01)^2 = 10000 - let w0 = model.weight(0.0); - assert!((w0 - 10000.0).abs() < 1.0); - - // Weight decreases with range - let w1 = model.weight(1.0); - let w5 = model.weight(5.0); - assert!(w0 > w1); - assert!(w1 > w5); - } - - #[test] - fn test_lidar_noise_model_weights_batch() { - let model = LidarNoiseModel::default(); - let ranges = vec![0.5, 1.0, 2.0, 5.0]; - let weights = model.weights(&ranges); - - assert_eq!(weights.len(), 4); - // Weights should decrease with range - assert!(weights[0] > weights[1]); - assert!(weights[1] > weights[2]); - assert!(weights[2] > weights[3]); - } - - #[test] - fn test_compute_weights_into() { - let model = LidarNoiseModel::default(); - let ranges = vec![0.5, 1.0, 2.0, 5.0]; - let mut weights = vec![0.0; ranges.len()]; - - model.compute_weights_into(&ranges, &mut weights); - - // Should match allocating version - let expected = model.weights(&ranges); - for (w, e) in weights.iter().zip(expected.iter()) { - assert!((w - e).abs() < 1e-6); - } - } - - #[test] - fn test_compute_weights_for_cloud() { - let model = LidarNoiseModel::default(); - let xs = vec![1.0, 0.0, 3.0, 4.0]; - let ys = vec![0.0, 2.0, 4.0, 0.0]; - let mut weights = vec![0.0; xs.len()]; - - model.compute_weights_for_cloud(&xs, &ys, &mut weights); - - // Check against manual range computation - let ranges: Vec = xs - .iter() - .zip(ys.iter()) - .map(|(x, y)| (x * x + y * y).sqrt()) - .collect(); - let expected = model.weights(&ranges); - - for (w, e) in weights.iter().zip(expected.iter()) { - assert!((w - e).abs() < 1e-6); - } - } - - #[test] - fn test_exploration_config_default() { - let config = ExplorationConfig::default(); - - assert_eq!(config.refit_interval, 10); - assert_eq!(config.min_points_per_line, 10); - assert!((config.max_point_distance - 0.10).abs() < 1e-6); - assert!((config.gap_threshold - 0.30).abs() < 1e-6); - } - - #[test] - fn test_exploration_config_builder() { - let config = ExplorationConfig::new() - .with_refit_interval(20) - .with_min_points(15) - .with_max_point_distance(0.15) - .with_gap_threshold(0.5); - - assert_eq!(config.refit_interval, 20); - assert_eq!(config.min_points_per_line, 15); - assert!((config.max_point_distance - 0.15).abs() < 1e-6); - assert!((config.gap_threshold - 0.5).abs() < 1e-6); + fn test_to_map_config() { + let config = VastuConfig::default(); + let map_config = config.to_map_config(); + assert_eq!(map_config.grid.resolution, 0.025); + assert_eq!(map_config.sensor.robot_radius, 0.17); } } diff --git a/vastu-map/src/core/README.md b/vastu-map/src/core/README.md deleted file mode 100644 index e99de6c..0000000 --- a/vastu-map/src/core/README.md +++ /dev/null @@ -1,92 +0,0 @@ -# Core Module - -Fundamental geometric types for 2D SLAM operations. - -## Components - -| File | Description | -|------|-------------| -| `point.rs` | `Point2D` - 2D point/vector with geometric operations | -| `pose.rs` | `Pose2D` - 2D position and orientation with frame transforms | -| `scan.rs` | `PolarScan` and `PointCloud2D` - Lidar data representations | -| `bounds.rs` | `Bounds` - Axis-aligned bounding box | -| `math.rs` | Common math utilities (angle normalization, etc.) | - -## Coordinate Frame - -All coordinates follow ROS REP-103 convention: -- **X-forward**: Positive X is ahead of the robot -- **Y-left**: Positive Y is to the robot's left -- **Rotation**: Counter-clockwise positive (radians) - -## Key Types - -### Point2D - -2D point with full geometric operations: - -```rust -let p = Point2D::new(3.0, 4.0); -p.length(); // Distance from origin (5.0) -p.normalized(); // Unit vector -p.rotate(angle); // Rotate around origin -p.dot(other); // Dot product -p.cross(other); // 2D cross product (scalar) -``` - -### Pose2D - -Robot pose with frame transformation: - -```rust -let pose = Pose2D::new(x, y, theta); - -// Transform point from robot frame to world frame -let world_pt = pose.transform_point(local_pt); - -// Compose poses: pose_C = pose_A.compose(pose_B) -let combined = pose_a.compose(pose_b); - -// Inverse transform -let local_pt = pose.inverse().transform_point(world_pt); -``` - -### PointCloud2D - -Point cloud with SoA (Struct-of-Arrays) layout for SIMD efficiency: - -```rust -let cloud = PointCloud2D::from_points(&points); - -// Transform entire cloud -let world_cloud = cloud.transform(&pose); - -// Access raw arrays for SIMD -let xs: &[f32] = cloud.xs(); -let ys: &[f32] = cloud.ys(); -``` - -### PolarScan - -Raw lidar data in polar coordinates: - -```rust -let mut scan = PolarScan::new(); -scan.push(angle, distance, quality); - -// Convert to Cartesian, filtering by quality/range -let cloud = scan.to_cartesian(min_quality, min_range, max_range); -``` - -## Data Layout - -`PointCloud2D` uses Struct-of-Arrays for cache-friendly SIMD operations: - -``` -AoS (typical): SoA (this implementation): -[Point{x,y}] xs: [x0, x1, x2, x3, ...] -[Point{x,y}] → ys: [y0, y1, y2, y3, ...] -[Point{x,y}] -``` - -This enables 4-wide SIMD operations via `std::simd::f32x4`. diff --git a/vastu-map/src/core/bounds.rs b/vastu-map/src/core/bounds.rs deleted file mode 100644 index f1dd8b1..0000000 --- a/vastu-map/src/core/bounds.rs +++ /dev/null @@ -1,330 +0,0 @@ -//! Axis-aligned bounding box for spatial operations. -//! -//! [`Bounds`] represents a rectangular region in 2D space, commonly used for: -//! - Map extent tracking (how large is the explored area) -//! - Spatial queries (is a point inside a region) -//! - Collision detection (do two regions overlap) -//! - Viewport clipping (what's visible on screen) -//! -//! # Usage -//! -//! ```rust -//! use vastu_map::core::{Bounds, Point2D}; -//! -//! // Create bounds from corner points -//! let bounds = Bounds::new( -//! Point2D::new(0.0, 0.0), // min corner -//! Point2D::new(10.0, 8.0), // max corner -//! ); -//! -//! // Query properties -//! assert_eq!(bounds.width(), 10.0); -//! assert_eq!(bounds.height(), 8.0); -//! assert!(bounds.contains(Point2D::new(5.0, 4.0))); -//! -//! // Expand dynamically as robot explores -//! let mut map_bounds = Bounds::empty(); -//! map_bounds.expand_to_include(Point2D::new(1.0, 1.0)); -//! map_bounds.expand_to_include(Point2D::new(-2.0, 3.0)); -//! // Now bounds: min=(-2, 1), max=(1, 3) -//! ``` - -use super::point::Point2D; - -/// Axis-aligned bounding box. -/// -/// Represents a rectangular region in 2D space. -#[derive(Clone, Copy, Debug, Default, PartialEq)] -pub struct Bounds { - /// Minimum corner (smallest x and y values). - pub min: Point2D, - /// Maximum corner (largest x and y values). - pub max: Point2D, -} - -impl Bounds { - /// Create a new bounding box from min and max corners. - #[inline] - pub const fn new(min: Point2D, max: Point2D) -> Self { - Self { min, max } - } - - /// Create an empty (invalid) bounding box. - /// - /// The empty bounds has min > max, so it will expand to fit any point. - #[inline] - pub fn empty() -> Self { - Self { - min: Point2D::new(f32::INFINITY, f32::INFINITY), - max: Point2D::new(f32::NEG_INFINITY, f32::NEG_INFINITY), - } - } - - /// Check if the bounds are empty (invalid). - #[inline] - pub fn is_empty(&self) -> bool { - self.min.x > self.max.x || self.min.y > self.max.y - } - - /// Create bounds from a single point. - #[inline] - pub fn from_point(point: Point2D) -> Self { - Self { - min: point, - max: point, - } - } - - /// Width of the bounding box (x extent). - #[inline] - pub fn width(&self) -> f32 { - self.max.x - self.min.x - } - - /// Height of the bounding box (y extent). - #[inline] - pub fn height(&self) -> f32 { - self.max.y - self.min.y - } - - /// Size of the bounding box as a Point2D (width, height). - #[inline] - pub fn size(&self) -> Point2D { - Point2D::new(self.width(), self.height()) - } - - /// Center of the bounding box. - #[inline] - pub fn center(&self) -> Point2D { - Point2D::new( - (self.min.x + self.max.x) * 0.5, - (self.min.y + self.max.y) * 0.5, - ) - } - - /// Check if a point is inside the bounding box. - #[inline] - pub fn contains(&self, point: Point2D) -> bool { - point.x >= self.min.x - && point.x <= self.max.x - && point.y >= self.min.y - && point.y <= self.max.y - } - - /// Check if this bounds intersects with another. - #[inline] - pub fn intersects(&self, other: &Bounds) -> bool { - self.min.x <= other.max.x - && self.max.x >= other.min.x - && self.min.y <= other.max.y - && self.max.y >= other.min.y - } - - /// Compute the intersection of two bounds. - /// - /// Returns empty bounds if they don't intersect. - #[inline] - pub fn intersection(&self, other: &Bounds) -> Self { - Self { - min: self.min.max(other.min), - max: self.max.min(other.max), - } - } - - /// Compute the union of two bounds (smallest box containing both). - #[inline] - pub fn union(&self, other: &Bounds) -> Self { - if self.is_empty() { - return *other; - } - if other.is_empty() { - return *self; - } - Self { - min: self.min.min(other.min), - max: self.max.max(other.max), - } - } - - /// Expand bounds to include a point. - #[inline] - pub fn expand_to_include(&mut self, point: Point2D) { - self.min = self.min.min(point); - self.max = self.max.max(point); - } - - /// Expand bounds by a margin on all sides. - #[inline] - pub fn expand(&self, margin: f32) -> Self { - Self { - min: Point2D::new(self.min.x - margin, self.min.y - margin), - max: Point2D::new(self.max.x + margin, self.max.y + margin), - } - } - - /// Clamp a point to be inside the bounds. - #[inline] - pub fn clamp(&self, point: Point2D) -> Point2D { - Point2D::new( - point.x.max(self.min.x).min(self.max.x), - point.y.max(self.min.y).min(self.max.y), - ) - } - - /// Get corners of the bounding box. - /// - /// Returns [min, (max.x, min.y), max, (min.x, max.y)] in CCW order. - #[inline] - pub fn corners(&self) -> [Point2D; 4] { - [ - self.min, - Point2D::new(self.max.x, self.min.y), - self.max, - Point2D::new(self.min.x, self.max.y), - ] - } -} - -#[cfg(test)] -mod tests { - use super::*; - - #[test] - fn test_new() { - let bounds = Bounds::new(Point2D::new(0.0, 0.0), Point2D::new(10.0, 20.0)); - - assert_eq!(bounds.min, Point2D::new(0.0, 0.0)); - assert_eq!(bounds.max, Point2D::new(10.0, 20.0)); - } - - #[test] - fn test_empty() { - let bounds = Bounds::empty(); - assert!(bounds.is_empty()); - - let valid = Bounds::new(Point2D::new(0.0, 0.0), Point2D::new(1.0, 1.0)); - assert!(!valid.is_empty()); - } - - #[test] - fn test_from_point() { - let bounds = Bounds::from_point(Point2D::new(5.0, 10.0)); - - assert_eq!(bounds.min, Point2D::new(5.0, 10.0)); - assert_eq!(bounds.max, Point2D::new(5.0, 10.0)); - assert_eq!(bounds.width(), 0.0); - assert_eq!(bounds.height(), 0.0); - } - - #[test] - fn test_dimensions() { - let bounds = Bounds::new(Point2D::new(1.0, 2.0), Point2D::new(5.0, 8.0)); - - assert_eq!(bounds.width(), 4.0); - assert_eq!(bounds.height(), 6.0); - assert_eq!(bounds.size(), Point2D::new(4.0, 6.0)); - assert_eq!(bounds.center(), Point2D::new(3.0, 5.0)); - } - - #[test] - fn test_contains() { - let bounds = Bounds::new(Point2D::new(0.0, 0.0), Point2D::new(10.0, 10.0)); - - assert!(bounds.contains(Point2D::new(5.0, 5.0))); - assert!(bounds.contains(Point2D::new(0.0, 0.0))); // Edge - assert!(bounds.contains(Point2D::new(10.0, 10.0))); // Edge - assert!(!bounds.contains(Point2D::new(-1.0, 5.0))); - assert!(!bounds.contains(Point2D::new(5.0, 11.0))); - } - - #[test] - fn test_intersects() { - let a = Bounds::new(Point2D::new(0.0, 0.0), Point2D::new(10.0, 10.0)); - let b = Bounds::new(Point2D::new(5.0, 5.0), Point2D::new(15.0, 15.0)); - let c = Bounds::new(Point2D::new(20.0, 20.0), Point2D::new(30.0, 30.0)); - - assert!(a.intersects(&b)); - assert!(b.intersects(&a)); - assert!(!a.intersects(&c)); - assert!(!c.intersects(&a)); - } - - #[test] - fn test_intersection() { - let a = Bounds::new(Point2D::new(0.0, 0.0), Point2D::new(10.0, 10.0)); - let b = Bounds::new(Point2D::new(5.0, 5.0), Point2D::new(15.0, 15.0)); - - let inter = a.intersection(&b); - - assert_eq!(inter.min, Point2D::new(5.0, 5.0)); - assert_eq!(inter.max, Point2D::new(10.0, 10.0)); - } - - #[test] - fn test_union() { - let a = Bounds::new(Point2D::new(0.0, 0.0), Point2D::new(10.0, 10.0)); - let b = Bounds::new(Point2D::new(5.0, 5.0), Point2D::new(15.0, 15.0)); - - let u = a.union(&b); - - assert_eq!(u.min, Point2D::new(0.0, 0.0)); - assert_eq!(u.max, Point2D::new(15.0, 15.0)); - } - - #[test] - fn test_union_with_empty() { - let a = Bounds::new(Point2D::new(0.0, 0.0), Point2D::new(10.0, 10.0)); - let empty = Bounds::empty(); - - assert_eq!(a.union(&empty), a); - assert_eq!(empty.union(&a), a); - } - - #[test] - fn test_expand_to_include() { - let mut bounds = Bounds::empty(); - - bounds.expand_to_include(Point2D::new(5.0, 5.0)); - assert_eq!(bounds.min, Point2D::new(5.0, 5.0)); - assert_eq!(bounds.max, Point2D::new(5.0, 5.0)); - - bounds.expand_to_include(Point2D::new(0.0, 10.0)); - assert_eq!(bounds.min, Point2D::new(0.0, 5.0)); - assert_eq!(bounds.max, Point2D::new(5.0, 10.0)); - } - - #[test] - fn test_expand() { - let bounds = Bounds::new(Point2D::new(5.0, 5.0), Point2D::new(10.0, 10.0)); - let expanded = bounds.expand(2.0); - - assert_eq!(expanded.min, Point2D::new(3.0, 3.0)); - assert_eq!(expanded.max, Point2D::new(12.0, 12.0)); - } - - #[test] - fn test_clamp() { - let bounds = Bounds::new(Point2D::new(0.0, 0.0), Point2D::new(10.0, 10.0)); - - assert_eq!(bounds.clamp(Point2D::new(5.0, 5.0)), Point2D::new(5.0, 5.0)); - assert_eq!( - bounds.clamp(Point2D::new(-5.0, 5.0)), - Point2D::new(0.0, 5.0) - ); - assert_eq!( - bounds.clamp(Point2D::new(15.0, 15.0)), - Point2D::new(10.0, 10.0) - ); - } - - #[test] - fn test_corners() { - let bounds = Bounds::new(Point2D::new(0.0, 0.0), Point2D::new(10.0, 20.0)); - let corners = bounds.corners(); - - assert_eq!(corners[0], Point2D::new(0.0, 0.0)); - assert_eq!(corners[1], Point2D::new(10.0, 0.0)); - assert_eq!(corners[2], Point2D::new(10.0, 20.0)); - assert_eq!(corners[3], Point2D::new(0.0, 20.0)); - } -} diff --git a/vastu-map/src/core/cell.rs b/vastu-map/src/core/cell.rs new file mode 100644 index 0000000..521e9ec --- /dev/null +++ b/vastu-map/src/core/cell.rs @@ -0,0 +1,270 @@ +//! Cell types for the occupancy grid. +//! +//! VastuMap uses semantic cell types that distinguish between different +//! obstacle sources (walls, cliffs, invisible obstacles like glass). + +use serde::{Deserialize, Serialize}; + +/// Semantic cell type - what kind of obstacle/surface is this? +/// +/// The cell type hierarchy: +/// - `Unknown` - Not yet observed by any sensor +/// - `Floor` - Traversable surface (lidar sees through, no cliff, no bumper hit) +/// - `Wall` - Lidar-detected obstacle (solid wall, furniture) +/// - `Cliff` - Floor drop-off detected by cliff sensors (stairs, ledges) +/// - `Bump` - Invisible to lidar but bumper hit (glass doors, mirrors, thin legs) +#[derive(Clone, Copy, Debug, PartialEq, Eq, Hash, Default, Serialize, Deserialize)] +#[repr(u8)] +pub enum CellType { + /// Cell has never been observed by any sensor + #[default] + Unknown = 0, + + /// Traversable floor surface + /// - Lidar rays pass through (or hit far away) + /// - Cliff sensors report safe + /// - No bumper collision history + Floor = 1, + + /// Solid wall or obstacle detected by lidar + /// - Lidar ray terminates here + /// - Typical: walls, furniture, doors + Wall = 2, + + /// Floor drop-off detected by cliff sensors + /// - Lidar may see far (looks like floor) + /// - Cliff sensor triggered at this position + /// - Typical: stairs, ledges, thresholds + Cliff = 3, + + /// Invisible obstacle - lidar passes through but physical collision + /// - Lidar ray passes through (appears as Floor) + /// - Bumper triggered collision at this position + /// - Typical: glass doors, mirrors, thin table legs + Bump = 4, +} + +impl CellType { + /// Can the robot traverse this cell? + #[inline] + pub fn is_traversable(self) -> bool { + matches!(self, CellType::Floor) + } + + /// Is this cell an obstacle (any type)? + #[inline] + pub fn is_obstacle(self) -> bool { + matches!(self, CellType::Wall | CellType::Cliff | CellType::Bump) + } + + /// Has this cell been observed? + #[inline] + pub fn is_known(self) -> bool { + self != CellType::Unknown + } + + /// Convert from u8 (for deserialization) + #[inline] + pub fn from_u8(value: u8) -> Self { + match value { + 0 => CellType::Unknown, + 1 => CellType::Floor, + 2 => CellType::Wall, + 3 => CellType::Cliff, + 4 => CellType::Bump, + _ => CellType::Unknown, + } + } + + /// Single character representation for debugging + pub fn as_char(self) -> char { + match self { + CellType::Unknown => '?', + CellType::Floor => '.', + CellType::Wall => '#', + CellType::Cliff => 'v', + CellType::Bump => 'x', + } + } +} + +/// A single cell in the grid with metadata +#[derive(Clone, Copy, Debug, Default)] +pub struct Cell { + /// What type of cell is this? + pub cell_type: CellType, + + /// Confidence level (0-255) + /// - 0 = unknown/unobserved + /// - Higher = more confident (many consistent observations) + pub confidence: u8, + + /// Observation count (saturates at 255) + pub observation_count: u8, + + /// Has this cell been swept/cleaned? + pub swept: bool, +} + +impl Cell { + /// Create a new unknown cell + #[inline] + pub fn new() -> Self { + Self::default() + } + + /// Create a cell with a specific type + #[inline] + pub fn with_type(cell_type: CellType) -> Self { + Self { + cell_type, + confidence: 1, + observation_count: 1, + swept: false, + } + } + + /// Is this cell traversable? + #[inline] + pub fn is_traversable(&self) -> bool { + self.cell_type.is_traversable() + } + + /// Is this cell an obstacle? + #[inline] + pub fn is_obstacle(&self) -> bool { + self.cell_type.is_obstacle() + } + + /// Has this cell been observed? + #[inline] + pub fn is_known(&self) -> bool { + self.cell_type.is_known() + } + + /// Update cell with a new observation of the same type + /// Returns true if cell type changed + pub fn observe(&mut self, observed_type: CellType) -> bool { + let changed = self.cell_type != observed_type; + + if changed { + // Type changed - reset confidence + self.cell_type = observed_type; + self.confidence = 1; + self.observation_count = 1; + } else { + // Same type - increase confidence + self.confidence = self.confidence.saturating_add(1); + self.observation_count = self.observation_count.saturating_add(1); + } + + changed + } + + /// Update cell with a new observation, with priority rules + /// + /// Priority (higher overrides lower): + /// 1. Bump (physical collision is definitive) + /// 2. Cliff (safety critical) + /// 3. Wall (lidar detection) + /// 4. Floor (absence of obstacles) + /// 5. Unknown (no data) + pub fn observe_with_priority(&mut self, observed_type: CellType) -> bool { + let should_update = match (self.cell_type, observed_type) { + // Unknown can be updated by anything + (CellType::Unknown, _) => true, + + // Bump is highest priority - only update with another Bump + (CellType::Bump, CellType::Bump) => true, + (CellType::Bump, _) => false, + + // Cliff can be updated by Bump or Cliff + (CellType::Cliff, CellType::Bump) => true, + (CellType::Cliff, CellType::Cliff) => true, + (CellType::Cliff, _) => false, + + // Wall can be updated by Bump, Cliff, or Wall + (CellType::Wall, CellType::Bump) => true, + (CellType::Wall, CellType::Cliff) => true, + (CellType::Wall, CellType::Wall) => true, + (CellType::Wall, _) => false, + + // Floor can be updated by anything + (CellType::Floor, _) => true, + }; + + if should_update { + self.observe(observed_type) + } else { + // Still count the observation even if type didn't change + self.observation_count = self.observation_count.saturating_add(1); + false + } + } +} + +#[cfg(test)] +mod tests { + use super::*; + + #[test] + fn test_cell_type_traversable() { + assert!(!CellType::Unknown.is_traversable()); + assert!(CellType::Floor.is_traversable()); + assert!(!CellType::Wall.is_traversable()); + assert!(!CellType::Cliff.is_traversable()); + assert!(!CellType::Bump.is_traversable()); + } + + #[test] + fn test_cell_type_obstacle() { + assert!(!CellType::Unknown.is_obstacle()); + assert!(!CellType::Floor.is_obstacle()); + assert!(CellType::Wall.is_obstacle()); + assert!(CellType::Cliff.is_obstacle()); + assert!(CellType::Bump.is_obstacle()); + } + + #[test] + fn test_cell_observe() { + let mut cell = Cell::new(); + assert!(!cell.is_known()); + + // First observation + assert!(cell.observe(CellType::Floor)); + assert_eq!(cell.cell_type, CellType::Floor); + assert_eq!(cell.confidence, 1); + + // Same observation increases confidence + assert!(!cell.observe(CellType::Floor)); + assert_eq!(cell.confidence, 2); + + // Different observation resets confidence + assert!(cell.observe(CellType::Wall)); + assert_eq!(cell.cell_type, CellType::Wall); + assert_eq!(cell.confidence, 1); + } + + #[test] + fn test_cell_priority() { + let mut cell = Cell::with_type(CellType::Floor); + + // Wall overrides Floor + assert!(cell.observe_with_priority(CellType::Wall)); + assert_eq!(cell.cell_type, CellType::Wall); + + // Floor does NOT override Wall + assert!(!cell.observe_with_priority(CellType::Floor)); + assert_eq!(cell.cell_type, CellType::Wall); + + // Bump overrides Wall + assert!(cell.observe_with_priority(CellType::Bump)); + assert_eq!(cell.cell_type, CellType::Bump); + + // Nothing overrides Bump except Bump + assert!(!cell.observe_with_priority(CellType::Floor)); + assert!(!cell.observe_with_priority(CellType::Wall)); + assert!(!cell.observe_with_priority(CellType::Cliff)); + assert_eq!(cell.cell_type, CellType::Bump); + } +} diff --git a/vastu-map/src/core/math.rs b/vastu-map/src/core/math.rs deleted file mode 100644 index 71bbb72..0000000 --- a/vastu-map/src/core/math.rs +++ /dev/null @@ -1,457 +0,0 @@ -//! Mathematical utilities for angles and geometry. -//! -//! All angles are in radians. Coordinate frame follows ROS REP-103: -//! - X-forward, Y-left, Z-up -//! - Counter-clockwise positive rotation - -use std::f32::consts::PI; -use std::simd::{f32x4, num::SimdFloat}; - -/// Two times PI (full circle in radians). -pub const TWO_PI: f32 = 2.0 * PI; - -/// Normalize angle to [-π, π). -/// -/// # Example -/// ``` -/// use vastu_map::core::math::normalize_angle; -/// use std::f32::consts::PI; -/// -/// // Values near ±π may normalize to either +π or -π due to floating-point -/// assert!(normalize_angle(3.0 * PI).abs() - PI < 1e-5); -/// assert!(normalize_angle(-3.0 * PI).abs() - PI < 1e-5); -/// assert!((normalize_angle(PI / 2.0) - PI / 2.0).abs() < 1e-6); -/// ``` -#[inline] -pub fn normalize_angle(angle: f32) -> f32 { - let mut a = angle % TWO_PI; - if a >= PI { - a -= TWO_PI; - } else if a < -PI { - a += TWO_PI; - } - a -} - -/// Compute the signed angular difference between two angles. -/// -/// Returns the shortest angular distance from `from` to `to`, -/// in the range [-π, π). -/// -/// Positive result means counter-clockwise rotation from `from` to `to`. -/// -/// # Example -/// ``` -/// use vastu_map::core::math::angle_diff; -/// use std::f32::consts::PI; -/// -/// // 90° difference -/// let diff = angle_diff(0.0, PI / 2.0); -/// assert!((diff - PI / 2.0).abs() < 1e-6); -/// -/// // Crossing -π/π boundary -/// let diff = angle_diff(-0.9 * PI, 0.9 * PI); -/// assert!((diff - (-0.2 * PI)).abs() < 1e-5); -/// ``` -#[inline] -pub fn angle_diff(from: f32, to: f32) -> f32 { - normalize_angle(to - from) -} - -/// Check if two angles are approximately equal (within tolerance). -/// -/// Handles wrap-around at ±π correctly. -/// -/// # Example -/// ``` -/// use vastu_map::core::math::angles_approx_equal; -/// use std::f32::consts::PI; -/// -/// assert!(angles_approx_equal(PI - 0.001, -PI + 0.001, 0.01)); -/// assert!(!angles_approx_equal(0.0, PI, 0.1)); -/// ``` -#[inline] -pub fn angles_approx_equal(a: f32, b: f32, tolerance: f32) -> bool { - angle_diff(a, b).abs() <= tolerance -} - -// ───────────────────────────────────────────────────────────────────────────── -// Geometry Utilities (shared by line fitting, RANSAC, etc.) -// ───────────────────────────────────────────────────────────────────────────── - -use super::Point2D; - -/// Covariance matrix elements for 2D point sets. -/// -/// Represents a symmetric 2x2 matrix: -/// ```text -/// | cxx cxy | -/// | cxy cyy | -/// ``` -#[derive(Debug, Clone, Copy, Default)] -pub struct Covariance2D { - /// Sum of (x - cx)^2 - pub cxx: f32, - /// Sum of (y - cy)^2 - pub cyy: f32, - /// Sum of (x - cx)(y - cy) - pub cxy: f32, -} - -/// Compute the centroid of a set of points. -/// -/// SIMD-optimized for batches of 4+ points. -/// -/// # Example -/// ``` -/// use vastu_map::core::{Point2D, math::compute_centroid}; -/// -/// let points = [Point2D::new(0.0, 0.0), Point2D::new(2.0, 0.0), Point2D::new(1.0, 1.0)]; -/// let centroid = compute_centroid(&points); -/// assert!((centroid.x - 1.0).abs() < 1e-6); -/// assert!((centroid.y - 1.0 / 3.0).abs() < 1e-6); -/// ``` -#[inline] -pub fn compute_centroid(points: &[Point2D]) -> Point2D { - if points.is_empty() { - return Point2D::new(0.0, 0.0); - } - - let n = points.len(); - let chunks = n / 4; - let mut sum_x: f32 = 0.0; - let mut sum_y: f32 = 0.0; - - // SIMD path for 4+ points - if chunks > 0 { - let mut sum_x4 = f32x4::splat(0.0); - let mut sum_y4 = f32x4::splat(0.0); - - for i in 0..chunks { - let base = i * 4; - - // Load 4 points - let px = f32x4::from_array([ - points[base].x, - points[base + 1].x, - points[base + 2].x, - points[base + 3].x, - ]); - let py = f32x4::from_array([ - points[base].y, - points[base + 1].y, - points[base + 2].y, - points[base + 3].y, - ]); - - sum_x4 += px; - sum_y4 += py; - } - - sum_x = sum_x4.reduce_sum(); - sum_y = sum_y4.reduce_sum(); - } - - // Handle remainder - for p in points.iter().take(n).skip(chunks * 4) { - sum_x += p.x; - sum_y += p.y; - } - - Point2D::new(sum_x / n as f32, sum_y / n as f32) -} - -/// Compute the 2x2 covariance matrix elements for a set of points. -/// -/// The covariance matrix describes the spread and orientation of points -/// around their centroid. Used for Total Least Squares line fitting and PCA. -/// SIMD-optimized for batches of 4+ points. -/// -/// # Arguments -/// * `points` - Point set -/// * `centroid` - Pre-computed centroid (use [`compute_centroid`]) -/// -/// # Example -/// ``` -/// use vastu_map::core::{Point2D, math::{compute_centroid, compute_covariance}}; -/// -/// let points = [ -/// Point2D::new(0.0, 0.0), -/// Point2D::new(1.0, 1.0), -/// Point2D::new(2.0, 2.0), -/// ]; -/// let centroid = compute_centroid(&points); -/// let cov = compute_covariance(&points, centroid); -/// -/// // Points lie on y=x line, so cxx == cyy and cxy > 0 -/// assert!((cov.cxx - cov.cyy).abs() < 1e-6); -/// assert!(cov.cxy > 0.0); -/// ``` -#[inline] -pub fn compute_covariance(points: &[Point2D], centroid: Point2D) -> Covariance2D { - let n = points.len(); - let chunks = n / 4; - let mut cxx: f32 = 0.0; - let mut cyy: f32 = 0.0; - let mut cxy: f32 = 0.0; - - // SIMD path for 4+ points - if chunks > 0 { - let cx4 = f32x4::splat(centroid.x); - let cy4 = f32x4::splat(centroid.y); - let mut cxx4 = f32x4::splat(0.0); - let mut cyy4 = f32x4::splat(0.0); - let mut cxy4 = f32x4::splat(0.0); - - for i in 0..chunks { - let base = i * 4; - - // Load 4 points - let px = f32x4::from_array([ - points[base].x, - points[base + 1].x, - points[base + 2].x, - points[base + 3].x, - ]); - let py = f32x4::from_array([ - points[base].y, - points[base + 1].y, - points[base + 2].y, - points[base + 3].y, - ]); - - // Compute deltas - let dx = px - cx4; - let dy = py - cy4; - - // Accumulate covariance components - cxx4 += dx * dx; - cyy4 += dy * dy; - cxy4 += dx * dy; - } - - cxx = cxx4.reduce_sum(); - cyy = cyy4.reduce_sum(); - cxy = cxy4.reduce_sum(); - } - - // Handle remainder - for p in points.iter().take(n).skip(chunks * 4) { - let dx = p.x - centroid.x; - let dy = p.y - centroid.y; - cxx += dx * dx; - cyy += dy * dy; - cxy += dx * dy; - } - - Covariance2D { cxx, cyy, cxy } -} - -/// Compute the principal direction from a 2x2 covariance matrix. -/// -/// Returns the eigenvector corresponding to the larger eigenvalue, -/// which represents the direction of maximum variance. -/// -/// Returns `None` if the covariance matrix is degenerate (e.g., all points -/// coincident or forming a circle). -/// -/// # Example -/// ``` -/// use vastu_map::core::{Point2D, math::{compute_centroid, compute_covariance, principal_direction}}; -/// -/// // Points on a horizontal line -/// let points = [Point2D::new(0.0, 0.0), Point2D::new(1.0, 0.0), Point2D::new(2.0, 0.0)]; -/// let centroid = compute_centroid(&points); -/// let cov = compute_covariance(&points, centroid); -/// let dir = principal_direction(&cov).unwrap(); -/// -/// // Direction should be horizontal (1, 0) or (-1, 0) -/// assert!(dir.y.abs() < 1e-6); -/// assert!((dir.x.abs() - 1.0).abs() < 1e-6); -/// ``` -#[inline] -pub fn principal_direction(cov: &Covariance2D) -> Option { - // Find eigenvectors of 2x2 covariance matrix - // For 2x2 symmetric matrix, eigenvalues are: - // λ = (cxx + cyy)/2 ± sqrt(((cxx - cyy)/2)² + cxy²) - - let trace_half = (cov.cxx + cov.cyy) / 2.0; - let det_sqrt = ((cov.cxx - cov.cyy) / 2.0).powi(2) + cov.cxy * cov.cxy; - - if det_sqrt < f32::EPSILON { - // Degenerate case - points form a circle or single point - return None; - } - - let det_sqrt = det_sqrt.sqrt(); - let lambda1 = trace_half + det_sqrt; // Larger eigenvalue - - // Eigenvector for larger eigenvalue (line direction) - // For eigenvalue λ1, eigenvector satisfies (cxx - λ1)x + cxy*y = 0 - // Direction: (cxy, λ1 - cxx) or (-cxy, cxx - λ1) - - if cov.cxy.abs() > f32::EPSILON { - Some(Point2D::new(lambda1 - cov.cyy, cov.cxy).normalized()) - } else if cov.cxx > cov.cyy { - Some(Point2D::new(1.0, 0.0)) // Horizontal line - } else { - Some(Point2D::new(0.0, 1.0)) // Vertical line - } -} - -#[cfg(test)] -mod tests { - use super::*; - use approx::assert_relative_eq; - - #[test] - fn test_normalize_angle() { - assert_relative_eq!(normalize_angle(0.0), 0.0, epsilon = 1e-6); - // At ±π boundary, floating-point may give +π or -π; both are valid - assert!(normalize_angle(PI).abs() - PI < 1e-6); - assert!(normalize_angle(-PI).abs() - PI < 1e-6); - assert_relative_eq!(normalize_angle(TWO_PI), 0.0, epsilon = 1e-6); - // For 3π, should normalize to approximately ±π - assert!(normalize_angle(3.0 * PI).abs() - PI < 1e-5); - assert!(normalize_angle(-3.0 * PI).abs() - PI < 1e-5); - assert_relative_eq!(normalize_angle(PI / 2.0), PI / 2.0, epsilon = 1e-6); - assert_relative_eq!(normalize_angle(-PI / 2.0), -PI / 2.0, epsilon = 1e-6); - } - - #[test] - fn test_angle_diff() { - assert_relative_eq!(angle_diff(0.0, PI / 2.0), PI / 2.0, epsilon = 1e-6); - assert_relative_eq!(angle_diff(PI / 2.0, 0.0), -PI / 2.0, epsilon = 1e-6); - assert_relative_eq!(angle_diff(0.0, PI), -PI, epsilon = 1e-6); - assert_relative_eq!(angle_diff(0.0, -PI), -PI, epsilon = 1e-6); - - // Crossing boundary - assert_relative_eq!(angle_diff(-0.9 * PI, 0.9 * PI), -0.2 * PI, epsilon = 1e-5); - assert_relative_eq!(angle_diff(0.9 * PI, -0.9 * PI), 0.2 * PI, epsilon = 1e-5); - } - - #[test] - fn test_angles_approx_equal() { - assert!(angles_approx_equal(0.0, 0.001, 0.01)); - assert!(angles_approx_equal(PI - 0.001, -PI + 0.001, 0.01)); - assert!(!angles_approx_equal(0.0, PI, 0.1)); - } - - // ───────────────────────────────────────────────────────────────────────── - // Geometry Utility Tests - // ───────────────────────────────────────────────────────────────────────── - - #[test] - fn test_compute_centroid() { - // Empty points - let empty: &[Point2D] = &[]; - let c = compute_centroid(empty); - assert_eq!(c.x, 0.0); - assert_eq!(c.y, 0.0); - - // Single point - let single = [Point2D::new(3.0, 4.0)]; - let c = compute_centroid(&single); - assert_relative_eq!(c.x, 3.0, epsilon = 1e-6); - assert_relative_eq!(c.y, 4.0, epsilon = 1e-6); - - // Triangle - let triangle = [ - Point2D::new(0.0, 0.0), - Point2D::new(3.0, 0.0), - Point2D::new(0.0, 3.0), - ]; - let c = compute_centroid(&triangle); - assert_relative_eq!(c.x, 1.0, epsilon = 1e-6); - assert_relative_eq!(c.y, 1.0, epsilon = 1e-6); - } - - #[test] - fn test_compute_covariance() { - // Points on horizontal line (y=0) - let horizontal = [ - Point2D::new(0.0, 0.0), - Point2D::new(1.0, 0.0), - Point2D::new(2.0, 0.0), - ]; - let centroid = compute_centroid(&horizontal); - let cov = compute_covariance(&horizontal, centroid); - - // cxx > 0, cyy = 0, cxy = 0 - assert!(cov.cxx > 0.0); - assert_relative_eq!(cov.cyy, 0.0, epsilon = 1e-6); - assert_relative_eq!(cov.cxy, 0.0, epsilon = 1e-6); - - // Points on diagonal (y=x) - let diagonal = [ - Point2D::new(0.0, 0.0), - Point2D::new(1.0, 1.0), - Point2D::new(2.0, 2.0), - ]; - let centroid = compute_centroid(&diagonal); - let cov = compute_covariance(&diagonal, centroid); - - // cxx == cyy, cxy > 0 - assert_relative_eq!(cov.cxx, cov.cyy, epsilon = 1e-6); - assert!(cov.cxy > 0.0); - } - - #[test] - fn test_principal_direction_horizontal() { - let points = [ - Point2D::new(0.0, 0.0), - Point2D::new(1.0, 0.0), - Point2D::new(2.0, 0.0), - ]; - let centroid = compute_centroid(&points); - let cov = compute_covariance(&points, centroid); - let dir = principal_direction(&cov).unwrap(); - - // Direction should be horizontal - assert!(dir.y.abs() < 1e-6); - assert!((dir.x.abs() - 1.0).abs() < 1e-6); - } - - #[test] - fn test_principal_direction_vertical() { - let points = [ - Point2D::new(0.0, 0.0), - Point2D::new(0.0, 1.0), - Point2D::new(0.0, 2.0), - ]; - let centroid = compute_centroid(&points); - let cov = compute_covariance(&points, centroid); - let dir = principal_direction(&cov).unwrap(); - - // Direction should be vertical - assert!(dir.x.abs() < 1e-6); - assert!((dir.y.abs() - 1.0).abs() < 1e-6); - } - - #[test] - fn test_principal_direction_diagonal() { - let points = [ - Point2D::new(0.0, 0.0), - Point2D::new(1.0, 1.0), - Point2D::new(2.0, 2.0), - ]; - let centroid = compute_centroid(&points); - let cov = compute_covariance(&points, centroid); - let dir = principal_direction(&cov).unwrap(); - - // Direction should be 45° (normalized: ~0.707, ~0.707) - assert_relative_eq!(dir.x.abs(), dir.y.abs(), epsilon = 1e-5); - assert!((dir.length() - 1.0).abs() < 1e-6); - } - - #[test] - fn test_principal_direction_degenerate() { - // Single point - degenerate - let cov = Covariance2D { - cxx: 0.0, - cyy: 0.0, - cxy: 0.0, - }; - assert!(principal_direction(&cov).is_none()); - } -} diff --git a/vastu-map/src/core/mod.rs b/vastu-map/src/core/mod.rs index c89bb92..f8c672d 100644 --- a/vastu-map/src/core/mod.rs +++ b/vastu-map/src/core/mod.rs @@ -1,81 +1,19 @@ -//! Core types for vastu-map. +//! Core types for the vastu-map occupancy grid library. //! -//! This module provides fundamental geometric types for 2D SLAM: -//! -//! - [`Point2D`]: 2D point/vector with full geometric operations -//! - [`Pose2D`]: 2D position and orientation with frame transforms -//! - [`PolarScan`]: Lidar scan in polar coordinates (raw sensor data) -//! - [`PointCloud2D`]: Point cloud with SoA layout for SIMD efficiency -//! - [`Bounds`]: Axis-aligned bounding box for spatial queries -//! -//! # Coordinate Frame -//! -//! All coordinates follow the ROS REP-103 convention: -//! - **X-forward**: Positive X is ahead of the robot -//! - **Y-left**: Positive Y is to the robot's left -//! - **Z-up**: Positive Z is upward (unused in 2D) -//! - **Rotation**: Counter-clockwise positive (right-hand rule) -//! -//! # Usage Examples -//! -//! ## Working with Points -//! -//! ```rust -//! use vastu_map::core::Point2D; -//! -//! let p = Point2D::new(3.0, 4.0); -//! assert_eq!(p.length(), 5.0); // Distance from origin -//! -//! let direction = p.normalized(); // Unit vector -//! let rotated = p.rotate(std::f32::consts::FRAC_PI_2); // Rotate 90° -//! ``` -//! -//! ## Working with Poses -//! -//! ```rust -//! use vastu_map::core::{Pose2D, Point2D}; -//! -//! // Robot at (1, 0) facing left (90° CCW from forward) -//! let pose = Pose2D::new(1.0, 0.0, std::f32::consts::FRAC_PI_2); -//! -//! // Transform a point from robot frame to world frame -//! let local_point = Point2D::new(1.0, 0.0); // 1m ahead in robot frame -//! let world_point = pose.transform_point(local_point); -//! // Result: (1.0, 1.0) - 1m to the left of robot position -//! ``` -//! -//! ## Processing Lidar Scans -//! -//! ```rust -//! use vastu_map::core::{PolarScan, PointCloud2D, Pose2D}; -//! -//! // Raw lidar data -//! let mut scan = PolarScan::new(); -//! scan.push(0.0, 2.0, 100); // 2m ahead, good quality -//! scan.push(1.57, 1.5, 100); // 1.5m to the left -//! -//! // Convert to Cartesian, filtering by quality and range -//! let cloud = scan.to_cartesian(50, 0.1, 10.0); -//! -//! // Transform to world frame -//! let robot_pose = Pose2D::new(0.0, 0.0, 0.0); -//! let world_cloud = cloud.transform(&robot_pose); -//! ``` -//! -//! # Data Layout -//! -//! [`PointCloud2D`] uses Struct-of-Arrays (SoA) layout for SIMD efficiency: -//! - `xs: Vec` and `ys: Vec` stored separately -//! - Enables 4-wide SIMD operations via auto-vectorization -//! - Cache-friendly for sequential access patterns +//! This module provides the fundamental types used throughout the library: +//! - [`Cell`] and [`CellType`]: Grid cell representation with semantic types +//! - [`GridCoord`] and [`WorldPoint`]: Coordinate types +//! - [`Pose2D`]: Robot pose (position + orientation) +//! - Sensor types: [`LidarScan`], [`CliffSensors`], [`BumperSensors`] -pub mod bounds; -pub mod math; -pub mod point; -pub mod pose; -pub mod scan; +mod cell; +mod point; +mod pose; +mod sensors; -pub use bounds::Bounds; -pub use point::Point2D; -pub use pose::Pose2D; -pub use scan::{PointCloud2D, PolarScan}; +pub mod simd; + +pub use cell::{Cell, CellType}; +pub use point::{GridCoord, WorldPoint}; +pub use pose::{Pose2D, interpolate_pose, normalize_angle}; +pub use sensors::{BumperSensors, CliffSensors, LidarScan, SensorObservation}; diff --git a/vastu-map/src/core/point.rs b/vastu-map/src/core/point.rs index 18d3aeb..6e26ab1 100644 --- a/vastu-map/src/core/point.rs +++ b/vastu-map/src/core/point.rs @@ -1,387 +1,237 @@ -//! 2D point type for geometric operations. -//! -//! Coordinate frame follows ROS REP-103: -//! - X-forward, Y-left, Z-up (right-handed) -//! - Counter-clockwise positive rotation +//! Point and coordinate types for the occupancy grid. -use std::ops::{Add, Div, Mul, Neg, Sub}; +use serde::{Deserialize, Serialize}; +use std::ops::{Add, Mul, Sub}; -/// A 2D point/vector in meters. -/// -/// Uses the ROS REP-103 coordinate convention: -/// - X: forward (positive ahead of robot) -/// - Y: left (positive to the left of robot) -/// -/// This type is used for both points and direction vectors. -#[derive(Clone, Copy, Debug, Default, PartialEq)] -pub struct Point2D { - /// X coordinate in meters (forward direction). - pub x: f32, - /// Y coordinate in meters (left direction). - pub y: f32, +/// Grid coordinates (integer cell indices) +#[derive(Clone, Copy, Debug, PartialEq, Eq, Hash, Default, Serialize, Deserialize)] +pub struct GridCoord { + /// X coordinate (column index) + pub x: i32, + /// Y coordinate (row index) + pub y: i32, } -impl Point2D { - /// Create a new point from x and y coordinates. +impl GridCoord { + /// Create a new grid coordinate #[inline] - pub const fn new(x: f32, y: f32) -> Self { + pub fn new(x: i32, y: i32) -> Self { Self { x, y } } - /// Create a zero point (origin). + /// Manhattan distance to another coordinate #[inline] - pub const fn zero() -> Self { - Self { x: 0.0, y: 0.0 } + pub fn manhattan_distance(&self, other: &GridCoord) -> i32 { + (self.x - other.x).abs() + (self.y - other.y).abs() } - /// Create a point from polar coordinates. - /// - /// # Arguments - /// * `angle` - Angle in radians from X axis (CCW positive) - /// * `distance` - Distance from origin in meters + /// Chebyshev distance (max of x and y distance) - used for 8-connected grids #[inline] - pub fn from_polar(angle: f32, distance: f32) -> Self { - Self { - x: distance * angle.cos(), - y: distance * angle.sin(), - } + pub fn chebyshev_distance(&self, other: &GridCoord) -> i32 { + (self.x - other.x).abs().max((self.y - other.y).abs()) } - /// Squared distance from origin (avoids sqrt). + /// Get the 4 cardinal neighbors (N, E, S, W) #[inline] - pub fn length_squared(self) -> f32 { - self.x * self.x + self.y * self.y + pub fn neighbors_4(&self) -> [GridCoord; 4] { + [ + GridCoord::new(self.x, self.y + 1), // North + GridCoord::new(self.x + 1, self.y), // East + GridCoord::new(self.x, self.y - 1), // South + GridCoord::new(self.x - 1, self.y), // West + ] } - /// Distance from origin. + /// Get the 8 neighbors (including diagonals) #[inline] - pub fn length(self) -> f32 { - self.length_squared().sqrt() + pub fn neighbors_8(&self) -> [GridCoord; 8] { + [ + GridCoord::new(self.x, self.y + 1), // N + GridCoord::new(self.x + 1, self.y + 1), // NE + GridCoord::new(self.x + 1, self.y), // E + GridCoord::new(self.x + 1, self.y - 1), // SE + GridCoord::new(self.x, self.y - 1), // S + GridCoord::new(self.x - 1, self.y - 1), // SW + GridCoord::new(self.x - 1, self.y), // W + GridCoord::new(self.x - 1, self.y + 1), // NW + ] + } +} + +impl Add for GridCoord { + type Output = Self; + + #[inline] + fn add(self, other: Self) -> Self { + GridCoord::new(self.x + other.x, self.y + other.y) + } +} + +impl Sub for GridCoord { + type Output = Self; + + #[inline] + fn sub(self, other: Self) -> Self { + GridCoord::new(self.x - other.x, self.y - other.y) + } +} + +/// World coordinates (meters, f32) +#[derive(Clone, Copy, Debug, PartialEq, Default, Serialize, Deserialize)] +pub struct WorldPoint { + /// X coordinate in meters (forward in ROS convention) + pub x: f32, + /// Y coordinate in meters (left in ROS convention) + pub y: f32, +} + +impl WorldPoint { + /// Create a new world point + #[inline] + pub fn new(x: f32, y: f32) -> Self { + Self { x, y } } - /// Squared distance to another point (avoids sqrt). + /// Zero point (origin) + pub const ZERO: WorldPoint = WorldPoint { x: 0.0, y: 0.0 }; + + /// Euclidean distance to another point #[inline] - pub fn distance_squared(self, other: Point2D) -> f32 { - (self - other).length_squared() + pub fn distance(&self, other: &WorldPoint) -> f32 { + let dx = self.x - other.x; + let dy = self.y - other.y; + (dx * dx + dy * dy).sqrt() } - /// Distance to another point. + /// Squared distance (faster, avoids sqrt) #[inline] - pub fn distance(self, other: Point2D) -> f32 { - self.distance_squared(other).sqrt() + pub fn distance_squared(&self, other: &WorldPoint) -> f32 { + let dx = self.x - other.x; + let dy = self.y - other.y; + dx * dx + dy * dy } - /// Normalize to unit length. Returns zero vector if length is zero. + /// Angle from this point to another (radians, CCW from +X) #[inline] - pub fn normalized(self) -> Self { + pub fn angle_to(&self, other: &WorldPoint) -> f32 { + let dx = other.x - self.x; + let dy = other.y - self.y; + dy.atan2(dx) + } + + /// Create a point at a given angle and distance from this point + #[inline] + pub fn point_at(&self, angle: f32, distance: f32) -> WorldPoint { + WorldPoint::new( + self.x + distance * angle.cos(), + self.y + distance * angle.sin(), + ) + } + + /// Rotate this point around the origin by angle (radians) + #[inline] + pub fn rotate(&self, angle: f32) -> WorldPoint { + let cos_a = angle.cos(); + let sin_a = angle.sin(); + WorldPoint::new( + self.x * cos_a - self.y * sin_a, + self.x * sin_a + self.y * cos_a, + ) + } + + /// Length (magnitude) of this point as a vector from origin + #[inline] + pub fn length(&self) -> f32 { + (self.x * self.x + self.y * self.y).sqrt() + } + + /// Normalize to unit length + #[inline] + pub fn normalize(&self) -> WorldPoint { let len = self.length(); - if len > f32::EPSILON { - self / len + if len > 0.0 { + WorldPoint::new(self.x / len, self.y / len) } else { - Self::zero() + *self } } - /// Dot product with another vector. + /// Dot product with another point (as vectors) #[inline] - pub fn dot(self, other: Point2D) -> f32 { + pub fn dot(&self, other: &WorldPoint) -> f32 { self.x * other.x + self.y * other.y } - /// 2D cross product (returns scalar z-component). - /// - /// This is the signed area of the parallelogram formed by the two vectors. - /// Positive if `other` is counter-clockwise from `self`. + /// Cross product (z-component of 3D cross product) #[inline] - pub fn cross(self, other: Point2D) -> f32 { + pub fn cross(&self, other: &WorldPoint) -> f32 { self.x * other.y - self.y * other.x } - - /// Perpendicular vector (90° counter-clockwise rotation). - #[inline] - pub fn perpendicular(self) -> Self { - Self { - x: -self.y, - y: self.x, - } - } - - /// Angle from X axis (in radians, range [-π, π]). - #[inline] - pub fn angle(self) -> f32 { - self.y.atan2(self.x) - } - - /// Rotate point by angle (radians, counter-clockwise positive). - #[inline] - pub fn rotate(self, angle: f32) -> Self { - let (sin, cos) = angle.sin_cos(); - Self { - x: self.x * cos - self.y * sin, - y: self.x * sin + self.y * cos, - } - } - - /// Linear interpolation between this point and another. - #[inline] - pub fn lerp(self, other: Point2D, t: f32) -> Self { - Self { - x: self.x + (other.x - self.x) * t, - y: self.y + (other.y - self.y) * t, - } - } - - /// Component-wise minimum. - #[inline] - pub fn min(self, other: Point2D) -> Self { - Self { - x: self.x.min(other.x), - y: self.y.min(other.y), - } - } - - /// Component-wise maximum. - #[inline] - pub fn max(self, other: Point2D) -> Self { - Self { - x: self.x.max(other.x), - y: self.y.max(other.y), - } - } - - /// Check if this point is approximately equal to another. - #[inline] - pub fn approx_eq(self, other: Point2D, epsilon: f32) -> bool { - (self.x - other.x).abs() <= epsilon && (self.y - other.y).abs() <= epsilon - } } -impl Add for Point2D { +impl Add for WorldPoint { type Output = Self; #[inline] - fn add(self, rhs: Self) -> Self { - Self { - x: self.x + rhs.x, - y: self.y + rhs.y, - } + fn add(self, other: Self) -> Self { + WorldPoint::new(self.x + other.x, self.y + other.y) } } -impl Sub for Point2D { +impl Sub for WorldPoint { type Output = Self; #[inline] - fn sub(self, rhs: Self) -> Self { - Self { - x: self.x - rhs.x, - y: self.y - rhs.y, - } + fn sub(self, other: Self) -> Self { + WorldPoint::new(self.x - other.x, self.y - other.y) } } -impl Mul for Point2D { +impl Mul for WorldPoint { type Output = Self; #[inline] - fn mul(self, rhs: f32) -> Self { - Self { - x: self.x * rhs, - y: self.y * rhs, - } - } -} - -impl Mul for f32 { - type Output = Point2D; - - #[inline] - fn mul(self, rhs: Point2D) -> Point2D { - Point2D { - x: self * rhs.x, - y: self * rhs.y, - } - } -} - -impl Div for Point2D { - type Output = Self; - - #[inline] - fn div(self, rhs: f32) -> Self { - Self { - x: self.x / rhs, - y: self.y / rhs, - } - } -} - -impl Neg for Point2D { - type Output = Self; - - #[inline] - fn neg(self) -> Self { - Self { - x: -self.x, - y: -self.y, - } + fn mul(self, scalar: f32) -> Self { + WorldPoint::new(self.x * scalar, self.y * scalar) } } #[cfg(test)] mod tests { use super::*; - use approx::assert_relative_eq; - use std::f32::consts::PI; #[test] - fn test_new_and_zero() { - let p = Point2D::new(1.0, 2.0); - assert_eq!(p.x, 1.0); - assert_eq!(p.y, 2.0); - - let z = Point2D::zero(); - assert_eq!(z.x, 0.0); - assert_eq!(z.y, 0.0); + fn test_grid_coord_neighbors() { + let c = GridCoord::new(5, 5); + let n4 = c.neighbors_4(); + assert_eq!(n4[0], GridCoord::new(5, 6)); // N + assert_eq!(n4[1], GridCoord::new(6, 5)); // E + assert_eq!(n4[2], GridCoord::new(5, 4)); // S + assert_eq!(n4[3], GridCoord::new(4, 5)); // W } #[test] - fn test_from_polar() { - let p = Point2D::from_polar(0.0, 1.0); - assert_relative_eq!(p.x, 1.0, epsilon = 1e-6); - assert_relative_eq!(p.y, 0.0, epsilon = 1e-6); - - let p = Point2D::from_polar(PI / 2.0, 1.0); - assert_relative_eq!(p.x, 0.0, epsilon = 1e-6); - assert_relative_eq!(p.y, 1.0, epsilon = 1e-6); - - let p = Point2D::from_polar(PI / 4.0, 2.0_f32.sqrt()); - assert_relative_eq!(p.x, 1.0, epsilon = 1e-6); - assert_relative_eq!(p.y, 1.0, epsilon = 1e-6); + fn test_world_point_distance() { + let a = WorldPoint::new(0.0, 0.0); + let b = WorldPoint::new(3.0, 4.0); + assert!((a.distance(&b) - 5.0).abs() < 1e-6); } #[test] - fn test_length() { - let p = Point2D::new(3.0, 4.0); - assert_eq!(p.length_squared(), 25.0); - assert_eq!(p.length(), 5.0); + fn test_world_point_angle() { + let origin = WorldPoint::ZERO; + let east = WorldPoint::new(1.0, 0.0); + let north = WorldPoint::new(0.0, 1.0); + + assert!((origin.angle_to(&east) - 0.0).abs() < 1e-6); + assert!((origin.angle_to(&north) - std::f32::consts::FRAC_PI_2).abs() < 1e-6); } #[test] - fn test_distance() { - let a = Point2D::new(0.0, 0.0); - let b = Point2D::new(3.0, 4.0); - assert_eq!(a.distance_squared(b), 25.0); - assert_eq!(a.distance(b), 5.0); - } - - #[test] - fn test_normalized() { - let p = Point2D::new(3.0, 4.0); - let n = p.normalized(); - assert_relative_eq!(n.length(), 1.0, epsilon = 1e-6); - assert_relative_eq!(n.x, 0.6, epsilon = 1e-6); - assert_relative_eq!(n.y, 0.8, epsilon = 1e-6); - - // Zero vector stays zero - let z = Point2D::zero().normalized(); - assert_eq!(z, Point2D::zero()); - } - - #[test] - fn test_dot() { - let a = Point2D::new(1.0, 0.0); - let b = Point2D::new(0.0, 1.0); - assert_eq!(a.dot(b), 0.0); // Perpendicular - - let c = Point2D::new(1.0, 2.0); - let d = Point2D::new(3.0, 4.0); - assert_eq!(c.dot(d), 11.0); // 1*3 + 2*4 = 11 - } - - #[test] - fn test_cross() { - let a = Point2D::new(1.0, 0.0); - let b = Point2D::new(0.0, 1.0); - assert_eq!(a.cross(b), 1.0); // CCW - assert_eq!(b.cross(a), -1.0); // CW - } - - #[test] - fn test_perpendicular() { - let p = Point2D::new(1.0, 0.0); - let perp = p.perpendicular(); - assert_relative_eq!(perp.x, 0.0, epsilon = 1e-6); - assert_relative_eq!(perp.y, 1.0, epsilon = 1e-6); - } - - #[test] - fn test_angle() { - assert_relative_eq!(Point2D::new(1.0, 0.0).angle(), 0.0, epsilon = 1e-6); - assert_relative_eq!(Point2D::new(0.0, 1.0).angle(), PI / 2.0, epsilon = 1e-6); - assert_relative_eq!(Point2D::new(-1.0, 0.0).angle(), PI, epsilon = 1e-6); - assert_relative_eq!(Point2D::new(0.0, -1.0).angle(), -PI / 2.0, epsilon = 1e-6); - } - - #[test] - fn test_rotate() { - let p = Point2D::new(1.0, 0.0); - - let r90 = p.rotate(PI / 2.0); - assert_relative_eq!(r90.x, 0.0, epsilon = 1e-6); - assert_relative_eq!(r90.y, 1.0, epsilon = 1e-6); - - let r180 = p.rotate(PI); - assert_relative_eq!(r180.x, -1.0, epsilon = 1e-6); - assert_relative_eq!(r180.y, 0.0, epsilon = 1e-6); - } - - #[test] - fn test_lerp() { - let a = Point2D::new(0.0, 0.0); - let b = Point2D::new(10.0, 20.0); - - let mid = a.lerp(b, 0.5); - assert_eq!(mid.x, 5.0); - assert_eq!(mid.y, 10.0); - - assert_eq!(a.lerp(b, 0.0), a); - assert_eq!(a.lerp(b, 1.0), b); - } - - #[test] - fn test_min_max() { - let a = Point2D::new(1.0, 5.0); - let b = Point2D::new(3.0, 2.0); - - let min = a.min(b); - assert_eq!(min.x, 1.0); - assert_eq!(min.y, 2.0); - - let max = a.max(b); - assert_eq!(max.x, 3.0); - assert_eq!(max.y, 5.0); - } - - #[test] - fn test_operators() { - let a = Point2D::new(1.0, 2.0); - let b = Point2D::new(3.0, 4.0); - - assert_eq!(a + b, Point2D::new(4.0, 6.0)); - assert_eq!(b - a, Point2D::new(2.0, 2.0)); - assert_eq!(a * 2.0, Point2D::new(2.0, 4.0)); - assert_eq!(2.0 * a, Point2D::new(2.0, 4.0)); - assert_eq!(a / 2.0, Point2D::new(0.5, 1.0)); - assert_eq!(-a, Point2D::new(-1.0, -2.0)); - } - - #[test] - fn test_approx_eq() { - let a = Point2D::new(1.0, 2.0); - let b = Point2D::new(1.001, 2.001); - - assert!(a.approx_eq(b, 0.01)); - assert!(!a.approx_eq(b, 0.0001)); + fn test_world_point_rotate() { + let p = WorldPoint::new(1.0, 0.0); + let rotated = p.rotate(std::f32::consts::FRAC_PI_2); // 90 degrees + assert!((rotated.x - 0.0).abs() < 1e-6); + assert!((rotated.y - 1.0).abs() < 1e-6); } } diff --git a/vastu-map/src/core/pose.rs b/vastu-map/src/core/pose.rs index f333973..e40e6fe 100644 --- a/vastu-map/src/core/pose.rs +++ b/vastu-map/src/core/pose.rs @@ -1,343 +1,232 @@ -//! 2D pose type for robot position and orientation. -//! -//! Coordinate frame follows ROS REP-103: -//! - X-forward, Y-left, Z-up (right-handed) -//! - Counter-clockwise positive rotation +//! Robot pose representation. -use super::math::normalize_angle; -use super::point::Point2D; +use super::point::WorldPoint; +use serde::{Deserialize, Serialize}; -/// A 2D pose representing position and orientation. +/// Robot pose in world coordinates (x, y, theta) /// -/// Uses the ROS REP-103 coordinate convention: -/// - Position: (x, y) in meters -/// - Theta: heading angle in radians, counter-clockwise from X-axis -/// -/// # Composition -/// -/// Poses can be composed using `*` operator (chain transformations): -/// ``` -/// use vastu_map::core::{Pose2D, Point2D}; -/// -/// let pose_a = Pose2D::new(1.0, 0.0, std::f32::consts::FRAC_PI_2); -/// let pose_b = Pose2D::new(1.0, 0.0, 0.0); -/// let combined = pose_a * pose_b; // Apply pose_b in pose_a's frame -/// ``` -#[derive(Clone, Copy, Debug, Default, PartialEq)] +/// Coordinate system: ROS REP-103 +/// - X: Forward (positive ahead of robot) +/// - Y: Left (positive to robot's left) +/// - Theta: Rotation angle in radians, CCW positive from +X axis +#[derive(Clone, Copy, Debug, Default, Serialize, Deserialize)] pub struct Pose2D { - /// X position in meters. + /// X position in meters pub x: f32, - /// Y position in meters. + /// Y position in meters pub y: f32, - /// Heading angle in radians [-π, π), CCW positive from X-axis. + /// Orientation in radians (CCW positive from +X) pub theta: f32, } impl Pose2D { - /// Create a new pose. - /// - /// # Arguments - /// * `x` - X position in meters - /// * `y` - Y position in meters - /// * `theta` - Heading angle in radians (will be normalized to [-π, π)) + /// Create a new pose #[inline] pub fn new(x: f32, y: f32, theta: f32) -> Self { - Self { - x, - y, - theta: normalize_angle(theta), - } + Self { x, y, theta } } - /// Create an identity pose (origin, facing forward). + /// Create a pose from position only (theta = 0) #[inline] - pub const fn identity() -> Self { + pub fn from_position(x: f32, y: f32) -> Self { + Self { x, y, theta: 0.0 } + } + + /// Create a pose from a WorldPoint (theta = 0) + #[inline] + pub fn from_point(point: WorldPoint) -> Self { Self { - x: 0.0, - y: 0.0, + x: point.x, + y: point.y, theta: 0.0, } } - /// Create a pose from position and angle. + /// Get the position as a WorldPoint #[inline] - pub fn from_position_angle(position: Point2D, theta: f32) -> Self { - Self::new(position.x, position.y, theta) + pub fn position(&self) -> WorldPoint { + WorldPoint::new(self.x, self.y) } - /// Get the position as a Point2D. + /// Get the forward direction as a unit vector #[inline] - pub fn position(self) -> Point2D { - Point2D::new(self.x, self.y) + pub fn forward(&self) -> WorldPoint { + WorldPoint::new(self.theta.cos(), self.theta.sin()) } - /// Get the forward direction (unit vector). + /// Get the left direction as a unit vector #[inline] - pub fn forward(self) -> Point2D { - Point2D::new(self.theta.cos(), self.theta.sin()) + pub fn left(&self) -> WorldPoint { + WorldPoint::new(-self.theta.sin(), self.theta.cos()) } - /// Get the left direction (unit vector, perpendicular to forward). - #[inline] - pub fn left(self) -> Point2D { - Point2D::new(-self.theta.sin(), self.theta.cos()) - } - - /// Transform a point from this pose's local frame to world frame. + /// Transform a point from robot frame to world frame /// - /// # Example - /// ``` - /// use vastu_map::core::{Pose2D, Point2D}; - /// use std::f32::consts::FRAC_PI_2; - /// - /// let pose = Pose2D::new(1.0, 0.0, FRAC_PI_2); // At (1,0), facing left - /// let local = Point2D::new(1.0, 0.0); // 1m forward in local frame - /// let world = pose.transform_point(local); - /// // Should be at (1, 1) in world frame - /// ``` + /// Robot frame: X forward, Y left + /// World frame: absolute coordinates #[inline] - pub fn transform_point(self, point: Point2D) -> Point2D { - let (sin, cos) = self.theta.sin_cos(); - Point2D { - x: self.x + point.x * cos - point.y * sin, - y: self.y + point.x * sin + point.y * cos, - } + pub fn transform_point(&self, robot_point: WorldPoint) -> WorldPoint { + let cos_t = self.theta.cos(); + let sin_t = self.theta.sin(); + + WorldPoint::new( + self.x + robot_point.x * cos_t - robot_point.y * sin_t, + self.y + robot_point.x * sin_t + robot_point.y * cos_t, + ) } - /// Transform a point from world frame to this pose's local frame. + /// Transform a point from world frame to robot frame #[inline] - pub fn inverse_transform_point(self, point: Point2D) -> Point2D { - let (sin, cos) = self.theta.sin_cos(); - let dx = point.x - self.x; - let dy = point.y - self.y; - Point2D { - x: dx * cos + dy * sin, - y: -dx * sin + dy * cos, - } + pub fn inverse_transform_point(&self, world_point: WorldPoint) -> WorldPoint { + let cos_t = self.theta.cos(); + let sin_t = self.theta.sin(); + + let dx = world_point.x - self.x; + let dy = world_point.y - self.y; + + WorldPoint::new(dx * cos_t + dy * sin_t, -dx * sin_t + dy * cos_t) } - /// Compose this pose with another (chain transformations). - /// - /// Returns a new pose representing: apply `other` in `self`'s frame. - /// This is equivalent to matrix multiplication: self * other. + /// Compose two poses: self * other + /// Applies other's transform in self's frame #[inline] - pub fn compose(self, other: Pose2D) -> Self { - let pos = self.transform_point(other.position()); - Self::new(pos.x, pos.y, self.theta + other.theta) + pub fn compose(&self, other: &Pose2D) -> Pose2D { + let transformed = self.transform_point(other.position()); + Pose2D::new( + transformed.x, + transformed.y, + normalize_angle(self.theta + other.theta), + ) } - /// Compute the inverse of this pose. - /// - /// The inverse pose, when composed with the original, yields identity: - /// `pose.compose(pose.inverse()) ≈ Pose2D::identity()` + /// Inverse of this pose #[inline] - pub fn inverse(self) -> Self { - let (sin, cos) = self.theta.sin_cos(); - Self::new( - -self.x * cos - self.y * sin, - self.x * sin - self.y * cos, + pub fn inverse(&self) -> Pose2D { + let cos_t = self.theta.cos(); + let sin_t = self.theta.sin(); + + Pose2D::new( + -self.x * cos_t - self.y * sin_t, + self.x * sin_t - self.y * cos_t, -self.theta, ) } - /// Compute the relative pose from `self` to `other`. - /// - /// Returns the pose that, when composed with `self`, yields `other`: - /// `self.compose(self.relative_to(other)) ≈ other` + /// Distance to another pose (position only) #[inline] - pub fn relative_to(self, other: Pose2D) -> Self { - self.inverse().compose(other) + pub fn distance(&self, other: &Pose2D) -> f32 { + self.position().distance(&other.position()) } - /// Linear interpolation between poses. - /// - /// Note: This interpolates theta linearly, which may not be ideal - /// for large angular differences. For smooth rotation interpolation, - /// consider using spherical interpolation. + /// Angular difference to another pose (in radians, normalized to [-pi, pi]) #[inline] - pub fn lerp(self, other: Pose2D, t: f32) -> Self { - use super::math::angle_diff; - Self::new( - self.x + (other.x - self.x) * t, - self.y + (other.y - self.y) * t, - self.theta + angle_diff(self.theta, other.theta) * t, - ) + pub fn angle_diff(&self, other: &Pose2D) -> f32 { + normalize_angle(other.theta - self.theta) } - /// Check if this pose is approximately equal to another. + /// Normalize the theta to [-pi, pi] #[inline] - pub fn approx_eq(self, other: Pose2D, pos_epsilon: f32, angle_epsilon: f32) -> bool { - use super::math::angles_approx_equal; - (self.x - other.x).abs() <= pos_epsilon - && (self.y - other.y).abs() <= pos_epsilon - && angles_approx_equal(self.theta, other.theta, angle_epsilon) + pub fn normalize(&self) -> Pose2D { + Pose2D::new(self.x, self.y, normalize_angle(self.theta)) } } -impl std::ops::Mul for Pose2D { - type Output = Self; - - /// Compose two poses (same as `compose`). - #[inline] - fn mul(self, rhs: Self) -> Self { - self.compose(rhs) +impl PartialEq for Pose2D { + fn eq(&self, other: &Self) -> bool { + (self.x - other.x).abs() < 1e-6 + && (self.y - other.y).abs() < 1e-6 + && (normalize_angle(self.theta - other.theta)).abs() < 1e-6 } } +/// Normalize an angle to [-pi, pi] +#[inline] +pub fn normalize_angle(angle: f32) -> f32 { + let mut a = angle; + while a > std::f32::consts::PI { + a -= 2.0 * std::f32::consts::PI; + } + while a < -std::f32::consts::PI { + a += 2.0 * std::f32::consts::PI; + } + a +} + +/// Interpolate between two poses +#[inline] +pub fn interpolate_pose(a: &Pose2D, b: &Pose2D, t: f32) -> Pose2D { + let t = t.clamp(0.0, 1.0); + + // Linear interpolation for position + let x = a.x + t * (b.x - a.x); + let y = a.y + t * (b.y - a.y); + + // Angular interpolation (shortest path) + let diff = normalize_angle(b.theta - a.theta); + let theta = normalize_angle(a.theta + t * diff); + + Pose2D::new(x, y, theta) +} + #[cfg(test)] mod tests { use super::*; - use approx::assert_relative_eq; use std::f32::consts::{FRAC_PI_2, PI}; #[test] - fn test_new_normalizes_angle() { - // At ±π boundary, floating-point may give +π or -π; both are valid - let pose = Pose2D::new(0.0, 0.0, 3.0 * PI); - assert!(pose.theta.abs() - PI < 1e-5); - - let pose = Pose2D::new(0.0, 0.0, -3.0 * PI); - assert!(pose.theta.abs() - PI < 1e-5); - } - - #[test] - fn test_identity() { - let pose = Pose2D::identity(); - assert_eq!(pose.x, 0.0); - assert_eq!(pose.y, 0.0); - assert_eq!(pose.theta, 0.0); - } - - #[test] - fn test_position() { - let pose = Pose2D::new(1.0, 2.0, 0.5); - let pos = pose.position(); - assert_eq!(pos.x, 1.0); - assert_eq!(pos.y, 2.0); - } - - #[test] - fn test_forward_and_left() { - let pose = Pose2D::new(0.0, 0.0, 0.0); - let fwd = pose.forward(); - assert_relative_eq!(fwd.x, 1.0, epsilon = 1e-6); - assert_relative_eq!(fwd.y, 0.0, epsilon = 1e-6); - - let left = pose.left(); - assert_relative_eq!(left.x, 0.0, epsilon = 1e-6); - assert_relative_eq!(left.y, 1.0, epsilon = 1e-6); - - // At 90 degrees - let pose90 = Pose2D::new(0.0, 0.0, FRAC_PI_2); - let fwd90 = pose90.forward(); - assert_relative_eq!(fwd90.x, 0.0, epsilon = 1e-6); - assert_relative_eq!(fwd90.y, 1.0, epsilon = 1e-6); - } - - #[test] - fn test_transform_point() { - // At origin, no rotation - let pose = Pose2D::identity(); - let local = Point2D::new(1.0, 0.0); - let world = pose.transform_point(local); - assert_relative_eq!(world.x, 1.0, epsilon = 1e-6); - assert_relative_eq!(world.y, 0.0, epsilon = 1e-6); - - // At (1, 0), rotated 90 degrees + fn test_pose_transform() { + // Robot at (1, 0) facing +Y (90 degrees) let pose = Pose2D::new(1.0, 0.0, FRAC_PI_2); - let local = Point2D::new(1.0, 0.0); - let world = pose.transform_point(local); - assert_relative_eq!(world.x, 1.0, epsilon = 1e-6); - assert_relative_eq!(world.y, 1.0, epsilon = 1e-6); + + // Point 1m ahead in robot frame should be at (1, 1) in world + let robot_point = WorldPoint::new(1.0, 0.0); + let world_point = pose.transform_point(robot_point); + + assert!((world_point.x - 1.0).abs() < 1e-5); + assert!((world_point.y - 1.0).abs() < 1e-5); } #[test] - fn test_inverse_transform_point() { + fn test_pose_inverse_transform() { let pose = Pose2D::new(1.0, 2.0, FRAC_PI_2); - let world = Point2D::new(3.0, 4.0); + let world_point = WorldPoint::new(1.0, 3.0); - let local = pose.inverse_transform_point(world); - let back = pose.transform_point(local); + let robot_point = pose.inverse_transform_point(world_point); + let back_to_world = pose.transform_point(robot_point); - assert_relative_eq!(back.x, world.x, epsilon = 1e-6); - assert_relative_eq!(back.y, world.y, epsilon = 1e-6); + assert!((back_to_world.x - world_point.x).abs() < 1e-5); + assert!((back_to_world.y - world_point.y).abs() < 1e-5); } #[test] - fn test_compose() { - // Translate then rotate - let translate = Pose2D::new(1.0, 0.0, 0.0); - let rotate = Pose2D::new(0.0, 0.0, FRAC_PI_2); - let combined = translate.compose(rotate); - - assert_relative_eq!(combined.x, 1.0, epsilon = 1e-6); - assert_relative_eq!(combined.y, 0.0, epsilon = 1e-6); - assert_relative_eq!(combined.theta, FRAC_PI_2, epsilon = 1e-6); - - // Rotate then translate (in rotated frame) - let combined2 = rotate.compose(translate); - assert_relative_eq!(combined2.x, 0.0, epsilon = 1e-6); - assert_relative_eq!(combined2.y, 1.0, epsilon = 1e-6); - assert_relative_eq!(combined2.theta, FRAC_PI_2, epsilon = 1e-6); + fn test_normalize_angle() { + assert!((normalize_angle(0.0) - 0.0).abs() < 1e-6); + assert!((normalize_angle(2.0 * PI) - 0.0).abs() < 1e-6); + assert!((normalize_angle(-2.0 * PI) - 0.0).abs() < 1e-6); + assert!((normalize_angle(3.0 * PI) - PI).abs() < 1e-5); } #[test] - fn test_inverse() { - let pose = Pose2D::new(1.0, 2.0, 0.5); - let inv = pose.inverse(); - let identity = pose.compose(inv); - - assert_relative_eq!(identity.x, 0.0, epsilon = 1e-5); - assert_relative_eq!(identity.y, 0.0, epsilon = 1e-5); - assert_relative_eq!(identity.theta, 0.0, epsilon = 1e-5); - } - - #[test] - fn test_relative_to() { - let pose_a = Pose2D::new(1.0, 0.0, 0.0); - let pose_b = Pose2D::new(2.0, 1.0, FRAC_PI_2); - - let relative = pose_a.relative_to(pose_b); - let reconstructed = pose_a.compose(relative); - - assert_relative_eq!(reconstructed.x, pose_b.x, epsilon = 1e-5); - assert_relative_eq!(reconstructed.y, pose_b.y, epsilon = 1e-5); - assert_relative_eq!(reconstructed.theta, pose_b.theta, epsilon = 1e-5); - } - - #[test] - fn test_lerp() { - // Use non-boundary angle to avoid π ambiguity - let a = Pose2D::new(0.0, 0.0, 0.0); - let b = Pose2D::new(2.0, 4.0, FRAC_PI_2); - - let mid = a.lerp(b, 0.5); - assert_relative_eq!(mid.x, 1.0, epsilon = 1e-6); - assert_relative_eq!(mid.y, 2.0, epsilon = 1e-6); - assert_relative_eq!(mid.theta, FRAC_PI_2 / 2.0, epsilon = 1e-6); // π/4 - - // Test full interpolation - assert_relative_eq!(a.lerp(b, 0.0).theta, 0.0, epsilon = 1e-6); - assert_relative_eq!(a.lerp(b, 1.0).theta, FRAC_PI_2, epsilon = 1e-6); - } - - #[test] - fn test_mul_operator() { + fn test_pose_compose() { let a = Pose2D::new(1.0, 0.0, FRAC_PI_2); let b = Pose2D::new(1.0, 0.0, 0.0); - let composed = a.compose(b); - let multiplied = a * b; + let c = a.compose(&b); - assert_eq!(composed, multiplied); + // Moving 1m forward from (1,0) facing +Y should put us at (1,1) + assert!((c.x - 1.0).abs() < 1e-5); + assert!((c.y - 1.0).abs() < 1e-5); + assert!((c.theta - FRAC_PI_2).abs() < 1e-5); } #[test] - fn test_approx_eq() { - let a = Pose2D::new(1.0, 2.0, 0.5); - let b = Pose2D::new(1.001, 2.001, 0.501); + fn test_interpolate_pose() { + let a = Pose2D::new(0.0, 0.0, 0.0); + let b = Pose2D::new(2.0, 2.0, PI); - assert!(a.approx_eq(b, 0.01, 0.01)); - assert!(!a.approx_eq(b, 0.0001, 0.0001)); + let mid = interpolate_pose(&a, &b, 0.5); + assert!((mid.x - 1.0).abs() < 1e-5); + assert!((mid.y - 1.0).abs() < 1e-5); + assert!((mid.theta - FRAC_PI_2).abs() < 1e-5); } } diff --git a/vastu-map/src/core/scan.rs b/vastu-map/src/core/scan.rs deleted file mode 100644 index 95e2bc2..0000000 --- a/vastu-map/src/core/scan.rs +++ /dev/null @@ -1,652 +0,0 @@ -//! Scan types for lidar data representation. -//! -//! This module provides: -//! - `PolarScan`: Raw lidar scan in polar coordinates -//! - `PointCloud2D`: Cartesian point cloud with SoA (Struct of Arrays) layout -//! -//! The SoA layout is optimized for SIMD auto-vectorization. - -use std::simd::{f32x4, num::SimdFloat}; - -use super::point::Point2D; -use super::pose::Pose2D; - -/// Raw lidar scan in polar coordinates. -/// -/// Points are ordered by angle (sequential around scan). -/// Coordinate frame follows ROS REP-103: -/// - Angle 0 is forward (X-axis) -/// - Positive angles are counter-clockwise (toward Y-axis) -#[derive(Clone, Debug, Default)] -pub struct PolarScan { - /// Points as (angle_rad, distance_m, quality). - /// - angle_rad: Angle from X-axis in radians, CCW positive - /// - distance_m: Range in meters - /// - quality: Measurement quality (0-255, higher is better) - pub points: Vec<(f32, f32, u8)>, -} - -impl PolarScan { - /// Create a new empty polar scan. - pub fn new() -> Self { - Self { points: Vec::new() } - } - - /// Create a polar scan with capacity. - pub fn with_capacity(capacity: usize) -> Self { - Self { - points: Vec::with_capacity(capacity), - } - } - - /// Add a point to the scan. - #[inline] - pub fn push(&mut self, angle: f32, distance: f32, quality: u8) { - self.points.push((angle, distance, quality)); - } - - /// Number of points in the scan. - #[inline] - pub fn len(&self) -> usize { - self.points.len() - } - - /// Check if the scan is empty. - #[inline] - pub fn is_empty(&self) -> bool { - self.points.is_empty() - } - - /// Convert to cartesian PointCloud2D. - /// - /// Filters by quality threshold and valid range. - /// - /// # Arguments - /// * `min_quality` - Minimum quality value to include - /// * `min_range` - Minimum valid range in meters - /// * `max_range` - Maximum valid range in meters - pub fn to_cartesian(&self, min_quality: u8, min_range: f32, max_range: f32) -> PointCloud2D { - let mut cloud = PointCloud2D::with_capacity(self.points.len()); - - for &(angle, dist, quality) in &self.points { - // Use 1.5% tolerance to include max-range readings (no wall hit) - // These are needed for CBVG to create nodes in open-space directions - if quality >= min_quality && dist >= min_range && dist <= max_range * 1.015 { - let (sin, cos) = angle.sin_cos(); - cloud.xs.push(dist * cos); - cloud.ys.push(dist * sin); - } - } - - cloud - } - - /// Convert to cartesian coordinates into a pre-existing PointCloud2D (zero-allocation). - /// - /// Clears the target cloud and fills it with converted points. - /// Filters by quality threshold and valid range. - /// - /// # Arguments - /// * `min_quality` - Minimum quality value to include - /// * `min_range` - Minimum valid range in meters - /// * `max_range` - Maximum valid range in meters - /// * `target` - Pre-allocated point cloud to fill (cleared first) - /// - /// # Example - /// ```rust,ignore - /// let mut cloud = PointCloud2D::with_capacity(360); - /// - /// for scan in scans { - /// // Reuse cloud buffer to avoid allocations - /// scan.to_cartesian_into(50, 0.1, 12.0, &mut cloud); - /// process(&cloud); - /// } - /// ``` - pub fn to_cartesian_into( - &self, - min_quality: u8, - min_range: f32, - max_range: f32, - target: &mut PointCloud2D, - ) { - target.clear(); - target.xs.reserve(self.points.len()); - target.ys.reserve(self.points.len()); - - for &(angle, dist, quality) in &self.points { - // Use 1.5% tolerance to include max-range readings (no wall hit) - // These are needed for CBVG to create nodes in open-space directions - if quality >= min_quality && dist >= min_range && dist <= max_range * 1.015 { - let (sin, cos) = angle.sin_cos(); - target.xs.push(dist * cos); - target.ys.push(dist * sin); - } - } - } -} - -/// Cartesian point cloud with SoA (Struct of Arrays) layout. -/// -/// This layout enables efficient SIMD operations: -/// - Transform operations can process 4 points per SIMD instruction -/// - Memory access patterns are cache-friendly for sequential processing -/// -/// Coordinate frame follows ROS REP-103: -/// - X-forward, Y-left -#[derive(Clone, Debug, Default)] -pub struct PointCloud2D { - /// X coordinates in meters (forward direction). - pub xs: Vec, - /// Y coordinates in meters (left direction). - pub ys: Vec, -} - -impl PointCloud2D { - /// Create a new empty point cloud. - pub fn new() -> Self { - Self { - xs: Vec::new(), - ys: Vec::new(), - } - } - - /// Create a point cloud with capacity. - pub fn with_capacity(capacity: usize) -> Self { - Self { - xs: Vec::with_capacity(capacity), - ys: Vec::with_capacity(capacity), - } - } - - /// Create from a slice of Point2D. - pub fn from_points(points: &[Point2D]) -> Self { - let mut cloud = Self::with_capacity(points.len()); - for p in points { - cloud.xs.push(p.x); - cloud.ys.push(p.y); - } - cloud - } - - /// Add a point to the cloud. - #[inline] - pub fn push(&mut self, x: f32, y: f32) { - self.xs.push(x); - self.ys.push(y); - } - - /// Add a Point2D to the cloud. - #[inline] - pub fn push_point(&mut self, point: Point2D) { - self.xs.push(point.x); - self.ys.push(point.y); - } - - /// Number of points in the cloud. - #[inline] - pub fn len(&self) -> usize { - self.xs.len() - } - - /// Check if the cloud is empty. - #[inline] - pub fn is_empty(&self) -> bool { - self.xs.is_empty() - } - - /// Get a point by index. - #[inline] - pub fn get(&self, index: usize) -> Option { - if index < self.len() { - Some(Point2D::new(self.xs[index], self.ys[index])) - } else { - None - } - } - - /// Get a point by index without bounds checking. - /// - /// This is useful in performance-critical loops where the caller - /// has already verified bounds (e.g., iterating with a known length). - /// - /// # Safety - /// - /// The caller must ensure that `index < self.len()`. Violating this - /// invariant causes undefined behavior: - /// - In debug builds: May panic (depending on Vec implementation) - /// - In release builds: Reads from invalid memory locations - /// - /// # Example - /// - /// ```rust,ignore - /// let cloud = PointCloud2D::from_points(&[Point2D::new(1.0, 2.0)]); - /// let len = cloud.len(); - /// for i in 0..len { - /// // SAFETY: i is always < len, which equals cloud.len() - /// let point = unsafe { cloud.get_unchecked(i) }; - /// } - /// ``` - #[inline] - pub unsafe fn get_unchecked(&self, index: usize) -> Point2D { - // SAFETY: Caller guarantees index < self.len(), so both - // self.xs[index] and self.ys[index] are valid. - unsafe { Point2D::new(*self.xs.get_unchecked(index), *self.ys.get_unchecked(index)) } - } - - /// Transform the point cloud by a pose. - /// - /// This is SIMD-optimized: processes 4 points per iteration using f32x4. - /// - /// # Performance - /// Uses explicit SIMD operations for efficient vectorization. - pub fn transform(&self, pose: &Pose2D) -> Self { - let n = self.len(); - let mut result = Self::with_capacity(n); - result.xs.resize(n, 0.0); - result.ys.resize(n, 0.0); - - let (sin, cos) = pose.theta.sin_cos(); - let cos4 = f32x4::splat(cos); - let sin4 = f32x4::splat(sin); - let tx4 = f32x4::splat(pose.x); - let ty4 = f32x4::splat(pose.y); - - // Process 4 points at a time (SIMD-friendly) - let chunks = n / 4; - for i in 0..chunks { - let base = i * 4; - - // Load 4 points - contiguous SIMD loads - let xs = f32x4::from_slice(&self.xs[base..]); - let ys = f32x4::from_slice(&self.ys[base..]); - - // Rotation + translation: - // x' = x*cos - y*sin + tx - // y' = x*sin + y*cos + ty - let new_xs = xs * cos4 - ys * sin4 + tx4; - let new_ys = xs * sin4 + ys * cos4 + ty4; - - // Store results - result.xs[base..base + 4].copy_from_slice(&new_xs.to_array()); - result.ys[base..base + 4].copy_from_slice(&new_ys.to_array()); - } - - // Handle remainder (scalar) - for i in (chunks * 4)..n { - let x = self.xs[i]; - let y = self.ys[i]; - result.xs[i] = x * cos - y * sin + pose.x; - result.ys[i] = x * sin + y * cos + pose.y; - } - - result - } - - /// Compute bounding box of the point cloud. - /// - /// Returns (min, max) corners, or None if cloud is empty. - /// SIMD-optimized using f32x4 min/max operations. - pub fn bounds(&self) -> Option<(Point2D, Point2D)> { - if self.is_empty() { - return None; - } - - let n = self.len(); - - // Initialize with first point - let mut min_x = self.xs[0]; - let mut max_x = self.xs[0]; - let mut min_y = self.ys[0]; - let mut max_y = self.ys[0]; - - // SIMD processing of remaining points - let chunks = (n - 1) / 4; - if chunks > 0 { - let mut min_x4 = f32x4::splat(min_x); - let mut max_x4 = f32x4::splat(max_x); - let mut min_y4 = f32x4::splat(min_y); - let mut max_y4 = f32x4::splat(max_y); - - for i in 0..chunks { - let base = 1 + i * 4; - - let xs = f32x4::from_slice(&self.xs[base..]); - let ys = f32x4::from_slice(&self.ys[base..]); - - min_x4 = min_x4.simd_min(xs); - max_x4 = max_x4.simd_max(xs); - min_y4 = min_y4.simd_min(ys); - max_y4 = max_y4.simd_max(ys); - } - - // Horizontal reduction - min_x = min_x4.reduce_min(); - max_x = max_x4.reduce_max(); - min_y = min_y4.reduce_min(); - max_y = max_y4.reduce_max(); - } - - // Handle remainder (scalar) - let remainder_start = 1 + chunks * 4; - for i in remainder_start..n { - min_x = min_x.min(self.xs[i]); - max_x = max_x.max(self.xs[i]); - min_y = min_y.min(self.ys[i]); - max_y = max_y.max(self.ys[i]); - } - - Some((Point2D::new(min_x, min_y), Point2D::new(max_x, max_y))) - } - - /// Compute centroid (mean position) of the point cloud. - /// - /// Returns None if cloud is empty. - pub fn centroid(&self) -> Option { - if self.is_empty() { - return None; - } - - let n = self.len(); - let mut sum_x: f32 = 0.0; - let mut sum_y: f32 = 0.0; - - // SIMD accumulation - let chunks = n / 4; - if chunks > 0 { - let mut sum_x4 = f32x4::splat(0.0); - let mut sum_y4 = f32x4::splat(0.0); - - for i in 0..chunks { - let base = i * 4; - let xs = f32x4::from_slice(&self.xs[base..]); - let ys = f32x4::from_slice(&self.ys[base..]); - - sum_x4 += xs; - sum_y4 += ys; - } - - sum_x = sum_x4.reduce_sum(); - sum_y = sum_y4.reduce_sum(); - } - - // Handle remainder - for i in (chunks * 4)..n { - sum_x += self.xs[i]; - sum_y += self.ys[i]; - } - - Some(Point2D::new(sum_x / n as f32, sum_y / n as f32)) - } - - /// Convert to `Vec` (AoS layout). - pub fn to_points(&self) -> Vec { - self.xs - .iter() - .zip(&self.ys) - .map(|(&x, &y)| Point2D::new(x, y)) - .collect() - } - - /// Iterate over points. - pub fn iter(&self) -> impl Iterator + '_ { - self.xs - .iter() - .zip(&self.ys) - .map(|(&x, &y)| Point2D::new(x, y)) - } - - /// Clear all points. - pub fn clear(&mut self) { - self.xs.clear(); - self.ys.clear(); - } -} - -#[cfg(test)] -mod tests { - use super::*; - use approx::assert_relative_eq; - use std::f32::consts::{FRAC_PI_2, PI}; - - #[test] - fn test_polar_scan_to_cartesian() { - let mut scan = PolarScan::new(); - scan.push(0.0, 1.0, 100); // Forward, 1m - scan.push(FRAC_PI_2, 2.0, 100); // Left, 2m - scan.push(PI, 1.5, 50); // Behind, 1.5m, low quality - - let cloud = scan.to_cartesian(60, 0.0, 10.0); - - assert_eq!(cloud.len(), 2); // Third point filtered by quality - - // First point: (1, 0) - assert_relative_eq!(cloud.xs[0], 1.0, epsilon = 1e-6); - assert_relative_eq!(cloud.ys[0], 0.0, epsilon = 1e-6); - - // Second point: (0, 2) - assert_relative_eq!(cloud.xs[1], 0.0, epsilon = 1e-6); - assert_relative_eq!(cloud.ys[1], 2.0, epsilon = 1e-6); - } - - #[test] - fn test_polar_scan_range_filter() { - let mut scan = PolarScan::new(); - scan.push(0.0, 0.05, 100); // Too close - scan.push(0.0, 0.5, 100); // Good - scan.push(0.0, 15.0, 100); // Too far - - let cloud = scan.to_cartesian(0, 0.1, 10.0); - assert_eq!(cloud.len(), 1); - assert_relative_eq!(cloud.xs[0], 0.5, epsilon = 1e-6); - } - - #[test] - fn test_polar_scan_to_cartesian_into() { - let mut scan = PolarScan::new(); - scan.push(0.0, 1.0, 100); // Forward, 1m - scan.push(FRAC_PI_2, 2.0, 100); // Left, 2m - - // First conversion - let mut cloud = PointCloud2D::new(); - scan.to_cartesian_into(50, 0.0, 10.0, &mut cloud); - - assert_eq!(cloud.len(), 2); - assert_relative_eq!(cloud.xs[0], 1.0, epsilon = 1e-6); - assert_relative_eq!(cloud.ys[0], 0.0, epsilon = 1e-6); - - // Reuse buffer - should be cleared and refilled - scan.push(PI, 3.0, 100); // Add another point - scan.to_cartesian_into(50, 0.0, 10.0, &mut cloud); - - assert_eq!(cloud.len(), 3); - // Buffer reused - no allocation - - // Verify equivalence to allocating version - let cloud_alloc = scan.to_cartesian(50, 0.0, 10.0); - assert_eq!(cloud.len(), cloud_alloc.len()); - for i in 0..cloud.len() { - assert_relative_eq!(cloud.xs[i], cloud_alloc.xs[i], epsilon = 1e-6); - assert_relative_eq!(cloud.ys[i], cloud_alloc.ys[i], epsilon = 1e-6); - } - } - - #[test] - fn test_point_cloud_from_points() { - let points = vec![ - Point2D::new(1.0, 2.0), - Point2D::new(3.0, 4.0), - Point2D::new(5.0, 6.0), - ]; - - let cloud = PointCloud2D::from_points(&points); - - assert_eq!(cloud.len(), 3); - assert_eq!(cloud.xs, vec![1.0, 3.0, 5.0]); - assert_eq!(cloud.ys, vec![2.0, 4.0, 6.0]); - } - - #[test] - fn test_point_cloud_transform_identity() { - let mut cloud = PointCloud2D::new(); - cloud.push(1.0, 2.0); - cloud.push(3.0, 4.0); - - let pose = Pose2D::identity(); - let transformed = cloud.transform(&pose); - - assert_eq!(transformed.len(), 2); - assert_relative_eq!(transformed.xs[0], 1.0, epsilon = 1e-6); - assert_relative_eq!(transformed.ys[0], 2.0, epsilon = 1e-6); - } - - #[test] - fn test_point_cloud_transform_translation() { - let mut cloud = PointCloud2D::new(); - cloud.push(1.0, 2.0); - - let pose = Pose2D::new(10.0, 20.0, 0.0); - let transformed = cloud.transform(&pose); - - assert_relative_eq!(transformed.xs[0], 11.0, epsilon = 1e-6); - assert_relative_eq!(transformed.ys[0], 22.0, epsilon = 1e-6); - } - - #[test] - fn test_point_cloud_transform_rotation() { - let mut cloud = PointCloud2D::new(); - cloud.push(1.0, 0.0); - - let pose = Pose2D::new(0.0, 0.0, FRAC_PI_2); - let transformed = cloud.transform(&pose); - - assert_relative_eq!(transformed.xs[0], 0.0, epsilon = 1e-6); - assert_relative_eq!(transformed.ys[0], 1.0, epsilon = 1e-6); - } - - #[test] - fn test_point_cloud_transform_combined() { - let mut cloud = PointCloud2D::new(); - cloud.push(1.0, 0.0); - - // Rotate 90° then translate by (1, 0) - let pose = Pose2D::new(1.0, 0.0, FRAC_PI_2); - let transformed = cloud.transform(&pose); - - // Point rotates to (0, 1), then translates to (1, 1) - assert_relative_eq!(transformed.xs[0], 1.0, epsilon = 1e-6); - assert_relative_eq!(transformed.ys[0], 1.0, epsilon = 1e-6); - } - - #[test] - fn test_point_cloud_transform_simd() { - // Test with more than 4 points to exercise SIMD path - let mut cloud = PointCloud2D::new(); - for i in 0..10 { - cloud.push(i as f32, 0.0); - } - - let pose = Pose2D::new(0.0, 0.0, FRAC_PI_2); - let transformed = cloud.transform(&pose); - - for i in 0..10 { - assert_relative_eq!(transformed.xs[i], 0.0, epsilon = 1e-5); - assert_relative_eq!(transformed.ys[i], i as f32, epsilon = 1e-5); - } - } - - #[test] - fn test_point_cloud_bounds() { - let mut cloud = PointCloud2D::new(); - cloud.push(1.0, 2.0); - cloud.push(-3.0, 4.0); - cloud.push(5.0, -6.0); - cloud.push(0.0, 0.0); - - let (min, max) = cloud.bounds().unwrap(); - - assert_eq!(min.x, -3.0); - assert_eq!(min.y, -6.0); - assert_eq!(max.x, 5.0); - assert_eq!(max.y, 4.0); - } - - #[test] - fn test_point_cloud_bounds_simd() { - // Test with more than 4 points - let mut cloud = PointCloud2D::new(); - for i in 0..10 { - cloud.push(i as f32 - 5.0, i as f32 - 5.0); - } - - let (min, max) = cloud.bounds().unwrap(); - - assert_eq!(min.x, -5.0); - assert_eq!(min.y, -5.0); - assert_eq!(max.x, 4.0); - assert_eq!(max.y, 4.0); - } - - #[test] - fn test_point_cloud_bounds_empty() { - let cloud = PointCloud2D::new(); - assert!(cloud.bounds().is_none()); - } - - #[test] - fn test_point_cloud_centroid() { - let mut cloud = PointCloud2D::new(); - cloud.push(0.0, 0.0); - cloud.push(2.0, 0.0); - cloud.push(2.0, 2.0); - cloud.push(0.0, 2.0); - - let centroid = cloud.centroid().unwrap(); - - assert_relative_eq!(centroid.x, 1.0, epsilon = 1e-6); - assert_relative_eq!(centroid.y, 1.0, epsilon = 1e-6); - } - - #[test] - fn test_point_cloud_centroid_simd() { - // Test with more than 4 points - let mut cloud = PointCloud2D::new(); - for _ in 0..8 { - cloud.push(4.0, 6.0); - } - - let centroid = cloud.centroid().unwrap(); - - assert_relative_eq!(centroid.x, 4.0, epsilon = 1e-6); - assert_relative_eq!(centroid.y, 6.0, epsilon = 1e-6); - } - - #[test] - fn test_point_cloud_centroid_empty() { - let cloud = PointCloud2D::new(); - assert!(cloud.centroid().is_none()); - } - - #[test] - fn test_point_cloud_iter() { - let mut cloud = PointCloud2D::new(); - cloud.push(1.0, 2.0); - cloud.push(3.0, 4.0); - - let points: Vec<_> = cloud.iter().collect(); - - assert_eq!(points.len(), 2); - assert_eq!(points[0], Point2D::new(1.0, 2.0)); - assert_eq!(points[1], Point2D::new(3.0, 4.0)); - } - - #[test] - fn test_point_cloud_get() { - let mut cloud = PointCloud2D::new(); - cloud.push(1.0, 2.0); - cloud.push(3.0, 4.0); - - assert_eq!(cloud.get(0), Some(Point2D::new(1.0, 2.0))); - assert_eq!(cloud.get(1), Some(Point2D::new(3.0, 4.0))); - assert_eq!(cloud.get(2), None); - } -} diff --git a/vastu-map/src/core/sensors.rs b/vastu-map/src/core/sensors.rs new file mode 100644 index 0000000..c1ba42b --- /dev/null +++ b/vastu-map/src/core/sensors.rs @@ -0,0 +1,368 @@ +//! Sensor input types for the occupancy grid. + +use super::point::WorldPoint; +use super::pose::Pose2D; +use serde::{Deserialize, Serialize}; + +/// Lidar scan data in polar coordinates +#[derive(Clone, Debug, Default)] +pub struct LidarScan { + /// Range measurements in meters (f32::INFINITY or > max_range = no return) + pub ranges: Vec, + /// Angle for each range in radians (0 = forward, CCW positive) + pub angles: Vec, + /// Minimum valid range (ignore closer measurements) + pub range_min: f32, + /// Maximum valid range (treat further as "no hit") + pub range_max: f32, +} + +impl LidarScan { + /// Create a new lidar scan + pub fn new(ranges: Vec, angles: Vec, range_min: f32, range_max: f32) -> Self { + assert_eq!( + ranges.len(), + angles.len(), + "ranges and angles must have same length" + ); + Self { + ranges, + angles, + range_min, + range_max, + } + } + + /// Create from SangamIO PointCloud2D message format + /// Input: Vec of (angle_rad, distance_m, quality) + pub fn from_sangamio(points: &[(f32, f32, u8)]) -> Self { + let mut ranges = Vec::with_capacity(points.len()); + let mut angles = Vec::with_capacity(points.len()); + + for (angle, distance, _quality) in points { + angles.push(*angle); + ranges.push(*distance); + } + + Self { + ranges, + angles, + range_min: 0.15, // Delta-2D lidar spec + range_max: 8.0, // Delta-2D lidar spec + } + } + + /// Number of rays in the scan + #[inline] + pub fn len(&self) -> usize { + self.ranges.len() + } + + /// Is the scan empty? + #[inline] + pub fn is_empty(&self) -> bool { + self.ranges.is_empty() + } + + /// Check if a range measurement is valid + #[inline] + pub fn is_valid_range(&self, range: f32) -> bool { + range >= self.range_min && range <= self.range_max && range.is_finite() + } + + /// Get valid points as (angle, range) pairs + pub fn valid_points(&self) -> impl Iterator + '_ { + self.angles + .iter() + .zip(self.ranges.iter()) + .filter(|&(_, r)| self.is_valid_range(*r)) + .map(|(&a, &r)| (a, r)) + } + + /// Convert to cartesian points in sensor frame + pub fn to_cartesian(&self) -> Vec { + self.valid_points() + .map(|(angle, range)| WorldPoint::new(range * angle.cos(), range * angle.sin())) + .collect() + } + + /// Convert to cartesian points in world frame given sensor pose + pub fn to_world_points(&self, sensor_pose: Pose2D) -> Vec { + self.valid_points() + .map(|(angle, range)| { + let local = WorldPoint::new(range * angle.cos(), range * angle.sin()); + sensor_pose.transform_point(local) + }) + .collect() + } +} + +/// Cliff sensor readings +#[derive(Clone, Copy, Debug, Default, PartialEq, Eq, Serialize, Deserialize)] +pub struct CliffSensors { + /// Left side cliff sensor triggered + pub left_side: bool, + /// Left front cliff sensor triggered + pub left_front: bool, + /// Right front cliff sensor triggered + pub right_front: bool, + /// Right side cliff sensor triggered + pub right_side: bool, +} + +impl CliffSensors { + /// All sensors safe (no cliffs) + pub const SAFE: CliffSensors = CliffSensors { + left_side: false, + left_front: false, + right_front: false, + right_side: false, + }; + + /// Any cliff sensor triggered? + #[inline] + pub fn any_triggered(&self) -> bool { + self.left_side || self.left_front || self.right_front || self.right_side + } + + /// Count of triggered sensors + #[inline] + pub fn count_triggered(&self) -> u8 { + self.left_side as u8 + + self.left_front as u8 + + self.right_front as u8 + + self.right_side as u8 + } + + /// Get positions of triggered sensors in robot frame + /// Positions based on CRL-200S robot configuration from SangamIO + pub fn triggered_positions(&self) -> Vec { + let mut positions = Vec::with_capacity(4); + + // Sensor positions in robot frame (from SangamIO config) + if self.left_side { + positions.push(WorldPoint::new(0.12, 0.10)); + } + if self.left_front { + positions.push(WorldPoint::new(0.15, 0.05)); + } + if self.right_front { + positions.push(WorldPoint::new(0.15, -0.05)); + } + if self.right_side { + positions.push(WorldPoint::new(0.12, -0.10)); + } + + positions + } + + /// Get positions of triggered sensors in world frame + pub fn triggered_world_positions(&self, robot_pose: Pose2D) -> Vec { + self.triggered_positions() + .into_iter() + .map(|p| robot_pose.transform_point(p)) + .collect() + } +} + +/// Bumper sensor readings +#[derive(Clone, Copy, Debug, Default, PartialEq, Eq, Serialize, Deserialize)] +pub struct BumperSensors { + /// Left bumper triggered + pub left: bool, + /// Right bumper triggered + pub right: bool, +} + +impl BumperSensors { + /// No bumpers triggered + pub const CLEAR: BumperSensors = BumperSensors { + left: false, + right: false, + }; + + /// Any bumper triggered? + #[inline] + pub fn any_triggered(&self) -> bool { + self.left || self.right + } + + /// Both bumpers triggered? + #[inline] + pub fn both_triggered(&self) -> bool { + self.left && self.right + } + + /// Estimate collision angle in robot frame (radians) + /// Returns None if no collision + /// - 0.0 = center collision + /// - positive = left side + /// - negative = right side + pub fn collision_angle(&self) -> Option { + match (self.left, self.right) { + (true, true) => Some(0.0), // Center collision + (true, false) => Some(0.5), // Left collision (~30°) + (false, true) => Some(-0.5), // Right collision (~-30°) + (false, false) => None, // No collision + } + } + + /// Estimate collision point in robot frame + /// Uses robot radius to compute approximate impact location + pub fn collision_point(&self, robot_radius: f32) -> Option { + self.collision_angle() + .map(|angle| WorldPoint::new(robot_radius * angle.cos(), robot_radius * angle.sin())) + } + + /// Estimate collision point in world frame + pub fn collision_world_point( + &self, + robot_pose: Pose2D, + robot_radius: f32, + ) -> Option { + self.collision_point(robot_radius) + .map(|p| robot_pose.transform_point(p)) + } +} + +/// Combined sensor observation for a single timestep +#[derive(Clone, Debug)] +pub struct SensorObservation { + /// Robot pose when observation was taken + pub pose: Pose2D, + /// Lidar scan (None if not available this tick) + pub lidar: Option, + /// Cliff sensor states + pub cliffs: CliffSensors, + /// Bumper sensor states + pub bumpers: BumperSensors, + /// Timestamp in microseconds since epoch + pub timestamp_us: u64, +} + +impl SensorObservation { + /// Create an observation with only pose + pub fn pose_only(pose: Pose2D, timestamp_us: u64) -> Self { + Self { + pose, + lidar: None, + cliffs: CliffSensors::SAFE, + bumpers: BumperSensors::CLEAR, + timestamp_us, + } + } + + /// Create an observation with lidar + pub fn with_lidar(pose: Pose2D, lidar: LidarScan, timestamp_us: u64) -> Self { + Self { + pose, + lidar: Some(lidar), + cliffs: CliffSensors::SAFE, + bumpers: BumperSensors::CLEAR, + timestamp_us, + } + } + + /// Any sensor triggered (cliff or bumper)? + pub fn any_triggered(&self) -> bool { + self.cliffs.any_triggered() || self.bumpers.any_triggered() + } +} + +#[cfg(test)] +mod tests { + use super::*; + use std::f32::consts::FRAC_PI_2; + + #[test] + fn test_lidar_scan_valid_points() { + let scan = LidarScan::new( + vec![1.0, 0.1, 5.0, 10.0, f32::INFINITY], + vec![0.0, 0.1, 0.2, 0.3, 0.4], + 0.15, + 8.0, + ); + + let valid: Vec<_> = scan.valid_points().collect(); + assert_eq!(valid.len(), 2); // Only 1.0 and 5.0 are valid + } + + #[test] + fn test_lidar_to_cartesian() { + let scan = LidarScan::new( + vec![1.0], + vec![0.0], // Forward + 0.15, + 8.0, + ); + + let points = scan.to_cartesian(); + assert_eq!(points.len(), 1); + assert!((points[0].x - 1.0).abs() < 1e-6); + assert!((points[0].y - 0.0).abs() < 1e-6); + } + + #[test] + fn test_cliff_sensors_triggered() { + let cliffs = CliffSensors { + left_side: true, + left_front: false, + right_front: true, + right_side: false, + }; + + assert!(cliffs.any_triggered()); + assert_eq!(cliffs.count_triggered(), 2); + assert_eq!(cliffs.triggered_positions().len(), 2); + } + + #[test] + fn test_bumper_collision_angle() { + assert_eq!(BumperSensors::CLEAR.collision_angle(), None); + assert_eq!( + BumperSensors { + left: true, + right: true + } + .collision_angle(), + Some(0.0) + ); + assert!( + BumperSensors { + left: true, + right: false + } + .collision_angle() + .unwrap() + > 0.0 + ); + assert!( + BumperSensors { + left: false, + right: true + } + .collision_angle() + .unwrap() + < 0.0 + ); + } + + #[test] + fn test_lidar_to_world_points() { + let scan = LidarScan::new( + vec![1.0], + vec![0.0], // Forward in sensor frame + 0.15, + 8.0, + ); + + // Robot at origin facing +Y (90 degrees) + let pose = Pose2D::new(0.0, 0.0, FRAC_PI_2); + let world_points = scan.to_world_points(pose); + + assert_eq!(world_points.len(), 1); + // Forward in robot frame (which is +Y in world) should be at (0, 1) + assert!((world_points[0].x - 0.0).abs() < 1e-5); + assert!((world_points[0].y - 1.0).abs() < 1e-5); + } +} diff --git a/vastu-map/src/core/simd.rs b/vastu-map/src/core/simd.rs new file mode 100644 index 0000000..e2ffd11 --- /dev/null +++ b/vastu-map/src/core/simd.rs @@ -0,0 +1,604 @@ +//! SIMD-accelerated batch operations for point transformations. +//! +//! This module provides vectorized operations for transforming point clouds, +//! optimized for ARM NEON (f32x4) and x86 SSE/AVX. +//! +//! ## Key Types +//! +//! - [`PointBatch`]: Structure-of-Arrays storage for 2D points +//! - [`RotationMatrix4`]: Pre-computed rotation matrix for SIMD transforms +//! +//! ## Usage +//! +//! ```rust,ignore +//! use vastu_map::core::simd::{PointBatch, transform_points_simd4, RotationMatrix4}; +//! +//! let points = PointBatch::from_tuples(&[(1.0, 0.0), (0.0, 1.0), (1.0, 1.0), (2.0, 0.0)]); +//! let pose = Pose2D::new(1.0, 2.0, 0.5); +//! let rot = RotationMatrix4::from_pose(&pose); +//! +//! let mut out_xs = vec![0.0f32; 4]; +//! let mut out_ys = vec![0.0f32; 4]; +//! transform_points_simd4(&points, &rot, &mut out_xs, &mut out_ys); +//! ``` + +use std::simd::{StdFloat, cmp::SimdPartialOrd, f32x4, i32x4, num::SimdFloat}; + +use super::Pose2D; + +/// Number of lanes for SIMD operations (4 for ARM NEON / SSE) +pub const LANES: usize = 4; + +/// Batch of 2D points in Structure-of-Arrays (SoA) layout. +/// +/// This layout is optimal for SIMD operations where x and y coordinates +/// are processed independently. +/// +/// ## Memory Layout +/// ```text +/// xs: [x0, x1, x2, x3, x4, ...] +/// ys: [y0, y1, y2, y3, y4, ...] +/// ``` +#[derive(Clone, Debug, Default)] +pub struct PointBatch { + /// X coordinates + pub xs: Vec, + /// Y coordinates + pub ys: Vec, +} + +impl PointBatch { + /// Create an empty batch. + #[inline] + pub fn new() -> Self { + Self::default() + } + + /// Create with specified capacity. + #[inline] + pub fn with_capacity(capacity: usize) -> Self { + Self { + xs: Vec::with_capacity(capacity), + ys: Vec::with_capacity(capacity), + } + } + + /// Create from slice of (x, y) tuples. + pub fn from_tuples(points: &[(f32, f32)]) -> Self { + let n = points.len(); + let mut xs = Vec::with_capacity(n); + let mut ys = Vec::with_capacity(n); + for &(x, y) in points { + xs.push(x); + ys.push(y); + } + Self { xs, ys } + } + + /// Create with padding to next multiple of LANES. + /// + /// Padded values are zero (neutral for transforms). + /// This eliminates remainder handling in SIMD loops. + pub fn from_tuples_padded(points: &[(f32, f32)]) -> Self { + let n = points.len(); + let padded_len = n.div_ceil(LANES) * LANES; + let mut xs = vec![0.0; padded_len]; + let mut ys = vec![0.0; padded_len]; + for (i, &(x, y)) in points.iter().enumerate() { + xs[i] = x; + ys[i] = y; + } + Self { xs, ys } + } + + /// Push a point. + #[inline] + pub fn push(&mut self, x: f32, y: f32) { + self.xs.push(x); + self.ys.push(y); + } + + /// Number of points. + #[inline] + pub fn len(&self) -> usize { + self.xs.len() + } + + /// Check if empty. + #[inline] + pub fn is_empty(&self) -> bool { + self.xs.is_empty() + } + + /// Clear all points. + #[inline] + pub fn clear(&mut self) { + self.xs.clear(); + self.ys.clear(); + } + + /// Original (unpadded) length for iteration. + /// + /// Use this when the batch was created with padding. + pub fn original_len(&self) -> usize { + // Find last non-zero element (assuming zeros are padding) + // This is a heuristic; for precise tracking, store original_len separately + self.len() + } +} + +/// Batch of grid coordinates in Structure-of-Arrays layout. +#[derive(Clone, Debug, Default)] +pub struct GridCoordBatch { + /// X coordinates (column indices) + pub xs: Vec, + /// Y coordinates (row indices) + pub ys: Vec, +} + +impl GridCoordBatch { + /// Create with specified capacity. + #[inline] + pub fn with_capacity(capacity: usize) -> Self { + Self { + xs: Vec::with_capacity(capacity), + ys: Vec::with_capacity(capacity), + } + } + + /// Resize to the given length, filling with zeros. + #[inline] + pub fn resize(&mut self, len: usize) { + self.xs.resize(len, 0); + self.ys.resize(len, 0); + } + + /// Number of coordinates. + #[inline] + pub fn len(&self) -> usize { + self.xs.len() + } + + /// Check if empty. + #[inline] + pub fn is_empty(&self) -> bool { + self.xs.is_empty() + } +} + +/// Pre-computed rotation matrix for batch transforms. +/// +/// Stores cos(theta) and sin(theta) as SIMD vectors to avoid +/// redundant broadcasts in tight loops. +#[derive(Clone, Copy)] +pub struct RotationMatrix4 { + /// cos(theta) broadcast to all lanes + pub cos: f32x4, + /// sin(theta) broadcast to all lanes + pub sin: f32x4, + /// Translation X (pose.x) broadcast to all lanes + pub tx: f32x4, + /// Translation Y (pose.y) broadcast to all lanes + pub ty: f32x4, +} + +impl RotationMatrix4 { + /// Create rotation matrix from a pose. + #[inline] + pub fn from_pose(pose: &Pose2D) -> Self { + let cos = pose.theta.cos(); + let sin = pose.theta.sin(); + Self { + cos: f32x4::splat(cos), + sin: f32x4::splat(sin), + tx: f32x4::splat(pose.x), + ty: f32x4::splat(pose.y), + } + } + + /// Create identity transform (no rotation or translation). + #[inline] + pub fn identity() -> Self { + Self { + cos: f32x4::splat(1.0), + sin: f32x4::splat(0.0), + tx: f32x4::splat(0.0), + ty: f32x4::splat(0.0), + } + } +} + +/// Transform points from sensor frame to world frame using SIMD. +/// +/// Processes 4 points per iteration using ARM NEON / SSE instructions. +/// +/// # Algorithm +/// ```text +/// world_x = pose.x + px * cos(theta) - py * sin(theta) +/// world_y = pose.y + px * sin(theta) + py * cos(theta) +/// ``` +/// +/// # Arguments +/// * `points` - Input points in sensor frame (SoA layout) +/// * `rot` - Pre-computed rotation matrix +/// * `out_xs` - Output X coordinates (must be at least `points.len()` long) +/// * `out_ys` - Output Y coordinates (must be at least `points.len()` long) +#[inline] +pub fn transform_points_simd4( + points: &PointBatch, + rot: &RotationMatrix4, + out_xs: &mut [f32], + out_ys: &mut [f32], +) { + debug_assert_eq!(points.xs.len(), points.ys.len()); + debug_assert!(points.xs.len() <= out_xs.len()); + debug_assert!(points.xs.len() <= out_ys.len()); + + let n = points.xs.len(); + let simd_end = n - (n % LANES); + + // SIMD path: process 4 points per iteration + for i in (0..simd_end).step_by(LANES) { + // Load 4 x and 4 y coordinates + let px = f32x4::from_slice(&points.xs[i..]); + let py = f32x4::from_slice(&points.ys[i..]); + + // Transform: world = translation + rotation * local + // Uses FMA (fused multiply-add) on supporting hardware + let world_x = rot.tx + px * rot.cos - py * rot.sin; + let world_y = rot.ty + px * rot.sin + py * rot.cos; + + // Store results + world_x.copy_to_slice(&mut out_xs[i..]); + world_y.copy_to_slice(&mut out_ys[i..]); + } + + // Scalar fallback for remainder + let cos = rot.cos[0]; + let sin = rot.sin[0]; + let tx = rot.tx[0]; + let ty = rot.ty[0]; + + for i in simd_end..n { + let px = points.xs[i]; + let py = points.ys[i]; + out_xs[i] = tx + px * cos - py * sin; + out_ys[i] = ty + px * sin + py * cos; + } +} + +/// Allocating version of `transform_points_simd4`. +/// +/// Returns a new `PointBatch` with the transformed points. +pub fn transform_points(points: &PointBatch, pose: &Pose2D) -> PointBatch { + let n = points.xs.len(); + let mut out_xs = vec![0.0; n]; + let mut out_ys = vec![0.0; n]; + + let rot = RotationMatrix4::from_pose(pose); + transform_points_simd4(points, &rot, &mut out_xs, &mut out_ys); + + PointBatch { + xs: out_xs, + ys: out_ys, + } +} + +/// Convert world coordinates to grid coordinates using SIMD. +/// +/// Performs: `grid_coord = floor((world - origin) / resolution)` +/// +/// # Arguments +/// * `world_xs` - World X coordinates +/// * `world_ys` - World Y coordinates +/// * `origin_x` - Grid origin X +/// * `origin_y` - Grid origin Y +/// * `inv_resolution` - Precomputed `1.0 / resolution` +/// * `out_xs` - Output grid X coordinates +/// * `out_ys` - Output grid Y coordinates +#[inline] +pub fn world_to_grid_simd4( + world_xs: &[f32], + world_ys: &[f32], + origin_x: f32, + origin_y: f32, + inv_resolution: f32, + out_xs: &mut [i32], + out_ys: &mut [i32], +) { + let n = world_xs.len(); + let simd_end = n - (n % LANES); + + let origin_x_v = f32x4::splat(origin_x); + let origin_y_v = f32x4::splat(origin_y); + let inv_res_v = f32x4::splat(inv_resolution); + + for i in (0..simd_end).step_by(LANES) { + let wx = f32x4::from_slice(&world_xs[i..]); + let wy = f32x4::from_slice(&world_ys[i..]); + + // (world - origin) / resolution + let gx_f = (wx - origin_x_v) * inv_res_v; + let gy_f = (wy - origin_y_v) * inv_res_v; + + // Floor and convert to i32 (SIMD cast) + let gx_floor = gx_f.floor(); + let gy_floor = gy_f.floor(); + + // Cast f32x4 to i32x4 and store directly + let gx_i32: i32x4 = gx_floor.cast(); + let gy_i32: i32x4 = gy_floor.cast(); + gx_i32.copy_to_slice(&mut out_xs[i..i + LANES]); + gy_i32.copy_to_slice(&mut out_ys[i..i + LANES]); + } + + // Scalar remainder + for i in simd_end..n { + out_xs[i] = ((world_xs[i] - origin_x) * inv_resolution).floor() as i32; + out_ys[i] = ((world_ys[i] - origin_y) * inv_resolution).floor() as i32; + } +} + +/// Calculate squared distances from origin for multiple points. +/// +/// Returns `dx*dx + dy*dy` for each point (avoids sqrt for efficiency). +#[inline] +pub fn distance_squared_simd4( + xs: &[f32], + ys: &[f32], + origin_x: f32, + origin_y: f32, + out: &mut [f32], +) { + let n = xs.len(); + let simd_end = n - (n % LANES); + + let ox = f32x4::splat(origin_x); + let oy = f32x4::splat(origin_y); + + for i in (0..simd_end).step_by(LANES) { + let px = f32x4::from_slice(&xs[i..]); + let py = f32x4::from_slice(&ys[i..]); + + let dx = px - ox; + let dy = py - oy; + + // dx * dx + dy * dy + let dist_sq = dx * dx + dy * dy; + dist_sq.copy_to_slice(&mut out[i..]); + } + + for i in simd_end..n { + let dx = xs[i] - origin_x; + let dy = ys[i] - origin_y; + out[i] = dx * dx + dy * dy; + } +} + +/// Calculate actual distances (with sqrt). +/// +/// Only use when actual distances are needed; squared is faster. +#[inline] +pub fn distance_simd4(xs: &[f32], ys: &[f32], origin_x: f32, origin_y: f32, out: &mut [f32]) { + distance_squared_simd4(xs, ys, origin_x, origin_y, out); + + let n = out.len(); + let simd_end = n - (n % LANES); + + for i in (0..simd_end).step_by(LANES) { + let sq = f32x4::from_slice(&out[i..]); + let dist = sq.sqrt(); + dist.copy_to_slice(&mut out[i..]); + } + + for val in out.iter_mut().skip(simd_end) { + *val = val.sqrt(); + } +} + +/// Check if grid coordinates are within bounds (batch operation). +/// +/// Returns a mask where `true` indicates the coordinate is valid. +#[inline] +pub fn is_valid_coords_simd4( + grid_xs: &[i32], + grid_ys: &[i32], + width: i32, + height: i32, + out_valid: &mut [bool], +) { + let n = grid_xs.len(); + let simd_end = n - (n % LANES); + + let zero_v = i32x4::splat(0); + let width_v = i32x4::splat(width); + let height_v = i32x4::splat(height); + + for i in (0..simd_end).step_by(LANES) { + let gx = i32x4::from_slice(&grid_xs[i..]); + let gy = i32x4::from_slice(&grid_ys[i..]); + + // Valid if: x >= 0 && y >= 0 && x < width && y < height + let x_ge_0 = gx.simd_ge(zero_v); + let y_ge_0 = gy.simd_ge(zero_v); + let x_lt_w = gx.simd_lt(width_v); + let y_lt_h = gy.simd_lt(height_v); + + let valid = x_ge_0 & y_ge_0 & x_lt_w & y_lt_h; + + // Extract mask bits + for j in 0..LANES { + out_valid[i + j] = valid.test(j); + } + } + + // Scalar remainder + for i in simd_end..n { + let gx = grid_xs[i]; + let gy = grid_ys[i]; + out_valid[i] = gx >= 0 && gy >= 0 && gx < width && gy < height; + } +} + +#[cfg(test)] +mod tests { + use super::*; + use std::f32::consts::FRAC_PI_2; + + #[test] + fn test_point_batch_creation() { + let points = PointBatch::from_tuples(&[(1.0, 2.0), (3.0, 4.0), (5.0, 6.0)]); + assert_eq!(points.len(), 3); + assert_eq!(points.xs[0], 1.0); + assert_eq!(points.ys[0], 2.0); + } + + #[test] + fn test_point_batch_padded() { + let points = PointBatch::from_tuples_padded(&[(1.0, 2.0), (3.0, 4.0), (5.0, 6.0)]); + assert_eq!(points.len(), 4); // Padded to next multiple of 4 + assert_eq!(points.xs[3], 0.0); // Padding is zero + } + + #[test] + fn test_transform_identity() { + let points = PointBatch::from_tuples(&[(1.0, 0.0), (0.0, 1.0), (1.0, 1.0), (2.0, 2.0)]); + let pose = Pose2D::new(0.0, 0.0, 0.0); + + let result = transform_points(&points, &pose); + + for i in 0..4 { + assert!((result.xs[i] - points.xs[i]).abs() < 1e-6); + assert!((result.ys[i] - points.ys[i]).abs() < 1e-6); + } + } + + #[test] + fn test_transform_translation() { + let points = PointBatch::from_tuples(&[(0.0, 0.0), (1.0, 0.0), (0.0, 1.0), (1.0, 1.0)]); + let pose = Pose2D::new(10.0, 20.0, 0.0); + + let result = transform_points(&points, &pose); + + assert!((result.xs[0] - 10.0).abs() < 1e-6); + assert!((result.ys[0] - 20.0).abs() < 1e-6); + assert!((result.xs[1] - 11.0).abs() < 1e-6); + assert!((result.ys[1] - 20.0).abs() < 1e-6); + } + + #[test] + fn test_transform_rotation_90deg() { + let points = PointBatch::from_tuples(&[(1.0, 0.0), (0.0, 1.0), (1.0, 1.0), (0.0, 0.0)]); + let pose = Pose2D::new(0.0, 0.0, FRAC_PI_2); + + let result = transform_points(&points, &pose); + + // (1, 0) rotated 90° CCW -> (0, 1) + assert!((result.xs[0] - 0.0).abs() < 1e-5); + assert!((result.ys[0] - 1.0).abs() < 1e-5); + + // (0, 1) rotated 90° CCW -> (-1, 0) + assert!((result.xs[1] - (-1.0)).abs() < 1e-5); + assert!((result.ys[1] - 0.0).abs() < 1e-5); + } + + #[test] + fn test_transform_matches_scalar() { + let points = + PointBatch::from_tuples(&[(1.0, 2.0), (3.0, 4.0), (5.0, 6.0), (7.0, 8.0), (9.0, 10.0)]); + let pose = Pose2D::new(1.0, 2.0, 0.5); + + let result = transform_points(&points, &pose); + + // Compare with scalar transform + let cos = pose.theta.cos(); + let sin = pose.theta.sin(); + + for i in 0..points.len() { + let px = points.xs[i]; + let py = points.ys[i]; + let expected_x = pose.x + px * cos - py * sin; + let expected_y = pose.y + px * sin + py * cos; + + assert!( + (result.xs[i] - expected_x).abs() < 1e-5, + "x mismatch at {}", + i + ); + assert!( + (result.ys[i] - expected_y).abs() < 1e-5, + "y mismatch at {}", + i + ); + } + } + + #[test] + fn test_world_to_grid() { + let world_xs = [0.0f32, 0.5, 1.0, 1.5, 2.0, 2.5, 3.0, 3.5]; + let world_ys = [0.0f32, 0.5, 1.0, 1.5, 2.0, 2.5, 3.0, 3.5]; + let mut grid_xs = [0i32; 8]; + let mut grid_ys = [0i32; 8]; + + world_to_grid_simd4( + &world_xs, + &world_ys, + 0.0, + 0.0, + 1.0 / 0.5, // 2 cells per meter + &mut grid_xs, + &mut grid_ys, + ); + + assert_eq!(grid_xs[0], 0); + assert_eq!(grid_xs[1], 1); + assert_eq!(grid_xs[2], 2); + assert_eq!(grid_xs[3], 3); + } + + #[test] + fn test_distance_squared() { + let xs = [0.0f32, 3.0, 0.0, 3.0]; + let ys = [0.0f32, 0.0, 4.0, 4.0]; + let mut out = [0.0f32; 4]; + + distance_squared_simd4(&xs, &ys, 0.0, 0.0, &mut out); + + assert!((out[0] - 0.0).abs() < 1e-6); + assert!((out[1] - 9.0).abs() < 1e-6); + assert!((out[2] - 16.0).abs() < 1e-6); + assert!((out[3] - 25.0).abs() < 1e-6); + } + + #[test] + fn test_distance() { + let xs = [0.0f32, 3.0, 0.0, 3.0]; + let ys = [0.0f32, 0.0, 4.0, 4.0]; + let mut out = [0.0f32; 4]; + + distance_simd4(&xs, &ys, 0.0, 0.0, &mut out); + + assert!((out[0] - 0.0).abs() < 1e-6); + assert!((out[1] - 3.0).abs() < 1e-6); + assert!((out[2] - 4.0).abs() < 1e-6); + assert!((out[3] - 5.0).abs() < 1e-6); + } + + #[test] + fn test_valid_coords() { + let grid_xs = [-1, 0, 5, 10, 0, 0, 0, 0]; + let grid_ys = [0, -1, 0, 0, 5, 10, 0, 0]; + let mut valid = [false; 8]; + + is_valid_coords_simd4(&grid_xs, &grid_ys, 10, 10, &mut valid); + + assert!(!valid[0]); // x < 0 + assert!(!valid[1]); // y < 0 + assert!(valid[2]); // valid + assert!(!valid[3]); // x >= width + assert!(valid[4]); // valid + assert!(!valid[5]); // y >= height + assert!(valid[6]); // valid (0, 0) + assert!(valid[7]); // valid (0, 0) + } +} diff --git a/vastu-map/src/exploration/collision.rs b/vastu-map/src/exploration/collision.rs deleted file mode 100644 index 61623c6..0000000 --- a/vastu-map/src/exploration/collision.rs +++ /dev/null @@ -1,215 +0,0 @@ -//! Collision event handling for exploration. -//! -//! Provides types for representing collision events from sensors -//! and computing virtual walls to add to the map. - -use crate::core::Point2D; -use crate::features::Line2D; -use std::f32::consts::FRAC_PI_2; - -/// Type of collision detected by sensors. -#[derive(Clone, Copy, Debug, PartialEq, Eq)] -pub enum CollisionType { - /// Left bumper triggered. - BumperLeft, - /// Right bumper triggered. - BumperRight, - /// Both bumpers triggered (head-on collision). - BumperBoth, - /// Cliff sensor triggered. - Cliff, -} - -impl CollisionType { - /// Check if this is a bumper collision. - pub fn is_bumper(&self) -> bool { - matches!( - self, - CollisionType::BumperLeft | CollisionType::BumperRight | CollisionType::BumperBoth - ) - } - - /// Check if this is a cliff detection. - pub fn is_cliff(&self) -> bool { - matches!(self, CollisionType::Cliff) - } -} - -/// Collision event from sensors. -/// -/// This struct represents a collision detected by the robot's sensors. -/// It is provided by DhruvaSLAM (or the test harness) to the exploration -/// controller, which uses it to update the map and plan recovery. -#[derive(Clone, Debug)] -pub struct CollisionEvent { - /// Type of collision detected. - pub collision_type: CollisionType, - /// Estimated collision point in world frame. - pub point: Point2D, - /// Robot heading at time of collision (radians). - pub heading: f32, -} - -impl CollisionEvent { - /// Create a new collision event. - pub fn new(collision_type: CollisionType, point: Point2D, heading: f32) -> Self { - Self { - collision_type, - point, - heading, - } - } - - /// Create a bumper collision event from robot pose. - /// - /// # Arguments - /// * `left` - Left bumper triggered - /// * `right` - Right bumper triggered - /// * `robot_x` - Robot X position - /// * `robot_y` - Robot Y position - /// * `robot_theta` - Robot heading - /// * `robot_radius` - Robot collision radius - pub fn from_bumper( - left: bool, - right: bool, - robot_x: f32, - robot_y: f32, - robot_theta: f32, - robot_radius: f32, - ) -> Option { - let collision_type = match (left, right) { - (true, true) => CollisionType::BumperBoth, - (true, false) => CollisionType::BumperLeft, - (false, true) => CollisionType::BumperRight, - (false, false) => return None, - }; - - // Compute collision point slightly ahead of robot - let offset = robot_radius + 0.02; // Small offset past radius - let point = Point2D::new( - robot_x + offset * robot_theta.cos(), - robot_y + offset * robot_theta.sin(), - ); - - Some(Self { - collision_type, - point, - heading: robot_theta, - }) - } - - /// Compute virtual wall to add at collision point. - /// - /// Creates a wall perpendicular to the robot's heading at the - /// collision point. This prevents future path planning through - /// areas that are physically blocked (e.g., glass/mirrors). - /// - /// # Arguments - /// * `wall_length` - Length of the virtual wall to create - pub fn to_virtual_wall(&self, wall_length: f32) -> VirtualWall { - // Wall is perpendicular to heading - let perp_angle = self.heading + FRAC_PI_2; - let half_len = wall_length / 2.0; - - let start = Point2D::new( - self.point.x + half_len * perp_angle.cos(), - self.point.y + half_len * perp_angle.sin(), - ); - let end = Point2D::new( - self.point.x - half_len * perp_angle.cos(), - self.point.y - half_len * perp_angle.sin(), - ); - - VirtualWall { - line: Line2D::new(start, end), - source: self.collision_type, - } - } - - /// Get the opposite direction for backoff. - /// - /// Returns the heading angle pointing away from the collision. - pub fn backoff_heading(&self) -> f32 { - // Back off in the opposite direction - self.heading + std::f32::consts::PI - } -} - -/// Virtual wall to add to map after collision. -/// -/// When the robot collides with an obstacle that wasn't detected by lidar -/// (e.g., glass, mirrors), a virtual wall is added to the map to prevent -/// future path planning through that area. -#[derive(Clone, Debug)] -pub struct VirtualWall { - /// The line segment representing the wall. - pub line: Line2D, - /// The collision type that caused this wall to be created. - pub source: CollisionType, -} - -impl VirtualWall { - /// Create a new virtual wall. - pub fn new(line: Line2D, source: CollisionType) -> Self { - Self { line, source } - } -} - -#[cfg(test)] -mod tests { - use super::*; - use std::f32::consts::PI; - - #[test] - fn test_collision_type_checks() { - assert!(CollisionType::BumperLeft.is_bumper()); - assert!(CollisionType::BumperRight.is_bumper()); - assert!(CollisionType::BumperBoth.is_bumper()); - assert!(!CollisionType::Cliff.is_bumper()); - - assert!(CollisionType::Cliff.is_cliff()); - assert!(!CollisionType::BumperLeft.is_cliff()); - } - - #[test] - fn test_from_bumper() { - // No collision - assert!(CollisionEvent::from_bumper(false, false, 0.0, 0.0, 0.0, 0.15).is_none()); - - // Left bumper - let event = CollisionEvent::from_bumper(true, false, 1.0, 2.0, 0.0, 0.15).unwrap(); - assert_eq!(event.collision_type, CollisionType::BumperLeft); - assert!(event.point.x > 1.0); // Point is ahead of robot - - // Both bumpers - let event = CollisionEvent::from_bumper(true, true, 0.0, 0.0, PI / 2.0, 0.15).unwrap(); - assert_eq!(event.collision_type, CollisionType::BumperBoth); - assert!(event.point.y > 0.0); // Point is ahead when facing +Y - } - - #[test] - fn test_to_virtual_wall() { - let event = CollisionEvent::new(CollisionType::BumperBoth, Point2D::new(1.0, 0.0), 0.0); - - let wall = event.to_virtual_wall(0.3); - - // Wall should be perpendicular to heading (vertical when heading is 0) - assert_eq!(wall.source, CollisionType::BumperBoth); - assert!((wall.line.start.x - 1.0).abs() < 0.01); - assert!((wall.line.end.x - 1.0).abs() < 0.01); - assert!((wall.line.start.y - 0.15).abs() < 0.01); - assert!((wall.line.end.y + 0.15).abs() < 0.01); - } - - #[test] - fn test_backoff_heading() { - let event = CollisionEvent::new(CollisionType::BumperBoth, Point2D::new(0.0, 0.0), 0.0); - let backoff = event.backoff_heading(); - assert!((backoff - PI).abs() < 0.01); - - let event2 = - CollisionEvent::new(CollisionType::BumperBoth, Point2D::new(0.0, 0.0), PI / 2.0); - let backoff2 = event2.backoff_heading(); - assert!((backoff2 - 3.0 * PI / 2.0).abs() < 0.01); - } -} diff --git a/vastu-map/src/exploration/config.rs b/vastu-map/src/exploration/config.rs index 1a28e79..29746bc 100644 --- a/vastu-map/src/exploration/config.rs +++ b/vastu-map/src/exploration/config.rs @@ -1,169 +1,70 @@ -//! Configuration for exploration behavior. +//! Exploration controller configuration. -use crate::query::frontier::FrontierConfig; +use serde::{Deserialize, Serialize}; -use super::planner::PlannerConfig; +use crate::query::RobotFootprint; -/// Configuration for exploration behavior. -#[derive(Clone, Debug)] +/// Exploration controller configuration +#[derive(Clone, Debug, Serialize, Deserialize)] pub struct ExplorationConfig { - /// Length of virtual walls added at collision points (meters). - /// Default: 0.3 - pub virtual_wall_length: f32, + /// Robot footprint for collision checking + pub footprint: RobotFootprint, - /// Distance to back off after collision (meters). - /// Default: 0.15 - pub backoff_distance: f32, + /// Minimum frontier size to consider (cells) + pub min_frontier_size: usize, - /// Distance threshold to consider waypoint reached (meters). - /// Default: 0.1 - pub waypoint_tolerance: f32, + /// Distance threshold to consider frontier reached (meters) + pub frontier_reached_threshold: f32, - /// Maximum linear velocity (m/s). - /// Default: 0.3 - pub max_linear_speed: f32, + /// Distance threshold for waypoint reached (meters) + pub waypoint_threshold: f32, - /// Maximum angular velocity (rad/s). - /// Default: 1.0 - pub max_angular_speed: f32, + /// Maximum distance to plan at once (meters) + pub max_plan_distance: f32, - /// Robot radius for path planning clearance (meters). - /// Default: 0.15 - pub robot_radius: f32, + /// Replan interval when path is blocked (seconds) + pub replan_interval: f32, - /// Lookahead distance for pure pursuit path following (meters). - /// Default: 0.3 - pub lookahead_distance: f32, + /// Maximum consecutive failures before giving up + pub max_consecutive_failures: usize, - /// Frontier detection configuration. - /// Note: With shadow-based frontiers, viewpoints are already safe observation points. - pub frontier_config: FrontierConfig, + /// Clearance weight for path planning (higher = prefer more clearance) + pub path_clearance_weight: f32, - /// Region-based exploration planner configuration. - pub planner_config: PlannerConfig, + /// Enable path smoothing + pub smooth_paths: bool, - /// Cell size for visit history tracking (meters). - /// Default: 0.5 - pub history_cell_size: f32, + /// Recovery turn angle (radians) when stuck + pub recovery_turn_angle: f32, - /// Maximum number of recent positions to track in history. - /// Default: 100 - pub history_max_recent: usize, + /// Recovery backup distance (meters) when stuck + pub recovery_backup_distance: f32, } impl Default for ExplorationConfig { fn default() -> Self { Self { - virtual_wall_length: 0.3, - backoff_distance: 0.15, - waypoint_tolerance: 0.1, - max_linear_speed: 0.3, - max_angular_speed: 1.0, - robot_radius: 0.15, - lookahead_distance: 0.3, - frontier_config: FrontierConfig::default(), - planner_config: PlannerConfig::default(), - history_cell_size: 0.5, - history_max_recent: 100, + footprint: RobotFootprint::default(), + min_frontier_size: 5, + frontier_reached_threshold: 0.3, // 30cm + waypoint_threshold: 0.1, // 10cm + max_plan_distance: 5.0, // 5m + replan_interval: 1.0, // 1 second + max_consecutive_failures: 5, + path_clearance_weight: 0.0, + smooth_paths: true, + recovery_turn_angle: std::f32::consts::FRAC_PI_4, // 45 degrees + recovery_backup_distance: 0.1, // 10cm } } } impl ExplorationConfig { - /// Create a new configuration with default values. - pub fn new() -> Self { - Self::default() - } - - /// Builder-style setter for virtual wall length. - pub fn with_virtual_wall_length(mut self, length: f32) -> Self { - self.virtual_wall_length = length; - self - } - - /// Builder-style setter for backoff distance. - pub fn with_backoff_distance(mut self, distance: f32) -> Self { - self.backoff_distance = distance; - self - } - - /// Builder-style setter for waypoint tolerance. - pub fn with_waypoint_tolerance(mut self, tolerance: f32) -> Self { - self.waypoint_tolerance = tolerance; - self - } - - /// Builder-style setter for max linear speed. - pub fn with_max_linear_speed(mut self, speed: f32) -> Self { - self.max_linear_speed = speed; - self - } - - /// Builder-style setter for max angular speed. - pub fn with_max_angular_speed(mut self, speed: f32) -> Self { - self.max_angular_speed = speed; - self - } - - /// Builder-style setter for robot radius. - pub fn with_robot_radius(mut self, radius: f32) -> Self { - self.robot_radius = radius; - self - } - - /// Builder-style setter for lookahead distance. - pub fn with_lookahead_distance(mut self, distance: f32) -> Self { - self.lookahead_distance = distance; - self - } - - /// Builder-style setter for frontier config. - pub fn with_frontier_config(mut self, config: FrontierConfig) -> Self { - self.frontier_config = config; - self - } - - /// Builder-style setter for planner config. - pub fn with_planner_config(mut self, config: PlannerConfig) -> Self { - self.planner_config = config; - self - } - - /// Builder-style setter for history cell size. - pub fn with_history_cell_size(mut self, size: f32) -> Self { - self.history_cell_size = size; - self - } - - /// Builder-style setter for history max recent. - pub fn with_history_max_recent(mut self, max: usize) -> Self { - self.history_max_recent = max; - self - } -} - -#[cfg(test)] -mod tests { - use super::*; - - #[test] - fn test_default_config() { - let config = ExplorationConfig::default(); - assert_eq!(config.virtual_wall_length, 0.3); - assert_eq!(config.backoff_distance, 0.15); - assert_eq!(config.waypoint_tolerance, 0.1); - assert_eq!(config.max_linear_speed, 0.3); - assert_eq!(config.max_angular_speed, 1.0); - assert_eq!(config.robot_radius, 0.15); - } - - #[test] - fn test_builder_pattern() { - let config = ExplorationConfig::new() - .with_max_linear_speed(0.5) - .with_robot_radius(0.2); - - assert_eq!(config.max_linear_speed, 0.5); - assert_eq!(config.robot_radius, 0.2); + /// Create with custom robot footprint + pub fn with_footprint(footprint: RobotFootprint) -> Self { + Self { + footprint, + ..Default::default() + } } } diff --git a/vastu-map/src/exploration/controller.rs b/vastu-map/src/exploration/controller.rs index 97248e7..461d1f8 100644 --- a/vastu-map/src/exploration/controller.rs +++ b/vastu-map/src/exploration/controller.rs @@ -1,1286 +1,888 @@ -//! Exploration controller state machine. -//! -//! Manages frontier-based exploration with collision recovery. -//! The primary goal is complete map coverage, with obstacle avoidance -//! as the secondary priority. -//! -//! # Region-Based Exploration -//! -//! This controller uses a hierarchical exploration strategy: -//! 1. Cluster frontiers into exploration regions -//! 2. Order regions using greedy TSP to minimize travel -//! 3. Select frontiers within each region using heading-aligned selection -//! -//! This approach eliminates ping-pong behavior and improves coverage efficiency. +//! Exploration controller implementation. -use std::collections::{HashMap, VecDeque}; +use crate::core::{Pose2D, WorldPoint}; +use crate::grid::GridStorage; +use crate::pathfinding::{AStarConfig, AStarPlanner, PathFailure, PathSmoother, SmoothingConfig}; +use crate::query::{Frontier, FrontierDetector, TraversabilityChecker}; +use log::{debug, info, trace, warn}; -use crate::core::{Point2D, Pose2D}; -use crate::{Frontier, Map, Path}; - -use super::collision::{CollisionEvent, VirtualWall}; use super::config::ExplorationConfig; -use super::history::ExplorationHistory; -use super::path_follower::{PathFollower, VelocityCommand}; -use super::planner::ExplorationPlanner; +use super::state::{ExplorationCommand, ExplorationEvent, ExplorationState, RecoveryAction}; -/// Current exploration state. -#[derive(Clone, Debug)] -pub enum ExplorationState { - /// Idle, waiting for start. - Idle, - - /// Selecting next frontier to explore. - SelectingFrontier, - - /// Following path to target frontier. - FollowingPath { - /// Current path being followed. - path: Path, - /// Current waypoint index. - waypoint_idx: usize, - /// Target frontier point. - target_frontier: Point2D, - /// CBVG goal node index used for this path (for collision tracking). - goal_node_idx: Option, - }, - - /// Recovering from collision (backing off). - RecoveringFromCollision { - /// Distance remaining to back off (meters). - backoff_remaining: f32, - /// Heading to back off towards (radians). - backoff_heading: f32, - /// Position when backoff started. - start_position: Point2D, - /// The frontier we were trying to reach (for alternative path planning). - target_frontier: Point2D, - /// The CBVG goal node that led to collision (to exclude in retry). - failed_goal_node: Option, - /// Number of recovery collision attempts. - recovery_attempts: u32, - }, - - /// Exploration complete - no frontiers remain. - Complete, - - /// Exploration failed (stuck, unreachable frontiers). - Failed { - /// Reason for failure. - reason: String, - }, -} - -impl ExplorationState { - /// Get a short description of the state. - pub fn name(&self) -> &'static str { - match self { - ExplorationState::Idle => "Idle", - ExplorationState::SelectingFrontier => "SelectingFrontier", - ExplorationState::FollowingPath { .. } => "FollowingPath", - ExplorationState::RecoveringFromCollision { .. } => "RecoveringFromCollision", - ExplorationState::Complete => "Complete", - ExplorationState::Failed { .. } => "Failed", - } - } -} - -/// Result of a single exploration step. -#[derive(Clone, Debug)] -pub struct ExplorationStep { - /// Velocity command to execute (None if no action needed). - pub velocity: Option, - - /// Current state after this step. - pub state: ExplorationState, - - /// Virtual wall to add to map (if collision occurred). - pub virtual_wall: Option, - - /// Whether a new path was planned this step. - pub path_planned: bool, - - /// Current target frontier (if any). - pub target_frontier: Option, - - /// Distance to current target. - pub distance_to_target: f32, -} - -impl ExplorationStep { - /// Create a step with no action. - fn idle(state: ExplorationState) -> Self { - Self { - velocity: None, - state, - virtual_wall: None, - path_planned: false, - target_frontier: None, - distance_to_target: 0.0, - } - } - - /// Create a step with a velocity command. - fn with_velocity(velocity: VelocityCommand, state: ExplorationState) -> Self { - Self { - velocity: Some(velocity), - state, - virtual_wall: None, - path_planned: false, - target_frontier: None, - distance_to_target: 0.0, - } - } -} - -/// Exploration controller. +/// Exploration controller /// -/// Manages autonomous exploration using frontier-based strategy: -/// -/// 1. Detect frontiers (unexplored areas) -/// 2. Select the best frontier based on distance and openness -/// 3. Plan a path to the frontier -/// 4. Follow the path while avoiding obstacles -/// 5. Handle collisions by adding virtual walls and replanning -/// 6. Repeat until no frontiers remain -/// -/// # Primary Goals -/// -/// 1. **Complete map coverage** - Explore every reachable area -/// 2. **Avoid obstacles** - Navigate safely using path planning with robot radius clearance -/// -/// # Usage -/// -/// ```rust,ignore -/// use vastu_map::exploration::{ExplorationController, ExplorationConfig, CollisionEvent}; -/// use vastu_map::{VectorMap, Pose2D}; -/// -/// let config = ExplorationConfig::default(); -/// let mut controller = ExplorationController::new(config); -/// controller.start(); -/// -/// loop { -/// let map = get_slam_map(); -/// let pose = get_current_pose(); -/// let collision = check_sensors(); -/// -/// let step = controller.update(&map, pose, collision); -/// -/// if let Some(wall) = step.virtual_wall { -/// add_wall_to_map(wall); -/// } -/// -/// if let Some(vel) = step.velocity { -/// send_velocity_command(vel); -/// } -/// -/// if controller.is_complete() { -/// break; -/// } -/// } -/// ``` +/// Implements a state machine for autonomous frontier-based exploration. pub struct ExplorationController { - /// Current exploration state. + /// Current state state: ExplorationState, - - /// Path follower for smooth motion. - path_follower: PathFollower, - - /// Configuration. + /// Configuration config: ExplorationConfig, - - /// Number of consecutive failed path planning attempts for current frontier. - failed_planning_attempts: usize, - - /// Maximum failed attempts before giving up on a frontier. - max_failed_attempts: usize, - - /// The frontier viewpoint we're currently attempting (to track per-frontier failures). - current_attempt_frontier: Option, - - /// Frontiers that have been marked as unreachable. - unreachable_frontiers: Vec, - - /// Distance threshold to consider frontiers as same location. - frontier_match_distance: f32, - - /// Region-based exploration planner. - planner: ExplorationPlanner, - - /// Exploration history for visit tracking. - history: ExplorationHistory, - - /// Time accumulator for dt calculation (seconds). - last_update_time: Option, - - /// Robot position when unreachable frontiers were last cleared. - /// Used to re-evaluate unreachable frontiers after robot moves significantly. - position_at_last_unreachable_clear: Option, - - /// CBVG nodes that led to collision for each frontier viewpoint. - /// Key is grid-quantized frontier position, value is list of failed node indices. - /// Used to find alternative approach angles after collision. - collision_nodes: HashMap<(i32, i32), Vec>, - - /// Recent frontier targets for oscillation detection. - /// Stores (grid_x, grid_y) of last N selected frontiers. - recent_targets: VecDeque<(i32, i32)>, - - /// Frontiers temporarily suppressed due to oscillation detection. - /// Key is grid position, value is suppression count (decrements each cycle). - suppressed_frontiers: HashMap<(i32, i32), u32>, + /// Consecutive failure count + consecutive_failures: usize, + /// Total frontiers explored + frontiers_explored: usize, + /// Total distance traveled (approximate) + distance_traveled: f32, + /// Last known robot pose + last_pose: Pose2D, + /// Recently failed frontier centroids (to avoid re-selecting) + failed_frontier_centroids: Vec, + /// Steps since last significant progress during navigation + steps_without_progress: usize, + /// Last recorded position for stuck detection + stuck_check_position: WorldPoint, + /// Consecutive StartBlocked events (localization drift) + start_blocked_count: usize, } impl ExplorationController { - /// Create a new exploration controller. + /// Create a new exploration controller pub fn new(config: ExplorationConfig) -> Self { - let path_follower = PathFollower::new(&config); - let planner = ExplorationPlanner::new(config.planner_config.clone()); - let history = ExplorationHistory::new(config.history_cell_size, config.history_max_recent); - Self { state: ExplorationState::Idle, - path_follower, config, - failed_planning_attempts: 0, - max_failed_attempts: 3, - current_attempt_frontier: None, - unreachable_frontiers: Vec::new(), - frontier_match_distance: 0.5, - planner, - history, - last_update_time: None, - position_at_last_unreachable_clear: None, - collision_nodes: HashMap::new(), - recent_targets: VecDeque::with_capacity(8), - suppressed_frontiers: HashMap::new(), + consecutive_failures: 0, + frontiers_explored: 0, + distance_traveled: 0.0, + last_pose: Pose2D::default(), + failed_frontier_centroids: Vec::new(), + steps_without_progress: 0, + stuck_check_position: WorldPoint::ZERO, + start_blocked_count: 0, } } - /// Start exploration. - pub fn start(&mut self) { - self.state = ExplorationState::SelectingFrontier; - self.failed_planning_attempts = 0; - self.unreachable_frontiers.clear(); - self.collision_nodes.clear(); - self.planner.reset(); - self.history.clear(); - self.last_update_time = Some(std::time::Instant::now()); - self.position_at_last_unreachable_clear = None; - self.recent_targets.clear(); - self.suppressed_frontiers.clear(); + /// Create with default configuration + pub fn with_defaults() -> Self { + Self::new(ExplorationConfig::default()) } - /// Reset to idle state. - pub fn reset(&mut self) { - self.state = ExplorationState::Idle; - self.failed_planning_attempts = 0; - self.unreachable_frontiers.clear(); - self.collision_nodes.clear(); - self.planner.reset(); - self.history.clear(); - self.last_update_time = None; - self.position_at_last_unreachable_clear = None; - self.recent_targets.clear(); - self.suppressed_frontiers.clear(); - } - - /// Get current state. + /// Get current state pub fn state(&self) -> &ExplorationState { &self.state } - /// Check if exploration is complete. - pub fn is_complete(&self) -> bool { - matches!(self.state, ExplorationState::Complete) - } - - /// Check if exploration failed. - pub fn is_failed(&self) -> bool { - matches!(self.state, ExplorationState::Failed { .. }) - } - - /// Check if exploration is active (not idle, complete, or failed). - pub fn is_active(&self) -> bool { - !matches!( - self.state, - ExplorationState::Idle | ExplorationState::Complete | ExplorationState::Failed { .. } - ) - } - - /// Main update function - call every control loop iteration. - /// - /// # Arguments - /// * `map` - Current SLAM map (implements Map trait) - /// * `current_pose` - Current robot pose estimate - /// * `collision` - Collision event if any - /// - /// # Returns - /// Exploration step with velocity command and state updates. - pub fn update( - &mut self, - map: &M, - current_pose: Pose2D, - collision: Option, - ) -> ExplorationStep { - // Handle collision first (highest priority for safety) - if let Some(event) = collision { - return self.handle_collision(event, current_pose); - } - - // State machine - match self.state.clone() { - ExplorationState::Idle => ExplorationStep::idle(self.state.clone()), - - ExplorationState::SelectingFrontier => self.select_frontier(map, current_pose), - - ExplorationState::FollowingPath { - path, - waypoint_idx, - target_frontier, - goal_node_idx, - } => self.follow_path( - map, - current_pose, - path, - waypoint_idx, - target_frontier, - goal_node_idx, - ), - - ExplorationState::RecoveringFromCollision { - backoff_remaining, - backoff_heading, - start_position, - target_frontier, - failed_goal_node, - recovery_attempts, - } => self.execute_backoff( - current_pose, - backoff_remaining, - backoff_heading, - start_position, - target_frontier, - failed_goal_node, - recovery_attempts, - ), - - ExplorationState::Complete => ExplorationStep::idle(self.state.clone()), - - ExplorationState::Failed { .. } => ExplorationStep::idle(self.state.clone()), - } - } - - /// Handle a collision event. - /// - /// When collision occurs during path following: - /// 1. Record the failed CBVG goal node (if any) - /// 2. Enter recovery state with the target frontier preserved - /// 3. After backoff completes, path planning will try alternative nodes - /// - /// When collision occurs during recovery: - /// 1. Increment recovery attempts counter - /// 2. If too many attempts, mark frontier as unreachable - fn handle_collision(&mut self, event: CollisionEvent, current_pose: Pose2D) -> ExplorationStep { - log::info!( - "Collision detected: {:?} at ({:.2}, {:.2})", - event.collision_type, - event.point.x, - event.point.y - ); - - // Create virtual wall at collision point - let virtual_wall = event.to_virtual_wall(self.config.virtual_wall_length); - - // Handle collision during recovery - increment attempts and possibly give up - if let ExplorationState::RecoveringFromCollision { - target_frontier, - failed_goal_node, - recovery_attempts, - backoff_heading, - .. - } = &self.state - { - let attempts = *recovery_attempts + 1; - let target = *target_frontier; - let failed_node = *failed_goal_node; - - log::warn!( - "Recovery collision at ({:.2}, {:.2}) - attempt {} of 3", - event.point.x, - event.point.y, - attempts - ); - - if attempts >= 3 { - // Too many recovery collisions - mark frontier as unreachable - log::warn!( - "Marking frontier at ({:.2}, {:.2}) as unreachable after {} recovery collisions", - target.x, - target.y, - attempts - ); - - // Track position for unreachable frontier reset logic - if self.unreachable_frontiers.is_empty() { - self.position_at_last_unreachable_clear = Some(current_pose.position()); - } - self.unreachable_frontiers.push(target); - - // Go back to selecting a different frontier - self.state = ExplorationState::SelectingFrontier; - let mut step = ExplorationStep::idle(self.state.clone()); - step.virtual_wall = Some(virtual_wall); - return step; - } - - // Continue recovery with incremented counter - // Keep original start_position so backoff distance calculation is correct - let original_start = - if let ExplorationState::RecoveringFromCollision { start_position, .. } = - &self.state - { - *start_position - } else { - current_pose.position() - }; - - self.state = ExplorationState::RecoveringFromCollision { - backoff_remaining: self.config.backoff_distance, - backoff_heading: *backoff_heading, - start_position: original_start, // Keep original start for correct distance calculation - target_frontier: target, - failed_goal_node: failed_node, - recovery_attempts: attempts, - }; - - let mut step = - ExplorationStep::with_velocity(VelocityCommand::stop(), self.state.clone()); - step.virtual_wall = Some(virtual_wall); - return step; - } - - // Collision during FollowingPath - extract values then record the failed node - let (target_frontier, failed_goal_node) = if let ExplorationState::FollowingPath { - target_frontier, - goal_node_idx, - .. - } = &self.state - { - (*target_frontier, *goal_node_idx) - } else { - // Collision in some other state (shouldn't happen, but handle gracefully) - (current_pose.position(), None) - }; - - // Record this node as failed for this frontier (after extracting values to avoid borrow conflict) - if let Some(node_idx) = failed_goal_node { - self.add_collision_node_for_frontier(target_frontier, node_idx); - } - - log::info!( - "Collision while heading to frontier ({:.2}, {:.2}), failed node: {:?}", - target_frontier.x, - target_frontier.y, - failed_goal_node - ); - - // Transition to recovery state - preserve target frontier for alternative path planning - self.state = ExplorationState::RecoveringFromCollision { - backoff_remaining: self.config.backoff_distance, - backoff_heading: event.backoff_heading(), - start_position: current_pose.position(), - target_frontier, - failed_goal_node, - recovery_attempts: 0, - }; - - // Return stop command with virtual wall - let mut step = ExplorationStep::with_velocity(VelocityCommand::stop(), self.state.clone()); - step.virtual_wall = Some(virtual_wall); - step - } - - /// Select the next frontier to explore using region-based strategy. - /// - /// The algorithm: - /// 1. Update exploration history with current position - /// 2. Update the region-based planner with current frontiers - /// 3. Select the best frontier from the current region - /// 4. Plan a path to the selected frontier - /// 5. If path planning fails, try next frontier or advance to next region - fn select_frontier(&mut self, map: &M, current_pose: Pose2D) -> ExplorationStep { - let robot_pos = current_pose.position(); - - // Calculate dt for planner update - let dt = self - .last_update_time - .map(|t| t.elapsed().as_secs_f32()) - .unwrap_or(0.1); - self.last_update_time = Some(std::time::Instant::now()); - - // Decay suppression counters from oscillation detection - self.decay_suppressed_frontiers(); - - // Update history with current position - self.history.mark_visited(robot_pos); - - // Get all frontiers - let all_frontiers = map.frontiers(); - - // Re-evaluate unreachable frontiers when robot has moved significantly. - // This allows frontiers that were previously unreachable from position A - // to be reconsidered when the robot has moved to position B. - // Use 1.5m for more aggressive retry - robot tries new positions sooner. - const UNREACHABLE_RESET_DISTANCE: f32 = 1.5; // meters - if !self.unreachable_frontiers.is_empty() { - let should_clear = match self.position_at_last_unreachable_clear { - Some(last_pos) => robot_pos.distance(last_pos) > UNREACHABLE_RESET_DISTANCE, - None => false, // First time through, don't clear - }; - - if should_clear { - log::debug!( - "Clearing {} unreachable frontiers and {} collision node entries after robot moved {:.1}m", - self.unreachable_frontiers.len(), - self.collision_nodes.len(), - UNREACHABLE_RESET_DISTANCE - ); - self.unreachable_frontiers.clear(); - self.collision_nodes.clear(); // Also reset collision history - self.position_at_last_unreachable_clear = Some(robot_pos); - } - } - - // Filter out unreachable and suppressed frontiers (using viewpoint for distance checks) - let reachable_frontiers: Vec = all_frontiers - .iter() - .filter(|f| !self.is_frontier_unreachable(f.viewpoint)) - .filter(|f| !self.is_frontier_suppressed(f.viewpoint)) - .cloned() - .collect(); - - if reachable_frontiers.is_empty() { - if all_frontiers.is_empty() { - // No frontiers at all - exploration complete! - log::info!("Exploration complete: no frontiers remain"); - self.state = ExplorationState::Complete; - return ExplorationStep::idle(self.state.clone()); - } else { - // All frontiers are unreachable - log::warn!( - "Exploration stuck: {} frontiers exist but all are unreachable", - all_frontiers.len() - ); - self.state = ExplorationState::Failed { - reason: format!("{} frontiers unreachable", all_frontiers.len()), - }; - return ExplorationStep::idle(self.state.clone()); - } - } - - // Update the region-based planner - self.planner - .update(&reachable_frontiers, robot_pos, &self.history, dt); - - // Try to find a frontier with a valid path - // Safety limit to prevent infinite loops - max iterations = 2 * number of frontiers + regions - const MAX_LOOP_ITERATIONS: usize = 100; - let mut loop_iterations = 0; - - loop { - loop_iterations += 1; - if loop_iterations > MAX_LOOP_ITERATIONS { - log::error!( - "Exploration stuck: exceeded {} iterations in select_frontier loop", - MAX_LOOP_ITERATIONS - ); - self.state = ExplorationState::Failed { - reason: format!( - "frontier selection loop exceeded {} iterations", - MAX_LOOP_ITERATIONS - ), - }; - return ExplorationStep::idle(self.state.clone()); - } - - // Check if we've exhausted all regions - if self.planner.plan().is_complete() { - // Re-filter frontiers to exclude those marked unreachable during this loop - let still_reachable: Vec = reachable_frontiers - .iter() - .filter(|f| !self.is_frontier_unreachable(f.viewpoint)) - .cloned() - .collect(); - - if still_reachable.is_empty() { - // All frontiers are now unreachable - log::warn!( - "Exploration stuck: all {} frontiers are unreachable", - reachable_frontiers.len() - ); - self.state = ExplorationState::Failed { - reason: format!("{} frontiers unreachable", reachable_frontiers.len()), - }; - return ExplorationStep::idle(self.state.clone()); - } - - // Force replan with updated reachable frontiers - self.planner - .force_replan(&still_reachable, robot_pos, &self.history); - - if self.planner.plan().is_complete() || self.planner.plan().is_empty() { - // Still no regions - exploration complete - log::info!("Exploration complete: all regions exhausted"); - self.state = ExplorationState::Complete; - return ExplorationStep::idle(self.state.clone()); - } - } - - // Get current region - let region = match self.planner.plan().current_region() { - Some(r) => r, - None => { - self.state = ExplorationState::Complete; - return ExplorationStep::idle(self.state.clone()); - } - }; - - // Select best frontier in current region (heading-aligned) - let selected_frontier = - self.planner - .select_frontier_in_region(region, robot_pos, current_pose.theta); - - // Find nearest CBVG node to the frontier - guaranteed reachable - // CBVG nodes are in visible areas with proper wall clearance - let (target, frontier_viewpoint) = match selected_frontier { - Some(f) => { - // Try to find nearest CBVG node (within 2m of frontier viewpoint) - let target_pos = match map.nearest_cbvg_node(f.viewpoint, 2.0) { - Some(node_pos) => node_pos, - None => f.viewpoint, // Fallback to viewpoint if no node nearby - }; - (target_pos, f.viewpoint) - } - None => { - // Current region exhausted, advance to next - log::debug!("Region {} exhausted, advancing to next", region.id); - self.planner.plan_mut().advance_region(); - continue; - } - }; - - // Check if we're already close enough to this viewpoint - let distance_to_target = robot_pos.distance(target); - if distance_to_target <= self.config.waypoint_tolerance { - // Already at this viewpoint - skip to next - log::debug!( - "Skipping frontier viewpoint at ({:.2}, {:.2}) - already there ({:.2}m <= {:.2}m)", - target.x, - target.y, - distance_to_target, - self.config.waypoint_tolerance - ); - // Mark as visited so we don't keep selecting it - self.history.mark_visited(target); - - // Remove this frontier from the current region so it won't be selected again - if let Some(region) = self.planner.plan_mut().current_region_mut() { - region - .frontiers - .retain(|f| f.viewpoint.distance(frontier_viewpoint) > 0.1); - } - - // If region is now empty, advance to next region - if self - .planner - .plan() - .current_region() - .is_none_or(|r| r.is_empty()) - { - self.planner.plan_mut().advance_region(); - } - continue; - } - - // Get excluded nodes for this frontier (nodes that led to collision before) - let excluded_nodes = self.get_collision_nodes_for_frontier(frontier_viewpoint); - - log::debug!( - "Selected frontier viewpoint at ({:.2}, {:.2}) from region {}, distance: {:.2}m, excluded nodes: {:?}", - target.x, - target.y, - region.id, - distance_to_target, - excluded_nodes - ); - - // Plan path directly to viewpoint, excluding nodes that led to collision - match map.get_path_excluding_nodes(robot_pos, target, &excluded_nodes) { - Some((path, goal_node_idx)) => { - log::debug!( - "Path planned: {} waypoints, {:.2}m total, via node {}", - path.points.len(), - path.length, - goal_node_idx - ); - - // Check for oscillation before committing to this frontier - // If oscillation is detected, suppress the "other" frontier and continue with this one - if let Some(suppress_key) = - self.record_frontier_and_check_oscillation(frontier_viewpoint) - { - log::info!( - "Oscillation detected - suppressing other frontier at grid {:?}, continuing with ({:.2}, {:.2})", - suppress_key, - frontier_viewpoint.x, - frontier_viewpoint.y - ); - // Suppress the other frontier for 10 cycles - self.suppressed_frontiers.insert(suppress_key, 10); - - // Also remove the suppressed frontier from planner's current region - // so it won't be selected again in subsequent loop iterations - if let Some(region) = self.planner.plan_mut().current_region_mut() { - let grid_cell_size = 0.5; // Same as frontier_grid_key - region.frontiers.retain(|f| { - let key = ( - (f.viewpoint.x / grid_cell_size) as i32, - (f.viewpoint.y / grid_cell_size) as i32, - ); - key != suppress_key - }); - } - // Continue with the current frontier (don't skip it) - } - - self.failed_planning_attempts = 0; - self.current_attempt_frontier = None; - - // Immediately start following the path - let result = self.path_follower.follow( - current_pose, - &path, - 0, - self.config.waypoint_tolerance, - ); - - self.state = ExplorationState::FollowingPath { - path: path.clone(), - waypoint_idx: result.waypoint_idx, - // Use frontier_viewpoint (not target/CBVG node) so collision tracking matches - target_frontier: frontier_viewpoint, - goal_node_idx: Some(goal_node_idx), // Track which node we're using - }; - - let mut step = - ExplorationStep::with_velocity(result.velocity, self.state.clone()); - step.path_planned = true; - step.target_frontier = Some(target); - step.distance_to_target = result.distance_to_waypoint; - return step; - } - None => { - log::warn!( - "Failed to plan path to frontier at ({:.2}, {:.2})", - target.x, - target.y - ); - - // Track per-frontier failures - reset counter if we switched frontiers - let is_same_frontier = self - .current_attempt_frontier - .map(|prev| prev.distance(frontier_viewpoint) < 0.1) - .unwrap_or(false); - - if is_same_frontier { - self.failed_planning_attempts += 1; - } else { - // New frontier - reset counter - self.current_attempt_frontier = Some(frontier_viewpoint); - self.failed_planning_attempts = 1; - } - - if self.failed_planning_attempts >= self.max_failed_attempts { - // Track position when we first mark any frontier as unreachable - // (so we can re-evaluate after robot moves 3m) - if self.unreachable_frontiers.is_empty() { - self.position_at_last_unreachable_clear = Some(robot_pos); - } - - // Mark this frontier as unreachable (use viewpoint, not CBVG node) - self.unreachable_frontiers.push(frontier_viewpoint); - self.failed_planning_attempts = 0; - self.current_attempt_frontier = None; - log::warn!( - "Frontier marked as unreachable after {} failed attempts", - self.max_failed_attempts - ); - - // Remove from current region and try again (use viewpoint for matching) - if let Some(region) = self.planner.plan_mut().current_region_mut() { - region - .frontiers - .retain(|f| f.viewpoint.distance(frontier_viewpoint) > 0.1); - } - continue; - } - - // Try another frontier in same region - continue; - } - } - } - } - - /// Follow the current path. - fn follow_path( - &mut self, - _map: &M, - current_pose: Pose2D, - path: Path, - waypoint_idx: usize, - target_frontier: Point2D, - goal_node_idx: Option, - ) -> ExplorationStep { - let result = self.path_follower.follow( - current_pose, - &path, - waypoint_idx, - self.config.waypoint_tolerance, - ); - - if result.path_complete { - // Path complete - go back to selecting frontier - log::debug!( - "Reached frontier at ({:.2}, {:.2})", - target_frontier.x, - target_frontier.y - ); - // Clear collision nodes for this frontier since we reached it successfully - self.clear_collision_nodes_for_frontier(target_frontier); - self.state = ExplorationState::SelectingFrontier; - - return ExplorationStep::idle(self.state.clone()); - } - - // Note: We don't check frontier existence every step for performance. - // If the frontier closes while following, we'll select a new one when - // the path completes or we get close enough. - - // Update state with new waypoint index - self.state = ExplorationState::FollowingPath { - path, - waypoint_idx: result.waypoint_idx, - target_frontier, - goal_node_idx, - }; - - let mut step = ExplorationStep::with_velocity(result.velocity, self.state.clone()); - step.target_frontier = Some(target_frontier); - step.distance_to_target = result.distance_to_waypoint; - step - } - - /// Execute backoff motion after collision. - fn execute_backoff( - &mut self, - current_pose: Pose2D, - backoff_remaining: f32, - backoff_heading: f32, - start_position: Point2D, - target_frontier: Point2D, - failed_goal_node: Option, - recovery_attempts: u32, - ) -> ExplorationStep { - // Calculate how far we've backed up - let backed_up = current_pose.position().distance(start_position); - let remaining = backoff_remaining - backed_up; - - if remaining <= 0.0 { - // Backoff complete - go back to selecting frontier - // The collision node has already been recorded in handle_collision - log::debug!( - "Backoff complete, selecting new frontier (failed node: {:?}, attempts: {})", - failed_goal_node, - recovery_attempts - ); - self.state = ExplorationState::SelectingFrontier; - return ExplorationStep::idle(self.state.clone()); - } - - // Update remaining distance - self.state = ExplorationState::RecoveringFromCollision { - backoff_remaining: remaining, - backoff_heading, - start_position, - target_frontier, - failed_goal_node, - recovery_attempts, - }; - - // Command slow reverse motion - let backup_speed = 0.1; // m/s - ExplorationStep::with_velocity(VelocityCommand::backup(backup_speed), self.state.clone()) - } - - /// Check if a frontier point is marked as unreachable. - fn is_frontier_unreachable(&self, point: Point2D) -> bool { - self.unreachable_frontiers - .iter() - .any(|p| p.distance(point) < self.frontier_match_distance) - } - - /// Get the number of unreachable frontiers. - pub fn unreachable_frontier_count(&self) -> usize { - self.unreachable_frontiers.len() - } - - /// Clear unreachable frontiers (useful after map updates). - pub fn clear_unreachable_frontiers(&mut self) { - self.unreachable_frontiers.clear(); - self.position_at_last_unreachable_clear = None; - } - - /// Get the current configuration. + /// Get configuration pub fn config(&self) -> &ExplorationConfig { &self.config } - // ───────────────────────────────────────────────────────────────────────── - // Oscillation Detection (prevents ping-pong between nearby frontiers) - // ───────────────────────────────────────────────────────────────────────── - - /// Record a frontier selection and check for oscillation or repeated visits. - /// Returns the frontier to suppress if a problematic pattern is detected. - fn record_frontier_and_check_oscillation(&mut self, frontier: Point2D) -> Option<(i32, i32)> { - let key = self.frontier_grid_key(frontier); - - // Add to recent targets (keep last 8) - self.recent_targets.push_back(key); - if self.recent_targets.len() > 8 { - self.recent_targets.pop_front(); + /// Get exploration progress + pub fn progress(&self) -> ExplorationProgress { + ExplorationProgress { + state: self.state.name().to_string(), + frontiers_explored: self.frontiers_explored, + distance_traveled: self.distance_traveled, + consecutive_failures: self.consecutive_failures, + is_active: self.state.is_active(), + is_complete: matches!(self.state, ExplorationState::Complete), } + } - let targets: Vec<_> = self.recent_targets.iter().collect(); - let len = targets.len(); + /// Start exploration + pub fn start(&mut self) { + if matches!(self.state, ExplorationState::Idle) { + self.state = ExplorationState::SearchingFrontiers; + self.consecutive_failures = 0; + } + } - // Check 1: A-B-A-B pattern (oscillation between two frontiers) - if len >= 4 { - let a = targets[len - 4]; - let b = targets[len - 3]; - let c = targets[len - 2]; - let d = targets[len - 1]; // This is the current selection (key) + /// Stop exploration + pub fn stop(&mut self) { + self.state = ExplorationState::Idle; + } - // Pattern: A, B, A, B (same A and B repeating) - if a == c && b == d && a != b { - log::warn!( - "Oscillation detected between frontiers at grid {:?} and {:?}", - a, - b + /// Update controller with current pose and map + /// + /// Returns the command to execute + pub fn update(&mut self, pose: Pose2D, storage: &GridStorage) -> ExplorationCommand { + // Track distance traveled + let dist = pose.position().distance(&self.last_pose.position()); + if dist > 0.01 { + // Only count movement > 1cm + self.distance_traveled += dist; + } + self.last_pose = pose; + + match &self.state { + ExplorationState::Idle => ExplorationCommand::None, + + ExplorationState::SearchingFrontiers => self.search_frontiers(pose, storage), + + ExplorationState::Planning { target } => self.plan_path(pose, target.clone(), storage), + + ExplorationState::Navigating { + path, + current_waypoint, + target, + } => self.navigate( + pose, + path.clone(), + *current_waypoint, + target.clone(), + storage, + ), + + ExplorationState::Scanning { + position, + rotation_remaining, + } => self.handle_scanning(*position, *rotation_remaining, pose), + + ExplorationState::Recovering { action, attempts } => { + self.handle_recovery(action.clone(), *attempts) + } + + ExplorationState::Complete => ExplorationCommand::ExplorationComplete, + + ExplorationState::Failed { reason } => ExplorationCommand::ExplorationFailed { + reason: reason.clone(), + }, + } + } + + /// Handle an external event + pub fn handle_event(&mut self, event: ExplorationEvent) { + match event { + ExplorationEvent::Start => self.start(), + ExplorationEvent::Stop => self.stop(), + ExplorationEvent::RecoveryComplete => { + // Go back to searching after recovery + self.state = ExplorationState::SearchingFrontiers; + } + ExplorationEvent::ScanComplete => { + // Finished scanning, look for next frontier + self.frontiers_explored += 1; + self.consecutive_failures = 0; + self.state = ExplorationState::SearchingFrontiers; + } + ExplorationEvent::ObstacleDetected | ExplorationEvent::PathBlocked => { + self.enter_recovery(); + } + _ => {} + } + } + + // Private state handlers + + fn search_frontiers(&mut self, pose: Pose2D, storage: &GridStorage) -> ExplorationCommand { + let detector = FrontierDetector::with_min_size(self.config.min_frontier_size); + let robot_grid = storage.world_to_grid(pose.position()); + + // Get all frontiers and filter out recently failed ones + let frontiers = detector.detect_frontiers(storage); + let total_frontiers = frontiers.len(); + let blacklisted_count = frontiers + .iter() + .filter(|f| self.is_recently_failed_frontier(&f.centroid_world)) + .count(); + + let valid_frontiers: Vec<_> = frontiers + .into_iter() + .filter(|f| !self.is_recently_failed_frontier(&f.centroid_world)) + .collect(); + + debug!( + "[Explore] SearchingFrontiers: {} total, {} blacklisted, {} valid", + total_frontiers, + blacklisted_count, + valid_frontiers.len() + ); + + // Find the best frontier from the valid ones + let best = valid_frontiers.into_iter().max_by(|a, b| { + let distance_a = robot_grid.manhattan_distance(&a.centroid_grid) as f32; + let distance_b = robot_grid.manhattan_distance(&b.centroid_grid) as f32; + let score_a = if distance_a > 0.0 { + a.exploration_score() / (1.0 + distance_a * 0.1) + } else { + a.exploration_score() * 2.0 + }; + let score_b = if distance_b > 0.0 { + b.exploration_score() / (1.0 + distance_b * 0.1) + } else { + b.exploration_score() * 2.0 + }; + score_a.total_cmp(&score_b) + }); + + if let Some(frontier) = best { + let distance = robot_grid.manhattan_distance(&frontier.centroid_grid); + info!( + "[Explore] Selected frontier: ({:.2}, {:.2}), size={}, score={:.1}, dist={}", + frontier.centroid_world.x, + frontier.centroid_world.y, + frontier.size, + frontier.exploration_score(), + distance + ); + // Reset stuck detection when starting to plan for a new frontier + self.steps_without_progress = 0; + self.stuck_check_position = pose.position(); + self.state = ExplorationState::Planning { target: frontier }; + ExplorationCommand::None + } else { + // Check if map has enough explored area before declaring complete + // If we have very few floor cells, we haven't really started exploring yet + let counts = storage.count_by_type(); + let resolution = storage.resolution(); + let explored_area_m2 = counts.floor as f32 * resolution * resolution; + + if counts.floor < 100 { + // Not enough explored yet - wait for more sensor data + debug!( + "[Explore] No valid frontiers, but only {} floor cells ({:.2}m²) - waiting", + counts.floor, explored_area_m2 ); + ExplorationCommand::None + } else { + // No frontiers and we have explored a reasonable amount - complete + info!( + "[Explore] COMPLETE: No frontiers remaining. Floor={} ({:.2}m²), Wall={}, Unknown={}", + counts.floor, explored_area_m2, counts.wall, counts.unknown + ); + self.state = ExplorationState::Complete; + ExplorationCommand::ExplorationComplete + } + } + } - // Copy the key to suppress before clearing - let suppress_key = *c; - self.recent_targets.clear(); - return Some(suppress_key); + /// Check if a frontier centroid is in the recently failed list + fn is_recently_failed_frontier(&self, centroid: &WorldPoint) -> bool { + const MATCH_THRESHOLD: f32 = 0.5; // 50cm tolerance + self.failed_frontier_centroids + .iter() + .any(|failed| centroid.distance(failed) < MATCH_THRESHOLD) + } + + fn plan_path( + &mut self, + pose: Pose2D, + target: Frontier, + storage: &GridStorage, + ) -> ExplorationCommand { + let astar_config = AStarConfig { + footprint: self.config.footprint.clone(), + clearance_weight: self.config.path_clearance_weight, + exploration_mode: true, // Allow Unknown cells in footprint during exploration + ..Default::default() + }; + + let robot_pos = pose.position(); + let robot_grid = storage.world_to_grid(robot_pos); + + // Try to find a valid observation point for this frontier + let goal_world = match self.find_observation_point(&target, robot_pos, robot_grid, storage) + { + Some(goal) => goal, + None => { + // No valid observation point found - blacklist this frontier but don't + // count as a "real" failure (it's a goal selection issue, not pathfinding) + debug!( + "[Explore] No valid observation point for frontier at ({:.2},{:.2}), skipping", + target.centroid_world.x, target.centroid_world.y + ); + self.failed_frontier_centroids.push(target.centroid_world); + const MAX_FAILED_FRONTIERS: usize = 20; + if self.failed_frontier_centroids.len() > MAX_FAILED_FRONTIERS { + self.failed_frontier_centroids.remove(0); + } + self.state = ExplorationState::SearchingFrontiers; + return ExplorationCommand::None; + } + }; + + trace!( + "[Explore] Planning: robot=({:.2},{:.2}) → goal=({:.2},{:.2}), frontier=({:.2},{:.2})", + robot_pos.x, + robot_pos.y, + goal_world.x, + goal_world.y, + target.centroid_world.x, + target.centroid_world.y + ); + + let planner = AStarPlanner::new(storage, astar_config.clone()); + let result = planner.find_path_world(robot_pos, goal_world); + + if result.success { + // Reset failure counts on successful planning + self.consecutive_failures = 0; + self.start_blocked_count = 0; + + let path_length_m = result.length_meters(); + let path = if self.config.smooth_paths { + let smoother_config = SmoothingConfig { + footprint: self.config.footprint.clone(), + ..Default::default() + }; + let smoother = PathSmoother::new(storage, smoother_config); + smoother.smooth(&result.path_world) + } else { + result.path_world + }; + + info!( + "[Explore] Planning SUCCESS: path length={} waypoints, {:.2}m", + path.len(), + path_length_m + ); + + self.state = ExplorationState::Navigating { + path, + current_waypoint: 0, + target, + }; + ExplorationCommand::None + } else { + // Planning failed - handle different failure types + let failure_reason = result.failure_reason.clone(); + + // StartBlocked: Robot's current position is considered blocked. + // This typically happens due to localization drift - trigger recovery + // to move the robot to a safer position. + let is_start_blocked = matches!(failure_reason, Some(PathFailure::StartBlocked)); + if is_start_blocked { + self.start_blocked_count += 1; + + // After too many consecutive StartBlocked events, the robot is likely stuck + // due to localization drift. Check if we've explored enough to call it complete. + const MAX_START_BLOCKED: usize = 10; + if self.start_blocked_count >= MAX_START_BLOCKED { + let counts = storage.count_by_type(); + let explored_area_m2 = + counts.floor as f32 * storage.resolution() * storage.resolution(); + + if explored_area_m2 > 10.0 && self.frontiers_explored > 0 { + info!( + "[Explore] COMPLETE (localization drift): explored {:.2}m², {} frontiers done after {} StartBlocked events", + explored_area_m2, self.frontiers_explored, self.start_blocked_count + ); + self.state = ExplorationState::Complete; + return ExplorationCommand::ExplorationComplete; + } else { + warn!( + "[Explore] {} consecutive StartBlocked events, only {:.2}m² explored - entering recovery", + self.start_blocked_count, explored_area_m2 + ); + } + } + + debug!( + "[Explore] StartBlocked at robot position ({}/{})", + self.start_blocked_count, MAX_START_BLOCKED + ); + self.enter_recovery(); + return ExplorationCommand::Stop; + } else { + // Reset counter on non-StartBlocked events + self.start_blocked_count = 0; + } + + // GoalBlocked means our observation point selection failed (shouldn't happen after + // find_observation_point, but handle it gracefully). Don't count as consecutive failure. + let is_goal_blocked = matches!(failure_reason, Some(PathFailure::GoalBlocked)); + + if is_goal_blocked { + debug!( + "[Explore] Goal blocked at ({:.2},{:.2}), blacklisting frontier", + goal_world.x, goal_world.y + ); + } else { + warn!( + "[Explore] Planning FAILED to ({:.2},{:.2}): {:?}, failures={}/{}", + goal_world.x, + goal_world.y, + failure_reason, + self.consecutive_failures + 1, + self.config.max_consecutive_failures + ); + self.consecutive_failures += 1; + } + + self.failed_frontier_centroids.push(target.centroid_world); + const MAX_FAILED_FRONTIERS: usize = 20; + if self.failed_frontier_centroids.len() > MAX_FAILED_FRONTIERS { + self.failed_frontier_centroids.remove(0); + } + + if self.consecutive_failures >= self.config.max_consecutive_failures { + // Before failing, check if we've explored a significant area + let counts = storage.count_by_type(); + let explored_area_m2 = + counts.floor as f32 * storage.resolution() * storage.resolution(); + + if explored_area_m2 > 10.0 && self.frontiers_explored > 0 { + info!( + "[Explore] COMPLETE (planning failures): explored {:.2}m², {} frontiers done", + explored_area_m2, self.frontiers_explored + ); + self.state = ExplorationState::Complete; + return ExplorationCommand::ExplorationComplete; + } else if explored_area_m2 > 20.0 { + info!( + "[Explore] COMPLETE (large area): explored {:.2}m² with {} frontiers", + explored_area_m2, self.frontiers_explored + ); + self.state = ExplorationState::Complete; + return ExplorationCommand::ExplorationComplete; + } else { + warn!( + "[Explore] FAILED: {} consecutive planning failures, only {:.2}m² explored", + self.consecutive_failures, explored_area_m2 + ); + self.state = ExplorationState::Failed { + reason: "Too many planning failures".to_string(), + }; + } + } else { + self.state = ExplorationState::SearchingFrontiers; + } + ExplorationCommand::None + } + } + + /// Find a valid observation point for a frontier. + /// + /// Tries multiple candidate positions near the frontier and returns the first + /// one that is traversable (robot can fit there without hitting obstacles). + fn find_observation_point( + &self, + target: &Frontier, + robot_pos: WorldPoint, + robot_grid: crate::core::GridCoord, + storage: &GridStorage, + ) -> Option { + let checker = TraversabilityChecker::new(storage, self.config.footprint.clone()); + let offset_distance = self.config.footprint.total_radius() + storage.resolution() * 2.0; + + // Get the closest frontier cell to the robot + let frontier_world = target + .closest_cell_to(robot_grid) + .map(|c| storage.grid_to_world(c.coord)) + .unwrap_or(target.centroid_world); + + // Generate candidate observation points + let candidates = self.generate_observation_candidates( + frontier_world, + robot_pos, + offset_distance, + storage, + ); + + // Find the first valid candidate (traversable in exploration mode) + for candidate in candidates { + if checker.is_position_safe_mode(candidate, true) { + trace!( + "[Explore] Found valid observation point at ({:.2},{:.2})", + candidate.x, candidate.y + ); + return Some(candidate); } } - // Check 2: Same frontier visited 3+ times in last 6 selections (stuck on one frontier) - if len >= 6 { - let current = targets[len - 1]; - let count = targets[len - 6..].iter().filter(|&&t| t == current).count(); + // If no candidate worked, try frontier cells directly + // (the robot might be able to observe from multiple cells in the frontier) + for cell in target.cells.iter().take(10) { + let cell_world = storage.grid_to_world(cell.coord); + let dx = robot_pos.x - cell_world.x; + let dy = robot_pos.y - cell_world.y; + let dist = (dx * dx + dy * dy).sqrt(); - if count >= 3 { - log::warn!( - "Repeated visits detected: frontier at grid {:?} visited {} times in last 6", - current, - count - ); + if dist > offset_distance { + let ratio = offset_distance / dist; + let candidate = + WorldPoint::new(cell_world.x + dx * ratio, cell_world.y + dy * ratio); - // Suppress this frontier since we keep returning to it - let suppress_key = *current; - self.recent_targets.clear(); - return Some(suppress_key); - } - } - - // Check 3: Three frontiers in rotation (A-B-C-A-B-C pattern) - if len >= 6 { - let a = targets[len - 6]; - let b = targets[len - 5]; - let c = targets[len - 4]; - let d = targets[len - 3]; - let e = targets[len - 2]; - let f = targets[len - 1]; - - // Pattern: A, B, C, A, B, C - if a == d && b == e && c == f && a != b && b != c && a != c { - log::warn!("Three-way rotation detected: {:?}, {:?}, {:?}", a, b, c); - - // Suppress the oldest frontier in the pattern - let suppress_key = *a; - self.recent_targets.clear(); - return Some(suppress_key); + if checker.is_position_safe_mode(candidate, true) { + trace!( + "[Explore] Found valid observation point from cell at ({:.2},{:.2})", + candidate.x, candidate.y + ); + return Some(candidate); + } } } None } - /// Check if a frontier is currently suppressed due to oscillation. - fn is_frontier_suppressed(&self, frontier: Point2D) -> bool { - let key = self.frontier_grid_key(frontier); - self.suppressed_frontiers.get(&key).copied().unwrap_or(0) > 0 + /// Generate candidate observation points around a frontier. + fn generate_observation_candidates( + &self, + frontier_world: WorldPoint, + robot_pos: WorldPoint, + offset_distance: f32, + _storage: &GridStorage, + ) -> Vec { + let mut candidates = Vec::with_capacity(9); + + let dx = robot_pos.x - frontier_world.x; + let dy = robot_pos.y - frontier_world.y; + let dist = (dx * dx + dy * dy).sqrt(); + + // Primary candidate: offset toward robot + if dist > offset_distance { + let ratio = offset_distance / dist; + candidates.push(WorldPoint::new( + frontier_world.x + dx * ratio, + frontier_world.y + dy * ratio, + )); + } else if dist > 0.01 { + // Robot is close, just move slightly toward robot + candidates.push(robot_pos); + } + + // Additional candidates at different angles around the frontier + let base_angle = dy.atan2(dx); + let angles = [ + 0.0, + std::f32::consts::FRAC_PI_4, + -std::f32::consts::FRAC_PI_4, + std::f32::consts::FRAC_PI_2, + -std::f32::consts::FRAC_PI_2, + std::f32::consts::FRAC_PI_4 * 3.0, + -std::f32::consts::FRAC_PI_4 * 3.0, + std::f32::consts::PI, + ]; + + for angle_offset in angles { + let angle = base_angle + angle_offset; + candidates.push(WorldPoint::new( + frontier_world.x + offset_distance * angle.cos(), + frontier_world.y + offset_distance * angle.sin(), + )); + } + + candidates } - /// Decrement suppression counters (call once per frontier selection cycle). - fn decay_suppressed_frontiers(&mut self) { - self.suppressed_frontiers.retain(|_, count| { - *count = count.saturating_sub(1); - *count > 0 - }); - } + fn navigate( + &mut self, + pose: Pose2D, + path: Vec, + current_waypoint: usize, + target: Frontier, + _storage: &GridStorage, + ) -> ExplorationCommand { + if current_waypoint >= path.len() { + // Reached end of path - start scanning rotation + self.state = ExplorationState::Scanning { + position: pose.position(), + rotation_remaining: std::f32::consts::TAU, // Full 360° rotation + }; + return ExplorationCommand::Stop; + } - // ───────────────────────────────────────────────────────────────────────── - // Collision Node Tracking (for alternative approach angles) - // ───────────────────────────────────────────────────────────────────────── + let waypoint = path[current_waypoint]; + let distance = pose.position().distance(&waypoint); - /// Convert a frontier point to a grid key for collision tracking. - /// Uses 0.5m grid cells to group nearby frontiers. - fn frontier_grid_key(&self, p: Point2D) -> (i32, i32) { - ((p.x / 0.5) as i32, (p.y / 0.5) as i32) - } + // Check if waypoint reached + if distance < self.config.waypoint_threshold { + // Move to next waypoint + let next_waypoint = current_waypoint + 1; - /// Record a failed CBVG goal node for a frontier. - /// This node led to a collision when trying to reach the frontier. - fn add_collision_node_for_frontier(&mut self, frontier: Point2D, node_idx: usize) { - let key = self.frontier_grid_key(frontier); - let nodes = self.collision_nodes.entry(key).or_default(); - if !nodes.contains(&node_idx) { - nodes.push(node_idx); - log::debug!( - "Recorded failed node {} for frontier at ({:.2}, {:.2})", - node_idx, - frontier.x, - frontier.y - ); + // Check if we've reached the frontier (within threshold of centroid) + let distance_to_target = pose.position().distance(&target.centroid_world); + if distance_to_target < self.config.frontier_reached_threshold { + self.state = ExplorationState::Scanning { + position: pose.position(), + rotation_remaining: std::f32::consts::TAU, + }; + return ExplorationCommand::Stop; + } + + if next_waypoint < path.len() { + let next_target = path[next_waypoint]; + self.state = ExplorationState::Navigating { + path, + current_waypoint: next_waypoint, + target, + }; + return ExplorationCommand::MoveTo { + target: next_target, + max_speed: 0.3, // 30 cm/s default + }; + } else { + // Reached end of path - transition to Scanning + // (we're close enough to observe the frontier) + self.state = ExplorationState::Scanning { + position: pose.position(), + rotation_remaining: std::f32::consts::TAU, + }; + return ExplorationCommand::Stop; + } + } + + // Stuck detection: only check when robot should be moving forward + // Calculate angle to waypoint + let dx = waypoint.x - pose.x; + let dy = waypoint.y - pose.y; + let target_angle = dy.atan2(dx); + let angle_error = (target_angle - pose.theta).abs(); + // Normalize to [0, PI] + let angle_error = if angle_error > std::f32::consts::PI { + std::f32::consts::TAU - angle_error + } else { + angle_error + }; + + // Only check stuck if angle is small (robot should be moving forward) + if angle_error < 0.5 { + // ~30 degrees + let progress = pose.position().distance(&self.stuck_check_position); + if progress > 0.05 { + // Made meaningful progress (>5cm) - reset stuck counter + self.steps_without_progress = 0; + self.stuck_check_position = pose.position(); + } else { + self.steps_without_progress += 1; + } + + // If stuck for too long (~5 seconds at 10Hz), trigger recovery + const MAX_STUCK_STEPS: usize = 50; + if self.steps_without_progress > MAX_STUCK_STEPS { + warn!( + "[Explore] Stuck detected! {} steps without progress at ({:.2},{:.2})", + self.steps_without_progress, pose.x, pose.y + ); + self.steps_without_progress = 0; + // Add this frontier to failed list and try recovery + self.failed_frontier_centroids.push(target.centroid_world); + self.enter_recovery(); + return ExplorationCommand::Stop; + } + } else { + // Robot is rotating to face waypoint - reset stuck counter + self.steps_without_progress = 0; + } + + // Continue to current waypoint + ExplorationCommand::MoveTo { + target: waypoint, + max_speed: 0.3, } } - /// Get failed CBVG goal nodes for a frontier. - /// Returns nodes that should be excluded when planning paths to this frontier. - fn get_collision_nodes_for_frontier(&self, frontier: Point2D) -> Vec { - let key = self.frontier_grid_key(frontier); - self.collision_nodes.get(&key).cloned().unwrap_or_default() + fn handle_scanning( + &mut self, + position: WorldPoint, + rotation_remaining: f32, + pose: Pose2D, + ) -> ExplorationCommand { + // Rotate to scan the area - each update rotates by a fixed amount based on angular speed + // Assuming ~0.5 rad/s angular speed and ~0.05s update rate = 0.025 rad per update + const ROTATION_PER_STEP: f32 = 0.1; // ~6 degrees per step + + let new_remaining = rotation_remaining - ROTATION_PER_STEP; + + if new_remaining <= 0.0 { + // Scan complete - frontier successfully explored + self.frontiers_explored += 1; + self.consecutive_failures = 0; + self.failed_frontier_centroids.clear(); // Reset on successful exploration + info!( + "[Explore] Scan complete at ({:.2},{:.2}), frontiers_explored={}", + position.x, position.y, self.frontiers_explored + ); + self.state = ExplorationState::SearchingFrontiers; + ExplorationCommand::None + } else { + // Continue rotating + self.state = ExplorationState::Scanning { + position, + rotation_remaining: new_remaining, + }; + ExplorationCommand::Rotate { + target_heading: pose.theta + ROTATION_PER_STEP, + max_angular_speed: 0.5, + } + } } - /// Clear collision nodes for a frontier (called when frontier is successfully reached). - fn clear_collision_nodes_for_frontier(&mut self, frontier: Point2D) { - let key = self.frontier_grid_key(frontier); - self.collision_nodes.remove(&key); + fn handle_recovery(&mut self, action: RecoveryAction, attempts: usize) -> ExplorationCommand { + debug!( + "[Explore] Recovery: {:?}, attempt {}/3", + action, + attempts + 1 + ); + + if attempts >= 3 { + // Too many recovery attempts + self.consecutive_failures += 1; + warn!( + "[Explore] Recovery exhausted after 3 attempts, failures={}/{}", + self.consecutive_failures, self.config.max_consecutive_failures + ); + if self.consecutive_failures >= self.config.max_consecutive_failures { + self.state = ExplorationState::Failed { + reason: "Too many recovery failures".to_string(), + }; + } else { + self.state = ExplorationState::SearchingFrontiers; + } + return ExplorationCommand::None; + } + + match action { + RecoveryAction::BackUp(distance) => { + // After backing up, try turning + self.state = ExplorationState::Recovering { + action: RecoveryAction::TurnInPlace(self.config.recovery_turn_angle), + attempts: attempts + 1, + }; + ExplorationCommand::MoveTo { + target: WorldPoint::new( + self.last_pose.x - distance * self.last_pose.theta.cos(), + self.last_pose.y - distance * self.last_pose.theta.sin(), + ), + max_speed: 0.1, + } + } + RecoveryAction::TurnInPlace(angle) => { + // After turning, go back to searching + self.state = ExplorationState::SearchingFrontiers; + ExplorationCommand::Rotate { + target_heading: self.last_pose.theta + angle, + max_angular_speed: 0.5, + } + } + RecoveryAction::Wait => { + // Just wait, then try again + self.state = ExplorationState::SearchingFrontiers; + ExplorationCommand::None + } + } } + + fn enter_recovery(&mut self) { + self.state = ExplorationState::Recovering { + action: RecoveryAction::BackUp(self.config.recovery_backup_distance), + attempts: 0, + }; + } +} + +/// Exploration progress tracking +#[derive(Clone, Debug, Default)] +pub struct ExplorationProgress { + /// Current state name + pub state: String, + /// Number of frontiers explored + pub frontiers_explored: usize, + /// Total distance traveled (meters) + pub distance_traveled: f32, + /// Current consecutive failure count + pub consecutive_failures: usize, + /// Is exploration active? + pub is_active: bool, + /// Is exploration complete? + pub is_complete: bool, } #[cfg(test)] mod tests { use super::*; - use crate::Occupancy; - use crate::core::Bounds; - use crate::features::Line2D; + use crate::core::{CellType, GridCoord}; - /// Mock map for testing - struct MockMap { - frontiers: Vec, - lines: Vec, + fn create_test_storage() -> GridStorage { + GridStorage::centered(50, 50, 0.1) // 5m x 5m at 10cm resolution } - impl MockMap { - fn new() -> Self { - Self { - frontiers: Vec::new(), - lines: Vec::new(), + fn fill_floor_with_frontiers(storage: &mut GridStorage) { + // Create a floor area with unknown edges (frontiers) + for x in 10..40 { + for y in 10..40 { + storage.set_type(GridCoord::new(x, y), CellType::Floor); + } + } + } + + #[test] + fn test_controller_creation() { + let controller = ExplorationController::with_defaults(); + assert!(matches!(controller.state(), ExplorationState::Idle)); + } + + #[test] + fn test_start_exploration() { + let mut controller = ExplorationController::with_defaults(); + controller.start(); + assert!(matches!( + controller.state(), + ExplorationState::SearchingFrontiers + )); + } + + #[test] + fn test_stop_exploration() { + let mut controller = ExplorationController::with_defaults(); + controller.start(); + controller.stop(); + assert!(matches!(controller.state(), ExplorationState::Idle)); + } + + #[test] + fn test_exploration_finds_frontiers() { + let mut storage = create_test_storage(); + fill_floor_with_frontiers(&mut storage); + + let config = ExplorationConfig { + footprint: crate::query::RobotFootprint::new(0.05, 0.01), + ..Default::default() + }; + let mut controller = ExplorationController::new(config); + + controller.start(); + + // Update with a pose inside the floor area + let pose = Pose2D::new(2.0, 2.0, 0.0); + let _cmd = controller.update(pose, &storage); + + // Should be planning or navigating after finding frontier + assert!( + matches!(controller.state(), ExplorationState::Planning { .. }) + || matches!(controller.state(), ExplorationState::Navigating { .. }) + ); + } + + #[test] + fn test_exploration_completes_when_no_frontiers() { + let mut storage = create_test_storage(); + // Larger fully explored area (no frontiers - all cells are known) + // Need >100 floor cells for exploration to complete + let center = storage.world_to_grid(WorldPoint::ZERO); + for dx in -6..=6 { + for dy in -6..=6 { + let coord = center + GridCoord::new(dx, dy); + storage.set_type(coord, CellType::Floor); } } - fn with_frontier(mut self, x: f32, y: f32) -> Self { - // Create a frontier with viewpoint at (x, y) - // In real usage, the viewpoint is computed from line endpoints - self.frontiers.push(Frontier { - viewpoint: Point2D::new(x, y), - look_direction: Point2D::new(1.0, 0.0), // Default: looking right - endpoint: Point2D::new(x - 0.4, y), // Endpoint is offset back - line_idx: 0, - estimated_area: 4.0, - }); - self - } - } - - impl Map for MockMap { - fn observe( - &mut self, - _scan: &crate::core::PointCloud2D, - _odometry: Pose2D, - ) -> crate::ObserveResult { - unimplemented!() - } - - fn raycast(&self, _from: Point2D, _direction: Point2D, max_range: f32) -> f32 { - max_range - } - - fn query(&self, _point: Point2D) -> Occupancy { - Occupancy::Free - } - - fn frontiers(&self) -> Vec { - self.frontiers.clone() - } - - fn get_path(&self, from: Point2D, to: Point2D) -> Option { - // Simple direct path - Some(Path { - points: vec![from, to], - length: from.distance(to), - }) - } - - fn get_path_excluding_nodes( - &self, - from: Point2D, - to: Point2D, - _excluded_goal_nodes: &[usize], - ) -> Option<(Path, usize)> { - // Simple direct path, return node index 0 - Some(( - Path { - points: vec![from, to], - length: from.distance(to), - }, - 0, - )) - } - - fn nearest_cbvg_node(&self, target: Point2D, _max_distance: f32) -> Option { - // For testing, just return the target point - Some(target) - } - - fn is_path_clear(&self, _from: Point2D, _to: Point2D) -> bool { - true - } - - fn bounds(&self) -> Bounds { - Bounds::new(Point2D::new(-10.0, -10.0), Point2D::new(10.0, 10.0)) - } - - fn clear(&mut self) { - self.frontiers.clear(); - self.lines.clear(); - } - } - - #[test] - fn test_controller_initial_state() { - let config = ExplorationConfig::default(); - let controller = ExplorationController::new(config); - - assert!(matches!(controller.state(), ExplorationState::Idle)); - assert!(!controller.is_active()); - assert!(!controller.is_complete()); - } - - #[test] - fn test_controller_start() { - let config = ExplorationConfig::default(); + let config = ExplorationConfig { + footprint: crate::query::RobotFootprint::new(0.05, 0.01), + // Very large min size means no frontiers found (floor area is ~13x13 = 169 cells, + // frontier edges would be ~48 cells, so 100 ensures none qualify) + min_frontier_size: 100, + ..Default::default() + }; let mut controller = ExplorationController::new(config); controller.start(); + let pose = Pose2D::new(0.0, 0.0, 0.0); + let cmd = controller.update(pose, &storage); - assert!(matches!( - controller.state(), - ExplorationState::SelectingFrontier - )); - assert!(controller.is_active()); + assert!(matches!(controller.state(), ExplorationState::Complete)); + assert!(matches!(cmd, ExplorationCommand::ExplorationComplete)); } #[test] - fn test_exploration_complete_no_frontiers() { - let config = ExplorationConfig::default(); - let mut controller = ExplorationController::new(config); - controller.start(); + fn test_exploration_progress() { + let controller = ExplorationController::with_defaults(); + let progress = controller.progress(); - let map = MockMap::new(); // No frontiers - let pose = Pose2D::identity(); - - let step = controller.update(&map, pose, None); - - assert!(controller.is_complete()); - assert!(step.velocity.is_none()); - } - - #[test] - fn test_exploration_selects_frontier() { - let config = ExplorationConfig::default(); - let mut controller = ExplorationController::new(config); - controller.start(); - - let map = MockMap::new().with_frontier(2.0, 0.0); - let pose = Pose2D::identity(); - - let step = controller.update(&map, pose, None); - - assert!(step.path_planned); - assert!(step.target_frontier.is_some()); - assert!(matches!( - controller.state(), - ExplorationState::FollowingPath { .. } - )); - } - - #[test] - fn test_collision_triggers_recovery() { - let config = ExplorationConfig::default(); - let mut controller = ExplorationController::new(config); - controller.start(); - - let map = MockMap::new().with_frontier(2.0, 0.0); - let pose = Pose2D::identity(); - - // First update to start following path - controller.update(&map, pose, None); - - // Simulate collision - let collision = CollisionEvent::new( - super::super::collision::CollisionType::BumperBoth, - Point2D::new(0.5, 0.0), - 0.0, - ); - - let step = controller.update(&map, pose, Some(collision)); - - assert!(step.virtual_wall.is_some()); - assert!(matches!( - controller.state(), - ExplorationState::RecoveringFromCollision { .. } - )); - } - - #[test] - fn test_reset() { - let config = ExplorationConfig::default(); - let mut controller = ExplorationController::new(config); - controller.start(); - - let map = MockMap::new().with_frontier(2.0, 0.0); - let pose = Pose2D::identity(); - - controller.update(&map, pose, None); - controller.reset(); - - assert!(matches!(controller.state(), ExplorationState::Idle)); - assert!(!controller.is_active()); + assert_eq!(progress.state, "Idle"); + assert_eq!(progress.frontiers_explored, 0); + assert!(!progress.is_active); + assert!(!progress.is_complete); } } diff --git a/vastu-map/src/exploration/history.rs b/vastu-map/src/exploration/history.rs deleted file mode 100644 index db975c9..0000000 --- a/vastu-map/src/exploration/history.rs +++ /dev/null @@ -1,264 +0,0 @@ -//! Exploration history tracking. -//! -//! Tracks visited areas using a spatial grid to avoid revisiting -//! recently explored regions. - -use std::collections::{HashMap, VecDeque}; - -use crate::core::Point2D; - -/// Tracks exploration history using a spatial grid. -/// -/// Used to penalize revisiting recently explored areas during -/// region selection and frontier scoring. -#[derive(Clone, Debug)] -pub struct ExplorationHistory { - /// Grid of visited cells mapping (cell_x, cell_y) -> visit count. - visited_grid: HashMap<(i32, i32), u8>, - - /// Cell size for the grid (meters). - cell_size: f32, - - /// Recently visited positions (ring buffer). - recent_positions: VecDeque, - - /// Maximum number of recent positions to track. - max_recent: usize, -} - -impl ExplorationHistory { - /// Create a new exploration history. - /// - /// # Arguments - /// * `cell_size` - Size of each grid cell in meters (default: 0.5) - /// * `max_recent` - Maximum number of recent positions to track - pub fn new(cell_size: f32, max_recent: usize) -> Self { - Self { - visited_grid: HashMap::with_capacity(1000), - cell_size, - recent_positions: VecDeque::with_capacity(max_recent), - max_recent, - } - } - - /// Create with default parameters. - pub fn with_defaults() -> Self { - Self::new(0.5, 100) - } - - /// Mark a position as visited. - pub fn mark_visited(&mut self, position: Point2D) { - // Update grid cell - let cell = self.point_to_cell(position); - let count = self.visited_grid.entry(cell).or_insert(0); - *count = count.saturating_add(1); - - // Update recent positions - self.recent_positions.push_back(position); - if self.recent_positions.len() > self.max_recent { - self.recent_positions.pop_front(); - } - } - - /// Check if a position is in a recently visited area. - /// - /// Returns true if the position is within the same grid cell - /// as any recent position. - pub fn is_recently_visited(&self, position: Point2D) -> bool { - let cell = self.point_to_cell(position); - self.visited_grid.get(&cell).copied().unwrap_or(0) > 0 - } - - /// Check if an area around a position has been recently visited. - /// - /// Checks a 3x3 neighborhood of cells around the position. - pub fn is_area_recently_visited(&self, position: Point2D) -> bool { - let (cx, cy) = self.point_to_cell(position); - - for dx in -1..=1 { - for dy in -1..=1 { - let cell = (cx + dx, cy + dy); - if self.visited_grid.get(&cell).copied().unwrap_or(0) > 0 { - return true; - } - } - } - - false - } - - /// Get the visit count for a position. - pub fn get_visit_count(&self, position: Point2D) -> u8 { - let cell = self.point_to_cell(position); - self.visited_grid.get(&cell).copied().unwrap_or(0) - } - - /// Get the total number of cells visited. - pub fn cells_visited(&self) -> usize { - self.visited_grid.len() - } - - /// Get the number of recent positions tracked. - pub fn recent_count(&self) -> usize { - self.recent_positions.len() - } - - /// Get the distance to the nearest recent position. - /// - /// Returns None if there are no recent positions. - pub fn distance_to_nearest_recent(&self, position: Point2D) -> Option { - self.recent_positions - .iter() - .map(|&p| position.distance(p)) - .min_by(|a, b| a.partial_cmp(b).unwrap_or(std::cmp::Ordering::Equal)) - } - - /// Clear all history. - pub fn clear(&mut self) { - self.visited_grid.clear(); - self.recent_positions.clear(); - } - - /// Convert a point to a grid cell coordinate. - #[inline] - fn point_to_cell(&self, point: Point2D) -> (i32, i32) { - ( - (point.x / self.cell_size).floor() as i32, - (point.y / self.cell_size).floor() as i32, - ) - } - - /// Get estimated memory usage in bytes. - pub fn memory_usage(&self) -> usize { - // HashMap overhead + entries - let grid_size = self.visited_grid.len() - * (std::mem::size_of::<(i32, i32)>() + std::mem::size_of::() + 16); - // VecDeque + Point2D entries - let recent_size = self.recent_positions.len() * std::mem::size_of::(); - - grid_size + recent_size - } -} - -impl Default for ExplorationHistory { - fn default() -> Self { - Self::with_defaults() - } -} - -#[cfg(test)] -mod tests { - use super::*; - - #[test] - fn test_empty_history() { - let history = ExplorationHistory::new(0.5, 100); - assert_eq!(history.cells_visited(), 0); - assert_eq!(history.recent_count(), 0); - assert!(!history.is_recently_visited(Point2D::new(0.0, 0.0))); - } - - #[test] - fn test_mark_visited() { - let mut history = ExplorationHistory::new(0.5, 100); - - history.mark_visited(Point2D::new(1.0, 1.0)); - - assert_eq!(history.cells_visited(), 1); - assert_eq!(history.recent_count(), 1); - assert!(history.is_recently_visited(Point2D::new(1.0, 1.0))); - assert!(history.is_recently_visited(Point2D::new(1.2, 1.2))); // Same cell - assert!(!history.is_recently_visited(Point2D::new(5.0, 5.0))); // Different cell - } - - #[test] - fn test_visit_count() { - let mut history = ExplorationHistory::new(0.5, 100); - - // Visit same cell multiple times - history.mark_visited(Point2D::new(1.0, 1.0)); - history.mark_visited(Point2D::new(1.1, 1.1)); - history.mark_visited(Point2D::new(1.2, 1.2)); - - assert_eq!(history.get_visit_count(Point2D::new(1.0, 1.0)), 3); - assert_eq!(history.cells_visited(), 1); - } - - #[test] - fn test_recent_positions_limit() { - let mut history = ExplorationHistory::new(0.5, 3); - - history.mark_visited(Point2D::new(0.0, 0.0)); - history.mark_visited(Point2D::new(1.0, 0.0)); - history.mark_visited(Point2D::new(2.0, 0.0)); - history.mark_visited(Point2D::new(3.0, 0.0)); - - // Should only have last 3 positions - assert_eq!(history.recent_count(), 3); - - // But all cells should still be marked - assert_eq!(history.cells_visited(), 4); - } - - #[test] - fn test_area_recently_visited() { - let mut history = ExplorationHistory::new(1.0, 100); - - history.mark_visited(Point2D::new(5.0, 5.0)); - - // Position in same cell - assert!(history.is_area_recently_visited(Point2D::new(5.5, 5.5))); - - // Position in adjacent cell (within 3x3 neighborhood) - assert!(history.is_area_recently_visited(Point2D::new(6.5, 5.5))); - - // Position far away - assert!(!history.is_area_recently_visited(Point2D::new(10.0, 10.0))); - } - - #[test] - fn test_distance_to_nearest_recent() { - let mut history = ExplorationHistory::new(0.5, 100); - - assert!( - history - .distance_to_nearest_recent(Point2D::new(0.0, 0.0)) - .is_none() - ); - - history.mark_visited(Point2D::new(0.0, 0.0)); - history.mark_visited(Point2D::new(3.0, 0.0)); - - let dist = history.distance_to_nearest_recent(Point2D::new(1.0, 0.0)); - assert!(dist.is_some()); - assert!((dist.unwrap() - 1.0).abs() < 0.001); - } - - #[test] - fn test_clear() { - let mut history = ExplorationHistory::new(0.5, 100); - - history.mark_visited(Point2D::new(1.0, 1.0)); - history.mark_visited(Point2D::new(2.0, 2.0)); - - assert_eq!(history.cells_visited(), 2); - - history.clear(); - - assert_eq!(history.cells_visited(), 0); - assert_eq!(history.recent_count(), 0); - } - - #[test] - fn test_cell_boundaries() { - let mut history = ExplorationHistory::new(1.0, 100); - - // Points at cell boundaries - history.mark_visited(Point2D::new(0.99, 0.99)); // Cell (0, 0) - history.mark_visited(Point2D::new(1.01, 1.01)); // Cell (1, 1) - - assert_eq!(history.cells_visited(), 2); - assert_eq!(history.get_visit_count(Point2D::new(0.5, 0.5)), 1); - assert_eq!(history.get_visit_count(Point2D::new(1.5, 1.5)), 1); - } -} diff --git a/vastu-map/src/exploration/mod.rs b/vastu-map/src/exploration/mod.rs index 9724667..5c6397a 100644 --- a/vastu-map/src/exploration/mod.rs +++ b/vastu-map/src/exploration/mod.rs @@ -1,79 +1,52 @@ -//! Autonomous exploration module. +//! Autonomous exploration controller. //! -//! Provides frontier-based exploration with collision recovery for -//! complete map coverage. This module is designed to be used by -//! DhruvaSLAM for real robot exploration. +//! This module provides a state machine for frontier-based exploration: //! -//! # Primary Goals +//! - **Frontier detection**: Find unexplored boundaries +//! - **Path planning**: Navigate to frontiers +//! - **Recovery**: Handle obstacles and stuck conditions //! -//! 1. **Complete map coverage** - Explore every reachable area until no frontiers remain -//! 2. **Avoid obstacles** - Navigate safely using path planning with robot radius clearance -//! -//! # Architecture -//! -//! The exploration module is sensor-agnostic. It provides planning and control logic, -//! while the caller (DhruvaSLAM) handles: -//! - Sensor reading (bumpers, cliffs, lidar) -//! - SLAM map updates -//! - Velocity command execution -//! -//! # Example +//! ## Usage //! //! ```rust,ignore -//! use vastu_map::exploration::{ -//! ExplorationController, ExplorationConfig, -//! CollisionEvent, CollisionType, -//! }; -//! use vastu_map::{VectorMap, Pose2D}; +//! use vastu_map::exploration::{ExplorationController, ExplorationConfig}; //! -//! // Create controller //! let config = ExplorationConfig::default(); //! let mut controller = ExplorationController::new(config); +//! //! controller.start(); //! -//! // Main loop //! loop { -//! // Get current state from SLAM -//! let map = get_slam_map(); -//! let pose = get_current_pose(); -//! -//! // Check sensors -//! let collision = check_bumpers_and_cliffs(); -//! -//! // Update exploration -//! let step = controller.update(&map, pose, collision); -//! -//! // Handle virtual wall (for mirror/glass obstacles) -//! if let Some(wall) = step.virtual_wall { -//! map.add_line(wall.line); -//! } -//! -//! // Execute velocity command -//! if let Some(vel) = step.velocity { -//! send_velocity(vel.linear, vel.angular); -//! } -//! -//! // Check completion -//! if controller.is_complete() { -//! println!("Exploration complete!"); -//! break; +//! let cmd = controller.update(robot_pose, map.storage()); +//! match cmd { +//! ExplorationCommand::MoveTo { target, max_speed } => { +//! // Send motion command to robot +//! } +//! ExplorationCommand::ExplorationComplete => { +//! break; +//! } +//! _ => {} //! } //! } //! ``` +//! +//! ## State Machine +//! +//! The controller cycles through these states: +//! +//! 1. **Idle** - Waiting to start +//! 2. **SearchingFrontiers** - Looking for unexplored areas +//! 3. **Planning** - Computing path to selected frontier +//! 4. **Navigating** - Following path to frontier +//! 5. **Scanning** - At frontier, allowing sensors to update map +//! 6. **Recovering** - Handling obstacles or stuck conditions +//! 7. **Complete** - All frontiers explored +//! 8. **Failed** - Too many errors -mod collision; -mod config; -mod controller; -pub mod history; -mod path_follower; -pub mod planner; -pub mod region; +pub mod config; +pub mod controller; +pub mod state; -// Re-export public types -pub use collision::{CollisionEvent, CollisionType, VirtualWall}; pub use config::ExplorationConfig; -pub use controller::{ExplorationController, ExplorationState, ExplorationStep}; -pub use history::ExplorationHistory; -pub use path_follower::{FollowResult, PathFollower, VelocityCommand}; -pub use planner::{ExplorationPlan, ExplorationPlanner, PlannerConfig}; -pub use region::{ExplorationRegion, RegionConfig, RegionDetector}; +pub use controller::{ExplorationController, ExplorationProgress}; +pub use state::{ExplorationCommand, ExplorationEvent, ExplorationState, RecoveryAction}; diff --git a/vastu-map/src/exploration/path_follower.rs b/vastu-map/src/exploration/path_follower.rs deleted file mode 100644 index cebc31b..0000000 --- a/vastu-map/src/exploration/path_follower.rs +++ /dev/null @@ -1,374 +0,0 @@ -//! Pure pursuit path following for exploration. -//! -//! Implements a simple pure pursuit controller that generates velocity -//! commands to follow a path of waypoints. - -use crate::Path; -use crate::core::{Point2D, Pose2D}; - -use super::config::ExplorationConfig; - -/// Velocity command for the robot. -#[derive(Clone, Copy, Debug, Default, PartialEq)] -pub struct VelocityCommand { - /// Linear velocity in m/s (positive = forward). - pub linear: f32, - /// Angular velocity in rad/s (positive = counter-clockwise). - pub angular: f32, -} - -impl VelocityCommand { - /// Create a new velocity command. - pub fn new(linear: f32, angular: f32) -> Self { - Self { linear, angular } - } - - /// Create a zero velocity command (stop). - pub fn stop() -> Self { - Self::default() - } - - /// Create a command for backing up. - pub fn backup(speed: f32) -> Self { - Self { - linear: -speed.abs(), - angular: 0.0, - } - } - - /// Check if this is a stop command. - pub fn is_stop(&self) -> bool { - self.linear.abs() < 0.001 && self.angular.abs() < 0.001 - } -} - -/// Result of a path following step. -#[derive(Clone, Debug)] -pub struct FollowResult { - /// Velocity command to execute. - pub velocity: VelocityCommand, - /// Updated waypoint index. - pub waypoint_idx: usize, - /// Whether the path is complete (reached final waypoint). - pub path_complete: bool, - /// Distance to current target waypoint. - pub distance_to_waypoint: f32, -} - -/// Path follower for waypoint-based navigation. -/// -/// Generates velocity commands to follow a path of waypoints: -/// -/// 1. Transform target waypoint to robot frame -/// 2. Compute angle to target -/// 3. Generate proportional angular velocity for steering -/// 4. Reduce linear velocity when turning sharply -/// -/// The follower visits each waypoint in sequence to ensure the robot -/// follows the planned CBVG path through the medial axis of free space. -/// -/// # Example -/// -/// ```rust,ignore -/// use vastu_map::exploration::{PathFollower, ExplorationConfig}; -/// use vastu_map::{Path, Pose2D, Point2D}; -/// -/// let config = ExplorationConfig::default(); -/// let follower = PathFollower::new(&config); -/// -/// let path = Path { -/// points: vec![Point2D::new(1.0, 0.0), Point2D::new(2.0, 0.0)], -/// length: 1.0, -/// }; -/// -/// let pose = Pose2D::new(0.0, 0.0, 0.0); -/// let result = follower.follow(pose, &path, 0, 0.1); -/// ``` -#[derive(Clone, Debug)] -pub struct PathFollower { - /// Maximum linear velocity. - max_linear: f32, - /// Maximum angular velocity. - max_angular: f32, - /// Proportional gain for angular control. - angular_gain: f32, - /// Proportional gain for linear control based on distance. - linear_gain: f32, -} - -impl PathFollower { - /// Create a new path follower from exploration config. - pub fn new(config: &ExplorationConfig) -> Self { - Self { - max_linear: config.max_linear_speed, - max_angular: config.max_angular_speed, - angular_gain: 2.0, // Responsive steering - linear_gain: 0.5, // Smooth approach - } - } - - /// Create a path follower with custom parameters. - pub fn with_params(max_linear: f32, max_angular: f32) -> Self { - Self { - max_linear, - max_angular, - angular_gain: 2.0, - linear_gain: 0.5, - } - } - - /// Compute velocity command to follow path. - /// - /// # Arguments - /// * `current_pose` - Current robot pose in world frame - /// * `path` - Path to follow - /// * `waypoint_idx` - Current waypoint index - /// * `tolerance` - Distance to consider waypoint reached - /// - /// # Returns - /// Follow result with velocity command and updated waypoint index. - pub fn follow( - &self, - current_pose: Pose2D, - path: &Path, - waypoint_idx: usize, - tolerance: f32, - ) -> FollowResult { - // Empty path or past end - if path.points.is_empty() || waypoint_idx >= path.points.len() { - return FollowResult { - velocity: VelocityCommand::stop(), - waypoint_idx, - path_complete: true, - distance_to_waypoint: 0.0, - }; - } - - let robot_pos = current_pose.position(); - let mut current_idx = waypoint_idx; - - // Find the next waypoint to pursue (skip waypoints we've passed) - while current_idx < path.points.len() { - let waypoint = path.points[current_idx]; - let distance = robot_pos.distance(waypoint); - - if distance >= tolerance { - break; - } - - current_idx += 1; - } - - // Check if path is complete - if current_idx >= path.points.len() { - return FollowResult { - velocity: VelocityCommand::stop(), - waypoint_idx: current_idx, - path_complete: true, - distance_to_waypoint: 0.0, - }; - } - - // Target current waypoint directly (no lookahead skipping) - // This ensures the robot follows each CBVG node in sequence - let target = path.points[current_idx]; - - // Transform target to robot frame - let local_target = current_pose.inverse_transform_point(target); - - // Compute angle to target (in robot frame, forward is +X) - let angle_to_target = local_target.y.atan2(local_target.x); - let distance_to_waypoint = local_target.length(); - - // Compute velocity command - let velocity = self.compute_velocity(angle_to_target, distance_to_waypoint); - - FollowResult { - velocity, - waypoint_idx: current_idx, - path_complete: false, - distance_to_waypoint, - } - } - - /// Compute velocity command from angle and distance to target. - fn compute_velocity(&self, angle_to_target: f32, distance: f32) -> VelocityCommand { - // Angular velocity: proportional to angle error - let angular = - (angle_to_target * self.angular_gain).clamp(-self.max_angular, self.max_angular); - - // Speed reduction when turning sharply - // At 90 degrees, reduce speed to 20% of max - let turn_factor = 1.0 - (angle_to_target.abs() / std::f32::consts::PI).min(0.8); - - // Linear velocity: proportional to distance, reduced when turning - let linear = (distance * self.linear_gain).min(self.max_linear) * turn_factor; - - // Ensure minimum forward velocity when not at target - let linear = if distance > 0.05 && linear < 0.05 { - 0.05 * turn_factor - } else { - linear - }; - - VelocityCommand::new(linear, angular) - } - - /// Generate a command to rotate in place towards a target. - pub fn rotate_to_target(&self, current_pose: Pose2D, target: Point2D) -> VelocityCommand { - let local_target = current_pose.inverse_transform_point(target); - let angle_to_target = local_target.y.atan2(local_target.x); - - // Pure rotation - let angular = - (angle_to_target * self.angular_gain).clamp(-self.max_angular, self.max_angular); - - VelocityCommand::new(0.0, angular) - } - - /// Check if the robot is approximately facing the target. - pub fn is_facing_target(&self, current_pose: Pose2D, target: Point2D, tolerance: f32) -> bool { - let local_target = current_pose.inverse_transform_point(target); - let angle_to_target = local_target.y.atan2(local_target.x); - angle_to_target.abs() < tolerance - } -} - -#[cfg(test)] -mod tests { - use super::*; - use std::f32::consts::FRAC_PI_4; - - fn make_path(points: &[(f32, f32)]) -> Path { - let pts: Vec = points.iter().map(|(x, y)| Point2D::new(*x, *y)).collect(); - let mut length = 0.0; - for i in 1..pts.len() { - length += pts[i - 1].distance(pts[i]); - } - Path { - points: pts, - length, - } - } - - #[test] - fn test_velocity_command() { - let cmd = VelocityCommand::new(0.3, 0.5); - assert_eq!(cmd.linear, 0.3); - assert_eq!(cmd.angular, 0.5); - assert!(!cmd.is_stop()); - - let stop = VelocityCommand::stop(); - assert!(stop.is_stop()); - - let backup = VelocityCommand::backup(0.2); - assert_eq!(backup.linear, -0.2); - } - - #[test] - fn test_follow_empty_path() { - let config = ExplorationConfig::default(); - let follower = PathFollower::new(&config); - - let path = Path::new(); - let pose = Pose2D::identity(); - - let result = follower.follow(pose, &path, 0, 0.1); - assert!(result.path_complete); - assert!(result.velocity.is_stop()); - } - - #[test] - fn test_follow_straight_path() { - let config = ExplorationConfig::default(); - let follower = PathFollower::new(&config); - - let path = make_path(&[(1.0, 0.0), (2.0, 0.0), (3.0, 0.0)]); - let pose = Pose2D::new(0.0, 0.0, 0.0); // At origin, facing +X - - let result = follower.follow(pose, &path, 0, 0.1); - - // Should move forward with minimal turning - assert!(!result.path_complete); - assert!(result.velocity.linear > 0.0); - assert!(result.velocity.angular.abs() < 0.1); // Nearly straight - } - - #[test] - fn test_follow_requires_turn() { - let config = ExplorationConfig::default(); - let follower = PathFollower::new(&config); - - let path = make_path(&[(0.0, 1.0), (0.0, 2.0)]); // Path to the left - let pose = Pose2D::new(0.0, 0.0, 0.0); // At origin, facing +X - - let result = follower.follow(pose, &path, 0, 0.1); - - // Should turn left (positive angular) - assert!(!result.path_complete); - assert!(result.velocity.angular > 0.0); - // Linear should be reduced due to turning - assert!(result.velocity.linear < config.max_linear_speed); - } - - #[test] - fn test_follow_path_complete() { - let config = ExplorationConfig::default(); - let follower = PathFollower::new(&config); - - let path = make_path(&[(0.0, 0.0)]); - let pose = Pose2D::new(0.0, 0.0, 0.0); // Already at target - - let result = follower.follow(pose, &path, 0, 0.1); - - assert!(result.path_complete); - assert!(result.velocity.is_stop()); - } - - #[test] - fn test_waypoint_advancement() { - let config = ExplorationConfig::default(); - let follower = PathFollower::new(&config); - - let path = make_path(&[(0.5, 0.0), (1.0, 0.0), (1.5, 0.0)]); - - // Robot at (0.45, 0) - very close to first waypoint - let pose = Pose2D::new(0.45, 0.0, 0.0); - - let result = follower.follow(pose, &path, 0, 0.1); - - // Should skip first waypoint and target second - assert_eq!(result.waypoint_idx, 1); - } - - #[test] - fn test_rotate_to_target() { - let config = ExplorationConfig::default(); - let follower = PathFollower::new(&config); - - let pose = Pose2D::new(0.0, 0.0, 0.0); // Facing +X - let target = Point2D::new(0.0, 1.0); // Target is to the left - - let cmd = follower.rotate_to_target(pose, target); - - assert_eq!(cmd.linear, 0.0); // No forward motion - assert!(cmd.angular > 0.0); // Turn left (CCW) - } - - #[test] - fn test_is_facing_target() { - let config = ExplorationConfig::default(); - let follower = PathFollower::new(&config); - - let pose = Pose2D::new(0.0, 0.0, 0.0); - - // Target directly ahead - assert!(follower.is_facing_target(pose, Point2D::new(1.0, 0.0), 0.1)); - - // Target to the side - assert!(!follower.is_facing_target(pose, Point2D::new(0.0, 1.0), 0.1)); - - // Target at 45 degrees - assert!(follower.is_facing_target(pose, Point2D::new(1.0, 0.1), FRAC_PI_4)); - } -} diff --git a/vastu-map/src/exploration/planner.rs b/vastu-map/src/exploration/planner.rs deleted file mode 100644 index d7ff4b2..0000000 --- a/vastu-map/src/exploration/planner.rs +++ /dev/null @@ -1,580 +0,0 @@ -//! Exploration planning with region-based TSP ordering. -//! -//! Provides global exploration planning by ordering regions to minimize -//! total travel distance while ensuring complete coverage. - -use std::f32::consts::PI; - -use crate::Frontier; -use crate::core::Point2D; - -use super::history::ExplorationHistory; -use super::region::{ExplorationRegion, RegionConfig, RegionDetector}; - -/// Configuration for exploration planning. -#[derive(Clone, Debug)] -pub struct PlannerConfig { - /// Weight for region priority in selection (0.0 - 1.0). - /// Default: 0.3 - pub priority_weight: f32, - - /// Weight for distance penalty in region selection. - /// Default: 0.5 - pub distance_weight: f32, - - /// Penalty for revisiting regions. - /// Default: 0.5 - pub revisit_penalty: f32, - - /// Bonus per frontier in a region. - /// Default: 0.1 - pub frontier_bonus: f32, - - /// Weight for heading alignment when selecting frontiers within a region. - /// Default: 0.3 - pub heading_weight: f32, - - /// Weight for frontier openness. - /// Default: 0.4 - pub openness_weight: f32, - - /// Weight for local distance within a region. - /// Default: 0.3 - pub local_distance_weight: f32, - - /// Interval before forcing a full replan (seconds). - /// Default: 10.0 - pub replan_interval_secs: f32, - - /// Threshold of frontier count change to trigger replan. - /// Default: 5 - pub frontier_change_threshold: usize, - - /// Region detection configuration. - pub region_config: RegionConfig, -} - -impl Default for PlannerConfig { - fn default() -> Self { - Self { - priority_weight: 0.3, - distance_weight: 0.5, - revisit_penalty: 0.5, - frontier_bonus: 0.1, - heading_weight: 0.3, - openness_weight: 0.4, - local_distance_weight: 0.3, - replan_interval_secs: 120.0, // Replan less often to maintain region focus - frontier_change_threshold: 50, // Only replan on major changes - region_config: RegionConfig::default(), - } - } -} - -impl PlannerConfig { - /// Create a new configuration with default values. - pub fn new() -> Self { - Self::default() - } - - /// Builder-style setter for heading weight. - pub fn with_heading_weight(mut self, weight: f32) -> Self { - self.heading_weight = weight; - self - } - - /// Builder-style setter for openness weight. - pub fn with_openness_weight(mut self, weight: f32) -> Self { - self.openness_weight = weight; - self - } - - /// Builder-style setter for local distance weight. - pub fn with_local_distance_weight(mut self, weight: f32) -> Self { - self.local_distance_weight = weight; - self - } - - /// Builder-style setter for region config. - pub fn with_region_config(mut self, config: RegionConfig) -> Self { - self.region_config = config; - self - } -} - -/// Global exploration plan with ordered regions. -#[derive(Clone, Debug)] -pub struct ExplorationPlan { - /// Ordered list of regions to visit. - pub regions: Vec, - - /// Current region index being explored. - pub current_region_idx: usize, - - /// Whether the plan needs recomputation. - pub stale: bool, - - /// Number of frontiers at last planning. - pub last_frontier_count: usize, - - /// Elapsed time since last replan (seconds). - pub time_since_replan: f32, -} - -impl Default for ExplorationPlan { - fn default() -> Self { - Self::new() - } -} - -impl ExplorationPlan { - /// Create a new empty exploration plan. - pub fn new() -> Self { - Self { - regions: Vec::new(), - current_region_idx: 0, - stale: true, - last_frontier_count: 0, - time_since_replan: 0.0, - } - } - - /// Check if the plan is empty (no regions). - pub fn is_empty(&self) -> bool { - self.regions.is_empty() - } - - /// Get the current region being explored. - pub fn current_region(&self) -> Option<&ExplorationRegion> { - self.regions.get(self.current_region_idx) - } - - /// Get the current region mutably. - pub fn current_region_mut(&mut self) -> Option<&mut ExplorationRegion> { - self.regions.get_mut(self.current_region_idx) - } - - /// Advance to the next region. - pub fn advance_region(&mut self) { - if let Some(region) = self.regions.get_mut(self.current_region_idx) { - region.visit_count += 1; - } - self.current_region_idx += 1; - } - - /// Check if all regions have been processed. - pub fn is_complete(&self) -> bool { - self.current_region_idx >= self.regions.len() - } - - /// Get the total number of remaining frontiers across all regions. - pub fn remaining_frontier_count(&self) -> usize { - self.regions - .iter() - .skip(self.current_region_idx) - .map(|r| r.frontier_count()) - .sum() - } -} - -/// Exploration planner that manages region detection and ordering. -pub struct ExplorationPlanner { - /// Region detector instance. - region_detector: RegionDetector, - - /// Current exploration plan. - plan: ExplorationPlan, - - /// Configuration. - config: PlannerConfig, -} - -impl ExplorationPlanner { - /// Create a new exploration planner. - pub fn new(config: PlannerConfig) -> Self { - Self { - region_detector: RegionDetector::new(), - plan: ExplorationPlan::new(), - config, - } - } - - /// Get the current exploration plan. - pub fn plan(&self) -> &ExplorationPlan { - &self.plan - } - - /// Get the current exploration plan mutably. - pub fn plan_mut(&mut self) -> &mut ExplorationPlan { - &mut self.plan - } - - /// Update the exploration plan if needed. - /// - /// # Arguments - /// * `frontiers` - Current list of frontiers - /// * `robot_pos` - Current robot position - /// * `history` - Exploration history for visit tracking - /// * `dt` - Time since last update (seconds) - pub fn update( - &mut self, - frontiers: &[Frontier], - robot_pos: Point2D, - history: &ExplorationHistory, - dt: f32, - ) { - self.plan.time_since_replan += dt; - - if self.should_replan(frontiers) { - self.replan(frontiers, robot_pos, history); - } else { - // Incremental update: refresh frontiers in current region - self.update_current_region(frontiers); - } - } - - /// Force a full replan. - pub fn force_replan( - &mut self, - frontiers: &[Frontier], - robot_pos: Point2D, - history: &ExplorationHistory, - ) { - self.replan(frontiers, robot_pos, history); - } - - /// Check if a replan is needed. - fn should_replan(&self, frontiers: &[Frontier]) -> bool { - // Plan is stale - if self.plan.stale { - return true; - } - - // Time-based replan - if self.plan.time_since_replan >= self.config.replan_interval_secs { - return true; - } - - // Frontier count changed significantly - let frontier_change = - (frontiers.len() as i32 - self.plan.last_frontier_count as i32).unsigned_abs() as usize; - if frontier_change >= self.config.frontier_change_threshold { - return true; - } - - // Current region exhausted - if self.plan.current_region_idx >= self.plan.regions.len() { - return true; - } - - // Current region has no frontiers left - if let Some(region) = self.plan.current_region() - && region.is_empty() - { - return true; - } - - false - } - - /// Perform a full replan. - fn replan(&mut self, frontiers: &[Frontier], robot_pos: Point2D, history: &ExplorationHistory) { - // Remember previous region centroid to try to maintain context - let prev_centroid = self.plan.current_region().map(|r| r.centroid); - - // Detect regions - let mut regions = self - .region_detector - .detect(frontiers, &self.config.region_config); - - // Order regions using greedy TSP - let ordered_regions = self.order_regions(&mut regions, robot_pos, history); - - // Update plan - self.plan.regions = ordered_regions; - self.plan.stale = false; - self.plan.last_frontier_count = frontiers.len(); - self.plan.time_since_replan = 0.0; - - // Try to find a region close to where we were exploring - // This maintains continuity instead of jumping to a new area - if let Some(prev) = prev_centroid { - let closest_idx = self - .plan - .regions - .iter() - .enumerate() - .min_by(|(_, a), (_, b)| { - let dist_a = a.centroid.distance(prev); - let dist_b = b.centroid.distance(prev); - dist_a - .partial_cmp(&dist_b) - .unwrap_or(std::cmp::Ordering::Equal) - }) - .map(|(i, _)| i) - .unwrap_or(0); - - // Only use the closest region if it's reasonably close (within 2m) - if let Some(region) = self.plan.regions.get(closest_idx) - && region.centroid.distance(prev) < 2.0 - { - self.plan.current_region_idx = closest_idx; - return; - } - } - - // Default: start from the first (best) region - self.plan.current_region_idx = 0; - } - - /// Order regions using greedy TSP with priority weighting. - fn order_regions( - &self, - regions: &mut Vec, - robot_pos: Point2D, - history: &ExplorationHistory, - ) -> Vec { - if regions.is_empty() { - return Vec::new(); - } - - // Compute priorities for all regions - for region in regions.iter_mut() { - region.priority = self.compute_region_priority(region, robot_pos, history); - } - - // Greedy TSP: always pick the best next region - let mut ordered = Vec::with_capacity(regions.len()); - let mut remaining: Vec = std::mem::take(regions); - let mut current_pos = robot_pos; - - while !remaining.is_empty() { - // Find best next region - let best_idx = remaining - .iter() - .enumerate() - .map(|(i, region)| { - let distance = current_pos.distance(region.centroid); - let score = region.priority * self.config.priority_weight - - distance * self.config.distance_weight - - region.visit_count as f32 * self.config.revisit_penalty - + region.frontier_count() as f32 * self.config.frontier_bonus; - (i, score) - }) - .max_by(|a, b| a.1.partial_cmp(&b.1).unwrap_or(std::cmp::Ordering::Equal)) - .map(|(i, _)| i) - .unwrap_or(0); - - let selected = remaining.remove(best_idx); - current_pos = selected.centroid; - ordered.push(selected); - } - - ordered - } - - /// Compute the priority of a region. - fn compute_region_priority( - &self, - region: &ExplorationRegion, - _robot_pos: Point2D, - history: &ExplorationHistory, - ) -> f32 { - // Base priority from estimated area (larger regions are more valuable) - let area_score = (region.estimated_area / 10.0).min(1.0); - - // Penalty for regions near recently visited areas - let visit_penalty = if history.is_area_recently_visited(region.centroid) { - 0.5 - } else { - 0.0 - }; - - // Bonus for regions with more frontiers - let frontier_score = (region.frontier_count() as f32 / 5.0).min(1.0); - - area_score * 0.3 + frontier_score * 0.7 - visit_penalty - } - - /// Update the current region with valid frontiers. - fn update_current_region(&mut self, frontiers: &[Frontier]) { - if let Some(region) = self.plan.current_region_mut() { - region.retain_valid_frontiers(frontiers); - } - } - - /// Select the best frontier within the current region. - /// - /// Uses heading-aligned selection to prevent ping-pong behavior. - pub fn select_frontier_in_region<'a>( - &self, - region: &'a ExplorationRegion, - robot_pos: Point2D, - robot_heading: f32, - ) -> Option<&'a Frontier> { - if region.is_empty() { - return None; - } - - // Score each frontier - - region - .frontiers - .iter() - .map(|frontier| { - let score = self.compute_frontier_score(frontier, robot_pos, robot_heading); - (frontier, score) - }) - .max_by(|a, b| a.1.partial_cmp(&b.1).unwrap_or(std::cmp::Ordering::Equal)) - .map(|(f, _)| f) - } - - /// Compute the score of a frontier for selection. - fn compute_frontier_score( - &self, - frontier: &Frontier, - robot_pos: Point2D, - robot_heading: f32, - ) -> f32 { - // Use viewpoint for distance calculations (safe navigation target) - let direction = Point2D::new( - frontier.viewpoint.x - robot_pos.x, - frontier.viewpoint.y - robot_pos.y, - ); - let distance = direction.length(); - - if distance < 0.001 { - return 0.0; - } - - // Heading alignment score (1.0 = same direction, 0.0 = opposite) - let angle_to_frontier = direction.y.atan2(direction.x); - let angle_diff = normalize_angle(angle_to_frontier - robot_heading); - let heading_score = 1.0 - (angle_diff.abs() / PI); - - // Distance score (prefer closer frontiers within the region) - let distance_score = 1.0 / (1.0 + distance * 0.5); - - // Combined score - heading_score * self.config.heading_weight - + distance_score * self.config.local_distance_weight - + self.config.openness_weight * 0.5 // Placeholder for openness (not currently available on Frontier) - } - - /// Reset the planner state. - pub fn reset(&mut self) { - self.plan = ExplorationPlan::new(); - self.region_detector.reset_ids(); - } -} - -/// Normalize an angle to [-PI, PI]. -/// Uses modular arithmetic to avoid potential infinite loops with extreme values. -fn normalize_angle(angle: f32) -> f32 { - if !angle.is_finite() { - return 0.0; - } - let mut result = angle % (2.0 * PI); - if result > PI { - result -= 2.0 * PI; - } else if result < -PI { - result += 2.0 * PI; - } - result -} - -#[cfg(test)] -mod tests { - use super::*; - - fn make_frontier(x: f32, y: f32) -> Frontier { - Frontier { - viewpoint: Point2D::new(x, y), - look_direction: Point2D::new(1.0, 0.0), - endpoint: Point2D::new(x - 0.4, y), - line_idx: 0, - estimated_area: 4.0, - } - } - - #[test] - fn test_empty_plan() { - let config = PlannerConfig::default(); - let planner = ExplorationPlanner::new(config); - - assert!(planner.plan().is_empty()); - assert!(planner.plan().is_complete()); - } - - #[test] - fn test_replan_with_frontiers() { - let config = PlannerConfig::default(); - let mut planner = ExplorationPlanner::new(config); - let history = ExplorationHistory::new(0.5, 100); - - let frontiers = vec![ - make_frontier(1.0, 0.0), - make_frontier(1.5, 0.0), - make_frontier(5.0, 5.0), - make_frontier(5.5, 5.0), - ]; - - planner.force_replan(&frontiers, Point2D::new(0.0, 0.0), &history); - - assert!(!planner.plan().is_empty()); - assert!(!planner.plan().is_complete()); - } - - #[test] - fn test_frontier_selection_prefers_heading() { - let config = PlannerConfig::default().with_heading_weight(0.9); - let planner = ExplorationPlanner::new(config); - - // Robot facing right (heading = 0) - let robot_pos = Point2D::new(0.0, 0.0); - let robot_heading = 0.0; - - // Two frontiers: one ahead (x=2), one behind (x=-2) - let frontiers = vec![make_frontier(2.0, 0.0), make_frontier(-2.0, 0.0)]; - - let region = ExplorationRegion::from_frontiers(0, frontiers); - - let selected = planner.select_frontier_in_region(®ion, robot_pos, robot_heading); - - assert!(selected.is_some()); - let selected = selected.unwrap(); - // Should prefer the frontier ahead (positive x) - assert!(selected.viewpoint.x > 0.0); - } - - #[test] - fn test_normalize_angle() { - assert!((normalize_angle(0.0) - 0.0).abs() < 0.001); - assert!((normalize_angle(PI) - PI).abs() < 0.001); - assert!((normalize_angle(-PI) - (-PI)).abs() < 0.001); - assert!((normalize_angle(3.0 * PI) - PI).abs() < 0.001); - assert!((normalize_angle(-3.0 * PI) - (-PI)).abs() < 0.001); - } - - #[test] - fn test_plan_advance_region() { - let mut plan = ExplorationPlan::new(); - let frontiers = vec![make_frontier(1.0, 0.0)]; - plan.regions - .push(ExplorationRegion::from_frontiers(0, frontiers.clone())); - plan.regions - .push(ExplorationRegion::from_frontiers(1, frontiers)); - plan.stale = false; - - assert_eq!(plan.current_region_idx, 0); - assert!(!plan.is_complete()); - - plan.advance_region(); - assert_eq!(plan.current_region_idx, 1); - assert!(!plan.is_complete()); - - plan.advance_region(); - assert_eq!(plan.current_region_idx, 2); - assert!(plan.is_complete()); - } -} diff --git a/vastu-map/src/exploration/region.rs b/vastu-map/src/exploration/region.rs deleted file mode 100644 index 38c163d..0000000 --- a/vastu-map/src/exploration/region.rs +++ /dev/null @@ -1,424 +0,0 @@ -//! Exploration region detection and clustering. -//! -//! Groups nearby frontiers into exploration regions using hierarchical clustering. -//! This enables region-based exploration strategies that avoid ping-pong behavior. - -use std::collections::HashMap; - -use crate::Frontier; -use crate::core::{Bounds, Point2D}; - -/// Configuration for region detection. -#[derive(Clone, Debug)] -pub struct RegionConfig { - /// Maximum distance for clustering frontiers into regions (meters). - /// Default: 1.5 - pub cluster_distance: f32, - - /// Minimum number of frontiers to form a valid region. - /// Default: 1 - pub min_region_size: usize, -} - -impl Default for RegionConfig { - fn default() -> Self { - Self { - cluster_distance: 1.5, - min_region_size: 1, - } - } -} - -impl RegionConfig { - /// Create a new configuration with default values. - pub fn new() -> Self { - Self::default() - } - - /// Builder-style setter for cluster distance. - pub fn with_cluster_distance(mut self, distance: f32) -> Self { - self.cluster_distance = distance; - self - } - - /// Builder-style setter for minimum region size. - pub fn with_min_region_size(mut self, size: usize) -> Self { - self.min_region_size = size; - self - } -} - -/// A clustered group of frontiers representing an unexplored area. -#[derive(Clone, Debug)] -pub struct ExplorationRegion { - /// Unique region identifier. - pub id: u32, - - /// Centroid of the region (mean of all frontier points). - pub centroid: Point2D, - - /// Individual frontiers in this region. - pub frontiers: Vec, - - /// Bounding box of the region. - pub bounds: Bounds, - - /// Estimated unexplored area (heuristic based on spread). - pub estimated_area: f32, - - /// Priority score (higher = explore first). - pub priority: f32, - - /// Number of times we've attempted to visit this region. - pub visit_count: u32, -} - -impl ExplorationRegion { - /// Create a new region from a cluster of frontiers. - pub fn from_frontiers(id: u32, frontiers: Vec) -> Self { - let centroid = compute_centroid(&frontiers); - let bounds = compute_bounds(&frontiers); - let estimated_area = estimate_area(&bounds); - - Self { - id, - centroid, - frontiers, - bounds, - estimated_area, - priority: 0.0, - visit_count: 0, - } - } - - /// Get the number of frontiers in this region. - pub fn frontier_count(&self) -> usize { - self.frontiers.len() - } - - /// Check if the region has no frontiers left. - pub fn is_empty(&self) -> bool { - self.frontiers.is_empty() - } - - /// Remove frontiers that are no longer valid (not in the current frontier list). - pub fn retain_valid_frontiers(&mut self, valid_frontiers: &[Frontier]) { - self.frontiers.retain(|f| { - valid_frontiers - .iter() - .any(|vf| f.viewpoint.distance(vf.viewpoint) < 0.1) - }); - - // Update centroid and bounds if we still have frontiers - if !self.frontiers.is_empty() { - self.centroid = compute_centroid(&self.frontiers); - self.bounds = compute_bounds(&self.frontiers); - self.estimated_area = estimate_area(&self.bounds); - } - } -} - -/// Detect exploration regions from frontiers using hierarchical clustering. -pub struct RegionDetector { - /// Reusable union-find parent array. - parent: Vec, - - /// Reusable union-find rank array. - rank: Vec, - - /// Next region ID to assign. - next_id: u32, - - /// Spatial hash grid for efficient neighbor lookup. - spatial_grid: HashMap<(i32, i32), Vec>, -} - -impl Default for RegionDetector { - fn default() -> Self { - Self::new() - } -} - -impl RegionDetector { - /// Create a new region detector. - pub fn new() -> Self { - Self { - parent: Vec::with_capacity(100), - rank: Vec::with_capacity(100), - next_id: 0, - spatial_grid: HashMap::with_capacity(100), - } - } - - /// Detect regions from a list of frontiers. - pub fn detect( - &mut self, - frontiers: &[Frontier], - config: &RegionConfig, - ) -> Vec { - if frontiers.is_empty() { - return Vec::new(); - } - - // Cluster frontiers using union-find - let clusters = self.cluster_frontiers(frontiers, config.cluster_distance); - - // Convert clusters to regions - let mut regions = Vec::new(); - for cluster in clusters { - if cluster.len() >= config.min_region_size { - let region_frontiers: Vec = - cluster.iter().map(|&idx| frontiers[idx].clone()).collect(); - - let region = ExplorationRegion::from_frontiers(self.next_id, region_frontiers); - self.next_id += 1; - regions.push(region); - } - } - - regions - } - - /// Cluster frontiers using union-find with spatial hashing. - fn cluster_frontiers(&mut self, frontiers: &[Frontier], max_distance: f32) -> Vec> { - let n = frontiers.len(); - - // Initialize union-find - self.parent.clear(); - self.parent.resize(n, 0); - self.rank.clear(); - self.rank.resize(n, 0); - for i in 0..n { - self.parent[i] = i; - } - - // Clear spatial grid completely to avoid stale indices - self.spatial_grid.clear(); - - // Build spatial hash grid (using viewpoint for clustering) - let cell_size = max_distance; - for (i, frontier) in frontiers.iter().enumerate() { - let cell = ( - (frontier.viewpoint.x / cell_size).floor() as i32, - (frontier.viewpoint.y / cell_size).floor() as i32, - ); - - self.spatial_grid.entry(cell).or_default().push(i); - } - - // Collect pairs to union (avoids borrow conflict) - let mut pairs_to_union: Vec<(usize, usize)> = Vec::new(); - - for (i, frontier) in frontiers.iter().enumerate() { - let cx = (frontier.viewpoint.x / cell_size).floor() as i32; - let cy = (frontier.viewpoint.y / cell_size).floor() as i32; - - // Check 3x3 neighborhood - for dx in -1..=1 { - for dy in -1..=1 { - let neighbor_cell = (cx + dx, cy + dy); - if let Some(indices) = self.spatial_grid.get(&neighbor_cell) { - for &j in indices { - if i < j { - let dist = frontier.viewpoint.distance(frontiers[j].viewpoint); - if dist <= max_distance { - pairs_to_union.push((i, j)); - } - } - } - } - } - } - } - - // Perform unions - for (i, j) in pairs_to_union { - self.union(i, j); - } - - // Extract clusters - let mut cluster_map: HashMap> = HashMap::new(); - for i in 0..n { - let root = self.find(i); - cluster_map.entry(root).or_default().push(i); - } - - cluster_map.into_values().collect() - } - - /// Union-find: find with path compression. - fn find(&mut self, x: usize) -> usize { - if self.parent[x] != x { - self.parent[x] = self.find(self.parent[x]); - } - self.parent[x] - } - - /// Union-find: union by rank. - fn union(&mut self, x: usize, y: usize) { - let root_x = self.find(x); - let root_y = self.find(y); - - if root_x != root_y { - if self.rank[root_x] < self.rank[root_y] { - self.parent[root_x] = root_y; - } else if self.rank[root_x] > self.rank[root_y] { - self.parent[root_y] = root_x; - } else { - self.parent[root_y] = root_x; - self.rank[root_x] += 1; - } - } - } - - /// Reset the region ID counter. - pub fn reset_ids(&mut self) { - self.next_id = 0; - } -} - -/// Compute the centroid of a cluster of frontiers (using viewpoints). -fn compute_centroid(frontiers: &[Frontier]) -> Point2D { - if frontiers.is_empty() { - return Point2D::new(0.0, 0.0); - } - - let sum_x: f32 = frontiers.iter().map(|f| f.viewpoint.x).sum(); - let sum_y: f32 = frontiers.iter().map(|f| f.viewpoint.y).sum(); - let n = frontiers.len() as f32; - - Point2D::new(sum_x / n, sum_y / n) -} - -/// Compute the bounding box of a cluster of frontiers (using viewpoints). -fn compute_bounds(frontiers: &[Frontier]) -> Bounds { - if frontiers.is_empty() { - return Bounds::empty(); - } - - let mut min_x = f32::INFINITY; - let mut min_y = f32::INFINITY; - let mut max_x = f32::NEG_INFINITY; - let mut max_y = f32::NEG_INFINITY; - - for frontier in frontiers { - min_x = min_x.min(frontier.viewpoint.x); - min_y = min_y.min(frontier.viewpoint.y); - max_x = max_x.max(frontier.viewpoint.x); - max_y = max_y.max(frontier.viewpoint.y); - } - - Bounds::new(Point2D::new(min_x, min_y), Point2D::new(max_x, max_y)) -} - -/// Estimate the unexplored area of a region based on its bounding box. -fn estimate_area(bounds: &Bounds) -> f32 { - let width = bounds.max.x - bounds.min.x; - let height = bounds.max.y - bounds.min.y; - - // Add a minimum area to avoid zero for single-point regions - (width * height).max(0.1) -} - -#[cfg(test)] -mod tests { - use super::*; - - fn make_frontier(x: f32, y: f32) -> Frontier { - Frontier { - viewpoint: Point2D::new(x, y), - look_direction: Point2D::new(1.0, 0.0), - endpoint: Point2D::new(x - 0.4, y), - line_idx: 0, - estimated_area: 4.0, - } - } - - #[test] - fn test_empty_frontiers() { - let mut detector = RegionDetector::new(); - let config = RegionConfig::default(); - let regions = detector.detect(&[], &config); - assert!(regions.is_empty()); - } - - #[test] - fn test_single_frontier() { - let mut detector = RegionDetector::new(); - let config = RegionConfig::default(); - let frontiers = vec![make_frontier(1.0, 1.0)]; - let regions = detector.detect(&frontiers, &config); - - assert_eq!(regions.len(), 1); - assert_eq!(regions[0].frontiers.len(), 1); - } - - #[test] - fn test_two_clusters() { - let mut detector = RegionDetector::new(); - let config = RegionConfig::default().with_cluster_distance(1.0); - - // Two clusters: (0,0), (0.5, 0) and (10, 10), (10.5, 10) - let frontiers = vec![ - make_frontier(0.0, 0.0), - make_frontier(0.5, 0.0), - make_frontier(10.0, 10.0), - make_frontier(10.5, 10.0), - ]; - - let regions = detector.detect(&frontiers, &config); - - assert_eq!(regions.len(), 2); - assert_eq!(regions[0].frontiers.len(), 2); - assert_eq!(regions[1].frontiers.len(), 2); - } - - #[test] - fn test_centroid_calculation() { - let frontiers = vec![ - make_frontier(0.0, 0.0), - make_frontier(2.0, 0.0), - make_frontier(1.0, 2.0), - ]; - - let centroid = compute_centroid(&frontiers); - assert!((centroid.x - 1.0).abs() < 0.001); - assert!((centroid.y - 0.667).abs() < 0.01); - } - - #[test] - fn test_bounds_calculation() { - let frontiers = vec![ - make_frontier(1.0, 2.0), - make_frontier(3.0, 4.0), - make_frontier(2.0, 1.0), - ]; - - let bounds = compute_bounds(&frontiers); - assert_eq!(bounds.min.x, 1.0); - assert_eq!(bounds.min.y, 1.0); - assert_eq!(bounds.max.x, 3.0); - assert_eq!(bounds.max.y, 4.0); - } - - #[test] - fn test_min_region_size() { - let mut detector = RegionDetector::new(); - let config = RegionConfig::default() - .with_cluster_distance(0.5) - .with_min_region_size(2); - - // One isolated frontier and two nearby ones - let frontiers = vec![ - make_frontier(0.0, 0.0), // Isolated - make_frontier(10.0, 10.0), - make_frontier(10.2, 10.0), // Together - ]; - - let regions = detector.detect(&frontiers, &config); - - // Only the cluster with 2 frontiers should be included - assert_eq!(regions.len(), 1); - assert_eq!(regions[0].frontiers.len(), 2); - } -} diff --git a/vastu-map/src/exploration/state.rs b/vastu-map/src/exploration/state.rs new file mode 100644 index 0000000..9ff65e4 --- /dev/null +++ b/vastu-map/src/exploration/state.rs @@ -0,0 +1,182 @@ +//! Exploration state machine states. + +use crate::core::WorldPoint; +use crate::query::Frontier; + +/// Exploration state +#[derive(Clone, Debug)] +pub enum ExplorationState { + /// Idle - waiting to start exploration + Idle, + + /// Searching for frontiers + SearchingFrontiers, + + /// Planning path to selected frontier + Planning { + /// Target frontier + target: Frontier, + }, + + /// Navigating to frontier + Navigating { + /// Current path as world points + path: Vec, + /// Current waypoint index + current_waypoint: usize, + /// Target frontier + target: Frontier, + }, + + /// Reached frontier, scanning area by rotating + Scanning { + /// Position where we're scanning + position: WorldPoint, + /// Remaining rotation in radians (full rotation = 2π) + rotation_remaining: f32, + }, + + /// Recovering from obstacle or stuck condition + Recovering { + /// Recovery action being performed + action: RecoveryAction, + /// Number of recovery attempts + attempts: usize, + }, + + /// Exploration complete (no more frontiers) + Complete, + + /// Failed (too many errors) + Failed { + /// Reason for failure + reason: String, + }, +} + +impl ExplorationState { + /// Is this a terminal state? + pub fn is_terminal(&self) -> bool { + matches!( + self, + ExplorationState::Complete | ExplorationState::Failed { .. } + ) + } + + /// Is exploration active? + pub fn is_active(&self) -> bool { + !matches!( + self, + ExplorationState::Idle | ExplorationState::Complete | ExplorationState::Failed { .. } + ) + } + + /// State name for logging + pub fn name(&self) -> &'static str { + match self { + ExplorationState::Idle => "Idle", + ExplorationState::SearchingFrontiers => "SearchingFrontiers", + ExplorationState::Planning { .. } => "Planning", + ExplorationState::Navigating { .. } => "Navigating", + ExplorationState::Scanning { .. } => "Scanning", + ExplorationState::Recovering { .. } => "Recovering", + ExplorationState::Complete => "Complete", + ExplorationState::Failed { .. } => "Failed", + } + } +} + +/// Recovery action types +#[derive(Clone, Debug)] +pub enum RecoveryAction { + /// Turn in place by specified angle (radians) + TurnInPlace(f32), + /// Back up by specified distance (meters) + BackUp(f32), + /// Wait for obstacle to clear + Wait, +} + +/// Command output from exploration controller +#[derive(Clone, Debug)] +pub enum ExplorationCommand { + /// No action needed + None, + + /// Move to target position + MoveTo { + /// Target position in world coordinates + target: WorldPoint, + /// Maximum speed (m/s) + max_speed: f32, + }, + + /// Rotate in place + Rotate { + /// Target heading (radians) + target_heading: f32, + /// Maximum angular speed (rad/s) + max_angular_speed: f32, + }, + + /// Stop immediately + Stop, + + /// Exploration is complete + ExplorationComplete, + + /// Exploration failed + ExplorationFailed { + /// Reason for failure + reason: String, + }, +} + +/// Event that can trigger state transitions +#[derive(Clone, Debug)] +pub enum ExplorationEvent { + /// Start exploration + Start, + + /// Stop exploration + Stop, + + /// Resume from pause + Resume, + + /// Frontier detected + FrontierFound(Frontier), + + /// No frontiers available + NoFrontiers, + + /// Path planned successfully + PathPlanned(Vec), + + /// Path planning failed + PlanningFailed, + + /// Reached current waypoint + WaypointReached, + + /// Reached final destination + DestinationReached, + + /// Obstacle detected + ObstacleDetected, + + /// Path is blocked + PathBlocked, + + /// Recovery completed + RecoveryComplete, + + /// Recovery failed + RecoveryFailed, + + /// Scan complete + ScanComplete, + + /// Timeout + Timeout, +} diff --git a/vastu-map/src/explore/config.rs b/vastu-map/src/explore/config.rs new file mode 100644 index 0000000..679a887 --- /dev/null +++ b/vastu-map/src/explore/config.rs @@ -0,0 +1,125 @@ +//! Explorer configuration. + +use serde::{Deserialize, Serialize}; + +use crate::exploration::ExplorationConfig; +use crate::grid::{GridConfig, MapConfig, SensorConfig}; +use crate::slam::CorrelativeMatcherConfig; +use crate::slam::loop_closure::{LoopClosureConfig, PoseGraphConfig}; + +/// Configuration for the VastuExplorer. +/// +/// This combines all the configuration needed for autonomous exploration: +/// - Grid/map settings +/// - Sensor integration +/// - Scan matching +/// - Loop closure +/// - Exploration behavior +#[derive(Clone, Debug, Serialize, Deserialize)] +pub struct ExplorerConfig { + /// Grid storage configuration. + #[serde(default)] + pub grid: GridConfig, + + /// Sensor integration configuration. + #[serde(default)] + pub sensor: SensorConfig, + + /// Exploration controller configuration. + #[serde(default)] + pub exploration: ExplorationConfig, + + /// Scan-to-map matching configuration. + #[serde(default)] + pub matching: CorrelativeMatcherConfig, + + /// Loop closure detection configuration. + #[serde(default)] + pub loop_closure: LoopClosureConfig, + + /// Pose graph optimization configuration. + #[serde(default)] + pub pose_graph: PoseGraphConfig, + + /// Control loop update rate in Hz. + #[serde(default = "default_update_rate")] + pub update_rate_hz: f32, + + /// Maximum exploration time in seconds (0 = unlimited). + #[serde(default)] + pub max_time_secs: f32, + + /// Stop if battery falls below this percentage. + #[serde(default = "default_min_battery")] + pub min_battery_percent: u8, +} + +fn default_update_rate() -> f32 { + 10.0 // 10 Hz +} + +fn default_min_battery() -> u8 { + 20 // 20% +} + +impl Default for ExplorerConfig { + fn default() -> Self { + Self { + grid: GridConfig::default(), + sensor: SensorConfig::default(), + exploration: ExplorationConfig::default(), + matching: CorrelativeMatcherConfig::default(), + loop_closure: LoopClosureConfig::default(), + pose_graph: PoseGraphConfig::default(), + update_rate_hz: default_update_rate(), + max_time_secs: 0.0, + min_battery_percent: default_min_battery(), + } + } +} + +impl ExplorerConfig { + /// Create a fast configuration for quick testing. + pub fn fast() -> Self { + Self { + matching: CorrelativeMatcherConfig::fast(), + update_rate_hz: 5.0, + ..Default::default() + } + } + + /// Create a thorough configuration for best quality maps. + pub fn thorough() -> Self { + Self { + matching: CorrelativeMatcherConfig::thorough(), + update_rate_hz: 10.0, + ..Default::default() + } + } + + /// Convert to MapConfig for OccupancyGridMap creation. + pub fn to_map_config(&self) -> MapConfig { + MapConfig { + grid: self.grid.clone(), + sensor: self.sensor.clone(), + } + } +} + +#[cfg(test)] +mod tests { + use super::*; + + #[test] + fn test_default_config() { + let config = ExplorerConfig::default(); + assert!((config.update_rate_hz - 10.0).abs() < 0.001); + assert_eq!(config.min_battery_percent, 20); + } + + #[test] + fn test_fast_config() { + let config = ExplorerConfig::fast(); + assert!((config.update_rate_hz - 5.0).abs() < 0.001); + } +} diff --git a/vastu-map/src/explore/explorer.rs b/vastu-map/src/explore/explorer.rs new file mode 100644 index 0000000..131493b --- /dev/null +++ b/vastu-map/src/explore/explorer.rs @@ -0,0 +1,428 @@ +//! VastuExplorer - High-level autonomous exploration. + +use std::time::Instant; + +use crate::OccupancyGridMap; +use crate::core::Pose2D; +use crate::exploration::{ExplorationCommand, ExplorationController, ExplorationProgress}; +use crate::slam::CorrelativeMatcher; +use crate::slam::loop_closure::{LoopClosureDetector, PoseGraph}; + +use super::config::ExplorerConfig; +use super::source::SensorSource; + +/// Result of a completed exploration. +pub enum ExploreResult { + /// Exploration completed successfully - all frontiers explored. + Complete(OccupancyGridMap), + /// Exploration failed or was interrupted. + Failed { + /// Partial map collected before failure. + map: OccupancyGridMap, + /// Reason for failure. + reason: String, + }, +} + +/// Status returned from a single exploration step. +#[derive(Clone, Debug)] +pub enum ExploreStatus { + /// Still exploring. + InProgress { + /// Current exploration progress. + progress: ExplorationProgress, + }, + /// Exploration complete. + Complete, + /// Exploration failed. + Failed { + /// Reason for failure. + reason: String, + }, +} + +/// High-level autonomous exploration controller. +/// +/// Combines all components needed for autonomous map exploration: +/// - Occupancy grid mapping +/// - Scan-to-map matching for drift correction +/// - Loop closure detection +/// - Pose graph optimization +/// - Frontier-based exploration +/// +/// # Example +/// +/// ```ignore +/// let mut explorer = VastuExplorer::new(ExplorerConfig::default()); +/// let mut robot = MyRobotSource::new(); +/// +/// // Run exploration to completion +/// match explorer.explore(&mut robot) { +/// ExploreResult::Complete(map) => { /* save map */ } +/// ExploreResult::Failed { map, reason } => { /* handle error */ } +/// } +/// ``` +pub struct VastuExplorer { + /// Configuration. + config: ExplorerConfig, + /// Occupancy grid map. + map: OccupancyGridMap, + /// Exploration controller. + controller: ExplorationController, + /// Scan-to-map matcher. + matcher: CorrelativeMatcher, + /// Loop closure detector. + loop_detector: LoopClosureDetector, + /// Pose graph for optimization. + pose_graph: PoseGraph, + /// Last corrected pose. + last_pose: Pose2D, + /// Last odometry pose (for delta calculation). + last_odom_pose: Option, + /// Start time. + start_time: Option, + /// Whether exploration has started. + started: bool, +} + +impl VastuExplorer { + /// Create a new explorer with the given configuration. + pub fn new(config: ExplorerConfig) -> Self { + let map_config = config.to_map_config(); + let map = OccupancyGridMap::new(map_config); + + let controller = ExplorationController::new(config.exploration.clone()); + let matcher = CorrelativeMatcher::new(config.matching.clone()); + let loop_detector = LoopClosureDetector::new(config.loop_closure.clone()); + let pose_graph = PoseGraph::new(config.pose_graph.clone()); + + Self { + config, + map, + controller, + matcher, + loop_detector, + pose_graph, + last_pose: Pose2D::default(), + last_odom_pose: None, + start_time: None, + started: false, + } + } + + /// Create with default configuration. + pub fn with_defaults() -> Self { + Self::new(ExplorerConfig::default()) + } + + /// Get the current map. + pub fn map(&self) -> &OccupancyGridMap { + &self.map + } + + /// Get exploration progress. + pub fn progress(&self) -> ExplorationProgress { + let mut progress = self.controller.progress(); + let map_stats = self.map.coverage_stats(); + progress.frontiers_explored = map_stats.frontier_count; + progress + } + + /// Get the current corrected pose. + pub fn current_pose(&self) -> Pose2D { + self.last_pose + } + + /// Get elapsed time since exploration started. + pub fn elapsed_secs(&self) -> f32 { + self.start_time + .map(|t| t.elapsed().as_secs_f32()) + .unwrap_or(0.0) + } + + /// Run exploration to completion (blocking). + /// + /// This method loops until exploration is complete or fails. + /// It handles all sensor integration, SLAM, and navigation internally. + /// + /// # Arguments + /// * `source` - Sensor source providing lidar, odometry, and accepting commands + /// + /// # Returns + /// `ExploreResult::Complete` with the finished map, or + /// `ExploreResult::Failed` with a partial map and reason. + pub fn explore(mut self, source: &mut S) -> ExploreResult { + // Start exploration + self.controller.start(); + self.start_time = Some(Instant::now()); + self.started = true; + + // Control loop + let step_duration = std::time::Duration::from_secs_f32(1.0 / self.config.update_rate_hz); + + loop { + let step_start = Instant::now(); + + // Check connection + if !source.is_connected() { + source.stop(); + return ExploreResult::Failed { + map: self.map, + reason: "Robot connection lost".to_string(), + }; + } + + // Check battery + if let Some(battery) = source.battery_percent() + && battery < self.config.min_battery_percent + { + source.stop(); + return ExploreResult::Failed { + map: self.map, + reason: format!("Battery low: {}%", battery), + }; + } + + // Check time limit + if self.config.max_time_secs > 0.0 && self.elapsed_secs() > self.config.max_time_secs { + source.stop(); + return ExploreResult::Failed { + map: self.map, + reason: "Time limit exceeded".to_string(), + }; + } + + // Run one exploration step + match self.step(source) { + ExploreStatus::InProgress { .. } => { + // Continue + } + ExploreStatus::Complete => { + source.stop(); + return ExploreResult::Complete(self.map); + } + ExploreStatus::Failed { reason } => { + source.stop(); + return ExploreResult::Failed { + map: self.map, + reason, + }; + } + } + + // Rate limiting + let elapsed = step_start.elapsed(); + if elapsed < step_duration { + std::thread::sleep(step_duration - elapsed); + } + } + } + + /// Run a single exploration step (non-blocking). + /// + /// Use this for custom control loops or when you need to + /// interleave exploration with other tasks. + /// + /// # Arguments + /// * `source` - Sensor source + /// + /// # Returns + /// Current exploration status. + pub fn step(&mut self, source: &mut S) -> ExploreStatus { + // Initialize if needed + if !self.started { + self.controller.start(); + self.start_time = Some(Instant::now()); + self.started = true; + } + + // Poll sensors + source.poll(); + + // Get current odometry pose + let odom_pose = source.get_pose(); + + // Calculate odometry delta + let odom_delta = self.last_odom_pose.map(|last| { + Pose2D::new( + odom_pose.x - last.x, + odom_pose.y - last.y, + odom_pose.theta - last.theta, + ) + }); + self.last_odom_pose = Some(odom_pose); + + // Process lidar scan if available + if let Some(scan) = source.get_lidar_scan() { + // Scan matching + let match_result = self + .matcher + .match_scan(&scan, odom_pose, self.map.storage()); + + // Use matched pose if good, else use odometry + let corrected_pose = if match_result.converged { + match_result.pose + } else { + odom_pose + }; + + // Integrate scan into map + self.map.observe_lidar(&scan, corrected_pose); + + // Update pose graph + self.pose_graph.add_pose(corrected_pose, odom_delta); + + // Check for loop closure + if let Some(closure) = self.loop_detector.add_scan(&scan, corrected_pose) { + self.pose_graph.add_loop_closure(closure); + // Optimize when loop detected + self.pose_graph.optimize(); + } + + self.last_pose = corrected_pose; + } else if let Some(delta) = odom_delta { + // No scan available - update pose from odometry delta + // This prevents the controller from using stale positions + self.last_pose = Pose2D::new( + self.last_pose.x + delta.x, + self.last_pose.y + delta.y, + self.last_pose.theta + delta.theta, + ); + } + + // Handle cliff/bumper events + if source.is_cliff_detected() { + self.controller + .handle_event(crate::exploration::ExplorationEvent::ObstacleDetected); + } + if source.is_bumper_triggered() { + self.controller + .handle_event(crate::exploration::ExplorationEvent::ObstacleDetected); + } + + // Update exploration controller + let command = self.controller.update(self.last_pose, self.map.storage()); + + // Execute command + match command { + ExplorationCommand::MoveTo { target, max_speed } => { + let (linear, angular) = + self.compute_velocity(self.last_pose, target.x, target.y, max_speed); + source.send_velocity(linear, angular); + } + ExplorationCommand::Rotate { + target_heading, + max_angular_speed, + } => { + let angular = self.compute_angular_velocity( + self.last_pose.theta, + target_heading, + max_angular_speed, + ); + source.send_velocity(0.0, angular); + } + ExplorationCommand::Stop => { + source.stop(); + } + ExplorationCommand::ExplorationComplete => { + source.stop(); + return ExploreStatus::Complete; + } + ExplorationCommand::ExplorationFailed { reason } => { + source.stop(); + return ExploreStatus::Failed { reason }; + } + ExplorationCommand::None => { + // Continue with current velocity or stop + } + } + + ExploreStatus::InProgress { + progress: self.controller.progress(), + } + } + + /// Compute velocity command to move towards target. + fn compute_velocity( + &self, + pose: Pose2D, + target_x: f32, + target_y: f32, + max_speed: f32, + ) -> (f32, f32) { + let dx = target_x - pose.x; + let dy = target_y - pose.y; + let distance = (dx * dx + dy * dy).sqrt(); + + if distance < 0.05 { + return (0.0, 0.0); + } + + let target_angle = dy.atan2(dx); + let angle_error = normalize_angle(target_angle - pose.theta); + + // Turn towards target first if angle is large + if angle_error.abs() > 0.3 { + let angular = angle_error.signum() * 2.0f32.min(angle_error.abs() * 2.0); + return (0.0, angular); + } + + // Move forward with proportional angular correction + let linear = max_speed.min(distance); + let angular = (angle_error * 2.0).clamp(-2.0, 2.0); + + (linear, angular) + } + + /// Compute angular velocity to rotate to target heading. + fn compute_angular_velocity(&self, current: f32, target: f32, max_speed: f32) -> f32 { + let error = normalize_angle(target - current); + if error.abs() < 0.05 { + 0.0 + } else { + (error * 2.0).clamp(-max_speed, max_speed) + } + } + + /// Reset explorer for a new exploration run. + pub fn reset(&mut self) { + let map_config = self.config.to_map_config(); + self.map = OccupancyGridMap::new(map_config); + self.controller = ExplorationController::new(self.config.exploration.clone()); + self.loop_detector.reset(); + self.pose_graph.reset(); + self.last_pose = Pose2D::default(); + self.last_odom_pose = None; + self.start_time = None; + self.started = false; + } +} + +/// Normalize angle to [-π, π). +pub fn normalize_angle(angle: f32) -> f32 { + let mut a = angle % std::f32::consts::TAU; + if a > std::f32::consts::PI { + a -= std::f32::consts::TAU; + } else if a < -std::f32::consts::PI { + a += std::f32::consts::TAU; + } + a +} + +#[cfg(test)] +mod tests { + use super::*; + + #[test] + fn test_explorer_creation() { + let explorer = VastuExplorer::with_defaults(); + assert!(!explorer.started); + assert_eq!(explorer.elapsed_secs(), 0.0); + } + + #[test] + fn test_normalize_angle() { + assert!((normalize_angle(0.0) - 0.0).abs() < 0.001); + assert!((normalize_angle(std::f32::consts::PI * 2.0) - 0.0).abs() < 0.001); + } +} diff --git a/vastu-map/src/explore/mod.rs b/vastu-map/src/explore/mod.rs new file mode 100644 index 0000000..255d879 --- /dev/null +++ b/vastu-map/src/explore/mod.rs @@ -0,0 +1,40 @@ +//! High-level autonomous exploration API. +//! +//! This module provides a simple, unified API for autonomous map exploration. +//! The `VastuExplorer` handles all the complexity of SLAM, scan matching, +//! loop closure, and frontier-based exploration internally. +//! +//! # Example +//! +//! ```ignore +//! use vastu_map::explore::{VastuExplorer, SensorSource, ExplorerConfig}; +//! +//! // Implement SensorSource for your robot +//! struct MyRobot { /* ... */ } +//! impl SensorSource for MyRobot { /* ... */ } +//! +//! // Create explorer and run +//! let config = ExplorerConfig::default(); +//! let mut explorer = VastuExplorer::new(config); +//! let mut robot = MyRobot::new(); +//! +//! match explorer.explore(&mut robot) { +//! ExploreResult::Complete(map) => { +//! // Save the completed map +//! vastu_map::io::save_vastu(map.storage(), "map.vastu").unwrap(); +//! } +//! ExploreResult::Failed { map, reason } => { +//! println!("Exploration failed: {}", reason); +//! } +//! } +//! ``` + +mod config; +mod explorer; +mod source; +mod submap_explorer; + +pub use config::ExplorerConfig; +pub use explorer::{ExploreResult, ExploreStatus, VastuExplorer, normalize_angle}; +pub use source::SensorSource; +pub use submap_explorer::{SubmapExplorerConfig, VastuSubmapExplorer}; diff --git a/vastu-map/src/explore/source.rs b/vastu-map/src/explore/source.rs new file mode 100644 index 0000000..d50de34 --- /dev/null +++ b/vastu-map/src/explore/source.rs @@ -0,0 +1,101 @@ +//! Sensor source trait for robot abstraction. + +use crate::core::{LidarScan, Pose2D}; + +/// Trait for providing sensor data and accepting commands. +/// +/// Implement this trait to connect VastuExplorer to your robot hardware +/// or simulation environment. +/// +/// # Example +/// +/// ```ignore +/// struct MyRobot { +/// // Hardware connections +/// } +/// +/// impl SensorSource for MyRobot { +/// fn get_pose(&self) -> Pose2D { +/// // Read from encoders/odometry +/// Pose2D::new(self.x, self.y, self.theta) +/// } +/// +/// fn get_lidar_scan(&self) -> Option { +/// // Read from lidar sensor +/// Some(self.latest_scan.clone()) +/// } +/// +/// fn send_velocity(&mut self, linear: f32, angular: f32) { +/// // Send to motor controller +/// self.motor.set_velocity(linear, angular); +/// } +/// +/// fn is_connected(&self) -> bool { +/// self.connection.is_alive() +/// } +/// } +/// ``` +pub trait SensorSource { + /// Get current robot pose from odometry/encoders. + /// + /// This is the initial pose estimate before scan matching. + /// The pose should be in the world frame (x, y in meters, theta in radians). + fn get_pose(&self) -> Pose2D; + + /// Get the latest lidar scan. + /// + /// Returns None if no new scan is available since the last call. + /// The scan should contain ranges in meters and angles in radians. + fn get_lidar_scan(&self) -> Option; + + /// Send velocity command to the robot. + /// + /// # Arguments + /// * `linear` - Forward velocity in m/s (positive = forward) + /// * `angular` - Angular velocity in rad/s (positive = counter-clockwise) + fn send_velocity(&mut self, linear: f32, angular: f32); + + /// Check if the robot is still connected and operational. + /// + /// Returns false if the connection is lost or the robot is in an error state. + /// The explorer will stop if this returns false. + fn is_connected(&self) -> bool; + + /// Update sensor readings (optional polling). + /// + /// Called at the beginning of each exploration step. + /// Use this to poll sensors if they don't push data automatically. + /// Default implementation does nothing. + fn poll(&mut self) {} + + /// Get cliff sensor state (optional). + /// + /// Returns true if any cliff sensor is triggered. + /// Default returns false (no cliff sensors). + fn is_cliff_detected(&self) -> bool { + false + } + + /// Get bumper state (optional). + /// + /// Returns true if any bumper is triggered. + /// Default returns false (no bumpers). + fn is_bumper_triggered(&self) -> bool { + false + } + + /// Stop the robot immediately (emergency or normal stop). + /// + /// Default implementation sends zero velocity. + fn stop(&mut self) { + self.send_velocity(0.0, 0.0); + } + + /// Get current battery level as percentage (0-100). + /// + /// Returns None if battery monitoring is not available. + /// Default returns None. + fn battery_percent(&self) -> Option { + None + } +} diff --git a/vastu-map/src/explore/submap_explorer.rs b/vastu-map/src/explore/submap_explorer.rs new file mode 100644 index 0000000..657f5ba --- /dev/null +++ b/vastu-map/src/explore/submap_explorer.rs @@ -0,0 +1,548 @@ +//! VastuSubmapExplorer - Submap-based autonomous exploration. +//! +//! This explorer uses the submap architecture for proper loop closure correction. +//! Unlike `VastuExplorer` which uses a single global map, this version: +//! - Stores raw scans in submaps for reversible map updates +//! - Uses submap-level pose graph optimization +//! - Can correctly apply loop closure corrections without ghost walls + +use std::time::Instant; + +use crate::core::{LidarScan, Pose2D}; +use crate::exploration::{ExplorationCommand, ExplorationController, ExplorationProgress}; +use crate::grid::GridStorage; +use crate::slam::CorrelativeMatcherConfig; +use crate::submap::{ + MultiSubmapMatchConfig, SubmapConfig, SubmapLoopClosure, SubmapManager, SubmapPoseGraph, +}; + +use super::config::ExplorerConfig; +use super::explorer::{ExploreResult, ExploreStatus, normalize_angle}; +use super::source::SensorSource; + +/// Configuration specific to submap-based exploration. +#[derive(Clone, Debug)] +pub struct SubmapExplorerConfig { + /// Base explorer configuration. + pub base: ExplorerConfig, + /// Submap configuration. + pub submap: SubmapConfig, + /// Multi-submap matching configuration. + pub multi_match: MultiSubmapMatchConfig, + /// Minimum score to accept a loop closure. + pub loop_closure_min_score: f32, + /// Minimum submap gap for loop closure (submaps between). + pub loop_closure_min_gap: usize, +} + +impl Default for SubmapExplorerConfig { + fn default() -> Self { + Self { + base: ExplorerConfig::default(), + submap: SubmapConfig::default(), + multi_match: MultiSubmapMatchConfig::default(), + loop_closure_min_score: 0.7, + loop_closure_min_gap: 3, + } + } +} + +impl SubmapExplorerConfig { + /// Create a fast configuration for quick testing. + pub fn fast() -> Self { + Self { + base: ExplorerConfig::fast(), + submap: SubmapConfig { + scans_per_submap: 30, + overlap_scans: 5, + ..Default::default() + }, + ..Default::default() + } + } +} + +/// High-level submap-based exploration controller. +/// +/// This explorer uses the submap architecture for improved SLAM accuracy, +/// especially during loop closure. When a loop is detected, submap origins +/// are corrected and grids are regenerated from stored scans. +/// +/// # Key Differences from VastuExplorer +/// +/// 1. **Submap Management**: Scans are organized into locally-consistent submaps +/// 2. **Reversible Updates**: Raw scans are stored for regeneration after optimization +/// 3. **Submap-level Pose Graph**: Fewer nodes (one per submap) for efficient optimization +/// 4. **Multi-submap Matching**: Can match against active + overlapping finalized submaps +pub struct VastuSubmapExplorer { + /// Configuration. + config: SubmapExplorerConfig, + /// Submap manager (replaces OccupancyGridMap). + manager: SubmapManager, + /// Submap-level pose graph. + pose_graph: SubmapPoseGraph, + /// Exploration controller. + controller: ExplorationController, + /// Matcher configuration. + matcher_config: CorrelativeMatcherConfig, + /// Last corrected world pose. + last_pose: Pose2D, + /// Last odometry pose (for delta calculation). + last_odom_pose: Option, + /// Last submap origin pose (for computing delta between submaps). + last_submap_origin: Option, + /// Scan counter for timestamps. + scan_count: u64, + /// Last submap count (to detect new submap creation). + last_submap_count: usize, + /// Start time. + start_time: Option, + /// Whether exploration has started. + started: bool, +} + +impl VastuSubmapExplorer { + /// Create a new submap explorer with the given configuration. + pub fn new(config: SubmapExplorerConfig) -> Self { + let map_config = config.base.to_map_config(); + let manager = SubmapManager::new(config.submap.clone(), &map_config); + let pose_graph = SubmapPoseGraph::new(Default::default()); + let controller = ExplorationController::new(config.base.exploration.clone()); + let matcher_config = config.base.matching.clone(); + + Self { + config, + manager, + pose_graph, + controller, + matcher_config, + last_pose: Pose2D::default(), + last_odom_pose: None, + last_submap_origin: None, + scan_count: 0, + last_submap_count: 0, + start_time: None, + started: false, + } + } + + /// Create with default configuration. + pub fn with_defaults() -> Self { + Self::new(SubmapExplorerConfig::default()) + } + + /// Get a reference to the current global grid for visualization/planning. + /// + /// This lazily regenerates the global grid if needed. + pub fn global_grid(&mut self) -> &GridStorage { + self.manager.global_grid() + } + + /// Get the submap manager for inspection. + pub fn manager(&self) -> &SubmapManager { + &self.manager + } + + /// Get exploration progress. + pub fn progress(&self) -> ExplorationProgress { + self.controller.progress() + } + + /// Get the current corrected pose. + pub fn current_pose(&self) -> Pose2D { + self.last_pose + } + + /// Get elapsed time since exploration started. + pub fn elapsed_secs(&self) -> f32 { + self.start_time + .map(|t| t.elapsed().as_secs_f32()) + .unwrap_or(0.0) + } + + /// Get the number of submaps created. + pub fn submap_count(&self) -> usize { + self.manager.submaps().len() + } + + /// Get the number of scans processed. + pub fn scan_count(&self) -> u64 { + self.scan_count + } + + /// Has a loop closure been detected and applied? + pub fn has_loop_closures(&self) -> bool { + self.pose_graph.has_loops() + } + + /// Run exploration to completion (blocking). + pub fn explore(mut self, source: &mut S) -> ExploreResult { + self.controller.start(); + self.start_time = Some(Instant::now()); + self.started = true; + + let step_duration = + std::time::Duration::from_secs_f32(1.0 / self.config.base.update_rate_hz); + + loop { + let step_start = Instant::now(); + + // Check connection + if !source.is_connected() { + source.stop(); + return ExploreResult::Failed { + map: self.to_occupancy_grid_map(), + reason: "Robot connection lost".to_string(), + }; + } + + // Check battery + if let Some(battery) = source.battery_percent() + && battery < self.config.base.min_battery_percent + { + source.stop(); + return ExploreResult::Failed { + map: self.to_occupancy_grid_map(), + reason: format!("Battery low: {}%", battery), + }; + } + + // Check time limit + if self.config.base.max_time_secs > 0.0 + && self.elapsed_secs() > self.config.base.max_time_secs + { + source.stop(); + return ExploreResult::Failed { + map: self.to_occupancy_grid_map(), + reason: "Time limit exceeded".to_string(), + }; + } + + // Run one exploration step + match self.step(source) { + ExploreStatus::InProgress { .. } => { + // Continue + } + ExploreStatus::Complete => { + source.stop(); + return ExploreResult::Complete(self.to_occupancy_grid_map()); + } + ExploreStatus::Failed { reason } => { + source.stop(); + return ExploreResult::Failed { + map: self.to_occupancy_grid_map(), + reason, + }; + } + } + + // Rate limiting + let elapsed = step_start.elapsed(); + if elapsed < step_duration { + std::thread::sleep(step_duration - elapsed); + } + } + } + + /// Run a single exploration step (non-blocking). + pub fn step(&mut self, source: &mut S) -> ExploreStatus { + // Initialize if needed + if !self.started { + self.controller.start(); + self.start_time = Some(Instant::now()); + self.started = true; + } + + // Poll sensors + source.poll(); + + // Get current odometry pose + let odom_pose = source.get_pose(); + + // Calculate odometry delta + let odom_delta = self.last_odom_pose.map(|last| { + Pose2D::new( + odom_pose.x - last.x, + odom_pose.y - last.y, + odom_pose.theta - last.theta, + ) + }); + self.last_odom_pose = Some(odom_pose); + + // Process lidar scan if available + if let Some(scan) = source.get_lidar_scan() { + let corrected_pose = self.process_scan(&scan, odom_pose); + self.last_pose = corrected_pose; + } else if let Some(delta) = odom_delta { + // No scan - update from odometry delta + self.last_pose = Pose2D::new( + self.last_pose.x + delta.x, + self.last_pose.y + delta.y, + self.last_pose.theta + delta.theta, + ); + } + + // Handle cliff/bumper events + if source.is_cliff_detected() || source.is_bumper_triggered() { + self.controller + .handle_event(crate::exploration::ExplorationEvent::ObstacleDetected); + } + + // Get current global grid for planning + let global_grid = self.manager.global_grid(); + + // Update exploration controller + let command = self.controller.update(self.last_pose, global_grid); + + // Execute command + match command { + ExplorationCommand::MoveTo { target, max_speed } => { + let (linear, angular) = + self.compute_velocity(self.last_pose, target.x, target.y, max_speed); + source.send_velocity(linear, angular); + } + ExplorationCommand::Rotate { + target_heading, + max_angular_speed, + } => { + let angular = self.compute_angular_velocity( + self.last_pose.theta, + target_heading, + max_angular_speed, + ); + source.send_velocity(0.0, angular); + } + ExplorationCommand::Stop => { + source.stop(); + } + ExplorationCommand::ExplorationComplete => { + source.stop(); + return ExploreStatus::Complete; + } + ExplorationCommand::ExplorationFailed { reason } => { + source.stop(); + return ExploreStatus::Failed { reason }; + } + ExplorationCommand::None => { + // Continue with current velocity or stop + } + } + + ExploreStatus::InProgress { + progress: self.controller.progress(), + } + } + + /// Process a lidar scan with submap-based SLAM. + fn process_scan(&mut self, scan: &LidarScan, odom_pose: Pose2D) -> Pose2D { + // Match against active + overlapping submaps + let match_result = self.manager.match_scan_multi( + scan, + odom_pose, + &self.matcher_config, + &self.config.multi_match, + ); + + // Use matched pose if converged + let corrected_pose = if match_result.converged() { + match_result.world_pose() + } else { + odom_pose + }; + + // Track submap count before insert + let submaps_before = self.manager.submaps().len(); + + // Insert scan into submap manager + self.scan_count += 1; + let _insert_result = self + .manager + .insert_scan(scan, corrected_pose, self.scan_count); + + // Check if new submap was created + let submaps_after = self.manager.submaps().len(); + if submaps_after > submaps_before { + // New submap created - add to pose graph + if let Some(active) = self.manager.active_submap() { + // Compute delta from previous submap origin + let odom_delta = self.last_submap_origin.map(|prev| { + Pose2D::new( + active.origin.x - prev.x, + active.origin.y - prev.y, + active.origin.theta - prev.theta, + ) + }); + + self.pose_graph + .add_submap(active.id, active.origin, odom_delta); + self.last_submap_origin = Some(active.origin); + } + + // Check for loop closure candidates + self.check_loop_closure(scan, corrected_pose); + + self.last_submap_count = submaps_after; + } + + corrected_pose + } + + /// Check for loop closure against finalized submaps. + fn check_loop_closure(&mut self, scan: &LidarScan, current_pose: Pose2D) { + use crate::submap::SubmapId; + + // Collect candidate info to avoid borrow issues + let candidates: Vec<(SubmapId, Pose2D)> = self + .manager + .loop_closure_candidates(current_pose) + .iter() + .map(|s| (s.id, s.origin)) + .collect(); + + for (candidate_id, candidate_origin) in candidates { + // Skip if already have a loop edge to this submap + if self + .pose_graph + .loop_edges() + .iter() + .any(|e| e.from_id == candidate_id || e.to_id == candidate_id) + { + continue; + } + + // Get the candidate submap's grid for matching + let Some(candidate) = self.manager.get_submap(candidate_id) else { + continue; + }; + + // Match against the candidate submap + let matcher = crate::slam::CorrelativeMatcher::new(self.matcher_config.clone()); + let local_pose = candidate_origin.inverse().compose(¤t_pose); + let match_result = matcher.match_scan(scan, local_pose, &candidate.grid); + + if match_result.converged && match_result.score >= self.config.loop_closure_min_score { + // Good match - add loop closure constraint + let matched_world_pose = candidate_origin.compose(&match_result.pose); + + if let Some(active) = self.manager.active_submap() { + // Compute relative pose between submaps + let relative_pose = candidate_origin.inverse().compose(&matched_world_pose); + + let closure = SubmapLoopClosure { + from_submap: active.id, + to_submap: candidate_id, + relative_pose, + confidence: match_result.score, + }; + + self.pose_graph.add_loop_closure(closure); + + // Optimize pose graph + let (_, corrections) = self.pose_graph.optimize(); + + // Apply corrections to submaps + if !corrections.is_empty() { + self.manager.apply_corrections(&corrections); + } + } + } + } + } + + /// Convert to OccupancyGridMap for compatibility with ExploreResult. + fn to_occupancy_grid_map(&mut self) -> crate::OccupancyGridMap { + let map_config = self.config.base.to_map_config(); + let mut map = crate::OccupancyGridMap::new(map_config); + + // Copy global grid contents + let global = self.manager.global_grid(); + let dest_storage = map.storage_mut(); + + // Copy each cell from global grid to map storage + for (coord, cell) in global.iter() { + if let Some(dest_cell) = dest_storage.get_mut(coord) { + *dest_cell.cell_type = cell.cell_type as u8; + *dest_cell.observation_count = cell.observation_count; + *dest_cell.swept = if cell.swept { 1 } else { 0 }; + } + } + + map + } + + /// Compute velocity command to move towards target. + fn compute_velocity( + &self, + pose: Pose2D, + target_x: f32, + target_y: f32, + max_speed: f32, + ) -> (f32, f32) { + let dx = target_x - pose.x; + let dy = target_y - pose.y; + let distance = (dx * dx + dy * dy).sqrt(); + + if distance < 0.05 { + return (0.0, 0.0); + } + + let target_angle = dy.atan2(dx); + let angle_error = normalize_angle(target_angle - pose.theta); + + // Turn towards target first if angle is large + if angle_error.abs() > 0.3 { + let angular = angle_error.signum() * 2.0f32.min(angle_error.abs() * 2.0); + return (0.0, angular); + } + + // Move forward with proportional angular correction + let linear = max_speed.min(distance); + let angular = (angle_error * 2.0).clamp(-2.0, 2.0); + + (linear, angular) + } + + /// Compute angular velocity to rotate to target heading. + fn compute_angular_velocity(&self, current: f32, target: f32, max_speed: f32) -> f32 { + let error = normalize_angle(target - current); + if error.abs() < 0.05 { + 0.0 + } else { + (error * 2.0).clamp(-max_speed, max_speed) + } + } + + /// Reset explorer for a new exploration run. + pub fn reset(&mut self) { + let map_config = self.config.base.to_map_config(); + self.manager = SubmapManager::new(self.config.submap.clone(), &map_config); + self.pose_graph = SubmapPoseGraph::new(Default::default()); + self.controller = ExplorationController::new(self.config.base.exploration.clone()); + self.last_pose = Pose2D::default(); + self.last_odom_pose = None; + self.last_submap_origin = None; + self.scan_count = 0; + self.last_submap_count = 0; + self.start_time = None; + self.started = false; + } +} + +#[cfg(test)] +mod tests { + use super::*; + + #[test] + fn test_submap_explorer_creation() { + let explorer = VastuSubmapExplorer::with_defaults(); + assert!(!explorer.started); + assert_eq!(explorer.scan_count(), 0); + assert_eq!(explorer.submap_count(), 0); + } + + #[test] + fn test_submap_explorer_config() { + let config = SubmapExplorerConfig::fast(); + assert_eq!(config.submap.scans_per_submap, 30); + } +} diff --git a/vastu-map/src/extraction/README.md b/vastu-map/src/extraction/README.md deleted file mode 100644 index 84877e3..0000000 --- a/vastu-map/src/extraction/README.md +++ /dev/null @@ -1,98 +0,0 @@ -# Extraction Module - -Feature extraction algorithms for converting point clouds to geometric features. - -## Components - -| File | Description | -|------|-------------| -| `line_fitting.rs` | Total Least Squares (TLS) line fitting with weighted variants | -| `split_merge/` | Split-and-Merge algorithm for line extraction | -| `corner_detection.rs` | Corner detection at line intersections | -| `traits.rs` | `LineExtractor` trait for extensibility | - -## Algorithms - -### Line Fitting (Total Least Squares) - -Orthogonal regression minimizes perpendicular distance to the line: - -```rust -// Fit line to points -let line = fit_line(&points); - -// Weighted fitting (accounts for range-based uncertainty) -let weights = compute_range_weights(&points, sensor_pos); -let line = fit_line_weighted(&points, &weights); - -// Fit from sensor position (auto-computes weights) -let line = fit_line_from_sensor(&points, sensor_pos, None); -``` - -### Split-and-Merge - -Recursive algorithm for extracting lines from ordered point sequences: - -1. **Split**: Find point with maximum distance from line -2. **Recurse**: If distance > threshold, split at that point -3. **Merge**: Combine collinear adjacent segments - -```rust -let config = SplitMergeConfig::default(); -let lines = extract_lines(&points, &config); - -// With sensor position for range weighting -let lines = extract_lines_from_sensor(&points, sensor_pos, &config); - -// Adaptive threshold based on point density -let threshold = adaptive_split_threshold(&points, base_threshold); -``` - -Configuration options: -- `split_threshold`: Max perpendicular distance before split (meters) -- `min_segment_points`: Minimum points per segment -- `min_segment_length`: Minimum segment length (meters) -- `merge_max_angle`: Max angle difference for merging (radians) -- `merge_max_gap`: Max gap between segments for merging (meters) - -### Corner Detection - -Find corners at line intersections: - -```rust -let config = CornerConfig::default(); -let corners = detect_corners(&lines, &config); - -// Curvature-based detection (from point cloud) -let corners = detect_curvature_corners(&points, &config); - -// Hybrid: intersection + curvature -let corners = detect_hybrid_corners(&lines, &points, &config); - -// Remove duplicate corners -let unique = deduplicate_corners(&corners, min_distance); -``` - -## Extensibility - -Implement `LineExtractor` trait for custom extraction algorithms: - -```rust -pub trait LineExtractor: Send + Sync { - fn extract(&self, points: &[Point2D]) -> Vec; - fn extract_with_pose(&self, points: &[Point2D], sensor_pose: Pose2D) -> Vec; -} -``` - -Built-in implementation: -- `SplitMergeExtractor`: Default split-merge algorithm - -## Pipeline - -Typical extraction pipeline: - -``` -PolarScan → PointCloud2D → Split-Merge → Lines → Corner Detection → Corners - │ │ │ - └─ to_cartesian() └─ extract_lines() └─ detect_corners() -``` diff --git a/vastu-map/src/extraction/corner_detection.rs b/vastu-map/src/extraction/corner_detection.rs deleted file mode 100644 index d62fa1c..0000000 --- a/vastu-map/src/extraction/corner_detection.rs +++ /dev/null @@ -1,784 +0,0 @@ -//! Corner detection from line segments and point curvature. -//! -//! This module provides two approaches to corner detection: -//! -//! 1. **Line Intersection**: Corners at intersections of adjacent line segments -//! that meet angle and distance criteria. -//! -//! 2. **Curvature-Based**: Corners at points where the local curvature exceeds -//! a threshold (sharp direction changes in the point sequence). -//! -//! The hybrid approach combines both methods for robust detection: -//! - Curvature catches corners even when line extraction fails -//! - Line intersection provides precise positioning -//! -//! # Example -//! -//! ```rust,ignore -//! use vastu_map::extraction::{CornerConfig, detect_hybrid_corners, extract_lines}; -//! use vastu_map::core::Point2D; -//! -//! let points = vec![/* lidar hits */]; -//! let lines = extract_lines(&points, &config); -//! let corners = detect_hybrid_corners(&points, &lines, &corner_config); -//! ``` - -use serde::{Deserialize, Serialize}; - -use crate::core::Point2D; -use crate::features::{Corner2D, Line2D}; -use std::f32::consts::PI; - -/// Configuration for corner detection. -#[derive(Clone, Debug, Serialize, Deserialize)] -pub struct CornerConfig { - /// Minimum angle between lines to form a corner (radians). - /// Lines that are nearly parallel (angle below this) don't form corners. - /// Default: π/6 (30°) - pub min_angle: f32, - - /// Maximum angle between lines to form a corner (radians). - /// Lines that are nearly collinear (angle above this) don't form corners. - /// Default: 5π/6 (150°) - pub max_angle: f32, - - /// Maximum distance between line endpoints to form a corner. - /// If the nearest endpoints are farther apart, no corner is detected. - /// Default: 0.05m (5cm) - pub max_endpoint_distance: f32, - - // ───────────────────────────────────────────────────────────────────────── - // Curvature Detection Parameters - // ───────────────────────────────────────────────────────────────────────── - /// Window size for curvature computation (number of points on each side). - /// Larger windows are more robust to noise but may miss sharp corners. - /// Default: 3 points (uses 7-point window: 3 before + center + 3 after) - pub curvature_window_size: usize, - - /// Minimum curvature angle (radians) to be considered a corner candidate. - /// This is the angle change between the before-segment and after-segment. - /// Default: π/4 (45°) - same as min_angle for consistency - pub curvature_threshold: f32, - - /// Distance for merging corners from different detection methods. - /// Corners within this distance are considered duplicates. - /// Default: 0.10m (10cm) - pub merge_distance: f32, - - /// Non-maximum suppression radius for curvature corners. - /// Only the highest curvature point within this radius is kept. - /// Default: 0.15m (15cm) - pub nms_radius: f32, -} - -impl Default for CornerConfig { - fn default() -> Self { - Self { - min_angle: PI / 6.0, // 30° - max_angle: 5.0 * PI / 6.0, // 150° - max_endpoint_distance: 0.05, // 5cm - - // Curvature detection - curvature_window_size: 3, // 7-point window - curvature_threshold: PI / 4.0, // 45° - merge_distance: 0.10, // 10cm - nms_radius: 0.15, // 15cm - } - } -} - -impl CornerConfig { - /// Create a new configuration with default values. - pub fn new() -> Self { - Self::default() - } - - /// Builder-style setter for minimum angle. - pub fn with_min_angle(mut self, radians: f32) -> Self { - self.min_angle = radians; - self - } - - /// Builder-style setter for maximum angle. - pub fn with_max_angle(mut self, radians: f32) -> Self { - self.max_angle = radians; - self - } - - /// Builder-style setter for maximum endpoint distance. - pub fn with_max_endpoint_distance(mut self, meters: f32) -> Self { - self.max_endpoint_distance = meters; - self - } - - /// Builder-style setter for curvature window size. - pub fn with_curvature_window_size(mut self, points: usize) -> Self { - self.curvature_window_size = points.max(1); - self - } - - /// Builder-style setter for curvature threshold. - pub fn with_curvature_threshold(mut self, radians: f32) -> Self { - self.curvature_threshold = radians; - self - } - - /// Builder-style setter for merge distance. - pub fn with_merge_distance(mut self, meters: f32) -> Self { - self.merge_distance = meters; - self - } - - /// Builder-style setter for NMS radius. - pub fn with_nms_radius(mut self, meters: f32) -> Self { - self.nms_radius = meters; - self - } -} - -/// Detect corners from adjacent line segments. -/// -/// Adjacent segments are checked for corner formation based on: -/// - Endpoint proximity -/// - Angle between lines -/// -/// # Arguments -/// * `lines` - Line segments (assumed to be in order, e.g., from split-merge) -/// * `config` - Corner detection configuration -/// -/// # Returns -/// Vector of detected corners with line indices and angles. -pub fn detect_corners(lines: &[Line2D], config: &CornerConfig) -> Vec { - if lines.len() < 2 { - return Vec::new(); - } - - let mut corners = Vec::new(); - - for i in 0..lines.len() - 1 { - if let Some(corner) = detect_corner_between(&lines[i], &lines[i + 1], i, i + 1, config) { - corners.push(corner); - } - } - - corners -} - -/// Detect corner between two specific lines. -fn detect_corner_between( - line1: &Line2D, - line2: &Line2D, - idx1: usize, - idx2: usize, - config: &CornerConfig, -) -> Option { - // Find nearest endpoints - let endpoints1 = [line1.start, line1.end]; - let endpoints2 = [line2.start, line2.end]; - - let mut min_dist = f32::MAX; - let mut corner_point = Point2D::zero(); - let mut _ep1_idx = 0; - let mut _ep2_idx = 0; - - for (i, &ep1) in endpoints1.iter().enumerate() { - for (j, &ep2) in endpoints2.iter().enumerate() { - let dist = ep1.distance(ep2); - if dist < min_dist { - min_dist = dist; - corner_point = Point2D::new((ep1.x + ep2.x) / 2.0, (ep1.y + ep2.y) / 2.0); - _ep1_idx = i; - _ep2_idx = j; - } - } - } - - // Check distance threshold - if min_dist > config.max_endpoint_distance { - return None; - } - - // Compute angle between lines - let angle = angle_between_lines(line1, line2); - - // Check angle thresholds - if angle < config.min_angle || angle > config.max_angle { - return None; - } - - // Alternatively, use the intersection point if lines actually intersect - if let Some(intersection) = line_intersection_point(line1, line2) { - // Use intersection if it's close to the endpoints - if intersection.distance(corner_point) < config.max_endpoint_distance * 2.0 { - corner_point = intersection; - } - } - - Some(Corner2D::new(corner_point, idx1, idx2, angle)) -} - -/// Compute the angle between two lines (0 to π). -/// -/// Returns the acute or obtuse angle at their intersection. -fn angle_between_lines(line1: &Line2D, line2: &Line2D) -> f32 { - let dir1 = line1.unit_direction(); - let dir2 = line2.unit_direction(); - - // Use absolute dot product to get angle regardless of direction - let dot = dir1.dot(dir2).abs(); - - // Clamp to handle floating-point errors - let dot = dot.clamp(-1.0, 1.0); - - // acos gives angle between directions - // We want the supplementary angle if > 90° - dot.acos() -} - -/// Find intersection point of two infinite lines. -fn line_intersection_point(line1: &Line2D, line2: &Line2D) -> Option { - let d1 = line1.direction(); - let d2 = line2.direction(); - - let cross = d1.cross(d2); - if cross.abs() < f32::EPSILON { - // Parallel lines - return None; - } - - let diff = line2.start - line1.start; - let t = diff.cross(d2) / cross; - - Some(line1.point_at(t)) -} - -/// Filter corners by removing duplicates within a tolerance. -/// -/// Corners at approximately the same position are merged, -/// keeping the one with highest observation count. -pub fn deduplicate_corners(corners: &mut Vec, distance_tolerance: f32) { - if corners.len() < 2 { - return; - } - - // Sort by x coordinate for spatial coherence - corners.sort_by(|a, b| a.position.x.partial_cmp(&b.position.x).unwrap()); - - let mut keep = vec![true; corners.len()]; - - for i in 0..corners.len() { - if !keep[i] { - continue; - } - - for j in i + 1..corners.len() { - if !keep[j] { - continue; - } - - // Early exit if too far in x - if corners[j].position.x - corners[i].position.x > distance_tolerance { - break; - } - - if corners[i].distance(&corners[j]) < distance_tolerance { - // Keep the one with higher observation count - if corners[j].observation_count > corners[i].observation_count { - keep[i] = false; - } else { - keep[j] = false; - } - } - } - } - - let mut idx = 0; - corners.retain(|_| { - let result = keep[idx]; - idx += 1; - result - }); -} - -// ============================================================================= -// Curvature-Based Corner Detection -// ============================================================================= - -/// Curvature candidate before non-maximum suppression. -#[derive(Clone, Debug)] -struct CurvatureCandidate { - position: Point2D, - point_idx: usize, - curvature: f32, // Angle change in radians -} - -/// Detect corners using curvature analysis on raw point cloud. -/// -/// This method finds sharp direction changes in the point sequence, -/// independent of line extraction. It's useful for detecting corners -/// that line extraction might miss. -/// -/// # Algorithm -/// 1. For each point, compute direction vectors to neighbors within window -/// 2. Compute angle between before-segment and after-segment directions -/// 3. Keep points where angle exceeds threshold -/// 4. Apply non-maximum suppression -/// -/// # Arguments -/// * `points` - Ordered point sequence (e.g., from lidar scan) -/// * `config` - Corner detection configuration -pub fn detect_curvature_corners(points: &[Point2D], config: &CornerConfig) -> Vec { - let window = config.curvature_window_size; - let min_points = 2 * window + 1; - - if points.len() < min_points { - return Vec::new(); - } - - // Step 1: Compute curvature at each point - let mut candidates = Vec::new(); - - for i in window..points.len() - window { - let curvature = compute_curvature_at(points, i, window); - - if curvature >= config.curvature_threshold { - candidates.push(CurvatureCandidate { - position: points[i], - point_idx: i, - curvature, - }); - } - } - - // Step 2: Non-maximum suppression - let filtered = non_maximum_suppression(&candidates, config.nms_radius); - - // Step 3: Convert to Corner2D - filtered - .into_iter() - .map(|c| { - // Use point index as both line indices (no line association) - Corner2D::new(c.position, c.point_idx, c.point_idx, c.curvature) - }) - .collect() -} - -/// Compute curvature (angle change) at a specific point. -/// -/// Uses points before and after within the window to compute direction vectors, -/// then calculates the angle between them. -fn compute_curvature_at(points: &[Point2D], center: usize, window: usize) -> f32 { - // Get points for direction computation - let before_start = center.saturating_sub(window); - let before_end = center; - let after_start = center; - let after_end = (center + window).min(points.len() - 1); - - // Compute direction vectors using regression through window points - let dir_before = compute_direction(&points[before_start..=before_end]); - let dir_after = compute_direction(&points[after_start..=after_end]); - - // Compute angle between directions - let dot = dir_before.dot(dir_after); - let cross = dir_before.cross(dir_after); - - // Use atan2 for signed angle, then take absolute value - cross.atan2(dot).abs() -} - -/// Compute average direction through a set of points. -fn compute_direction(points: &[Point2D]) -> Point2D { - if points.len() < 2 { - return Point2D::new(1.0, 0.0); - } - - // Use first and last points for direction - let dir = points[points.len() - 1] - points[0]; - let len = (dir.x * dir.x + dir.y * dir.y).sqrt(); - - if len < f32::EPSILON { - Point2D::new(1.0, 0.0) - } else { - Point2D::new(dir.x / len, dir.y / len) - } -} - -/// Apply non-maximum suppression to curvature candidates. -/// -/// Within each neighborhood, only keep the candidate with highest curvature. -fn non_maximum_suppression( - candidates: &[CurvatureCandidate], - radius: f32, -) -> Vec { - if candidates.is_empty() { - return Vec::new(); - } - - let mut keep = vec![true; candidates.len()]; - let radius_sq = radius * radius; - - for i in 0..candidates.len() { - if !keep[i] { - continue; - } - - for j in i + 1..candidates.len() { - if !keep[j] { - continue; - } - - let dx = candidates[i].position.x - candidates[j].position.x; - let dy = candidates[i].position.y - candidates[j].position.y; - let dist_sq = dx * dx + dy * dy; - - if dist_sq < radius_sq { - // Keep the one with higher curvature - if candidates[j].curvature > candidates[i].curvature { - keep[i] = false; - break; - } else { - keep[j] = false; - } - } - } - } - - candidates - .iter() - .enumerate() - .filter(|(i, _)| keep[*i]) - .map(|(_, c)| c.clone()) - .collect() -} - -// ============================================================================= -// Hybrid Corner Detection -// ============================================================================= - -/// Detect corners using both curvature and line intersection methods. -/// -/// This hybrid approach combines: -/// 1. Curvature-based detection on raw points (catches corners even without line extraction) -/// 2. Line intersection detection (provides precise positioning) -/// -/// Corners from both methods are merged and deduplicated. -/// -/// # Arguments -/// * `points` - Ordered point sequence (e.g., from lidar scan) -/// * `lines` - Extracted line segments -/// * `config` - Corner detection configuration -/// -/// # Example -/// ``` -/// use vastu_map::extraction::{CornerConfig, detect_hybrid_corners, extract_lines, SplitMergeConfig}; -/// use vastu_map::core::Point2D; -/// -/// // L-shaped points -/// let points = vec![ -/// Point2D::new(0.0, 0.0), -/// Point2D::new(0.5, 0.0), -/// Point2D::new(1.0, 0.0), -/// Point2D::new(1.0, 0.5), -/// Point2D::new(1.0, 1.0), -/// ]; -/// -/// let split_config = SplitMergeConfig::default(); -/// let lines = extract_lines(&points, &split_config); -/// let corner_config = CornerConfig::default(); -/// let corners = detect_hybrid_corners(&points, &lines, &corner_config); -/// ``` -pub fn detect_hybrid_corners( - points: &[Point2D], - lines: &[Line2D], - config: &CornerConfig, -) -> Vec { - // Step 1: Detect curvature-based corners - let mut curvature_corners = detect_curvature_corners(points, config); - - // Step 2: Detect line intersection corners - let intersection_corners = detect_corners(lines, config); - - // Step 3: Merge and deduplicate - // Intersection corners are generally more precise, so prefer them - curvature_corners.extend(intersection_corners); - - deduplicate_corners(&mut curvature_corners, config.merge_distance); - - curvature_corners -} - -#[cfg(test)] -mod tests { - use super::*; - use approx::assert_relative_eq; - use std::f32::consts::{FRAC_PI_2, FRAC_PI_4}; - - #[test] - fn test_detect_corner_perpendicular() { - // Two perpendicular lines meeting at origin - let line1 = Line2D::new(Point2D::new(-1.0, 0.0), Point2D::new(0.0, 0.0)); - let line2 = Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(0.0, 1.0)); - - let config = CornerConfig::default(); - let corners = detect_corners(&[line1, line2], &config); - - assert_eq!(corners.len(), 1); - assert_relative_eq!(corners[0].angle, FRAC_PI_2, epsilon = 0.1); - assert!(corners[0].position.distance(Point2D::zero()) < 0.1); - } - - #[test] - fn test_detect_corner_45_degrees() { - // Two lines meeting at 45 degrees - let line1 = Line2D::new(Point2D::new(-1.0, 0.0), Point2D::new(0.0, 0.0)); - let line2 = Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(1.0, 1.0)); - - let config = CornerConfig::default(); - let corners = detect_corners(&[line1, line2], &config); - - assert_eq!(corners.len(), 1); - assert_relative_eq!(corners[0].angle, FRAC_PI_4, epsilon = 0.1); - } - - #[test] - fn test_no_corner_parallel_lines() { - // Parallel lines don't form corners - let line1 = Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(1.0, 0.0)); - let line2 = Line2D::new(Point2D::new(1.01, 0.0), Point2D::new(2.0, 0.0)); - - let config = CornerConfig::default(); - let corners = detect_corners(&[line1, line2], &config); - - // Should be empty - lines are collinear - assert!(corners.is_empty()); - } - - #[test] - fn test_no_corner_too_far() { - // Lines too far apart - let line1 = Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(1.0, 0.0)); - let line2 = Line2D::new(Point2D::new(0.0, 1.0), Point2D::new(0.0, 2.0)); - - let config = CornerConfig::default().with_max_endpoint_distance(0.05); - let corners = detect_corners(&[line1, line2], &config); - - assert!(corners.is_empty()); - } - - #[test] - fn test_no_corner_angle_too_small() { - // Angle below threshold (nearly parallel) - let line1 = Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(1.0, 0.0)); - let line2 = Line2D::new(Point2D::new(1.0, 0.0), Point2D::new(2.0, 0.1)); // ~5.7° deviation - - let config = CornerConfig::default().with_min_angle(PI / 6.0); // 30° - let corners = detect_corners(&[line1, line2], &config); - - assert!(corners.is_empty()); - } - - #[test] - fn test_detect_multiple_corners() { - // Three lines forming two corners - let lines = vec![ - Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(1.0, 0.0)), - Line2D::new(Point2D::new(1.0, 0.0), Point2D::new(1.0, 1.0)), - Line2D::new(Point2D::new(1.0, 1.0), Point2D::new(0.0, 1.0)), - ]; - - let config = CornerConfig::default(); - let corners = detect_corners(&lines, &config); - - assert_eq!(corners.len(), 2); - } - - #[test] - fn test_angle_between_lines() { - // Perpendicular lines - let line1 = Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(1.0, 0.0)); - let line2 = Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(0.0, 1.0)); - - let angle = angle_between_lines(&line1, &line2); - assert_relative_eq!(angle, FRAC_PI_2, epsilon = 0.01); - - // Parallel lines - let line3 = Line2D::new(Point2D::new(0.0, 1.0), Point2D::new(1.0, 1.0)); - let angle2 = angle_between_lines(&line1, &line3); - assert_relative_eq!(angle2, 0.0, epsilon = 0.01); - } - - #[test] - fn test_deduplicate_corners() { - let mut corners = vec![ - Corner2D::new(Point2D::new(0.0, 0.0), 0, 1, FRAC_PI_2), - Corner2D::new(Point2D::new(0.01, 0.01), 0, 1, FRAC_PI_2), // Duplicate - Corner2D::new(Point2D::new(5.0, 5.0), 2, 3, FRAC_PI_2), - ]; - - deduplicate_corners(&mut corners, 0.05); - - assert_eq!(corners.len(), 2); - } - - #[test] - fn test_line_intersection() { - let line1 = Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(2.0, 2.0)); - let line2 = Line2D::new(Point2D::new(0.0, 2.0), Point2D::new(2.0, 0.0)); - - let intersection = line_intersection_point(&line1, &line2).unwrap(); - - assert_relative_eq!(intersection.x, 1.0, epsilon = 0.01); - assert_relative_eq!(intersection.y, 1.0, epsilon = 0.01); - } - - // ======================================== - // Curvature Detection Tests - // ======================================== - - #[test] - fn test_curvature_l_shape() { - // L-shaped points with sharp corner - let mut points = Vec::new(); - - // Horizontal segment - for i in 0..10 { - points.push(Point2D::new(i as f32 * 0.1, 0.0)); - } - - // Vertical segment - for i in 1..10 { - points.push(Point2D::new(0.9, i as f32 * 0.1)); - } - - let config = CornerConfig::default() - .with_curvature_window_size(2) - .with_curvature_threshold(PI / 4.0); // 45° - - let corners = detect_curvature_corners(&points, &config); - - // Should detect the corner at the L junction - assert!( - corners.len() >= 1, - "Expected at least 1 corner, got {}", - corners.len() - ); - - // Corner should be near (0.9, 0.0) - let has_corner_at_junction = corners - .iter() - .any(|c| c.position.distance(Point2D::new(0.9, 0.0)) < 0.2); - - assert!( - has_corner_at_junction, - "Expected corner near (0.9, 0.0), got {:?}", - corners.iter().map(|c| c.position).collect::>() - ); - } - - #[test] - fn test_curvature_no_corner_straight_line() { - // Straight line should have no curvature corners - let points: Vec<_> = (0..20).map(|i| Point2D::new(i as f32 * 0.1, 0.0)).collect(); - - let config = CornerConfig::default(); - let corners = detect_curvature_corners(&points, &config); - - assert!( - corners.is_empty(), - "Expected no corners on straight line, got {}", - corners.len() - ); - } - - #[test] - fn test_curvature_too_few_points() { - // Not enough points for the window - let points = vec![ - Point2D::new(0.0, 0.0), - Point2D::new(1.0, 0.0), - Point2D::new(1.0, 1.0), - ]; - - let config = CornerConfig::default().with_curvature_window_size(3); // needs 7 points - - let corners = detect_curvature_corners(&points, &config); - - assert!(corners.is_empty()); - } - - #[test] - fn test_hybrid_detection() { - // L-shaped points - let mut points = Vec::new(); - - // Horizontal segment (enough points for lines) - for i in 0..15 { - points.push(Point2D::new(i as f32 * 0.1, 0.0)); - } - - // Vertical segment - for i in 1..15 { - points.push(Point2D::new(1.4, i as f32 * 0.1)); - } - - // Extract lines - let split_config = crate::extraction::SplitMergeConfig::default() - .with_min_points(5) - .with_min_length(0.3); - let lines = crate::extraction::extract_lines(&points, &split_config); - - let config = CornerConfig::default() - .with_curvature_window_size(2) - .with_merge_distance(0.2); - - let corners = detect_hybrid_corners(&points, &lines, &config); - - // Should detect corner(s) at the junction - assert!( - !corners.is_empty(), - "Expected at least 1 corner from hybrid detection" - ); - } - - #[test] - fn test_non_maximum_suppression() { - let candidates = vec![ - CurvatureCandidate { - position: Point2D::new(0.0, 0.0), - point_idx: 0, - curvature: 1.0, - }, - CurvatureCandidate { - position: Point2D::new(0.05, 0.0), // Too close to first - point_idx: 1, - curvature: 0.5, - }, - CurvatureCandidate { - position: Point2D::new(1.0, 0.0), // Far enough - point_idx: 10, - curvature: 0.8, - }, - ]; - - let filtered = non_maximum_suppression(&candidates, 0.1); - - // Should keep first (higher curvature) and third (far enough) - assert_eq!(filtered.len(), 2); - assert_eq!(filtered[0].point_idx, 0); - assert_eq!(filtered[1].point_idx, 10); - } - - #[test] - fn test_config_curvature_builders() { - let config = CornerConfig::default() - .with_curvature_window_size(5) - .with_curvature_threshold(PI / 3.0) - .with_merge_distance(0.15) - .with_nms_radius(0.2); - - assert_eq!(config.curvature_window_size, 5); - assert_relative_eq!(config.curvature_threshold, PI / 3.0, epsilon = 0.01); - assert_relative_eq!(config.merge_distance, 0.15, epsilon = 0.01); - assert_relative_eq!(config.nms_radius, 0.2, epsilon = 0.01); - } -} diff --git a/vastu-map/src/extraction/line_fitting.rs b/vastu-map/src/extraction/line_fitting.rs deleted file mode 100644 index e565b3c..0000000 --- a/vastu-map/src/extraction/line_fitting.rs +++ /dev/null @@ -1,1054 +0,0 @@ -//! Line fitting using Total Least Squares (Orthogonal Regression). -//! -//! TLS minimizes perpendicular distances to the line, not vertical distances -//! like ordinary least squares. This is appropriate for lidar data where -//! errors are isotropic. -//! -//! # Weighted TLS -//! -//! For lidar data, measurement uncertainty increases with range. Points closer -//! to the sensor are more reliable than distant points. Weighted TLS allows -//! incorporating this range-based uncertainty into the fitting. -//! -//! ```rust,ignore -//! use vastu_map::extraction::fit_line_from_sensor; -//! use vastu_map::core::Point2D; -//! -//! let sensor_pos = Point2D::new(0.0, 0.0); // Robot position -//! let points = vec![/* lidar hits */]; -//! let line = fit_line_from_sensor(&points, sensor_pos, None); -//! ``` - -use std::simd::{f32x4, num::SimdFloat}; - -use crate::config::LidarNoiseModel; -use crate::core::Point2D; -use crate::core::math::{compute_centroid, compute_covariance, principal_direction}; -use crate::features::Line2D; - -// ============================================================================ -// Line Splitting -// ============================================================================ - -/// Split a line into smaller segments if it exceeds the maximum length. -/// -/// Returns a vector of line segments, each no longer than `max_length`. -/// The point counts are distributed proportionally across segments. -/// -/// # Arguments -/// * `line` - The line to potentially split -/// * `max_length` - Maximum length for each segment -/// -/// # Returns -/// Vector of line segments. If the line is already short enough, -/// returns a vector containing just the original line. -pub fn split_line_by_length(line: &Line2D, max_length: f32) -> Vec { - let length = line.length(); - - if length <= max_length || max_length <= 0.0 { - return vec![*line]; - } - - // Calculate number of segments needed - let num_segments = (length / max_length).ceil() as usize; - - // Direction vector (not normalized - used with t parameter 0..1) - let dir = line.direction(); - - let mut segments = Vec::with_capacity(num_segments); - - // Distribute point count proportionally - let points_per_segment = if line.point_count > 0 { - (line.point_count as f32 / num_segments as f32).ceil() as u32 - } else { - 0 - }; - - for i in 0..num_segments { - // Use t parameter (0 to 1) to interpolate along line - let t_start = i as f32 / num_segments as f32; - let t_end = (i + 1) as f32 / num_segments as f32; - - let start = Point2D::new( - line.start.x + dir.x * t_start, - line.start.y + dir.y * t_start, - ); - let end = if i == num_segments - 1 { - // Use exact endpoint for last segment to avoid floating point drift - line.end - } else { - Point2D::new(line.start.x + dir.x * t_end, line.start.y + dir.y * t_end) - }; - - // Each segment inherits observation_count=1, proportional point_count - let segment_point_count = if i == num_segments - 1 { - // Last segment gets remaining points - line.point_count - .saturating_sub(points_per_segment * (num_segments - 1) as u32) - } else { - points_per_segment - }; - - segments.push(Line2D::full(start, end, 1, segment_point_count)); - } - - segments -} - -// ============================================================================ -// Error Types -// ============================================================================ - -/// Errors that can occur during line fitting. -#[derive(Debug, Clone, Copy, PartialEq)] -pub enum FitError { - /// Not enough points to fit a line. - InsufficientPoints { - /// Number of points provided. - got: usize, - /// Minimum required (2). - required: usize, - }, - /// Points are collocated, form a circle, or are otherwise degenerate. - DegenerateGeometry, - /// Weights array length doesn't match points array length. - WeightsMismatch { - /// Number of points. - points_len: usize, - /// Number of weights. - weights_len: usize, - }, - /// Total weight is zero or negative. - InvalidWeights, -} - -impl std::fmt::Display for FitError { - fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result { - match self { - FitError::InsufficientPoints { got, required } => { - write!( - f, - "insufficient points for line fitting: got {}, need {}", - got, required - ) - } - FitError::DegenerateGeometry => { - write!( - f, - "degenerate geometry: points are collocated or form a circle" - ) - } - FitError::WeightsMismatch { - points_len, - weights_len, - } => { - write!( - f, - "weights length mismatch: {} points, {} weights", - points_len, weights_len - ) - } - FitError::InvalidWeights => { - write!(f, "invalid weights: total weight is zero or negative") - } - } - } -} - -impl std::error::Error for FitError {} - -/// Fit a line to a set of points using Total Least Squares. -/// -/// The algorithm: -/// 1. Compute centroid of points -/// 2. Compute covariance matrix -/// 3. Find eigenvector of smallest eigenvalue (perpendicular to line) -/// 4. Line direction is eigenvector of largest eigenvalue -/// -/// Returns None if: -/// - Fewer than 2 points -/// - Points are collinear in one direction (degenerate case) -/// -/// # Example -/// ``` -/// use vastu_map::extraction::fit_line; -/// use vastu_map::core::Point2D; -/// -/// let points = vec![ -/// Point2D::new(0.0, 0.0), -/// Point2D::new(1.0, 0.1), // Slight noise -/// Point2D::new(2.0, 0.0), -/// Point2D::new(3.0, -0.1), -/// Point2D::new(4.0, 0.0), -/// ]; -/// -/// let line = fit_line(&points).unwrap(); -/// // Line should be approximately horizontal -/// ``` -pub fn fit_line(points: &[Point2D]) -> Option { - fit_line_impl(points).ok() -} - -/// Internal implementation shared by fit_line and fit_line_checked. -fn fit_line_impl(points: &[Point2D]) -> Result { - if points.len() < 2 { - return Err(FitError::InsufficientPoints { - got: points.len(), - required: 2, - }); - } - - // Use shared geometry utilities - let centroid = compute_centroid(points); - let cov = compute_covariance(points, centroid); - - // Get principal direction from covariance matrix - let direction = match principal_direction(&cov) { - Some(dir) => dir, - None => { - // Degenerate case - try simple fallback - return fit_line_simple_impl(points); - } - }; - - // Find extent of points along line direction - let mut t_min = f32::MAX; - let mut t_max = f32::MIN; - - for p in points { - let t = (p.x - centroid.x) * direction.x + (p.y - centroid.y) * direction.y; - t_min = t_min.min(t); - t_max = t_max.max(t); - } - - // Create line from extent - let start = Point2D::new( - centroid.x + t_min * direction.x, - centroid.y + t_min * direction.y, - ); - let end = Point2D::new( - centroid.x + t_max * direction.x, - centroid.y + t_max * direction.y, - ); - - Ok(Line2D::with_point_count(start, end, points.len() as u32)) -} - -/// Simple line fitting fallback (shared implementation). -fn fit_line_simple_impl(points: &[Point2D]) -> Result { - if points.len() < 2 { - return Err(FitError::InsufficientPoints { - got: points.len(), - required: 2, - }); - } - Ok(Line2D::with_point_count( - points[0], - points[points.len() - 1], - points.len() as u32, - )) -} - -/// Simple line fitting fallback using first and last points. -fn fit_line_simple(points: &[Point2D]) -> Option { - fit_line_simple_impl(points).ok() -} - -/// Compute range-based weights for line fitting. -/// -/// Points closer to the sensor receive higher weights because they have -/// lower measurement uncertainty. Uses the formula: -/// σ(r) = σ_base + σ_range × r² -/// weight = 1 / σ(r)² -/// -/// # Arguments -/// * `points` - Points to compute weights for -/// * `sensor_pos` - Position of the sensor (typically robot position) -/// * `noise_model` - Lidar noise model parameters (uses default if None) -pub fn compute_range_weights( - points: &[Point2D], - sensor_pos: Point2D, - noise_model: Option<&LidarNoiseModel>, -) -> Vec { - let model = noise_model.cloned().unwrap_or_default(); - points - .iter() - .map(|p| { - let range = p.distance(sensor_pos); - model.weight(range) - }) - .collect() -} - -/// Fit a line to points with explicit weights using Weighted Total Least Squares. -/// -/// Each point contributes to the fit proportionally to its weight. -/// Higher weights mean the point has more influence on the result. -/// -/// # Algorithm -/// 1. Compute weighted centroid: c = Σ(w_i × p_i) / Σw_i -/// 2. Compute weighted covariance matrix: C = Σ(w_i × (p_i - c) × (p_i - c)ᵀ) -/// 3. Find eigenvector of largest eigenvalue (line direction) -/// 4. Project points onto line to find extent -/// -/// # Arguments -/// * `points` - Points to fit line to -/// * `weights` - Weights for each point (must match points length) -/// -/// # Returns -/// Fitted line segment, or None if points are degenerate -pub fn fit_line_weighted(points: &[Point2D], weights: &[f32]) -> Option { - if points.len() < 2 || weights.len() != points.len() { - return None; - } - - // Compute weighted centroid - let mut sum_w: f32 = 0.0; - let mut sum_wx: f32 = 0.0; - let mut sum_wy: f32 = 0.0; - - for (p, &w) in points.iter().zip(weights.iter()) { - sum_w += w; - sum_wx += w * p.x; - sum_wy += w * p.y; - } - - if sum_w < f32::EPSILON { - return fit_line_simple(points); - } - - let centroid = Point2D::new(sum_wx / sum_w, sum_wy / sum_w); - - // Compute weighted covariance matrix elements - // Cov = | cxx cxy | - // | cxy cyy | - let mut cxx: f32 = 0.0; - let mut cyy: f32 = 0.0; - let mut cxy: f32 = 0.0; - - for (p, &w) in points.iter().zip(weights.iter()) { - let dx = p.x - centroid.x; - let dy = p.y - centroid.y; - cxx += w * dx * dx; - cyy += w * dy * dy; - cxy += w * dx * dy; - } - - // Find eigenvectors of 2x2 covariance matrix - // For 2x2 symmetric matrix, eigenvalues are: - // λ = (cxx + cyy)/2 ± sqrt(((cxx - cyy)/2)² + cxy²) - - let trace_half = (cxx + cyy) / 2.0; - let det_sqrt = ((cxx - cyy) / 2.0).powi(2) + cxy * cxy; - - if det_sqrt < f32::EPSILON { - // Degenerate case - points form a circle or single point - return fit_line_simple(points); - } - - let det_sqrt = det_sqrt.sqrt(); - let lambda1 = trace_half + det_sqrt; // Larger eigenvalue - - // Eigenvector for larger eigenvalue (line direction) - let direction = if cxy.abs() > f32::EPSILON { - Point2D::new(lambda1 - cyy, cxy).normalized() - } else if cxx > cyy { - Point2D::new(1.0, 0.0) // Horizontal line - } else { - Point2D::new(0.0, 1.0) // Vertical line - }; - - // Find extent of points along line direction - let mut t_min = f32::MAX; - let mut t_max = f32::MIN; - - for p in points { - let t = (p.x - centroid.x) * direction.x + (p.y - centroid.y) * direction.y; - t_min = t_min.min(t); - t_max = t_max.max(t); - } - - // Create line from extent - let start = Point2D::new( - centroid.x + t_min * direction.x, - centroid.y + t_min * direction.y, - ); - let end = Point2D::new( - centroid.x + t_max * direction.x, - centroid.y + t_max * direction.y, - ); - - Some(Line2D::with_point_count(start, end, points.len() as u32)) -} - -/// Fit a line to points using range-based weighting from sensor position. -/// -/// This is the preferred method for fitting lines to lidar data, as it -/// accounts for the fact that closer measurements are more reliable. -/// -/// # Arguments -/// * `points` - Points to fit line to -/// * `sensor_pos` - Position of the sensor (typically robot position) -/// * `noise_model` - Lidar noise model parameters (uses default if None) -/// -/// # Example -/// ``` -/// use vastu_map::extraction::fit_line_from_sensor; -/// use vastu_map::core::Point2D; -/// -/// let sensor_pos = Point2D::new(0.0, 0.0); -/// let points = vec![ -/// Point2D::new(1.0, 0.0), // 1m away - high weight -/// Point2D::new(2.0, 0.05), // 2m away - medium weight -/// Point2D::new(4.0, 0.0), // 4m away - lower weight -/// ]; -/// -/// let line = fit_line_from_sensor(&points, sensor_pos, None).unwrap(); -/// // Line is influenced more by the closer point at (1.0, 0.0) -/// ``` -pub fn fit_line_from_sensor( - points: &[Point2D], - sensor_pos: Point2D, - noise_model: Option<&LidarNoiseModel>, -) -> Option { - if points.len() < 2 { - return None; - } - - let weights = compute_range_weights(points, sensor_pos, noise_model); - fit_line_weighted(points, &weights) -} - -/// Compute weighted fitting error (RMS distance to line, weighted by measurement quality). -/// -/// Returns the weighted root-mean-square of perpendicular distances. -pub fn fitting_error_weighted(points: &[Point2D], line: &Line2D, weights: &[f32]) -> f32 { - if points.is_empty() || weights.len() != points.len() { - return 0.0; - } - - let mut sum_w: f32 = 0.0; - let mut sum_w_sq: f32 = 0.0; - - for (p, &w) in points.iter().zip(weights.iter()) { - let dist = line.distance_to_point(*p); - sum_w += w; - sum_w_sq += w * dist * dist; - } - - if sum_w < f32::EPSILON { - return fitting_error(points, line); - } - - (sum_w_sq / sum_w).sqrt() -} - -/// Compute the perpendicular fitting error (RMS distance to line). -/// -/// Returns the root-mean-square of perpendicular distances from points to the line. -/// SIMD-optimized for batches of 4+ points. -pub fn fitting_error(points: &[Point2D], line: &Line2D) -> f32 { - if points.is_empty() { - return 0.0; - } - - let n = points.len(); - - // Pre-compute line parameters for SIMD - let dx = line.end.x - line.start.x; - let dy = line.end.y - line.start.y; - let length_sq = dx * dx + dy * dy; - - if length_sq < f32::EPSILON { - // Degenerate line - fallback to point distance - let mut sum_sq: f32 = 0.0; - for p in points { - let d = p.distance(line.start); - sum_sq += d * d; - } - return (sum_sq / n as f32).sqrt(); - } - - let inv_length = 1.0 / length_sq.sqrt(); - - // SIMD path for 4+ points - let chunks = n / 4; - let mut sum_sq: f32 = 0.0; - - if chunks > 0 { - let dx4 = f32x4::splat(dx); - let dy4 = f32x4::splat(dy); - let sx4 = f32x4::splat(line.start.x); - let sy4 = f32x4::splat(line.start.y); - let inv_len4 = f32x4::splat(inv_length); - let mut sum_sq4 = f32x4::splat(0.0); - - for i in 0..chunks { - let base = i * 4; - - // Load 4 points - let px = f32x4::from_array([ - points[base].x, - points[base + 1].x, - points[base + 2].x, - points[base + 3].x, - ]); - let py = f32x4::from_array([ - points[base].y, - points[base + 1].y, - points[base + 2].y, - points[base + 3].y, - ]); - - // Compute cross product: dx * (py - sy) - dy * (px - sx) - let to_px = px - sx4; - let to_py = py - sy4; - let cross = dx4 * to_py - dy4 * to_px; - - // Distance = |cross| / length - let dist = cross.abs() * inv_len4; - sum_sq4 += dist * dist; - } - - sum_sq = sum_sq4.reduce_sum(); - } - - // Handle remainder (scalar) - for p in points.iter().take(n).skip(chunks * 4) { - let to_px = p.x - line.start.x; - let to_py = p.y - line.start.y; - let cross = dx * to_py - dy * to_px; - let dist = cross.abs() * inv_length; - sum_sq += dist * dist; - } - - (sum_sq / n as f32).sqrt() -} - -/// Find the point with maximum distance from a line. -/// -/// Returns (index, distance) of the farthest point. -/// Returns None if points is empty. -/// SIMD-optimized for batches of 4+ points. -pub fn max_distance_point(points: &[Point2D], line: &Line2D) -> Option<(usize, f32)> { - if points.is_empty() { - return None; - } - - let n = points.len(); - - // Pre-compute line parameters - let dx = line.end.x - line.start.x; - let dy = line.end.y - line.start.y; - let length_sq = dx * dx + dy * dy; - - if length_sq < f32::EPSILON { - // Degenerate line - find max point distance - let mut max_idx = 0; - let mut max_dist: f32 = 0.0; - for (i, p) in points.iter().enumerate() { - let dist = p.distance(line.start); - if dist > max_dist { - max_dist = dist; - max_idx = i; - } - } - return Some((max_idx, max_dist)); - } - - let inv_length = 1.0 / length_sq.sqrt(); - - // SIMD path - compute distances in batches, track max - let chunks = n / 4; - let mut distances = Vec::with_capacity(n); - - if chunks > 0 { - let dx4 = f32x4::splat(dx); - let dy4 = f32x4::splat(dy); - let sx4 = f32x4::splat(line.start.x); - let sy4 = f32x4::splat(line.start.y); - let inv_len4 = f32x4::splat(inv_length); - - for i in 0..chunks { - let base = i * 4; - - // Load 4 points - let px = f32x4::from_array([ - points[base].x, - points[base + 1].x, - points[base + 2].x, - points[base + 3].x, - ]); - let py = f32x4::from_array([ - points[base].y, - points[base + 1].y, - points[base + 2].y, - points[base + 3].y, - ]); - - // Compute cross product and distance - let to_px = px - sx4; - let to_py = py - sy4; - let cross = dx4 * to_py - dy4 * to_px; - let dist = cross.abs() * inv_len4; - - distances.extend_from_slice(&dist.to_array()); - } - } - - // Handle remainder - for p in points.iter().take(n).skip(chunks * 4) { - let to_px = p.x - line.start.x; - let to_py = p.y - line.start.y; - let cross = dx * to_py - dy * to_px; - let dist = cross.abs() * inv_length; - distances.push(dist); - } - - // Find max using SIMD reduction if we have enough distances - let mut max_idx = 0; - let mut max_dist: f32 = 0.0; - - let dist_chunks = distances.len() / 4; - if dist_chunks > 0 { - let mut max4 = f32x4::splat(0.0); - - for i in 0..dist_chunks { - let base = i * 4; - let d4 = f32x4::from_slice(&distances[base..]); - max4 = max4.simd_max(d4); - } - - // Reduce to find overall max, then scan for index - max_dist = max4.reduce_max(); - - // Handle remainder - for &d in distances.iter().skip(dist_chunks * 4) { - if d > max_dist { - max_dist = d; - } - } - - // Find the index (linear scan - could be optimized but typically dominated by SIMD part) - for (i, &d) in distances.iter().enumerate() { - if d == max_dist { - max_idx = i; - break; - } - } - } else { - // Small input - scalar - for (i, &d) in distances.iter().enumerate() { - if d > max_dist { - max_dist = d; - max_idx = i; - } - } - } - - Some((max_idx, max_dist)) -} - -#[cfg(test)] -mod tests { - use super::*; - use approx::assert_relative_eq; - use std::f32::consts::{FRAC_PI_2, FRAC_PI_4}; - - #[test] - fn test_fit_horizontal_line() { - let points = vec![ - Point2D::new(0.0, 0.0), - Point2D::new(1.0, 0.0), - Point2D::new(2.0, 0.0), - Point2D::new(3.0, 0.0), - Point2D::new(4.0, 0.0), - ]; - - let line = fit_line(&points).unwrap(); - - // Line should be horizontal - assert_relative_eq!(line.angle(), 0.0, epsilon = 0.01); - - // Should span from (0,0) to (4,0) - assert_relative_eq!(line.start.x, 0.0, epsilon = 0.01); - assert_relative_eq!(line.end.x, 4.0, epsilon = 0.01); - assert_relative_eq!(line.start.y, 0.0, epsilon = 0.01); - assert_relative_eq!(line.end.y, 0.0, epsilon = 0.01); - } - - #[test] - fn test_fit_vertical_line() { - let points = vec![ - Point2D::new(0.0, 0.0), - Point2D::new(0.0, 1.0), - Point2D::new(0.0, 2.0), - Point2D::new(0.0, 3.0), - ]; - - let line = fit_line(&points).unwrap(); - - // Line should be vertical - assert_relative_eq!(line.angle().abs(), FRAC_PI_2, epsilon = 0.01); - } - - #[test] - fn test_fit_diagonal_line() { - let points = vec![ - Point2D::new(0.0, 0.0), - Point2D::new(1.0, 1.0), - Point2D::new(2.0, 2.0), - Point2D::new(3.0, 3.0), - ]; - - let line = fit_line(&points).unwrap(); - - // Line should be at 45 degrees - assert_relative_eq!(line.angle().abs(), FRAC_PI_4, epsilon = 0.01); - } - - #[test] - fn test_fit_with_noise() { - let points = vec![ - Point2D::new(0.0, 0.02), - Point2D::new(1.0, -0.01), - Point2D::new(2.0, 0.03), - Point2D::new(3.0, -0.02), - Point2D::new(4.0, 0.01), - ]; - - let line = fit_line(&points).unwrap(); - - // Line should still be approximately horizontal - assert!(line.angle().abs() < 0.1); - - // Fitting error should be small - let error = fitting_error(&points, &line); - assert!(error < 0.03); - } - - #[test] - fn test_fit_two_points() { - let points = vec![Point2D::new(0.0, 0.0), Point2D::new(5.0, 3.0)]; - - let line = fit_line(&points).unwrap(); - - assert_relative_eq!(line.start.distance(points[0]), 0.0, epsilon = 0.01); - assert_relative_eq!(line.end.distance(points[1]), 0.0, epsilon = 0.01); - } - - #[test] - fn test_fit_one_point() { - let points = vec![Point2D::new(0.0, 0.0)]; - assert!(fit_line(&points).is_none()); - } - - #[test] - fn test_fit_empty() { - let points: Vec = vec![]; - assert!(fit_line(&points).is_none()); - } - - #[test] - fn test_fitting_error() { - let line = Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(10.0, 0.0)); - - // Points on the line - let points_on = vec![ - Point2D::new(0.0, 0.0), - Point2D::new(5.0, 0.0), - Point2D::new(10.0, 0.0), - ]; - assert_relative_eq!(fitting_error(&points_on, &line), 0.0, epsilon = 1e-6); - - // Points 1 unit away - let points_off = vec![ - Point2D::new(0.0, 1.0), - Point2D::new(5.0, 1.0), - Point2D::new(10.0, 1.0), - ]; - assert_relative_eq!(fitting_error(&points_off, &line), 1.0, epsilon = 1e-6); - } - - #[test] - fn test_max_distance_point() { - let line = Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(10.0, 0.0)); - - let points = vec![ - Point2D::new(2.0, 1.0), - Point2D::new(5.0, 3.0), // Farthest - Point2D::new(8.0, 2.0), - ]; - - let (idx, dist) = max_distance_point(&points, &line).unwrap(); - - assert_eq!(idx, 1); - assert_relative_eq!(dist, 3.0, epsilon = 1e-6); - } - - // ======================================== - // Weighted TLS Tests - // ======================================== - - #[test] - fn test_compute_range_weights() { - let sensor_pos = Point2D::new(0.0, 0.0); - let points = vec![ - Point2D::new(1.0, 0.0), // 1m away - Point2D::new(2.0, 0.0), // 2m away - Point2D::new(5.0, 0.0), // 5m away - ]; - - let weights = compute_range_weights(&points, sensor_pos, None); - - assert_eq!(weights.len(), 3); - // Closer points should have higher weights - assert!( - weights[0] > weights[1], - "1m point should have higher weight than 2m" - ); - assert!( - weights[1] > weights[2], - "2m point should have higher weight than 5m" - ); - } - - #[test] - fn test_compute_range_weights_custom_model() { - let sensor_pos = Point2D::new(0.0, 0.0); - let points = vec![Point2D::new(1.0, 0.0)]; - - let custom_model = LidarNoiseModel::new(0.02, 0.002); - let weights_custom = compute_range_weights(&points, sensor_pos, Some(&custom_model)); - let weights_default = compute_range_weights(&points, sensor_pos, None); - - // Custom model has higher base sigma, so weight should be lower - assert!(weights_custom[0] < weights_default[0]); - } - - #[test] - fn test_fit_line_weighted_uniform() { - // With uniform weights, should match unweighted fit - let points = vec![ - Point2D::new(0.0, 0.0), - Point2D::new(1.0, 0.0), - Point2D::new(2.0, 0.0), - Point2D::new(3.0, 0.0), - ]; - let weights = vec![1.0; 4]; - - let weighted_line = fit_line_weighted(&points, &weights).unwrap(); - let unweighted_line = fit_line(&points).unwrap(); - - assert_relative_eq!( - weighted_line.angle(), - unweighted_line.angle(), - epsilon = 0.01 - ); - assert_relative_eq!( - weighted_line.start.x, - unweighted_line.start.x, - epsilon = 0.01 - ); - assert_relative_eq!(weighted_line.end.x, unweighted_line.end.x, epsilon = 0.01); - } - - #[test] - fn test_fit_line_weighted_pulls_toward_high_weight() { - // Line with an outlier - weighting should affect result - let points = vec![ - Point2D::new(0.0, 0.0), // Close point, on y=0 - Point2D::new(1.0, 0.0), // Close point, on y=0 - Point2D::new(2.0, 0.0), // Close point, on y=0 - Point2D::new(5.0, 0.5), // Far point with noise (above y=0) - ]; - - // High weights for close points, low weight for far point - let weighted_weights = vec![10.0, 10.0, 10.0, 0.1]; - // Equal weights - let uniform_weights = vec![1.0, 1.0, 1.0, 1.0]; - - let weighted_line = fit_line_weighted(&points, &weighted_weights).unwrap(); - let uniform_line = fit_line_weighted(&points, &uniform_weights).unwrap(); - - // Weighted line should be closer to horizontal (less affected by outlier) - assert!( - weighted_line.angle().abs() < uniform_line.angle().abs(), - "Weighted line should be more horizontal: weighted={:.4}, uniform={:.4}", - weighted_line.angle(), - uniform_line.angle() - ); - } - - #[test] - fn test_fit_line_from_sensor_horizontal() { - let sensor_pos = Point2D::new(0.0, 0.0); - let points = vec![ - Point2D::new(1.0, 0.0), - Point2D::new(2.0, 0.0), - Point2D::new(3.0, 0.0), - Point2D::new(4.0, 0.0), - ]; - - let line = fit_line_from_sensor(&points, sensor_pos, None).unwrap(); - - // Should be horizontal - assert_relative_eq!(line.angle(), 0.0, epsilon = 0.01); - } - - #[test] - fn test_fit_line_from_sensor_with_noise() { - let sensor_pos = Point2D::new(0.0, 0.0); - // Points with noise - closer points are on line, far point has noise - let points = vec![ - Point2D::new(1.0, 0.0), // 1m, on line - Point2D::new(2.0, 0.0), // 2m, on line - Point2D::new(5.0, 0.5), // 5m, noisy - ]; - - let line = fit_line_from_sensor(&points, sensor_pos, None).unwrap(); - let unweighted_line = fit_line(&points).unwrap(); - - // Weighted line should be more horizontal because far point has less weight - // Both should be near-horizontal, but weighted should be closer to 0 - assert!( - line.angle().abs() <= unweighted_line.angle().abs() + 0.01, - "Weighted angle {} should be <= unweighted angle {}", - line.angle(), - unweighted_line.angle() - ); - } - - #[test] - fn test_fit_line_weighted_empty() { - let points: Vec = vec![]; - let weights: Vec = vec![]; - assert!(fit_line_weighted(&points, &weights).is_none()); - } - - #[test] - fn test_fit_line_weighted_mismatched_lengths() { - let points = vec![Point2D::new(0.0, 0.0), Point2D::new(1.0, 0.0)]; - let weights = vec![1.0]; // Wrong length - assert!(fit_line_weighted(&points, &weights).is_none()); - } - - #[test] - fn test_fitting_error_weighted() { - let line = Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(10.0, 0.0)); - - // Points at different distances from line - let points = vec![ - Point2D::new(2.0, 1.0), // 1m from line - Point2D::new(5.0, 2.0), // 2m from line - ]; - - // Give high weight to the 1m point, low weight to 2m point - let weights = vec![10.0, 0.1]; - - let weighted_error = fitting_error_weighted(&points, &line, &weights); - - // Unweighted RMS would be sqrt((1 + 4) / 2) = sqrt(2.5) ≈ 1.58 - // Weighted should be much closer to 1.0 because first point dominates - assert!( - weighted_error < 1.2, - "Weighted error {} should be < 1.2", - weighted_error - ); - } - - // ======================================== - // Line Splitting Tests - // ======================================== - - #[test] - fn test_split_line_short_line_no_split() { - let line = Line2D::with_point_count(Point2D::new(0.0, 0.0), Point2D::new(0.5, 0.0), 10); - - let segments = split_line_by_length(&line, 1.0); - - assert_eq!(segments.len(), 1); - assert_relative_eq!(segments[0].start.x, 0.0, epsilon = 1e-6); - assert_relative_eq!(segments[0].end.x, 0.5, epsilon = 1e-6); - assert_eq!(segments[0].point_count, 10); - } - - #[test] - fn test_split_line_exact_max_length() { - let line = Line2D::with_point_count(Point2D::new(0.0, 0.0), Point2D::new(1.0, 0.0), 10); - - let segments = split_line_by_length(&line, 1.0); - - assert_eq!(segments.len(), 1); - } - - #[test] - fn test_split_line_into_two() { - let line = Line2D::with_point_count(Point2D::new(0.0, 0.0), Point2D::new(2.0, 0.0), 20); - - let segments = split_line_by_length(&line, 1.0); - - assert_eq!(segments.len(), 2); - - // First segment - assert_relative_eq!(segments[0].start.x, 0.0, epsilon = 1e-6); - assert_relative_eq!(segments[0].end.x, 1.0, epsilon = 1e-6); - assert_eq!(segments[0].point_count, 10); - - // Second segment - assert_relative_eq!(segments[1].start.x, 1.0, epsilon = 1e-6); - assert_relative_eq!(segments[1].end.x, 2.0, epsilon = 1e-6); - assert_eq!(segments[1].point_count, 10); - } - - #[test] - fn test_split_line_into_multiple() { - let line = Line2D::with_point_count(Point2D::new(0.0, 0.0), Point2D::new(5.0, 0.0), 50); - - let segments = split_line_by_length(&line, 1.0); - - assert_eq!(segments.len(), 5); - - // Total length should be preserved - let total_length: f32 = segments.iter().map(|s| s.length()).sum(); - assert_relative_eq!(total_length, 5.0, epsilon = 1e-5); - - // Total point count should be preserved - let total_points: u32 = segments.iter().map(|s| s.point_count).sum(); - assert_eq!(total_points, 50); - } - - #[test] - fn test_split_line_diagonal() { - // Diagonal line of length 5*sqrt(2) ≈ 7.07 - let line = Line2D::with_point_count(Point2D::new(0.0, 0.0), Point2D::new(5.0, 5.0), 30); - - let segments = split_line_by_length(&line, 2.0); - - // 7.07 / 2.0 = 3.54, so 4 segments - assert_eq!(segments.len(), 4); - - // Segments should be collinear - for seg in &segments { - assert_relative_eq!(seg.angle(), FRAC_PI_4, epsilon = 0.01); - } - - // Endpoints should be preserved - assert_relative_eq!(segments[0].start.x, 0.0, epsilon = 1e-5); - assert_relative_eq!(segments[0].start.y, 0.0, epsilon = 1e-5); - assert_relative_eq!(segments.last().unwrap().end.x, 5.0, epsilon = 1e-5); - assert_relative_eq!(segments.last().unwrap().end.y, 5.0, epsilon = 1e-5); - } - - #[test] - fn test_split_line_zero_max_length() { - let line = Line2D::with_point_count(Point2D::new(0.0, 0.0), Point2D::new(2.0, 0.0), 10); - - // Zero or negative max_length should return original line - let segments = split_line_by_length(&line, 0.0); - assert_eq!(segments.len(), 1); - - let segments_neg = split_line_by_length(&line, -1.0); - assert_eq!(segments_neg.len(), 1); - } -} diff --git a/vastu-map/src/extraction/mod.rs b/vastu-map/src/extraction/mod.rs deleted file mode 100644 index 9b2218a..0000000 --- a/vastu-map/src/extraction/mod.rs +++ /dev/null @@ -1,174 +0,0 @@ -//! Feature extraction from point clouds. -//! -//! This module provides algorithms for extracting geometric features -//! (lines and corners) from lidar point clouds. -//! -//! # Algorithms -//! -//! - **Line Fitting**: Total Least Squares (orthogonal regression), with weighted variant -//! - **Line Extraction**: Split-and-Merge algorithm for ordered point sequences -//! - **RANSAC Lines**: Random Sample Consensus for robust line extraction -//! - **Hybrid Extraction**: RANSAC for dominant lines, then split-merge for remaining -//! - **Corner Detection**: Find corners at line intersections -//! -//! # Weighted Line Fitting -//! -//! For lidar data, measurement uncertainty increases with range. The weighted -//! TLS functions allow incorporating this range-based uncertainty: -//! -//! ```rust,ignore -//! use vastu_map::extraction::fit_line_from_sensor; -//! use vastu_map::core::Point2D; -//! -//! let sensor_pos = Point2D::new(0.0, 0.0); -//! let points = vec![/* lidar hits */]; -//! let line = fit_line_from_sensor(&points, sensor_pos, None); -//! ``` -//! -//! # Hybrid Extraction -//! -//! The hybrid approach combines RANSAC's robustness with split-merge's -//! ability to handle structured point sequences: -//! -//! ```rust,ignore -//! use vastu_map::extraction::{extract_lines_hybrid, RansacLineConfig, SplitMergeConfig}; -//! -//! let ransac_config = RansacLineConfig::default(); -//! let split_merge_config = SplitMergeConfig::default(); -//! let lines = extract_lines_hybrid(&points, &ransac_config, &split_merge_config); -//! ``` - -use crate::core::Point2D; -use crate::features::Line2D; - -pub mod corner_detection; -pub mod line_fitting; -pub mod ransac_lines; -pub mod split_merge; -pub mod traits; - -pub use corner_detection::{ - CornerConfig, deduplicate_corners, detect_corners, detect_curvature_corners, - detect_hybrid_corners, -}; -pub use line_fitting::{ - FitError, compute_range_weights, fit_line, fit_line_from_sensor, fit_line_weighted, - fitting_error, fitting_error_weighted, max_distance_point, split_line_by_length, -}; -// Re-export from ransac_lines module -pub use ransac_lines::{ - RansacLineConfig, RansacScratchSpace, extract_lines_ransac, extract_lines_ransac_with_scratch, -}; -// Re-export from split_merge submodule -pub use split_merge::{ - SplitMergeConfig, adaptive_split_threshold, extract_lines, extract_lines_from_sensor, -}; -// Re-export traits -pub use traits::{LineExtractor, SplitMergeExtractor}; - -/// Extract lines using hybrid RANSAC + split-merge approach. -/// -/// This method combines RANSAC's robustness to outliers with split-merge's -/// ability to handle structured point sequences: -/// -/// 1. **RANSAC first**: Extracts dominant lines that may span across gaps -/// 2. **Split-merge second**: Processes remaining points for additional structure -/// -/// This is useful when: -/// - Point cloud has significant outliers (RANSAC handles these) -/// - Some features are weak/fragmented (split-merge catches these) -/// - You want the best of both algorithms -/// -/// # Arguments -/// * `points` - Input point cloud -/// * `ransac_config` - Configuration for RANSAC extraction -/// * `split_merge_config` - Configuration for split-merge extraction -/// -/// # Returns -/// Combined list of extracted lines from both algorithms. -/// -/// # Example -/// ```rust,ignore -/// use vastu_map::extraction::{extract_lines_hybrid, RansacLineConfig, SplitMergeConfig}; -/// -/// let ransac_config = RansacLineConfig::default() -/// .with_max_lines(5) // Limit RANSAC to 5 dominant lines -/// .with_inlier_threshold(0.02); -/// -/// let split_merge_config = SplitMergeConfig::default(); -/// -/// let all_lines = extract_lines_hybrid(&points, &ransac_config, &split_merge_config); -/// ``` -pub fn extract_lines_hybrid( - points: &[Point2D], - ransac_config: &RansacLineConfig, - split_merge_config: &SplitMergeConfig, -) -> Vec { - if points.is_empty() { - return Vec::new(); - } - - // Step 1: RANSAC for dominant lines - let (mut lines, remaining_indices) = extract_lines_ransac(points, ransac_config); - - // Step 2: Split-merge on remaining points - if remaining_indices.len() >= 3 { - let remaining_points: Vec = remaining_indices.iter().map(|&i| points[i]).collect(); - - let additional_lines = extract_lines(&remaining_points, split_merge_config); - lines.extend(additional_lines); - } - - lines -} - -/// Extract lines using hybrid RANSAC + split-merge with pre-allocated scratch space. -/// -/// This is the zero-allocation version for real-time applications. -/// Reuse the scratch space across multiple calls to eliminate allocations. -/// -/// # Arguments -/// * `points` - Input point cloud -/// * `ransac_config` - Configuration for RANSAC extraction -/// * `split_merge_config` - Configuration for split-merge extraction -/// * `scratch` - Pre-allocated RANSAC scratch space -/// * `output_lines` - Buffer to store output lines (cleared and filled) -/// -/// # Example -/// ```rust,ignore -/// let mut scratch = RansacScratchSpace::with_capacity(400); -/// let mut lines = Vec::new(); -/// -/// for scan in scans { -/// extract_lines_hybrid_with_scratch(&scan, &ransac_cfg, &sm_cfg, &mut scratch, &mut lines); -/// // lines contains extracted features, scratch is reused -/// } -/// ``` -pub fn extract_lines_hybrid_with_scratch( - points: &[Point2D], - ransac_config: &RansacLineConfig, - split_merge_config: &SplitMergeConfig, - scratch: &mut RansacScratchSpace, - output_lines: &mut Vec, -) { - output_lines.clear(); - - if points.is_empty() { - return; - } - - // Step 1: RANSAC for dominant lines (using scratch space) - let (ransac_lines, remaining_indices) = - extract_lines_ransac_with_scratch(points, ransac_config, scratch); - output_lines.extend_from_slice(ransac_lines); - - // Step 2: Split-merge on remaining points - // Note: We still need to collect remaining points for split-merge, - // but at least the expensive RANSAC iterations are allocation-free - if remaining_indices.len() >= 3 { - let remaining_points: Vec = remaining_indices.iter().map(|&i| points[i]).collect(); - - let additional_lines = extract_lines(&remaining_points, split_merge_config); - output_lines.extend(additional_lines); - } -} diff --git a/vastu-map/src/extraction/ransac_lines.rs b/vastu-map/src/extraction/ransac_lines.rs deleted file mode 100644 index a1839b6..0000000 --- a/vastu-map/src/extraction/ransac_lines.rs +++ /dev/null @@ -1,1117 +0,0 @@ -//! RANSAC-based line extraction. -//! -//! This module provides RANSAC (Random Sample Consensus) for extracting -//! dominant lines from point clouds. RANSAC is robust to outliers and -//! can find strong lines even in cluttered environments. -//! -//! # Usage -//! -//! ```rust,ignore -//! use vastu_map::extraction::{extract_lines_ransac, RansacLineConfig, RansacScratchSpace}; -//! -//! let config = RansacLineConfig::default(); -//! -//! // Zero-allocation version with scratch space -//! let mut scratch = RansacScratchSpace::with_capacity(400); -//! let (lines, remaining) = extract_lines_ransac_with_scratch(&points, &config, &mut scratch); -//! -//! // Reuse scratch for next scan - no allocations! -//! let (lines2, remaining2) = extract_lines_ransac_with_scratch(&points2, &config, &mut scratch); -//! ``` - -use serde::{Deserialize, Serialize}; - -use crate::core::Point2D; -use crate::core::math::{compute_centroid, compute_covariance}; -use crate::features::Line2D; - -use super::line_fitting::fit_line; - -// ============================================================================ -// Scratch Space for Zero-Allocation RANSAC -// ============================================================================ - -/// Pre-allocated scratch space for RANSAC operations. -/// -/// Reuse this struct across multiple `extract_lines_ransac_with_scratch` calls -/// to eliminate per-iteration allocations. This is critical for real-time SLAM -/// where RANSAC may run 100+ iterations per scan. -/// -/// # Performance Impact -/// -/// Without scratch space, each RANSAC call allocates: -/// - `remaining_mask`: O(n) bools -/// - `remaining`: O(n) indices, rebuilt every outer iteration -/// - `current_inliers`: O(n) indices, rebuilt every RANSAC iteration (100x) -/// - `best_inliers`: O(n) indices -/// - `inlier_points`: O(n) Point2Ds -/// -/// With scratch space, these allocations happen once and are reused. -/// -/// # Example -/// ```rust,ignore -/// use vastu_map::extraction::{RansacScratchSpace, extract_lines_ransac_with_scratch}; -/// -/// // Create once, sized for typical lidar scans -/// let mut scratch = RansacScratchSpace::with_capacity(400); -/// -/// for scan in scans { -/// let (lines, remaining) = extract_lines_ransac_with_scratch(&scan, &config, &mut scratch); -/// // scratch is reused, no allocations! -/// } -/// ``` -#[derive(Clone, Debug)] -pub struct RansacScratchSpace { - /// Bit mask for remaining (unconsumed) points. - /// Avoids rebuilding Vec each iteration. - remaining_mask: Vec, - - /// Buffer for current iteration's inlier indices. - /// Reused across all RANSAC iterations. - current_inliers: Vec, - - /// Buffer for best inliers found so far. - best_inliers: Vec, - - /// Buffer for inlier points (for line fitting and gap splitting). - inlier_points: Vec, - - /// Buffer for inlier indices corresponding to inlier_points. - inlier_indices: Vec, - - /// Buffer for output lines. - output_lines: Vec, - - /// Buffer for remaining indices output. - remaining_output: Vec, - - /// Buffer for segment points in split_at_gaps. - segment_points: Vec, - - /// Buffer for segment indices in split_at_gaps. - segment_indices: Vec, - - /// Buffer for projection sorting in split_at_gaps. - projection_buffer: Vec<(usize, f32)>, -} - -impl RansacScratchSpace { - /// Create new scratch space with capacity hints. - /// - /// # Arguments - /// * `max_points` - Expected maximum scan points (e.g., 360-400 for typical lidar) - pub fn with_capacity(max_points: usize) -> Self { - Self { - remaining_mask: Vec::with_capacity(max_points), - current_inliers: Vec::with_capacity(max_points), - best_inliers: Vec::with_capacity(max_points), - inlier_points: Vec::with_capacity(max_points), - inlier_indices: Vec::with_capacity(max_points), - output_lines: Vec::with_capacity(32), // Typical max lines per scan - remaining_output: Vec::with_capacity(max_points), - segment_points: Vec::with_capacity(max_points), - segment_indices: Vec::with_capacity(max_points), - projection_buffer: Vec::with_capacity(max_points), - } - } - - /// Create with default capacity (400 points). - pub fn default_capacity() -> Self { - Self::with_capacity(400) - } - - /// Reset scratch space for a new extraction run. - /// Clears all buffers but retains allocated capacity. - #[inline] - pub fn reset(&mut self, num_points: usize) { - self.remaining_mask.clear(); - self.remaining_mask.resize(num_points, true); - self.current_inliers.clear(); - self.best_inliers.clear(); - self.inlier_points.clear(); - self.inlier_indices.clear(); - self.output_lines.clear(); - self.remaining_output.clear(); - self.segment_points.clear(); - self.segment_indices.clear(); - self.projection_buffer.clear(); - } - - /// Count remaining active points using the mask. - #[inline] - fn count_remaining(&self) -> usize { - self.remaining_mask.iter().filter(|&&b| b).count() - } -} - -impl Default for RansacScratchSpace { - fn default() -> Self { - Self::default_capacity() - } -} - -/// Configuration for RANSAC line extraction. -#[derive(Clone, Debug, Serialize, Deserialize)] -pub struct RansacLineConfig { - /// Number of RANSAC iterations per line. - /// More iterations = better chance of finding optimal line. - /// Default: 100 - pub iterations: usize, - - /// Inlier distance threshold (meters). - /// Points within this distance of a line are considered inliers. - /// Default: 0.02m (2cm) - pub inlier_threshold: f32, - - /// Minimum inliers for a valid line. - /// Lines with fewer inliers are rejected. - /// Default: 5 - pub min_inliers: usize, - - /// Minimum line length (meters). - /// Lines shorter than this are rejected. - /// Default: 0.1m (10cm) - pub min_line_length: f32, - - /// Maximum gap between consecutive inliers along a line (meters). - /// If inliers have a gap larger than this, the line is split. - /// This prevents connecting collinear walls separated by doorways. - /// Default: 0.10m (10cm) - pub max_point_gap: f32, - - /// Maximum lines to extract (0 = unlimited). - /// Stops after extracting this many lines. - /// Default: 0 (unlimited) - pub max_lines: usize, - - /// Random seed for reproducibility. - /// 0 = use entropy-based seed (non-deterministic). - /// Default: 0 - pub seed: u64, -} - -impl Default for RansacLineConfig { - fn default() -> Self { - Self { - iterations: 100, - inlier_threshold: 0.02, - min_inliers: 5, - min_line_length: 0.1, - max_point_gap: 0.10, - max_lines: 0, - seed: 0, - } - } -} - -impl RansacLineConfig { - /// Create a new config with default values. - pub fn new() -> Self { - Self::default() - } - - /// Builder-style setter for iterations. - pub fn with_iterations(mut self, iterations: usize) -> Self { - self.iterations = iterations; - self - } - - /// Builder-style setter for inlier threshold. - pub fn with_inlier_threshold(mut self, threshold: f32) -> Self { - self.inlier_threshold = threshold; - self - } - - /// Builder-style setter for minimum inliers. - pub fn with_min_inliers(mut self, min_inliers: usize) -> Self { - self.min_inliers = min_inliers; - self - } - - /// Builder-style setter for minimum line length. - pub fn with_min_line_length(mut self, length: f32) -> Self { - self.min_line_length = length; - self - } - - /// Builder-style setter for maximum lines. - pub fn with_max_lines(mut self, max_lines: usize) -> Self { - self.max_lines = max_lines; - self - } - - /// Builder-style setter for maximum point gap. - pub fn with_max_point_gap(mut self, gap: f32) -> Self { - self.max_point_gap = gap; - self - } - - /// Builder-style setter for random seed. - pub fn with_seed(mut self, seed: u64) -> Self { - self.seed = seed; - self - } -} - -/// Simple LCG random number generator for deterministic behavior. -struct SimpleRng { - state: u64, -} - -impl SimpleRng { - fn new(seed: u64) -> Self { - // Use a non-zero seed - Self { - state: if seed == 0 { - // Use a simple time-based seed - std::time::SystemTime::now() - .duration_since(std::time::UNIX_EPOCH) - .map(|d| d.as_nanos() as u64) - .unwrap_or(12345) - } else { - seed - }, - } - } - - fn next(&mut self) -> u64 { - // LCG parameters from Numerical Recipes - self.state = self.state.wrapping_mul(6364136223846793005).wrapping_add(1); - self.state - } - - fn gen_range(&mut self, max: usize) -> usize { - if max == 0 { - return 0; - } - (self.next() % (max as u64)) as usize - } -} - -/// Split inliers at gaps to prevent connecting separate wall segments. -/// -/// Points are sorted along the line direction and split wherever -/// consecutive points have a gap larger than `max_point_gap`. -fn split_at_gaps( - inlier_points: &[Point2D], - inlier_indices: &[usize], - config: &RansacLineConfig, -) -> Vec<(Vec, Vec)> { - if inlier_points.len() < 2 { - return vec![(inlier_points.to_vec(), inlier_indices.to_vec())]; - } - - // Use shared utilities for centroid and covariance - let centroid = compute_centroid(inlier_points); - let cov = compute_covariance(inlier_points, centroid); - - // Principal direction from dominant eigenvector (using atan2 for angle) - let theta = 0.5 * (2.0 * cov.cxy).atan2(cov.cxx - cov.cyy); - let dir_x = theta.cos(); - let dir_y = theta.sin(); - - // Project points onto principal direction and sort - let mut indexed: Vec<(usize, f32)> = inlier_points - .iter() - .enumerate() - .map(|(i, p)| { - let proj = (p.x - centroid.x) * dir_x + (p.y - centroid.y) * dir_y; - (i, proj) - }) - .collect(); - - indexed.sort_by(|a, b| a.1.partial_cmp(&b.1).unwrap_or(std::cmp::Ordering::Equal)); - - // Split at gaps - let mut segments = Vec::new(); - let mut current_points = Vec::new(); - let mut current_indices = Vec::new(); - - for (i, &(orig_idx, _proj)) in indexed.iter().enumerate() { - current_points.push(inlier_points[orig_idx]); - current_indices.push(inlier_indices[orig_idx]); - - // Check if there's a gap to the next point - if i + 1 < indexed.len() { - let next_orig_idx = indexed[i + 1].0; - let dist = inlier_points[orig_idx].distance(inlier_points[next_orig_idx]); - - if dist > config.max_point_gap { - // Gap found - save current segment and start new one - if !current_points.is_empty() { - segments.push(( - std::mem::take(&mut current_points), - std::mem::take(&mut current_indices), - )); - } - } - } - } - - // Add final segment - if !current_points.is_empty() { - segments.push((current_points, current_indices)); - } - - segments -} - -/// Extract lines using RANSAC. -/// -/// Returns extracted lines and indices of points not matched to any line. -/// -/// # Arguments -/// * `points` - Input point cloud -/// * `config` - RANSAC configuration -/// -/// # Returns -/// Tuple of (extracted_lines, remaining_point_indices) -pub fn extract_lines_ransac( - points: &[Point2D], - config: &RansacLineConfig, -) -> (Vec, Vec) { - if points.len() < config.min_inliers { - return (Vec::new(), (0..points.len()).collect()); - } - - let mut remaining_mask = vec![true; points.len()]; - let mut lines = Vec::new(); - let mut rng = SimpleRng::new(config.seed); - - loop { - // Check max lines limit - if config.max_lines > 0 && lines.len() >= config.max_lines { - break; - } - - // Collect remaining point indices - let remaining: Vec = remaining_mask - .iter() - .enumerate() - .filter_map(|(i, &active)| if active { Some(i) } else { None }) - .collect(); - - if remaining.len() < config.min_inliers { - break; - } - - // Run RANSAC to find best line - let mut best_inliers: Vec = Vec::new(); - - for _ in 0..config.iterations { - // Random sample of 2 distinct points - let idx1 = remaining[rng.gen_range(remaining.len())]; - let idx2 = remaining[rng.gen_range(remaining.len())]; - if idx1 == idx2 { - continue; - } - - let p1 = points[idx1]; - let p2 = points[idx2]; - - // Check minimum distance between sample points - let sample_dist = p1.distance(p2); - if sample_dist < config.min_line_length * 0.3 { - continue; - } - - // Create candidate line - let line = Line2D::new(p1, p2); - - // Count inliers - let inliers: Vec = remaining - .iter() - .copied() - .filter(|&i| line.distance_to_point(points[i]) < config.inlier_threshold) - .collect(); - - if inliers.len() > best_inliers.len() { - best_inliers = inliers; - } - } - - // Check if we found enough inliers - if best_inliers.len() < config.min_inliers { - break; - } - - // Get inlier points - let inlier_points: Vec = best_inliers.iter().map(|&i| points[i]).collect(); - - // Split inliers at gaps to avoid connecting separate wall segments - let segments = split_at_gaps(&inlier_points, &best_inliers, config); - - let mut added_any = false; - for (segment_points, segment_indices) in segments { - if segment_points.len() < config.min_inliers { - continue; - } - - if let Some(refined_line) = fit_line(&segment_points) { - // Check minimum length - if refined_line.length() >= config.min_line_length { - lines.push(refined_line); - added_any = true; - - // Mark segment inliers as used - for &i in &segment_indices { - remaining_mask[i] = false; - } - } - } - } - - // If we couldn't add any valid lines from this iteration, stop - if !added_any { - break; - } - } - - // Collect remaining point indices - let remaining: Vec = remaining_mask - .iter() - .enumerate() - .filter_map(|(i, &active)| if active { Some(i) } else { None }) - .collect(); - - (lines, remaining) -} - -/// Extract lines using RANSAC with pre-allocated scratch space. -/// -/// This is the zero-allocation version for real-time applications. -/// Reuse the scratch space across multiple calls to eliminate allocations. -/// -/// Returns extracted lines and indices of points not matched to any line. -/// The returned vectors are borrowed from scratch space and are valid until -/// the next call to this function. -/// -/// # Arguments -/// * `points` - Input point cloud -/// * `config` - RANSAC configuration -/// * `scratch` - Pre-allocated scratch space (reused across calls) -/// -/// # Returns -/// Tuple of (&[Line2D], &[usize]) - references into scratch space -/// -/// # Example -/// ```rust,ignore -/// let mut scratch = RansacScratchSpace::with_capacity(400); -/// let (lines, remaining) = extract_lines_ransac_with_scratch(&points, &config, &mut scratch); -/// // Process lines and remaining before next call -/// ``` -pub fn extract_lines_ransac_with_scratch<'a>( - points: &[Point2D], - config: &RansacLineConfig, - scratch: &'a mut RansacScratchSpace, -) -> (&'a [Line2D], &'a [usize]) { - // Reset scratch for this extraction - scratch.reset(points.len()); - - if points.len() < config.min_inliers { - // Return remaining as all indices - scratch.remaining_output.extend(0..points.len()); - return (&scratch.output_lines, &scratch.remaining_output); - } - - let mut rng = SimpleRng::new(config.seed); - - loop { - // Check max lines limit - if config.max_lines > 0 && scratch.output_lines.len() >= config.max_lines { - break; - } - - // Count remaining points without allocating - let remaining_count = scratch.count_remaining(); - if remaining_count < config.min_inliers { - break; - } - - // Run RANSAC to find best line - scratch.best_inliers.clear(); - - for _ in 0..config.iterations { - // Sample random indices from remaining points - // We need to pick the nth remaining point, not the nth point - let sample_pos1 = rng.gen_range(remaining_count); - let sample_pos2 = rng.gen_range(remaining_count); - if sample_pos1 == sample_pos2 { - continue; - } - - // Convert sample positions to actual indices - let idx1 = nth_remaining(&scratch.remaining_mask, sample_pos1); - let idx2 = nth_remaining(&scratch.remaining_mask, sample_pos2); - - let p1 = points[idx1]; - let p2 = points[idx2]; - - // Check minimum distance between sample points - let sample_dist = p1.distance(p2); - if sample_dist < config.min_line_length * 0.3 { - continue; - } - - // Create candidate line - let line = Line2D::new(p1, p2); - - // Count inliers into scratch buffer (reused each iteration) - scratch.current_inliers.clear(); - for (i, &is_remaining) in scratch.remaining_mask.iter().enumerate() { - if is_remaining && line.distance_to_point(points[i]) < config.inlier_threshold { - scratch.current_inliers.push(i); - } - } - - // Update best if this is better - if scratch.current_inliers.len() > scratch.best_inliers.len() { - scratch.best_inliers.clear(); - scratch - .best_inliers - .extend_from_slice(&scratch.current_inliers); - } - } - - // Check if we found enough inliers - if scratch.best_inliers.len() < config.min_inliers { - break; - } - - // Get inlier points into scratch buffer - scratch.inlier_points.clear(); - scratch.inlier_indices.clear(); - for &i in &scratch.best_inliers { - scratch.inlier_points.push(points[i]); - scratch.inlier_indices.push(i); - } - - // Split at gaps and process segments - let added_any = split_at_gaps_with_scratch(config, scratch); - - // If we couldn't add any valid lines from this iteration, stop - if !added_any { - break; - } - } - - // Collect remaining point indices into output buffer - scratch.remaining_output.clear(); - for (i, &is_remaining) in scratch.remaining_mask.iter().enumerate() { - if is_remaining { - scratch.remaining_output.push(i); - } - } - - (&scratch.output_lines, &scratch.remaining_output) -} - -/// Find the nth remaining (active) index in the mask. -#[inline] -fn nth_remaining(mask: &[bool], mut n: usize) -> usize { - for (i, &active) in mask.iter().enumerate() { - if active { - if n == 0 { - return i; - } - n -= 1; - } - } - // Fallback (shouldn't happen if n < count_remaining) - 0 -} - -/// Split inliers at gaps using scratch buffers, add valid lines to output. -/// Returns true if any lines were added. -fn split_at_gaps_with_scratch(config: &RansacLineConfig, scratch: &mut RansacScratchSpace) -> bool { - let num_inliers = scratch.inlier_points.len(); - - if num_inliers < 2 { - // Single point segment - try to fit - if num_inliers >= config.min_inliers - && let Some(line) = fit_line(&scratch.inlier_points) - && line.length() >= config.min_line_length - { - scratch.output_lines.push(line); - for i in 0..scratch.inlier_indices.len() { - let idx = scratch.inlier_indices[i]; - scratch.remaining_mask[idx] = false; - } - return true; - } - return false; - } - - // Use shared utilities for centroid and covariance - let centroid = compute_centroid(&scratch.inlier_points); - let cov = compute_covariance(&scratch.inlier_points, centroid); - - // Principal direction from dominant eigenvector (using atan2 for angle) - let theta = 0.5 * (2.0 * cov.cxy).atan2(cov.cxx - cov.cyy); - let dir_x = theta.cos(); - let dir_y = theta.sin(); - - // Project points onto principal direction and sort - scratch.projection_buffer.clear(); - for (i, p) in scratch.inlier_points.iter().enumerate() { - let proj = (p.x - centroid.x) * dir_x + (p.y - centroid.y) * dir_y; - scratch.projection_buffer.push((i, proj)); - } - scratch - .projection_buffer - .sort_by(|a, b| a.1.partial_cmp(&b.1).unwrap_or(std::cmp::Ordering::Equal)); - - // Process segments separated by gaps - // We need to iterate through projection_buffer and build segments, - // then mark consumed indices. To avoid borrow conflicts, we'll - // collect segment boundaries first, then process them. - let mut added_any = false; - scratch.segment_points.clear(); - scratch.segment_indices.clear(); - - let projection_len = scratch.projection_buffer.len(); - let mut segment_start = 0usize; - - for i in 0..projection_len { - let (orig_idx, _proj) = scratch.projection_buffer[i]; - - // Check if there's a gap to the next point - let is_gap = if i + 1 < projection_len { - let next_orig_idx = scratch.projection_buffer[i + 1].0; - let p1 = scratch.inlier_points[orig_idx]; - let p2 = scratch.inlier_points[next_orig_idx]; - p1.distance(p2) > config.max_point_gap - } else { - true // End of points - }; - - if is_gap { - // Process segment from segment_start to i (inclusive) - let segment_len = i - segment_start + 1; - - if segment_len >= config.min_inliers { - // Collect segment points - scratch.segment_points.clear(); - scratch.segment_indices.clear(); - - for j in segment_start..=i { - let idx = scratch.projection_buffer[j].0; - scratch.segment_points.push(scratch.inlier_points[idx]); - scratch.segment_indices.push(scratch.inlier_indices[idx]); - } - - // Try to fit line - if let Some(refined_line) = fit_line(&scratch.segment_points) - && refined_line.length() >= config.min_line_length - { - scratch.output_lines.push(refined_line); - added_any = true; - - // Mark segment inliers as consumed - for j in 0..scratch.segment_indices.len() { - let idx = scratch.segment_indices[j]; - scratch.remaining_mask[idx] = false; - } - } - } - - segment_start = i + 1; - } - } - - added_any -} - -#[cfg(test)] -mod tests { - use super::*; - use approx::assert_relative_eq; - - #[test] - fn test_config_default() { - let config = RansacLineConfig::default(); - assert_eq!(config.iterations, 100); - assert_relative_eq!(config.inlier_threshold, 0.02); - assert_eq!(config.min_inliers, 5); - } - - #[test] - fn test_config_builder() { - let config = RansacLineConfig::new() - .with_iterations(50) - .with_inlier_threshold(0.05) - .with_min_inliers(3) - .with_seed(42); - - assert_eq!(config.iterations, 50); - assert_relative_eq!(config.inlier_threshold, 0.05); - assert_eq!(config.min_inliers, 3); - assert_eq!(config.seed, 42); - } - - #[test] - fn test_extract_empty_points() { - let config = RansacLineConfig::default(); - let (lines, remaining) = extract_lines_ransac(&[], &config); - - assert!(lines.is_empty()); - assert!(remaining.is_empty()); - } - - #[test] - fn test_extract_too_few_points() { - let points = vec![Point2D::new(0.0, 0.0), Point2D::new(1.0, 0.0)]; - let config = RansacLineConfig::default().with_min_inliers(5); - let (lines, remaining) = extract_lines_ransac(&points, &config); - - assert!(lines.is_empty()); - assert_eq!(remaining.len(), 2); - } - - #[test] - fn test_extract_single_line() { - // Points along a horizontal line (5cm spacing, under 10cm gap threshold) - let points: Vec = (0..20) - .map(|i| Point2D::new(i as f32 * 0.05, 0.0)) - .collect(); - - let config = RansacLineConfig::default() - .with_seed(42) - .with_min_inliers(5); - - let (lines, remaining) = extract_lines_ransac(&points, &config); - - assert_eq!(lines.len(), 1); - // Most points should be consumed - assert!(remaining.len() < 5); - // Line should be roughly horizontal - let angle = lines[0].angle().abs(); - assert!(angle < 0.1 || (std::f32::consts::PI - angle).abs() < 0.1); - } - - #[test] - fn test_extract_two_lines() { - // Points along a horizontal line (5cm spacing) - let mut points: Vec = (0..15) - .map(|i| Point2D::new(i as f32 * 0.05, 0.0)) - .collect(); - - // Points along a vertical line (5cm spacing) - for i in 0..15 { - points.push(Point2D::new(0.0, i as f32 * 0.05)); - } - - let config = RansacLineConfig::default() - .with_seed(42) - .with_min_inliers(5) - .with_max_lines(2); - - let (lines, _remaining) = extract_lines_ransac(&points, &config); - - assert_eq!(lines.len(), 2); - } - - #[test] - fn test_extract_with_noise() { - // Points along a horizontal line with some noise (5cm spacing) - let mut points: Vec = (0..20) - .map(|i| Point2D::new(i as f32 * 0.05, (i as f32 * 0.05).sin() * 0.01)) - .collect(); - - // Add some outliers - points.push(Point2D::new(0.5, 1.0)); - points.push(Point2D::new(1.0, -1.0)); - - let config = RansacLineConfig::default() - .with_seed(42) - .with_inlier_threshold(0.03) - .with_min_inliers(5); - - let (lines, _remaining) = extract_lines_ransac(&points, &config); - - // Should find the main line despite outliers - assert!(!lines.is_empty()); - } - - #[test] - fn test_max_lines_limit() { - // Many points that could form multiple lines (5cm spacing) - let mut points: Vec = Vec::new(); - for j in 0..4 { - for i in 0..10 { - points.push(Point2D::new(i as f32 * 0.05, j as f32 * 0.5)); - } - } - - let config = RansacLineConfig::default() - .with_seed(42) - .with_min_inliers(5) - .with_max_lines(2); - - let (lines, _remaining) = extract_lines_ransac(&points, &config); - - assert!(lines.len() <= 2); - } - - #[test] - fn test_gap_splitting_no_gap() { - // Continuous line with no gaps (3cm spacing, well under 10cm threshold) - let points: Vec = (0..20) - .map(|i| Point2D::new(i as f32 * 0.03, 0.0)) // 3cm spacing - .collect(); - let indices: Vec = (0..20).collect(); - - let config = RansacLineConfig::default().with_max_point_gap(0.10); - - let segments = split_at_gaps(&points, &indices, &config); - - // Should result in a single segment (all points within gap threshold) - assert_eq!(segments.len(), 1); - assert_eq!(segments[0].0.len(), 20); - } - - #[test] - fn test_gap_splitting_with_gap() { - // Two wall segments separated by a doorway (gap > 10cm) - let mut points = Vec::new(); - // First wall segment: 5 points with 3cm spacing - for i in 0..5 { - points.push(Point2D::new(i as f32 * 0.03, 0.0)); - } - // Second wall segment: starts at 0.35m (gap of ~23cm from last point) - for i in 0..5 { - points.push(Point2D::new(0.35 + i as f32 * 0.03, 0.0)); - } - let indices: Vec = (0..10).collect(); - - let config = RansacLineConfig::default().with_max_point_gap(0.10); - - let segments = split_at_gaps(&points, &indices, &config); - - // Should split into two segments due to gap - assert_eq!(segments.len(), 2); - assert_eq!(segments[0].0.len(), 5); - assert_eq!(segments[1].0.len(), 5); - } - - #[test] - fn test_gap_splitting_multiple_gaps() { - // Three wall segments with gaps - let mut points = Vec::new(); - // Segment 1: 3 points with 3cm spacing (0.0 to 0.06m) - for i in 0..3 { - points.push(Point2D::new(i as f32 * 0.03, 0.0)); - } - // Gap of ~19cm - // Segment 2: 3 points starting at 0.25m - for i in 0..3 { - points.push(Point2D::new(0.25 + i as f32 * 0.03, 0.0)); - } - // Gap of ~19cm - // Segment 3: 3 points starting at 0.50m - for i in 0..3 { - points.push(Point2D::new(0.50 + i as f32 * 0.03, 0.0)); - } - let indices: Vec = (0..9).collect(); - - let config = RansacLineConfig::default().with_max_point_gap(0.10); - - let segments = split_at_gaps(&points, &indices, &config); - - // Should split into three segments - assert_eq!(segments.len(), 3); - for segment in &segments { - assert_eq!(segment.0.len(), 3); - } - } - - #[test] - fn test_extract_lines_respects_gap() { - // Collinear points with a gap (simulating doorway between walls) - let mut points = Vec::new(); - // Left wall segment: 10 points with 3cm spacing (0.27m length) - for i in 0..10 { - points.push(Point2D::new(i as f32 * 0.03, 0.0)); - } - // Gap of ~25cm - // Right wall segment: 10 points with 3cm spacing starting at 0.52m - for i in 0..10 { - points.push(Point2D::new(0.52 + i as f32 * 0.03, 0.0)); - } - - let config = RansacLineConfig::default() - .with_seed(42) - .with_min_inliers(5) - .with_max_point_gap(0.10); - - let (lines, _remaining) = extract_lines_ransac(&points, &config); - - // Should find two separate lines, not one long line - assert_eq!(lines.len(), 2); - - // Check each line is approximately the expected length - for line in &lines { - // Each wall segment is ~0.27m (10 points * 0.03m spacing) - assert!(line.length() < 0.35, "Line too long: {}", line.length()); - assert!(line.length() > 0.15, "Line too short: {}", line.length()); - } - } - - // ========================================================================= - // Tests for scratch space version - // ========================================================================= - - #[test] - fn test_scratch_space_creation() { - let scratch = RansacScratchSpace::with_capacity(400); - assert!(scratch.remaining_mask.capacity() >= 400); - assert!(scratch.current_inliers.capacity() >= 400); - } - - #[test] - fn test_scratch_extract_single_line() { - // Same test as non-scratch version - let points: Vec = (0..20) - .map(|i| Point2D::new(i as f32 * 0.05, 0.0)) - .collect(); - - let config = RansacLineConfig::default() - .with_seed(42) - .with_min_inliers(5); - - let mut scratch = RansacScratchSpace::with_capacity(400); - let (lines, remaining) = extract_lines_ransac_with_scratch(&points, &config, &mut scratch); - - assert_eq!(lines.len(), 1); - assert!(remaining.len() < 5); - let angle = lines[0].angle().abs(); - assert!(angle < 0.1 || (std::f32::consts::PI - angle).abs() < 0.1); - } - - #[test] - fn test_scratch_extract_two_lines() { - let mut points: Vec = (0..15) - .map(|i| Point2D::new(i as f32 * 0.05, 0.0)) - .collect(); - - for i in 0..15 { - points.push(Point2D::new(0.0, i as f32 * 0.05)); - } - - let config = RansacLineConfig::default() - .with_seed(42) - .with_min_inliers(5) - .with_max_lines(2); - - let mut scratch = RansacScratchSpace::with_capacity(400); - let (lines, _remaining) = extract_lines_ransac_with_scratch(&points, &config, &mut scratch); - - assert_eq!(lines.len(), 2); - } - - #[test] - fn test_scratch_reuse() { - // Test that scratch space can be reused across multiple calls - let mut scratch = RansacScratchSpace::with_capacity(400); - - // First extraction - let points1: Vec = (0..20) - .map(|i| Point2D::new(i as f32 * 0.05, 0.0)) - .collect(); - let config = RansacLineConfig::default() - .with_seed(42) - .with_min_inliers(5); - - let (lines1, _) = extract_lines_ransac_with_scratch(&points1, &config, &mut scratch); - assert_eq!(lines1.len(), 1); - - // Second extraction with different points - reusing same scratch - let points2: Vec = (0..20) - .map(|i| Point2D::new(0.0, i as f32 * 0.05)) - .collect(); - - let (lines2, _) = extract_lines_ransac_with_scratch(&points2, &config, &mut scratch); - assert_eq!(lines2.len(), 1); - - // Verify the second extraction is independent - let angle2 = lines2[0].angle().abs(); - let expected_vertical = std::f32::consts::FRAC_PI_2; - assert!((angle2 - expected_vertical).abs() < 0.1); - } - - #[test] - fn test_scratch_equivalence_to_original() { - // Verify scratch version produces same results as original - let points: Vec = (0..25) - .map(|i| Point2D::new(i as f32 * 0.04, 0.0)) - .collect(); - - let config = RansacLineConfig::default() - .with_seed(123) - .with_min_inliers(5) - .with_iterations(50); - - // Original version - let (lines_orig, remaining_orig) = extract_lines_ransac(&points, &config); - - // Scratch version - let mut scratch = RansacScratchSpace::with_capacity(100); - let (lines_scratch, remaining_scratch) = - extract_lines_ransac_with_scratch(&points, &config, &mut scratch); - - // Same number of lines - assert_eq!(lines_orig.len(), lines_scratch.len()); - // Same number of remaining points - assert_eq!(remaining_orig.len(), remaining_scratch.len()); - } - - #[test] - fn test_scratch_gap_splitting() { - // Collinear points with a gap - let mut points = Vec::new(); - for i in 0..10 { - points.push(Point2D::new(i as f32 * 0.03, 0.0)); - } - for i in 0..10 { - points.push(Point2D::new(0.52 + i as f32 * 0.03, 0.0)); - } - - let config = RansacLineConfig::default() - .with_seed(42) - .with_min_inliers(5) - .with_max_point_gap(0.10); - - let mut scratch = RansacScratchSpace::with_capacity(100); - let (lines, _) = extract_lines_ransac_with_scratch(&points, &config, &mut scratch); - - assert_eq!(lines.len(), 2); - for line in lines { - assert!(line.length() < 0.35); - assert!(line.length() > 0.15); - } - } - - #[test] - fn test_scratch_empty_input() { - let points: Vec = vec![]; - let config = RansacLineConfig::default(); - let mut scratch = RansacScratchSpace::with_capacity(100); - - let (lines, remaining) = extract_lines_ransac_with_scratch(&points, &config, &mut scratch); - - assert!(lines.is_empty()); - assert!(remaining.is_empty()); - } - - #[test] - fn test_scratch_too_few_points() { - let points = vec![Point2D::new(0.0, 0.0), Point2D::new(1.0, 0.0)]; - let config = RansacLineConfig::default().with_min_inliers(5); - let mut scratch = RansacScratchSpace::with_capacity(100); - - let (lines, remaining) = extract_lines_ransac_with_scratch(&points, &config, &mut scratch); - - assert!(lines.is_empty()); - assert_eq!(remaining.len(), 2); - } -} diff --git a/vastu-map/src/extraction/split_merge/algorithm.rs b/vastu-map/src/extraction/split_merge/algorithm.rs deleted file mode 100644 index 38c012b..0000000 --- a/vastu-map/src/extraction/split_merge/algorithm.rs +++ /dev/null @@ -1,742 +0,0 @@ -//! Core Split-and-Merge algorithm implementation. -//! -//! Contains the recursive split phase and the main split-and-merge orchestration. - -use crate::core::Point2D; -use crate::features::Line2D; - -use super::config::SplitMergeConfig; -use super::helpers::{ - adaptive_split_threshold, find_best_split_point, merge_segments, merge_segments_from_sensor, - split_by_gaps_indices, -}; -use crate::extraction::line_fitting::{fit_line, fit_line_from_sensor}; - -/// Extract line segments from an ordered sequence of points. -/// -/// Points should be ordered (e.g., by angle in a lidar scan). -/// Returns extracted line segments. -/// -/// # Example -/// ``` -/// use vastu_map::extraction::{SplitMergeConfig, extract_lines}; -/// use vastu_map::core::Point2D; -/// -/// // Simulated lidar points forming two perpendicular walls -/// let points = vec![ -/// // Horizontal wall -/// Point2D::new(0.0, 1.0), -/// Point2D::new(0.5, 1.0), -/// Point2D::new(1.0, 1.0), -/// Point2D::new(1.5, 1.0), -/// Point2D::new(2.0, 1.0), -/// // Corner transition -/// // Vertical wall -/// Point2D::new(2.0, 1.5), -/// Point2D::new(2.0, 2.0), -/// Point2D::new(2.0, 2.5), -/// Point2D::new(2.0, 3.0), -/// Point2D::new(2.0, 3.5), -/// ]; -/// -/// let config = SplitMergeConfig::default(); -/// let lines = extract_lines(&points, &config); -/// // Should extract approximately 2 lines -/// ``` -pub fn extract_lines(points: &[Point2D], config: &SplitMergeConfig) -> Vec { - if points.len() < config.min_points { - return Vec::new(); - } - - // Split points into continuous sequences by index pairs (avoids copying) - let sequences = split_by_gaps_indices(points, config.max_point_gap); - - let mut all_lines = Vec::new(); - - for (start, end) in sequences { - let seq_len = end - start; - if seq_len < config.min_points { - continue; - } - - // Extract lines from this sequence using split-and-merge - let sequence = &points[start..end]; - let lines = split_and_merge(sequence, config); - all_lines.extend(lines); - } - - all_lines -} - -/// Extract line segments with sensor-aware processing. -/// -/// This is the preferred method for lidar data. It uses: -/// 1. **Adaptive thresholds**: Split threshold scales with range (farther = more lenient) -/// 2. **Weighted line fitting**: Closer points have more influence on line direction -/// -/// # Arguments -/// * `points` - Ordered sequence of lidar hits (e.g., by angle) -/// * `config` - Split-and-merge configuration -/// * `sensor_pos` - Position of the lidar sensor (typically robot position) -/// -/// # Example -/// ``` -/// use vastu_map::extraction::{SplitMergeConfig, extract_lines_from_sensor}; -/// use vastu_map::core::Point2D; -/// -/// let sensor_pos = Point2D::new(0.0, 0.0); -/// let points = vec![ -/// Point2D::new(1.0, 0.0), -/// Point2D::new(2.0, 0.0), -/// Point2D::new(3.0, 0.0), -/// Point2D::new(4.0, 0.0), -/// Point2D::new(5.0, 0.0), -/// ]; -/// -/// let config = SplitMergeConfig::default(); -/// let lines = extract_lines_from_sensor(&points, &config, sensor_pos); -/// ``` -pub fn extract_lines_from_sensor( - points: &[Point2D], - config: &SplitMergeConfig, - sensor_pos: Point2D, -) -> Vec { - if points.len() < config.min_points { - return Vec::new(); - } - - // Split points into continuous sequences by index pairs (avoids copying) - let sequences = split_by_gaps_indices(points, config.max_point_gap); - - let mut all_lines = Vec::new(); - - for (start, end) in sequences { - let seq_len = end - start; - if seq_len < config.min_points { - continue; - } - - // Extract lines from this sequence using sensor-aware split-and-merge - let sequence = &points[start..end]; - let lines = split_and_merge_from_sensor(sequence, config, sensor_pos); - all_lines.extend(lines); - } - - all_lines -} - -/// Main split-and-merge algorithm on a continuous sequence. -fn split_and_merge(points: &[Point2D], config: &SplitMergeConfig) -> Vec { - // Split phase: find breakpoints - let mut breakpoints = vec![0]; - split_recursive(points, 0, points.len() - 1, config, &mut breakpoints); - breakpoints.push(points.len() - 1); - breakpoints.sort_unstable(); - breakpoints.dedup(); - - #[cfg(test)] - { - if points.len() > 100 { - eprintln!( - "split_and_merge: {} points, {} breakpoints: {:?}", - points.len(), - breakpoints.len(), - &breakpoints[..breakpoints.len().min(20)] - ); - } - } - - // Create line segments from breakpoints - let mut segments: Vec<(usize, usize, Line2D)> = Vec::new(); - - for i in 0..breakpoints.len() - 1 { - let start = breakpoints[i]; - let end = breakpoints[i + 1]; - - if end - start + 1 < config.min_points { - continue; - } - - let segment_points = &points[start..=end]; - if let Some(line) = fit_line(segment_points) - && line.length() >= config.min_length - { - segments.push((start, end, line)); - } - } - - #[cfg(test)] - { - if points.len() > 100 { - eprintln!("split_and_merge: {} segments before merge", segments.len()); - for (i, (start, end, line)) in segments.iter().enumerate().take(10) { - eprintln!( - " Seg {}: pts {}..{}, angle={:.1}°", - i, - start, - end, - line.angle().to_degrees() - ); - } - } - } - - // Merge phase: combine adjacent segments that fit well together - let merged = merge_segments(points, segments, config); - - #[cfg(test)] - { - if points.len() > 100 { - eprintln!("split_and_merge: {} segments after merge", merged.len()); - } - } - - merged.into_iter().map(|(_, _, line)| line).collect() -} - -/// Recursive split function. -fn split_recursive( - points: &[Point2D], - start: usize, - end: usize, - config: &SplitMergeConfig, - breakpoints: &mut Vec, -) { - if end <= start + 1 { - return; - } - - let segment_points = &points[start..=end]; - - // Fit line to segment - let Some(line) = fit_line(segment_points) else { - return; - }; - - // Find point with maximum deviation, excluding boundary points - // Boundary points can't be used as split points without causing degenerate recursion - #[allow(unused_variables)] // max_dist only used in debug output under cfg(test) - let Some((split_rel_idx, max_dist)) = - find_best_split_point(segment_points, &line, config.split_threshold) - else { - return; - }; - - #[cfg(test)] - { - if points.len() > 100 && end - start > 50 { - eprintln!( - "split_recursive: {}..{} ({} pts), line_angle={:.1}°, max_dist={:.3} at rel_idx={}, split_thresh={}", - start, - end, - end - start + 1, - line.angle().to_degrees(), - max_dist, - split_rel_idx, - config.split_threshold - ); - } - } - - let split_idx = start + split_rel_idx; - breakpoints.push(split_idx); - - // Recursively split both halves - split_recursive(points, start, split_idx, config, breakpoints); - split_recursive(points, split_idx, end, config, breakpoints); -} - -// ============================================================================= -// Sensor-Aware Split-and-Merge (with adaptive thresholds and weighted fitting) -// ============================================================================= - -/// Main split-and-merge algorithm with sensor-aware processing. -/// -/// Uses adaptive thresholds based on range and weighted line fitting. -fn split_and_merge_from_sensor( - points: &[Point2D], - config: &SplitMergeConfig, - sensor_pos: Point2D, -) -> Vec { - // Split phase: find breakpoints with adaptive thresholds - let mut breakpoints = vec![0]; - split_recursive_from_sensor( - points, - 0, - points.len() - 1, - config, - sensor_pos, - &mut breakpoints, - ); - breakpoints.push(points.len() - 1); - breakpoints.sort_unstable(); - breakpoints.dedup(); - - #[cfg(test)] - { - if points.len() > 100 { - eprintln!( - "split_and_merge_from_sensor: {} points, {} breakpoints", - points.len(), - breakpoints.len() - ); - } - } - - // Create line segments from breakpoints using weighted fitting - let mut segments: Vec<(usize, usize, Line2D)> = Vec::new(); - - for i in 0..breakpoints.len() - 1 { - let start = breakpoints[i]; - let end = breakpoints[i + 1]; - - if end - start + 1 < config.min_points { - continue; - } - - let segment_points = &points[start..=end]; - // Use weighted line fitting for better accuracy - if let Some(line) = fit_line_from_sensor(segment_points, sensor_pos, None) - && line.length() >= config.min_length - { - segments.push((start, end, line)); - } - } - - // Merge phase: combine adjacent segments with adaptive thresholds - let merged = merge_segments_from_sensor(points, segments, config, sensor_pos); - - merged.into_iter().map(|(_, _, line)| line).collect() -} - -/// Recursive split function with adaptive thresholds based on range. -fn split_recursive_from_sensor( - points: &[Point2D], - start: usize, - end: usize, - config: &SplitMergeConfig, - sensor_pos: Point2D, - breakpoints: &mut Vec, -) { - if end <= start + 1 { - return; - } - - let segment_points = &points[start..=end]; - - // Fit line using weighted TLS - let Some(line) = fit_line_from_sensor(segment_points, sensor_pos, None) else { - return; - }; - - // Compute adaptive threshold for this segment based on average range - let adaptive_threshold = adaptive_split_threshold( - segment_points, - config.split_threshold, - sensor_pos, - config.adaptive_range_scale, - ); - - // Find point with maximum deviation using adaptive threshold - let Some((split_rel_idx, _max_dist)) = - find_best_split_point(segment_points, &line, adaptive_threshold) - else { - return; - }; - - let split_idx = start + split_rel_idx; - breakpoints.push(split_idx); - - // Recursively split both halves - split_recursive_from_sensor(points, start, split_idx, config, sensor_pos, breakpoints); - split_recursive_from_sensor(points, split_idx, end, config, sensor_pos, breakpoints); -} - -#[cfg(test)] -mod tests { - use super::*; - use approx::assert_relative_eq; - use std::f32::consts::FRAC_PI_2; - - #[test] - fn test_extract_single_line() { - // Generate points along a horizontal line - let points: Vec<_> = (0..20).map(|i| Point2D::new(i as f32 * 0.1, 0.0)).collect(); - - let config = SplitMergeConfig::default(); - let lines = extract_lines(&points, &config); - - assert_eq!(lines.len(), 1); - assert_relative_eq!(lines[0].angle(), 0.0, epsilon = 0.1); - } - - #[test] - fn test_extract_two_lines_corner() { - // L-shaped points: horizontal then vertical - let mut points = Vec::new(); - - // Horizontal segment (10 points) - for i in 0..10 { - points.push(Point2D::new(i as f32 * 0.2, 0.0)); - } - - // Vertical segment (10 points), starting from end of horizontal - for i in 1..10 { - points.push(Point2D::new(1.8, i as f32 * 0.2)); - } - - let config = SplitMergeConfig::default() - .with_min_points(5) - .with_min_length(0.5); - - let lines = extract_lines(&points, &config); - - // Should extract 2 lines - assert!( - lines.len() >= 1, - "Expected at least 1 line, got {}", - lines.len() - ); - - // One should be approximately horizontal, one approximately vertical - let angles: Vec<_> = lines.iter().map(|l| l.angle()).collect(); - - if lines.len() >= 2 { - // Check that we have both orientations - let has_horizontal = angles.iter().any(|a| a.abs() < 0.3); - let has_vertical = angles.iter().any(|a| (a.abs() - FRAC_PI_2).abs() < 0.3); - - assert!( - has_horizontal || has_vertical, - "Expected horizontal or vertical lines, got angles: {:?}", - angles - ); - } - } - - #[test] - fn test_extract_noisy_line() { - // Points along a line with some noise - let points: Vec<_> = (0..20) - .map(|i| { - let noise = if i % 2 == 0 { 0.02 } else { -0.02 }; - Point2D::new(i as f32 * 0.1, noise) - }) - .collect(); - - let config = SplitMergeConfig::default().with_split_threshold(0.05); - let lines = extract_lines(&points, &config); - - // Should still extract as single line (noise within threshold) - assert_eq!(lines.len(), 1); - } - - #[test] - fn test_extract_min_points_filter() { - // Only 3 points - below minimum - let points = vec![ - Point2D::new(0.0, 0.0), - Point2D::new(1.0, 0.0), - Point2D::new(2.0, 0.0), - ]; - - let config = SplitMergeConfig::default().with_min_points(5); - let lines = extract_lines(&points, &config); - - assert!(lines.is_empty()); - } - - #[test] - fn test_extract_min_length_filter() { - // Short line - let points: Vec<_> = (0..10) - .map(|i| Point2D::new(i as f32 * 0.005, 0.0)) // Total length 0.045m - .collect(); - - let config = SplitMergeConfig::default() - .with_min_points(3) - .with_min_length(0.1); - - let lines = extract_lines(&points, &config); - - assert!(lines.is_empty()); - } - - #[test] - fn test_extract_with_gap() { - // Two separate line segments with a gap - let mut points = Vec::new(); - - // First segment - for i in 0..10 { - points.push(Point2D::new(i as f32 * 0.1, 0.0)); - } - - // Gap of 1.0m - // Second segment - for i in 0..10 { - points.push(Point2D::new(2.0 + i as f32 * 0.1, 1.0)); - } - - let config = SplitMergeConfig::default() - .with_max_point_gap(0.5) - .with_min_points(5); - - let lines = extract_lines(&points, &config); - - // Should extract 2 separate lines - assert_eq!(lines.len(), 2); - } - - #[test] - fn test_empty_input() { - let points: Vec = vec![]; - let config = SplitMergeConfig::default(); - let lines = extract_lines(&points, &config); - - assert!(lines.is_empty()); - } - - #[test] - fn test_circular_points_no_infinite_recursion() { - // Simulate a 360° lidar scan forming a rough circle. - // This previously caused infinite recursion when max deviation - // point was at index 0, causing split_idx == start. - use std::f32::consts::PI; - - let num_points = 100; - let radius = 2.0; - let points: Vec<_> = (0..num_points) - .map(|i| { - let angle = 2.0 * PI * (i as f32) / (num_points as f32); - // Add some noise to create deviation - let r = radius + 0.1 * ((i % 5) as f32 - 2.0); - Point2D::new(r * angle.cos(), r * angle.sin()) - }) - .collect(); - - let config = SplitMergeConfig::default() - .with_min_points(3) - .with_split_threshold(0.1); - - // This should complete without hanging (previously infinite recursion) - let lines = extract_lines(&points, &config); - - // We should get some lines (exact number depends on noise pattern) - // The important thing is that it terminates without panic - let _ = lines.len(); - } - - #[test] - fn test_square_room_360_scan() { - // Simulate a 360° lidar scan of a 4m x 4m square room - // with robot at center (0, 0) facing +X. - // This tests the algorithm's ability to detect axis-aligned walls. - use std::f32::consts::{PI, TAU}; - - // Room walls at x = ±2, y = ±2 - let half_size = 2.0; - let num_rays = 360; - let mut points = Vec::with_capacity(num_rays); - - for i in 0..num_rays { - let angle = TAU * (i as f32) / (num_rays as f32); - let (sin, cos) = angle.sin_cos(); - - // Compute distance to walls using ray-box intersection - let t_right = if cos > 0.0 { - half_size / cos - } else { - f32::INFINITY - }; - let t_left = if cos < 0.0 { - -half_size / cos - } else { - f32::INFINITY - }; - let t_top = if sin > 0.0 { - half_size / sin - } else { - f32::INFINITY - }; - let t_bottom = if sin < 0.0 { - -half_size / sin - } else { - f32::INFINITY - }; - - let t = t_right.min(t_left).min(t_top).min(t_bottom); - let point = Point2D::new(t * cos, t * sin); - points.push(point); - } - - let config = SplitMergeConfig::default() - .with_min_points(5) - .with_min_length(0.5) - .with_split_threshold(0.05); - - let lines = extract_lines(&points, &config); - - // Debug: print extracted lines - println!("\nExtracted {} lines from square room scan:", lines.len()); - for (i, line) in lines.iter().enumerate() { - let angle_deg = line.angle().to_degrees(); - println!( - " Line {}: ({:.2}, {:.2}) to ({:.2}, {:.2}), angle={:.1}°, len={:.2}", - i, - line.start.x, - line.start.y, - line.end.x, - line.end.y, - angle_deg, - line.length() - ); - } - - // We should get 4 lines (one per wall) - // Allow some flexibility in case corners create extra short segments - assert!( - lines.len() >= 4 && lines.len() <= 8, - "Expected 4-8 lines, got {}", - lines.len() - ); - - // Check that lines are axis-aligned (within 10 degrees) - let axis_aligned_count = lines - .iter() - .filter(|line| { - let angle = line.angle().abs(); - let normalized = angle % PI; - let dist_horiz = normalized.min(PI - normalized); - let dist_vert = (normalized - FRAC_PI_2).abs(); - dist_horiz < 0.175 || dist_vert < 0.175 - }) - .count(); - - println!( - "Axis-aligned lines: {} / {}", - axis_aligned_count, - lines.len() - ); - - // At least 80% of lines should be axis-aligned in a square room - let axis_ratio = axis_aligned_count as f32 / lines.len() as f32; - assert!( - axis_ratio >= 0.75, - "Only {}% of lines are axis-aligned (expected >= 75%)", - (axis_ratio * 100.0) as i32 - ); - } - - #[test] - fn test_degenerate_split_at_boundary() { - // Test case where all points except first/last are collinear, - // forcing max deviation at boundary indices. - let points = vec![ - Point2D::new(0.0, 1.0), // Outlier at start - Point2D::new(0.1, 0.0), - Point2D::new(0.2, 0.0), - Point2D::new(0.3, 0.0), - Point2D::new(0.4, 0.0), - Point2D::new(0.5, 0.0), - Point2D::new(0.6, 0.0), - Point2D::new(0.7, 0.0), - Point2D::new(0.8, 0.0), - Point2D::new(0.9, 1.0), // Outlier at end - ]; - - let config = SplitMergeConfig::default() - .with_min_points(3) - .with_split_threshold(0.05); - - // Should terminate without infinite recursion - let lines = extract_lines(&points, &config); - let _ = lines.len(); - } - - #[test] - fn test_extract_lines_from_sensor_horizontal() { - let sensor_pos = Point2D::new(0.0, 0.0); - let points: Vec<_> = (0..20) - .map(|i| Point2D::new(2.0 + i as f32 * 0.1, 0.0)) - .collect(); - - let config = SplitMergeConfig::default(); - let lines = extract_lines_from_sensor(&points, &config, sensor_pos); - - assert_eq!(lines.len(), 1); - assert_relative_eq!(lines[0].angle(), 0.0, epsilon = 0.1); - } - - #[test] - fn test_extract_lines_from_sensor_with_noise() { - let sensor_pos = Point2D::new(0.0, 0.0); - - // Points at 5m range with noise that's within adaptive threshold - let points: Vec<_> = (0..20) - .map(|i| { - let noise = if i % 2 == 0 { 0.08 } else { -0.08 }; - Point2D::new(5.0 + i as f32 * 0.1, noise) - }) - .collect(); - - let config = SplitMergeConfig::default() - .with_split_threshold(0.05) - .with_adaptive_range_scale(0.03); - - // Without sensor awareness, this might over-split due to noise - let lines_no_sensor = extract_lines(&points, &config); - - // With sensor awareness, adaptive threshold should tolerate more noise at 5m - let lines_from_sensor = extract_lines_from_sensor(&points, &config, sensor_pos); - - // Sensor-aware should produce same or fewer lines (less over-splitting) - assert!( - lines_from_sensor.len() <= lines_no_sensor.len(), - "Sensor-aware should not over-split: {} vs {} lines", - lines_from_sensor.len(), - lines_no_sensor.len() - ); - } - - #[test] - fn test_extract_lines_from_sensor_corner() { - let sensor_pos = Point2D::new(0.0, 0.0); - - // L-shaped points: horizontal then vertical - let mut points = Vec::new(); - - // Horizontal segment at y=2 (2m range) - for i in 0..10 { - points.push(Point2D::new(i as f32 * 0.2, 2.0)); - } - - // Vertical segment at x=1.8 - for i in 1..10 { - points.push(Point2D::new(1.8, 2.0 + i as f32 * 0.2)); - } - - let config = SplitMergeConfig::default() - .with_min_points(5) - .with_min_length(0.5); - - let lines = extract_lines_from_sensor(&points, &config, sensor_pos); - - // Should extract at least 1 line - assert!( - lines.len() >= 1, - "Expected at least 1 line, got {}", - lines.len() - ); - } - - #[test] - fn test_config_adaptive_range_scale() { - let config = SplitMergeConfig::default().with_adaptive_range_scale(0.05); - assert_eq!(config.adaptive_range_scale, 0.05); - - // Default should be 0.03 - let default_config = SplitMergeConfig::default(); - assert_eq!(default_config.adaptive_range_scale, 0.03); - } -} diff --git a/vastu-map/src/extraction/split_merge/config.rs b/vastu-map/src/extraction/split_merge/config.rs deleted file mode 100644 index d48735f..0000000 --- a/vastu-map/src/extraction/split_merge/config.rs +++ /dev/null @@ -1,88 +0,0 @@ -//! Configuration for Split-and-Merge line extraction. - -use serde::{Deserialize, Serialize}; - -/// Configuration for Split-and-Merge algorithm. -#[derive(Clone, Debug, Serialize, Deserialize)] -pub struct SplitMergeConfig { - /// Maximum perpendicular distance for a point to be considered on the line. - /// If any point deviates more than this, the segment is split. - /// Default: 0.05m (5cm) - pub split_threshold: f32, - - /// Minimum number of points required to form a line segment. - /// Segments with fewer points are discarded. - /// Default: 5 - pub min_points: usize, - - /// Minimum line segment length. - /// Segments shorter than this are discarded. - /// Default: 0.10m (10cm) - pub min_length: f32, - - /// Maximum gap between consecutive points. - /// If gap exceeds this, points are split into separate sequences. - /// Default: 0.30m (30cm) - pub max_point_gap: f32, - - /// Merge threshold for adjacent segments. - /// Segments whose combined fit error is below this are merged. - /// Default: same as split_threshold - pub merge_threshold: f32, - - /// Coefficient for range-adaptive threshold scaling. - /// When using adaptive thresholds: - /// effective_threshold = split_threshold × (1 + adaptive_range_scale × avg_range) - /// Default: 0.03 (3% increase per meter) - pub adaptive_range_scale: f32, -} - -impl Default for SplitMergeConfig { - fn default() -> Self { - Self { - split_threshold: 0.05, - min_points: 5, - min_length: 0.10, - max_point_gap: 0.30, - merge_threshold: 0.05, - adaptive_range_scale: 0.03, // 3% per meter - } - } -} - -impl SplitMergeConfig { - /// Create a new configuration with default values. - pub fn new() -> Self { - Self::default() - } - - /// Builder-style setter for split threshold. - pub fn with_split_threshold(mut self, value: f32) -> Self { - self.split_threshold = value; - self - } - - /// Builder-style setter for minimum points. - pub fn with_min_points(mut self, value: usize) -> Self { - self.min_points = value; - self - } - - /// Builder-style setter for minimum length. - pub fn with_min_length(mut self, value: f32) -> Self { - self.min_length = value; - self - } - - /// Builder-style setter for maximum point gap. - pub fn with_max_point_gap(mut self, value: f32) -> Self { - self.max_point_gap = value; - self - } - - /// Builder-style setter for adaptive range scale. - pub fn with_adaptive_range_scale(mut self, value: f32) -> Self { - self.adaptive_range_scale = value; - self - } -} diff --git a/vastu-map/src/extraction/split_merge/helpers.rs b/vastu-map/src/extraction/split_merge/helpers.rs deleted file mode 100644 index f535842..0000000 --- a/vastu-map/src/extraction/split_merge/helpers.rs +++ /dev/null @@ -1,470 +0,0 @@ -//! Helper functions for Split-and-Merge algorithm. -//! -//! Contains gap detection, corner detection, segment merging, and adaptive threshold computation. - -use std::simd::{f32x4, num::SimdFloat}; - -use crate::core::Point2D; -use crate::features::Line2D; - -use super::config::SplitMergeConfig; -use crate::extraction::line_fitting::{fit_line, fit_line_from_sensor, max_distance_point}; - -/// Compute adaptive split threshold based on average range from sensor. -/// -/// Farther points have higher measurement uncertainty, so a larger threshold -/// is appropriate. The formula is: -/// threshold = base_threshold × (1 + scale × avg_range) -/// -/// # Arguments -/// * `points` - Points to compute average range for -/// * `base_threshold` - Base threshold (e.g., 0.05m) -/// * `sensor_pos` - Position of the sensor -/// * `scale` - Range scaling factor (e.g., 0.03 = 3% per meter) -#[inline] -pub fn adaptive_split_threshold( - points: &[Point2D], - base_threshold: f32, - sensor_pos: Point2D, - scale: f32, -) -> f32 { - if points.is_empty() { - return base_threshold; - } - - let avg_range = - points.iter().map(|p| p.distance(sensor_pos)).sum::() / points.len() as f32; - - // Threshold scales with range: farther points have more noise - base_threshold * (1.0 + scale * avg_range) -} - -/// Split points into continuous sequences based on gap threshold and corner detection. -/// Returns (start, end) index pairs instead of copying points. -/// -/// This function splits when: -/// 1. Gap between consecutive points exceeds max_gap, OR -/// 2. Direction change between consecutive segments exceeds corner threshold (~45°) -/// -/// This is critical for 360° lidar scans where corners don't create gaps. -pub(crate) fn split_by_gaps_indices(points: &[Point2D], max_gap: f32) -> Vec<(usize, usize)> { - if points.is_empty() { - return Vec::new(); - } - - if points.len() < 3 { - return vec![(0, points.len())]; - } - - let mut sequences = Vec::new(); - let mut seq_start = 0; - - // Corner detection threshold (cosine of 45 degrees = 0.707) - // A sharp direction change indicates a corner between two walls - const CORNER_COS_THRESHOLD: f32 = 0.707; - - for i in 1..points.len() { - let gap = points[i].distance(points[i - 1]); - let is_corner = if i >= 2 { - detect_corner_at(points, i, CORNER_COS_THRESHOLD) - } else { - false - }; - - if gap > max_gap || is_corner { - // End current sequence and start new one - if i > seq_start { - sequences.push((seq_start, i)); - } - seq_start = i; - } - } - - // Add final sequence - if points.len() > seq_start { - sequences.push((seq_start, points.len())); - } - - sequences -} - -/// Detect if there's a corner at the given index by checking direction change. -/// -/// A corner is detected when the direction from points[i-2] to points[i-1] -/// differs significantly from the direction from points[i-1] to points[i]. -fn detect_corner_at(points: &[Point2D], i: usize, cos_threshold: f32) -> bool { - if i < 2 || i >= points.len() { - return false; - } - - // Direction from i-2 to i-1 - let dir1_x = points[i - 1].x - points[i - 2].x; - let dir1_y = points[i - 1].y - points[i - 2].y; - let len1 = (dir1_x * dir1_x + dir1_y * dir1_y).sqrt(); - - // Direction from i-1 to i - let dir2_x = points[i].x - points[i - 1].x; - let dir2_y = points[i].y - points[i - 1].y; - let len2 = (dir2_x * dir2_x + dir2_y * dir2_y).sqrt(); - - // Skip if either segment is too short - if len1 < 0.01 || len2 < 0.01 { - return false; - } - - // Compute cosine of angle between directions - let dot = (dir1_x * dir2_x + dir1_y * dir2_y) / (len1 * len2); - - // Corner if directions are not aligned (cos < threshold) - dot < cos_threshold -} - -/// Split points into continuous sequences based on gap threshold. -/// Returns copied sequences (for compatibility with tests). -#[cfg(test)] -pub(crate) fn split_by_gaps(points: &[Point2D], max_gap: f32) -> Vec> { - split_by_gaps_indices(points, max_gap) - .into_iter() - .map(|(start, end)| points[start..end].to_vec()) - .collect() -} - -/// Find the best split point in a segment. -/// -/// Returns (relative_index, max_distance) of the best point to split at. -/// The split point must: -/// 1. Have deviation > threshold -/// 2. Not be at the very first or last index (to prevent degenerate recursion) -/// 3. Prefer points that are well away from boundaries (actual corners) -/// -/// Returns None if no valid split point exists. -/// SIMD-optimized for batches of 4+ points. -pub(crate) fn find_best_split_point( - segment_points: &[Point2D], - line: &Line2D, - threshold: f32, -) -> Option<(usize, f32)> { - if segment_points.len() < 3 { - return None; - } - - let n = segment_points.len(); - let min_boundary_distance = n / 10; // At least 10% away from boundary - - // Pre-compute line parameters for SIMD distance calculation - let dx = line.end.x - line.start.x; - let dy = line.end.y - line.start.y; - let length_sq = dx * dx + dy * dy; - - if length_sq < f32::EPSILON { - // Degenerate line - use scalar fallback - return find_best_split_point_scalar(segment_points, line, threshold); - } - - let inv_length = 1.0 / length_sq.sqrt(); - - // Compute all deviations using SIMD - let chunks = n / 4; - let mut deviations = Vec::with_capacity(n); - - if chunks > 0 { - let dx4 = f32x4::splat(dx); - let dy4 = f32x4::splat(dy); - let sx4 = f32x4::splat(line.start.x); - let sy4 = f32x4::splat(line.start.y); - let inv_len4 = f32x4::splat(inv_length); - - for i in 0..chunks { - let base = i * 4; - - // Load 4 points - let px = f32x4::from_array([ - segment_points[base].x, - segment_points[base + 1].x, - segment_points[base + 2].x, - segment_points[base + 3].x, - ]); - let py = f32x4::from_array([ - segment_points[base].y, - segment_points[base + 1].y, - segment_points[base + 2].y, - segment_points[base + 3].y, - ]); - - // Compute cross product and distance - let to_px = px - sx4; - let to_py = py - sy4; - let cross = dx4 * to_py - dy4 * to_px; - let dist = cross.abs() * inv_len4; - - deviations.extend_from_slice(&dist.to_array()); - } - } - - // Handle remainder - for p in segment_points.iter().take(n).skip(chunks * 4) { - let to_px = p.x - line.start.x; - let to_py = p.y - line.start.y; - let cross = dx * to_py - dy * to_px; - let dist = cross.abs() * inv_length; - deviations.push(dist); - } - - // Find max deviation point, preferring points away from boundaries - let mut best_idx = None; - let mut best_dist: f32 = 0.0; - - for (i, &dist) in deviations.iter().enumerate() { - // Skip boundary points (degenerate splits) - if i == 0 || i == n - 1 { - continue; - } - - // Check if this point is far enough from boundaries - let dist_from_boundary = i.min(n - 1 - i); - let is_interior = dist_from_boundary >= min_boundary_distance.max(1); - - // For interior points, use normal threshold - // For near-boundary points, require much higher deviation - let effective_threshold = if is_interior { - threshold - } else { - // Near-boundary points need 5x threshold to be considered - threshold * 5.0 - }; - - if dist > effective_threshold && dist > best_dist { - best_dist = dist; - best_idx = Some(i); - } - } - - best_idx.map(|idx| (idx, best_dist)) -} - -/// Scalar fallback for degenerate lines. -fn find_best_split_point_scalar( - segment_points: &[Point2D], - line: &Line2D, - threshold: f32, -) -> Option<(usize, f32)> { - let n = segment_points.len(); - let min_boundary_distance = n / 10; - - let deviations: Vec = segment_points - .iter() - .map(|p| line.distance_to_point(*p)) - .collect(); - - let mut best_idx = None; - let mut best_dist: f32 = 0.0; - - for (i, &dist) in deviations.iter().enumerate() { - if i == 0 || i == n - 1 { - continue; - } - - let dist_from_boundary = i.min(n - 1 - i); - let is_interior = dist_from_boundary >= min_boundary_distance.max(1); - - let effective_threshold = if is_interior { - threshold - } else { - threshold * 5.0 - }; - - if dist > effective_threshold && dist > best_dist { - best_dist = dist; - best_idx = Some(i); - } - } - - best_idx.map(|idx| (idx, best_dist)) -} - -/// Merge adjacent segments that can be represented by a single line. -pub(crate) fn merge_segments( - points: &[Point2D], - mut segments: Vec<(usize, usize, Line2D)>, - config: &SplitMergeConfig, -) -> Vec<(usize, usize, Line2D)> { - if segments.len() < 2 { - return segments; - } - - let mut merged = true; - while merged { - merged = false; - let mut new_segments = Vec::new(); - let mut i = 0; - - while i < segments.len() { - if i + 1 < segments.len() { - let (start1, end1, _) = segments[i]; - let (start2, end2, _) = segments[i + 1]; - - // Check if segments are adjacent - if end1 == start2 || end1 + 1 == start2 { - // Try to merge - let merged_points = &points[start1..=end2]; - if let Some(merged_line) = fit_line(merged_points) { - // Check if merged line fits well - if let Some((_, max_dist)) = max_distance_point(merged_points, &merged_line) - && max_dist <= config.merge_threshold - && merged_line.length() >= config.min_length - { - // Merge successful - new_segments.push((start1, end2, merged_line)); - i += 2; - merged = true; - continue; - } - } - } - } - - // Keep original segment - new_segments.push(segments[i]); - i += 1; - } - - segments = new_segments; - } - - segments -} - -/// Merge adjacent segments with adaptive thresholds and weighted fitting. -pub(crate) fn merge_segments_from_sensor( - points: &[Point2D], - mut segments: Vec<(usize, usize, Line2D)>, - config: &SplitMergeConfig, - sensor_pos: Point2D, -) -> Vec<(usize, usize, Line2D)> { - if segments.len() < 2 { - return segments; - } - - let mut merged = true; - while merged { - merged = false; - let mut new_segments = Vec::new(); - let mut i = 0; - - while i < segments.len() { - if i + 1 < segments.len() { - let (start1, end1, _) = segments[i]; - let (start2, end2, _) = segments[i + 1]; - - // Check if segments are adjacent - if end1 == start2 || end1 + 1 == start2 { - // Try to merge with weighted fitting - let merged_points = &points[start1..=end2]; - if let Some(merged_line) = fit_line_from_sensor(merged_points, sensor_pos, None) - { - // Compute adaptive merge threshold - let adaptive_threshold = adaptive_split_threshold( - merged_points, - config.merge_threshold, - sensor_pos, - config.adaptive_range_scale, - ); - - // Check if merged line fits well - if let Some((_, max_dist)) = max_distance_point(merged_points, &merged_line) - && max_dist <= adaptive_threshold - && merged_line.length() >= config.min_length - { - // Merge successful - new_segments.push((start1, end2, merged_line)); - i += 2; - merged = true; - continue; - } - } - } - } - - // Keep original segment - new_segments.push(segments[i]); - i += 1; - } - - segments = new_segments; - } - - segments -} - -#[cfg(test)] -mod tests { - use super::*; - - #[test] - fn test_split_by_gaps() { - let points = vec![ - Point2D::new(0.0, 0.0), - Point2D::new(0.1, 0.0), - Point2D::new(0.2, 0.0), - // Gap - Point2D::new(1.0, 0.0), - Point2D::new(1.1, 0.0), - ]; - - let sequences = split_by_gaps(&points, 0.3); - - assert_eq!(sequences.len(), 2); - assert_eq!(sequences[0].len(), 3); - assert_eq!(sequences[1].len(), 2); - } - - #[test] - fn test_adaptive_split_threshold_basic() { - let sensor_pos = Point2D::new(0.0, 0.0); - - // Close points (1m range) - let close_points = vec![ - Point2D::new(1.0, 0.0), - Point2D::new(1.0, 0.1), - Point2D::new(1.0, 0.2), - ]; - - // Far points (5m range) - let far_points = vec![ - Point2D::new(5.0, 0.0), - Point2D::new(5.0, 0.1), - Point2D::new(5.0, 0.2), - ]; - - let base_threshold = 0.05; - let scale = 0.03; - - let close_threshold = - adaptive_split_threshold(&close_points, base_threshold, sensor_pos, scale); - let far_threshold = - adaptive_split_threshold(&far_points, base_threshold, sensor_pos, scale); - - // Far points should have higher threshold - assert!( - far_threshold > close_threshold, - "Far threshold {} should be > close threshold {}", - far_threshold, - close_threshold - ); - - // Close threshold should be ~0.05 * (1 + 0.03 * 1) = 0.0515 - assert!(close_threshold > base_threshold); - assert!(close_threshold < base_threshold * 1.1); - - // Far threshold should be ~0.05 * (1 + 0.03 * 5) = 0.0575 - assert!(far_threshold > base_threshold * 1.1); - } - - #[test] - fn test_adaptive_split_threshold_empty() { - let sensor_pos = Point2D::new(0.0, 0.0); - let points: Vec = vec![]; - let threshold = adaptive_split_threshold(&points, 0.05, sensor_pos, 0.03); - assert_eq!(threshold, 0.05); // Should return base threshold - } -} diff --git a/vastu-map/src/extraction/split_merge/mod.rs b/vastu-map/src/extraction/split_merge/mod.rs deleted file mode 100644 index 0a9ff76..0000000 --- a/vastu-map/src/extraction/split_merge/mod.rs +++ /dev/null @@ -1,38 +0,0 @@ -//! Split-and-Merge line extraction algorithm. -//! -//! This algorithm extracts line segments from an ordered sequence of points -//! (e.g., from a lidar scan). It works by: -//! -//! 1. **Split Phase**: Recursively split point sequences where the maximum -//! deviation from the fitted line exceeds a threshold. -//! -//! 2. **Merge Phase**: Adjacent segments that can be represented by a single -//! line within tolerance are merged. -//! -//! Requirements: -//! - Points must be ordered (sequential around scan) -//! - Works best with relatively uniform point density -//! -//! # Adaptive Thresholds -//! -//! When sensor position is known, the split threshold can adapt to range: -//! farther points have higher measurement uncertainty, so a larger threshold -//! is used. This prevents over-splitting distant segments while maintaining -//! accuracy for close-range features. -//! -//! ```rust,ignore -//! use vastu_map::extraction::{SplitMergeConfig, extract_lines_from_sensor}; -//! use vastu_map::core::Point2D; -//! -//! let sensor_pos = Point2D::new(0.0, 0.0); -//! let lines = extract_lines_from_sensor(&points, &config, sensor_pos); -//! ``` - -mod algorithm; -mod config; -mod helpers; - -// Re-export public API -pub use algorithm::{extract_lines, extract_lines_from_sensor}; -pub use config::SplitMergeConfig; -pub use helpers::adaptive_split_threshold; diff --git a/vastu-map/src/extraction/traits.rs b/vastu-map/src/extraction/traits.rs deleted file mode 100644 index 6cd4db0..0000000 --- a/vastu-map/src/extraction/traits.rs +++ /dev/null @@ -1,122 +0,0 @@ -//! Traits for line extraction algorithms. -//! -//! This module defines the `LineExtractor` trait which allows different -//! line extraction algorithms to be used interchangeably. - -use crate::core::Point2D; -use crate::features::Line2D; - -/// Trait for line extraction algorithms. -/// -/// Implementations extract line segments from ordered point sequences -/// (typically from a lidar scan). -/// -/// # Example -/// -/// ```rust,ignore -/// use vastu_map::extraction::{LineExtractor, SplitMergeExtractor, SplitMergeConfig}; -/// use vastu_map::core::Point2D; -/// -/// let extractor = SplitMergeExtractor::new(SplitMergeConfig::default()); -/// let points = vec![ -/// Point2D::new(0.0, 1.0), -/// Point2D::new(0.5, 1.0), -/// Point2D::new(1.0, 1.0), -/// ]; -/// let lines = extractor.extract(&points); -/// ``` -pub trait LineExtractor: Send + Sync { - /// Extract line segments from an ordered sequence of points. - /// - /// Points should be ordered (e.g., by angle in a lidar scan). - fn extract(&self, points: &[Point2D]) -> Vec; - - /// Extract line segments with sensor position for adaptive processing. - /// - /// Uses sensor position for: - /// - Adaptive thresholds (farther points get more lenient thresholds) - /// - Weighted line fitting (closer points have more influence) - /// - /// Default implementation ignores sensor position and calls `extract()`. - fn extract_from_sensor(&self, points: &[Point2D], _sensor_pos: Point2D) -> Vec { - self.extract(points) - } -} - -/// Split-and-Merge line extractor. -/// -/// Wraps the split-and-merge algorithm in a `LineExtractor` trait implementation. -#[derive(Clone, Debug)] -pub struct SplitMergeExtractor { - config: super::SplitMergeConfig, -} - -impl SplitMergeExtractor { - /// Create a new extractor with the given configuration. - pub fn new(config: super::SplitMergeConfig) -> Self { - Self { config } - } - - /// Get the configuration. - pub fn config(&self) -> &super::SplitMergeConfig { - &self.config - } -} - -impl Default for SplitMergeExtractor { - fn default() -> Self { - Self::new(super::SplitMergeConfig::default()) - } -} - -impl LineExtractor for SplitMergeExtractor { - fn extract(&self, points: &[Point2D]) -> Vec { - super::extract_lines(points, &self.config) - } - - fn extract_from_sensor(&self, points: &[Point2D], sensor_pos: Point2D) -> Vec { - super::extract_lines_from_sensor(points, &self.config, sensor_pos) - } -} - -#[cfg(test)] -mod tests { - use super::*; - - #[test] - fn test_split_merge_extractor() { - let extractor = SplitMergeExtractor::default(); - - // Generate points along a horizontal line - let points: Vec = (0..20).map(|i| Point2D::new(i as f32 * 0.1, 0.0)).collect(); - - let lines = extractor.extract(&points); - - assert_eq!(lines.len(), 1); - } - - #[test] - fn test_extractor_from_sensor() { - let extractor = SplitMergeExtractor::default(); - let sensor_pos = Point2D::new(0.0, 0.0); - - let points: Vec = (0..20) - .map(|i| Point2D::new(2.0 + i as f32 * 0.1, 0.0)) - .collect(); - - let lines = extractor.extract_from_sensor(&points, sensor_pos); - - assert_eq!(lines.len(), 1); - } - - #[test] - fn test_trait_object() { - // Verify trait can be used as a trait object - let extractor: Box = Box::new(SplitMergeExtractor::default()); - - let points: Vec = (0..20).map(|i| Point2D::new(i as f32 * 0.1, 0.0)).collect(); - - let lines = extractor.extract(&points); - assert_eq!(lines.len(), 1); - } -} diff --git a/vastu-map/src/features/README.md b/vastu-map/src/features/README.md deleted file mode 100644 index 1f34de3..0000000 --- a/vastu-map/src/features/README.md +++ /dev/null @@ -1,108 +0,0 @@ -# Features Module - -Geometric feature types for VectorMap SLAM. - -## Components - -| File | Description | -|------|-------------| -| `line.rs` | `Line2D` - Line segment defined by endpoints | -| `line_collection.rs` | `LineCollection` - SoA collection for SIMD batch operations | -| `corner.rs` | `Corner2D` and `CornerCollection` - Corner features at line intersections | -| `feature_set.rs` | `FeatureSet` - Combined container for lines and corners | -| `descriptors.rs` | `CornerDescriptor` and `ScanDescriptor` - Shape context descriptors for loop closure | - -## Design Rationale - -### Why Lines Instead of Points? - -Indoor environments are dominated by straight edges (walls, furniture, doorframes). Using line segments provides: - -- **Compact representation**: One line replaces dozens of points -- **Robust matching**: Lines survive partial occlusion and noise -- **Meaningful geometry**: Lines encode structure, not just shape -- **Efficient ICP**: Point-to-line ICP converges faster than point-to-point - -### Why Endpoints, Not Parametric Form? - -Lines use endpoint representation `(start, end)` instead of parametric `(rho, theta)`: - -- **Simpler transforms**: Just rotate+translate two points -- **SIMD-friendly**: 4 floats per line, no trig during transform -- **No angle wrapping**: No discontinuity at +/-pi -- **Built-in bounds**: Segment extent is implicit in endpoints - -### Why Corners? - -Corners (line intersections) provide: - -- **Reliable landmarks**: Corners are stable across viewpoints -- **Loop closure signatures**: Corner patterns are distinctive -- **Pose constraints**: Two corners fully constrain 2D pose - -## Key Types - -### Line2D - -```rust -let line = Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(5.0, 0.0)); - -line.length(); // Segment length -line.midpoint(); // Center point -line.direction(); // Unit direction vector -line.normal(); // Perpendicular unit vector -line.distance_to_point(point); // Perpendicular distance -line.project_point(point); // Closest point on line -``` - -### LineCollection (SoA Layout) - -```rust -let mut collection = LineCollection::new(); -collection.push(&line); - -// Batch transform (SIMD-friendly) -let transformed = collection.transform(&pose); - -// Access raw arrays -let start_xs: &[f32] = collection.start_xs(); -``` - -### Corner2D - -```rust -let corner = Corner2D::new( - point, // Corner position - line1_idx, // First line index - line2_idx, // Second line index - angle, // Interior angle -); -``` - -### ScanDescriptor - -Shape context descriptor for loop closure: - -```rust -let descriptor = ScanDescriptor::compute(&lines, &corners, radius); - -// Quick pre-filter with bit signature -let sig_diff = (desc_a.quick_signature() ^ desc_b.quick_signature()).count_ones(); - -// Full descriptor distance -let distance = desc_a.distance(&desc_b); -``` - -## Data Layout - -Feature collections use Struct-of-Arrays for SIMD efficiency: - -``` -AoS (Array of Structs): SoA (Struct of Arrays): -[Line{x1,y1,x2,y2}] start_xs: [x1, x1, x1, ...] -[Line{x1,y1,x2,y2}] → start_ys: [y1, y1, y1, ...] -[Line{x1,y1,x2,y2}] end_xs: [x2, x2, x2, ...] - end_ys: [y2, y2, y2, ...] -``` - -SoA enables processing 4 lines per SIMD instruction. diff --git a/vastu-map/src/features/corner.rs b/vastu-map/src/features/corner.rs deleted file mode 100644 index 782ef67..0000000 --- a/vastu-map/src/features/corner.rs +++ /dev/null @@ -1,341 +0,0 @@ -//! Corner feature type representing line intersections. -//! -//! Corners are detected where two lines meet at a significant angle. - -use crate::core::{Point2D, Pose2D}; - -/// A corner (vertex) feature detected at a line intersection. -/// -/// Corners are stable features useful for localization because they're -/// viewpoint-invariant and easy to detect consistently. -#[derive(Clone, Copy, Debug, PartialEq)] -pub struct Corner2D { - /// Position of the corner in world coordinates. - pub position: Point2D, - /// Index of the first line forming this corner. - pub line1_idx: usize, - /// Index of the second line forming this corner. - pub line2_idx: usize, - /// Angle between the two lines (in radians, 0 to π). - pub angle: f32, - /// Number of times this corner has been observed. - pub observation_count: u32, -} - -impl Corner2D { - /// Create a new corner. - #[inline] - pub fn new(position: Point2D, line1_idx: usize, line2_idx: usize, angle: f32) -> Self { - Self { - position, - line1_idx, - line2_idx, - angle, - observation_count: 1, - } - } - - /// Create a corner with a specific observation count. - #[inline] - pub fn with_observation_count( - position: Point2D, - line1_idx: usize, - line2_idx: usize, - angle: f32, - count: u32, - ) -> Self { - Self { - position, - line1_idx, - line2_idx, - angle, - observation_count: count, - } - } - - /// Transform corner by a pose. - #[inline] - pub fn transform(&self, pose: &Pose2D) -> Self { - Self { - position: pose.transform_point(self.position), - line1_idx: self.line1_idx, - line2_idx: self.line2_idx, - angle: self.angle, - observation_count: self.observation_count, - } - } - - /// Distance to another corner. - #[inline] - pub fn distance(&self, other: &Corner2D) -> f32 { - self.position.distance(other.position) - } - - /// Check if this corner is approximately equal to another. - /// - /// Compares position and angle, ignoring line indices. - #[inline] - pub fn approx_eq(&self, other: &Corner2D, pos_epsilon: f32, angle_epsilon: f32) -> bool { - self.position.approx_eq(other.position, pos_epsilon) - && (self.angle - other.angle).abs() <= angle_epsilon - } -} - -impl Default for Corner2D { - fn default() -> Self { - Self { - position: Point2D::zero(), - line1_idx: 0, - line2_idx: 0, - angle: 0.0, - observation_count: 0, - } - } -} - -/// Collection of corners with efficient storage. -#[derive(Clone, Debug, Default)] -pub struct CornerCollection { - /// Positions of all corners (SoA: x coordinates). - pub xs: Vec, - /// Positions of all corners (SoA: y coordinates). - pub ys: Vec, - /// First line index for each corner. - pub line1_indices: Vec, - /// Second line index for each corner. - pub line2_indices: Vec, - /// Angles at each corner. - pub angles: Vec, - /// Observation counts. - pub observation_counts: Vec, -} - -impl CornerCollection { - /// Create a new empty corner collection. - pub fn new() -> Self { - Self::default() - } - - /// Create with capacity. - pub fn with_capacity(capacity: usize) -> Self { - Self { - xs: Vec::with_capacity(capacity), - ys: Vec::with_capacity(capacity), - line1_indices: Vec::with_capacity(capacity), - line2_indices: Vec::with_capacity(capacity), - angles: Vec::with_capacity(capacity), - observation_counts: Vec::with_capacity(capacity), - } - } - - /// Create from a slice of Corner2D. - pub fn from_corners(corners: &[Corner2D]) -> Self { - let mut collection = Self::with_capacity(corners.len()); - for corner in corners { - collection.push_corner(corner); - } - collection - } - - /// Add a corner to the collection. - #[inline] - pub fn push_corner(&mut self, corner: &Corner2D) { - self.xs.push(corner.position.x); - self.ys.push(corner.position.y); - self.line1_indices.push(corner.line1_idx); - self.line2_indices.push(corner.line2_idx); - self.angles.push(corner.angle); - self.observation_counts.push(corner.observation_count); - } - - /// Number of corners in the collection. - #[inline] - pub fn len(&self) -> usize { - self.xs.len() - } - - /// Check if the collection is empty. - #[inline] - pub fn is_empty(&self) -> bool { - self.xs.is_empty() - } - - /// Get a corner by index. - #[inline] - pub fn get(&self, index: usize) -> Option { - if index < self.len() { - Some(Corner2D::with_observation_count( - Point2D::new(self.xs[index], self.ys[index]), - self.line1_indices[index], - self.line2_indices[index], - self.angles[index], - self.observation_counts[index], - )) - } else { - None - } - } - - /// Convert to `Vec`. - pub fn to_corners(&self) -> Vec { - (0..self.len()) - .map(|i| { - Corner2D::with_observation_count( - Point2D::new(self.xs[i], self.ys[i]), - self.line1_indices[i], - self.line2_indices[i], - self.angles[i], - self.observation_counts[i], - ) - }) - .collect() - } - - /// Find the nearest corner to a point. - /// - /// Returns (index, distance) of the nearest corner, or None if collection is empty. - pub fn nearest_corner(&self, point: Point2D) -> Option<(usize, f32)> { - if self.is_empty() { - return None; - } - - let mut min_idx = 0; - let mut min_dist_sq = f32::MAX; - - for i in 0..self.len() { - let dx = point.x - self.xs[i]; - let dy = point.y - self.ys[i]; - let dist_sq = dx * dx + dy * dy; - - if dist_sq < min_dist_sq { - min_dist_sq = dist_sq; - min_idx = i; - } - } - - Some((min_idx, min_dist_sq.sqrt())) - } - - /// Clear all corners. - pub fn clear(&mut self) { - self.xs.clear(); - self.ys.clear(); - self.line1_indices.clear(); - self.line2_indices.clear(); - self.angles.clear(); - self.observation_counts.clear(); - } - - /// Iterate over corners. - pub fn iter(&self) -> impl Iterator + '_ { - (0..self.len()).map(move |i| { - Corner2D::with_observation_count( - Point2D::new(self.xs[i], self.ys[i]), - self.line1_indices[i], - self.line2_indices[i], - self.angles[i], - self.observation_counts[i], - ) - }) - } -} - -#[cfg(test)] -mod tests { - use super::*; - use approx::assert_relative_eq; - use std::f32::consts::{FRAC_PI_2, FRAC_PI_4}; - - #[test] - fn test_corner_new() { - let corner = Corner2D::new(Point2D::new(1.0, 2.0), 0, 1, FRAC_PI_2); - - assert_eq!(corner.position, Point2D::new(1.0, 2.0)); - assert_eq!(corner.line1_idx, 0); - assert_eq!(corner.line2_idx, 1); - assert_relative_eq!(corner.angle, FRAC_PI_2, epsilon = 1e-6); - assert_eq!(corner.observation_count, 1); - } - - #[test] - fn test_corner_transform() { - let corner = Corner2D::new(Point2D::new(1.0, 0.0), 0, 1, FRAC_PI_2); - let pose = Pose2D::new(0.0, 0.0, FRAC_PI_2); - - let transformed = corner.transform(&pose); - - assert_relative_eq!(transformed.position.x, 0.0, epsilon = 1e-6); - assert_relative_eq!(transformed.position.y, 1.0, epsilon = 1e-6); - // Angle stays the same (corner angle, not orientation) - assert_relative_eq!(transformed.angle, FRAC_PI_2, epsilon = 1e-6); - } - - #[test] - fn test_corner_distance() { - let c1 = Corner2D::new(Point2D::new(0.0, 0.0), 0, 1, FRAC_PI_2); - let c2 = Corner2D::new(Point2D::new(3.0, 4.0), 2, 3, FRAC_PI_2); - - assert_eq!(c1.distance(&c2), 5.0); - } - - #[test] - fn test_corner_collection_from_corners() { - let corners = vec![ - Corner2D::new(Point2D::new(0.0, 0.0), 0, 1, FRAC_PI_2), - Corner2D::new(Point2D::new(1.0, 1.0), 1, 2, FRAC_PI_4), - ]; - - let collection = CornerCollection::from_corners(&corners); - - assert_eq!(collection.len(), 2); - assert_eq!(collection.xs, vec![0.0, 1.0]); - assert_eq!(collection.ys, vec![0.0, 1.0]); - } - - #[test] - fn test_corner_collection_to_corners() { - let corners = vec![ - Corner2D::new(Point2D::new(0.0, 0.0), 0, 1, FRAC_PI_2), - Corner2D::new(Point2D::new(1.0, 1.0), 1, 2, FRAC_PI_4), - ]; - - let collection = CornerCollection::from_corners(&corners); - let recovered = collection.to_corners(); - - assert_eq!(recovered.len(), 2); - assert_eq!(recovered[0].position, corners[0].position); - assert_eq!(recovered[1].position, corners[1].position); - } - - #[test] - fn test_corner_collection_nearest() { - let corners = vec![ - Corner2D::new(Point2D::new(0.0, 0.0), 0, 1, FRAC_PI_2), - Corner2D::new(Point2D::new(10.0, 0.0), 1, 2, FRAC_PI_2), - Corner2D::new(Point2D::new(5.0, 5.0), 2, 3, FRAC_PI_2), - ]; - - let collection = CornerCollection::from_corners(&corners); - let point = Point2D::new(4.0, 4.0); - - let (idx, dist) = collection.nearest_corner(point).unwrap(); - - assert_eq!(idx, 2); // (5, 5) is nearest - assert_relative_eq!(dist, 2.0_f32.sqrt(), epsilon = 1e-6); - } - - #[test] - fn test_corner_collection_iter() { - let corners = vec![ - Corner2D::new(Point2D::new(0.0, 0.0), 0, 1, FRAC_PI_2), - Corner2D::new(Point2D::new(1.0, 1.0), 1, 2, FRAC_PI_4), - ]; - - let collection = CornerCollection::from_corners(&corners); - let collected: Vec<_> = collection.iter().collect(); - - assert_eq!(collected.len(), 2); - assert_eq!(collected[0].position, Point2D::new(0.0, 0.0)); - assert_eq!(collected[1].position, Point2D::new(1.0, 1.0)); - } -} diff --git a/vastu-map/src/features/descriptors.rs b/vastu-map/src/features/descriptors.rs deleted file mode 100644 index fddb481..0000000 --- a/vastu-map/src/features/descriptors.rs +++ /dev/null @@ -1,564 +0,0 @@ -//! Shape context descriptors for loop closure detection. -//! -//! This module provides descriptors for corners and scans that enable -//! matching between different observations of the same place. -//! -//! # Geometric Concept: Shape Context Descriptors -//! -//! Shape context is a rich descriptor for 2D shapes that captures the spatial -//! distribution of features relative to a reference point. This implementation -//! adapts the technique for line-based SLAM. -//! -//! ## How It Works -//! -//! Given a corner point, we examine all lines within a radius and build histograms: -//! -//! ```text -//! Angle Bins (8 bins, 45° each) -//! -//! bin 1 -//! │ -//! bin 0 │ bin 2 -//! ╲ │ ╱ -//! ╲ │ ╱ -//! bin 7 ─── ● ─── bin 3 ← Corner position -//! ╱ │ ╲ -//! ╱ │ ╲ -//! bin 6 │ bin 4 -//! │ -//! bin 5 -//! -//! Distance Bins (4 bins) -//! ┌─────┬─────┬─────┬─────────────┐ -//! │0-0.5│0.5-1│1-2m │ 2+m │ -//! └─────┴─────┴─────┴─────────────┘ -//! ``` -//! -//! ## Matching -//! -//! Two corners are similar if their histograms are similar, regardless of -//! the corner's absolute position or orientation. This enables recognizing -//! the same place from different viewpoints. -//! -//! ## Distance Metric -//! -//! Chi-squared distance compares histograms: χ² = Σ (a_i - b_i)² / (a_i + b_i) -//! - 0.0 = identical descriptors -//! - Higher values = more different -//! -//! # Corner Descriptors -//! -//! Each corner is described by histograms of nearby line properties: -//! - Angle histogram: orientation of nearby lines (8 bins, 45° each) -//! - Distance histogram: distances to nearby lines (4 bins) -//! - Length histogram: lengths of nearby lines (4 bins) -//! -//! # Scan Descriptors -//! -//! Aggregate descriptor for an entire scan, combining corner descriptors -//! with global statistics (line count, corner count, total length). - -use crate::features::{Corner2D, Line2D}; -use std::f32::consts::PI; - -/// Number of bins in the angle histogram. -pub const ANGLE_BINS: usize = 8; -/// Number of bins in the distance histogram. -pub const DISTANCE_BINS: usize = 4; -/// Number of bins in the length histogram. -pub const LENGTH_BINS: usize = 4; - -/// Distance bin boundaries in meters. -const DISTANCE_BIN_EDGES: [f32; DISTANCE_BINS] = [0.5, 1.0, 2.0, f32::INFINITY]; -/// Length bin boundaries in meters. -const LENGTH_BIN_EDGES: [f32; LENGTH_BINS] = [0.3, 0.6, 1.0, f32::INFINITY]; - -/// Shape context descriptor for a corner feature. -/// -/// Encodes the local structure around a corner by histogramming -/// properties of nearby lines. This is invariant to rotation -/// when using relative angles. -#[derive(Clone, Debug)] -pub struct CornerDescriptor { - /// 8-bin histogram: angles of nearby lines relative to corner bisector. - /// Each bin covers 45° (π/4 radians). - pub angle_histogram: [f32; ANGLE_BINS], - - /// 4-bin histogram: distances to nearby lines. - /// Bins: [0-0.5m, 0.5-1m, 1-2m, 2+m] - pub distance_histogram: [f32; DISTANCE_BINS], - - /// 4-bin histogram: lengths of nearby lines. - /// Bins: [0-0.3m, 0.3-0.6m, 0.6-1m, 1+m] - pub length_histogram: [f32; LENGTH_BINS], - - /// Number of lines used to compute this descriptor. - pub num_lines: usize, -} - -impl Default for CornerDescriptor { - fn default() -> Self { - Self { - angle_histogram: [0.0; ANGLE_BINS], - distance_histogram: [0.0; DISTANCE_BINS], - length_histogram: [0.0; LENGTH_BINS], - num_lines: 0, - } - } -} - -impl CornerDescriptor { - /// Compute descriptor for a corner from nearby lines. - /// - /// # Arguments - /// * `corner` - The corner to describe - /// * `lines` - All lines in the scan/map - /// * `radius` - Maximum distance to consider lines (default: 3.0m) - /// - /// # Example - /// ``` - /// use vastu_map::features::{Corner2D, Line2D, CornerDescriptor}; - /// use vastu_map::core::Point2D; - /// use std::f32::consts::FRAC_PI_2; - /// - /// let corner = Corner2D::new(Point2D::new(0.0, 0.0), 0, 1, FRAC_PI_2); - /// let lines = vec![ - /// Line2D::new(Point2D::new(-1.0, 0.0), Point2D::new(0.0, 0.0)), - /// Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(0.0, 1.0)), - /// ]; - /// - /// let descriptor = CornerDescriptor::compute(&corner, &lines, 3.0); - /// assert!(descriptor.num_lines >= 2); - /// ``` - pub fn compute(corner: &Corner2D, lines: &[Line2D], radius: f32) -> Self { - let mut desc = Self::default(); - let radius_sq = radius * radius; - - for line in lines { - // Check if line is within radius of corner - let dist = line.distance_to_point(corner.position); - if dist > radius { - continue; - } - - // Check if either endpoint is within radius - let dist_start = corner.position.distance(line.start); - let dist_end = corner.position.distance(line.end); - let min_endpoint_dist = dist_start.min(dist_end); - - if min_endpoint_dist * min_endpoint_dist > radius_sq * 4.0 { - continue; - } - - desc.num_lines += 1; - - // Add to angle histogram - let line_angle = line.angle(); - // Normalize to [0, 2π) then bin into 8 segments - let normalized_angle = if line_angle < 0.0 { - line_angle + 2.0 * PI - } else { - line_angle - }; - let angle_bin = ((normalized_angle / (2.0 * PI)) * ANGLE_BINS as f32) as usize; - let angle_bin = angle_bin.min(ANGLE_BINS - 1); - desc.angle_histogram[angle_bin] += 1.0; - - // Add to distance histogram - let dist_bin = Self::find_bin(dist, &DISTANCE_BIN_EDGES); - desc.distance_histogram[dist_bin] += 1.0; - - // Add to length histogram - let length = line.length(); - let length_bin = Self::find_bin(length, &LENGTH_BIN_EDGES); - desc.length_histogram[length_bin] += 1.0; - } - - // Normalize histograms - desc.normalize(); - - desc - } - - /// Find the bin index for a value given bin edges. - #[inline] - fn find_bin(value: f32, edges: &[f32]) -> usize { - for (i, &edge) in edges.iter().enumerate() { - if value < edge { - return i; - } - } - edges.len() - 1 - } - - /// Normalize all histograms to sum to 1. - fn normalize(&mut self) { - Self::normalize_histogram(&mut self.angle_histogram); - Self::normalize_histogram(&mut self.distance_histogram); - Self::normalize_histogram(&mut self.length_histogram); - } - - /// Normalize a single histogram. - fn normalize_histogram(hist: &mut [f32]) { - let sum: f32 = hist.iter().sum(); - if sum > f32::EPSILON { - for val in hist.iter_mut() { - *val /= sum; - } - } - } - - /// Compute chi-squared distance between two descriptors. - /// - /// Lower values indicate more similar descriptors. - /// Returns 0.0 for identical descriptors, up to ~2.0 for very different. - /// - /// # Formula - /// χ² = Σ (a_i - b_i)² / (a_i + b_i + ε) - pub fn distance(&self, other: &Self) -> f32 { - let epsilon = 1e-10; - - let mut total = 0.0; - - // Compare angle histograms - for i in 0..ANGLE_BINS { - let a = self.angle_histogram[i]; - let b = other.angle_histogram[i]; - let denom = a + b + epsilon; - total += (a - b).powi(2) / denom; - } - - // Compare distance histograms - for i in 0..DISTANCE_BINS { - let a = self.distance_histogram[i]; - let b = other.distance_histogram[i]; - let denom = a + b + epsilon; - total += (a - b).powi(2) / denom; - } - - // Compare length histograms - for i in 0..LENGTH_BINS { - let a = self.length_histogram[i]; - let b = other.length_histogram[i]; - let denom = a + b + epsilon; - total += (a - b).powi(2) / denom; - } - - total / 2.0 // Normalize to [0, 1] range for identical mass - } - - /// Check if this descriptor is empty (no lines contributed). - #[inline] - pub fn is_empty(&self) -> bool { - self.num_lines == 0 - } -} - -/// Aggregate descriptor for an entire scan. -/// -/// Combines individual corner descriptors with global statistics -/// about the scan's line and corner content. -#[derive(Clone, Debug)] -pub struct ScanDescriptor { - /// Descriptors for each corner in the scan. - pub corner_descriptors: Vec, - - /// Number of lines in the scan. - pub num_lines: usize, - - /// Number of corners in the scan. - pub num_corners: usize, - - /// Total length of all lines. - pub total_line_length: f32, - - /// Average corner angle (for quick filtering). - pub avg_corner_angle: f32, -} - -impl Default for ScanDescriptor { - fn default() -> Self { - Self { - corner_descriptors: Vec::new(), - num_lines: 0, - num_corners: 0, - total_line_length: 0.0, - avg_corner_angle: 0.0, - } - } -} - -impl ScanDescriptor { - /// Compute scan descriptor from lines and corners. - /// - /// # Arguments - /// * `lines` - Extracted line segments - /// * `corners` - Detected corners - /// * `descriptor_radius` - Radius for computing corner descriptors (default: 3.0m) - /// - /// # Example - /// ``` - /// use vastu_map::features::{Corner2D, Line2D, ScanDescriptor}; - /// use vastu_map::core::Point2D; - /// use std::f32::consts::FRAC_PI_2; - /// - /// let lines = vec![ - /// Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(1.0, 0.0)), - /// Line2D::new(Point2D::new(1.0, 0.0), Point2D::new(1.0, 1.0)), - /// ]; - /// let corners = vec![ - /// Corner2D::new(Point2D::new(1.0, 0.0), 0, 1, FRAC_PI_2), - /// ]; - /// - /// let descriptor = ScanDescriptor::compute(&lines, &corners, 3.0); - /// assert_eq!(descriptor.num_lines, 2); - /// assert_eq!(descriptor.num_corners, 1); - /// ``` - pub fn compute(lines: &[Line2D], corners: &[Corner2D], descriptor_radius: f32) -> Self { - // Compute average corner angle - let avg_corner_angle = if corners.is_empty() { - 0.0 - } else { - corners.iter().map(|c| c.angle).sum::() / corners.len() as f32 - }; - - Self { - num_lines: lines.len(), - num_corners: corners.len(), - total_line_length: lines.iter().map(|l| l.length()).sum(), - corner_descriptors: corners - .iter() - .map(|c| CornerDescriptor::compute(c, lines, descriptor_radius)) - .collect(), - avg_corner_angle, - } - } - - /// Compute distance between two scan descriptors. - /// - /// Uses a combination of: - /// 1. Global statistics comparison - /// 2. Best-match corner descriptor distances - /// - /// Returns a value in [0, 1] where lower is more similar. - pub fn distance(&self, other: &Self) -> f32 { - // Quick rejection based on global stats - let line_diff = (self.num_lines as f32 - other.num_lines as f32).abs(); - let corner_diff = (self.num_corners as f32 - other.num_corners as f32).abs(); - - // If counts differ significantly, scans are different - let max_lines = self.num_lines.max(other.num_lines) as f32; - let max_corners = self.num_corners.max(other.num_corners) as f32; - - if max_lines > 0.0 && line_diff / max_lines > 0.5 { - return 1.0; - } - if max_corners > 0.0 && corner_diff / max_corners > 0.5 { - return 1.0; - } - - // Match corner descriptors (greedy best-match) - if self.corner_descriptors.is_empty() || other.corner_descriptors.is_empty() { - // No corners to compare - use global stats only - let length_diff = (self.total_line_length - other.total_line_length).abs(); - let max_length = self.total_line_length.max(other.total_line_length); - if max_length > f32::EPSILON { - return (length_diff / max_length).min(1.0); - } - return 0.5; // Indeterminate - } - - // Compute average best-match distance - let mut total_dist = 0.0; - let mut matches = 0; - - for desc1 in &self.corner_descriptors { - if desc1.is_empty() { - continue; - } - - let mut best_dist = f32::MAX; - for desc2 in &other.corner_descriptors { - if desc2.is_empty() { - continue; - } - let dist = desc1.distance(desc2); - best_dist = best_dist.min(dist); - } - - if best_dist < f32::MAX { - total_dist += best_dist; - matches += 1; - } - } - - if matches > 0 { - total_dist / matches as f32 - } else { - 1.0 - } - } - - /// Check if this descriptor has enough content for matching. - #[inline] - pub fn is_valid_for_matching(&self) -> bool { - self.num_corners >= 2 && self.num_lines >= 3 - } - - /// Compute a quick hash for pre-filtering (based on feature counts). - pub fn quick_signature(&self) -> u32 { - // Encode line count (4 bits), corner count (4 bits), and length bucket (8 bits) - let line_bucket = (self.num_lines.min(15)) as u32; - let corner_bucket = (self.num_corners.min(15)) as u32; - let length_bucket = ((self.total_line_length / 0.5).min(255.0)) as u32; - - (line_bucket << 12) | (corner_bucket << 8) | length_bucket - } -} - -#[cfg(test)] -mod tests { - use super::*; - use crate::core::Point2D; - use approx::assert_relative_eq; - use std::f32::consts::FRAC_PI_2; - - #[test] - fn test_corner_descriptor_empty() { - let corner = Corner2D::new(Point2D::new(0.0, 0.0), 0, 1, FRAC_PI_2); - let lines: Vec = vec![]; - - let desc = CornerDescriptor::compute(&corner, &lines, 3.0); - - assert!(desc.is_empty()); - assert_eq!(desc.num_lines, 0); - } - - #[test] - fn test_corner_descriptor_with_lines() { - let corner = Corner2D::new(Point2D::new(0.0, 0.0), 0, 1, FRAC_PI_2); - let lines = vec![ - Line2D::new(Point2D::new(-1.0, 0.0), Point2D::new(0.0, 0.0)), - Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(0.0, 1.0)), - ]; - - let desc = CornerDescriptor::compute(&corner, &lines, 3.0); - - assert!(!desc.is_empty()); - assert_eq!(desc.num_lines, 2); - - // Histograms should be normalized - let angle_sum: f32 = desc.angle_histogram.iter().sum(); - assert_relative_eq!(angle_sum, 1.0, epsilon = 0.01); - } - - #[test] - fn test_corner_descriptor_distance_identical() { - let corner = Corner2D::new(Point2D::new(0.0, 0.0), 0, 1, FRAC_PI_2); - let lines = vec![ - Line2D::new(Point2D::new(-1.0, 0.0), Point2D::new(0.0, 0.0)), - Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(0.0, 1.0)), - ]; - - let desc = CornerDescriptor::compute(&corner, &lines, 3.0); - - // Distance to itself should be 0 - let dist = desc.distance(&desc); - assert_relative_eq!(dist, 0.0, epsilon = 0.001); - } - - #[test] - fn test_corner_descriptor_distance_different() { - let corner1 = Corner2D::new(Point2D::new(0.0, 0.0), 0, 1, FRAC_PI_2); - let lines1 = vec![ - Line2D::new(Point2D::new(-1.0, 0.0), Point2D::new(0.0, 0.0)), - Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(0.0, 1.0)), - ]; - - let corner2 = Corner2D::new(Point2D::new(5.0, 5.0), 0, 1, FRAC_PI_2); - let lines2 = vec![ - Line2D::new(Point2D::new(5.0, 5.0), Point2D::new(10.0, 5.0)), - Line2D::new(Point2D::new(5.0, 5.0), Point2D::new(5.0, 10.0)), - Line2D::new(Point2D::new(5.0, 5.0), Point2D::new(7.0, 7.0)), - ]; - - let desc1 = CornerDescriptor::compute(&corner1, &lines1, 3.0); - let desc2 = CornerDescriptor::compute(&corner2, &lines2, 3.0); - - // Distance should be positive - let dist = desc1.distance(&desc2); - assert!(dist > 0.0); - } - - #[test] - fn test_scan_descriptor_empty() { - let desc = ScanDescriptor::compute(&[], &[], 3.0); - - assert_eq!(desc.num_lines, 0); - assert_eq!(desc.num_corners, 0); - assert!(!desc.is_valid_for_matching()); - } - - #[test] - fn test_scan_descriptor_with_features() { - let lines = vec![ - Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(1.0, 0.0)), - Line2D::new(Point2D::new(1.0, 0.0), Point2D::new(1.0, 1.0)), - Line2D::new(Point2D::new(1.0, 1.0), Point2D::new(0.0, 1.0)), - ]; - let corners = vec![ - Corner2D::new(Point2D::new(1.0, 0.0), 0, 1, FRAC_PI_2), - Corner2D::new(Point2D::new(1.0, 1.0), 1, 2, FRAC_PI_2), - ]; - - let desc = ScanDescriptor::compute(&lines, &corners, 3.0); - - assert_eq!(desc.num_lines, 3); - assert_eq!(desc.num_corners, 2); - assert_eq!(desc.corner_descriptors.len(), 2); - assert!(desc.is_valid_for_matching()); - assert!(desc.total_line_length > 2.0); - } - - #[test] - fn test_scan_descriptor_distance_identical() { - let lines = vec![ - Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(1.0, 0.0)), - Line2D::new(Point2D::new(1.0, 0.0), Point2D::new(1.0, 1.0)), - Line2D::new(Point2D::new(1.0, 1.0), Point2D::new(0.0, 1.0)), - ]; - let corners = vec![ - Corner2D::new(Point2D::new(1.0, 0.0), 0, 1, FRAC_PI_2), - Corner2D::new(Point2D::new(1.0, 1.0), 1, 2, FRAC_PI_2), - ]; - - let desc = ScanDescriptor::compute(&lines, &corners, 3.0); - - // Distance to itself should be very small - let dist = desc.distance(&desc); - assert!(dist < 0.1, "Self-distance should be near 0, got {}", dist); - } - - #[test] - fn test_scan_descriptor_quick_signature() { - let lines = vec![ - Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(1.0, 0.0)), - Line2D::new(Point2D::new(1.0, 0.0), Point2D::new(1.0, 1.0)), - ]; - let corners = vec![Corner2D::new(Point2D::new(1.0, 0.0), 0, 1, FRAC_PI_2)]; - - let desc = ScanDescriptor::compute(&lines, &corners, 3.0); - let sig = desc.quick_signature(); - - // Signature should be non-zero - assert!(sig > 0); - } - - #[test] - fn test_find_bin() { - assert_eq!(CornerDescriptor::find_bin(0.3, &DISTANCE_BIN_EDGES), 0); - assert_eq!(CornerDescriptor::find_bin(0.7, &DISTANCE_BIN_EDGES), 1); - assert_eq!(CornerDescriptor::find_bin(1.5, &DISTANCE_BIN_EDGES), 2); - assert_eq!(CornerDescriptor::find_bin(5.0, &DISTANCE_BIN_EDGES), 3); - } -} diff --git a/vastu-map/src/features/feature_set.rs b/vastu-map/src/features/feature_set.rs deleted file mode 100644 index bd016be..0000000 --- a/vastu-map/src/features/feature_set.rs +++ /dev/null @@ -1,354 +0,0 @@ -//! Feature set container holding lines and corners. -//! -//! A FeatureSet represents all features extracted from a single scan -//! or the accumulated features in a map. - -use super::corner::{Corner2D, CornerCollection}; -use super::line::Line2D; -use super::line_collection::LineCollection; -use crate::core::{Bounds, Point2D, Pose2D}; - -/// A set of geometric features (lines and corners). -/// -/// Used to represent: -/// - Features extracted from a single lidar scan -/// - Accumulated features in a map -/// -/// Provides both AoS (Array of Structs) and SoA (Struct of Arrays) access -/// for flexibility between ease of use and SIMD performance. -#[derive(Clone, Debug, Default)] -pub struct FeatureSet { - /// Lines stored in SoA layout for SIMD operations. - lines: LineCollection, - /// Corners stored in SoA layout. - corners: CornerCollection, -} - -impl FeatureSet { - /// Create a new empty feature set. - pub fn new() -> Self { - Self::default() - } - - /// Create a feature set with capacity hints. - pub fn with_capacity(line_capacity: usize, corner_capacity: usize) -> Self { - Self { - lines: LineCollection::with_capacity(line_capacity), - corners: CornerCollection::with_capacity(corner_capacity), - } - } - - /// Create from lines and corners. - pub fn from_features(lines: &[Line2D], corners: &[Corner2D]) -> Self { - Self { - lines: LineCollection::from_lines(lines), - corners: CornerCollection::from_corners(corners), - } - } - - // ───────────────────────────────────────────────────────────────────────── - // Line operations - // ───────────────────────────────────────────────────────────────────────── - - /// Add a line to the feature set. - #[inline] - pub fn add_line(&mut self, line: Line2D) { - self.lines.push_line(&line); - } - - /// Number of lines. - #[inline] - pub fn line_count(&self) -> usize { - self.lines.len() - } - - /// Get a line by index. - #[inline] - pub fn get_line(&self, index: usize) -> Option { - self.lines.get(index) - } - - /// Get all lines as a vector. - pub fn lines(&self) -> Vec { - self.lines.to_lines() - } - - /// Get reference to the line collection (SoA layout). - #[inline] - pub fn line_collection(&self) -> &LineCollection { - &self.lines - } - - /// Get mutable reference to the line collection. - #[inline] - pub fn line_collection_mut(&mut self) -> &mut LineCollection { - &mut self.lines - } - - /// Iterate over lines. - pub fn iter_lines(&self) -> impl Iterator + '_ { - self.lines.iter() - } - - // ───────────────────────────────────────────────────────────────────────── - // Corner operations - // ───────────────────────────────────────────────────────────────────────── - - /// Add a corner to the feature set. - #[inline] - pub fn add_corner(&mut self, corner: Corner2D) { - self.corners.push_corner(&corner); - } - - /// Number of corners. - #[inline] - pub fn corner_count(&self) -> usize { - self.corners.len() - } - - /// Get a corner by index. - #[inline] - pub fn get_corner(&self, index: usize) -> Option { - self.corners.get(index) - } - - /// Get all corners as a vector. - pub fn corners(&self) -> Vec { - self.corners.to_corners() - } - - /// Get reference to the corner collection. - #[inline] - pub fn corner_collection(&self) -> &CornerCollection { - &self.corners - } - - /// Get mutable reference to the corner collection. - #[inline] - pub fn corner_collection_mut(&mut self) -> &mut CornerCollection { - &mut self.corners - } - - /// Iterate over corners. - pub fn iter_corners(&self) -> impl Iterator + '_ { - self.corners.iter() - } - - // ───────────────────────────────────────────────────────────────────────── - // Combined operations - // ───────────────────────────────────────────────────────────────────────── - - /// Total number of features (lines + corners). - #[inline] - pub fn total_count(&self) -> usize { - self.line_count() + self.corner_count() - } - - /// Check if the feature set is empty. - #[inline] - pub fn is_empty(&self) -> bool { - self.lines.is_empty() && self.corners.is_empty() - } - - /// Transform all features by a pose. - pub fn transform(&self, pose: &Pose2D) -> Self { - Self { - lines: self.lines.transform(pose), - corners: { - let mut transformed = CornerCollection::with_capacity(self.corners.len()); - for corner in self.corners.iter() { - transformed.push_corner(&corner.transform(pose)); - } - transformed - }, - } - } - - /// Compute bounding box of all features. - pub fn bounds(&self) -> Bounds { - let mut bounds = Bounds::empty(); - - // Include all line endpoints - for i in 0..self.lines.len() { - bounds.expand_to_include(Point2D::new(self.lines.start_xs[i], self.lines.start_ys[i])); - bounds.expand_to_include(Point2D::new(self.lines.end_xs[i], self.lines.end_ys[i])); - } - - // Include all corner positions - for i in 0..self.corners.len() { - bounds.expand_to_include(Point2D::new(self.corners.xs[i], self.corners.ys[i])); - } - - bounds - } - - /// Clear all features. - pub fn clear(&mut self) { - self.lines.clear(); - self.corners.clear(); - } - - /// Find the nearest line to a point. - /// - /// Returns (index, distance) or None if no lines. - #[inline] - pub fn nearest_line(&self, point: Point2D) -> Option<(usize, f32)> { - self.lines.nearest_line(point) - } - - /// Merge another feature set into this one. - /// - /// Note: This is a simple append. For intelligent merging with - /// association and deduplication, use the integration module. - pub fn append(&mut self, other: &FeatureSet) { - for line in other.lines.iter() { - self.add_line(line); - } - for corner in other.corners.iter() { - self.add_corner(corner); - } - } -} - -#[cfg(test)] -mod tests { - use super::*; - use approx::assert_relative_eq; - use std::f32::consts::FRAC_PI_2; - - #[test] - fn test_new_empty() { - let fs = FeatureSet::new(); - assert!(fs.is_empty()); - assert_eq!(fs.line_count(), 0); - assert_eq!(fs.corner_count(), 0); - assert_eq!(fs.total_count(), 0); - } - - #[test] - fn test_add_line() { - let mut fs = FeatureSet::new(); - fs.add_line(Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(1.0, 0.0))); - fs.add_line(Line2D::new(Point2D::new(0.0, 1.0), Point2D::new(1.0, 1.0))); - - assert_eq!(fs.line_count(), 2); - assert!(!fs.is_empty()); - - let line = fs.get_line(0).unwrap(); - assert_eq!(line.start, Point2D::new(0.0, 0.0)); - } - - #[test] - fn test_add_corner() { - let mut fs = FeatureSet::new(); - fs.add_corner(Corner2D::new(Point2D::new(5.0, 5.0), 0, 1, FRAC_PI_2)); - - assert_eq!(fs.corner_count(), 1); - - let corner = fs.get_corner(0).unwrap(); - assert_eq!(corner.position, Point2D::new(5.0, 5.0)); - } - - #[test] - fn test_from_features() { - let lines = vec![ - Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(1.0, 0.0)), - Line2D::new(Point2D::new(0.0, 1.0), Point2D::new(1.0, 1.0)), - ]; - let corners = vec![Corner2D::new(Point2D::new(0.0, 0.0), 0, 1, FRAC_PI_2)]; - - let fs = FeatureSet::from_features(&lines, &corners); - - assert_eq!(fs.line_count(), 2); - assert_eq!(fs.corner_count(), 1); - assert_eq!(fs.total_count(), 3); - } - - #[test] - fn test_transform() { - let mut fs = FeatureSet::new(); - fs.add_line(Line2D::new(Point2D::new(1.0, 0.0), Point2D::new(2.0, 0.0))); - fs.add_corner(Corner2D::new(Point2D::new(1.0, 0.0), 0, 0, FRAC_PI_2)); - - let pose = Pose2D::new(0.0, 0.0, FRAC_PI_2); - let transformed = fs.transform(&pose); - - let line = transformed.get_line(0).unwrap(); - assert_relative_eq!(line.start.x, 0.0, epsilon = 1e-6); - assert_relative_eq!(line.start.y, 1.0, epsilon = 1e-6); - - let corner = transformed.get_corner(0).unwrap(); - assert_relative_eq!(corner.position.x, 0.0, epsilon = 1e-6); - assert_relative_eq!(corner.position.y, 1.0, epsilon = 1e-6); - } - - #[test] - fn test_bounds() { - let mut fs = FeatureSet::new(); - fs.add_line(Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(10.0, 0.0))); - fs.add_line(Line2D::new(Point2D::new(5.0, -5.0), Point2D::new(5.0, 5.0))); - - let bounds = fs.bounds(); - - assert_eq!(bounds.min, Point2D::new(0.0, -5.0)); - assert_eq!(bounds.max, Point2D::new(10.0, 5.0)); - } - - #[test] - fn test_bounds_empty() { - let fs = FeatureSet::new(); - let bounds = fs.bounds(); - assert!(bounds.is_empty()); - } - - #[test] - fn test_nearest_line() { - let mut fs = FeatureSet::new(); - fs.add_line(Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(10.0, 0.0))); - fs.add_line(Line2D::new(Point2D::new(0.0, 5.0), Point2D::new(10.0, 5.0))); - - let point = Point2D::new(5.0, 2.0); - let (idx, dist) = fs.nearest_line(point).unwrap(); - - assert_eq!(idx, 0); - assert_relative_eq!(dist, 2.0, epsilon = 1e-6); - } - - #[test] - fn test_iter_lines() { - let mut fs = FeatureSet::new(); - fs.add_line(Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(1.0, 0.0))); - fs.add_line(Line2D::new(Point2D::new(0.0, 1.0), Point2D::new(1.0, 1.0))); - - let lines: Vec<_> = fs.iter_lines().collect(); - assert_eq!(lines.len(), 2); - } - - #[test] - fn test_append() { - let mut fs1 = FeatureSet::new(); - fs1.add_line(Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(1.0, 0.0))); - - let mut fs2 = FeatureSet::new(); - fs2.add_line(Line2D::new(Point2D::new(0.0, 1.0), Point2D::new(1.0, 1.0))); - fs2.add_corner(Corner2D::new(Point2D::new(0.0, 0.0), 0, 0, FRAC_PI_2)); - - fs1.append(&fs2); - - assert_eq!(fs1.line_count(), 2); - assert_eq!(fs1.corner_count(), 1); - } - - #[test] - fn test_clear() { - let mut fs = FeatureSet::new(); - fs.add_line(Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(1.0, 0.0))); - fs.add_corner(Corner2D::new(Point2D::new(0.0, 0.0), 0, 0, FRAC_PI_2)); - - fs.clear(); - - assert!(fs.is_empty()); - assert_eq!(fs.line_count(), 0); - assert_eq!(fs.corner_count(), 0); - } -} diff --git a/vastu-map/src/features/line.rs b/vastu-map/src/features/line.rs deleted file mode 100644 index 9e0a497..0000000 --- a/vastu-map/src/features/line.rs +++ /dev/null @@ -1,586 +0,0 @@ -//! Line segment type using endpoint representation. -//! -//! Lines are represented by their start and end points only (no parametric form). -//! This design choice simplifies transforms and enables better SIMD compatibility. - -use crate::core::{Point2D, Pose2D}; - -/// A 2D line segment defined by its endpoints. -/// -/// # Design Decision: Endpoint-Only Representation -/// -/// We use endpoints instead of parametric (ρ,θ) form because: -/// - **Simpler transforms**: Just rotate+translate two points -/// - **Better SIMD layout**: 4 homogeneous floats per line -/// - **No angle wrapping**: No discontinuity at ±π -/// - **Built-in bounds**: Segment extent is implicit -/// -/// # Point-to-Line Distance -/// -/// Uses cross product formula (no trig functions): -/// ```text -/// distance = |cross(p - start, direction)| / |direction| -/// ``` -#[derive(Clone, Copy, Debug, PartialEq)] -pub struct Line2D { - /// Start point of the line segment. - pub start: Point2D, - /// End point of the line segment. - pub end: Point2D, - /// Number of times this line has been observed (for merging). - pub observation_count: u32, - /// Number of scan points that formed this line (for dominance). - pub point_count: u32, -} - -impl Line2D { - /// Create a new line segment from two points. - #[inline] - pub fn new(start: Point2D, end: Point2D) -> Self { - Self { - start, - end, - observation_count: 1, - point_count: 0, - } - } - - /// Create a line segment with a specific observation count. - #[inline] - pub fn with_observation_count(start: Point2D, end: Point2D, count: u32) -> Self { - Self { - start, - end, - observation_count: count, - point_count: 0, - } - } - - /// Create a line segment with a specific point count. - /// - /// Used when extracting lines from scan points where point_count - /// indicates how many scan points formed this line. - #[inline] - pub fn with_point_count(start: Point2D, end: Point2D, point_count: u32) -> Self { - Self { - start, - end, - observation_count: 1, - point_count, - } - } - - /// Create a line segment with all fields specified. - #[inline] - pub fn full(start: Point2D, end: Point2D, observation_count: u32, point_count: u32) -> Self { - Self { - start, - end, - observation_count, - point_count, - } - } - - /// Direction vector from start to end (not normalized). - #[inline] - pub fn direction(&self) -> Point2D { - self.end - self.start - } - - /// Unit direction vector from start to end. - #[inline] - pub fn unit_direction(&self) -> Point2D { - self.direction().normalized() - } - - /// Normal vector (perpendicular to direction, pointing left). - /// - /// If the line goes from left to right, the normal points up. - #[inline] - pub fn normal(&self) -> Point2D { - self.unit_direction().perpendicular() - } - - /// Length of the line segment. - #[inline] - pub fn length(&self) -> f32 { - self.direction().length() - } - - /// Squared length of the line segment (avoids sqrt). - #[inline] - pub fn length_squared(&self) -> f32 { - self.direction().length_squared() - } - - /// Midpoint of the line segment. - #[inline] - pub fn midpoint(&self) -> Point2D { - Point2D::new( - (self.start.x + self.end.x) * 0.5, - (self.start.y + self.end.y) * 0.5, - ) - } - - /// Get a point along the line at parameter t. - /// - /// - `t = 0`: returns start point - /// - `t = 1`: returns end point - /// - `t = 0.5`: returns midpoint - #[inline] - pub fn point_at(&self, t: f32) -> Point2D { - Point2D::new( - self.start.x + t * (self.end.x - self.start.x), - self.start.y + t * (self.end.y - self.start.y), - ) - } - - /// Angle of the line direction from X-axis (in radians, [-π, π]). - #[inline] - pub fn angle(&self) -> f32 { - self.direction().angle() - } - - /// Perpendicular distance from a point to the infinite line. - /// - /// Uses the cross product formula: - /// ```text - /// distance = |cross(p - start, end - start)| / |end - start| - /// ``` - /// - /// This is always non-negative. - #[inline] - pub fn distance_to_point(&self, point: Point2D) -> f32 { - let dir = self.direction(); - let len_sq = dir.length_squared(); - - if len_sq < f32::EPSILON { - // Degenerate line (zero length) - return point.distance(self.start); - } - - let to_point = point - self.start; - (to_point.cross(dir)).abs() / len_sq.sqrt() - } - - /// Signed perpendicular distance from a point to the infinite line. - /// - /// Positive if the point is to the left of the line (in direction of normal), - /// negative if to the right. - #[inline] - pub fn signed_distance_to_point(&self, point: Point2D) -> f32 { - let dir = self.direction(); - let len_sq = dir.length_squared(); - - if len_sq < f32::EPSILON { - return point.distance(self.start); - } - - let to_point = point - self.start; - // Cross product: dir × to_point gives positive when point is to the left - dir.cross(to_point) / len_sq.sqrt() - } - - /// Project a point onto the infinite line, returning parameter t. - /// - /// The projection point is: `start + t * (end - start)` - /// - `t = 0`: projection is at start - /// - `t = 1`: projection is at end - /// - `0 < t < 1`: projection is between endpoints - /// - `t < 0` or `t > 1`: projection is outside segment - #[inline] - pub fn project_point(&self, point: Point2D) -> f32 { - let dir = self.direction(); - let len_sq = dir.length_squared(); - - if len_sq < f32::EPSILON { - return 0.0; - } - - let to_point = point - self.start; - to_point.dot(dir) / len_sq - } - - /// Check if a projection parameter t falls within the segment [0, 1]. - #[inline] - pub fn contains_projection(&self, t: f32) -> bool { - (0.0..=1.0).contains(&t) - } - - /// Check if a projection parameter t falls within the segment with extension tolerance. - /// - /// Allows points to project slightly beyond line endpoints by `extension` amount. - /// The valid range becomes `[-extension, 1 + extension]`. - /// - /// # Arguments - /// * `t` - Projection parameter (0 = start, 1 = end) - /// * `extension` - Tolerance for projection beyond endpoints (e.g., 0.2 = 20% extension) - /// - /// # Example - /// ```rust,ignore - /// let line = Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(10.0, 0.0)); - /// let t = line.project_point(Point2D::new(-1.0, 0.5)); // t ≈ -0.1 - /// - /// assert!(!line.contains_projection(t)); // Outside [0, 1] - /// assert!(line.contains_projection_extended(t, 0.2)); // Inside [-0.2, 1.2] - /// ``` - #[inline] - pub fn contains_projection_extended(&self, t: f32, extension: f32) -> bool { - t >= -extension && t <= 1.0 + extension - } - - /// Distance from a point to the line segment (not infinite line). - /// - /// Returns the minimum distance to any point on the segment. - #[inline] - pub fn distance_to_point_segment(&self, point: Point2D) -> f32 { - let t = self.project_point(point).clamp(0.0, 1.0); - point.distance(self.point_at(t)) - } - - /// Transform line by a pose (rotate and translate both endpoints). - #[inline] - pub fn transform(&self, pose: &Pose2D) -> Self { - Self { - start: pose.transform_point(self.start), - end: pose.transform_point(self.end), - observation_count: self.observation_count, - point_count: self.point_count, - } - } - - /// Compute the overlap between two line segments. - /// - /// Returns the length of the overlap when projected onto this line. - /// Lines must be roughly parallel for meaningful overlap. - /// - /// Returns 0 if lines don't overlap or aren't parallel. - pub fn overlap_length(&self, other: &Line2D, angle_tolerance: f32) -> f32 { - use crate::core::math::angle_diff; - - // Check if lines are roughly parallel - let angle_diff = angle_diff(self.angle(), other.angle()).abs(); - if angle_diff > angle_tolerance && angle_diff < std::f32::consts::PI - angle_tolerance { - return 0.0; - } - - // Project other line's endpoints onto this line - let t1 = self.project_point(other.start); - let t2 = self.project_point(other.end); - - // Find overlap in parameter space - let (t_min, t_max) = if t1 < t2 { (t1, t2) } else { (t2, t1) }; - - // Clamp to [0, 1] (this line's extent) - let overlap_start = t_min.max(0.0); - let overlap_end = t_max.min(1.0); - - if overlap_end <= overlap_start { - return 0.0; - } - - (overlap_end - overlap_start) * self.length() - } - - /// Check if a ray intersects this line segment. - /// - /// Returns the distance along the ray to the intersection point, - /// or None if no intersection. - /// - /// # Arguments - /// * `ray_origin` - Origin of the ray - /// * `ray_direction` - Direction of the ray (will be normalized internally) - pub fn ray_intersection(&self, ray_origin: Point2D, ray_direction: Point2D) -> Option { - let dir = ray_direction.normalized(); - let seg = self.direction(); - - // Use parametric form: ray_origin + t*dir = start + s*seg - // Solve using cross products - let cross_dir_seg = dir.cross(seg); - - if cross_dir_seg.abs() < f32::EPSILON { - // Parallel lines - return None; - } - - let origin_to_start = self.start - ray_origin; - let t = origin_to_start.cross(seg) / cross_dir_seg; - let s = origin_to_start.cross(dir) / cross_dir_seg; - - // t must be positive (forward along ray) - // s must be in [0, 1] (within segment) - if t >= 0.0 && (0.0..=1.0).contains(&s) { - Some(t) - } else { - None - } - } - - /// Find intersection point with another line segment. - /// - /// Returns the intersection point if the segments intersect, - /// None otherwise. - pub fn intersection(&self, other: &Line2D) -> Option { - let d1 = self.direction(); - let d2 = other.direction(); - - let cross = d1.cross(d2); - if cross.abs() < f32::EPSILON { - // Parallel lines - return None; - } - - let origin_diff = other.start - self.start; - let t = origin_diff.cross(d2) / cross; - let s = origin_diff.cross(d1) / cross; - - if (0.0..=1.0).contains(&t) && (0.0..=1.0).contains(&s) { - Some(self.point_at(t)) - } else { - None - } - } - - /// Check if this line approximately equals another. - #[inline] - pub fn approx_eq(&self, other: &Line2D, epsilon: f32) -> bool { - self.start.approx_eq(other.start, epsilon) && self.end.approx_eq(other.end, epsilon) - } -} - -impl Default for Line2D { - fn default() -> Self { - Self { - start: Point2D::zero(), - end: Point2D::zero(), - observation_count: 0, - point_count: 0, - } - } -} - -#[cfg(test)] -mod tests { - use super::*; - use approx::assert_relative_eq; - use std::f32::consts::{FRAC_PI_2, FRAC_PI_4}; - - #[test] - fn test_new() { - let line = Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(1.0, 0.0)); - assert_eq!(line.start, Point2D::new(0.0, 0.0)); - assert_eq!(line.end, Point2D::new(1.0, 0.0)); - assert_eq!(line.observation_count, 1); - } - - #[test] - fn test_direction_and_length() { - let line = Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(3.0, 4.0)); - - assert_eq!(line.direction(), Point2D::new(3.0, 4.0)); - assert_eq!(line.length(), 5.0); - assert_eq!(line.length_squared(), 25.0); - - let unit = line.unit_direction(); - assert_relative_eq!(unit.x, 0.6, epsilon = 1e-6); - assert_relative_eq!(unit.y, 0.8, epsilon = 1e-6); - } - - #[test] - fn test_midpoint() { - let line = Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(4.0, 6.0)); - let mid = line.midpoint(); - assert_eq!(mid.x, 2.0); - assert_eq!(mid.y, 3.0); - } - - #[test] - fn test_angle() { - let horizontal = Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(1.0, 0.0)); - assert_relative_eq!(horizontal.angle(), 0.0, epsilon = 1e-6); - - let vertical = Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(0.0, 1.0)); - assert_relative_eq!(vertical.angle(), FRAC_PI_2, epsilon = 1e-6); - - let diagonal = Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(1.0, 1.0)); - assert_relative_eq!(diagonal.angle(), FRAC_PI_4, epsilon = 1e-6); - } - - #[test] - fn test_distance_to_point() { - let line = Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(10.0, 0.0)); - - // Point above the line - let dist = line.distance_to_point(Point2D::new(5.0, 3.0)); - assert_relative_eq!(dist, 3.0, epsilon = 1e-6); - - // Point below the line - let dist = line.distance_to_point(Point2D::new(5.0, -4.0)); - assert_relative_eq!(dist, 4.0, epsilon = 1e-6); - - // Point on the line - let dist = line.distance_to_point(Point2D::new(5.0, 0.0)); - assert_relative_eq!(dist, 0.0, epsilon = 1e-6); - } - - #[test] - fn test_signed_distance() { - let line = Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(10.0, 0.0)); - - // Point above (left of line direction) - positive - let dist = line.signed_distance_to_point(Point2D::new(5.0, 3.0)); - assert_relative_eq!(dist, 3.0, epsilon = 1e-6); - - // Point below (right of line direction) - negative - let dist = line.signed_distance_to_point(Point2D::new(5.0, -3.0)); - assert_relative_eq!(dist, -3.0, epsilon = 1e-6); - } - - #[test] - fn test_project_point() { - let line = Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(10.0, 0.0)); - - // Project point to midpoint - let t = line.project_point(Point2D::new(5.0, 3.0)); - assert_relative_eq!(t, 0.5, epsilon = 1e-6); - - // Project point at start - let t = line.project_point(Point2D::new(0.0, 5.0)); - assert_relative_eq!(t, 0.0, epsilon = 1e-6); - - // Project point beyond end - let t = line.project_point(Point2D::new(15.0, 0.0)); - assert_relative_eq!(t, 1.5, epsilon = 1e-6); - - // Project point before start - let t = line.project_point(Point2D::new(-5.0, 0.0)); - assert_relative_eq!(t, -0.5, epsilon = 1e-6); - } - - #[test] - fn test_contains_projection() { - let line = Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(10.0, 0.0)); - - assert!(line.contains_projection(0.0)); - assert!(line.contains_projection(0.5)); - assert!(line.contains_projection(1.0)); - assert!(!line.contains_projection(-0.1)); - assert!(!line.contains_projection(1.1)); - } - - #[test] - fn test_contains_projection_extended() { - let line = Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(10.0, 0.0)); - - // With 0.2 extension, valid range is [-0.2, 1.2] - assert!(line.contains_projection_extended(0.0, 0.2)); - assert!(line.contains_projection_extended(0.5, 0.2)); - assert!(line.contains_projection_extended(1.0, 0.2)); - assert!(line.contains_projection_extended(-0.1, 0.2)); // Within extension - assert!(line.contains_projection_extended(1.1, 0.2)); // Within extension - assert!(!line.contains_projection_extended(-0.3, 0.2)); // Beyond extension - assert!(!line.contains_projection_extended(1.3, 0.2)); // Beyond extension - - // Zero extension = strict [0, 1] - assert!(!line.contains_projection_extended(-0.1, 0.0)); - assert!(!line.contains_projection_extended(1.1, 0.0)); - } - - #[test] - fn test_point_at() { - let line = Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(10.0, 0.0)); - - assert_eq!(line.point_at(0.0), line.start); - assert_eq!(line.point_at(1.0), line.end); - assert_eq!(line.point_at(0.5), Point2D::new(5.0, 0.0)); - } - - #[test] - fn test_distance_to_point_segment() { - let line = Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(10.0, 0.0)); - - // Point above segment (closest to segment) - let dist = line.distance_to_point_segment(Point2D::new(5.0, 3.0)); - assert_relative_eq!(dist, 3.0, epsilon = 1e-6); - - // Point beyond end (closest to endpoint) - let dist = line.distance_to_point_segment(Point2D::new(13.0, 4.0)); - assert_relative_eq!(dist, 5.0, epsilon = 1e-6); // Distance to (10, 0) - } - - #[test] - fn test_transform() { - let line = Line2D::new(Point2D::new(1.0, 0.0), Point2D::new(2.0, 0.0)); - - // Rotate 90 degrees at origin - let pose = Pose2D::new(0.0, 0.0, FRAC_PI_2); - let transformed = line.transform(&pose); - - assert_relative_eq!(transformed.start.x, 0.0, epsilon = 1e-6); - assert_relative_eq!(transformed.start.y, 1.0, epsilon = 1e-6); - assert_relative_eq!(transformed.end.x, 0.0, epsilon = 1e-6); - assert_relative_eq!(transformed.end.y, 2.0, epsilon = 1e-6); - } - - #[test] - fn test_ray_intersection() { - let line = Line2D::new(Point2D::new(5.0, -5.0), Point2D::new(5.0, 5.0)); - - // Ray from origin pointing right - let dist = line.ray_intersection(Point2D::new(0.0, 0.0), Point2D::new(1.0, 0.0)); - assert_relative_eq!(dist.unwrap(), 5.0, epsilon = 1e-6); - - // Ray pointing away from line - let dist = line.ray_intersection(Point2D::new(0.0, 0.0), Point2D::new(-1.0, 0.0)); - assert!(dist.is_none()); - - // Ray parallel to line - let dist = line.ray_intersection(Point2D::new(0.0, 0.0), Point2D::new(0.0, 1.0)); - assert!(dist.is_none()); - } - - #[test] - fn test_segment_intersection() { - let line1 = Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(10.0, 0.0)); - let line2 = Line2D::new(Point2D::new(5.0, -5.0), Point2D::new(5.0, 5.0)); - - let intersection = line1.intersection(&line2); - assert!(intersection.is_some()); - let p = intersection.unwrap(); - assert_relative_eq!(p.x, 5.0, epsilon = 1e-6); - assert_relative_eq!(p.y, 0.0, epsilon = 1e-6); - - // Non-intersecting segments - let line3 = Line2D::new(Point2D::new(20.0, 0.0), Point2D::new(30.0, 0.0)); - assert!(line1.intersection(&line3).is_none()); - } - - #[test] - fn test_overlap_length() { - let line1 = Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(10.0, 0.0)); - let line2 = Line2D::new(Point2D::new(5.0, 0.1), Point2D::new(15.0, 0.1)); // Slightly offset, parallel - - let overlap = line1.overlap_length(&line2, 0.1); - assert_relative_eq!(overlap, 5.0, epsilon = 0.1); - - // Non-overlapping - let line3 = Line2D::new(Point2D::new(15.0, 0.0), Point2D::new(25.0, 0.0)); - let overlap = line1.overlap_length(&line3, 0.1); - assert_eq!(overlap, 0.0); - - // Perpendicular lines (not parallel) - let line4 = Line2D::new(Point2D::new(5.0, -5.0), Point2D::new(5.0, 5.0)); - let overlap = line1.overlap_length(&line4, 0.1); - assert_eq!(overlap, 0.0); - } - - #[test] - fn test_normal() { - let line = Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(1.0, 0.0)); - let normal = line.normal(); - - // For a line pointing right, normal points up - assert_relative_eq!(normal.x, 0.0, epsilon = 1e-6); - assert_relative_eq!(normal.y, 1.0, epsilon = 1e-6); - } -} diff --git a/vastu-map/src/features/line_collection.rs b/vastu-map/src/features/line_collection.rs deleted file mode 100644 index 34a6658..0000000 --- a/vastu-map/src/features/line_collection.rs +++ /dev/null @@ -1,996 +0,0 @@ -//! Line collection with SoA (Struct of Arrays) layout for SIMD operations. -//! -//! This layout enables efficient batch operations via SIMD: -//! - 4 floats per line = clean SIMD alignment -//! - Batch distance computations -//! - Parallel transforms -//! -//! # Performance Optimizations -//! -//! Pre-computed cached values avoid redundant calculations in hot paths: -//! - `normal_xs`, `normal_ys`: Unit normal vectors (avoids sqrt per distance calc) -//! - `inv_lengths`: Reciprocal of line lengths (avoids division in inner loops) -//! -//! # SIMD Implementation -//! -//! Uses `std::simd::f32x4` for explicit 4-wide SIMD operations via the -//! `portable_simd` feature. - -use std::simd::{f32x4, num::SimdFloat}; - -use super::line::Line2D; -use super::spatial_index::LineSpatialIndex; -use crate::core::{Point2D, Pose2D}; - -/// Collection of line segments with SoA layout for SIMD operations. -/// -/// Each line is represented by 4 floats: (start_x, start_y, end_x, end_y). -/// This layout enables efficient batch operations using 4-wide SIMD. -/// -/// # Pre-computed Values -/// -/// For performance, the collection caches: -/// - **Normals**: Unit normal vectors for each line -/// - **Inverse lengths**: `1.0 / length` to avoid division in inner loops -/// -/// These are automatically computed when lines are added. -/// -/// # Example -/// ``` -/// use vastu_map::features::{Line2D, LineCollection}; -/// use vastu_map::core::Point2D; -/// -/// let lines = vec![ -/// Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(1.0, 0.0)), -/// Line2D::new(Point2D::new(0.0, 1.0), Point2D::new(1.0, 1.0)), -/// ]; -/// -/// let collection = LineCollection::from_lines(&lines); -/// assert_eq!(collection.len(), 2); -/// ``` -#[derive(Clone, Debug, Default)] -pub struct LineCollection { - /// X coordinates of line start points. - pub start_xs: Vec, - /// Y coordinates of line start points. - pub start_ys: Vec, - /// X coordinates of line end points. - pub end_xs: Vec, - /// Y coordinates of line end points. - pub end_ys: Vec, - /// Observation counts for each line. - pub observation_counts: Vec, - /// Point counts for each line (scan points that formed the line). - pub point_counts: Vec, - - // === Pre-computed cached values for performance === - /// X components of unit normal vectors. - normal_xs: Vec, - /// Y components of unit normal vectors. - normal_ys: Vec, - /// Reciprocal of line lengths (1.0 / length). - /// Used to avoid division in distance calculations. - inv_lengths: Vec, -} - -impl LineCollection { - /// Create a new empty line collection. - pub fn new() -> Self { - Self::default() - } - - /// Create a line collection with capacity. - pub fn with_capacity(capacity: usize) -> Self { - Self { - start_xs: Vec::with_capacity(capacity), - start_ys: Vec::with_capacity(capacity), - end_xs: Vec::with_capacity(capacity), - end_ys: Vec::with_capacity(capacity), - observation_counts: Vec::with_capacity(capacity), - point_counts: Vec::with_capacity(capacity), - normal_xs: Vec::with_capacity(capacity), - normal_ys: Vec::with_capacity(capacity), - inv_lengths: Vec::with_capacity(capacity), - } - } - - /// Create from a slice of Line2D. - pub fn from_lines(lines: &[Line2D]) -> Self { - let mut collection = Self::with_capacity(lines.len()); - for line in lines { - collection.push_line(line); - } - collection - } - - /// Compute and cache normal and inverse length for a line. - #[inline] - fn compute_cached_values( - start_x: f32, - start_y: f32, - end_x: f32, - end_y: f32, - ) -> (f32, f32, f32) { - let dx = end_x - start_x; - let dy = end_y - start_y; - let len_sq = dx * dx + dy * dy; - - if len_sq < f32::EPSILON { - // Degenerate line - use arbitrary normal and large inv_length - (0.0, 1.0, 0.0) - } else { - let len = len_sq.sqrt(); - let inv_len = 1.0 / len; - // Normal is perpendicular to direction: (-dy, dx) normalized - let nx = -dy * inv_len; - let ny = dx * inv_len; - (nx, ny, inv_len) - } - } - - /// Add a line to the collection. - #[inline] - pub fn push_full( - &mut self, - start_x: f32, - start_y: f32, - end_x: f32, - end_y: f32, - obs_count: u32, - pt_count: u32, - ) { - let (nx, ny, inv_len) = Self::compute_cached_values(start_x, start_y, end_x, end_y); - - self.start_xs.push(start_x); - self.start_ys.push(start_y); - self.end_xs.push(end_x); - self.end_ys.push(end_y); - self.observation_counts.push(obs_count); - self.point_counts.push(pt_count); - self.normal_xs.push(nx); - self.normal_ys.push(ny); - self.inv_lengths.push(inv_len); - } - - /// Add a Line2D to the collection. - #[inline] - pub fn push_line(&mut self, line: &Line2D) { - self.push_full( - line.start.x, - line.start.y, - line.end.x, - line.end.y, - line.observation_count, - line.point_count, - ); - } - - /// Get the pre-computed normal vector for a line. - #[inline] - pub fn normal(&self, index: usize) -> Option { - if index < self.len() { - Some(Point2D::new(self.normal_xs[index], self.normal_ys[index])) - } else { - None - } - } - - /// Get the pre-computed inverse length for a line. - #[inline] - pub fn inv_length(&self, index: usize) -> Option { - self.inv_lengths.get(index).copied() - } - - /// Get all normals as slices (for batch operations). - #[inline] - pub fn normals(&self) -> (&[f32], &[f32]) { - (&self.normal_xs, &self.normal_ys) - } - - /// Get all inverse lengths (for batch operations). - #[inline] - pub fn inv_lengths(&self) -> &[f32] { - &self.inv_lengths - } - - /// Number of lines in the collection. - #[inline] - pub fn len(&self) -> usize { - self.start_xs.len() - } - - /// Check if the collection is empty. - #[inline] - pub fn is_empty(&self) -> bool { - self.start_xs.is_empty() - } - - /// Get a line by index. - #[inline] - pub fn get(&self, index: usize) -> Option { - if index < self.len() { - Some(Line2D::full( - Point2D::new(self.start_xs[index], self.start_ys[index]), - Point2D::new(self.end_xs[index], self.end_ys[index]), - self.observation_counts[index], - self.point_counts[index], - )) - } else { - None - } - } - - /// Convert to `Vec`. - pub fn to_lines(&self) -> Vec { - (0..self.len()) - .map(|i| { - Line2D::full( - Point2D::new(self.start_xs[i], self.start_ys[i]), - Point2D::new(self.end_xs[i], self.end_ys[i]), - self.observation_counts[i], - self.point_counts[i], - ) - }) - .collect() - } - - /// Compute perpendicular distances from a point to all lines. - /// - /// Uses SIMD-optimized batch computation with pre-computed inverse lengths. - /// Returns a vector of distances (always non-negative). - /// - /// # Formula - /// ```text - /// distance = |cross(p - start, end - start)| * inv_length - /// = |((px - sx) * (ey - sy) - (py - sy) * (ex - sx))| * inv_length - /// ``` - /// - /// # Performance - /// Uses pre-computed `inv_lengths` to avoid sqrt/division in the inner loop. - pub fn distances_to_point(&self, point: Point2D) -> Vec { - let n = self.len(); - let mut distances = vec![0.0; n]; - - let px = f32x4::splat(point.x); - let py = f32x4::splat(point.y); - - // Process 4 lines at a time - let chunks = n / 4; - for i in 0..chunks { - let base = i * 4; - - // Load line data - contiguous SIMD loads - let sx = f32x4::from_slice(&self.start_xs[base..]); - let sy = f32x4::from_slice(&self.start_ys[base..]); - let ex = f32x4::from_slice(&self.end_xs[base..]); - let ey = f32x4::from_slice(&self.end_ys[base..]); - - // Load pre-computed inverse lengths - let inv_len = f32x4::from_slice(&self.inv_lengths[base..]); - - // Compute direction vectors - let dx = ex - sx; - let dy = ey - sy; - - // Compute vectors from start to point - let to_px = px - sx; - let to_py = py - sy; - - // Cross product: (px - sx) * (ey - sy) - (py - sy) * (ex - sx) - // = to_px * dy - to_py * dx - let cross = to_px * dy - to_py * dx; - - // Distance = |cross| * inv_length (no sqrt needed!) - let abs_cross = cross.abs(); - let dist_4 = abs_cross * inv_len; - distances[base..base + 4].copy_from_slice(&dist_4.to_array()); - } - - // Handle remainder (scalar) using pre-computed values - #[allow(clippy::needless_range_loop)] - for i in (chunks * 4)..n { - let sx = self.start_xs[i]; - let sy = self.start_ys[i]; - let ex = self.end_xs[i]; - let ey = self.end_ys[i]; - let inv_len = self.inv_lengths[i]; - - let dx = ex - sx; - let dy = ey - sy; - let to_px = point.x - sx; - let to_py = point.y - sy; - let cross = to_px * dy - to_py * dx; - - distances[i] = cross.abs() * inv_len; - } - - distances - } - - /// Compute projection parameters for a point onto all lines. - /// - /// Returns a vector of projection parameters t, where: - /// - t = 0 means the projection is at line start - /// - t = 1 means the projection is at line end - /// - t < 0 or t > 1 means the projection is outside the segment - /// - /// # Formula - /// ```text - /// t = dot(point - start, end - start) / |end - start|² - /// ``` - pub fn project_point(&self, point: Point2D) -> Vec { - let n = self.len(); - let mut projections = vec![0.0; n]; - - let px = f32x4::splat(point.x); - let py = f32x4::splat(point.y); - - // Process 4 lines at a time - let chunks = n / 4; - for i in 0..chunks { - let base = i * 4; - - // Load line data - contiguous SIMD loads - let sx = f32x4::from_slice(&self.start_xs[base..]); - let sy = f32x4::from_slice(&self.start_ys[base..]); - let ex = f32x4::from_slice(&self.end_xs[base..]); - let ey = f32x4::from_slice(&self.end_ys[base..]); - - // Compute direction vectors - let dx = ex - sx; - let dy = ey - sy; - - // Compute vectors from start to point - let to_px = px - sx; - let to_py = py - sy; - - // Dot product: (px - sx) * dx + (py - sy) * dy - let dot = to_px * dx + to_py * dy; - - // Length squared: dx*dx + dy*dy - let len_sq = dx * dx + dy * dy; - - // Compute projection t = dot / len_sq - let dot_arr = dot.to_array(); - let len_sq_arr = len_sq.to_array(); - - for j in 0..4 { - projections[base + j] = if len_sq_arr[j] > f32::EPSILON { - dot_arr[j] / len_sq_arr[j] - } else { - // Degenerate line - return 0.5 (midpoint) - 0.5 - }; - } - } - - // Handle remainder (scalar) - for (i, proj) in projections.iter_mut().enumerate().skip(chunks * 4) { - let line = self.get(i).unwrap(); - *proj = line.project_point(point); - } - - projections - } - - /// Find the nearest line to a point. - /// - /// Returns (index, distance) of the nearest line, or None if collection is empty. - pub fn nearest_line(&self, point: Point2D) -> Option<(usize, f32)> { - if self.is_empty() { - return None; - } - - let distances = self.distances_to_point(point); - - let mut min_idx = 0; - let mut min_dist = distances[0]; - - for (i, &d) in distances.iter().enumerate().skip(1) { - if d < min_dist { - min_dist = d; - min_idx = i; - } - } - - Some((min_idx, min_dist)) - } - - /// Compute perpendicular distances from a point to all lines into a pre-allocated buffer. - /// - /// Zero-allocation variant of `distances_to_point()` for hot paths. - /// Uses SIMD-optimized batch computation with pre-computed inverse lengths. - /// - /// # Panics - /// Panics if `buffer.len() < self.len()`. - pub fn distances_to_point_into(&self, point: Point2D, buffer: &mut [f32]) { - let n = self.len(); - debug_assert!( - buffer.len() >= n, - "Buffer too small: {} < {}", - buffer.len(), - n - ); - - let px = f32x4::splat(point.x); - let py = f32x4::splat(point.y); - - // Process 4 lines at a time - let chunks = n / 4; - for i in 0..chunks { - let base = i * 4; - - // Load line data - contiguous SIMD loads - let sx = f32x4::from_slice(&self.start_xs[base..]); - let sy = f32x4::from_slice(&self.start_ys[base..]); - let ex = f32x4::from_slice(&self.end_xs[base..]); - let ey = f32x4::from_slice(&self.end_ys[base..]); - - // Load pre-computed inverse lengths - let inv_len = f32x4::from_slice(&self.inv_lengths[base..]); - - // Compute direction vectors - let dx = ex - sx; - let dy = ey - sy; - - // Compute vectors from start to point - let to_px = px - sx; - let to_py = py - sy; - - // Cross product: (px - sx) * (ey - sy) - (py - sy) * (ex - sx) - // = to_px * dy - to_py * dx - let cross = to_px * dy - to_py * dx; - - // Distance = |cross| * inv_length (no sqrt needed!) - let abs_cross = cross.abs(); - let dist_4 = abs_cross * inv_len; - buffer[base..base + 4].copy_from_slice(&dist_4.to_array()); - } - - // Handle remainder (scalar) using pre-computed values - #[allow(clippy::needless_range_loop)] - for i in (chunks * 4)..n { - let sx = self.start_xs[i]; - let sy = self.start_ys[i]; - let ex = self.end_xs[i]; - let ey = self.end_ys[i]; - let inv_len = self.inv_lengths[i]; - - let dx = ex - sx; - let dy = ey - sy; - let to_px = point.x - sx; - let to_py = point.y - sy; - let cross = to_px * dy - to_py * dx; - - buffer[i] = cross.abs() * inv_len; - } - } - - /// Compute distances AND projections in a single pass (cache-friendly). - /// - /// This fused method loads line data once and computes both values, - /// providing better cache utilization than calling `distances_to_point_into` - /// and `project_point` separately. - /// - /// # Panics - /// Panics if `distances.len() < self.len()` or `projections.len() < self.len()`. - pub fn distances_and_projections_into( - &self, - point: Point2D, - distances: &mut [f32], - projections: &mut [f32], - ) { - let n = self.len(); - debug_assert!( - distances.len() >= n, - "Distance buffer too small: {} < {}", - distances.len(), - n - ); - debug_assert!( - projections.len() >= n, - "Projection buffer too small: {} < {}", - projections.len(), - n - ); - - let px = f32x4::splat(point.x); - let py = f32x4::splat(point.y); - - // Process 4 lines at a time - let chunks = n / 4; - for i in 0..chunks { - let base = i * 4; - - // Load line data once - the key optimization - let sx = f32x4::from_slice(&self.start_xs[base..]); - let sy = f32x4::from_slice(&self.start_ys[base..]); - let ex = f32x4::from_slice(&self.end_xs[base..]); - let ey = f32x4::from_slice(&self.end_ys[base..]); - let inv_len = f32x4::from_slice(&self.inv_lengths[base..]); - - // Compute direction vectors - let dx = ex - sx; - let dy = ey - sy; - - // Compute vectors from start to point - let to_px = px - sx; - let to_py = py - sy; - - // === Distance computation === - // Cross product: to_px * dy - to_py * dx - let cross = to_px * dy - to_py * dx; - let dist_4 = cross.abs() * inv_len; - distances[base..base + 4].copy_from_slice(&dist_4.to_array()); - - // === Projection computation === - // t = dot(point - start, end - start) / |end - start|² - let dot = to_px * dx + to_py * dy; - let len_sq = dx * dx + dy * dy; - - // Handle degenerate lines: use 0.5 if len_sq < epsilon - let len_sq_arr = len_sq.to_array(); - let dot_arr = dot.to_array(); - - for j in 0..4 { - projections[base + j] = if len_sq_arr[j] > f32::EPSILON { - dot_arr[j] / len_sq_arr[j] - } else { - 0.5 // Degenerate line - midpoint - }; - } - } - - // Handle remainder (scalar) - #[allow(clippy::needless_range_loop)] - for i in (chunks * 4)..n { - let sx = self.start_xs[i]; - let sy = self.start_ys[i]; - let ex = self.end_xs[i]; - let ey = self.end_ys[i]; - let inv_len = self.inv_lengths[i]; - - let dx = ex - sx; - let dy = ey - sy; - let to_px = point.x - sx; - let to_py = point.y - sy; - - // Distance - let cross = to_px * dy - to_py * dx; - distances[i] = cross.abs() * inv_len; - - // Projection - let len_sq = dx * dx + dy * dy; - projections[i] = if len_sq > f32::EPSILON { - (to_px * dx + to_py * dy) / len_sq - } else { - 0.5 - }; - } - } - - /// Find the nearest line to a point using a reusable buffer. - /// - /// Zero-allocation variant of `nearest_line()` for hot paths like ICP. - /// The buffer will be resized if needed. - /// - /// Returns (index, distance) of the nearest line, or None if collection is empty. - pub fn nearest_line_with_buffer( - &self, - point: Point2D, - buffer: &mut Vec, - ) -> Option<(usize, f32)> { - if self.is_empty() { - return None; - } - - let n = self.len(); - buffer.resize(n, 0.0); - self.distances_to_point_into(point, buffer); - - let mut min_idx = 0; - let mut min_dist = buffer[0]; - - for (i, &d) in buffer.iter().enumerate().skip(1).take(n - 1) { - if d < min_dist { - min_dist = d; - min_idx = i; - } - } - - Some((min_idx, min_dist)) - } - - /// Transform all lines by a pose. - /// - /// SIMD-optimized: processes 4 points per iteration. - /// Normals are rotated (not recomputed) since rotation preserves angles. - /// Inverse lengths are preserved since rotation+translation preserves length. - pub fn transform(&self, pose: &Pose2D) -> Self { - let n = self.len(); - let mut result = Self::with_capacity(n); - result.start_xs.resize(n, 0.0); - result.start_ys.resize(n, 0.0); - result.end_xs.resize(n, 0.0); - result.end_ys.resize(n, 0.0); - result.normal_xs.resize(n, 0.0); - result.normal_ys.resize(n, 0.0); - result.observation_counts = self.observation_counts.clone(); - result.point_counts = self.point_counts.clone(); - // Inverse lengths are preserved by rotation+translation - result.inv_lengths = self.inv_lengths.clone(); - - let (sin, cos) = pose.theta.sin_cos(); - let cos4 = f32x4::splat(cos); - let sin4 = f32x4::splat(sin); - let tx4 = f32x4::splat(pose.x); - let ty4 = f32x4::splat(pose.y); - - // Process 4 start points at a time - let chunks = n / 4; - for i in 0..chunks { - let base = i * 4; - - // Transform start points - contiguous SIMD loads - let sx = f32x4::from_slice(&self.start_xs[base..]); - let sy = f32x4::from_slice(&self.start_ys[base..]); - - // new_sx = sx*cos - sy*sin + tx - // new_sy = sx*sin + sy*cos + ty - let new_sx = sx * cos4 - sy * sin4 + tx4; - let new_sy = sx * sin4 + sy * cos4 + ty4; - - result.start_xs[base..base + 4].copy_from_slice(&new_sx.to_array()); - result.start_ys[base..base + 4].copy_from_slice(&new_sy.to_array()); - - // Transform end points - let ex = f32x4::from_slice(&self.end_xs[base..]); - let ey = f32x4::from_slice(&self.end_ys[base..]); - - let new_ex = ex * cos4 - ey * sin4 + tx4; - let new_ey = ex * sin4 + ey * cos4 + ty4; - - result.end_xs[base..base + 4].copy_from_slice(&new_ex.to_array()); - result.end_ys[base..base + 4].copy_from_slice(&new_ey.to_array()); - - // Rotate normals (no translation needed for unit vectors) - let nx = f32x4::from_slice(&self.normal_xs[base..]); - let ny = f32x4::from_slice(&self.normal_ys[base..]); - - // Rotate normal: n' = R * n (no translation) - let new_nx = nx * cos4 - ny * sin4; - let new_ny = nx * sin4 + ny * cos4; - - result.normal_xs[base..base + 4].copy_from_slice(&new_nx.to_array()); - result.normal_ys[base..base + 4].copy_from_slice(&new_ny.to_array()); - } - - // Handle remainder (scalar) - for i in (chunks * 4)..n { - let sx = self.start_xs[i]; - let sy = self.start_ys[i]; - let ex = self.end_xs[i]; - let ey = self.end_ys[i]; - let nx = self.normal_xs[i]; - let ny = self.normal_ys[i]; - - result.start_xs[i] = sx * cos - sy * sin + pose.x; - result.start_ys[i] = sx * sin + sy * cos + pose.y; - result.end_xs[i] = ex * cos - ey * sin + pose.x; - result.end_ys[i] = ex * sin + ey * cos + pose.y; - result.normal_xs[i] = nx * cos - ny * sin; - result.normal_ys[i] = nx * sin + ny * cos; - } - - result - } - - /// Clear all lines. - pub fn clear(&mut self) { - self.start_xs.clear(); - self.start_ys.clear(); - self.end_xs.clear(); - self.end_ys.clear(); - self.observation_counts.clear(); - self.point_counts.clear(); - self.normal_xs.clear(); - self.normal_ys.clear(); - self.inv_lengths.clear(); - } - - /// Remove a line by index (swap-remove for efficiency). - pub fn swap_remove(&mut self, index: usize) { - self.start_xs.swap_remove(index); - self.start_ys.swap_remove(index); - self.end_xs.swap_remove(index); - self.end_ys.swap_remove(index); - self.observation_counts.swap_remove(index); - self.point_counts.swap_remove(index); - self.normal_xs.swap_remove(index); - self.normal_ys.swap_remove(index); - self.inv_lengths.swap_remove(index); - } - - /// Iterate over lines. - pub fn iter(&self) -> impl Iterator + '_ { - (0..self.len()).map(move |i| { - Line2D::full( - Point2D::new(self.start_xs[i], self.start_ys[i]), - Point2D::new(self.end_xs[i], self.end_ys[i]), - self.observation_counts[i], - self.point_counts[i], - ) - }) - } - - /// Build a spatial index for fast proximity queries. - /// - /// # Arguments - /// * `cell_size` - Grid cell size in meters (typically 0.5-1.0m) - /// - /// # Example - /// ```rust,ignore - /// let lines = LineCollection::from_lines(&[...]); - /// let index = lines.build_spatial_index(0.5); - /// - /// // Query for lines near a point - /// let candidates = index.query_radius(Point2D::new(1.0, 2.0), 0.3); - /// ``` - #[inline] - pub fn build_spatial_index(&self, cell_size: f32) -> LineSpatialIndex { - LineSpatialIndex::build(self, cell_size) - } - - /// Build a spatial index optimized for a specific query radius. - /// - /// Uses cell_size = 2 × query_radius for optimal performance. - #[inline] - pub fn build_spatial_index_for_radius(&self, query_radius: f32) -> LineSpatialIndex { - LineSpatialIndex::build_for_radius(self, query_radius) - } -} - -#[cfg(test)] -mod tests { - use super::*; - use approx::assert_relative_eq; - use std::f32::consts::FRAC_PI_2; - - #[test] - fn test_from_lines() { - let lines = vec![ - Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(1.0, 0.0)), - Line2D::new(Point2D::new(0.0, 1.0), Point2D::new(1.0, 1.0)), - ]; - - let collection = LineCollection::from_lines(&lines); - - assert_eq!(collection.len(), 2); - assert_eq!(collection.start_xs, vec![0.0, 0.0]); - assert_eq!(collection.start_ys, vec![0.0, 1.0]); - assert_eq!(collection.end_xs, vec![1.0, 1.0]); - assert_eq!(collection.end_ys, vec![0.0, 1.0]); - } - - #[test] - fn test_to_lines() { - let mut collection = LineCollection::new(); - collection.push_full(0.0, 0.0, 1.0, 0.0, 1, 10); - collection.push_full(0.0, 1.0, 1.0, 1.0, 2, 20); - - let lines = collection.to_lines(); - - assert_eq!(lines.len(), 2); - assert_eq!(lines[0].start, Point2D::new(0.0, 0.0)); - assert_eq!(lines[0].end, Point2D::new(1.0, 0.0)); - assert_eq!(lines[0].observation_count, 1); - assert_eq!(lines[0].point_count, 10); - assert_eq!(lines[1].observation_count, 2); - assert_eq!(lines[1].point_count, 20); - } - - #[test] - fn test_distances_to_point() { - let lines = vec![ - Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(10.0, 0.0)), // Horizontal at y=0 - Line2D::new(Point2D::new(0.0, 5.0), Point2D::new(10.0, 5.0)), // Horizontal at y=5 - ]; - - let collection = LineCollection::from_lines(&lines); - let point = Point2D::new(5.0, 2.0); - - let distances = collection.distances_to_point(point); - - assert_eq!(distances.len(), 2); - assert_relative_eq!(distances[0], 2.0, epsilon = 1e-6); // 2 units above first line - assert_relative_eq!(distances[1], 3.0, epsilon = 1e-6); // 3 units below second line - } - - #[test] - fn test_distances_to_point_simd() { - // Test with more than 4 lines to exercise SIMD path - let mut lines = Vec::new(); - for i in 0..8 { - lines.push(Line2D::new( - Point2D::new(0.0, i as f32), - Point2D::new(10.0, i as f32), - )); - } - - let collection = LineCollection::from_lines(&lines); - let point = Point2D::new(5.0, 3.5); - - let distances = collection.distances_to_point(point); - - assert_eq!(distances.len(), 8); - assert_relative_eq!(distances[3], 0.5, epsilon = 1e-6); // Closest to y=3 line - assert_relative_eq!(distances[4], 0.5, epsilon = 1e-6); // Equal distance to y=4 line - } - - #[test] - fn test_nearest_line() { - let lines = vec![ - Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(10.0, 0.0)), - Line2D::new(Point2D::new(0.0, 5.0), Point2D::new(10.0, 5.0)), - Line2D::new(Point2D::new(0.0, 2.0), Point2D::new(10.0, 2.0)), - ]; - - let collection = LineCollection::from_lines(&lines); - let point = Point2D::new(5.0, 1.8); - - let (idx, dist) = collection.nearest_line(point).unwrap(); - - assert_eq!(idx, 2); // y=2 line is nearest - assert_relative_eq!(dist, 0.2, epsilon = 1e-6); - } - - #[test] - fn test_transform() { - let lines = vec![Line2D::new(Point2D::new(1.0, 0.0), Point2D::new(2.0, 0.0))]; - - let collection = LineCollection::from_lines(&lines); - - // Rotate 90 degrees - let pose = Pose2D::new(0.0, 0.0, FRAC_PI_2); - let transformed = collection.transform(&pose); - - assert_relative_eq!(transformed.start_xs[0], 0.0, epsilon = 1e-6); - assert_relative_eq!(transformed.start_ys[0], 1.0, epsilon = 1e-6); - assert_relative_eq!(transformed.end_xs[0], 0.0, epsilon = 1e-6); - assert_relative_eq!(transformed.end_ys[0], 2.0, epsilon = 1e-6); - } - - #[test] - fn test_transform_simd() { - // Test with more than 4 lines - let mut lines = Vec::new(); - for i in 0..8 { - lines.push(Line2D::new( - Point2D::new(i as f32, 0.0), - Point2D::new(i as f32 + 1.0, 0.0), - )); - } - - let collection = LineCollection::from_lines(&lines); - let pose = Pose2D::new(10.0, 20.0, 0.0); // Just translation - - let transformed = collection.transform(&pose); - - for i in 0..8 { - assert_relative_eq!(transformed.start_xs[i], i as f32 + 10.0, epsilon = 1e-5); - assert_relative_eq!(transformed.start_ys[i], 20.0, epsilon = 1e-5); - } - } - - #[test] - fn test_swap_remove() { - let lines = vec![ - Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(1.0, 0.0)), - Line2D::new(Point2D::new(0.0, 1.0), Point2D::new(1.0, 1.0)), - Line2D::new(Point2D::new(0.0, 2.0), Point2D::new(1.0, 2.0)), - ]; - - let mut collection = LineCollection::from_lines(&lines); - collection.swap_remove(0); - - assert_eq!(collection.len(), 2); - // First element is now the last one (swap-remove) - assert_eq!(collection.start_ys[0], 2.0); - assert_eq!(collection.start_ys[1], 1.0); - } - - #[test] - fn test_iter() { - let lines = vec![ - Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(1.0, 0.0)), - Line2D::new(Point2D::new(0.0, 1.0), Point2D::new(1.0, 1.0)), - ]; - - let collection = LineCollection::from_lines(&lines); - let collected: Vec<_> = collection.iter().collect(); - - assert_eq!(collected.len(), 2); - assert_eq!(collected[0].start, Point2D::new(0.0, 0.0)); - assert_eq!(collected[1].start, Point2D::new(0.0, 1.0)); - } - - #[test] - fn test_distances_to_point_into() { - let lines = vec![ - Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(10.0, 0.0)), // Horizontal at y=0 - Line2D::new(Point2D::new(0.0, 5.0), Point2D::new(10.0, 5.0)), // Horizontal at y=5 - ]; - - let collection = LineCollection::from_lines(&lines); - let point = Point2D::new(5.0, 2.0); - - // Test buffer reuse variant - let mut buffer = vec![0.0; 10]; // Oversized buffer - collection.distances_to_point_into(point, &mut buffer); - - assert_relative_eq!(buffer[0], 2.0, epsilon = 1e-6); - assert_relative_eq!(buffer[1], 3.0, epsilon = 1e-6); - - // Verify matches allocating version - let distances = collection.distances_to_point(point); - assert_relative_eq!(buffer[0], distances[0], epsilon = 1e-6); - assert_relative_eq!(buffer[1], distances[1], epsilon = 1e-6); - } - - #[test] - fn test_nearest_line_with_buffer() { - let lines = vec![ - Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(10.0, 0.0)), - Line2D::new(Point2D::new(0.0, 5.0), Point2D::new(10.0, 5.0)), - Line2D::new(Point2D::new(0.0, 2.0), Point2D::new(10.0, 2.0)), - ]; - - let collection = LineCollection::from_lines(&lines); - let point = Point2D::new(5.0, 1.8); - - // Test buffer reuse variant - let mut buffer = Vec::new(); - let (idx, dist) = collection - .nearest_line_with_buffer(point, &mut buffer) - .unwrap(); - - assert_eq!(idx, 2); // y=2 line is nearest - assert_relative_eq!(dist, 0.2, epsilon = 1e-6); - - // Buffer should be reused on subsequent calls - let (idx2, dist2) = collection - .nearest_line_with_buffer(point, &mut buffer) - .unwrap(); - assert_eq!(idx, idx2); - assert_relative_eq!(dist, dist2, epsilon = 1e-6); - - // Verify matches allocating version - let (idx_alloc, dist_alloc) = collection.nearest_line(point).unwrap(); - assert_eq!(idx, idx_alloc); - assert_relative_eq!(dist, dist_alloc, epsilon = 1e-6); - } - - #[test] - fn test_distances_to_point_into_simd() { - // Test with more than 4 lines to exercise SIMD path - let mut lines = Vec::new(); - for i in 0..8 { - lines.push(Line2D::new( - Point2D::new(0.0, i as f32), - Point2D::new(10.0, i as f32), - )); - } - - let collection = LineCollection::from_lines(&lines); - let point = Point2D::new(5.0, 3.5); - - let mut buffer = vec![0.0; 8]; - collection.distances_to_point_into(point, &mut buffer); - - // Verify matches allocating version - let distances = collection.distances_to_point(point); - for i in 0..8 { - assert_relative_eq!(buffer[i], distances[i], epsilon = 1e-6); - } - } -} diff --git a/vastu-map/src/features/mod.rs b/vastu-map/src/features/mod.rs deleted file mode 100644 index 3eecd1a..0000000 --- a/vastu-map/src/features/mod.rs +++ /dev/null @@ -1,70 +0,0 @@ -//! Geometric feature types for VectorMap SLAM. -//! -//! This module provides the core feature representations used throughout vastu-map: -//! -//! - [`Line2D`]: Line segment defined by endpoints -//! - [`LineCollection`]: SoA collection for SIMD batch operations -//! - [`Corner2D`]: Corner feature at line intersections -//! - [`CornerCollection`]: SoA collection for corners -//! - [`FeatureSet`]: Combined container for lines and corners -//! - [`CornerDescriptor`]: Shape context descriptor for loop closure -//! - [`ScanDescriptor`]: Aggregate scan descriptor for place recognition -//! -//! # Design Rationale -//! -//! ## Why Lines Instead of Points? -//! -//! Indoor environments are dominated by straight edges (walls, furniture, doorframes). -//! Using line segments as the primary feature type provides: -//! -//! - **Compact representation**: One line replaces dozens of points -//! - **Robust matching**: Lines survive partial occlusion and noise -//! - **Meaningful geometry**: Lines encode structure, not just shape -//! - **Efficient ICP**: Point-to-line ICP converges faster than point-to-point -//! -//! ## Why Endpoints, Not Parametric Form? -//! -//! Lines use endpoint representation `(start, end)` instead of parametric `(rho, theta)`: -//! -//! - **Simpler transforms**: Just rotate+translate two points -//! - **SIMD-friendly**: 4 floats per line, no trig during transform -//! - **No angle wrapping**: No discontinuity at ±π -//! - **Built-in bounds**: Segment extent is implicit in endpoints -//! -//! ## Why Corners? -//! -//! Corners (line intersections) provide: -//! -//! - **Reliable landmarks**: Corners are stable across viewpoints -//! - **Loop closure signatures**: Corner patterns are distinctive -//! - **Pose constraints**: Two corners fully constrain 2D pose -//! -//! # Data Layout -//! -//! Feature collections use Struct-of-Arrays (SoA) for SIMD efficiency: -//! -//! ```text -//! AoS (Array of Structs): SoA (Struct of Arrays): -//! [Line{x1,y1,x2,y2}] start_xs: [x1, x1, x1, ...] -//! [Line{x1,y1,x2,y2}] → start_ys: [y1, y1, y1, ...] -//! [Line{x1,y1,x2,y2}] end_xs: [x2, x2, x2, ...] -//! end_ys: [y2, y2, y2, ...] -//! ``` -//! -//! SoA enables processing 4 lines per SIMD instruction. - -pub mod corner; -pub mod descriptors; -pub mod feature_set; -pub mod line; -pub mod line_collection; -pub mod spatial_index; - -pub use corner::{Corner2D, CornerCollection}; -pub use descriptors::{CornerDescriptor, ScanDescriptor}; -pub use feature_set::FeatureSet; -pub use line::Line2D; -pub use line_collection::LineCollection; -pub use spatial_index::{ - LineSpatialIndex, MIN_CELL_SIZE, MIN_LINES_FOR_INDEX, should_use_spatial_index, -}; diff --git a/vastu-map/src/features/spatial_index.rs b/vastu-map/src/features/spatial_index.rs deleted file mode 100644 index fe46354..0000000 --- a/vastu-map/src/features/spatial_index.rs +++ /dev/null @@ -1,482 +0,0 @@ -//! Grid-based spatial index for fast line proximity queries. -//! -//! This module provides O(1) average-case spatial lookup for line segments, -//! reducing ICP correspondence search from O(n×m) to O(n×k) where k is the -//! average number of lines per grid cell. -//! -//! # Algorithm -//! -//! 1. Divide 2D space into uniform grid cells of size `cell_size` -//! 2. Each line is stored in all cells its bounding box overlaps -//! 3. Point queries return line indices from the cell containing the point -//! plus neighboring cells (to handle lines near cell boundaries) -//! -//! # Performance -//! -//! - Build: O(m) where m = number of lines -//! - Query: O(k) where k = average lines per cell (~10-20 for typical indoor maps) -//! - Memory: O(m × overlap_factor) where overlap_factor ≈ 2-4 for typical line lengths -//! -//! # When to Use -//! -//! Spatial indexing is only beneficial for larger maps. For small rooms (< 4m), -//! the overhead of building and querying the index exceeds brute-force search. -//! Use `should_use_spatial_index()` to check if indexing is worthwhile. - -use std::collections::HashMap; - -use super::LineCollection; -use crate::core::Point2D; - -/// Minimum cell size in meters. -/// For typical indoor SLAM with ~4m rooms, cells smaller than this create -/// unnecessary overhead without improving query performance. -pub const MIN_CELL_SIZE: f32 = 4.0; - -/// Minimum number of lines before spatial indexing is beneficial. -/// Below this threshold, brute-force O(n×m) is faster than index overhead. -pub const MIN_LINES_FOR_INDEX: usize = 30; - -/// Check if spatial indexing should be used for a given line collection. -/// -/// Returns `true` if the map is large enough to benefit from spatial indexing. -/// For small maps (< 30 lines or < 8m extent), brute-force is faster. -#[inline] -pub fn should_use_spatial_index(lines: &LineCollection) -> bool { - // Too few lines - brute force is faster - if lines.len() < MIN_LINES_FOR_INDEX { - return false; - } - - // Check if map extent is large enough to benefit from spatial indexing - // For a 4x4m room with 4m cells, we'd have just 1 cell - not worth it - let (min_x, min_y, max_x, max_y) = lines_bounds(lines); - let extent_x = max_x - min_x; - let extent_y = max_y - min_y; - - // Need at least 2 cells in one dimension to benefit - extent_x > MIN_CELL_SIZE * 2.0 || extent_y > MIN_CELL_SIZE * 2.0 -} - -/// Compute bounds of a line collection. -fn lines_bounds(lines: &LineCollection) -> (f32, f32, f32, f32) { - if lines.is_empty() { - return (0.0, 0.0, 0.0, 0.0); - } - - let mut min_x = f32::MAX; - let mut min_y = f32::MAX; - let mut max_x = f32::MIN; - let mut max_y = f32::MIN; - - for i in 0..lines.len() { - min_x = min_x.min(lines.start_xs[i]).min(lines.end_xs[i]); - min_y = min_y.min(lines.start_ys[i]).min(lines.end_ys[i]); - max_x = max_x.max(lines.start_xs[i]).max(lines.end_xs[i]); - max_y = max_y.max(lines.start_ys[i]).max(lines.end_ys[i]); - } - - (min_x, min_y, max_x, max_y) -} - -/// Grid cell coordinate. -#[derive(Clone, Copy, Debug, PartialEq, Eq, Hash)] -struct CellCoord { - x: i32, - y: i32, -} - -impl CellCoord { - #[inline] - fn new(x: i32, y: i32) -> Self { - Self { x, y } - } - - /// Get all neighboring cells (including self) for a query radius. - #[inline] - fn neighbors(&self, radius: i32) -> impl Iterator + '_ { - let r = radius; - (-r..=r) - .flat_map(move |dx| (-r..=r).map(move |dy| CellCoord::new(self.x + dx, self.y + dy))) - } -} - -/// Grid-based spatial index for line segments. -/// -/// Provides fast spatial queries for finding lines near a point. -/// Used to accelerate ICP correspondence search. -/// -/// # Example -/// ```rust,ignore -/// use vastu_map::features::{LineCollection, LineSpatialIndex}; -/// -/// let lines = LineCollection::from_lines(&[...]); -/// let index = LineSpatialIndex::build(&lines, 0.5); // 0.5m cell size -/// -/// // Query for lines near a point (returns indices) -/// let candidates = index.query_radius(Point2D::new(1.0, 2.0), 0.3); -/// ``` -#[derive(Clone, Debug)] -pub struct LineSpatialIndex { - /// Grid cell size in meters. - cell_size: f32, - /// Inverse cell size for fast coordinate conversion. - inv_cell_size: f32, - /// Map from cell coordinate to line indices in that cell. - grid: HashMap>, - /// Total number of lines indexed. - num_lines: usize, - /// Bounds of indexed area. - min_x: f32, - min_y: f32, - max_x: f32, - max_y: f32, -} - -impl LineSpatialIndex { - /// Build a spatial index from a line collection. - /// - /// # Arguments - /// * `lines` - Line collection to index - /// * `cell_size` - Grid cell size in meters (will be clamped to MIN_CELL_SIZE minimum) - /// - /// # Performance - /// Larger cell sizes = fewer cells but more lines per cell. - /// Smaller cell sizes = more cells but fewer lines per cell. - /// For small rooms (~4m), use MIN_CELL_SIZE to avoid overhead. - pub fn build(lines: &LineCollection, cell_size: f32) -> Self { - // Enforce minimum cell size to avoid overhead for small maps - let cell_size = cell_size.max(MIN_CELL_SIZE); - let inv_cell_size = 1.0 / cell_size; - let num_lines = lines.len(); - - let mut grid: HashMap> = HashMap::new(); - let mut min_x = f32::MAX; - let mut min_y = f32::MAX; - let mut max_x = f32::MIN; - let mut max_y = f32::MIN; - - for i in 0..num_lines { - let sx = lines.start_xs[i]; - let sy = lines.start_ys[i]; - let ex = lines.end_xs[i]; - let ey = lines.end_ys[i]; - - // Update bounds - min_x = min_x.min(sx).min(ex); - min_y = min_y.min(sy).min(ey); - max_x = max_x.max(sx).max(ex); - max_y = max_y.max(sy).max(ey); - - // Compute bounding box cells - let cell_min_x = (sx.min(ex) * inv_cell_size).floor() as i32; - let cell_min_y = (sy.min(ey) * inv_cell_size).floor() as i32; - let cell_max_x = (sx.max(ex) * inv_cell_size).floor() as i32; - let cell_max_y = (sy.max(ey) * inv_cell_size).floor() as i32; - - // Insert line into all overlapping cells - for cx in cell_min_x..=cell_max_x { - for cy in cell_min_y..=cell_max_y { - let coord = CellCoord::new(cx, cy); - grid.entry(coord).or_default().push(i); - } - } - } - - Self { - cell_size, - inv_cell_size, - grid, - num_lines, - min_x, - min_y, - max_x, - max_y, - } - } - - /// Build with automatic cell size based on query radius. - /// - /// Uses cell_size = 2 × query_radius for optimal performance. - #[inline] - pub fn build_for_radius(lines: &LineCollection, query_radius: f32) -> Self { - Self::build(lines, query_radius * 2.0) - } - - /// Query lines near a point within a radius. - /// - /// Returns indices of lines whose bounding boxes are within `radius` of the point. - /// The actual distances should still be computed for precise filtering. - /// - /// # Arguments - /// * `point` - Query point - /// * `radius` - Search radius in meters - /// - /// # Returns - /// Vector of line indices that may be within radius (sorted and deduplicated). - #[inline] - pub fn query_radius(&self, point: Point2D, radius: f32) -> Vec { - let mut result = Vec::new(); - self.query_radius_into(point, radius, &mut result); - result - } - - /// Query lines near a point, returning results in a reusable buffer. - /// - /// This is the zero-allocation version for hot paths. - /// The buffer is cleared and filled with candidate line indices. - /// - /// # Arguments - /// * `point` - Query point - /// * `radius` - Search radius in meters - /// * `buffer` - Reusable buffer for results (will be cleared and sorted) - /// - /// # Note - /// Results are sorted and deduplicated to handle lines spanning multiple cells. - pub fn query_radius_into(&self, point: Point2D, radius: f32, buffer: &mut Vec) { - buffer.clear(); - - let cell = self.point_to_cell(point); - let cell_radius = (radius * self.inv_cell_size).ceil() as i32; - - // Collect all candidates - for neighbor in cell.neighbors(cell_radius) { - if let Some(indices) = self.grid.get(&neighbor) { - buffer.extend(indices.iter().copied()); - } - } - - // Sort and deduplicate (lines can appear in multiple cells) - if buffer.len() > 1 { - buffer.sort_unstable(); - buffer.dedup(); - } - } - - /// Convert a point to its grid cell coordinate. - #[inline] - fn point_to_cell(&self, point: Point2D) -> CellCoord { - CellCoord::new( - (point.x * self.inv_cell_size).floor() as i32, - (point.y * self.inv_cell_size).floor() as i32, - ) - } - - /// Get the number of lines in the index. - #[inline] - pub fn len(&self) -> usize { - self.num_lines - } - - /// Check if the index is empty. - #[inline] - pub fn is_empty(&self) -> bool { - self.num_lines == 0 - } - - /// Get the cell size used by this index. - #[inline] - pub fn cell_size(&self) -> f32 { - self.cell_size - } - - /// Get the number of non-empty cells. - #[inline] - pub fn num_cells(&self) -> usize { - self.grid.len() - } - - /// Get the average number of lines per cell. - pub fn avg_lines_per_cell(&self) -> f32 { - if self.grid.is_empty() { - return 0.0; - } - let total: usize = self.grid.values().map(|v| v.len()).sum(); - total as f32 / self.grid.len() as f32 - } - - /// Get the bounds of the indexed area. - #[inline] - pub fn bounds(&self) -> (f32, f32, f32, f32) { - (self.min_x, self.min_y, self.max_x, self.max_y) - } -} - -impl Default for LineSpatialIndex { - fn default() -> Self { - Self { - cell_size: 0.5, - inv_cell_size: 2.0, - grid: HashMap::new(), - num_lines: 0, - min_x: 0.0, - min_y: 0.0, - max_x: 0.0, - max_y: 0.0, - } - } -} - -#[cfg(test)] -mod tests { - use super::*; - use crate::features::Line2D; - - fn make_grid_lines() -> LineCollection { - // Create a 3x3 grid of horizontal lines - let mut lines = Vec::new(); - for y in 0..3 { - lines.push(Line2D::new( - Point2D::new(0.0, y as f32), - Point2D::new(3.0, y as f32), - )); - } - // Add some vertical lines - for x in 0..3 { - lines.push(Line2D::new( - Point2D::new(x as f32, 0.0), - Point2D::new(x as f32, 3.0), - )); - } - LineCollection::from_lines(&lines) - } - - #[test] - fn test_build_empty() { - let lines = LineCollection::new(); - let index = LineSpatialIndex::build(&lines, 0.5); - - assert!(index.is_empty()); - assert_eq!(index.num_cells(), 0); - } - - #[test] - fn test_build_single_line() { - let lines = LineCollection::from_lines(&[Line2D::new( - Point2D::new(0.0, 0.0), - Point2D::new(1.0, 0.0), - )]); - - let index = LineSpatialIndex::build(&lines, 0.5); - - assert_eq!(index.len(), 1); - assert!(!index.is_empty()); - } - - #[test] - fn test_query_finds_nearby_lines() { - let lines = make_grid_lines(); - let index = LineSpatialIndex::build(&lines, 0.5); - - // Query near origin - should find lines at y=0 and x=0 - let candidates = index.query_radius(Point2D::new(0.1, 0.1), 0.5); - - assert!(!candidates.is_empty()); - // Should contain at least the horizontal line at y=0 and vertical line at x=0 - } - - #[test] - fn test_query_radius_into() { - let lines = make_grid_lines(); - let index = LineSpatialIndex::build(&lines, 0.5); - - let mut buffer = Vec::new(); - index.query_radius_into(Point2D::new(0.1, 0.1), 0.5, &mut buffer); - - assert!(!buffer.is_empty()); - // Results should be sorted and unique - for i in 1..buffer.len() { - assert!( - buffer[i] > buffer[i - 1], - "Buffer should be sorted and unique" - ); - } - } - - #[test] - fn test_query_no_results_far_away() { - let lines = LineCollection::from_lines(&[Line2D::new( - Point2D::new(0.0, 0.0), - Point2D::new(1.0, 0.0), - )]); - - let index = LineSpatialIndex::build(&lines, 0.5); - - // Query far from the line - let candidates = index.query_radius(Point2D::new(100.0, 100.0), 0.5); - - assert!(candidates.is_empty()); - } - - #[test] - fn test_line_in_multiple_cells() { - // Long line spanning multiple cells - let lines = LineCollection::from_lines(&[Line2D::new( - Point2D::new(0.0, 0.0), - Point2D::new(5.0, 0.0), - )]); - - let index = LineSpatialIndex::build(&lines, 0.5); - - // Line should be found from any cell along its length - for x in 0..5 { - let candidates = index.query_radius(Point2D::new(x as f32 + 0.25, 0.0), 0.3); - assert!(candidates.contains(&0), "Line should be found at x={}", x); - } - } - - #[test] - fn test_build_for_radius() { - let lines = make_grid_lines(); - let index = LineSpatialIndex::build_for_radius(&lines, 0.25); - - // Cell size should be at least MIN_CELL_SIZE (4.0m) - // Even though 2 × 0.25 = 0.5, it's clamped to MIN_CELL_SIZE - assert!((index.cell_size() - MIN_CELL_SIZE).abs() < 0.01); - } - - #[test] - fn test_should_use_spatial_index_small_map() { - // Small map with few lines - should NOT use spatial index - let lines = make_grid_lines(); // 6 lines in 3x3m area - assert!(!should_use_spatial_index(&lines)); - } - - #[test] - fn test_should_use_spatial_index_large_map() { - // Large map with many lines - should use spatial index - let mut lines = LineCollection::new(); - for i in 0..50 { - let y = i as f32 * 0.5; - lines.push_full(0.0, y, 20.0, y, 1, 10); // 20m wide lines - } - assert!(should_use_spatial_index(&lines)); - } - - #[test] - fn test_avg_lines_per_cell() { - let lines = make_grid_lines(); - let index = LineSpatialIndex::build(&lines, 0.5); - - let avg = index.avg_lines_per_cell(); - assert!(avg > 0.0); - // With 6 lines (3 horizontal + 3 vertical) over a 3x3 area, - // average should be reasonable - } - - #[test] - fn test_bounds() { - let lines = LineCollection::from_lines(&[Line2D::new( - Point2D::new(-1.0, -2.0), - Point2D::new(3.0, 4.0), - )]); - - let index = LineSpatialIndex::build(&lines, 0.5); - let (min_x, min_y, max_x, max_y) = index.bounds(); - - assert!(min_x <= -1.0); - assert!(min_y <= -2.0); - assert!(max_x >= 3.0); - assert!(max_y >= 4.0); - } -} diff --git a/vastu-map/src/grid/bumper_update.rs b/vastu-map/src/grid/bumper_update.rs new file mode 100644 index 0000000..e35fc42 --- /dev/null +++ b/vastu-map/src/grid/bumper_update.rs @@ -0,0 +1,267 @@ +//! Bumper sensor update for occupancy grid. +//! +//! Processes bumper collision events to mark cells as Bump. +//! Bump cells represent invisible obstacles (glass, mirrors, thin furniture legs) +//! that lidar cannot detect but the robot physically collides with. + +use crate::ObserveResult; +use crate::core::{BumperSensors, CellType, Pose2D, WorldPoint}; +use crate::grid::storage::GridStorage; + +/// Default robot radius for collision position estimation. +const DEFAULT_ROBOT_RADIUS: f32 = 0.17; + +/// Angular offset for left bumper (radians from forward). +const LEFT_BUMPER_ANGLE: f32 = 0.5; // ~30 degrees left + +/// Angular offset for right bumper (radians from forward). +const RIGHT_BUMPER_ANGLE: f32 = -0.5; // ~30 degrees right + +/// Update the grid with bumper collision readings. +/// +/// For each triggered bumper: +/// 1. Estimate collision position based on bumper zone +/// 2. Mark the cell at that position as Bump +/// +/// Bump has the highest priority (won't be overwritten by anything). +pub fn update_from_bumper( + storage: &mut GridStorage, + bumpers: &BumperSensors, + robot_pose: Pose2D, + robot_radius: f32, +) -> ObserveResult { + let mut result = ObserveResult::default(); + + if !bumpers.any_triggered() { + return result; + } + + // Determine collision angle(s) + let collision_angles: Vec = match (bumpers.left, bumpers.right) { + (true, true) => { + // Both bumpers - mark center and spread + vec![0.0, LEFT_BUMPER_ANGLE * 0.5, RIGHT_BUMPER_ANGLE * 0.5] + } + (true, false) => { + // Left bumper only + vec![LEFT_BUMPER_ANGLE] + } + (false, true) => { + // Right bumper only + vec![RIGHT_BUMPER_ANGLE] + } + (false, false) => { + return result; + } + }; + + // Mark cells at collision positions + for angle in collision_angles { + // Collision position in robot frame + let local_pos = WorldPoint::new(robot_radius * angle.cos(), robot_radius * angle.sin()); + + // Transform to world frame + let world_pos = robot_pose.transform_point(local_pos); + let coord = storage.world_to_grid(world_pos); + + if storage.is_valid_coord(coord) { + // Mark as Bump with highest priority + if storage.set_type_with_priority(coord, CellType::Bump) { + result.cells_bump += 1; + } + result.cells_updated += 1; + } + } + + result +} + +/// Update the grid with bumper collision using default robot radius. +pub fn update_from_bumper_default( + storage: &mut GridStorage, + bumpers: &BumperSensors, + robot_pose: Pose2D, +) -> ObserveResult { + update_from_bumper(storage, bumpers, robot_pose, DEFAULT_ROBOT_RADIUS) +} + +/// Mark a specific world position as a bump obstacle. +/// +/// Useful for manually marking invisible obstacles. +pub fn mark_bump_at(storage: &mut GridStorage, position: WorldPoint) -> bool { + let coord = storage.world_to_grid(position); + if storage.is_valid_coord(coord) { + storage.set_type_with_priority(coord, CellType::Bump) + } else { + false + } +} + +/// Mark an arc of cells as bump obstacles. +/// +/// Useful for marking a wider area when the exact collision point is uncertain. +pub fn mark_bump_arc( + storage: &mut GridStorage, + robot_pose: Pose2D, + robot_radius: f32, + center_angle: f32, + arc_width: f32, + num_samples: usize, +) -> usize { + let mut count = 0; + let half_arc = arc_width / 2.0; + let step = arc_width / (num_samples as f32 - 1.0).max(1.0); + + for i in 0..num_samples { + let angle = center_angle - half_arc + step * i as f32; + + let local_pos = WorldPoint::new(robot_radius * angle.cos(), robot_radius * angle.sin()); + + let world_pos = robot_pose.transform_point(local_pos); + let coord = storage.world_to_grid(world_pos); + + if storage.is_valid_coord(coord) && storage.set_type_with_priority(coord, CellType::Bump) { + count += 1; + } + } + + count +} + +#[cfg(test)] +mod tests { + use super::*; + + fn create_test_storage() -> GridStorage { + GridStorage::centered(100, 100, 0.05) // 5m x 5m at 5cm resolution + } + + #[test] + fn test_left_bumper_update() { + let mut storage = create_test_storage(); + let robot_pose = Pose2D::new(0.0, 0.0, 0.0); + + let bumpers = BumperSensors { + left: true, + right: false, + }; + + let result = update_from_bumper_default(&mut storage, &bumpers, robot_pose); + + assert!(result.cells_updated > 0); + assert!(result.cells_bump > 0); + + // Check that a cell in the left-front area is marked as Bump + let expected_pos = robot_pose.transform_point(WorldPoint::new( + DEFAULT_ROBOT_RADIUS * LEFT_BUMPER_ANGLE.cos(), + DEFAULT_ROBOT_RADIUS * LEFT_BUMPER_ANGLE.sin(), + )); + let coord = storage.world_to_grid(expected_pos); + assert_eq!(storage.get_type(coord), CellType::Bump); + } + + #[test] + fn test_right_bumper_update() { + let mut storage = create_test_storage(); + let robot_pose = Pose2D::new(0.0, 0.0, 0.0); + + let bumpers = BumperSensors { + left: false, + right: true, + }; + + let result = update_from_bumper_default(&mut storage, &bumpers, robot_pose); + + assert!(result.cells_updated > 0); + assert!(result.cells_bump > 0); + } + + #[test] + fn test_both_bumpers_update() { + let mut storage = create_test_storage(); + let robot_pose = Pose2D::new(0.0, 0.0, 0.0); + + let bumpers = BumperSensors { + left: true, + right: true, + }; + + let result = update_from_bumper_default(&mut storage, &bumpers, robot_pose); + + // Should mark multiple cells (center + sides) + assert!(result.cells_updated >= 3); + assert!(result.cells_bump >= 1); + } + + #[test] + fn test_no_bumper_triggered() { + let mut storage = create_test_storage(); + let robot_pose = Pose2D::new(0.0, 0.0, 0.0); + + let bumpers = BumperSensors::CLEAR; + + let result = update_from_bumper_default(&mut storage, &bumpers, robot_pose); + + assert_eq!(result.cells_updated, 0); + assert_eq!(result.cells_bump, 0); + } + + #[test] + fn test_bump_priority_over_wall() { + let mut storage = create_test_storage(); + let coord = storage.world_to_grid(WorldPoint::new(0.0, 0.0)); + + // First mark as Wall + storage.set_type_with_priority(coord, CellType::Wall); + assert_eq!(storage.get_type(coord), CellType::Wall); + + // Now mark as Bump - should override Wall + storage.set_type_with_priority(coord, CellType::Bump); + assert_eq!(storage.get_type(coord), CellType::Bump); + + // Try to mark as Wall again - should NOT override Bump + storage.set_type_with_priority(coord, CellType::Wall); + assert_eq!(storage.get_type(coord), CellType::Bump); + } + + #[test] + fn test_mark_bump_arc() { + let mut storage = create_test_storage(); + let robot_pose = Pose2D::new(0.0, 0.0, 0.0); + + // Mark a 60-degree arc centered at 0 (forward) + let count = mark_bump_arc( + &mut storage, + robot_pose, + 0.17, + 0.0, // center angle + std::f32::consts::FRAC_PI_3, // 60 degrees + 5, // 5 samples + ); + + assert!(count > 0); + + // Check that center is marked + let center_pos = robot_pose.transform_point(WorldPoint::new(0.17, 0.0)); + let coord = storage.world_to_grid(center_pos); + assert_eq!(storage.get_type(coord), CellType::Bump); + } + + #[test] + fn test_rotated_robot_bumper() { + let mut storage = create_test_storage(); + // Robot rotated 90 degrees (facing +Y) + let robot_pose = Pose2D::new(0.0, 0.0, std::f32::consts::FRAC_PI_2); + + let bumpers = BumperSensors { + left: true, + right: false, + }; + + let result = update_from_bumper_default(&mut storage, &bumpers, robot_pose); + assert!(result.cells_bump > 0); + + // The bump should be in a different world position due to rotation + // Left bumper on a robot facing +Y should be in the +X direction + } +} diff --git a/vastu-map/src/grid/cliff_update.rs b/vastu-map/src/grid/cliff_update.rs new file mode 100644 index 0000000..8d45e69 --- /dev/null +++ b/vastu-map/src/grid/cliff_update.rs @@ -0,0 +1,208 @@ +//! Cliff sensor update for occupancy grid. +//! +//! Processes cliff sensor readings to mark cells as Cliff. +//! Cliff cells are floor drop-offs (stairs, ledges) that the robot cannot traverse. + +use crate::ObserveResult; +use crate::core::{CellType, CliffSensors, Pose2D, WorldPoint}; +use crate::grid::storage::GridStorage; + +/// Cliff sensor positions in robot frame (from CRL-200S configuration). +/// These are the physical locations of the 4 cliff sensors on the robot. +const CLIFF_SENSOR_POSITIONS: [(f32, f32); 4] = [ + (0.12, 0.10), // left_side + (0.15, 0.05), // left_front + (0.15, -0.05), // right_front + (0.12, -0.10), // right_side +]; + +/// Update the grid with cliff sensor readings. +/// +/// For each triggered cliff sensor: +/// 1. Transform sensor position to world coordinates +/// 2. Mark the cell at that position as Cliff +/// +/// Cliff has higher priority than Floor (won't be overwritten by lidar). +pub fn update_from_cliff( + storage: &mut GridStorage, + cliffs: &CliffSensors, + robot_pose: Pose2D, +) -> ObserveResult { + let mut result = ObserveResult::default(); + + // Process each sensor + let sensors = [ + (cliffs.left_side, CLIFF_SENSOR_POSITIONS[0]), + (cliffs.left_front, CLIFF_SENSOR_POSITIONS[1]), + (cliffs.right_front, CLIFF_SENSOR_POSITIONS[2]), + (cliffs.right_side, CLIFF_SENSOR_POSITIONS[3]), + ]; + + for (triggered, (x, y)) in sensors { + if triggered { + // Transform sensor position to world frame + let sensor_pos = robot_pose.transform_point(WorldPoint::new(x, y)); + let coord = storage.world_to_grid(sensor_pos); + + if storage.is_valid_coord(coord) { + // Mark as Cliff with priority (won't be overwritten by Floor) + if storage.set_type_with_priority(coord, CellType::Cliff) { + result.cells_cliff += 1; + } + result.cells_updated += 1; + } + } + } + + result +} + +/// Mark a specific world position as a cliff. +/// +/// Useful for manually marking cliff areas or integrating with other sensors. +pub fn mark_cliff_at(storage: &mut GridStorage, position: WorldPoint) -> bool { + let coord = storage.world_to_grid(position); + if storage.is_valid_coord(coord) { + storage.set_type_with_priority(coord, CellType::Cliff) + } else { + false + } +} + +/// Mark a circular area as cliff. +/// +/// Useful for marking a safety zone around detected cliffs. +pub fn mark_cliff_radius(storage: &mut GridStorage, center: WorldPoint, radius: f32) -> usize { + let resolution = storage.resolution(); + let cells_radius = (radius / resolution).ceil() as i32; + let center_coord = storage.world_to_grid(center); + let mut count = 0; + + for dy in -cells_radius..=cells_radius { + for dx in -cells_radius..=cells_radius { + let coord = center_coord + crate::core::GridCoord::new(dx, dy); + + if !storage.is_valid_coord(coord) { + continue; + } + + // Check if within circular radius + let cell_center = storage.grid_to_world(coord); + if cell_center.distance(¢er) <= radius + && storage.set_type_with_priority(coord, CellType::Cliff) + { + count += 1; + } + } + } + + count +} + +#[cfg(test)] +mod tests { + use super::*; + + fn create_test_storage() -> GridStorage { + GridStorage::centered(100, 100, 0.05) // 5m x 5m at 5cm resolution + } + + #[test] + fn test_cliff_sensor_update() { + let mut storage = create_test_storage(); + let robot_pose = Pose2D::new(0.0, 0.0, 0.0); + + // Trigger left_front cliff sensor + let cliffs = CliffSensors { + left_side: false, + left_front: true, + right_front: false, + right_side: false, + }; + + let result = update_from_cliff(&mut storage, &cliffs, robot_pose); + + assert_eq!(result.cells_updated, 1); + assert_eq!(result.cells_cliff, 1); + + // Check that the cell at the sensor position is marked as Cliff + let sensor_pos = robot_pose.transform_point(WorldPoint::new(0.15, 0.05)); + let coord = storage.world_to_grid(sensor_pos); + assert_eq!(storage.get_type(coord), CellType::Cliff); + } + + #[test] + fn test_multiple_cliff_sensors() { + let mut storage = create_test_storage(); + let robot_pose = Pose2D::new(0.0, 0.0, 0.0); + + // Trigger both front cliff sensors + let cliffs = CliffSensors { + left_side: false, + left_front: true, + right_front: true, + right_side: false, + }; + + let result = update_from_cliff(&mut storage, &cliffs, robot_pose); + + assert_eq!(result.cells_updated, 2); + assert_eq!(result.cells_cliff, 2); + } + + #[test] + fn test_cliff_priority_over_floor() { + let mut storage = create_test_storage(); + let coord = storage.world_to_grid(WorldPoint::new(0.0, 0.0)); + + // First mark as Floor + storage.set_type(coord, CellType::Floor); + assert_eq!(storage.get_type(coord), CellType::Floor); + + // Now mark as Cliff - should override + storage.set_type_with_priority(coord, CellType::Cliff); + assert_eq!(storage.get_type(coord), CellType::Cliff); + + // Try to mark as Floor again - should NOT override + storage.set_type_with_priority(coord, CellType::Floor); + assert_eq!(storage.get_type(coord), CellType::Cliff); + } + + #[test] + fn test_mark_cliff_radius() { + let mut storage = create_test_storage(); + let center = WorldPoint::new(0.0, 0.0); + let radius = 0.1; // 10cm radius + + let count = mark_cliff_radius(&mut storage, center, radius); + + // Should mark multiple cells in a circle + assert!(count > 0); + + // Check that center is marked + let center_coord = storage.world_to_grid(center); + assert_eq!(storage.get_type(center_coord), CellType::Cliff); + } + + #[test] + fn test_rotated_robot() { + let mut storage = create_test_storage(); + // Robot rotated 90 degrees (facing +Y) + let robot_pose = Pose2D::new(0.0, 0.0, std::f32::consts::FRAC_PI_2); + + let cliffs = CliffSensors { + left_side: false, + left_front: true, + right_front: false, + right_side: false, + }; + + let result = update_from_cliff(&mut storage, &cliffs, robot_pose); + assert_eq!(result.cells_cliff, 1); + + // Sensor should be in a different world position due to rotation + let expected_pos = robot_pose.transform_point(WorldPoint::new(0.15, 0.05)); + let coord = storage.world_to_grid(expected_pos); + assert_eq!(storage.get_type(coord), CellType::Cliff); + } +} diff --git a/vastu-map/src/grid/config.rs b/vastu-map/src/grid/config.rs new file mode 100644 index 0000000..cf902b3 --- /dev/null +++ b/vastu-map/src/grid/config.rs @@ -0,0 +1,205 @@ +//! Configuration types for the occupancy grid. + +use crate::core::WorldPoint; +use serde::{Deserialize, Serialize}; + +/// Grid configuration +#[derive(Clone, Debug, Serialize, Deserialize)] +pub struct GridConfig { + /// Meters per cell (e.g., 0.025 = 2.5cm cells) + pub resolution: f32, + + /// Initial grid width in cells + pub initial_width: usize, + + /// Initial grid height in cells + pub initial_height: usize, + + /// World coordinates of cell (0,0) corner + /// If None, grid will be centered at origin + pub origin: Option, + + /// Enable dynamic grid expansion when robot moves beyond bounds + pub auto_expand: bool, + + /// Maximum grid width in cells (to limit memory) + pub max_width: usize, + + /// Maximum grid height in cells (to limit memory) + pub max_height: usize, +} + +impl Default for GridConfig { + fn default() -> Self { + Self { + resolution: 0.025, // 2.5cm cells + initial_width: 800, // 20m at 2.5cm + initial_height: 800, // 20m at 2.5cm + origin: None, // Centered at origin + auto_expand: true, + max_width: 2000, // 50m max + max_height: 2000, // 50m max + } + } +} + +impl GridConfig { + /// Create a configuration for a specific area size (in meters) + pub fn for_area(width_m: f32, height_m: f32, resolution: f32) -> Self { + let width = (width_m / resolution).ceil() as usize; + let height = (height_m / resolution).ceil() as usize; + + Self { + resolution, + initial_width: width, + initial_height: height, + origin: None, + auto_expand: true, + max_width: width * 2, + max_height: height * 2, + } + } + + /// Calculate the origin for a centered grid + pub fn centered_origin(&self) -> WorldPoint { + let half_width = (self.initial_width as f32 * self.resolution) / 2.0; + let half_height = (self.initial_height as f32 * self.resolution) / 2.0; + WorldPoint::new(-half_width, -half_height) + } + + /// Get the effective origin (uses centered_origin if origin is None) + pub fn effective_origin(&self) -> WorldPoint { + self.origin.unwrap_or_else(|| self.centered_origin()) + } + + /// Calculate memory usage in bytes for the initial grid + pub fn initial_memory_bytes(&self) -> usize { + // Each Cell is 4 bytes (CellType + confidence + observation_count + swept) + self.initial_width * self.initial_height * 4 + } + + /// Calculate maximum memory usage in bytes + pub fn max_memory_bytes(&self) -> usize { + self.max_width * self.max_height * 4 + } +} + +/// Sensor configuration for grid updates +#[derive(Clone, Debug, Serialize, Deserialize)] +pub struct SensorConfig { + /// Robot radius in meters (for obstacle inflation) + pub robot_radius: f32, + + /// Lidar mounting offset from robot center (x, y) in robot frame + pub lidar_offset: WorldPoint, + + /// How many observations needed for a cell to be "confident" + pub confidence_threshold: u8, + + /// Maximum lidar range to trust (meters) + pub max_lidar_range: f32, + + /// Minimum lidar range to trust (meters) + pub min_lidar_range: f32, +} + +impl Default for SensorConfig { + fn default() -> Self { + Self { + robot_radius: 0.17, + lidar_offset: WorldPoint::new(-0.110, 0.0), // From SangamIO CRL-200S config + confidence_threshold: 3, + max_lidar_range: 8.0, + min_lidar_range: 0.15, + } + } +} + +/// Full map configuration +#[derive(Clone, Debug, Default, Serialize, Deserialize)] +pub struct MapConfig { + /// Grid configuration (size, resolution, origin) + pub grid: GridConfig, + /// Sensor configuration (lidar, robot geometry) + pub sensor: SensorConfig, +} + +impl MapConfig { + /// Load configuration from a YAML file + pub fn from_yaml_file(path: &std::path::Path) -> Result { + let contents = + std::fs::read_to_string(path).map_err(|e| ConfigError::IoError(e.to_string()))?; + Self::from_yaml(&contents) + } + + /// Load configuration from a YAML string + pub fn from_yaml(yaml: &str) -> Result { + serde_yaml::from_str(yaml).map_err(|e| ConfigError::ParseError(e.to_string())) + } + + /// Save configuration to a YAML file + pub fn to_yaml_file(&self, path: &std::path::Path) -> Result<(), ConfigError> { + let yaml = self.to_yaml()?; + std::fs::write(path, yaml).map_err(|e| ConfigError::IoError(e.to_string())) + } + + /// Serialize to YAML string + pub fn to_yaml(&self) -> Result { + serde_yaml::to_string(self).map_err(|e| ConfigError::ParseError(e.to_string())) + } +} + +/// Configuration error type +#[derive(Debug, Clone)] +pub enum ConfigError { + /// File I/O error + IoError(String), + /// YAML parsing error + ParseError(String), +} + +impl std::fmt::Display for ConfigError { + fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result { + match self { + ConfigError::IoError(msg) => write!(f, "IO error: {}", msg), + ConfigError::ParseError(msg) => write!(f, "Parse error: {}", msg), + } + } +} + +impl std::error::Error for ConfigError {} + +#[cfg(test)] +mod tests { + use super::*; + + #[test] + fn test_default_config() { + let config = GridConfig::default(); + assert_eq!(config.resolution, 0.025); + assert_eq!(config.initial_width, 800); + } + + #[test] + fn test_for_area() { + let config = GridConfig::for_area(10.0, 10.0, 0.05); + assert_eq!(config.initial_width, 200); + assert_eq!(config.initial_height, 200); + } + + #[test] + fn test_memory_calculation() { + let config = GridConfig::default(); + let mem = config.initial_memory_bytes(); + // 800 * 800 * 4 = 2,560,000 bytes = 2.44 MB + assert_eq!(mem, 2560000); + } + + #[test] + fn test_yaml_serialization() { + let config = MapConfig::default(); + let yaml = config.to_yaml().unwrap(); + let parsed: MapConfig = MapConfig::from_yaml(&yaml).unwrap(); + assert_eq!(parsed.grid.resolution, config.grid.resolution); + } +} diff --git a/vastu-map/src/grid/lidar_update.rs b/vastu-map/src/grid/lidar_update.rs new file mode 100644 index 0000000..c803d3a --- /dev/null +++ b/vastu-map/src/grid/lidar_update.rs @@ -0,0 +1,284 @@ +//! Lidar sensor update for occupancy grid. +//! +//! Processes lidar scans to update the grid: +//! - Cells along rays are marked as Floor (free space) +//! - Cells at ray endpoints (within range) are marked as Wall (obstacle) + +use crate::ObserveResult; +use crate::core::{CellType, LidarScan, Pose2D, WorldPoint}; +use crate::grid::config::{GridConfig, SensorConfig}; +use crate::grid::raycaster::BresenhamLine; +use crate::grid::storage::GridStorage; + +/// Update the grid with a lidar scan. +/// +/// For each valid ray in the scan: +/// 1. Mark all cells along the ray as Floor (free space) +/// 2. If the ray hit something (range < max), mark the endpoint as Wall +/// +/// If `grid_config.auto_expand` is true, the grid will automatically +/// expand to include lidar endpoints that fall outside the current bounds. +/// +/// After updating cells, the distance field is updated for new wall cells +/// to support Gaussian scoring in scan matching. +/// +/// Returns the number of cells updated and statistics. +pub fn update_from_lidar( + storage: &mut GridStorage, + scan: &LidarScan, + robot_pose: Pose2D, + sensor_config: &SensorConfig, + grid_config: &GridConfig, +) -> ObserveResult { + use crate::core::GridCoord; + + let mut result = ObserveResult::default(); + + // Track newly added wall cells for distance field update + let mut new_wall_coords: Vec = Vec::new(); + + // Calculate sensor position in world frame + let sensor_pos = robot_pose.transform_point(sensor_config.lidar_offset); + let mut sensor_coord = storage.world_to_grid(sensor_pos); + + // Process each ray + for (&angle, &range) in scan.angles.iter().zip(scan.ranges.iter()) { + // Skip invalid ranges + if !scan.is_valid_range(range) { + continue; + } + + // Calculate world angle (sensor angle + robot heading) + let world_angle = angle + robot_pose.theta; + + // Calculate endpoint in world coordinates + let endpoint = WorldPoint::new( + sensor_pos.x + range * world_angle.cos(), + sensor_pos.y + range * world_angle.sin(), + ); + let endpoint_coord = storage.world_to_grid(endpoint); + + // Handle endpoint outside grid bounds + let mut endpoint_coord = endpoint_coord; + if !storage.is_valid_coord(endpoint_coord) { + if grid_config.auto_expand { + // Try to expand the grid to include this endpoint + if storage.expand_to_include( + endpoint, + grid_config.max_width, + grid_config.max_height, + ) { + // Grid expanded - recalculate coordinates since origin may have changed + sensor_coord = storage.world_to_grid(sensor_pos); + endpoint_coord = storage.world_to_grid(endpoint); + } else { + // Expansion failed (would exceed max size), skip this ray + continue; + } + } else { + // Auto-expand disabled, skip rays outside grid + continue; + } + } + + // Mark cells along the ray as Floor (iterate directly, no allocation) + let bresenham = BresenhamLine::new(sensor_coord, endpoint_coord); + + // Use peekable to detect the last element without collecting + let mut iter = bresenham.peekable(); + while let Some(coord) = iter.next() { + if !storage.is_valid_coord(coord) { + continue; + } + + let is_last = iter.peek().is_none(); + + if !is_last { + // Not the endpoint - mark as Floor + if storage.set_type(coord, CellType::Floor) { + result.cells_floor += 1; + } + result.cells_updated += 1; + } else { + // Endpoint - mark as Wall if within max range + if range < sensor_config.max_lidar_range { + if storage.set_type_with_priority(coord, CellType::Wall) { + result.cells_wall += 1; + new_wall_coords.push(coord); + } + result.cells_updated += 1; + } + } + } + } + + // Update distance field for newly added walls + if !new_wall_coords.is_empty() { + storage.update_distance_field_batch(&new_wall_coords); + } + + result +} + +/// Update the grid with a single lidar ray. +/// +/// This is a lower-level function useful for incremental updates. +pub fn update_single_ray( + storage: &mut GridStorage, + sensor_pos: WorldPoint, + angle: f32, + range: f32, + max_range: f32, +) -> (usize, bool) { + let sensor_coord = storage.world_to_grid(sensor_pos); + + // Calculate endpoint + let endpoint = WorldPoint::new( + sensor_pos.x + range * angle.cos(), + sensor_pos.y + range * angle.sin(), + ); + let endpoint_coord = storage.world_to_grid(endpoint); + + if !storage.is_valid_coord(endpoint_coord) { + return (0, false); + } + + let mut cells_updated = 0; + let mut hit_wall = false; + + // Trace ray (iterate directly, no allocation) + let bresenham = BresenhamLine::new(sensor_coord, endpoint_coord); + let mut iter = bresenham.peekable(); + + while let Some(coord) = iter.next() { + if !storage.is_valid_coord(coord) { + continue; + } + + let is_last = iter.peek().is_none(); + + if !is_last { + storage.set_type(coord, CellType::Floor); + } else if range < max_range { + storage.set_type_with_priority(coord, CellType::Wall); + hit_wall = true; + } + cells_updated += 1; + } + + (cells_updated, hit_wall) +} + +#[cfg(test)] +mod tests { + use super::*; + + fn create_test_storage() -> GridStorage { + GridStorage::centered(100, 100, 0.05) // 5m x 5m at 5cm resolution + } + + #[test] + fn test_single_ray_update() { + let mut storage = create_test_storage(); + let sensor_pos = WorldPoint::new(0.0, 0.0); + + // Cast a ray 1m forward + let (cells, hit) = update_single_ray(&mut storage, sensor_pos, 0.0, 1.0, 8.0); + + assert!(cells > 0); + assert!(hit); + + // Check that cells along the ray are Floor + let mid_coord = storage.world_to_grid(WorldPoint::new(0.5, 0.0)); + assert_eq!(storage.get_type(mid_coord), CellType::Floor); + + // Check that endpoint is Wall + let end_coord = storage.world_to_grid(WorldPoint::new(1.0, 0.0)); + assert_eq!(storage.get_type(end_coord), CellType::Wall); + } + + #[test] + fn test_lidar_scan_update() { + let mut storage = create_test_storage(); + let sensor_config = SensorConfig::default(); + let grid_config = GridConfig::default(); + let robot_pose = Pose2D::new(0.0, 0.0, 0.0); + + // Create a simple scan with 4 rays + let scan = LidarScan::new( + vec![1.0, 1.0, 1.0, 1.0], + vec![ + 0.0, + std::f32::consts::FRAC_PI_2, + std::f32::consts::PI, + -std::f32::consts::FRAC_PI_2, + ], + 0.15, + 8.0, + ); + + let result = update_from_lidar( + &mut storage, + &scan, + robot_pose, + &sensor_config, + &grid_config, + ); + + assert!(result.cells_updated > 0); + assert!(result.cells_wall > 0); + assert!(result.cells_floor > 0); + + // Check walls in 4 directions + // Note: sensor offset moves the lidar backwards + let check_points = [ + WorldPoint::new(1.0 - 0.110, 0.0), // Forward (adjusted for lidar offset) + ]; + + for point in &check_points { + let coord = storage.world_to_grid(*point); + // The endpoint should be a wall + assert!( + storage.get_type(coord) == CellType::Wall + || storage.get_type(coord) == CellType::Floor, + "Expected Wall or Floor at {:?}", + coord + ); + } + } + + #[test] + fn test_max_range_no_wall() { + let mut storage = create_test_storage(); + let sensor_pos = WorldPoint::new(0.0, 0.0); + + // Cast a ray at max range - should not mark wall + // Use 2.0m range to stay within the 5m grid + let (cells, hit) = update_single_ray(&mut storage, sensor_pos, 0.0, 2.0, 2.0); + + assert!(cells > 0); + assert!(!hit); // No wall at max range + } + + #[test] + fn test_diagonal_ray() { + let mut storage = create_test_storage(); + let sensor_pos = WorldPoint::new(0.0, 0.0); + + // Cast a diagonal ray + let angle = std::f32::consts::FRAC_PI_4; // 45 degrees + let (cells, hit) = update_single_ray(&mut storage, sensor_pos, angle, 1.0, 8.0); + + assert!(cells > 0); + assert!(hit); + + // Check diagonal endpoint + let dist = 1.0 / std::f32::consts::SQRT_2; + let end_coord = storage.world_to_grid(WorldPoint::new(dist, dist)); + // The actual endpoint might be slightly different due to resolution + assert!( + storage.get_type(end_coord) == CellType::Wall + || storage.get_type(end_coord) == CellType::Floor + ); + } +} diff --git a/vastu-map/src/grid/mod.rs b/vastu-map/src/grid/mod.rs new file mode 100644 index 0000000..ed271b8 --- /dev/null +++ b/vastu-map/src/grid/mod.rs @@ -0,0 +1,19 @@ +//! Occupancy grid implementation. +//! +//! This module provides the core occupancy grid storage and update mechanisms: +//! +//! - [`GridStorage`]: Core grid storage with coordinate conversion +//! - [`raycaster`]: Bresenham ray casting for lidar updates +//! - [`lidar_update`]: Process lidar scans to mark Floor/Wall cells +//! - [`cliff_update`]: Process cliff sensors to mark Cliff cells +//! - [`bumper_update`]: Process bumper collisions to mark Bump cells + +pub mod bumper_update; +pub mod cliff_update; +mod config; +pub mod lidar_update; +pub mod raycaster; +mod storage; + +pub use config::{ConfigError, GridConfig, MapConfig, SensorConfig}; +pub use storage::{CellCounts, GridStorage}; diff --git a/vastu-map/src/grid/raycaster.rs b/vastu-map/src/grid/raycaster.rs new file mode 100644 index 0000000..786a433 --- /dev/null +++ b/vastu-map/src/grid/raycaster.rs @@ -0,0 +1,309 @@ +//! Bresenham ray casting for occupancy grid updates. +//! +//! This module provides efficient line drawing algorithms for: +//! - Marking cells along lidar rays as free (Floor) +//! - Marking endpoint cells as occupied (Wall) + +use crate::core::{GridCoord, WorldPoint}; + +/// Bresenham's line algorithm iterator. +/// +/// Generates all grid cells along a line from start to end. +/// Uses the standard Bresenham algorithm for efficient integer arithmetic. +pub struct BresenhamLine { + x: i32, + y: i32, + dx: i32, + dy: i32, + x_inc: i32, + y_inc: i32, + error: i32, + steep: bool, + end_x: i32, + end_y: i32, + done: bool, +} + +impl BresenhamLine { + /// Create a new Bresenham line iterator from start to end coordinates. + pub fn new(start: GridCoord, end: GridCoord) -> Self { + let dx = (end.x - start.x).abs(); + let dy = (end.y - start.y).abs(); + let steep = dy > dx; + + let (x, y, end_x, end_y, dx, dy) = if steep { + (start.y, start.x, end.y, end.x, dy, dx) + } else { + (start.x, start.y, end.x, end.y, dx, dy) + }; + + let x_inc = if end_x > x { 1 } else { -1 }; + let y_inc = if end_y > y { 1 } else { -1 }; + + Self { + x, + y, + dx, + dy, + x_inc, + y_inc, + error: dx / 2, + steep, + end_x, + end_y, + done: false, + } + } + + /// Create a line iterator from world points given a resolution. + pub fn from_world( + start: WorldPoint, + end: WorldPoint, + origin: WorldPoint, + resolution: f32, + ) -> Self { + let start_coord = GridCoord::new( + ((start.x - origin.x) / resolution).floor() as i32, + ((start.y - origin.y) / resolution).floor() as i32, + ); + let end_coord = GridCoord::new( + ((end.x - origin.x) / resolution).floor() as i32, + ((end.y - origin.y) / resolution).floor() as i32, + ); + Self::new(start_coord, end_coord) + } +} + +impl Iterator for BresenhamLine { + type Item = GridCoord; + + fn next(&mut self) -> Option { + if self.done { + return None; + } + + let result = if self.steep { + GridCoord::new(self.y, self.x) + } else { + GridCoord::new(self.x, self.y) + }; + + // Check if we've reached the end + if self.x == self.end_x && self.y == self.end_y { + self.done = true; + return Some(result); + } + + // Advance to next cell + self.error -= self.dy; + if self.error < 0 { + self.y += self.y_inc; + self.error += self.dx; + } + self.x += self.x_inc; + + Some(result) + } +} + +/// Ray casting result for a single ray. +#[derive(Clone, Copy, Debug)] +pub struct RaycastResult { + /// Distance to hit (or max_range if no hit) + pub distance: f32, + /// Grid coordinate of hit cell (if any) + pub hit_coord: Option, + /// Did the ray hit an obstacle? + pub hit: bool, +} + +/// Cast a ray through the grid and return cells along the path. +/// +/// This is a more flexible version that allows custom handling of each cell. +/// Returns an iterator of (GridCoord, distance_from_origin). +pub struct RayCast { + /// Current position along the ray (world coordinates) + current: WorldPoint, + /// Direction unit vector + direction: WorldPoint, + /// Step size (typically resolution / 2 for accuracy) + step_size: f32, + /// Maximum range + max_range: f32, + /// Current distance traveled + distance: f32, + /// Grid origin + origin: WorldPoint, + /// Grid resolution + resolution: f32, + /// Last grid coordinate (to avoid duplicates) + last_coord: Option, +} + +impl RayCast { + /// Create a new ray cast from origin in a given direction. + pub fn new( + start: WorldPoint, + angle: f32, + max_range: f32, + grid_origin: WorldPoint, + resolution: f32, + ) -> Self { + let direction = WorldPoint::new(angle.cos(), angle.sin()); + // Step size is half the resolution for accuracy + let step_size = resolution * 0.5; + + Self { + current: start, + direction, + step_size, + max_range, + distance: 0.0, + origin: grid_origin, + resolution, + last_coord: None, + } + } + + /// Get the current world position + pub fn current_position(&self) -> WorldPoint { + self.current + } + + /// Get the current distance traveled + pub fn current_distance(&self) -> f32 { + self.distance + } +} + +impl Iterator for RayCast { + type Item = (GridCoord, f32); + + fn next(&mut self) -> Option { + loop { + if self.distance > self.max_range { + return None; + } + + // Calculate current grid coordinate + let coord = GridCoord::new( + ((self.current.x - self.origin.x) / self.resolution).floor() as i32, + ((self.current.y - self.origin.y) / self.resolution).floor() as i32, + ); + + // Advance position + self.current.x += self.direction.x * self.step_size; + self.current.y += self.direction.y * self.step_size; + self.distance += self.step_size; + + // Only yield if this is a new cell + if self.last_coord != Some(coord) { + self.last_coord = Some(coord); + return Some((coord, self.distance - self.step_size)); + } + } + } +} + +/// Collect all cells along a ray from start to end. +/// +/// Uses Bresenham's algorithm which is faster than stepping but +/// doesn't provide distance information. +pub fn cells_along_ray(start: GridCoord, end: GridCoord) -> Vec { + BresenhamLine::new(start, end).collect() +} + +/// Collect all cells along a ray, excluding the endpoint. +pub fn cells_along_ray_excluding_end(start: GridCoord, end: GridCoord) -> Vec { + let mut cells: Vec<_> = BresenhamLine::new(start, end).collect(); + if !cells.is_empty() { + cells.pop(); // Remove endpoint + } + cells +} + +#[cfg(test)] +mod tests { + use super::*; + + #[test] + fn test_bresenham_horizontal() { + let start = GridCoord::new(0, 0); + let end = GridCoord::new(5, 0); + let cells: Vec<_> = BresenhamLine::new(start, end).collect(); + + assert_eq!(cells.len(), 6); + assert_eq!(cells[0], GridCoord::new(0, 0)); + assert_eq!(cells[5], GridCoord::new(5, 0)); + } + + #[test] + fn test_bresenham_vertical() { + let start = GridCoord::new(0, 0); + let end = GridCoord::new(0, 5); + let cells: Vec<_> = BresenhamLine::new(start, end).collect(); + + assert_eq!(cells.len(), 6); + assert_eq!(cells[0], GridCoord::new(0, 0)); + assert_eq!(cells[5], GridCoord::new(0, 5)); + } + + #[test] + fn test_bresenham_diagonal() { + let start = GridCoord::new(0, 0); + let end = GridCoord::new(5, 5); + let cells: Vec<_> = BresenhamLine::new(start, end).collect(); + + assert_eq!(cells.len(), 6); + assert_eq!(cells[0], GridCoord::new(0, 0)); + assert_eq!(cells[5], GridCoord::new(5, 5)); + } + + #[test] + fn test_bresenham_negative() { + let start = GridCoord::new(5, 5); + let end = GridCoord::new(0, 0); + let cells: Vec<_> = BresenhamLine::new(start, end).collect(); + + assert_eq!(cells.len(), 6); + assert_eq!(cells[0], GridCoord::new(5, 5)); + assert_eq!(cells[5], GridCoord::new(0, 0)); + } + + #[test] + fn test_bresenham_steep() { + let start = GridCoord::new(0, 0); + let end = GridCoord::new(2, 5); + let cells: Vec<_> = BresenhamLine::new(start, end).collect(); + + // Should have 6 cells (max of dx, dy + 1) + assert_eq!(cells.len(), 6); + assert_eq!(cells[0], GridCoord::new(0, 0)); + assert_eq!(cells[5], GridCoord::new(2, 5)); + } + + #[test] + fn test_raycast_cells() { + let origin = WorldPoint::new(0.0, 0.0); + let resolution = 0.1; + + // Cast ray from (0.05, 0.05) at 0 degrees for 0.5m + let start = WorldPoint::new(0.05, 0.05); + let ray = RayCast::new(start, 0.0, 0.5, origin, resolution); + let cells: Vec<_> = ray.collect(); + + // Should hit multiple cells along X axis + assert!(!cells.is_empty()); + assert_eq!(cells[0].0, GridCoord::new(0, 0)); + } + + #[test] + fn test_cells_excluding_end() { + let start = GridCoord::new(0, 0); + let end = GridCoord::new(5, 0); + let cells = cells_along_ray_excluding_end(start, end); + + assert_eq!(cells.len(), 5); + assert!(!cells.contains(&end)); + } +} diff --git a/vastu-map/src/grid/storage.rs b/vastu-map/src/grid/storage.rs new file mode 100644 index 0000000..75a8e64 --- /dev/null +++ b/vastu-map/src/grid/storage.rs @@ -0,0 +1,1095 @@ +//! Grid storage implementation for the occupancy grid. +//! +//! Uses Structure-of-Arrays (SoA) layout for SIMD optimization. +//! Cell data is stored in separate arrays for each field, enabling +//! efficient vectorized operations on ARM NEON and x86 SSE/AVX. + +use crate::core::{Cell, CellType, GridCoord, WorldPoint}; +use std::simd::{cmp::SimdPartialEq, u8x16}; + +/// Grid storage using Structure-of-Arrays (SoA) layout. +/// +/// The grid uses a coordinate system where: +/// - (0, 0) is at `origin` in world coordinates +/// - Positive X is to the right +/// - Positive Y is up +/// - Cell (x, y) covers the area from (origin + x*resolution) to (origin + (x+1)*resolution) +/// +/// ## Memory Layout +/// +/// Each cell field is stored in a separate contiguous array: +/// ```text +/// cell_types: [T T T T T T T T T T T T T T T T|...] +/// confidences: [C C C C C C C C C C C C C C C C|...] +/// observation_counts: [O O O O O O O O O O O O O O O O|...] +/// swept_flags: [S S S S S S S S S S S S S S S S|...] +/// ``` +/// +/// This enables SIMD operations to process 16 cells at a time (u8x16). +#[derive(Clone, Debug)] +pub struct GridStorage { + // === SoA Data Arrays === + /// Cell types (CellType as u8: Unknown=0, Floor=1, Wall=2, Cliff=3, Bump=4) + cell_types: Vec, + /// Confidence levels (0-255) + confidences: Vec, + /// Observation counts (0-255, saturating) + observation_counts: Vec, + /// Swept/cleaned flags (0 = not swept, 1 = swept) + swept_flags: Vec, + /// Distance to nearest wall in meters (for Gaussian scoring in scan matching) + /// f32::MAX indicates no wall nearby (unknown areas) + distance_field: Vec, + + // === Grid Metadata === + /// Grid width in cells + width: usize, + /// Grid height in cells + height: usize, + /// Resolution in meters per cell + resolution: f32, + /// World coordinates of cell (0, 0) center + origin: WorldPoint, + /// Maximum distance to propagate in distance field (meters) + max_distance: f32, +} + +impl GridStorage { + /// Default max distance for distance field propagation (meters) + const DEFAULT_MAX_DISTANCE: f32 = 1.0; + + /// Create a new grid with the given dimensions + pub fn new(width: usize, height: usize, resolution: f32, origin: WorldPoint) -> Self { + let size = width * height; + Self { + cell_types: vec![CellType::Unknown as u8; size], + confidences: vec![0; size], + observation_counts: vec![0; size], + swept_flags: vec![0; size], + distance_field: vec![f32::MAX; size], + width, + height, + resolution, + origin, + max_distance: Self::DEFAULT_MAX_DISTANCE, + } + } + + /// Create a grid centered at the origin + pub fn centered(width: usize, height: usize, resolution: f32) -> Self { + let half_width = (width as f32 * resolution) / 2.0; + let half_height = (height as f32 * resolution) / 2.0; + let origin = WorldPoint::new(-half_width, -half_height); + Self::new(width, height, resolution, origin) + } + + /// Grid width in cells + #[inline] + pub fn width(&self) -> usize { + self.width + } + + /// Grid height in cells + #[inline] + pub fn height(&self) -> usize { + self.height + } + + /// Resolution in meters per cell + #[inline] + pub fn resolution(&self) -> f32 { + self.resolution + } + + /// World coordinates of cell (0, 0) + #[inline] + pub fn origin(&self) -> WorldPoint { + self.origin + } + + /// Total number of cells + #[inline] + pub fn cell_count(&self) -> usize { + self.width * self.height + } + + /// Raw access to cell types array (for SIMD operations) + #[inline] + pub fn cell_types_raw(&self) -> &[u8] { + &self.cell_types + } + + /// World bounds: (min_point, max_point) + pub fn bounds(&self) -> (WorldPoint, WorldPoint) { + let min = self.origin; + let max = WorldPoint::new( + self.origin.x + self.width as f32 * self.resolution, + self.origin.y + self.height as f32 * self.resolution, + ); + (min, max) + } + + /// Convert world coordinates to grid coordinates + #[inline] + pub fn world_to_grid(&self, point: WorldPoint) -> GridCoord { + let x = ((point.x - self.origin.x) / self.resolution).floor() as i32; + let y = ((point.y - self.origin.y) / self.resolution).floor() as i32; + GridCoord::new(x, y) + } + + /// Convert grid coordinates to world coordinates (cell center) + #[inline] + pub fn grid_to_world(&self, coord: GridCoord) -> WorldPoint { + WorldPoint::new( + self.origin.x + (coord.x as f32 + 0.5) * self.resolution, + self.origin.y + (coord.y as f32 + 0.5) * self.resolution, + ) + } + + /// Check if grid coordinates are within bounds + #[inline] + pub fn is_valid_coord(&self, coord: GridCoord) -> bool { + coord.x >= 0 + && coord.y >= 0 + && (coord.x as usize) < self.width + && (coord.y as usize) < self.height + } + + /// Check if world point is within grid bounds + #[inline] + pub fn contains_point(&self, point: WorldPoint) -> bool { + let (min, max) = self.bounds(); + point.x >= min.x && point.x < max.x && point.y >= min.y && point.y < max.y + } + + /// Convert grid coordinates to flat array index + #[inline] + pub fn coord_to_index(&self, coord: GridCoord) -> Option { + if self.is_valid_coord(coord) { + Some(coord.y as usize * self.width + coord.x as usize) + } else { + None + } + } + + /// Convert flat array index to grid coordinates + #[inline] + pub fn index_to_coord(&self, index: usize) -> GridCoord { + GridCoord::new((index % self.width) as i32, (index / self.width) as i32) + } + + /// Get cell at grid coordinates (reconstructs Cell from SoA data) + #[inline] + pub fn get(&self, coord: GridCoord) -> Option { + self.coord_to_index(coord).map(|i| Cell { + cell_type: CellType::from_u8(self.cell_types[i]), + confidence: self.confidences[i], + observation_count: self.observation_counts[i], + swept: self.swept_flags[i] != 0, + }) + } + + /// Get mutable cell at grid coordinates. + /// + /// Returns a `CellMut` that allows modifying individual fields. + #[inline] + pub fn get_mut(&mut self, coord: GridCoord) -> Option> { + self.coord_to_index(coord).map(move |i| CellMut { + cell_type: &mut self.cell_types[i], + confidence: &mut self.confidences[i], + observation_count: &mut self.observation_counts[i], + swept: &mut self.swept_flags[i], + }) + } + + /// Get cell at world coordinates + #[inline] + pub fn get_world(&self, point: WorldPoint) -> Option { + self.get(self.world_to_grid(point)) + } + + /// Get mutable cell at world coordinates + #[inline] + pub fn get_world_mut(&mut self, point: WorldPoint) -> Option> { + let coord = self.world_to_grid(point); + self.get_mut(coord) + } + + /// Get cell type at grid coordinates (returns Unknown if out of bounds) + #[inline] + pub fn get_type(&self, coord: GridCoord) -> CellType { + self.coord_to_index(coord) + .map(|i| CellType::from_u8(self.cell_types[i])) + .unwrap_or(CellType::Unknown) + } + + /// Get cell type at world coordinates + #[inline] + pub fn get_type_world(&self, point: WorldPoint) -> CellType { + self.get_type(self.world_to_grid(point)) + } + + /// Set cell type at grid coordinates + /// Returns true if the cell was updated + #[inline] + pub fn set_type(&mut self, coord: GridCoord, cell_type: CellType) -> bool { + if let Some(i) = self.coord_to_index(coord) { + self.observe_at_index(i, cell_type) + } else { + false + } + } + + /// Set cell type with priority rules + /// Returns true if the cell type changed + #[inline] + pub fn set_type_with_priority(&mut self, coord: GridCoord, cell_type: CellType) -> bool { + if let Some(i) = self.coord_to_index(coord) { + self.observe_with_priority_at_index(i, cell_type) + } else { + false + } + } + + /// Observe a cell type at a given index (internal helper) + #[inline] + fn observe_at_index(&mut self, i: usize, cell_type: CellType) -> bool { + let old_type = self.cell_types[i]; + let new_type = cell_type as u8; + let changed = old_type != new_type; + + if changed { + self.cell_types[i] = new_type; + self.confidences[i] = 1; + self.observation_counts[i] = 1; + } else { + self.confidences[i] = self.confidences[i].saturating_add(1); + self.observation_counts[i] = self.observation_counts[i].saturating_add(1); + } + + changed + } + + /// Observe a cell type with priority at a given index (internal helper) + #[inline] + fn observe_with_priority_at_index(&mut self, i: usize, cell_type: CellType) -> bool { + let old_type = CellType::from_u8(self.cell_types[i]); + let should_update = match (old_type, cell_type) { + // Unknown can be updated by anything + (CellType::Unknown, _) => true, + // Bump is highest priority - only update with another Bump + (CellType::Bump, CellType::Bump) => true, + (CellType::Bump, _) => false, + // Cliff can be updated by Bump or Cliff + (CellType::Cliff, CellType::Bump) => true, + (CellType::Cliff, CellType::Cliff) => true, + (CellType::Cliff, _) => false, + // Wall can be updated by Bump, Cliff, or Wall + (CellType::Wall, CellType::Bump) => true, + (CellType::Wall, CellType::Cliff) => true, + (CellType::Wall, CellType::Wall) => true, + (CellType::Wall, _) => false, + // Floor can be updated by anything + (CellType::Floor, _) => true, + }; + + if should_update { + self.observe_at_index(i, cell_type) + } else { + // Still count the observation even if type didn't change + self.observation_counts[i] = self.observation_counts[i].saturating_add(1); + false + } + } + + /// Clear all cells to Unknown + pub fn clear(&mut self) { + self.cell_types.fill(CellType::Unknown as u8); + self.confidences.fill(0); + self.observation_counts.fill(0); + self.swept_flags.fill(0); + self.distance_field.fill(f32::MAX); + } + + /// Iterate over all cells with their coordinates + pub fn iter(&self) -> impl Iterator + '_ { + (0..self.cell_types.len()).map(move |i| { + let x = (i % self.width) as i32; + let y = (i / self.width) as i32; + ( + GridCoord::new(x, y), + Cell { + cell_type: CellType::from_u8(self.cell_types[i]), + confidence: self.confidences[i], + observation_count: self.observation_counts[i], + swept: self.swept_flags[i] != 0, + }, + ) + }) + } + + /// Iterate over all cells mutably with their coordinates + pub fn iter_mut(&mut self) -> impl Iterator)> { + let width = self.width; + let types_ptr = self.cell_types.as_mut_ptr(); + let conf_ptr = self.confidences.as_mut_ptr(); + let obs_ptr = self.observation_counts.as_mut_ptr(); + let swept_ptr = self.swept_flags.as_mut_ptr(); + let len = self.cell_types.len(); + + (0..len).map(move |i| { + let x = (i % width) as i32; + let y = (i / width) as i32; + // SAFETY: We iterate exactly once over each index in range + unsafe { + ( + GridCoord::new(x, y), + CellMut { + cell_type: &mut *types_ptr.add(i), + confidence: &mut *conf_ptr.add(i), + observation_count: &mut *obs_ptr.add(i), + swept: &mut *swept_ptr.add(i), + }, + ) + } + }) + } + + /// Get raw cell types slice (for SIMD operations and serialization) + #[inline] + pub fn cell_types(&self) -> &[u8] { + &self.cell_types + } + + /// Get mutable raw cell types slice + #[inline] + pub fn cell_types_mut(&mut self) -> &mut [u8] { + &mut self.cell_types + } + + /// Get raw confidences slice + #[inline] + pub fn confidences(&self) -> &[u8] { + &self.confidences + } + + /// Get raw observation counts slice + #[inline] + pub fn observation_counts(&self) -> &[u8] { + &self.observation_counts + } + + /// Get raw swept flags slice + #[inline] + pub fn swept_flags(&self) -> &[u8] { + &self.swept_flags + } + + /// Get mutable swept flags slice + #[inline] + pub fn swept_flags_mut(&mut self) -> &mut [u8] { + &mut self.swept_flags + } + + /// Get raw cells slice (for serialization compatibility) + /// + /// Note: This reconstructs Cell structs from SoA data. + /// For performance-critical code, use the individual field slices directly. + pub fn cells(&self) -> Vec { + (0..self.cell_types.len()) + .map(|i| Cell { + cell_type: CellType::from_u8(self.cell_types[i]), + confidence: self.confidences[i], + observation_count: self.observation_counts[i], + swept: self.swept_flags[i] != 0, + }) + .collect() + } + + /// Count cells by type (SIMD-optimized implementation) + pub fn count_by_type(&self) -> CellCounts { + self.count_by_type_simd() + } + + /// Scalar implementation of cell counting (always available for testing) + #[allow(dead_code)] + pub fn count_by_type_scalar(&self) -> CellCounts { + let mut counts = CellCounts::default(); + for &cell_type in &self.cell_types { + match cell_type { + 0 => counts.unknown += 1, // Unknown + 1 => counts.floor += 1, // Floor + 2 => counts.wall += 1, // Wall + 3 => counts.cliff += 1, // Cliff + 4 => counts.bump += 1, // Bump + _ => counts.unknown += 1, // Invalid -> Unknown + } + } + counts + } + + /// SIMD-optimized cell counting (processes 16 cells at a time) + fn count_by_type_simd(&self) -> CellCounts { + let mut counts = CellCounts::default(); + + // SIMD constants for comparison + let unknown_vec = u8x16::splat(CellType::Unknown as u8); + let floor_vec = u8x16::splat(CellType::Floor as u8); + let wall_vec = u8x16::splat(CellType::Wall as u8); + let cliff_vec = u8x16::splat(CellType::Cliff as u8); + let bump_vec = u8x16::splat(CellType::Bump as u8); + + // Process 16 cells at a time + let chunks = self.cell_types.chunks_exact(16); + let remainder = chunks.remainder(); + + for chunk in chunks { + let data = u8x16::from_slice(chunk); + + // Compare and count matches using bitmask + counts.unknown += data.simd_eq(unknown_vec).to_bitmask().count_ones() as usize; + counts.floor += data.simd_eq(floor_vec).to_bitmask().count_ones() as usize; + counts.wall += data.simd_eq(wall_vec).to_bitmask().count_ones() as usize; + counts.cliff += data.simd_eq(cliff_vec).to_bitmask().count_ones() as usize; + counts.bump += data.simd_eq(bump_vec).to_bitmask().count_ones() as usize; + } + + // Handle remainder with scalar fallback + for &cell_type in remainder { + match cell_type { + 0 => counts.unknown += 1, + 1 => counts.floor += 1, + 2 => counts.wall += 1, + 3 => counts.cliff += 1, + 4 => counts.bump += 1, + _ => counts.unknown += 1, + } + } + + counts + } + + /// Check if all cells in a rectangular region are Floor (SIMD-optimized) + pub fn is_rect_all_floor( + &self, + start_x: usize, + start_y: usize, + rect_width: usize, + rect_height: usize, + ) -> bool { + let floor_vec = u8x16::splat(CellType::Floor as u8); + + for row in 0..rect_height { + let y = start_y + row; + if y >= self.height { + return false; + } + + let row_start = y * self.width + start_x; + let row_end = (row_start + rect_width).min(y * self.width + self.width); + + if row_end > self.cell_types.len() { + return false; + } + + let row_slice = &self.cell_types[row_start..row_end]; + + // Process 16 cells at a time + let chunks = row_slice.chunks_exact(16); + let remainder = chunks.remainder(); + + for chunk in chunks { + let data = u8x16::from_slice(chunk); + let is_floor = data.simd_eq(floor_vec); + if is_floor.to_bitmask() != 0xFFFF { + return false; // At least one cell is not Floor + } + } + + // Check remainder + for &cell_type in remainder { + if cell_type != CellType::Floor as u8 { + return false; + } + } + } + + true + } + + /// Expand the grid to include a world point + /// Returns true if the grid was expanded + pub fn expand_to_include( + &mut self, + point: WorldPoint, + max_width: usize, + max_height: usize, + ) -> bool { + let coord = self.world_to_grid(point); + + // Calculate required expansion + let expand_left = if coord.x < 0 { (-coord.x) as usize } else { 0 }; + let expand_right = if coord.x >= self.width as i32 { + coord.x as usize - self.width + 1 + } else { + 0 + }; + let expand_down = if coord.y < 0 { (-coord.y) as usize } else { 0 }; + let expand_up = if coord.y >= self.height as i32 { + coord.y as usize - self.height + 1 + } else { + 0 + }; + + if expand_left == 0 && expand_right == 0 && expand_down == 0 && expand_up == 0 { + return false; // No expansion needed + } + + let new_width = self.width + expand_left + expand_right; + let new_height = self.height + expand_down + expand_up; + + // Check max size + if new_width > max_width || new_height > max_height { + return false; // Would exceed max size + } + + let new_size = new_width * new_height; + + // Allocate new arrays + let mut new_cell_types = vec![CellType::Unknown as u8; new_size]; + let mut new_confidences = vec![0u8; new_size]; + let mut new_observation_counts = vec![0u8; new_size]; + let mut new_swept_flags = vec![0u8; new_size]; + let mut new_distance_field = vec![f32::MAX; new_size]; + + // Copy old data row by row + for y in 0..self.height { + let old_row_start = y * self.width; + let new_row_start = (y + expand_down) * new_width + expand_left; + + // Copy each array + new_cell_types[new_row_start..new_row_start + self.width] + .copy_from_slice(&self.cell_types[old_row_start..old_row_start + self.width]); + new_confidences[new_row_start..new_row_start + self.width] + .copy_from_slice(&self.confidences[old_row_start..old_row_start + self.width]); + new_observation_counts[new_row_start..new_row_start + self.width].copy_from_slice( + &self.observation_counts[old_row_start..old_row_start + self.width], + ); + new_swept_flags[new_row_start..new_row_start + self.width] + .copy_from_slice(&self.swept_flags[old_row_start..old_row_start + self.width]); + new_distance_field[new_row_start..new_row_start + self.width] + .copy_from_slice(&self.distance_field[old_row_start..old_row_start + self.width]); + } + + // Update state + self.cell_types = new_cell_types; + self.confidences = new_confidences; + self.observation_counts = new_observation_counts; + self.swept_flags = new_swept_flags; + self.distance_field = new_distance_field; + self.width = new_width; + self.height = new_height; + self.origin = WorldPoint::new( + self.origin.x - expand_left as f32 * self.resolution, + self.origin.y - expand_down as f32 * self.resolution, + ); + + true + } + + // === Distance Field Methods === + + /// Get distance to nearest wall at grid coordinates (in meters). + /// Returns f32::MAX if no wall is nearby or if coord is out of bounds. + #[inline] + pub fn get_distance(&self, coord: GridCoord) -> f32 { + self.coord_to_index(coord) + .map(|i| self.distance_field[i]) + .unwrap_or(f32::MAX) + } + + /// Get distance to nearest wall at world coordinates (in meters). + #[inline] + pub fn get_distance_world(&self, point: WorldPoint) -> f32 { + self.get_distance(self.world_to_grid(point)) + } + + /// Get raw distance field slice (for debugging/visualization) + #[inline] + pub fn distance_field(&self) -> &[f32] { + &self.distance_field + } + + /// Get distance to nearest wall with bilinear interpolation. + /// + /// Unlike `get_distance_world` which uses nearest-cell lookup, + /// this method provides smooth gradients for sub-pixel accuracy + /// in Gauss-Newton optimization during scan matching. + /// + /// Returns (distance, gradient_x, gradient_y) where gradients are + /// in meters per meter (dimensionless slope). + #[inline] + pub fn get_distance_interpolated(&self, point: WorldPoint) -> (f32, f32, f32) { + // Convert to continuous grid coordinates + let gx = (point.x - self.origin.x) / self.resolution; + let gy = (point.y - self.origin.y) / self.resolution; + + // Get integer cell coordinates (floor) + let x0 = gx.floor() as i32; + let y0 = gy.floor() as i32; + + // Fractional part within cell [0, 1) + let fx = gx - x0 as f32; + let fy = gy - y0 as f32; + + // Sample 2x2 neighborhood + let d00 = self.get_distance_safe(x0, y0); + let d10 = self.get_distance_safe(x0 + 1, y0); + let d01 = self.get_distance_safe(x0, y0 + 1); + let d11 = self.get_distance_safe(x0 + 1, y0 + 1); + + // Bilinear interpolation for distance + let d0 = d00 * (1.0 - fx) + d10 * fx; // Bottom edge + let d1 = d01 * (1.0 - fx) + d11 * fx; // Top edge + let distance = d0 * (1.0 - fy) + d1 * fy; + + // Gradient computation via finite differences + // ∂d/∂x ≈ (d(x+h) - d(x-h)) / (2h) where h = resolution + // Using the bilinear coefficients: + let grad_x = ((d10 - d00) * (1.0 - fy) + (d11 - d01) * fy) / self.resolution; + let grad_y = ((d01 - d00) * (1.0 - fx) + (d11 - d10) * fx) / self.resolution; + + (distance, grad_x, grad_y) + } + + /// Helper to get distance with safe bounds checking. + /// Returns max_distance for out-of-bounds or unknown areas. + #[inline] + fn get_distance_safe(&self, x: i32, y: i32) -> f32 { + if x >= 0 && y >= 0 && (x as usize) < self.width && (y as usize) < self.height { + let idx = y as usize * self.width + x as usize; + let d = self.distance_field[idx]; + // Clamp MAX values to max_distance for smoother interpolation + if d >= f32::MAX / 2.0 { + self.max_distance + } else { + d + } + } else { + self.max_distance + } + } + + /// Update distance field around a newly added wall cell. + /// Uses brushfire (BFS) algorithm to propagate distances outward. + pub fn update_distance_field_from_wall(&mut self, wall_coord: GridCoord) { + use std::collections::VecDeque; + + // Wall cell has distance 0 + if let Some(wall_idx) = self.coord_to_index(wall_coord) { + self.distance_field[wall_idx] = 0.0; + } else { + return; + } + + // BFS queue: (coord, distance) + let mut queue = VecDeque::new(); + queue.push_back((wall_coord, 0.0f32)); + + // 8-connected neighbors (including diagonals for smoother distance) + let neighbors_4 = [(0, 1), (1, 0), (0, -1), (-1, 0)]; + let neighbors_diag = [(1, 1), (1, -1), (-1, 1), (-1, -1)]; + + let diag_dist = self.resolution * std::f32::consts::SQRT_2; + + while let Some((coord, dist)) = queue.pop_front() { + // Process 4-connected neighbors (orthogonal) + for (dx, dy) in neighbors_4 { + let neighbor = GridCoord::new(coord.x + dx, coord.y + dy); + let new_dist = dist + self.resolution; + + if new_dist > self.max_distance { + continue; + } + + if let Some(idx) = self.coord_to_index(neighbor) + && new_dist < self.distance_field[idx] + { + self.distance_field[idx] = new_dist; + queue.push_back((neighbor, new_dist)); + } + } + + // Process diagonal neighbors + for (dx, dy) in neighbors_diag { + let neighbor = GridCoord::new(coord.x + dx, coord.y + dy); + let new_dist = dist + diag_dist; + + if new_dist > self.max_distance { + continue; + } + + if let Some(idx) = self.coord_to_index(neighbor) + && new_dist < self.distance_field[idx] + { + self.distance_field[idx] = new_dist; + queue.push_back((neighbor, new_dist)); + } + } + } + } + + /// Batch update distance field for multiple wall cells. + /// More efficient than calling update_distance_field_from_wall multiple times. + pub fn update_distance_field_batch(&mut self, wall_coords: &[GridCoord]) { + use std::collections::VecDeque; + + if wall_coords.is_empty() { + return; + } + + // BFS queue: (coord, distance) + let mut queue = VecDeque::with_capacity(wall_coords.len() * 4); + + // Initialize all wall cells with distance 0 + for &coord in wall_coords { + if let Some(idx) = self.coord_to_index(coord) { + self.distance_field[idx] = 0.0; + queue.push_back((coord, 0.0f32)); + } + } + + // 8-connected neighbors + let neighbors_4 = [(0, 1), (1, 0), (0, -1), (-1, 0)]; + let neighbors_diag = [(1, 1), (1, -1), (-1, 1), (-1, -1)]; + + let diag_dist = self.resolution * std::f32::consts::SQRT_2; + + while let Some((coord, dist)) = queue.pop_front() { + // Process 4-connected neighbors + for (dx, dy) in neighbors_4 { + let neighbor = GridCoord::new(coord.x + dx, coord.y + dy); + let new_dist = dist + self.resolution; + + if new_dist > self.max_distance { + continue; + } + + if let Some(idx) = self.coord_to_index(neighbor) + && new_dist < self.distance_field[idx] + { + self.distance_field[idx] = new_dist; + queue.push_back((neighbor, new_dist)); + } + } + + // Process diagonal neighbors + for (dx, dy) in neighbors_diag { + let neighbor = GridCoord::new(coord.x + dx, coord.y + dy); + let new_dist = dist + diag_dist; + + if new_dist > self.max_distance { + continue; + } + + if let Some(idx) = self.coord_to_index(neighbor) + && new_dist < self.distance_field[idx] + { + self.distance_field[idx] = new_dist; + queue.push_back((neighbor, new_dist)); + } + } + } + } + + /// Recompute entire distance field from scratch. + /// Call this after major changes to wall configuration. + pub fn recompute_distance_field(&mut self) { + // Reset all distances to MAX + self.distance_field.fill(f32::MAX); + + // Collect all wall cells + let wall_coords: Vec = (0..self.cell_types.len()) + .filter(|&i| self.cell_types[i] == CellType::Wall as u8) + .map(|i| self.index_to_coord(i)) + .collect(); + + // Batch update from all walls + self.update_distance_field_batch(&wall_coords); + } +} + +/// Mutable reference to a single cell's fields in the SoA storage. +/// +/// This allows modifying individual cell properties without reconstructing +/// the entire Cell struct. +pub struct CellMut<'a> { + /// Cell type as raw u8 + pub cell_type: &'a mut u8, + /// Confidence level + pub confidence: &'a mut u8, + /// Observation count + pub observation_count: &'a mut u8, + /// Swept flag (0 = not swept, 1 = swept) + pub swept: &'a mut u8, +} + +impl<'a> CellMut<'a> { + /// Get the cell type + #[inline] + pub fn get_type(&self) -> CellType { + CellType::from_u8(*self.cell_type) + } + + /// Set the cell type + #[inline] + pub fn set_type(&mut self, cell_type: CellType) { + *self.cell_type = cell_type as u8; + } + + /// Check if traversable + #[inline] + pub fn is_traversable(&self) -> bool { + *self.cell_type == CellType::Floor as u8 + } + + /// Check if obstacle + #[inline] + pub fn is_obstacle(&self) -> bool { + let t = *self.cell_type; + t == CellType::Wall as u8 || t == CellType::Cliff as u8 || t == CellType::Bump as u8 + } + + /// Update with a new observation + pub fn observe(&mut self, observed_type: CellType) -> bool { + let new_type = observed_type as u8; + let changed = *self.cell_type != new_type; + + if changed { + *self.cell_type = new_type; + *self.confidence = 1; + *self.observation_count = 1; + } else { + *self.confidence = self.confidence.saturating_add(1); + *self.observation_count = self.observation_count.saturating_add(1); + } + + changed + } + + /// Update with priority rules + pub fn observe_with_priority(&mut self, observed_type: CellType) -> bool { + let old_type = CellType::from_u8(*self.cell_type); + let should_update = match (old_type, observed_type) { + (CellType::Unknown, _) => true, + (CellType::Bump, CellType::Bump) => true, + (CellType::Bump, _) => false, + (CellType::Cliff, CellType::Bump | CellType::Cliff) => true, + (CellType::Cliff, _) => false, + (CellType::Wall, CellType::Bump | CellType::Cliff | CellType::Wall) => true, + (CellType::Wall, _) => false, + (CellType::Floor, _) => true, + }; + + if should_update { + self.observe(observed_type) + } else { + *self.observation_count = self.observation_count.saturating_add(1); + false + } + } +} + +/// Cell counts by type +#[derive(Clone, Copy, Debug, Default)] +pub struct CellCounts { + /// Unknown cells (not yet observed) + pub unknown: usize, + /// Floor cells (traversable) + pub floor: usize, + /// Wall cells (lidar-detected obstacles) + pub wall: usize, + /// Cliff cells (floor drop-offs) + pub cliff: usize, + /// Bump cells (invisible obstacles) + pub bump: usize, +} + +impl CellCounts { + /// Total known cells + pub fn known(&self) -> usize { + self.floor + self.wall + self.cliff + self.bump + } + + /// Total cells + pub fn total(&self) -> usize { + self.unknown + self.known() + } + + /// Total obstacle cells + pub fn obstacles(&self) -> usize { + self.wall + self.cliff + self.bump + } +} + +#[cfg(test)] +mod tests { + use super::*; + + #[test] + fn test_grid_creation() { + let grid = GridStorage::new(100, 100, 0.05, WorldPoint::ZERO); + assert_eq!(grid.width(), 100); + assert_eq!(grid.height(), 100); + assert_eq!(grid.resolution(), 0.05); + assert_eq!(grid.cell_count(), 10000); + } + + #[test] + fn test_centered_grid() { + let grid = GridStorage::centered(100, 100, 0.05); + let (min, max) = grid.bounds(); + + // Should be centered around origin + assert!((min.x + 2.5).abs() < 1e-6); + assert!((min.y + 2.5).abs() < 1e-6); + assert!((max.x - 2.5).abs() < 1e-6); + assert!((max.y - 2.5).abs() < 1e-6); + } + + #[test] + fn test_world_to_grid_conversion() { + let grid = GridStorage::new(100, 100, 0.05, WorldPoint::ZERO); + + // Origin should map to (0, 0) + let coord = grid.world_to_grid(WorldPoint::new(0.0, 0.0)); + assert_eq!(coord, GridCoord::new(0, 0)); + + // 1 meter should be 20 cells at 0.05 resolution + let coord = grid.world_to_grid(WorldPoint::new(1.0, 1.0)); + assert_eq!(coord, GridCoord::new(20, 20)); + } + + #[test] + fn test_grid_to_world_conversion() { + let grid = GridStorage::new(100, 100, 0.05, WorldPoint::ZERO); + + // Cell (0, 0) center should be at (0.025, 0.025) + let point = grid.grid_to_world(GridCoord::new(0, 0)); + assert!((point.x - 0.025).abs() < 1e-6); + assert!((point.y - 0.025).abs() < 1e-6); + } + + #[test] + fn test_get_set_cell() { + let mut grid = GridStorage::new(10, 10, 0.1, WorldPoint::ZERO); + + // Initially unknown + assert_eq!(grid.get_type(GridCoord::new(5, 5)), CellType::Unknown); + + // Set to floor + grid.set_type(GridCoord::new(5, 5), CellType::Floor); + assert_eq!(grid.get_type(GridCoord::new(5, 5)), CellType::Floor); + + // Out of bounds returns Unknown + assert_eq!(grid.get_type(GridCoord::new(100, 100)), CellType::Unknown); + } + + #[test] + fn test_grid_expansion() { + let mut grid = GridStorage::centered(10, 10, 0.1); + + // Initial size + assert_eq!(grid.width(), 10); + assert_eq!(grid.height(), 10); + + // Expand to include a point outside + let far_point = WorldPoint::new(2.0, 2.0); + assert!(grid.expand_to_include(far_point, 100, 100)); + + // Grid should be larger now + assert!(grid.width() > 10); + assert!(grid.height() > 10); + + // Point should now be within bounds + assert!(grid.contains_point(far_point)); + } + + #[test] + fn test_cell_counts() { + let mut grid = GridStorage::new(10, 10, 0.1, WorldPoint::ZERO); + + grid.set_type(GridCoord::new(0, 0), CellType::Floor); + grid.set_type(GridCoord::new(1, 0), CellType::Wall); + grid.set_type(GridCoord::new(2, 0), CellType::Cliff); + grid.set_type(GridCoord::new(3, 0), CellType::Bump); + + let counts = grid.count_by_type(); + assert_eq!(counts.floor, 1); + assert_eq!(counts.wall, 1); + assert_eq!(counts.cliff, 1); + assert_eq!(counts.bump, 1); + assert_eq!(counts.unknown, 96); + assert_eq!(counts.known(), 4); + } + + #[test] + fn test_soa_iter() { + let mut grid = GridStorage::new(10, 10, 0.1, WorldPoint::ZERO); + grid.set_type(GridCoord::new(5, 5), CellType::Wall); + + let mut found_wall = false; + for (coord, cell) in grid.iter() { + if coord == GridCoord::new(5, 5) { + assert_eq!(cell.cell_type, CellType::Wall); + found_wall = true; + } + } + assert!(found_wall); + } + + #[test] + fn test_cell_priority() { + let mut grid = GridStorage::new(10, 10, 0.1, WorldPoint::ZERO); + + // Set to Floor first + grid.set_type(GridCoord::new(0, 0), CellType::Floor); + assert_eq!(grid.get_type(GridCoord::new(0, 0)), CellType::Floor); + + // Wall overrides Floor + grid.set_type_with_priority(GridCoord::new(0, 0), CellType::Wall); + assert_eq!(grid.get_type(GridCoord::new(0, 0)), CellType::Wall); + + // Floor does NOT override Wall + grid.set_type_with_priority(GridCoord::new(0, 0), CellType::Floor); + assert_eq!(grid.get_type(GridCoord::new(0, 0)), CellType::Wall); + + // Bump overrides Wall + grid.set_type_with_priority(GridCoord::new(0, 0), CellType::Bump); + assert_eq!(grid.get_type(GridCoord::new(0, 0)), CellType::Bump); + } + + #[test] + fn test_simd_count_matches_scalar() { + let mut grid = GridStorage::new(100, 100, 0.05, WorldPoint::ZERO); + + // Set various cell types + for i in 0..100 { + grid.set_type(GridCoord::new(i, 0), CellType::Floor); + grid.set_type(GridCoord::new(i, 1), CellType::Wall); + } + for i in 0..50 { + grid.set_type(GridCoord::new(i, 2), CellType::Cliff); + grid.set_type(GridCoord::new(i, 3), CellType::Bump); + } + + let simd_counts = grid.count_by_type_simd(); + let scalar_counts = grid.count_by_type_scalar(); + + assert_eq!(simd_counts.unknown, scalar_counts.unknown); + assert_eq!(simd_counts.floor, scalar_counts.floor); + assert_eq!(simd_counts.wall, scalar_counts.wall); + assert_eq!(simd_counts.cliff, scalar_counts.cliff); + assert_eq!(simd_counts.bump, scalar_counts.bump); + } +} diff --git a/vastu-map/src/integration/README.md b/vastu-map/src/integration/README.md deleted file mode 100644 index 67c99f9..0000000 --- a/vastu-map/src/integration/README.md +++ /dev/null @@ -1,140 +0,0 @@ -# Integration Module - -Map integration for combining scan features with the map. - -## Components - -| File | Description | -|------|-------------| -| `spatial_index.rs` | R-tree spatial index for efficient queries | -| `association.rs` | Line association (matching scan lines to map lines) | -| `merger.rs` | Feature merging (updating map lines with observations) | - -## Integration Pipeline - -``` -Scan Features (Lines) - │ - ▼ -┌───────────────────┐ -│ 1. Associate │ ← Find correspondences to map lines -└────────┬──────────┘ - │ - ▼ -┌───────────────────┐ -│ 2. Merge │ ← Update matched map lines -└────────┬──────────┘ - │ - ▼ -┌───────────────────┐ -│ 3. Add │ ← Insert unmatched lines as new features -└────────┬──────────┘ - │ - ▼ - Updated Map -``` - -## Components - -### Spatial Index (R-Tree) - -Efficient spatial queries using rstar R-tree: - -```rust -let index = SpatialIndex::new(&lines); - -// Query lines near a point -let candidates = index.query_near(point, radius); - -// Query lines in a bounding box -let candidates = index.query_bounds(&bounds); -``` - -### Line Association - -Find correspondences between scan and map lines: - -```rust -let config = AssociationConfig { - max_distance: 0.3, // Max perpendicular distance - max_angle_diff: 0.15, // Max angle difference (radians) - min_overlap_ratio: 0.3, // Minimum segment overlap -}; - -// Find all associations -let associations = find_associations(&scan_lines, &map_lines, Some(&index), &config); - -// Find unique associations (one-to-one mapping) -let unique = find_unique_associations(&scan_lines, &map_lines, Some(&index), &config); - -// Find unmatched scan lines (for adding to map) -let unmatched = find_unmatched_scan_lines(&scan_lines, &map_lines, Some(&index), &config); -``` - -### Line Merging - -Update map lines with new observations: - -```rust -let config = MergerConfig { - position_weight: 0.3, // Weight for new observation - extend_endpoints: true, // Allow extending line endpoints - max_extension: 0.5, // Max extension distance - min_observation_count: 3, // Min observations for stable line -}; - -// Merge single line -let result = merge_lines(&scan_line, &mut map_line, &config); - -// Batch merge -batch_merge(&scan_lines, &mut map_lines, &associations, &config); - -// Create new line from unmatched scan line -let new_line = create_new_line(&scan_line); - -// Merge collinear lines in the map -merge_collinear_lines(&mut map_lines, max_gap, max_angle); -``` - -## Association Criteria - -Lines are associated when: - -1. **Distance**: Perpendicular distance < `max_distance` -2. **Angle**: Angular difference < `max_angle_diff` -3. **Overlap**: Segments overlap by at least `min_overlap_ratio` - -``` - Map Line - ←────────────────→ - ╲ - ╲ angle_diff - ╲ - ←──────────→ - Scan Line - ↑ - │ perpendicular distance -``` - -## Merge Strategy - -When merging lines: - -1. **Endpoint Extension**: Extend map line to cover scan line extent -2. **Position Averaging**: Weighted average of line positions -3. **Observation Counting**: Track observation count for stability - -``` -Before: Map: ════════════ - Scan: ══════════ - -After: Map: ═══════════════════ - (extended + averaged) -``` - -## Best Practices - -1. **Use spatial index** for maps with > 100 lines -2. **Tune association thresholds** based on sensor noise -3. **Merge collinear lines** periodically to reduce map size -4. **Track observation counts** to filter unstable features diff --git a/vastu-map/src/integration/association.rs b/vastu-map/src/integration/association.rs deleted file mode 100644 index 340cea2..0000000 --- a/vastu-map/src/integration/association.rs +++ /dev/null @@ -1,544 +0,0 @@ -//! Line association for map integration. -//! -//! Associates new scan lines with existing map lines based on: -//! - Spatial proximity -//! - Angular similarity -//! - Overlap extent - -use serde::{Deserialize, Serialize}; - -use crate::core::math::angle_diff; -use crate::features::Line2D; - -use super::spatial_index::SpatialIndex; - -/// Configuration for line association. -#[derive(Clone, Debug, Serialize, Deserialize)] -pub struct AssociationConfig { - /// Maximum perpendicular distance for association (meters). - /// Default: 0.15m - pub max_perpendicular_distance: f32, - - /// Maximum angular difference for association (radians). - /// Default: 0.2 (~11°) - pub max_angle_difference: f32, - - /// Minimum overlap ratio for valid association. - /// Overlap is measured as a fraction of the shorter line's length. - /// Default: 0.3 (30%) - pub min_overlap_ratio: f32, - - /// Maximum gap between lines for association (meters). - /// Lines that don't overlap but are close enough may still associate. - /// Default: 0.3m - pub max_endpoint_gap: f32, - - // === Quality Score Weights === - // These control how the quality score is computed for ranking associations. - /// Weight for distance component in quality score. - /// Higher values make distance more important in ranking. - /// Formula: `1.0 / (1.0 + distance * distance_weight)` - /// Default: 10.0 - pub distance_weight: f32, - - /// Weight for angle component in quality score. - /// Higher values make angular alignment more important in ranking. - /// Formula: `1.0 / (1.0 + angle_diff * angle_weight)` - /// Default: 5.0 - pub angle_weight: f32, -} - -impl Default for AssociationConfig { - fn default() -> Self { - Self { - max_perpendicular_distance: 0.15, - max_angle_difference: 0.2, - min_overlap_ratio: 0.3, - max_endpoint_gap: 0.3, - distance_weight: 10.0, - angle_weight: 5.0, - } - } -} - -impl AssociationConfig { - /// Create a new configuration with default values. - pub fn new() -> Self { - Self::default() - } - - /// Builder-style setter for maximum perpendicular distance. - pub fn with_max_perpendicular_distance(mut self, meters: f32) -> Self { - self.max_perpendicular_distance = meters; - self - } - - /// Builder-style setter for maximum angle difference. - pub fn with_max_angle_difference(mut self, radians: f32) -> Self { - self.max_angle_difference = radians; - self - } - - /// Builder-style setter for minimum overlap ratio. - pub fn with_min_overlap_ratio(mut self, ratio: f32) -> Self { - self.min_overlap_ratio = ratio; - self - } - - /// Builder-style setter for distance weight in quality scoring. - pub fn with_distance_weight(mut self, weight: f32) -> Self { - self.distance_weight = weight; - self - } - - /// Builder-style setter for angle weight in quality scoring. - pub fn with_angle_weight(mut self, weight: f32) -> Self { - self.angle_weight = weight; - self - } -} - -/// Result of line association. -#[derive(Clone, Debug)] -pub struct Association { - /// Index of the scan line. - pub scan_line_idx: usize, - /// Index of the associated map line. - pub map_line_idx: usize, - /// Perpendicular distance between lines. - pub distance: f32, - /// Angular difference between lines (radians). - pub angle_diff: f32, - /// Overlap length (meters). - pub overlap_length: f32, - /// Overlap ratio (0 to 1). - pub overlap_ratio: f32, -} - -impl Association { - /// Create a new association. - pub fn new( - scan_line_idx: usize, - map_line_idx: usize, - distance: f32, - angle_diff: f32, - overlap_length: f32, - overlap_ratio: f32, - ) -> Self { - Self { - scan_line_idx, - map_line_idx, - distance, - angle_diff, - overlap_length, - overlap_ratio, - } - } - - /// Compute a quality score for the association (higher is better). - /// - /// Uses default weights (distance=10.0, angle=5.0). - /// For custom weights, use `quality_score_with_config`. - pub fn quality_score(&self) -> f32 { - self.quality_score_with_weights(10.0, 5.0) - } - - /// Compute a quality score using custom weights from config. - /// - /// # Arguments - /// * `config` - Association configuration containing weight parameters - /// - /// # Example - /// ```rust,ignore - /// let config = AssociationConfig::default() - /// .with_distance_weight(20.0) // Prioritize distance - /// .with_angle_weight(2.0); // Relax angle constraint - /// - /// let score = association.quality_score_with_config(&config); - /// ``` - pub fn quality_score_with_config(&self, config: &AssociationConfig) -> f32 { - self.quality_score_with_weights(config.distance_weight, config.angle_weight) - } - - /// Compute a quality score with explicit weights. - /// - /// # Formula - /// ```text - /// score = dist_score * angle_score * overlap_ratio - /// where: - /// dist_score = 1.0 / (1.0 + distance * distance_weight) - /// angle_score = 1.0 / (1.0 + angle_diff * angle_weight) - /// ``` - #[inline] - pub fn quality_score_with_weights(&self, distance_weight: f32, angle_weight: f32) -> f32 { - let dist_score = 1.0 / (1.0 + self.distance * distance_weight); - let angle_score = 1.0 / (1.0 + self.angle_diff * angle_weight); - let overlap_score = self.overlap_ratio; - - dist_score * angle_score * overlap_score - } -} - -/// Find associations between scan lines and map lines. -/// -/// # Arguments -/// * `scan_lines` - Lines extracted from the current scan -/// * `map_lines` - Existing map lines -/// * `index` - Spatial index of map lines (optional, for efficiency) -/// * `config` - Association configuration -/// -/// # Returns -/// Vector of associations, one per scan line that found a match. -/// Scan lines without matches are not included. -pub fn find_associations( - scan_lines: &[Line2D], - map_lines: &[Line2D], - index: Option<&SpatialIndex>, - config: &AssociationConfig, -) -> Vec { - if scan_lines.is_empty() || map_lines.is_empty() { - return Vec::new(); - } - - let mut associations = Vec::with_capacity(scan_lines.len()); - - for (scan_idx, scan_line) in scan_lines.iter().enumerate() { - // Find candidate map lines using spatial index if available - let candidates: Vec = if let Some(idx) = index { - idx.potential_matches(scan_line, config.max_perpendicular_distance * 2.0) - } else { - (0..map_lines.len()).collect() - }; - - let mut best_association: Option = None; - let mut best_score = 0.0; - - for &map_idx in &candidates { - let map_line = &map_lines[map_idx]; - - if let Some(assoc) = try_associate(scan_idx, scan_line, map_idx, map_line, config) { - let score = assoc.quality_score(); - if score > best_score { - best_score = score; - best_association = Some(assoc); - } - } - } - - if let Some(assoc) = best_association { - associations.push(assoc); - } - } - - associations -} - -/// Try to associate two lines. -/// -/// Returns Some(Association) if lines are compatible, None otherwise. -fn try_associate( - scan_idx: usize, - scan_line: &Line2D, - map_idx: usize, - map_line: &Line2D, - config: &AssociationConfig, -) -> Option { - // Check angular compatibility - let scan_angle = scan_line.angle(); - let map_angle = map_line.angle(); - let angle_difference = angle_diff(scan_angle, map_angle).abs(); - - // Lines can be oriented either way, so check both directions - let angle_difference = angle_difference.min(std::f32::consts::PI - angle_difference); - - if angle_difference > config.max_angle_difference { - return None; - } - - // Compute perpendicular distance (average of endpoint distances) - let dist1 = map_line.distance_to_point(scan_line.start); - let dist2 = map_line.distance_to_point(scan_line.end); - let avg_distance = (dist1 + dist2) / 2.0; - - if avg_distance > config.max_perpendicular_distance { - return None; - } - - // Compute overlap - let overlap = compute_overlap(scan_line, map_line); - let scan_len = scan_line.length(); - let map_len = map_line.length(); - let shorter_len = scan_len.min(map_len); - - // Handle case where lines don't overlap but are close - if overlap <= 0.0 { - let gap = compute_endpoint_gap(scan_line, map_line); - if gap > config.max_endpoint_gap { - return None; - } - // Allow association with zero overlap if endpoints are close - return Some(Association::new( - scan_idx, - map_idx, - avg_distance, - angle_difference, - 0.0, - 0.0, - )); - } - - let overlap_ratio = if shorter_len > 0.0 { - overlap / shorter_len - } else { - 0.0 - }; - - if overlap_ratio < config.min_overlap_ratio { - return None; - } - - Some(Association::new( - scan_idx, - map_idx, - avg_distance, - angle_difference, - overlap, - overlap_ratio, - )) -} - -/// Compute overlap length between two lines. -/// -/// Projects both lines onto a common axis and computes intersection. -fn compute_overlap(line1: &Line2D, line2: &Line2D) -> f32 { - // Use Line2D's overlap_length method with default angle tolerance - line1.overlap_length(line2, 0.3) -} - -/// Compute minimum gap between line endpoints. -fn compute_endpoint_gap(line1: &Line2D, line2: &Line2D) -> f32 { - let d1 = line1.start.distance(line2.start); - let d2 = line1.start.distance(line2.end); - let d3 = line1.end.distance(line2.start); - let d4 = line1.end.distance(line2.end); - - d1.min(d2).min(d3).min(d4) -} - -/// Associate scan lines to map lines, ensuring each map line -/// has at most one association (best match wins). -/// -/// This is useful when updating the map to avoid duplicate associations. -pub fn find_unique_associations( - scan_lines: &[Line2D], - map_lines: &[Line2D], - index: Option<&SpatialIndex>, - config: &AssociationConfig, -) -> Vec { - let all_associations = find_associations(scan_lines, map_lines, index, config); - - // Group by map line and keep best match - let mut best_per_map: std::collections::HashMap = - std::collections::HashMap::new(); - - for assoc in all_associations { - let entry = best_per_map.entry(assoc.map_line_idx); - entry - .and_modify(|existing| { - if assoc.quality_score() > existing.quality_score() { - *existing = assoc.clone(); - } - }) - .or_insert(assoc); - } - - best_per_map.into_values().collect() -} - -/// Find scan lines that don't match any map lines. -/// -/// These are candidates for new features to add to the map. -pub fn find_unmatched_scan_lines( - scan_lines: &[Line2D], - map_lines: &[Line2D], - index: Option<&SpatialIndex>, - config: &AssociationConfig, -) -> Vec { - let associations = find_associations(scan_lines, map_lines, index, config); - let matched: std::collections::HashSet = - associations.iter().map(|a| a.scan_line_idx).collect(); - - (0..scan_lines.len()) - .filter(|i| !matched.contains(i)) - .collect() -} - -#[cfg(test)] -mod tests { - use super::*; - use crate::core::Point2D; - use approx::assert_relative_eq; - - #[test] - fn test_association_quality_score() { - let good = Association::new(0, 0, 0.01, 0.01, 1.0, 0.9); - let bad = Association::new(0, 0, 0.5, 0.5, 0.1, 0.1); - - assert!(good.quality_score() > bad.quality_score()); - } - - #[test] - fn test_association_quality_score_with_weights() { - let assoc = Association::new(0, 0, 0.1, 0.1, 1.0, 0.8); - - // Default weights - let default_score = assoc.quality_score(); - - // Higher distance weight = more penalty for distance - let high_dist_weight = assoc.quality_score_with_weights(20.0, 5.0); - assert!(high_dist_weight < default_score); - - // Lower angle weight = less penalty for angle difference - let low_angle_weight = assoc.quality_score_with_weights(10.0, 2.0); - assert!(low_angle_weight > default_score); - - // Using config - let config = AssociationConfig::default() - .with_distance_weight(5.0) - .with_angle_weight(2.0); - let config_score = assoc.quality_score_with_config(&config); - assert!(config_score > default_score); - } - - #[test] - fn test_find_associations_identical_lines() { - let scan_lines = vec![Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(5.0, 0.0))]; - let map_lines = vec![Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(5.0, 0.0))]; - - let config = AssociationConfig::default(); - let assocs = find_associations(&scan_lines, &map_lines, None, &config); - - assert_eq!(assocs.len(), 1); - assert_eq!(assocs[0].scan_line_idx, 0); - assert_eq!(assocs[0].map_line_idx, 0); - assert!(assocs[0].distance < 0.01); - assert!(assocs[0].overlap_ratio > 0.99); - } - - #[test] - fn test_find_associations_parallel_lines() { - // Parallel lines with small offset - let scan_lines = vec![Line2D::new(Point2D::new(0.0, 0.1), Point2D::new(5.0, 0.1))]; - let map_lines = vec![Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(5.0, 0.0))]; - - let config = AssociationConfig::default(); - let assocs = find_associations(&scan_lines, &map_lines, None, &config); - - assert_eq!(assocs.len(), 1); - assert_relative_eq!(assocs[0].distance, 0.1, epsilon = 0.01); - } - - #[test] - fn test_find_associations_no_match_angle() { - // Perpendicular lines - let scan_lines = vec![Line2D::new(Point2D::new(2.5, -1.0), Point2D::new(2.5, 1.0))]; - let map_lines = vec![Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(5.0, 0.0))]; - - let config = AssociationConfig::default(); - let assocs = find_associations(&scan_lines, &map_lines, None, &config); - - // Should not match due to angle difference - assert!(assocs.is_empty()); - } - - #[test] - fn test_find_associations_no_match_distance() { - // Parallel but far apart - let scan_lines = vec![Line2D::new(Point2D::new(0.0, 1.0), Point2D::new(5.0, 1.0))]; - let map_lines = vec![Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(5.0, 0.0))]; - - let config = AssociationConfig::default().with_max_perpendicular_distance(0.5); - let assocs = find_associations(&scan_lines, &map_lines, None, &config); - - // Should not match due to distance - assert!(assocs.is_empty()); - } - - #[test] - fn test_find_associations_partial_overlap() { - // Overlapping lines - let scan_lines = vec![Line2D::new(Point2D::new(2.0, 0.0), Point2D::new(7.0, 0.0))]; - let map_lines = vec![Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(5.0, 0.0))]; - - let config = AssociationConfig::default(); - let assocs = find_associations(&scan_lines, &map_lines, None, &config); - - assert_eq!(assocs.len(), 1); - assert!(assocs[0].overlap_length > 2.0); // Should overlap by 3 meters - } - - #[test] - fn test_find_associations_multiple() { - let scan_lines = vec![ - Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(5.0, 0.0)), - Line2D::new(Point2D::new(5.0, 0.0), Point2D::new(5.0, 5.0)), - ]; - let map_lines = vec![ - Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(5.0, 0.0)), - Line2D::new(Point2D::new(5.0, 0.0), Point2D::new(5.0, 5.0)), - ]; - - let config = AssociationConfig::default(); - let assocs = find_associations(&scan_lines, &map_lines, None, &config); - - assert_eq!(assocs.len(), 2); - } - - #[test] - fn test_find_unique_associations() { - // Two scan lines that could match the same map line - let scan_lines = vec![ - Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(2.5, 0.0)), - Line2D::new(Point2D::new(2.5, 0.0), Point2D::new(5.0, 0.0)), - ]; - let map_lines = vec![Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(5.0, 0.0))]; - - let config = AssociationConfig::default(); - let assocs = find_unique_associations(&scan_lines, &map_lines, None, &config); - - // Should only have one association (best match) - assert_eq!(assocs.len(), 1); - } - - #[test] - fn test_find_unmatched() { - let scan_lines = vec![ - Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(5.0, 0.0)), - Line2D::new(Point2D::new(0.0, 10.0), Point2D::new(5.0, 10.0)), // No match - ]; - let map_lines = vec![Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(5.0, 0.0))]; - - let config = AssociationConfig::default(); - let unmatched = find_unmatched_scan_lines(&scan_lines, &map_lines, None, &config); - - assert_eq!(unmatched.len(), 1); - assert_eq!(unmatched[0], 1); // Second scan line is unmatched - } - - #[test] - fn test_with_spatial_index() { - let scan_lines = vec![Line2D::new(Point2D::new(0.0, 0.1), Point2D::new(5.0, 0.1))]; - let map_lines = vec![ - Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(5.0, 0.0)), - Line2D::new(Point2D::new(0.0, 10.0), Point2D::new(5.0, 10.0)), - ]; - - let index = SpatialIndex::new(&map_lines); - let config = AssociationConfig::default(); - - let assocs = find_associations(&scan_lines, &map_lines, Some(&index), &config); - - assert_eq!(assocs.len(), 1); - assert_eq!(assocs[0].map_line_idx, 0); - } -} diff --git a/vastu-map/src/integration/merger.rs b/vastu-map/src/integration/merger.rs deleted file mode 100644 index f93494b..0000000 --- a/vastu-map/src/integration/merger.rs +++ /dev/null @@ -1,1017 +0,0 @@ -//! Feature merging for map integration. -//! -//! Merges associated lines into updated map lines, extending -//! endpoints and updating parameters based on observations. - -use serde::{Deserialize, Serialize}; - -use crate::core::Point2D; -use crate::features::Line2D; - -use super::association::Association; - -/// Configuration for line merging. -#[derive(Clone, Debug, Serialize, Deserialize)] -pub struct MergerConfig { - /// Weight given to existing map line (vs. scan line). - /// Higher values make the map more stable but slower to adapt. - /// Default: 0.8 (map line has 80% weight) - pub map_weight: f32, - - /// Whether to extend line endpoints based on observations. - /// Default: true - pub extend_endpoints: bool, - - /// Maximum endpoint extension per observation (meters). - /// Default: 0.5m - pub max_extension: f32, - - /// Minimum extension required to actually extend (meters). - /// Small extensions are ignored to avoid noise accumulation. - /// Default: 0.05m - pub min_extension: f32, -} - -impl Default for MergerConfig { - fn default() -> Self { - Self { - map_weight: 0.8, - extend_endpoints: true, - max_extension: 0.5, - min_extension: 0.05, - } - } -} - -impl MergerConfig { - /// Create a new configuration with default values. - pub fn new() -> Self { - Self::default() - } - - /// Builder-style setter for map weight. - pub fn with_map_weight(mut self, weight: f32) -> Self { - self.map_weight = weight; - self - } - - /// Builder-style setter for endpoint extension. - pub fn with_extend_endpoints(mut self, extend: bool) -> Self { - self.extend_endpoints = extend; - self - } - - /// Builder-style setter for maximum extension. - pub fn with_max_extension(mut self, meters: f32) -> Self { - self.max_extension = meters; - self - } -} - -/// Configuration for coplanar line merging optimization. -/// -/// Used by [`optimize_coplanar_lines`] to merge multiple lines -/// that lie on the same infinite line (within tolerances). -#[derive(Clone, Debug, Serialize, Deserialize)] -pub struct CoplanarMergeConfig { - /// Maximum angle difference for lines to be considered coplanar (radians). - /// Default: 0.052 rad (~3°) - pub max_angle_diff: f32, - - /// Maximum perpendicular distance between lines (meters). - /// Default: 0.05m (5cm) - pub max_perpendicular_dist: f32, - - /// Maximum gap between line endpoints to allow merging (meters). - /// Lines that are coplanar but separated by more than this gap - /// will not be merged. - /// Default: 0.3m (30cm) - pub max_gap: f32, - - /// Minimum point count for a line to become the dominant line. - /// Lines with fewer scan points may be absorbed but won't be chosen - /// as the base direction for merging. - /// Default: 10 - pub min_dominant_point_count: u32, -} - -impl Default for CoplanarMergeConfig { - fn default() -> Self { - Self { - max_angle_diff: 0.052, // ~3 degrees - max_perpendicular_dist: 0.05, // 5cm - max_gap: 0.3, // 30cm - min_dominant_point_count: 10, - } - } -} - -impl CoplanarMergeConfig { - /// Create a new configuration with default values. - pub fn new() -> Self { - Self::default() - } - - /// Builder-style setter for maximum angle difference. - pub fn with_max_angle_diff(mut self, radians: f32) -> Self { - self.max_angle_diff = radians; - self - } - - /// Builder-style setter for maximum perpendicular distance. - pub fn with_max_perpendicular_dist(mut self, meters: f32) -> Self { - self.max_perpendicular_dist = meters; - self - } - - /// Builder-style setter for maximum gap. - pub fn with_max_gap(mut self, meters: f32) -> Self { - self.max_gap = meters; - self - } - - /// Builder-style setter for minimum dominant point count. - pub fn with_min_dominant_point_count(mut self, count: u32) -> Self { - self.min_dominant_point_count = count; - self - } -} - -/// Result of merging a line. -#[derive(Clone, Debug)] -pub struct MergeResult { - /// The merged line. - pub line: Line2D, - /// Whether the line was extended at the start. - pub extended_start: bool, - /// Whether the line was extended at the end. - pub extended_end: bool, - /// Amount extended at start (meters, positive = extended). - pub start_extension: f32, - /// Amount extended at end (meters, positive = extended). - pub end_extension: f32, -} - -impl MergeResult { - /// Create a new merge result with no extension. - pub fn new(line: Line2D) -> Self { - Self { - line, - extended_start: false, - extended_end: false, - start_extension: 0.0, - end_extension: 0.0, - } - } -} - -/// Merge a scan line with a map line based on their association. -/// -/// # Arguments -/// * `scan_line` - The new observation -/// * `map_line` - The existing map line -/// * `association` - Association between the lines -/// * `config` - Merger configuration -/// -/// # Returns -/// Merge result with the updated line. -pub fn merge_lines( - scan_line: &Line2D, - map_line: &Line2D, - _association: &Association, - config: &MergerConfig, -) -> MergeResult { - let scan_weight = 1.0 - config.map_weight; - - // Merge direction using weighted average - let map_dir = map_line.unit_direction(); - let scan_dir = scan_line.unit_direction(); - - // Ensure directions are aligned (not pointing opposite ways) - let dot = map_dir.dot(scan_dir); - let aligned_scan_dir = if dot < 0.0 { -scan_dir } else { scan_dir }; - - // Weighted average of directions - let merged_dir = Point2D::new( - map_dir.x * config.map_weight + aligned_scan_dir.x * scan_weight, - map_dir.y * config.map_weight + aligned_scan_dir.y * scan_weight, - ) - .normalized(); - - // Get all four endpoints - let scan_start = scan_line.start; - let scan_end = scan_line.end; - let map_start = map_line.start; - let map_end = map_line.end; - - // Project all endpoints onto the merged direction line - // Use the map line's midpoint as reference - let reference = map_line.midpoint(); - - let project = |p: Point2D| -> f32 { (p - reference).dot(merged_dir) }; - - let scan_start_t = project(scan_start); - let scan_end_t = project(scan_end); - let map_start_t = project(map_start); - let map_end_t = project(map_end); - - // Find the extent of the merged line - let mut min_t = map_start_t.min(map_end_t); - let mut max_t = map_start_t.max(map_end_t); - - let mut extended_start = false; - let mut extended_end = false; - let mut start_extension = 0.0f32; - let mut end_extension = 0.0f32; - - if config.extend_endpoints { - // Check if scan extends beyond map - let scan_min_t = scan_start_t.min(scan_end_t); - let scan_max_t = scan_start_t.max(scan_end_t); - - if scan_min_t < min_t { - let extension = (min_t - scan_min_t).min(config.max_extension); - if extension >= config.min_extension { - min_t -= extension; - extended_start = true; - start_extension = extension; - } - } - - if scan_max_t > max_t { - let extension = (scan_max_t - max_t).min(config.max_extension); - if extension >= config.min_extension { - max_t += extension; - extended_end = true; - end_extension = extension; - } - } - } - - // Compute new endpoints - let new_start = reference + merged_dir * min_t; - let new_end = reference + merged_dir * max_t; - - // Average perpendicular offset (to preserve line position) - let map_perp = map_line.normal(); - let map_offset = (map_start - reference).dot(map_perp); - let scan_perp_offset = (scan_line.midpoint() - reference).dot(map_perp); - let merged_offset = map_offset * config.map_weight + scan_perp_offset * scan_weight; - - let perp = Point2D::new(-merged_dir.y, merged_dir.x); - let final_start = new_start + perp * merged_offset; - let final_end = new_end + perp * merged_offset; - - // Create merged line with incremented observation count and summed point count - let merged_line = Line2D::full( - final_start, - final_end, - map_line.observation_count.saturating_add(1), - map_line.point_count.saturating_add(scan_line.point_count), - ); - - MergeResult { - line: merged_line, - extended_start, - extended_end, - start_extension, - end_extension, - } -} - -/// Batch merge multiple associations. -/// -/// # Arguments -/// * `scan_lines` - All scan lines -/// * `map_lines` - All map lines (mutable for in-place update) -/// * `associations` - Associations between scan and map lines -/// * `config` - Merger configuration -/// -/// # Returns -/// Vector of (map_line_idx, MergeResult) for each merge performed. -pub fn batch_merge( - scan_lines: &[Line2D], - map_lines: &mut [Line2D], - associations: &[Association], - config: &MergerConfig, -) -> Vec<(usize, MergeResult)> { - let mut results = Vec::with_capacity(associations.len()); - - for assoc in associations { - let scan_line = &scan_lines[assoc.scan_line_idx]; - let map_line = &map_lines[assoc.map_line_idx]; - - let result = merge_lines(scan_line, map_line, assoc, config); - - // Update map line in place - map_lines[assoc.map_line_idx] = result.line; - - results.push((assoc.map_line_idx, result)); - } - - results -} - -/// Create a new line from a scan observation (for unmatched scan lines). -/// -/// This is used when adding new features to the map. -/// Preserves the scan line's point_count for dominance tracking. -pub fn create_new_line(scan_line: &Line2D) -> Line2D { - Line2D::full(scan_line.start, scan_line.end, 1, scan_line.point_count) -} - -/// Merge two line segments that represent the same physical feature. -/// -/// This is a simpler merge that just extends the endpoints. -pub fn merge_collinear_lines(line1: &Line2D, line2: &Line2D) -> Option { - // Check if lines are approximately collinear - let angle_diff = crate::core::math::angle_diff(line1.angle(), line2.angle()).abs(); - let angle_diff = angle_diff.min(std::f32::consts::PI - angle_diff); - - if angle_diff > 0.1 { - // ~5.7 degrees - return None; - } - - // Check perpendicular distance - let dist1 = line1.distance_to_point(line2.start); - let dist2 = line1.distance_to_point(line2.end); - let max_dist = dist1.max(dist2); - - if max_dist > 0.1 { - return None; - } - - // Merge by finding the extended endpoints - let dir = line1.unit_direction(); - let reference = line1.midpoint(); - - let project = |p: Point2D| -> f32 { (p - reference).dot(dir) }; - - let t1 = project(line1.start); - let t2 = project(line1.end); - let t3 = project(line2.start); - let t4 = project(line2.end); - - let min_t = t1.min(t2).min(t3).min(t4); - let max_t = t1.max(t2).max(t3).max(t4); - - let new_start = reference + dir * min_t; - let new_end = reference + dir * max_t; - - let obs_count = line1 - .observation_count - .saturating_add(line2.observation_count); - let pt_count = line1.point_count.saturating_add(line2.point_count); - - Some(Line2D::full(new_start, new_end, obs_count, pt_count)) -} - -/// Merge two line segments using configurable thresholds. -/// -/// This is like [`merge_collinear_lines`] but uses explicit configuration -/// instead of hardcoded values. -pub fn merge_collinear_lines_with_config( - line1: &Line2D, - line2: &Line2D, - config: &CoplanarMergeConfig, -) -> Option { - if !are_lines_coplanar(line1, line2, config) { - return None; - } - - // Merge by finding the extended endpoints - let dir = line1.unit_direction(); - let reference = line1.midpoint(); - - let project = |p: Point2D| -> f32 { (p - reference).dot(dir) }; - - let t1 = project(line1.start); - let t2 = project(line1.end); - let t3 = project(line2.start); - let t4 = project(line2.end); - - let min_t = t1.min(t2).min(t3).min(t4); - let max_t = t1.max(t2).max(t3).max(t4); - - let new_start = reference + dir * min_t; - let new_end = reference + dir * max_t; - - let obs_count = line1 - .observation_count - .saturating_add(line2.observation_count); - let pt_count = line1.point_count.saturating_add(line2.point_count); - - Some(Line2D::full(new_start, new_end, obs_count, pt_count)) -} - -/// Check if two lines are coplanar (on the same infinite line within tolerances). -/// -/// Two lines are considered coplanar if: -/// 1. Their angles differ by less than `max_angle_diff` (handling 180° ambiguity) -/// 2. The perpendicular distance between them is less than `max_perpendicular_dist` -fn are_lines_coplanar(line1: &Line2D, line2: &Line2D, config: &CoplanarMergeConfig) -> bool { - // Check angular compatibility - let angle_diff = crate::core::math::angle_diff(line1.angle(), line2.angle()).abs(); - let angle_diff = angle_diff.min(std::f32::consts::PI - angle_diff); - - if angle_diff > config.max_angle_diff { - return false; - } - - // Check perpendicular distance using midpoint of line2 - let dist = line1.distance_to_point(line2.midpoint()); - dist <= config.max_perpendicular_dist -} - -/// Check if two coplanar lines can be merged (overlap or small gap). -/// -/// Lines can be merged if they are coplanar AND either: -/// - They overlap, OR -/// - The gap between their closest endpoints is less than `max_gap` -fn lines_can_merge(line1: &Line2D, line2: &Line2D, config: &CoplanarMergeConfig) -> bool { - if !are_lines_coplanar(line1, line2, config) { - return false; - } - - // Project line2's endpoints onto line1's direction to check overlap/gap - let dir = line1.unit_direction(); - let reference = line1.midpoint(); - - let project = |p: Point2D| -> f32 { (p - reference).dot(dir) }; - - let t1_min = project(line1.start).min(project(line1.end)); - let t1_max = project(line1.start).max(project(line1.end)); - let t2_min = project(line2.start).min(project(line2.end)); - let t2_max = project(line2.start).max(project(line2.end)); - - // Check for overlap - if t1_max >= t2_min && t2_max >= t1_min { - return true; - } - - // Check gap - let gap = if t1_max < t2_min { - t2_min - t1_max - } else { - t1_min - t2_max - }; - - gap <= config.max_gap -} - -/// Merge a group of coplanar lines into a single extended line. -/// -/// Uses the line with most scan points as the base direction (for robust dominance). -/// Falls back to observation_count if no line has point_count set. -/// Both observation_count and point_count are summed. -/// -/// # Panics -/// Panics if `lines` is empty. -fn merge_coplanar_group(lines: &[&Line2D]) -> Line2D { - assert!(!lines.is_empty(), "Cannot merge empty group"); - - if lines.len() == 1 { - return *lines[0]; - } - - // Find the dominant line (most scan points, fallback to observation count) - let dominant = lines - .iter() - .max_by_key(|l| { - // Use point_count if available, otherwise fall back to observation_count - if l.point_count > 0 { - l.point_count - } else { - l.observation_count - } - }) - .unwrap(); - - let dir = dominant.unit_direction(); - let reference = dominant.midpoint(); - - let project = |p: Point2D| -> f32 { (p - reference).dot(dir) }; - - // Find extent across all lines and sum counts - let mut min_t = f32::MAX; - let mut max_t = f32::MIN; - let mut total_obs_count: u32 = 0; - let mut total_pt_count: u32 = 0; - - for line in lines { - let t_start = project(line.start); - let t_end = project(line.end); - min_t = min_t.min(t_start).min(t_end); - max_t = max_t.max(t_start).max(t_end); - total_obs_count = total_obs_count.saturating_add(line.observation_count); - total_pt_count = total_pt_count.saturating_add(line.point_count); - } - - let new_start = reference + dir * min_t; - let new_end = reference + dir * max_t; - - Line2D::full(new_start, new_end, total_obs_count, total_pt_count) -} - -/// Optimize map lines by merging coplanar segments. -/// -/// Groups lines by coplanarity (similar angle + perpendicular offset) and merges -/// each group into a single extended line using the most-observed line as the base. -/// -/// # Arguments -/// * `lines` - Mutable vector of lines to optimize (modified in place) -/// * `config` - Coplanar merge configuration -/// -/// # Returns -/// Number of lines merged (removed from the vector) -/// -/// # Example -/// ```rust,ignore -/// use vastu_map::integration::{optimize_coplanar_lines, CoplanarMergeConfig}; -/// -/// let mut lines = vec![...]; // Multiple lines on the same wall -/// let config = CoplanarMergeConfig::default(); -/// let merged_count = optimize_coplanar_lines(&mut lines, &config); -/// println!("Merged {} lines", merged_count); -/// ``` -pub fn optimize_coplanar_lines(lines: &mut Vec, config: &CoplanarMergeConfig) -> usize { - if lines.len() < 2 { - return 0; - } - - let n = lines.len(); - - // Union-Find for grouping coplanar lines - let mut parent: Vec = (0..n).collect(); - let mut rank: Vec = vec![0; n]; - - fn find(parent: &mut [usize], i: usize) -> usize { - if parent[i] != i { - parent[i] = find(parent, parent[i]); - } - parent[i] - } - - fn union(parent: &mut [usize], rank: &mut [usize], i: usize, j: usize) { - let pi = find(parent, i); - let pj = find(parent, j); - if pi != pj { - if rank[pi] < rank[pj] { - parent[pi] = pj; - } else if rank[pi] > rank[pj] { - parent[pj] = pi; - } else { - parent[pj] = pi; - rank[pi] += 1; - } - } - } - - // Group coplanar lines that can be merged - for i in 0..n { - for j in (i + 1)..n { - if lines_can_merge(&lines[i], &lines[j], config) { - union(&mut parent, &mut rank, i, j); - } - } - } - - // Collect groups - let mut groups: std::collections::HashMap> = std::collections::HashMap::new(); - for i in 0..n { - let root = find(&mut parent, i); - groups.entry(root).or_default().push(i); - } - - // Process each group and build result - let mut result: Vec = Vec::with_capacity(n); - let mut merged_count = 0; - - for (_root, indices) in groups { - if indices.len() == 1 { - // Single line, keep as-is - result.push(lines[indices[0]]); - } else { - // Check if any line in the group has enough scan points to be dominant - let has_dominant = indices.iter().any(|&i| { - let line = &lines[i]; - // Use point_count if available, fall back to observation_count - let confidence = if line.point_count > 0 { - line.point_count - } else { - line.observation_count - }; - confidence >= config.min_dominant_point_count - }); - - if !has_dominant { - // No dominant line, keep all lines as-is - for &idx in &indices { - result.push(lines[idx]); - } - } else { - // Merge the group - let group_lines: Vec<&Line2D> = indices.iter().map(|&i| &lines[i]).collect(); - let merged = merge_coplanar_group(&group_lines); - result.push(merged); - merged_count += indices.len() - 1; - } - } - } - - *lines = result; - merged_count -} - -#[cfg(test)] -mod tests { - use super::*; - use approx::assert_relative_eq; - - #[test] - fn test_merge_identical_lines() { - let scan = Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(5.0, 0.0)); - let map = Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(5.0, 0.0)); - let assoc = Association::new(0, 0, 0.0, 0.0, 5.0, 1.0); - - let config = MergerConfig::default(); - let result = merge_lines(&scan, &map, &assoc, &config); - - assert_relative_eq!(result.line.length(), 5.0, epsilon = 0.01); - assert!(!result.extended_start); - assert!(!result.extended_end); - } - - #[test] - fn test_merge_extends_line() { - let scan = Line2D::new(Point2D::new(-1.0, 0.0), Point2D::new(6.0, 0.0)); - let map = Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(5.0, 0.0)); - let assoc = Association::new(0, 0, 0.0, 0.0, 5.0, 1.0); - - let config = MergerConfig::default(); - let result = merge_lines(&scan, &map, &assoc, &config); - - assert!(result.line.length() > 5.0); - assert!(result.extended_start || result.extended_end); - } - - #[test] - fn test_merge_respects_max_extension() { - let scan = Line2D::new(Point2D::new(-5.0, 0.0), Point2D::new(10.0, 0.0)); - let map = Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(5.0, 0.0)); - let assoc = Association::new(0, 0, 0.0, 0.0, 5.0, 1.0); - - let config = MergerConfig::default().with_max_extension(0.5); - let result = merge_lines(&scan, &map, &assoc, &config); - - // Should not extend by more than 0.5m on each side - assert!(result.line.length() <= 6.0); // Original 5.0 + max 0.5 + 0.5 - } - - #[test] - fn test_merge_weighted_average() { - // Scan line slightly offset from map line - let scan = Line2D::new(Point2D::new(0.0, 0.1), Point2D::new(5.0, 0.1)); - let map = Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(5.0, 0.0)); - let assoc = Association::new(0, 0, 0.1, 0.0, 5.0, 1.0); - - let config = MergerConfig::default().with_map_weight(0.8); - let result = merge_lines(&scan, &map, &assoc, &config); - - // Result should be closer to map line (weighted 80%) - let mid_y = result.line.midpoint().y; - assert!(mid_y < 0.05); // Closer to map's y=0 than scan's y=0.1 - } - - #[test] - fn test_batch_merge() { - let scan_lines = vec![ - Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(5.0, 0.0)), - Line2D::new(Point2D::new(5.0, 0.0), Point2D::new(5.0, 5.0)), - ]; - let mut map_lines = vec![ - Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(5.0, 0.0)), - Line2D::new(Point2D::new(5.0, 0.0), Point2D::new(5.0, 5.0)), - ]; - let associations = vec![ - Association::new(0, 0, 0.0, 0.0, 5.0, 1.0), - Association::new(1, 1, 0.0, 0.0, 5.0, 1.0), - ]; - - let config = MergerConfig::default(); - let results = batch_merge(&scan_lines, &mut map_lines, &associations, &config); - - assert_eq!(results.len(), 2); - // Observation counts should be incremented - assert!(map_lines[0].observation_count > 1); - assert!(map_lines[1].observation_count > 1); - } - - #[test] - fn test_create_new_line() { - let scan = Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(5.0, 0.0)); - let new_line = create_new_line(&scan); - - assert_eq!(new_line.start, scan.start); - assert_eq!(new_line.end, scan.end); - assert_eq!(new_line.observation_count, 1); - } - - #[test] - fn test_merge_collinear_lines() { - let line1 = Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(3.0, 0.0)); - let line2 = Line2D::new(Point2D::new(2.0, 0.0), Point2D::new(5.0, 0.0)); - - let merged = merge_collinear_lines(&line1, &line2); - - assert!(merged.is_some()); - let merged = merged.unwrap(); - assert_relative_eq!(merged.length(), 5.0, epsilon = 0.01); - } - - #[test] - fn test_merge_collinear_rejects_perpendicular() { - let line1 = Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(5.0, 0.0)); - let line2 = Line2D::new(Point2D::new(2.5, -2.0), Point2D::new(2.5, 2.0)); - - let merged = merge_collinear_lines(&line1, &line2); - - assert!(merged.is_none()); - } - - #[test] - fn test_merge_collinear_rejects_far() { - let line1 = Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(5.0, 0.0)); - let line2 = Line2D::new(Point2D::new(0.0, 1.0), Point2D::new(5.0, 1.0)); - - let merged = merge_collinear_lines(&line1, &line2); - - assert!(merged.is_none()); - } - - // ===== Coplanar merge config tests ===== - - #[test] - fn test_coplanar_merge_config_default() { - let config = CoplanarMergeConfig::default(); - - assert_relative_eq!(config.max_angle_diff, 0.052, epsilon = 0.001); - assert_relative_eq!(config.max_perpendicular_dist, 0.05, epsilon = 0.001); - assert_relative_eq!(config.max_gap, 0.3, epsilon = 0.001); - assert_eq!(config.min_dominant_point_count, 10); - } - - #[test] - fn test_coplanar_merge_config_builders() { - let config = CoplanarMergeConfig::new() - .with_max_angle_diff(0.2) - .with_max_perpendicular_dist(0.08) - .with_max_gap(1.0) - .with_min_dominant_point_count(5); - - assert_relative_eq!(config.max_angle_diff, 0.2, epsilon = 0.001); - assert_relative_eq!(config.max_perpendicular_dist, 0.08, epsilon = 0.001); - assert_relative_eq!(config.max_gap, 1.0, epsilon = 0.001); - assert_eq!(config.min_dominant_point_count, 5); - } - - // ===== are_lines_coplanar tests ===== - - #[test] - fn test_are_lines_coplanar_same_line() { - let line1 = Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(5.0, 0.0)); - let line2 = Line2D::new(Point2D::new(3.0, 0.0), Point2D::new(8.0, 0.0)); - let config = CoplanarMergeConfig::default(); - - assert!(are_lines_coplanar(&line1, &line2, &config)); - } - - #[test] - fn test_are_lines_coplanar_parallel_close() { - let line1 = Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(5.0, 0.0)); - let line2 = Line2D::new(Point2D::new(0.0, 0.05), Point2D::new(5.0, 0.05)); // 5cm offset - let config = CoplanarMergeConfig::default(); - - assert!(are_lines_coplanar(&line1, &line2, &config)); - } - - #[test] - fn test_are_lines_coplanar_rejects_angle() { - let line1 = Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(5.0, 0.0)); - let line2 = Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(5.0, 1.0)); // ~11° angle - let config = CoplanarMergeConfig::default(); - - assert!(!are_lines_coplanar(&line1, &line2, &config)); - } - - #[test] - fn test_are_lines_coplanar_rejects_distance() { - let line1 = Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(5.0, 0.0)); - let line2 = Line2D::new(Point2D::new(0.0, 0.5), Point2D::new(5.0, 0.5)); // 50cm offset - let config = CoplanarMergeConfig::default(); - - assert!(!are_lines_coplanar(&line1, &line2, &config)); - } - - #[test] - fn test_are_lines_coplanar_opposite_direction() { - // Lines pointing in opposite directions should still be coplanar - let line1 = Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(5.0, 0.0)); - let line2 = Line2D::new(Point2D::new(10.0, 0.0), Point2D::new(6.0, 0.0)); // Opposite direction - let config = CoplanarMergeConfig::default(); - - assert!(are_lines_coplanar(&line1, &line2, &config)); - } - - // ===== lines_can_merge tests ===== - - #[test] - fn test_lines_can_merge_overlapping() { - let line1 = Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(5.0, 0.0)); - let line2 = Line2D::new(Point2D::new(3.0, 0.0), Point2D::new(8.0, 0.0)); - let config = CoplanarMergeConfig::default(); - - assert!(lines_can_merge(&line1, &line2, &config)); - } - - #[test] - fn test_lines_can_merge_with_small_gap() { - let line1 = Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(3.0, 0.0)); - let line2 = Line2D::new(Point2D::new(3.3, 0.0), Point2D::new(6.0, 0.0)); // 0.3m gap - let config = CoplanarMergeConfig::default(); - - assert!(lines_can_merge(&line1, &line2, &config)); - } - - #[test] - fn test_lines_can_merge_rejects_large_gap() { - let line1 = Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(3.0, 0.0)); - let line2 = Line2D::new(Point2D::new(4.0, 0.0), Point2D::new(7.0, 0.0)); // 1.0m gap - let config = CoplanarMergeConfig::default(); - - assert!(!lines_can_merge(&line1, &line2, &config)); - } - - // ===== optimize_coplanar_lines tests ===== - - #[test] - fn test_optimize_coplanar_lines_merges_overlapping() { - let mut lines = vec![ - Line2D::with_point_count( - Point2D::new(0.0, 0.0), - Point2D::new(3.0, 0.0), - 15, // Above threshold - ), - Line2D::with_point_count(Point2D::new(2.0, 0.0), Point2D::new(5.0, 0.0), 12), - ]; - let config = CoplanarMergeConfig::default(); - - let merged_count = optimize_coplanar_lines(&mut lines, &config); - - assert_eq!(merged_count, 1); - assert_eq!(lines.len(), 1); - assert_relative_eq!(lines[0].length(), 5.0, epsilon = 0.01); - assert_eq!(lines[0].point_count, 27); // 15 + 12 - } - - #[test] - fn test_optimize_coplanar_lines_merges_with_gap() { - let mut lines = vec![ - Line2D::with_point_count(Point2D::new(0.0, 0.0), Point2D::new(2.0, 0.0), 12), - Line2D::with_point_count( - Point2D::new(2.2, 0.0), // 0.2m gap, within default 0.3m - Point2D::new(5.0, 0.0), - 15, - ), - ]; - let config = CoplanarMergeConfig::default(); - - let merged_count = optimize_coplanar_lines(&mut lines, &config); - - assert_eq!(merged_count, 1); - assert_eq!(lines.len(), 1); - assert_relative_eq!(lines[0].length(), 5.0, epsilon = 0.01); - } - - #[test] - fn test_optimize_coplanar_lines_uses_dominant() { - // The line with most points should determine the direction - let mut lines = vec![ - Line2D::with_point_count( - Point2D::new(0.0, 0.0), - Point2D::new(2.0, 0.0), - 25, // Dominant (most points) - ), - Line2D::with_point_count( - Point2D::new(2.0, 0.0), - Point2D::new(4.0, 0.01), // Slightly off - 10, - ), - ]; - let config = CoplanarMergeConfig::default(); - - let merged_count = optimize_coplanar_lines(&mut lines, &config); - - assert_eq!(merged_count, 1); - assert_eq!(lines.len(), 1); - // The merged line should use the dominant line's direction - // So it should be close to horizontal - assert!(lines[0].angle().abs() < 0.02); - } - - #[test] - fn test_optimize_coplanar_lines_respects_max_gap() { - let mut lines = vec![ - Line2D::with_point_count(Point2D::new(0.0, 0.0), Point2D::new(2.0, 0.0), 15), - Line2D::with_point_count( - Point2D::new(4.0, 0.0), // 2.0m gap, above default 0.3m - Point2D::new(6.0, 0.0), - 15, - ), - ]; - let config = CoplanarMergeConfig::default(); // max_gap = 0.3m - - let merged_count = optimize_coplanar_lines(&mut lines, &config); - - assert_eq!(merged_count, 0); - assert_eq!(lines.len(), 2); - } - - #[test] - fn test_optimize_coplanar_lines_keeps_non_dominant() { - // When no line has enough points, don't merge - let mut lines = vec![ - Line2D::with_point_count( - Point2D::new(0.0, 0.0), - Point2D::new(2.0, 0.0), - 5, // Below threshold of 10 - ), - Line2D::with_point_count( - Point2D::new(2.0, 0.0), - Point2D::new(4.0, 0.0), - 5, // Below threshold of 10 - ), - ]; - let config = CoplanarMergeConfig::default(); // min_dominant_point_count = 10 - - let merged_count = optimize_coplanar_lines(&mut lines, &config); - - assert_eq!(merged_count, 0); - assert_eq!(lines.len(), 2); - } - - #[test] - fn test_optimize_coplanar_lines_multiple_groups() { - // Two separate walls (horizontal and vertical) - let mut lines = vec![ - // Horizontal wall (will merge) - Line2D::with_point_count(Point2D::new(0.0, 0.0), Point2D::new(2.0, 0.0), 15), - Line2D::with_point_count(Point2D::new(2.0, 0.0), Point2D::new(4.0, 0.0), 12), - // Vertical wall (will merge) - Line2D::with_point_count(Point2D::new(5.0, 0.0), Point2D::new(5.0, 2.0), 15), - Line2D::with_point_count(Point2D::new(5.0, 2.0), Point2D::new(5.0, 4.0), 12), - ]; - let config = CoplanarMergeConfig::default(); - - let merged_count = optimize_coplanar_lines(&mut lines, &config); - - assert_eq!(merged_count, 2); // Merged 2 lines (one from each group) - assert_eq!(lines.len(), 2); // Two merged lines remain - } - - #[test] - fn test_optimize_coplanar_lines_empty() { - let mut lines: Vec = vec![]; - let config = CoplanarMergeConfig::default(); - - let merged_count = optimize_coplanar_lines(&mut lines, &config); - - assert_eq!(merged_count, 0); - assert_eq!(lines.len(), 0); - } - - #[test] - fn test_optimize_coplanar_lines_single() { - let mut lines = vec![Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(5.0, 0.0))]; - let config = CoplanarMergeConfig::default(); - - let merged_count = optimize_coplanar_lines(&mut lines, &config); - - assert_eq!(merged_count, 0); - assert_eq!(lines.len(), 1); - } - - #[test] - fn test_merge_collinear_lines_with_config() { - let line1 = Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(3.0, 0.0)); - let line2 = Line2D::new(Point2D::new(2.0, 0.0), Point2D::new(5.0, 0.0)); - let config = CoplanarMergeConfig::default(); - - let merged = merge_collinear_lines_with_config(&line1, &line2, &config); - - assert!(merged.is_some()); - let merged = merged.unwrap(); - assert_relative_eq!(merged.length(), 5.0, epsilon = 0.01); - } -} diff --git a/vastu-map/src/integration/mod.rs b/vastu-map/src/integration/mod.rs deleted file mode 100644 index 03598a8..0000000 --- a/vastu-map/src/integration/mod.rs +++ /dev/null @@ -1,64 +0,0 @@ -//! Map integration for combining scan features with the map. -//! -//! This module provides algorithms for: -//! - Spatial indexing (R-tree) for efficient queries -//! - Line association (matching scan lines to map lines) -//! - Feature merging (updating map lines with observations) -//! -//! # Integration Pipeline -//! -//! 1. **Associate**: Find correspondences between scan and map lines -//! 2. **Merge**: Update matched map lines with new observations -//! 3. **Add**: Insert unmatched scan lines as new map features -//! -//! # Example -//! -//! ```rust,ignore -//! use vastu_map::integration::{ -//! SpatialIndex, find_associations, batch_merge, create_new_line, -//! AssociationConfig, MergerConfig, -//! }; -//! use vastu_map::features::Line2D; -//! -//! // Extract features from scan -//! let scan_lines: Vec = extract_from_scan(); -//! -//! // Find associations with map -//! let index = SpatialIndex::new(&map_lines); -//! let assoc_config = AssociationConfig::default(); -//! let associations = find_associations(&scan_lines, &map_lines, Some(&index), &assoc_config); -//! -//! // Merge matched lines -//! let merger_config = MergerConfig::default(); -//! batch_merge(&scan_lines, &mut map_lines, &associations, &merger_config); -//! -//! // Add unmatched lines as new features -//! let unmatched = find_unmatched_scan_lines(&scan_lines, &map_lines, Some(&index), &assoc_config); -//! for idx in unmatched { -//! map_lines.push(create_new_line(&scan_lines[idx])); -//! } -//! ``` - -pub mod association; -pub mod merger; -pub mod point_association; -pub mod refitter; -pub mod scan_store; -pub mod spatial_index; - -// Re-export main types -pub use association::{ - Association, AssociationConfig, find_associations, find_unique_associations, - find_unmatched_scan_lines, -}; -pub use merger::{ - CoplanarMergeConfig, MergeResult, MergerConfig, batch_merge, create_new_line, - merge_collinear_lines, merge_collinear_lines_with_config, merge_lines, optimize_coplanar_lines, -}; -pub use point_association::{ - AssociatedPoint, PointAssociationConfig, PointAssociationResult, associate_points_to_lines, - associate_scan_points, extract_points_for_line, lines_with_enough_points, -}; -pub use refitter::{RefitConfig, RefitResult, RefitStats, refit_line, refit_line_simple}; -pub use scan_store::{ScanStore, ScanStoreConfig, ScanStoreStats, StoredScan}; -pub use spatial_index::{IndexedLine, SpatialIndex}; diff --git a/vastu-map/src/integration/point_association.rs b/vastu-map/src/integration/point_association.rs deleted file mode 100644 index 91da554..0000000 --- a/vastu-map/src/integration/point_association.rs +++ /dev/null @@ -1,544 +0,0 @@ -//! Point-to-line association for line re-fitting. -//! -//! Associates world-frame points from ScanStore with existing map lines, -//! enabling periodic re-fitting of lines from accumulated point clouds. - -use crate::core::Point2D; -use crate::features::Line2D; -use crate::integration::{ScanStore, SpatialIndex}; - -/// Configuration for point-to-line association. -#[derive(Clone, Debug)] -pub struct PointAssociationConfig { - /// Maximum perpendicular distance from line to associate a point (meters). - /// Points further than this from all lines are unassociated. - /// Default: 0.10m (10cm) - pub max_distance: f32, - - /// Maximum projection extension beyond line endpoints. - /// Points can project up to this fraction beyond line ends. - /// 0.0 = strict segment bounds, 0.2 = 20% extension allowed. - /// Default: 0.1 (10% extension) - pub max_projection_extension: f32, -} - -impl Default for PointAssociationConfig { - fn default() -> Self { - Self { - max_distance: 0.10, - max_projection_extension: 0.1, - } - } -} - -impl PointAssociationConfig { - /// Create a new configuration with defaults. - pub fn new() -> Self { - Self::default() - } - - /// Set maximum association distance. - pub fn with_max_distance(mut self, meters: f32) -> Self { - self.max_distance = meters; - self - } - - /// Set maximum projection extension. - pub fn with_max_projection_extension(mut self, fraction: f32) -> Self { - self.max_projection_extension = fraction; - self - } -} - -/// A point associated with a map line. -#[derive(Clone, Debug)] -pub struct AssociatedPoint { - /// World-frame point. - pub point: Point2D, - /// Scan ID the point came from. - pub scan_id: u32, - /// Perpendicular distance to the line. - pub distance: f32, - /// Projection parameter on the line (0=start, 1=end). - pub projection_t: f32, -} - -/// Result of point-to-line association. -#[derive(Clone, Debug)] -pub struct PointAssociationResult { - /// Points associated with each map line, indexed by line index. - pub line_points: Vec>, - /// Number of points associated. - pub total_associated: usize, - /// Number of points not associated (too far from all lines). - pub total_unassociated: usize, -} - -impl PointAssociationResult { - /// Get the total number of points processed. - pub fn total_points(&self) -> usize { - self.total_associated + self.total_unassociated - } - - /// Get association rate (0.0 - 1.0). - pub fn association_rate(&self) -> f32 { - let total = self.total_points(); - if total == 0 { - 0.0 - } else { - self.total_associated as f32 / total as f32 - } - } -} - -/// Associate all points from ScanStore with map lines. -/// -/// Each point is associated with its nearest line if within `max_distance` -/// and within projection bounds (considering extension). -/// -/// # Arguments -/// * `scan_store` - Storage containing accumulated scans -/// * `lines` - Map lines to associate points with -/// * `index` - Spatial index for efficient line lookup -/// * `config` - Association configuration -/// -/// # Returns -/// Association result with points grouped by line index. -pub fn associate_points_to_lines( - scan_store: &ScanStore, - lines: &[Line2D], - index: &SpatialIndex, - config: &PointAssociationConfig, -) -> PointAssociationResult { - let num_lines = lines.len(); - let mut line_points: Vec> = vec![Vec::new(); num_lines]; - let mut total_associated = 0usize; - let mut total_unassociated = 0usize; - - // Iterate through all scans and their world-frame points - for scan in scan_store.iter() { - let pose = &scan.estimated_pose; - - for robot_point in scan.points() { - // Transform to world frame - let world_point = pose.transform_point(*robot_point); - - // Find nearest line using spatial index - if let Some((line_idx, distance)) = index.nearest_line(world_point) { - // Check distance threshold - if distance <= config.max_distance && line_idx < num_lines { - let line = &lines[line_idx]; - - // Check projection bounds - let t = line.project_point(world_point); - if line.contains_projection_extended(t, config.max_projection_extension) { - line_points[line_idx].push(AssociatedPoint { - point: world_point, - scan_id: scan.scan_id, - distance, - projection_t: t, - }); - total_associated += 1; - continue; - } - } - } - total_unassociated += 1; - } - } - - PointAssociationResult { - line_points, - total_associated, - total_unassociated, - } -} - -/// Associate points from a single scan with map lines. -/// -/// Similar to `associate_points_to_lines` but for a single scan. -/// Useful for incremental association during observations. -pub fn associate_scan_points( - points: &[Point2D], - scan_id: u32, - lines: &[Line2D], - index: &SpatialIndex, - config: &PointAssociationConfig, -) -> PointAssociationResult { - let num_lines = lines.len(); - let mut line_points: Vec> = vec![Vec::new(); num_lines]; - let mut total_associated = 0usize; - let mut total_unassociated = 0usize; - - for &world_point in points { - if let Some((line_idx, distance)) = index.nearest_line(world_point) - && distance <= config.max_distance - && line_idx < num_lines - { - let line = &lines[line_idx]; - let t = line.project_point(world_point); - if line.contains_projection_extended(t, config.max_projection_extension) { - line_points[line_idx].push(AssociatedPoint { - point: world_point, - scan_id, - distance, - projection_t: t, - }); - total_associated += 1; - continue; - } - } - total_unassociated += 1; - } - - PointAssociationResult { - line_points, - total_associated, - total_unassociated, - } -} - -/// Get line indices that have enough points for re-fitting. -/// -/// Returns indices of lines with at least `min_points` associated points. -pub fn lines_with_enough_points(result: &PointAssociationResult, min_points: usize) -> Vec { - result - .line_points - .iter() - .enumerate() - .filter(|(_, points)| points.len() >= min_points) - .map(|(idx, _)| idx) - .collect() -} - -/// Extract just the points for a specific line (without metadata). -/// -/// Useful when only the point coordinates are needed for fitting. -pub fn extract_points_for_line(result: &PointAssociationResult, line_idx: usize) -> Vec { - result - .line_points - .get(line_idx) - .map(|pts| pts.iter().map(|ap| ap.point).collect()) - .unwrap_or_default() -} - -#[cfg(test)] -mod tests { - use super::*; - use crate::core::{PointCloud2D, Pose2D}; - - fn make_horizontal_line(y: f32, x_start: f32, x_end: f32) -> Line2D { - Line2D::new(Point2D::new(x_start, y), Point2D::new(x_end, y)) - } - - fn make_vertical_line(x: f32, y_start: f32, y_end: f32) -> Line2D { - Line2D::new(Point2D::new(x, y_start), Point2D::new(x, y_end)) - } - - fn make_scan_store_with_points(points: &[(f32, f32)]) -> ScanStore { - use crate::integration::ScanStoreConfig; - - let mut store = ScanStore::new(ScanStoreConfig::default()); - let xs: Vec = points.iter().map(|(x, _)| *x).collect(); - let ys: Vec = points.iter().map(|(_, y)| *y).collect(); - let cloud = PointCloud2D { xs, ys }; - - // Use identity pose so robot-frame = world-frame - store.add_scan(Pose2D::identity(), Pose2D::identity(), &cloud, 0, 1.0); - store - } - - #[test] - fn test_config_defaults() { - let config = PointAssociationConfig::default(); - assert!((config.max_distance - 0.10).abs() < 0.001); - assert!((config.max_projection_extension - 0.1).abs() < 0.001); - } - - #[test] - fn test_config_builder() { - let config = PointAssociationConfig::new() - .with_max_distance(0.15) - .with_max_projection_extension(0.2); - assert!((config.max_distance - 0.15).abs() < 0.001); - assert!((config.max_projection_extension - 0.2).abs() < 0.001); - } - - #[test] - fn test_empty_inputs() { - let store = ScanStore::default(); - let lines: Vec = vec![]; - let index = SpatialIndex::empty(); - let config = PointAssociationConfig::default(); - - let result = associate_points_to_lines(&store, &lines, &index, &config); - assert_eq!(result.total_associated, 0); - assert_eq!(result.total_unassociated, 0); - assert!(result.line_points.is_empty()); - } - - #[test] - fn test_points_associate_to_nearest_line() { - // Create two horizontal lines - let lines = vec![ - make_horizontal_line(0.0, 0.0, 10.0), // y=0 - make_horizontal_line(5.0, 0.0, 10.0), // y=5 - ]; - let index = SpatialIndex::new(&lines); - let config = PointAssociationConfig::default().with_max_distance(0.5); - - // Points close to line 0 (y=0) - let store = make_scan_store_with_points(&[ - (2.0, 0.05), // Very close to y=0 - (5.0, 0.02), // Very close to y=0 - (8.0, 4.98), // Very close to y=5 - ]); - - let result = associate_points_to_lines(&store, &lines, &index, &config); - - assert_eq!(result.total_associated, 3); - assert_eq!(result.line_points[0].len(), 2); // Two points near y=0 - assert_eq!(result.line_points[1].len(), 1); // One point near y=5 - } - - #[test] - fn test_points_outside_max_distance() { - let lines = vec![make_horizontal_line(0.0, 0.0, 10.0)]; - let index = SpatialIndex::new(&lines); - let config = PointAssociationConfig::default().with_max_distance(0.1); - - // Points too far from line - let store = make_scan_store_with_points(&[ - (5.0, 0.5), // 0.5m away, too far - (5.0, 1.0), // 1.0m away, too far - ]); - - let result = associate_points_to_lines(&store, &lines, &index, &config); - - assert_eq!(result.total_associated, 0); - assert_eq!(result.total_unassociated, 2); - } - - #[test] - fn test_points_outside_projection_bounds() { - let lines = vec![make_horizontal_line(0.0, 0.0, 10.0)]; - let index = SpatialIndex::new(&lines); - // Strict projection bounds (no extension) - let config = PointAssociationConfig::default() - .with_max_distance(0.5) - .with_max_projection_extension(0.0); - - // Points beyond line endpoints - let store = make_scan_store_with_points(&[ - (-2.0, 0.0), // Before start - (12.0, 0.0), // After end - (5.0, 0.0), // On segment (should associate) - ]); - - let result = associate_points_to_lines(&store, &lines, &index, &config); - - assert_eq!(result.total_associated, 1); - assert_eq!(result.total_unassociated, 2); - } - - #[test] - fn test_projection_extension() { - // Line from (0,0) to (10,0), length 10 - let lines = vec![make_horizontal_line(0.0, 0.0, 10.0)]; - let index = SpatialIndex::new(&lines); - // Allow 10% extension = 1m beyond endpoints - let config = PointAssociationConfig::default() - .with_max_distance(0.5) - .with_max_projection_extension(0.1); - - let store = make_scan_store_with_points(&[ - (-0.5, 0.0), // 5% before start, within extension - (10.5, 0.0), // 5% after end, within extension - (-2.0, 0.0), // 20% before start, outside extension - ]); - - let result = associate_points_to_lines(&store, &lines, &index, &config); - - assert_eq!(result.total_associated, 2); - assert_eq!(result.total_unassociated, 1); - } - - #[test] - fn test_associated_point_metadata() { - let lines = vec![make_horizontal_line(0.0, 0.0, 10.0)]; - let index = SpatialIndex::new(&lines); - let config = PointAssociationConfig::default().with_max_distance(0.5); - - let store = make_scan_store_with_points(&[(5.0, 0.1)]); - - let result = associate_points_to_lines(&store, &lines, &index, &config); - - assert_eq!(result.line_points[0].len(), 1); - let ap = &result.line_points[0][0]; - assert!((ap.point.x - 5.0).abs() < 0.01); - assert!((ap.point.y - 0.1).abs() < 0.01); - assert_eq!(ap.scan_id, 0); - assert!((ap.distance - 0.1).abs() < 0.01); // Perpendicular distance - assert!((ap.projection_t - 0.5).abs() < 0.01); // Midpoint of line - } - - #[test] - fn test_multiple_scans() { - use crate::integration::ScanStoreConfig; - - let lines = vec![make_horizontal_line(0.0, 0.0, 10.0)]; - let index = SpatialIndex::new(&lines); - let config = PointAssociationConfig::default().with_max_distance(0.5); - - let mut store = ScanStore::new(ScanStoreConfig::default()); - - // Add first scan - let cloud1 = PointCloud2D { - xs: vec![2.0, 3.0], - ys: vec![0.05, 0.05], - }; - store.add_scan(Pose2D::identity(), Pose2D::identity(), &cloud1, 0, 1.0); - - // Add second scan - let cloud2 = PointCloud2D { - xs: vec![7.0, 8.0], - ys: vec![0.05, 0.05], - }; - store.add_scan(Pose2D::identity(), Pose2D::identity(), &cloud2, 0, 1.0); - - let result = associate_points_to_lines(&store, &lines, &index, &config); - - assert_eq!(result.total_associated, 4); - assert_eq!(result.line_points[0].len(), 4); - - // Verify scan IDs are preserved - let scan_ids: Vec = result.line_points[0].iter().map(|ap| ap.scan_id).collect(); - assert!(scan_ids.contains(&0)); - assert!(scan_ids.contains(&1)); - } - - #[test] - fn test_transformed_poses() { - use crate::integration::ScanStoreConfig; - - // Line at y=10 in world frame - let lines = vec![make_horizontal_line(10.0, 0.0, 10.0)]; - let index = SpatialIndex::new(&lines); - let config = PointAssociationConfig::default().with_max_distance(0.5); - - let mut store = ScanStore::new(ScanStoreConfig::default()); - - // Robot-frame points at (5, 0) - directly in front of robot - let cloud = PointCloud2D { - xs: vec![5.0], - ys: vec![0.0], - }; - - // Robot at (0, 10), facing +X -> world point at (5, 10) - let pose = Pose2D::new(0.0, 10.0, 0.0); - store.add_scan(Pose2D::identity(), pose, &cloud, 0, 1.0); - - let result = associate_points_to_lines(&store, &lines, &index, &config); - - // Point at (5, 10) should associate with line at y=10 - assert_eq!(result.total_associated, 1); - let ap = &result.line_points[0][0]; - assert!((ap.point.x - 5.0).abs() < 0.01); - assert!((ap.point.y - 10.0).abs() < 0.01); - } - - #[test] - fn test_associate_scan_points() { - let lines = vec![ - make_horizontal_line(0.0, 0.0, 10.0), - make_vertical_line(10.0, 0.0, 10.0), - ]; - let index = SpatialIndex::new(&lines); - let config = PointAssociationConfig::default().with_max_distance(0.5); - - let points = vec![ - Point2D::new(5.0, 0.05), // Near line 0 - Point2D::new(9.95, 5.0), // Near line 1 - Point2D::new(5.0, 5.0), // Far from both - ]; - - let result = associate_scan_points(&points, 42, &lines, &index, &config); - - assert_eq!(result.total_associated, 2); - assert_eq!(result.total_unassociated, 1); - assert_eq!(result.line_points[0].len(), 1); - assert_eq!(result.line_points[1].len(), 1); - assert_eq!(result.line_points[0][0].scan_id, 42); - } - - #[test] - fn test_lines_with_enough_points() { - let lines = vec![ - make_horizontal_line(0.0, 0.0, 10.0), - make_horizontal_line(5.0, 0.0, 10.0), - make_horizontal_line(10.0, 0.0, 10.0), - ]; - let index = SpatialIndex::new(&lines); - let config = PointAssociationConfig::default().with_max_distance(0.5); - - // Create points: 5 near line 0, 3 near line 1, 10 near line 2 - let mut points = vec![]; - for i in 0..5 { - points.push((i as f32, 0.05)); // Near line 0 - } - for i in 0..3 { - points.push((i as f32, 5.05)); // Near line 1 - } - for i in 0..10 { - points.push((i as f32, 10.05)); // Near line 2 - } - - let store = make_scan_store_with_points(&points); - let result = associate_points_to_lines(&store, &lines, &index, &config); - - // Lines with at least 5 points - let enough = lines_with_enough_points(&result, 5); - assert_eq!(enough.len(), 2); - assert!(enough.contains(&0)); - assert!(enough.contains(&2)); - assert!(!enough.contains(&1)); // Only 3 points - } - - #[test] - fn test_extract_points_for_line() { - let lines = vec![make_horizontal_line(0.0, 0.0, 10.0)]; - let index = SpatialIndex::new(&lines); - let config = PointAssociationConfig::default().with_max_distance(0.5); - - let store = make_scan_store_with_points(&[(2.0, 0.05), (5.0, 0.05), (8.0, 0.05)]); - - let result = associate_points_to_lines(&store, &lines, &index, &config); - let points = extract_points_for_line(&result, 0); - - assert_eq!(points.len(), 3); - assert!((points[0].x - 2.0).abs() < 0.01); - assert!((points[1].x - 5.0).abs() < 0.01); - assert!((points[2].x - 8.0).abs() < 0.01); - } - - #[test] - fn test_association_rate() { - let lines = vec![make_horizontal_line(0.0, 0.0, 10.0)]; - let index = SpatialIndex::new(&lines); - let config = PointAssociationConfig::default().with_max_distance(0.1); - - let store = make_scan_store_with_points(&[ - (5.0, 0.05), // Close enough - (5.0, 0.5), // Too far - (5.0, 0.08), // Close enough - (5.0, 1.0), // Too far - ]); - - let result = associate_points_to_lines(&store, &lines, &index, &config); - - assert_eq!(result.total_associated, 2); - assert_eq!(result.total_unassociated, 2); - assert!((result.association_rate() - 0.5).abs() < 0.001); - assert_eq!(result.total_points(), 4); - } -} diff --git a/vastu-map/src/integration/refitter.rs b/vastu-map/src/integration/refitter.rs deleted file mode 100644 index b98fa20..0000000 --- a/vastu-map/src/integration/refitter.rs +++ /dev/null @@ -1,706 +0,0 @@ -//! Line re-fitting from accumulated point clouds. -//! -//! Re-fits existing map lines using points accumulated in ScanStore, -//! eliminating first-scan bias in line geometry. - -use std::collections::HashMap; - -use crate::config::LidarNoiseModel; -use crate::core::{Point2D, Pose2D}; -use crate::extraction::fit_line_weighted; -use crate::features::Line2D; -use crate::integration::point_association::AssociatedPoint; - -/// Configuration for line re-fitting. -#[derive(Clone, Debug)] -pub struct RefitConfig { - /// Minimum points required to re-fit a line. - /// Lines with fewer points keep their original geometry. - /// Default: 10 - pub min_points: usize, - - /// Gap threshold for splitting lines (meters). - /// If consecutive points (sorted by projection) have a gap larger than this, - /// the line is split into multiple segments. - /// Default: 0.30m (30cm) - pub gap_threshold: f32, - - /// Noise model for range-based weighting. - /// Closer points get higher weights during fitting. - pub noise_model: LidarNoiseModel, -} - -impl Default for RefitConfig { - fn default() -> Self { - Self { - min_points: 10, - gap_threshold: 0.30, - noise_model: LidarNoiseModel::default(), - } - } -} - -impl RefitConfig { - /// Create a new configuration with defaults. - pub fn new() -> Self { - Self::default() - } - - /// Set minimum points for re-fitting. - pub fn with_min_points(mut self, min: usize) -> Self { - self.min_points = min; - self - } - - /// Set gap threshold for line splitting. - pub fn with_gap_threshold(mut self, gap: f32) -> Self { - self.gap_threshold = gap; - self - } - - /// Set the noise model for weighting. - pub fn with_noise_model(mut self, model: LidarNoiseModel) -> Self { - self.noise_model = model; - self - } -} - -/// Result of re-fitting a line. -#[derive(Clone, Debug)] -pub enum RefitResult { - /// Single line fitted (no gaps detected). - Single(Line2D), - /// Line was split due to gaps in point cloud. - Split(Vec), - /// Not enough points to re-fit (keeps original geometry). - Insufficient, -} - -impl RefitResult { - /// Get all lines from the result. - pub fn lines(&self) -> Vec { - match self { - RefitResult::Single(line) => vec![*line], - RefitResult::Split(lines) => lines.clone(), - RefitResult::Insufficient => vec![], - } - } - - /// Get number of lines produced. - pub fn line_count(&self) -> usize { - match self { - RefitResult::Single(_) => 1, - RefitResult::Split(lines) => lines.len(), - RefitResult::Insufficient => 0, - } - } - - /// Check if re-fitting was successful (not insufficient). - pub fn is_success(&self) -> bool { - !matches!(self, RefitResult::Insufficient) - } -} - -/// Statistics from a batch re-fitting operation. -#[derive(Clone, Debug, Default)] -pub struct RefitStats { - /// Number of lines re-fitted as single segments. - pub lines_refitted_single: usize, - /// Number of lines that were split due to gaps. - pub lines_split: usize, - /// Number of lines with insufficient points (kept original). - pub lines_insufficient: usize, - /// Total number of new line segments created (from splits). - pub total_segments_created: usize, -} - -impl RefitStats { - /// Total lines processed. - pub fn total_processed(&self) -> usize { - self.lines_refitted_single + self.lines_split + self.lines_insufficient - } - - /// Success rate (refitted / total). - pub fn success_rate(&self) -> f32 { - let total = self.total_processed(); - if total == 0 { - 0.0 - } else { - (self.lines_refitted_single + self.lines_split) as f32 / total as f32 - } - } -} - -/// Re-fit a line from associated points with gap detection. -/// -/// # Arguments -/// * `points` - Points associated with this line (from point_association) -/// * `original` - Original line geometry (for fallback and observation_count) -/// * `robot_poses` - Map of scan_id -> robot pose (for computing weights) -/// * `config` - Re-fitting configuration -/// -/// # Returns -/// - `RefitResult::Single(line)` if fitted as a single line -/// - `RefitResult::Split(lines)` if gaps were detected and line was split -/// - `RefitResult::Insufficient` if not enough points -pub fn refit_line( - points: &[AssociatedPoint], - original: &Line2D, - robot_poses: &HashMap, - config: &RefitConfig, -) -> RefitResult { - // Check minimum points - if points.len() < config.min_points { - return RefitResult::Insufficient; - } - - // Sort points by projection onto original line direction - let mut sorted_points: Vec<&AssociatedPoint> = points.iter().collect(); - sorted_points.sort_by(|a, b| { - a.projection_t - .partial_cmp(&b.projection_t) - .unwrap_or(std::cmp::Ordering::Equal) - }); - - // Detect gaps and find split indices - let split_indices = find_gap_splits(&sorted_points, config.gap_threshold, original); - - if split_indices.is_empty() { - // No gaps - fit as single line - let line = fit_segment( - &sorted_points, - robot_poses, - original.observation_count, - &config.noise_model, - ); - match line { - Some(l) => RefitResult::Single(l), - None => RefitResult::Insufficient, - } - } else { - // Gaps detected - split into segments - let segments = split_and_fit( - &sorted_points, - &split_indices, - robot_poses, - original.observation_count, - config, - ); - if segments.is_empty() { - RefitResult::Insufficient - } else if segments.len() == 1 { - RefitResult::Single(segments.into_iter().next().unwrap()) - } else { - RefitResult::Split(segments) - } - } -} - -/// Find indices where the point cloud should be split due to gaps. -/// -/// Returns a list of indices where splits should occur. -/// Each index represents the start of a new segment. -fn find_gap_splits( - sorted_points: &[&AssociatedPoint], - gap_threshold: f32, - original: &Line2D, -) -> Vec { - if sorted_points.len() < 2 { - return vec![]; - } - - let line_length = original.length(); - if line_length < f32::EPSILON { - return vec![]; - } - - let mut splits = Vec::new(); - - for i in 1..sorted_points.len() { - let prev = sorted_points[i - 1]; - let curr = sorted_points[i]; - - // Gap in projection space (as fraction of line length) - let t_gap = (curr.projection_t - prev.projection_t).abs(); - - // Convert to world distance - let world_gap = t_gap * line_length; - - if world_gap > gap_threshold { - splits.push(i); - } - } - - splits -} - -/// Split points at gap indices and fit each segment. -fn split_and_fit( - sorted_points: &[&AssociatedPoint], - split_indices: &[usize], - robot_poses: &HashMap, - observation_count: u32, - config: &RefitConfig, -) -> Vec { - let mut segments = Vec::new(); - - // Create segment ranges - let mut ranges: Vec<(usize, usize)> = Vec::new(); - let mut start = 0; - - for &split_idx in split_indices { - if split_idx > start { - ranges.push((start, split_idx)); - } - start = split_idx; - } - - // Add final segment - if start < sorted_points.len() { - ranges.push((start, sorted_points.len())); - } - - // Fit each segment - for (range_start, range_end) in ranges { - let segment_points: Vec<&AssociatedPoint> = sorted_points[range_start..range_end].to_vec(); - - if segment_points.len() < config.min_points { - continue; - } - - if let Some(line) = fit_segment( - &segment_points, - robot_poses, - observation_count, - &config.noise_model, - ) { - segments.push(line); - } - } - - segments -} - -/// Fit a line segment from points using weighted TLS. -fn fit_segment( - points: &[&AssociatedPoint], - robot_poses: &HashMap, - observation_count: u32, - noise_model: &LidarNoiseModel, -) -> Option { - if points.len() < 2 { - return None; - } - - // Extract Point2D and compute weights - let mut point_vec: Vec = Vec::with_capacity(points.len()); - let mut weights: Vec = Vec::with_capacity(points.len()); - - for ap in points { - point_vec.push(ap.point); - - // Compute weight based on range from robot - let weight = if let Some(pose) = robot_poses.get(&ap.scan_id) { - let robot_pos = Point2D::new(pose.x, pose.y); - let range = ap.point.distance(robot_pos); - noise_model.weight(range) - } else { - // Fallback: use default weight - 1.0 - }; - weights.push(weight); - } - - // Fit using weighted TLS - let mut line = fit_line_weighted(&point_vec, &weights)?; - - // Preserve observation count from original - line.observation_count = observation_count; - line.point_count = points.len() as u32; - - Some(line) -} - -/// Re-fit a line from points using simple (unweighted) fitting. -/// -/// Use this when robot poses are not available. -pub fn refit_line_simple( - points: &[AssociatedPoint], - original: &Line2D, - config: &RefitConfig, -) -> RefitResult { - // Use empty pose map for simple fitting - let empty_poses: HashMap = HashMap::new(); - refit_line(points, original, &empty_poses, config) -} - -#[cfg(test)] -mod tests { - use super::*; - - fn make_horizontal_line(y: f32, x_start: f32, x_end: f32) -> Line2D { - Line2D::with_observation_count(Point2D::new(x_start, y), Point2D::new(x_end, y), 5) - } - - fn make_associated_point(x: f32, y: f32, scan_id: u32, t: f32) -> AssociatedPoint { - AssociatedPoint { - point: Point2D::new(x, y), - scan_id, - distance: 0.0, - projection_t: t, - } - } - - fn make_points_along_line( - x_start: f32, - x_end: f32, - y: f32, - count: usize, - ) -> Vec { - let line_length = (x_end - x_start).abs(); - (0..count) - .map(|i| { - let t = i as f32 / (count - 1).max(1) as f32; - let x = x_start + t * (x_end - x_start); - // projection_t is the fraction along the line - make_associated_point(x, y, 0, x / line_length) - }) - .collect() - } - - /// Create a dense point cloud (no gaps > threshold). - fn make_dense_points( - x_start: f32, - x_end: f32, - y: f32, - gap_threshold: f32, - ) -> Vec { - let line_length = (x_end - x_start).abs(); - // Create points with spacing less than gap_threshold - let spacing = gap_threshold * 0.5; - let count = ((line_length / spacing) as usize).max(2); - (0..count) - .map(|i| { - let x = x_start + i as f32 * spacing; - let t = x / line_length; - make_associated_point(x.min(x_end), y, 0, t.min(1.0)) - }) - .collect() - } - - #[test] - fn test_config_defaults() { - let config = RefitConfig::default(); - assert_eq!(config.min_points, 10); - assert!((config.gap_threshold - 0.30).abs() < 0.001); - } - - #[test] - fn test_config_builder() { - let config = RefitConfig::new() - .with_min_points(5) - .with_gap_threshold(0.5); - assert_eq!(config.min_points, 5); - assert!((config.gap_threshold - 0.5).abs() < 0.001); - } - - #[test] - fn test_refit_insufficient_points() { - let original = make_horizontal_line(0.0, 0.0, 10.0); - let points = make_points_along_line(0.0, 10.0, 0.0, 5); // Only 5 points - let config = RefitConfig::default().with_min_points(10); - let poses = HashMap::new(); - - let result = refit_line(&points, &original, &poses, &config); - - assert!(matches!(result, RefitResult::Insufficient)); - assert!(!result.is_success()); - assert_eq!(result.line_count(), 0); - } - - #[test] - fn test_refit_single_line() { - let original = make_horizontal_line(0.0, 0.0, 10.0); - // Create dense points along the line with small noise (spacing < gap_threshold) - let gap_threshold = 0.30; - let spacing = gap_threshold * 0.5; // 0.15m spacing - let count = (10.0 / spacing) as usize; // ~67 points for 10m line - - let mut points: Vec = Vec::new(); - for i in 0..count { - let x = i as f32 * spacing; - let t = x / 10.0; - let y = if i % 2 == 0 { 0.01 } else { -0.01 }; // Small noise - points.push(make_associated_point(x, y, 0, t)); - } - - let config = RefitConfig::default() - .with_min_points(10) - .with_gap_threshold(gap_threshold); - let poses = HashMap::new(); - - let result = refit_line(&points, &original, &poses, &config); - - assert!(result.is_success()); - assert!(matches!(result, RefitResult::Single(_))); - - let lines = result.lines(); - assert_eq!(lines.len(), 1); - - // Verify line is approximately horizontal - let line = &lines[0]; - assert!(line.angle().abs() < 0.1); - // Verify point count is set - assert_eq!(line.point_count as usize, count); - } - - #[test] - fn test_refit_with_gap_split() { - let original = make_horizontal_line(0.0, 0.0, 10.0); - let gap_threshold = 0.5; - let spacing = 0.1; // Dense points within each segment - - // Create points with a gap in the middle - let mut points: Vec = Vec::new(); - - // First segment: 0-3m (dense points) - let mut x = 0.0; - while x <= 3.0 { - let t = x / 10.0; - points.push(make_associated_point(x, 0.0, 0, t)); - x += spacing; - } - - // GAP: 3m to 7m (4m gap > 0.5m threshold) - - // Second segment: 7-10m (dense points) - x = 7.0; - while x <= 10.0 { - let t = x / 10.0; - points.push(make_associated_point(x, 0.0, 1, t)); - x += spacing; - } - - let config = RefitConfig::default() - .with_min_points(5) - .with_gap_threshold(gap_threshold); - let poses = HashMap::new(); - - let result = refit_line(&points, &original, &poses, &config); - - assert!(result.is_success()); - assert!( - matches!(result, RefitResult::Split(_)), - "Expected Split, got {:?}", - result - ); - - let lines = result.lines(); - assert_eq!(lines.len(), 2); - - // First line should cover ~0-3m - assert!(lines[0].start.x < 1.0); - assert!(lines[0].end.x < 4.0); - - // Second line should cover ~7-10m - assert!(lines[1].start.x > 6.0); - assert!(lines[1].end.x > 9.0); - } - - #[test] - fn test_refit_with_poses_weighting() { - let original = make_horizontal_line(0.0, 0.0, 10.0); - let gap_threshold = 0.5; - let spacing = 0.1; // Dense points - - // Create dense points with noise - let mut points: Vec = Vec::new(); - - // Points close to robot (scan 0 at origin) - on y=0 - let mut x = 0.0; - while x <= 5.0 { - let t = x / 10.0; - points.push(make_associated_point(x, 0.0, 0, t)); - x += spacing; - } - - // Points far from robot (scan 1 at x=10) - with noise above y=0 - x = 5.0 + spacing; - while x <= 10.0 { - let t = x / 10.0; - points.push(make_associated_point(x, 0.5, 1, t)); - x += spacing; - } - - let mut poses = HashMap::new(); - poses.insert(0, Pose2D::new(0.0, 0.0, 0.0)); // Robot at origin for scan 0 - poses.insert(1, Pose2D::new(10.0, 0.0, 0.0)); // Robot at x=10 for scan 1 - - let config = RefitConfig::default() - .with_min_points(10) - .with_gap_threshold(gap_threshold); - - let result = refit_line(&points, &original, &poses, &config); - - assert!(result.is_success()); - let lines = result.lines(); - assert_eq!(lines.len(), 1); - - // Line should be closer to y=0 due to close points having higher weight - let line = &lines[0]; - let midpoint = line.midpoint(); - assert!( - midpoint.y < 0.3, - "Midpoint y={} should be < 0.3 (closer to close points)", - midpoint.y - ); - } - - #[test] - fn test_refit_preserves_observation_count() { - let mut original = make_horizontal_line(0.0, 0.0, 10.0); - original.observation_count = 42; - - // Use dense points to avoid gap splitting - let points = make_dense_points(0.0, 10.0, 0.0, 0.3); - let config = RefitConfig::default() - .with_min_points(10) - .with_gap_threshold(0.3); - let poses = HashMap::new(); - - let result = refit_line(&points, &original, &poses, &config); - - let lines = result.lines(); - assert_eq!(lines.len(), 1, "Expected 1 line, got {}", lines.len()); - assert_eq!(lines[0].observation_count, 42); - } - - #[test] - fn test_refit_simple() { - let original = make_horizontal_line(0.0, 0.0, 10.0); - // Use dense points to avoid gap splitting - let points = make_dense_points(0.0, 10.0, 0.05, 0.3); - let config = RefitConfig::default() - .with_min_points(10) - .with_gap_threshold(0.3); - - let result = refit_line_simple(&points, &original, &config); - - assert!(result.is_success()); - } - - #[test] - fn test_refit_stats() { - let stats = RefitStats { - lines_refitted_single: 5, - lines_split: 2, - lines_insufficient: 3, - total_segments_created: 5, // 2 splits creating ~2.5 segments each - }; - - assert_eq!(stats.total_processed(), 10); - assert!((stats.success_rate() - 0.7).abs() < 0.001); - } - - #[test] - fn test_find_gap_splits_no_gaps() { - let original = make_horizontal_line(0.0, 0.0, 10.0); - let gap_threshold = 0.5; - let spacing = 0.2; // Smaller than gap_threshold - - // Create dense points with no gaps - let points: Vec = (0..50) - .map(|i| { - let x = i as f32 * spacing; - let t = x / 10.0; - make_associated_point(x.min(10.0), 0.0, 0, t.min(1.0)) - }) - .collect(); - - let sorted: Vec<&AssociatedPoint> = points.iter().collect(); - let splits = find_gap_splits(&sorted, gap_threshold, &original); - - assert!(splits.is_empty(), "Expected no splits, got {:?}", splits); - } - - #[test] - fn test_find_gap_splits_with_gap() { - let original = make_horizontal_line(0.0, 0.0, 10.0); - let gap_threshold = 0.5; - - // Create points with a clear gap - let points = vec![ - make_associated_point(0.0, 0.0, 0, 0.0), - make_associated_point(0.2, 0.0, 0, 0.02), - make_associated_point(0.4, 0.0, 0, 0.04), - // Gap here: 0.4 to 5.0 = 4.6m > 0.5m threshold - make_associated_point(5.0, 0.0, 0, 0.5), - make_associated_point(5.2, 0.0, 0, 0.52), - ]; - - let sorted: Vec<&AssociatedPoint> = points.iter().collect(); - let splits = find_gap_splits(&sorted, gap_threshold, &original); - - assert_eq!(splits.len(), 1, "Expected 1 split, got {:?}", splits); - assert_eq!(splits[0], 3); // Split before the 4th point (index 3) - } - - #[test] - fn test_refitresult_lines() { - let line = make_horizontal_line(0.0, 0.0, 10.0); - - let single = RefitResult::Single(line.clone()); - assert_eq!(single.lines().len(), 1); - assert_eq!(single.line_count(), 1); - assert!(single.is_success()); - - let split = RefitResult::Split(vec![line.clone(), line.clone()]); - assert_eq!(split.lines().len(), 2); - assert_eq!(split.line_count(), 2); - assert!(split.is_success()); - - let insufficient = RefitResult::Insufficient; - assert!(insufficient.lines().is_empty()); - assert_eq!(insufficient.line_count(), 0); - assert!(!insufficient.is_success()); - } - - #[test] - fn test_multiple_gaps_creates_multiple_segments() { - let original = make_horizontal_line(0.0, 0.0, 10.0); - - // Create three clusters of points with gaps between - let mut points: Vec = Vec::new(); - - // Cluster 1: t = 0.0 to 0.2 (10 points) - for i in 0..10 { - let t = i as f32 / 50.0; - points.push(make_associated_point(t * 10.0, 0.0, 0, t)); - } - - // Cluster 2: t = 0.4 to 0.6 (10 points) - gap before - for i in 0..10 { - let t = 0.4 + i as f32 / 50.0; - points.push(make_associated_point(t * 10.0, 0.0, 0, t)); - } - - // Cluster 3: t = 0.8 to 1.0 (10 points) - gap before - for i in 0..10 { - let t = 0.8 + i as f32 / 50.0; - points.push(make_associated_point(t * 10.0, 0.0, 0, t)); - } - - let config = RefitConfig::default() - .with_min_points(5) - .with_gap_threshold(0.3); // 0.3m < 2m gaps - - let poses = HashMap::new(); - let result = refit_line(&points, &original, &poses, &config); - - assert!(result.is_success()); - assert!(matches!(result, RefitResult::Split(_))); - - let lines = result.lines(); - assert_eq!(lines.len(), 3); - } -} diff --git a/vastu-map/src/integration/scan_store.rs b/vastu-map/src/integration/scan_store.rs deleted file mode 100644 index 3d88b89..0000000 --- a/vastu-map/src/integration/scan_store.rs +++ /dev/null @@ -1,907 +0,0 @@ -//! Scan storage for exploration mode. -//! -//! Retains raw scan data during exploration for visualization, -//! debugging, and future point-cloud-based line fitting. - -use crate::core::{Point2D, PointCloud2D, Pose2D}; - -/// A stored scan with associated pose information. -#[derive(Clone, Debug)] -pub struct StoredScan { - /// Unique scan identifier (sequential). - pub scan_id: u32, - - /// Raw odometry pose (input to observe). - pub odometry: Pose2D, - - /// Estimated pose after ICP matching. - /// This is the corrected pose used for map integration. - pub estimated_pose: Pose2D, - - /// Scan points in robot frame. - /// Transform with `estimated_pose` to get world-frame points. - points: Vec, - - /// Number of features extracted from this scan. - pub features_extracted: usize, - - /// ICP matching confidence (0.0 - 1.0). - pub match_confidence: f32, -} - -impl StoredScan { - /// Create a new stored scan. - pub fn new( - scan_id: u32, - odometry: Pose2D, - estimated_pose: Pose2D, - points: Vec, - features_extracted: usize, - match_confidence: f32, - ) -> Self { - Self { - scan_id, - odometry, - estimated_pose, - points, - features_extracted, - match_confidence, - } - } - - /// Get robot-frame points. - #[inline] - pub fn points(&self) -> &[Point2D] { - &self.points - } - - /// Get the number of points in this scan. - #[inline] - pub fn point_count(&self) -> usize { - self.points.len() - } - - /// Get world-frame points (transformed by estimated pose). - pub fn world_points(&self) -> Vec { - self.points - .iter() - .map(|p| self.estimated_pose.transform_point(*p)) - .collect() - } - - /// Fill a buffer with world-frame points (zero-allocation if buffer is large enough). - pub fn world_points_into(&self, buffer: &mut Vec) { - buffer.clear(); - buffer.reserve(self.points.len()); - for p in &self.points { - buffer.push(self.estimated_pose.transform_point(*p)); - } - } - - /// Get the pose correction applied by ICP. - /// Returns the difference between estimated and odometry-predicted pose. - pub fn pose_correction(&self) -> Pose2D { - // correction = estimated * odometry.inverse() - // This gives the transform that was applied to correct odometry - self.estimated_pose.compose(self.odometry.inverse()) - } - - /// Memory usage in bytes (approximate). - pub fn memory_bytes(&self) -> usize { - std::mem::size_of::() + self.points.len() * std::mem::size_of::() - } -} - -/// Configuration for scan storage. -#[derive(Clone, Debug)] -pub struct ScanStoreConfig { - /// Maximum number of scans to store. - /// Oldest scans are removed when limit is reached. - /// None = unlimited. Default: None - pub max_scans: Option, - - /// Whether to store point data. - /// If false, only poses are stored (much less memory). - /// Default: true - pub store_points: bool, - - /// Minimum distance traveled to store a new scan (meters). - /// Helps reduce redundant scans when robot is stationary. - /// Default: 0.0 (store all scans) - pub min_distance: f32, - - /// Minimum rotation to store a new scan (radians). - /// Default: 0.0 (store all scans) - pub min_rotation: f32, -} - -impl Default for ScanStoreConfig { - fn default() -> Self { - Self { - max_scans: None, - store_points: true, - min_distance: 0.0, - min_rotation: 0.0, - } - } -} - -impl ScanStoreConfig { - /// Create a new configuration with defaults. - pub fn new() -> Self { - Self::default() - } - - /// Set maximum number of scans to store. - pub fn with_max_scans(mut self, max: usize) -> Self { - self.max_scans = Some(max); - self - } - - /// Set unlimited scan storage. - pub fn with_unlimited_scans(mut self) -> Self { - self.max_scans = None; - self - } - - /// Enable or disable point storage. - pub fn with_store_points(mut self, store: bool) -> Self { - self.store_points = store; - self - } - - /// Set minimum distance between stored scans. - pub fn with_min_distance(mut self, meters: f32) -> Self { - self.min_distance = meters; - self - } - - /// Set minimum rotation between stored scans. - pub fn with_min_rotation(mut self, radians: f32) -> Self { - self.min_rotation = radians; - self - } -} - -/// Storage for scans during exploration. -/// -/// Retains raw scan data for visualization, debugging, and -/// point-cloud-based algorithms. -/// -/// # Example -/// -/// ```rust,ignore -/// use vastu_map::integration::{ScanStore, ScanStoreConfig}; -/// -/// let config = ScanStoreConfig::default().with_max_scans(1000); -/// let mut store = ScanStore::new(config); -/// -/// // After each observation... -/// store.add_scan(odometry, estimated_pose, &scan_points, features, confidence); -/// -/// // For visualization -/// for scan in store.iter() { -/// let world_points = scan.world_points(); -/// draw_points(&world_points); -/// } -/// ``` -#[derive(Clone, Debug)] -pub struct ScanStore { - /// Stored scans (oldest first). - scans: Vec, - - /// Next scan ID to assign. - next_scan_id: u32, - - /// Configuration. - config: ScanStoreConfig, - - /// Last stored pose (for distance/rotation filtering). - last_stored_pose: Option, -} - -impl ScanStore { - /// Create a new empty scan store. - pub fn new(config: ScanStoreConfig) -> Self { - Self { - scans: Vec::new(), - next_scan_id: 0, - config, - last_stored_pose: None, - } - } - - /// Create with default configuration. - pub fn with_defaults() -> Self { - Self::new(ScanStoreConfig::default()) - } - - /// Get the configuration. - pub fn config(&self) -> &ScanStoreConfig { - &self.config - } - - /// Get the number of stored scans. - #[inline] - pub fn len(&self) -> usize { - self.scans.len() - } - - /// Check if the store is empty. - #[inline] - pub fn is_empty(&self) -> bool { - self.scans.is_empty() - } - - /// Get the next scan ID that will be assigned. - #[inline] - pub fn next_scan_id(&self) -> u32 { - self.next_scan_id - } - - /// Add a scan to the store. - /// - /// Returns the assigned scan_id, or None if the scan was filtered - /// (due to min_distance/min_rotation constraints). - pub fn add_scan( - &mut self, - odometry: Pose2D, - estimated_pose: Pose2D, - points: &PointCloud2D, - features_extracted: usize, - match_confidence: f32, - ) -> Option { - // Check distance/rotation filter - if let Some(last_pose) = self.last_stored_pose { - let dx = estimated_pose.x - last_pose.x; - let dy = estimated_pose.y - last_pose.y; - let dist = (dx * dx + dy * dy).sqrt(); - let dtheta = crate::core::math::angle_diff(estimated_pose.theta, last_pose.theta).abs(); - - // Determine which constraints are active (> 0 means enabled) - let distance_check_enabled = self.config.min_distance > 0.0; - let rotation_check_enabled = self.config.min_rotation > 0.0; - - let passes_distance = dist >= self.config.min_distance; - let passes_rotation = dtheta >= self.config.min_rotation; - - // Filter logic: - // - If only distance filter: require distance - // - If only rotation filter: require rotation - // - If both filters: require either (OR semantics) - // - If no filters: store all - let should_store = match (distance_check_enabled, rotation_check_enabled) { - (false, false) => true, - (true, false) => passes_distance, - (false, true) => passes_rotation, - (true, true) => passes_distance || passes_rotation, - }; - - if !should_store { - return None; - } - } - - // Enforce max_scans limit - if let Some(max) = self.config.max_scans { - while self.scans.len() >= max { - self.scans.remove(0); // Remove oldest - } - } - - let scan_id = self.next_scan_id; - self.next_scan_id += 1; - - let stored_points = if self.config.store_points { - points.iter().collect() - } else { - Vec::new() - }; - - let scan = StoredScan::new( - scan_id, - odometry, - estimated_pose, - stored_points, - features_extracted, - match_confidence, - ); - - self.last_stored_pose = Some(estimated_pose); - self.scans.push(scan); - - Some(scan_id) - } - - /// Add a scan from raw points (not PointCloud2D). - pub fn add_scan_from_points( - &mut self, - odometry: Pose2D, - estimated_pose: Pose2D, - points: &[Point2D], - features_extracted: usize, - match_confidence: f32, - ) -> Option { - // Check distance/rotation filter - if let Some(last_pose) = self.last_stored_pose { - let dx = estimated_pose.x - last_pose.x; - let dy = estimated_pose.y - last_pose.y; - let dist = (dx * dx + dy * dy).sqrt(); - let dtheta = crate::core::math::angle_diff(estimated_pose.theta, last_pose.theta).abs(); - - // Determine which constraints are active (> 0 means enabled) - let distance_check_enabled = self.config.min_distance > 0.0; - let rotation_check_enabled = self.config.min_rotation > 0.0; - - let passes_distance = dist >= self.config.min_distance; - let passes_rotation = dtheta >= self.config.min_rotation; - - let should_store = match (distance_check_enabled, rotation_check_enabled) { - (false, false) => true, - (true, false) => passes_distance, - (false, true) => passes_rotation, - (true, true) => passes_distance || passes_rotation, - }; - - if !should_store { - return None; - } - } - - // Enforce max_scans limit - if let Some(max) = self.config.max_scans { - while self.scans.len() >= max { - self.scans.remove(0); - } - } - - let scan_id = self.next_scan_id; - self.next_scan_id += 1; - - let stored_points = if self.config.store_points { - points.to_vec() - } else { - Vec::new() - }; - - let scan = StoredScan::new( - scan_id, - odometry, - estimated_pose, - stored_points, - features_extracted, - match_confidence, - ); - - self.last_stored_pose = Some(estimated_pose); - self.scans.push(scan); - - Some(scan_id) - } - - /// Get a scan by its ID. - pub fn get(&self, scan_id: u32) -> Option<&StoredScan> { - self.scans.iter().find(|s| s.scan_id == scan_id) - } - - /// Get a scan by index (0 = oldest). - pub fn get_by_index(&self, index: usize) -> Option<&StoredScan> { - self.scans.get(index) - } - - /// Get the most recent scan. - pub fn latest(&self) -> Option<&StoredScan> { - self.scans.last() - } - - /// Get the oldest scan. - pub fn oldest(&self) -> Option<&StoredScan> { - self.scans.first() - } - - /// Iterate over all stored scans (oldest first). - pub fn iter(&self) -> impl Iterator { - self.scans.iter() - } - - /// Iterate over scans in reverse order (newest first). - pub fn iter_rev(&self) -> impl Iterator { - self.scans.iter().rev() - } - - /// Iterate over scans within a pose range. - /// - /// Returns scans whose estimated_pose is within `radius` of `center`. - pub fn iter_near(&self, center: Point2D, radius: f32) -> impl Iterator { - let radius_sq = radius * radius; - self.scans.iter().filter(move |s| { - let dx = s.estimated_pose.x - center.x; - let dy = s.estimated_pose.y - center.y; - dx * dx + dy * dy <= radius_sq - }) - } - - /// Get all scans as a slice. - pub fn scans(&self) -> &[StoredScan] { - &self.scans - } - - /// Get all estimated poses. - pub fn poses(&self) -> Vec { - self.scans.iter().map(|s| s.estimated_pose).collect() - } - - /// Get the trajectory as a list of points. - pub fn trajectory(&self) -> Vec { - self.scans - .iter() - .map(|s| Point2D::new(s.estimated_pose.x, s.estimated_pose.y)) - .collect() - } - - /// Get all world-frame points from all scans. - /// - /// Warning: This can be memory-intensive for large stores. - pub fn all_world_points(&self) -> Vec { - let total_points: usize = self.scans.iter().map(|s| s.point_count()).sum(); - let mut result = Vec::with_capacity(total_points); - - for scan in &self.scans { - for p in scan.points() { - result.push(scan.estimated_pose.transform_point(*p)); - } - } - - result - } - - /// Get world-frame points with scan IDs. - /// - /// Returns (point, scan_id) pairs for all stored points. - pub fn all_world_points_with_ids(&self) -> Vec<(Point2D, u32)> { - let total_points: usize = self.scans.iter().map(|s| s.point_count()).sum(); - let mut result = Vec::with_capacity(total_points); - - for scan in &self.scans { - for p in scan.points() { - result.push((scan.estimated_pose.transform_point(*p), scan.scan_id)); - } - } - - result - } - - /// Update the estimated pose for a scan (after loop closure correction). - /// - /// Returns true if the scan was found and updated. - pub fn update_pose(&mut self, scan_id: u32, new_pose: Pose2D) -> bool { - if let Some(scan) = self.scans.iter_mut().find(|s| s.scan_id == scan_id) { - scan.estimated_pose = new_pose; - true - } else { - false - } - } - - /// Apply pose corrections to multiple scans. - /// - /// Takes a map of scan_id -> corrected_pose. - /// Returns the number of scans updated. - pub fn apply_pose_corrections( - &mut self, - corrections: &std::collections::HashMap, - ) -> usize { - let mut updated = 0; - for scan in &mut self.scans { - if let Some(new_pose) = corrections.get(&scan.scan_id) { - scan.estimated_pose = *new_pose; - updated += 1; - } - } - updated - } - - /// Clear all stored scans. - pub fn clear(&mut self) { - self.scans.clear(); - self.last_stored_pose = None; - // Note: next_scan_id is NOT reset to allow ID continuity - } - - /// Reset the store completely (including scan ID counter). - pub fn reset(&mut self) { - self.scans.clear(); - self.next_scan_id = 0; - self.last_stored_pose = None; - } - - /// Total number of points stored across all scans. - pub fn total_points(&self) -> usize { - self.scans.iter().map(|s| s.point_count()).sum() - } - - /// Memory usage statistics. - pub fn memory_stats(&self) -> ScanStoreStats { - let total_points = self.total_points(); - let point_bytes = total_points * std::mem::size_of::(); - let scan_overhead = self.scans.len() * std::mem::size_of::(); - let vec_overhead = self.scans.capacity() * std::mem::size_of::(); - - ScanStoreStats { - scan_count: self.scans.len(), - total_points, - point_bytes, - total_bytes: point_bytes + scan_overhead + vec_overhead, - avg_points_per_scan: if self.scans.is_empty() { - 0.0 - } else { - total_points as f32 / self.scans.len() as f32 - }, - } - } -} - -impl Default for ScanStore { - fn default() -> Self { - Self::with_defaults() - } -} - -/// Memory usage statistics for ScanStore. -#[derive(Clone, Debug)] -pub struct ScanStoreStats { - /// Number of stored scans. - pub scan_count: usize, - /// Total number of points across all scans. - pub total_points: usize, - /// Memory used by points (bytes). - pub point_bytes: usize, - /// Total estimated memory usage (bytes). - pub total_bytes: usize, - /// Average points per scan. - pub avg_points_per_scan: f32, -} - -impl ScanStoreStats { - /// Get memory usage in kilobytes. - pub fn total_kb(&self) -> f32 { - self.total_bytes as f32 / 1024.0 - } - - /// Get memory usage in megabytes. - pub fn total_mb(&self) -> f32 { - self.total_bytes as f32 / (1024.0 * 1024.0) - } -} - -impl std::fmt::Display for ScanStoreStats { - fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result { - write!( - f, - "ScanStore: {} scans, {} points, {:.2} KB", - self.scan_count, - self.total_points, - self.total_kb() - ) - } -} - -#[cfg(test)] -mod tests { - use super::*; - - fn make_test_points(n: usize) -> Vec { - (0..n).map(|i| Point2D::new(i as f32 * 0.1, 0.0)).collect() - } - - fn make_point_cloud(n: usize) -> PointCloud2D { - let xs: Vec = (0..n).map(|i| i as f32 * 0.1).collect(); - let ys: Vec = vec![0.0; n]; - PointCloud2D { xs, ys } - } - - #[test] - fn test_stored_scan_creation() { - let scan = StoredScan::new( - 0, - Pose2D::identity(), - Pose2D::new(1.0, 0.0, 0.0), - make_test_points(10), - 5, - 0.95, - ); - - assert_eq!(scan.scan_id, 0); - assert_eq!(scan.point_count(), 10); - assert_eq!(scan.features_extracted, 5); - assert!((scan.match_confidence - 0.95).abs() < 0.001); - } - - #[test] - fn test_stored_scan_world_points() { - let points = vec![Point2D::new(1.0, 0.0), Point2D::new(0.0, 1.0)]; - let pose = Pose2D::new(10.0, 20.0, std::f32::consts::FRAC_PI_2); // 90° rotation - - let scan = StoredScan::new(0, Pose2D::identity(), pose, points, 0, 1.0); - - let world = scan.world_points(); - assert_eq!(world.len(), 2); - - // (1, 0) rotated 90° + translated = (10 + 0, 20 + 1) = (10, 21) - assert!((world[0].x - 10.0).abs() < 0.01); - assert!((world[0].y - 21.0).abs() < 0.01); - - // (0, 1) rotated 90° + translated = (10 - 1, 20 + 0) = (9, 20) - assert!((world[1].x - 9.0).abs() < 0.01); - assert!((world[1].y - 20.0).abs() < 0.01); - } - - #[test] - fn test_scan_store_basic() { - let mut store = ScanStore::with_defaults(); - assert!(store.is_empty()); - assert_eq!(store.len(), 0); - - let cloud = make_point_cloud(50); - let id = store.add_scan( - Pose2D::identity(), - Pose2D::new(1.0, 0.0, 0.0), - &cloud, - 3, - 0.9, - ); - - assert_eq!(id, Some(0)); - assert_eq!(store.len(), 1); - assert!(!store.is_empty()); - - let scan = store.get(0).unwrap(); - assert_eq!(scan.point_count(), 50); - } - - #[test] - fn test_scan_store_max_scans() { - let config = ScanStoreConfig::default().with_max_scans(3); - let mut store = ScanStore::new(config); - - for i in 0..5 { - let cloud = make_point_cloud(10); - store.add_scan( - Pose2D::identity(), - Pose2D::new(i as f32, 0.0, 0.0), - &cloud, - 0, - 1.0, - ); - } - - assert_eq!(store.len(), 3); - // Oldest scans (0, 1) should be removed - assert!(store.get(0).is_none()); - assert!(store.get(1).is_none()); - assert!(store.get(2).is_some()); - assert!(store.get(3).is_some()); - assert!(store.get(4).is_some()); - } - - #[test] - fn test_scan_store_distance_filter() { - let config = ScanStoreConfig::default().with_min_distance(0.5); - let mut store = ScanStore::new(config); - - let cloud = make_point_cloud(10); - - // First scan always stored - let id1 = store.add_scan( - Pose2D::identity(), - Pose2D::new(0.0, 0.0, 0.0), - &cloud, - 0, - 1.0, - ); - assert!(id1.is_some()); - - // Second scan too close, filtered - let id2 = store.add_scan( - Pose2D::identity(), - Pose2D::new(0.1, 0.0, 0.0), - &cloud, - 0, - 1.0, - ); - assert!(id2.is_none()); - - // Third scan far enough - let id3 = store.add_scan( - Pose2D::identity(), - Pose2D::new(1.0, 0.0, 0.0), - &cloud, - 0, - 1.0, - ); - assert!(id3.is_some()); - - assert_eq!(store.len(), 2); - } - - #[test] - fn test_scan_store_rotation_filter() { - let config = ScanStoreConfig::default().with_min_rotation(0.5); - let mut store = ScanStore::new(config); - - let cloud = make_point_cloud(10); - - store.add_scan( - Pose2D::identity(), - Pose2D::new(0.0, 0.0, 0.0), - &cloud, - 0, - 1.0, - ); - - // Small rotation, filtered - let id = store.add_scan( - Pose2D::identity(), - Pose2D::new(0.0, 0.0, 0.1), - &cloud, - 0, - 1.0, - ); - assert!(id.is_none()); - - // Large rotation, stored - let id = store.add_scan( - Pose2D::identity(), - Pose2D::new(0.0, 0.0, 1.0), - &cloud, - 0, - 1.0, - ); - assert!(id.is_some()); - } - - #[test] - fn test_scan_store_no_points() { - let config = ScanStoreConfig::default().with_store_points(false); - let mut store = ScanStore::new(config); - - let cloud = make_point_cloud(100); - store.add_scan(Pose2D::identity(), Pose2D::identity(), &cloud, 0, 1.0); - - let scan = store.latest().unwrap(); - assert_eq!(scan.point_count(), 0); // Points not stored - } - - #[test] - fn test_scan_store_trajectory() { - let mut store = ScanStore::with_defaults(); - - for i in 0..5 { - let cloud = make_point_cloud(10); - store.add_scan( - Pose2D::identity(), - Pose2D::new(i as f32, i as f32 * 2.0, 0.0), - &cloud, - 0, - 1.0, - ); - } - - let traj = store.trajectory(); - assert_eq!(traj.len(), 5); - assert!((traj[2].x - 2.0).abs() < 0.01); - assert!((traj[2].y - 4.0).abs() < 0.01); - } - - #[test] - fn test_scan_store_iter_near() { - let mut store = ScanStore::with_defaults(); - let cloud = make_point_cloud(10); - - // Add scans at various positions - for i in 0..10 { - store.add_scan( - Pose2D::identity(), - Pose2D::new(i as f32, 0.0, 0.0), - &cloud, - 0, - 1.0, - ); - } - - // Find scans near (5, 0) with radius 1.5 - let near: Vec<_> = store.iter_near(Point2D::new(5.0, 0.0), 1.5).collect(); - assert_eq!(near.len(), 3); // scans at x=4, 5, 6 - } - - #[test] - fn test_scan_store_update_pose() { - let mut store = ScanStore::with_defaults(); - let cloud = make_point_cloud(10); - - store.add_scan( - Pose2D::identity(), - Pose2D::new(1.0, 0.0, 0.0), - &cloud, - 0, - 1.0, - ); - - let new_pose = Pose2D::new(1.5, 0.5, 0.1); - assert!(store.update_pose(0, new_pose)); - - let scan = store.get(0).unwrap(); - assert!((scan.estimated_pose.x - 1.5).abs() < 0.01); - assert!((scan.estimated_pose.y - 0.5).abs() < 0.01); - } - - #[test] - fn test_scan_store_memory_stats() { - let mut store = ScanStore::with_defaults(); - - for _ in 0..10 { - let cloud = make_point_cloud(100); - store.add_scan(Pose2D::identity(), Pose2D::identity(), &cloud, 0, 1.0); - } - - let stats = store.memory_stats(); - assert_eq!(stats.scan_count, 10); - assert_eq!(stats.total_points, 1000); - assert!((stats.avg_points_per_scan - 100.0).abs() < 0.1); - assert!(stats.total_kb() > 0.0); - } - - #[test] - fn test_scan_store_all_world_points() { - let mut store = ScanStore::with_defaults(); - - // Add two scans at different positions - let cloud1 = make_point_cloud(5); // Points at (0,0), (0.1,0), ... - store.add_scan( - Pose2D::identity(), - Pose2D::new(10.0, 0.0, 0.0), // Translate by (10, 0) - &cloud1, - 0, - 1.0, - ); - - let cloud2 = make_point_cloud(5); - store.add_scan( - Pose2D::identity(), - Pose2D::new(20.0, 0.0, 0.0), // Translate by (20, 0) - &cloud2, - 0, - 1.0, - ); - - let all_points = store.all_world_points(); - assert_eq!(all_points.len(), 10); - - // First 5 points should be around x=10 - assert!((all_points[0].x - 10.0).abs() < 0.01); - // Last 5 points should be around x=20 - assert!((all_points[5].x - 20.0).abs() < 0.01); - } - - #[test] - fn test_scan_store_clear_and_reset() { - let mut store = ScanStore::with_defaults(); - let cloud = make_point_cloud(10); - - store.add_scan(Pose2D::identity(), Pose2D::identity(), &cloud, 0, 1.0); - store.add_scan(Pose2D::identity(), Pose2D::identity(), &cloud, 0, 1.0); - - assert_eq!(store.next_scan_id(), 2); - - store.clear(); - assert!(store.is_empty()); - assert_eq!(store.next_scan_id(), 2); // ID preserved - - store.add_scan(Pose2D::identity(), Pose2D::identity(), &cloud, 0, 1.0); - assert_eq!(store.latest().unwrap().scan_id, 2); - - store.reset(); - assert!(store.is_empty()); - assert_eq!(store.next_scan_id(), 0); // ID reset - } -} diff --git a/vastu-map/src/integration/spatial_index.rs b/vastu-map/src/integration/spatial_index.rs deleted file mode 100644 index ba62b43..0000000 --- a/vastu-map/src/integration/spatial_index.rs +++ /dev/null @@ -1,406 +0,0 @@ -//! Spatial indexing for efficient map queries. -//! -//! Uses an R-tree to enable fast spatial queries: -//! - Nearest line to a point -//! - Lines within a bounding box -//! - Lines within a radius - -use rstar::{AABB, PointDistance, RTree, RTreeObject}; - -use crate::core::{Bounds, Point2D}; -use crate::features::Line2D; - -/// An indexed line segment for R-tree storage. -#[derive(Clone, Debug)] -pub struct IndexedLine { - /// The line segment. - pub line: Line2D, - /// Index of this line in the original collection. - pub index: usize, -} - -impl IndexedLine { - /// Create a new indexed line. - pub fn new(line: Line2D, index: usize) -> Self { - Self { line, index } - } -} - -impl RTreeObject for IndexedLine { - type Envelope = AABB<[f32; 2]>; - - fn envelope(&self) -> Self::Envelope { - let min_x = self.line.start.x.min(self.line.end.x); - let max_x = self.line.start.x.max(self.line.end.x); - let min_y = self.line.start.y.min(self.line.end.y); - let max_y = self.line.start.y.max(self.line.end.y); - - AABB::from_corners([min_x, min_y], [max_x, max_y]) - } -} - -impl PointDistance for IndexedLine { - fn distance_2(&self, point: &[f32; 2]) -> f32 { - let p = Point2D::new(point[0], point[1]); - let dist = self.line.distance_to_point(p); - dist * dist // Squared distance - } - - fn contains_point(&self, _point: &[f32; 2]) -> bool { - false // Lines don't contain points (zero area) - } -} - -/// Spatial index for line segments using R-tree. -/// -/// Enables efficient spatial queries for large maps. -/// -/// # Example -/// ```rust -/// use vastu_map::integration::SpatialIndex; -/// use vastu_map::features::Line2D; -/// use vastu_map::core::Point2D; -/// -/// let lines = vec![ -/// Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(5.0, 0.0)), -/// Line2D::new(Point2D::new(5.0, 0.0), Point2D::new(5.0, 5.0)), -/// ]; -/// -/// let index = SpatialIndex::new(&lines); -/// -/// // Find nearest line to a point -/// if let Some((idx, dist)) = index.nearest_line(Point2D::new(3.0, 0.5)) { -/// println!("Nearest line: {}, distance: {:.3}", idx, dist); -/// } -/// ``` -#[derive(Clone)] -pub struct SpatialIndex { - tree: RTree, -} - -impl SpatialIndex { - /// Create a new spatial index from lines. - pub fn new(lines: &[Line2D]) -> Self { - let indexed: Vec = lines - .iter() - .enumerate() - .map(|(i, line)| IndexedLine::new(*line, i)) - .collect(); - - Self { - tree: RTree::bulk_load(indexed), - } - } - - /// Create an empty spatial index. - pub fn empty() -> Self { - Self { tree: RTree::new() } - } - - /// Number of lines in the index. - pub fn len(&self) -> usize { - self.tree.size() - } - - /// Check if index is empty. - pub fn is_empty(&self) -> bool { - self.tree.size() == 0 - } - - /// Insert a line into the index. - /// - /// O(log n) insertion time. - pub fn insert(&mut self, line: Line2D, index: usize) { - self.tree.insert(IndexedLine::new(line, index)); - } - - /// Insert multiple lines into the index. - /// - /// For small batches (<20), uses incremental insert. - /// For large batches, triggers a rebuild for better tree balance. - /// - /// # Arguments - /// * `lines` - Slice of lines to insert - /// * `start_index` - Starting index for the new lines - pub fn insert_batch(&mut self, lines: &[Line2D], start_index: usize) { - // Threshold for incremental vs rebuild - const REBUILD_THRESHOLD: usize = 50; - - if lines.len() < REBUILD_THRESHOLD { - // Incremental insert for small batches - for (i, line) in lines.iter().enumerate() { - self.tree.insert(IndexedLine::new(*line, start_index + i)); - } - } else { - // For large batches, it's more efficient to rebuild - // Extract existing items and combine with new ones - let existing: Vec = self.tree.iter().cloned().collect(); - let new: Vec = lines - .iter() - .enumerate() - .map(|(i, line)| IndexedLine::new(*line, start_index + i)) - .collect(); - - let mut all = existing; - all.extend(new); - self.tree = RTree::bulk_load(all); - } - } - - /// Find the nearest line to a point. - /// - /// Returns (index, distance) of the nearest line, or None if index is empty. - pub fn nearest_line(&self, point: Point2D) -> Option<(usize, f32)> { - let p = [point.x, point.y]; - self.tree.nearest_neighbor(&p).map(|indexed| { - let dist = indexed.line.distance_to_point(point); - (indexed.index, dist) - }) - } - - /// Find the k nearest lines to a point. - /// - /// Returns vector of (index, distance) pairs, sorted by distance. - pub fn k_nearest_lines(&self, point: Point2D, k: usize) -> Vec<(usize, f32)> { - let p = [point.x, point.y]; - self.tree - .nearest_neighbor_iter(&p) - .take(k) - .map(|indexed| { - let dist = indexed.line.distance_to_point(point); - (indexed.index, dist) - }) - .collect() - } - - /// Find all lines within a given distance of a point. - pub fn lines_within_distance(&self, point: Point2D, max_distance: f32) -> Vec<(usize, f32)> { - let p = [point.x, point.y]; - let max_dist_sq = max_distance * max_distance; - - self.tree - .nearest_neighbor_iter(&p) - .take_while(|indexed| { - let dist = indexed.line.distance_to_point(point); - dist * dist <= max_dist_sq - }) - .map(|indexed| { - let dist = indexed.line.distance_to_point(point); - (indexed.index, dist) - }) - .collect() - } - - /// Find all lines intersecting a bounding box. - pub fn lines_in_bounds(&self, bounds: &Bounds) -> Vec { - let aabb = AABB::from_corners([bounds.min.x, bounds.min.y], [bounds.max.x, bounds.max.y]); - - self.tree - .locate_in_envelope_intersecting(&aabb) - .map(|indexed| indexed.index) - .collect() - } - - /// Find all lines that could potentially match a query line. - /// - /// Uses the query line's bounding box expanded by a margin. - pub fn potential_matches(&self, query: &Line2D, margin: f32) -> Vec { - let min_x = query.start.x.min(query.end.x) - margin; - let max_x = query.start.x.max(query.end.x) + margin; - let min_y = query.start.y.min(query.end.y) - margin; - let max_y = query.start.y.max(query.end.y) + margin; - - let aabb = AABB::from_corners([min_x, min_y], [max_x, max_y]); - - self.tree - .locate_in_envelope_intersecting(&aabb) - .map(|indexed| indexed.index) - .collect() - } - - /// Rebuild the index with new lines. - pub fn rebuild(&mut self, lines: &[Line2D]) { - let indexed: Vec = lines - .iter() - .enumerate() - .map(|(i, line)| IndexedLine::new(*line, i)) - .collect(); - - self.tree = RTree::bulk_load(indexed); - } - - /// Clear the index. - pub fn clear(&mut self) { - self.tree = RTree::new(); - } -} - -impl Default for SpatialIndex { - fn default() -> Self { - Self::empty() - } -} - -impl std::fmt::Debug for SpatialIndex { - fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result { - f.debug_struct("SpatialIndex") - .field("size", &self.tree.size()) - .finish() - } -} - -#[cfg(test)] -mod tests { - use super::*; - - fn make_test_lines() -> Vec { - vec![ - Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(5.0, 0.0)), // Horizontal at y=0 - Line2D::new(Point2D::new(5.0, 0.0), Point2D::new(5.0, 5.0)), // Vertical at x=5 - Line2D::new(Point2D::new(5.0, 5.0), Point2D::new(0.0, 5.0)), // Horizontal at y=5 - Line2D::new(Point2D::new(0.0, 5.0), Point2D::new(0.0, 0.0)), // Vertical at x=0 - Line2D::new(Point2D::new(10.0, 0.0), Point2D::new(15.0, 0.0)), // Far horizontal - ] - } - - #[test] - fn test_new_index() { - let lines = make_test_lines(); - let index = SpatialIndex::new(&lines); - - assert_eq!(index.len(), 5); - assert!(!index.is_empty()); - } - - #[test] - fn test_empty_index() { - let index = SpatialIndex::empty(); - - assert_eq!(index.len(), 0); - assert!(index.is_empty()); - assert!(index.nearest_line(Point2D::zero()).is_none()); - } - - #[test] - fn test_nearest_line() { - let lines = make_test_lines(); - let index = SpatialIndex::new(&lines); - - // Point near bottom line - let (idx, dist) = index.nearest_line(Point2D::new(2.5, 0.5)).unwrap(); - assert_eq!(idx, 0); - assert!((dist - 0.5).abs() < 0.01); - - // Point near right line - let (idx, dist) = index.nearest_line(Point2D::new(4.5, 2.5)).unwrap(); - assert_eq!(idx, 1); - assert!((dist - 0.5).abs() < 0.01); - } - - #[test] - fn test_k_nearest() { - let lines = make_test_lines(); - let index = SpatialIndex::new(&lines); - - // Point at corner (near lines 0 and 3) - let nearest = index.k_nearest_lines(Point2D::new(0.5, 0.5), 2); - - assert_eq!(nearest.len(), 2); - // Both should be at similar distances - assert!(nearest[0].1 < 1.0); - assert!(nearest[1].1 < 1.0); - } - - #[test] - fn test_lines_within_distance() { - let lines = make_test_lines(); - let index = SpatialIndex::new(&lines); - - // Point at center of the room - let nearby = index.lines_within_distance(Point2D::new(2.5, 2.5), 3.0); - - // Should find at least the 4 room walls (each at distance 2.5) - // The far line is at distance ~7.9 so should not be included - assert!(nearby.len() >= 4); - // Verify all returned distances are within threshold - for (_, dist) in &nearby { - assert!(*dist <= 3.0, "Distance {} exceeds threshold", dist); - } - } - - #[test] - fn test_lines_in_bounds() { - let lines = make_test_lines(); - let index = SpatialIndex::new(&lines); - - // Bounds covering just the room - let bounds = Bounds::new(Point2D::new(-1.0, -1.0), Point2D::new(6.0, 6.0)); - let in_bounds = index.lines_in_bounds(&bounds); - - // Should find the 4 room walls - assert_eq!(in_bounds.len(), 4); - - // Bounds covering everything - let big_bounds = Bounds::new(Point2D::new(-1.0, -1.0), Point2D::new(20.0, 10.0)); - let all = index.lines_in_bounds(&big_bounds); - assert_eq!(all.len(), 5); - } - - #[test] - fn test_potential_matches() { - let lines = make_test_lines(); - let index = SpatialIndex::new(&lines); - - // Query line similar to the bottom wall - let query = Line2D::new(Point2D::new(1.0, 0.1), Point2D::new(4.0, 0.1)); - let matches = index.potential_matches(&query, 0.5); - - // Should find the bottom wall - assert!(matches.contains(&0)); - } - - #[test] - fn test_insert() { - let mut index = SpatialIndex::empty(); - - index.insert( - Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(1.0, 0.0)), - 0, - ); - index.insert( - Line2D::new(Point2D::new(0.0, 1.0), Point2D::new(1.0, 1.0)), - 1, - ); - - assert_eq!(index.len(), 2); - - let (idx, _) = index.nearest_line(Point2D::new(0.5, 0.1)).unwrap(); - assert_eq!(idx, 0); - } - - #[test] - fn test_rebuild() { - let lines1 = vec![Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(1.0, 0.0))]; - let lines2 = vec![ - Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(1.0, 0.0)), - Line2D::new(Point2D::new(0.0, 1.0), Point2D::new(1.0, 1.0)), - ]; - - let mut index = SpatialIndex::new(&lines1); - assert_eq!(index.len(), 1); - - index.rebuild(&lines2); - assert_eq!(index.len(), 2); - } - - #[test] - fn test_clear() { - let lines = make_test_lines(); - let mut index = SpatialIndex::new(&lines); - - assert_eq!(index.len(), 5); - - index.clear(); - assert!(index.is_empty()); - } -} diff --git a/vastu-map/src/io/mod.rs b/vastu-map/src/io/mod.rs new file mode 100644 index 0000000..91c7f08 --- /dev/null +++ b/vastu-map/src/io/mod.rs @@ -0,0 +1,60 @@ +//! Map persistence and export. +//! +//! This module provides save/load functionality: +//! +//! - **Native .vastu format**: Binary format preserving all cell data +//! - **PGM export**: ROS-compatible occupancy grid images +//! - **SVG export**: Visualization with trajectories for auditing +//! - **Scenario parsing**: YAML-based test scenario definitions +//! +//! ## Saving and Loading Maps +//! +//! ```rust,ignore +//! use vastu_map::io::{save_vastu, load_vastu}; +//! use std::path::Path; +//! +//! // Save map +//! save_vastu(map.storage(), Path::new("map.vastu"))?; +//! +//! // Load map +//! let storage = load_vastu(Path::new("map.vastu"))?; +//! ``` +//! +//! ## ROS Export +//! +//! Export maps in ROS map_server format: +//! +//! ```rust,ignore +//! use vastu_map::io::export_ros_map; +//! use std::path::Path; +//! +//! // Creates map.pgm and map.yaml +//! export_ros_map(map.storage(), Path::new("map"))?; +//! ``` +//! +//! ## SVG Visualization +//! +//! Generate SVG audit files with map and trajectory visualization: +//! +//! ```rust,ignore +//! use vastu_map::io::{SvgVisualizer, SvgConfig, markers_by_distance}; +//! +//! let visualizer = SvgVisualizer::new(storage, SvgConfig::default()) +//! .with_title("Test Run") +//! .with_ground_truth(gt_poses, markers_by_distance(>_poses, 0.5)) +//! .with_estimated(est_poses, markers); +//! visualizer.save(Path::new("output.svg"))?; +//! ``` + +pub mod pgm; +pub mod scenario; +pub mod svg; +pub mod vastu_format; + +pub use vastu_format::{IoError, load_vastu, read_vastu, save_vastu, write_vastu}; + +pub use pgm::{export_pgm, export_ros_map, export_yaml, write_pgm}; + +pub use scenario::{Command, Scenario, ScenarioError, SeedConfig, StartPose, SvgOutputConfig}; + +pub use svg::{SvgConfig, SvgVisualizer, markers_by_distance, markers_by_time}; diff --git a/vastu-map/src/io/pgm.rs b/vastu-map/src/io/pgm.rs new file mode 100644 index 0000000..9f78b50 --- /dev/null +++ b/vastu-map/src/io/pgm.rs @@ -0,0 +1,140 @@ +//! PGM export for ROS-compatible occupancy grids. +//! +//! Exports the map in the standard ROS map_server format: +//! - PGM image file (P5 format, grayscale) +//! - YAML metadata file +//! +//! Gray values: +//! - 255: Free (Floor) +//! - 0: Occupied (Wall, Cliff, Bump) +//! - 205: Unknown + +use crate::core::{CellType, GridCoord}; +use crate::grid::GridStorage; +use std::io::Write; +use std::path::Path; + +use super::vastu_format::IoError; + +/// Gray value for free space +const FREE_VALUE: u8 = 254; + +/// Gray value for occupied +const OCCUPIED_VALUE: u8 = 0; + +/// Gray value for unknown +const UNKNOWN_VALUE: u8 = 205; + +/// Export map as PGM image +pub fn export_pgm(storage: &GridStorage, path: &Path) -> Result<(), IoError> { + let mut file = std::fs::File::create(path).map_err(|e| IoError::Io(e.to_string()))?; + + write_pgm(storage, &mut file) +} + +/// Write PGM to a writer +pub fn write_pgm(storage: &GridStorage, writer: &mut W) -> Result<(), IoError> { + let width = storage.width(); + let height = storage.height(); + + // Write PGM header (P5 = binary grayscale) + writeln!(writer, "P5").map_err(|e| IoError::Io(e.to_string()))?; + writeln!(writer, "# Generated by vastu-map").map_err(|e| IoError::Io(e.to_string()))?; + writeln!(writer, "{} {}", width, height).map_err(|e| IoError::Io(e.to_string()))?; + writeln!(writer, "255").map_err(|e| IoError::Io(e.to_string()))?; + + // Write pixel data (top-to-bottom, ROS convention) + let mut data = Vec::with_capacity(width * height); + for y in (0..height).rev() { + // Y is flipped in ROS + for x in 0..width { + let coord = GridCoord::new(x as i32, y as i32); + let cell_type = storage.get_type(coord); + let value = match cell_type { + CellType::Unknown => UNKNOWN_VALUE, + CellType::Floor => FREE_VALUE, + CellType::Wall | CellType::Cliff | CellType::Bump => OCCUPIED_VALUE, + }; + data.push(value); + } + } + + writer + .write_all(&data) + .map_err(|e| IoError::Io(e.to_string()))?; + + Ok(()) +} + +/// Export YAML metadata file (ROS map_server format) +pub fn export_yaml( + storage: &GridStorage, + pgm_filename: &str, + yaml_path: &Path, +) -> Result<(), IoError> { + let origin = storage.origin(); + let resolution = storage.resolution(); + + let yaml_content = format!( + r#"image: {} +resolution: {} +origin: [{}, {}, 0.0] +negate: 0 +occupied_thresh: 0.65 +free_thresh: 0.196 +"#, + pgm_filename, resolution, origin.x, origin.y + ); + + std::fs::write(yaml_path, yaml_content).map_err(|e| IoError::Io(e.to_string()))?; + + Ok(()) +} + +/// Export both PGM and YAML files +pub fn export_ros_map(storage: &GridStorage, base_path: &Path) -> Result<(), IoError> { + // Create PGM path + let pgm_path = base_path.with_extension("pgm"); + let yaml_path = base_path.with_extension("yaml"); + + // Get just the filename for the YAML reference + let pgm_filename = pgm_path + .file_name() + .and_then(|s| s.to_str()) + .unwrap_or("map.pgm"); + + // Export both files + export_pgm(storage, &pgm_path)?; + export_yaml(storage, pgm_filename, &yaml_path)?; + + Ok(()) +} + +#[cfg(test)] +mod tests { + use super::*; + #[test] + fn test_pgm_export() { + let mut storage = GridStorage::centered(10, 10, 0.1); + + // Set some cells + storage.set_type(GridCoord::new(5, 5), CellType::Floor); + storage.set_type(GridCoord::new(6, 5), CellType::Wall); + + let mut buffer = Vec::new(); + write_pgm(&storage, &mut buffer).unwrap(); + + // Check header + let content = String::from_utf8_lossy(&buffer); + assert!(content.starts_with("P5\n")); + assert!(content.contains("10 10\n")); + } + + #[test] + fn test_cell_type_to_gray() { + // Test that the values match ROS conventions + assert_eq!(FREE_VALUE, 254); + assert_eq!(OCCUPIED_VALUE, 0); + assert_eq!(UNKNOWN_VALUE, 205); + } +} diff --git a/vastu-map/src/io/scenario.rs b/vastu-map/src/io/scenario.rs new file mode 100644 index 0000000..7e05e99 --- /dev/null +++ b/vastu-map/src/io/scenario.rs @@ -0,0 +1,313 @@ +//! Scenario YAML parsing for scan matching tests. +//! +//! A scenario defines a test sequence with: +//! - Map to load +//! - Starting pose +//! - Sequence of movement commands (move/stay) +//! - SVG output configuration + +use serde::{Deserialize, Serialize}; +use std::path::Path; + +/// A test scenario loaded from YAML +#[derive(Clone, Debug, Serialize, Deserialize)] +pub struct Scenario { + /// Human-readable scenario name + pub name: String, + + /// Optional description + #[serde(default)] + pub description: String, + + /// Map name (looked up in maps/ folder) or full path + pub map: String, + + /// Initial pose (None = auto-detect free position) + #[serde(default)] + pub start_pose: Option, + + /// Seed configuration for initial map building + #[serde(default)] + pub seed: SeedConfig, + + /// Sequence of movement commands + pub commands: Vec, + + /// SVG output configuration + #[serde(default)] + pub svg_output: SvgOutputConfig, +} + +/// Starting pose specification +#[derive(Clone, Debug, Serialize, Deserialize)] +pub struct StartPose { + /// X position in meters + pub x: f32, + /// Y position in meters + pub y: f32, + /// Orientation in radians (CCW positive from +X) + #[serde(default)] + pub theta: f32, +} + +/// Map seeding configuration +#[derive(Clone, Debug, Serialize, Deserialize)] +pub struct SeedConfig { + /// Number of scans for seeding + #[serde(default = "default_seed_scans")] + pub scans: usize, + + /// Whether to rotate during seeding (360° coverage) + #[serde(default = "default_true")] + pub rotation: bool, +} + +impl Default for SeedConfig { + fn default() -> Self { + Self { + scans: 20, + rotation: true, + } + } +} + +/// A movement command +#[derive(Clone, Debug, Serialize, Deserialize)] +#[serde(tag = "type", rename_all = "snake_case")] +pub enum Command { + /// Move with given velocities for a duration + Move { + /// Linear velocity in m/s + linear_velocity: f32, + /// Angular velocity in rad/s + angular_velocity: f32, + /// Duration in milliseconds + duration_ms: u64, + }, + /// Stay still for a duration + Stay { + /// Duration in milliseconds + duration_ms: u64, + }, +} + +impl Command { + /// Get duration in milliseconds + pub fn duration_ms(&self) -> u64 { + match self { + Command::Move { duration_ms, .. } => *duration_ms, + Command::Stay { duration_ms } => *duration_ms, + } + } + + /// Get duration in seconds + pub fn duration_secs(&self) -> f32 { + self.duration_ms() as f32 / 1000.0 + } + + /// Get velocities (linear, angular) + pub fn velocities(&self) -> (f32, f32) { + match self { + Command::Move { + linear_velocity, + angular_velocity, + .. + } => (*linear_velocity, *angular_velocity), + Command::Stay { .. } => (0.0, 0.0), + } + } +} + +/// SVG output configuration +#[derive(Clone, Debug, Serialize, Deserialize)] +pub struct SvgOutputConfig { + /// Output filename (default: scenario name + .svg) + #[serde(default)] + pub filename: Option, + + /// Scale: pixels per meter + #[serde(default = "default_svg_scale")] + pub scale: f32, + + /// Distance interval for pose markers (meters) + #[serde(default)] + pub marker_interval_m: Option, + + /// Time interval for pose markers (seconds) + #[serde(default)] + pub marker_interval_s: Option, + + /// Whether to show individual lidar scan points + #[serde(default)] + pub show_scan_points: bool, + + /// Line width for trajectories + #[serde(default = "default_trajectory_width")] + pub trajectory_width: f32, +} + +impl Default for SvgOutputConfig { + fn default() -> Self { + Self { + filename: None, + scale: 50.0, + marker_interval_m: Some(0.5), + marker_interval_s: None, + show_scan_points: false, + trajectory_width: 2.0, + } + } +} + +// Default value functions for serde +fn default_seed_scans() -> usize { + 20 +} +fn default_true() -> bool { + true +} +fn default_svg_scale() -> f32 { + 50.0 +} +fn default_trajectory_width() -> f32 { + 2.0 +} + +/// Error type for scenario loading +#[derive(Debug, Clone)] +pub enum ScenarioError { + /// I/O error reading file + Io(String), + /// YAML parsing error + Parse(String), + /// Invalid map reference + InvalidMap(String), +} + +impl std::fmt::Display for ScenarioError { + fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result { + match self { + ScenarioError::Io(msg) => write!(f, "I/O error: {}", msg), + ScenarioError::Parse(msg) => write!(f, "Parse error: {}", msg), + ScenarioError::InvalidMap(msg) => write!(f, "Invalid map: {}", msg), + } + } +} + +impl std::error::Error for ScenarioError {} + +impl Scenario { + /// Load scenario from YAML file + pub fn load(path: &Path) -> Result { + let contents = + std::fs::read_to_string(path).map_err(|e| ScenarioError::Io(e.to_string()))?; + Self::from_yaml(&contents) + } + + /// Parse from YAML string + pub fn from_yaml(yaml: &str) -> Result { + serde_yaml::from_str(yaml).map_err(|e| ScenarioError::Parse(e.to_string())) + } + + /// Total duration of all commands in milliseconds + pub fn total_duration_ms(&self) -> u64 { + self.commands.iter().map(|c| c.duration_ms()).sum() + } + + /// Total duration of all commands in seconds + pub fn total_duration_secs(&self) -> f32 { + self.total_duration_ms() as f32 / 1000.0 + } + + /// Resolve map path (handles both name and full path) + pub fn resolve_map_path(&self, maps_dir: &Path) -> std::path::PathBuf { + let map_path = Path::new(&self.map); + if map_path.is_absolute() || map_path.exists() { + map_path.to_path_buf() + } else { + maps_dir.join(format!("{}.yaml", self.map)) + } + } + + /// Get the SVG output filename + pub fn svg_filename(&self) -> String { + self.svg_output + .filename + .clone() + .unwrap_or_else(|| format!("{}.svg", self.name.to_lowercase().replace(' ', "_"))) + } +} + +#[cfg(test)] +mod tests { + use super::*; + + #[test] + fn test_parse_simple_scenario() { + let yaml = r#" +name: "Test Scenario" +map: "medium_room" +commands: + - type: move + linear_velocity: 0.2 + angular_velocity: 0.0 + duration_ms: 5000 + - type: stay + duration_ms: 1000 +"#; + let scenario = Scenario::from_yaml(yaml).unwrap(); + assert_eq!(scenario.name, "Test Scenario"); + assert_eq!(scenario.map, "medium_room"); + assert_eq!(scenario.commands.len(), 2); + assert_eq!(scenario.total_duration_ms(), 6000); + } + + #[test] + fn test_parse_with_start_pose() { + let yaml = r#" +name: "With Start Pose" +map: "simple_room" +start_pose: + x: 2.0 + y: 3.0 + theta: 1.57 +commands: + - type: move + linear_velocity: 0.1 + angular_velocity: 0.5 + duration_ms: 2000 +"#; + let scenario = Scenario::from_yaml(yaml).unwrap(); + assert!(scenario.start_pose.is_some()); + let pose = scenario.start_pose.unwrap(); + assert!((pose.x - 2.0).abs() < 1e-6); + assert!((pose.y - 3.0).abs() < 1e-6); + assert!((pose.theta - 1.57).abs() < 1e-6); + } + + #[test] + fn test_command_velocities() { + let move_cmd = Command::Move { + linear_velocity: 0.2, + angular_velocity: 0.5, + duration_ms: 1000, + }; + assert_eq!(move_cmd.velocities(), (0.2, 0.5)); + assert_eq!(move_cmd.duration_secs(), 1.0); + + let stay_cmd = Command::Stay { duration_ms: 500 }; + assert_eq!(stay_cmd.velocities(), (0.0, 0.0)); + assert_eq!(stay_cmd.duration_secs(), 0.5); + } + + #[test] + fn test_svg_filename_generation() { + let yaml = r#" +name: "My Test Scenario" +map: "room" +commands: [] +"#; + let scenario = Scenario::from_yaml(yaml).unwrap(); + assert_eq!(scenario.svg_filename(), "my_test_scenario.svg"); + } +} diff --git a/vastu-map/src/io/svg.rs b/vastu-map/src/io/svg.rs new file mode 100644 index 0000000..fc0a925 --- /dev/null +++ b/vastu-map/src/io/svg.rs @@ -0,0 +1,553 @@ +//! SVG visualization for scan matching audit. +//! +//! Renders occupancy grid maps and robot trajectories to SVG format. +//! The SVG serves as an audit file showing: +//! - The built occupancy grid map +//! - Ground truth trajectory +//! - Estimated (scan-matched) trajectory +//! - Odometry trajectory (optional) + +use crate::core::{CellType, Pose2D, WorldPoint}; +use crate::grid::GridStorage; +use std::fmt::Write; +use std::path::Path; + +/// SVG color scheme for visualization +#[derive(Clone, Debug)] +pub struct SvgColorScheme { + /// Wall/obstacle color + pub wall: &'static str, + /// Floor color + pub floor: &'static str, + /// Unknown cell color + pub unknown: &'static str, + /// Ground truth trajectory color + pub gt_trajectory: &'static str, + /// Estimated trajectory color + pub estimated_trajectory: &'static str, + /// Odometry trajectory color + pub odom_trajectory: &'static str, +} + +impl Default for SvgColorScheme { + fn default() -> Self { + Self { + wall: "#333333", + floor: "#FFFFFF", + unknown: "#CCCCCC", + gt_trajectory: "#22AA22", + estimated_trajectory: "#2222AA", + odom_trajectory: "#AA2222", + } + } +} + +/// Configuration for SVG rendering +#[derive(Clone, Debug)] +pub struct SvgConfig { + /// Pixels per meter + pub scale: f32, + /// Trajectory line width + pub trajectory_width: f32, + /// Pose marker radius + pub marker_radius: f32, + /// Color scheme + pub colors: SvgColorScheme, + /// Padding around the map in pixels + pub padding: f32, +} + +impl Default for SvgConfig { + fn default() -> Self { + Self { + scale: 50.0, + trajectory_width: 2.0, + marker_radius: 4.0, + colors: SvgColorScheme::default(), + padding: 20.0, + } + } +} + +/// A trajectory (sequence of poses) for visualization +#[derive(Clone, Debug)] +pub struct Trajectory { + /// Name for legend + pub name: String, + /// Sequence of poses + pub poses: Vec, + /// Line color + pub color: String, + /// Marker positions (indices into poses) + pub marker_indices: Vec, +} + +/// SVG visualization builder +pub struct SvgVisualizer { + config: SvgConfig, + /// Grid storage for map + storage: GridStorage, + /// Ground truth trajectory + gt_trajectory: Option, + /// Estimated/corrected trajectory + est_trajectory: Option, + /// Odometry trajectory + odom_trajectory: Option, + /// Title to display + title: Option, +} + +impl SvgVisualizer { + /// Create a new SVG visualizer + pub fn new(storage: GridStorage, config: SvgConfig) -> Self { + Self { + config, + storage, + gt_trajectory: None, + est_trajectory: None, + odom_trajectory: None, + title: None, + } + } + + /// Set a title to display + pub fn with_title(mut self, title: impl Into) -> Self { + self.title = Some(title.into()); + self + } + + /// Add ground truth trajectory + pub fn with_ground_truth(mut self, poses: Vec, marker_indices: Vec) -> Self { + self.gt_trajectory = Some(Trajectory { + name: "Ground Truth".to_string(), + poses, + color: self.config.colors.gt_trajectory.to_string(), + marker_indices, + }); + self + } + + /// Add estimated trajectory + pub fn with_estimated(mut self, poses: Vec, marker_indices: Vec) -> Self { + self.est_trajectory = Some(Trajectory { + name: "Estimated".to_string(), + poses, + color: self.config.colors.estimated_trajectory.to_string(), + marker_indices, + }); + self + } + + /// Add odometry trajectory + pub fn with_odometry(mut self, poses: Vec, marker_indices: Vec) -> Self { + self.odom_trajectory = Some(Trajectory { + name: "Odometry".to_string(), + poses, + color: self.config.colors.odom_trajectory.to_string(), + marker_indices, + }); + self + } + + /// Render to SVG string + pub fn render(&self) -> String { + let mut svg = String::new(); + + // Calculate bounds + let (min_world, max_world) = self.storage.bounds(); + let map_width_px = (max_world.x - min_world.x) * self.config.scale; + let map_height_px = (max_world.y - min_world.y) * self.config.scale; + + let padding = self.config.padding; + let title_height = if self.title.is_some() { 30.0 } else { 0.0 }; + let legend_height = 100.0; + + let width = map_width_px + 2.0 * padding; + let height = map_height_px + 2.0 * padding + title_height + legend_height; + + // SVG header + writeln!(&mut svg, r#""#).unwrap(); + writeln!( + &mut svg, + r#""#, + width, height, width, height + ).unwrap(); + + // Background + writeln!( + &mut svg, + r##" "## + ) + .unwrap(); + + // Title + if let Some(ref title) = self.title { + writeln!( + &mut svg, + r##" {}"##, + width / 2.0, title + ).unwrap(); + } + + // Map group with translation + let map_offset_x = padding; + let map_offset_y = padding + title_height; + writeln!( + &mut svg, + r#" "#, + map_offset_x, map_offset_y + ) + .unwrap(); + + // Render grid cells + self.render_grid(&mut svg, min_world, map_height_px); + + // Render trajectories (order: odom, estimated, ground truth - so GT is on top) + if let Some(ref traj) = self.odom_trajectory { + self.render_trajectory(&mut svg, traj, min_world, map_height_px); + } + if let Some(ref traj) = self.est_trajectory { + self.render_trajectory(&mut svg, traj, min_world, map_height_px); + } + if let Some(ref traj) = self.gt_trajectory { + self.render_trajectory(&mut svg, traj, min_world, map_height_px); + } + + writeln!(&mut svg, " ").unwrap(); + + // Render legend + let legend_y = map_offset_y + map_height_px + 10.0; + self.render_legend(&mut svg, width, legend_y); + + // SVG footer + writeln!(&mut svg, "").unwrap(); + + svg + } + + /// Render grid cells + fn render_grid(&self, svg: &mut String, _min_world: WorldPoint, height_px: f32) { + let resolution = self.storage.resolution(); + let cell_size = resolution * self.config.scale; + + writeln!(svg, r#" "#).unwrap(); + + // Draw background for unknown area + writeln!( + svg, + r#" "#, + self.storage.width() as f32 * cell_size, + self.storage.height() as f32 * cell_size, + self.config.colors.unknown + ) + .unwrap(); + + for (coord, cell) in self.storage.iter() { + let x = coord.x as usize; + let y = coord.y as usize; + + let color = match cell.cell_type { + CellType::Unknown => continue, // Skip - already background + CellType::Floor => self.config.colors.floor, + CellType::Wall | CellType::Cliff | CellType::Bump => self.config.colors.wall, + }; + + // SVG Y-axis is flipped (0 at top) + let px_x = x as f32 * cell_size; + let px_y = height_px - ((y + 1) as f32 * cell_size); + + writeln!( + svg, + r#" "#, + px_x, px_y, cell_size, cell_size, color + ) + .unwrap(); + } + + writeln!(svg, " ").unwrap(); + } + + /// Render a trajectory + fn render_trajectory( + &self, + svg: &mut String, + traj: &Trajectory, + min_world: WorldPoint, + height_px: f32, + ) { + if traj.poses.is_empty() { + return; + } + + let id = traj.name.to_lowercase().replace(' ', "-"); + writeln!(svg, r#" "#, id).unwrap(); + + // Draw path line + let mut path_d = String::new(); + for (i, pose) in traj.poses.iter().enumerate() { + let px = (pose.x - min_world.x) * self.config.scale; + let py = height_px - (pose.y - min_world.y) * self.config.scale; + + if i == 0 { + write!(&mut path_d, "M {:.1} {:.1}", px, py).unwrap(); + } else { + write!(&mut path_d, " L {:.1} {:.1}", px, py).unwrap(); + } + } + + writeln!( + svg, + r#" "#, + path_d, traj.color, self.config.trajectory_width + ).unwrap(); + + // Draw markers + for &idx in &traj.marker_indices { + if idx < traj.poses.len() { + let pose = &traj.poses[idx]; + let px = (pose.x - min_world.x) * self.config.scale; + let py = height_px - (pose.y - min_world.y) * self.config.scale; + + // Draw direction indicator + let arrow_len = self.config.marker_radius * 2.0; + let dx = arrow_len * pose.theta.cos(); + let dy = -arrow_len * pose.theta.sin(); // Flip Y for SVG + + // Circle marker + writeln!( + svg, + r#" "#, + px, py, self.config.marker_radius, traj.color + ).unwrap(); + + // Direction line + writeln!( + svg, + r#" "#, + px, py, px + dx, py + dy + ).unwrap(); + writeln!( + svg, + r#" "#, + px, py, px + dx, py + dy, traj.color + ).unwrap(); + } + } + + writeln!(svg, " ").unwrap(); + } + + /// Render legend + fn render_legend(&self, svg: &mut String, svg_width: f32, y_offset: f32) { + writeln!( + svg, + r#" "#, + y_offset + ).unwrap(); + + // Legend background + writeln!( + svg, + r##" "##, + svg_width - 20.0 + ).unwrap(); + + let mut entry_y = 20.0; + + // Legend entries + let entries = [ + ( + self.gt_trajectory.as_ref(), + "Ground Truth", + self.config.colors.gt_trajectory, + ), + ( + self.est_trajectory.as_ref(), + "Estimated", + self.config.colors.estimated_trajectory, + ), + ( + self.odom_trajectory.as_ref(), + "Odometry", + self.config.colors.odom_trajectory, + ), + ]; + + for (traj_opt, label, color) in entries { + if traj_opt.is_some() { + // Color line + writeln!( + svg, + r#" "#, + entry_y, entry_y, color + ).unwrap(); + + // Label + writeln!( + svg, + r##" {}"##, + entry_y + 4.0, + label + ) + .unwrap(); + + entry_y += 20.0; + } + } + + // Map legend (right side) + let map_legend_x = svg_width - 150.0; + writeln!( + svg, + r#" "#, + map_legend_x, self.config.colors.wall + ) + .unwrap(); + writeln!( + svg, + r##" Walls/Obstacles"##, + map_legend_x + 20.0 + ) + .unwrap(); + + writeln!( + svg, + r##" "##, + map_legend_x, self.config.colors.floor + ) + .unwrap(); + writeln!( + svg, + r##" Floor"##, + map_legend_x + 20.0 + ) + .unwrap(); + + writeln!( + svg, + r#" "#, + map_legend_x, self.config.colors.unknown + ) + .unwrap(); + writeln!( + svg, + r##" Unknown"##, + map_legend_x + 20.0 + ) + .unwrap(); + + writeln!(svg, " ").unwrap(); + } + + /// Save to file + pub fn save(&self, path: &Path) -> Result<(), std::io::Error> { + let svg_content = self.render(); + std::fs::write(path, svg_content) + } +} + +/// Calculate marker indices based on distance interval +pub fn markers_by_distance(poses: &[Pose2D], interval_m: f32) -> Vec { + if poses.is_empty() { + return vec![]; + } + + let mut markers = vec![0]; // Always include start + let mut accumulated_dist = 0.0f32; + + for i in 1..poses.len() { + let dist = poses[i].distance(&poses[i - 1]); + accumulated_dist += dist; + + if accumulated_dist >= interval_m { + markers.push(i); + accumulated_dist = 0.0; + } + } + + // Always include end if different from last marker + if poses.len() > 1 && markers.last() != Some(&(poses.len() - 1)) { + markers.push(poses.len() - 1); + } + + markers +} + +/// Calculate marker indices based on time interval +pub fn markers_by_time(num_poses: usize, total_time_s: f32, interval_s: f32) -> Vec { + if num_poses == 0 { + return vec![]; + } + + let time_per_pose = total_time_s / num_poses as f32; + let poses_per_marker = (interval_s / time_per_pose).max(1.0) as usize; + + let mut markers: Vec = (0..num_poses).step_by(poses_per_marker).collect(); + + // Always include end + if num_poses > 1 && markers.last() != Some(&(num_poses - 1)) { + markers.push(num_poses - 1); + } + + markers +} + +#[cfg(test)] +mod tests { + use super::*; + use crate::core::GridCoord; + + #[test] + fn test_markers_by_distance() { + let poses = vec![ + Pose2D::new(0.0, 0.0, 0.0), + Pose2D::new(0.3, 0.0, 0.0), + Pose2D::new(0.6, 0.0, 0.0), + Pose2D::new(0.9, 0.0, 0.0), + Pose2D::new(1.2, 0.0, 0.0), + ]; + + let markers = markers_by_distance(&poses, 0.5); + // Should have markers at: 0 (start), ~1 (0.6m), ~3 (1.2m), 4 (end) + assert!(markers.contains(&0)); + assert!(markers.contains(&(poses.len() - 1))); + } + + #[test] + fn test_markers_by_time() { + let markers = markers_by_time(100, 10.0, 1.0); + // 100 poses over 10 seconds, marker every 1 second = every 10 poses + assert!(markers.contains(&0)); + assert!(markers.contains(&99)); + assert!(markers.len() >= 10); + } + + #[test] + fn test_svg_render_basic() { + let storage = GridStorage::new(10, 10, 0.1, WorldPoint::new(0.0, 0.0)); + let config = SvgConfig::default(); + let visualizer = SvgVisualizer::new(storage, config); + + let svg = visualizer.render(); + assert!(svg.contains("")); + assert!(svg.contains("grid")); + } + + #[test] + fn test_svg_with_trajectory() { + let mut storage = GridStorage::new(10, 10, 0.1, WorldPoint::new(0.0, 0.0)); + storage.set_type(GridCoord::new(5, 5), CellType::Wall); + + let poses = vec![Pose2D::new(0.1, 0.1, 0.0), Pose2D::new(0.5, 0.5, 0.785)]; + + let config = SvgConfig::default(); + let visualizer = SvgVisualizer::new(storage, config) + .with_title("Test") + .with_ground_truth(poses.clone(), vec![0, 1]); + + let svg = visualizer.render(); + assert!(svg.contains("ground-truth")); + assert!(svg.contains("Test")); + } +} diff --git a/vastu-map/src/io/vastu_format.rs b/vastu-map/src/io/vastu_format.rs new file mode 100644 index 0000000..4b5482a --- /dev/null +++ b/vastu-map/src/io/vastu_format.rs @@ -0,0 +1,320 @@ +//! Native .vastu binary format for map persistence. +//! +//! Format: +//! - Header (32 bytes): +//! - Magic: "VASTU" (5 bytes) +//! - Version: u8 (1 byte) +//! - Width: u32 (4 bytes, little-endian) +//! - Height: u32 (4 bytes, little-endian) +//! - Resolution: f32 (4 bytes, little-endian) +//! - Origin X: f32 (4 bytes, little-endian) +//! - Origin Y: f32 (4 bytes, little-endian) +//! - Reserved: 6 bytes +//! - Cell data: width * height bytes (1 byte per cell) + +use crate::core::{Cell, CellType, WorldPoint}; +use crate::grid::GridStorage; +use std::io::{Read, Write}; +use std::path::Path; + +/// Magic bytes for .vastu format +const MAGIC: &[u8; 5] = b"VASTU"; + +/// Current format version +const VERSION: u8 = 1; + +/// Header size in bytes +const HEADER_SIZE: usize = 32; + +/// Error type for I/O operations +#[derive(Debug, Clone)] +pub enum IoError { + /// File I/O error + Io(String), + /// Invalid format + InvalidFormat(String), + /// Version mismatch + VersionMismatch { + /// Expected format version + expected: u8, + /// Found format version + found: u8, + }, +} + +impl std::fmt::Display for IoError { + fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result { + match self { + IoError::Io(msg) => write!(f, "I/O error: {}", msg), + IoError::InvalidFormat(msg) => write!(f, "Invalid format: {}", msg), + IoError::VersionMismatch { expected, found } => { + write!( + f, + "Version mismatch: expected {}, found {}", + expected, found + ) + } + } + } +} + +impl std::error::Error for IoError {} + +/// Save a grid to .vastu binary format +pub fn save_vastu(storage: &GridStorage, path: &Path) -> Result<(), IoError> { + let mut file = std::fs::File::create(path).map_err(|e| IoError::Io(e.to_string()))?; + + write_vastu(storage, &mut file) +} + +/// Write grid to a writer in .vastu format +pub fn write_vastu(storage: &GridStorage, writer: &mut W) -> Result<(), IoError> { + // Write header + let mut header = [0u8; HEADER_SIZE]; + + // Magic + header[0..5].copy_from_slice(MAGIC); + + // Version + header[5] = VERSION; + + // Width (little-endian u32) + let width = storage.width() as u32; + header[6..10].copy_from_slice(&width.to_le_bytes()); + + // Height (little-endian u32) + let height = storage.height() as u32; + header[10..14].copy_from_slice(&height.to_le_bytes()); + + // Resolution (little-endian f32) + let resolution = storage.resolution(); + header[14..18].copy_from_slice(&resolution.to_le_bytes()); + + // Origin X (little-endian f32) + let origin = storage.origin(); + header[18..22].copy_from_slice(&origin.x.to_le_bytes()); + + // Origin Y (little-endian f32) + header[22..26].copy_from_slice(&origin.y.to_le_bytes()); + + // Reserved bytes (zeros) + // Already zero from initialization + + writer + .write_all(&header) + .map_err(|e| IoError::Io(e.to_string()))?; + + // Write cell data + let mut cell_data = Vec::with_capacity(storage.width() * storage.height()); + for y in 0..storage.height() { + for x in 0..storage.width() { + let coord = crate::core::GridCoord::new(x as i32, y as i32); + let cell = storage.get(coord).unwrap_or_default(); + cell_data.push(encode_cell(&cell)); + } + } + + writer + .write_all(&cell_data) + .map_err(|e| IoError::Io(e.to_string()))?; + + Ok(()) +} + +/// Load a grid from .vastu binary format +pub fn load_vastu(path: &Path) -> Result { + let mut file = std::fs::File::open(path).map_err(|e| IoError::Io(e.to_string()))?; + + read_vastu(&mut file) +} + +/// Read grid from a reader in .vastu format +pub fn read_vastu(reader: &mut R) -> Result { + // Read header + let mut header = [0u8; HEADER_SIZE]; + reader + .read_exact(&mut header) + .map_err(|e| IoError::Io(e.to_string()))?; + + // Verify magic + if &header[0..5] != MAGIC { + return Err(IoError::InvalidFormat("Invalid magic bytes".to_string())); + } + + // Check version + let version = header[5]; + if version != VERSION { + return Err(IoError::VersionMismatch { + expected: VERSION, + found: version, + }); + } + + // Read dimensions + let width = u32::from_le_bytes([header[6], header[7], header[8], header[9]]) as usize; + let height = u32::from_le_bytes([header[10], header[11], header[12], header[13]]) as usize; + + // Read resolution + let resolution = f32::from_le_bytes([header[14], header[15], header[16], header[17]]); + + // Read origin + let origin_x = f32::from_le_bytes([header[18], header[19], header[20], header[21]]); + let origin_y = f32::from_le_bytes([header[22], header[23], header[24], header[25]]); + let origin = WorldPoint::new(origin_x, origin_y); + + // Create storage + let mut storage = GridStorage::new(width, height, resolution, origin); + + // Read cell data + let mut cell_data = vec![0u8; width * height]; + reader + .read_exact(&mut cell_data) + .map_err(|e| IoError::Io(e.to_string()))?; + + // Decode cells + for (i, &byte) in cell_data.iter().enumerate() { + let x = (i % width) as i32; + let y = (i / width) as i32; + let coord = crate::core::GridCoord::new(x, y); + + let cell = decode_cell(byte); + if let Some(c) = storage.get_mut(coord) { + *c.cell_type = cell.cell_type as u8; + *c.confidence = cell.confidence; + *c.observation_count = cell.observation_count; + *c.swept = if cell.swept { 1 } else { 0 }; + } + } + + Ok(storage) +} + +/// Encode a cell to a single byte +fn encode_cell(cell: &Cell) -> u8 { + let type_bits = cell.cell_type as u8; + let flags = if cell.swept { 0x80 } else { 0 }; + type_bits | flags +} + +/// Decode a cell from a single byte +fn decode_cell(byte: u8) -> Cell { + let cell_type = match byte & 0x0F { + 0 => CellType::Unknown, + 1 => CellType::Floor, + 2 => CellType::Wall, + 3 => CellType::Cliff, + 4 => CellType::Bump, + _ => CellType::Unknown, + }; + let swept = (byte & 0x80) != 0; + + Cell { + cell_type, + swept, + ..Default::default() + } +} + +#[cfg(test)] +mod tests { + use super::*; + use crate::core::GridCoord; + use std::io::Cursor; + + #[test] + fn test_round_trip() { + // Create a test grid + let mut storage = GridStorage::centered(10, 10, 0.1); + + // Set some cells + storage.set_type(GridCoord::new(5, 5), CellType::Floor); + storage.set_type(GridCoord::new(6, 5), CellType::Wall); + storage.set_type(GridCoord::new(7, 5), CellType::Cliff); + storage.set_type(GridCoord::new(8, 5), CellType::Bump); + + // Mark one cell as swept + if let Some(cell) = storage.get_mut(GridCoord::new(5, 5)) { + *cell.swept = 1; + } + + // Write to buffer + let mut buffer = Vec::new(); + write_vastu(&storage, &mut buffer).unwrap(); + + // Read back + let mut cursor = Cursor::new(buffer); + let loaded = read_vastu(&mut cursor).unwrap(); + + // Verify dimensions + assert_eq!(loaded.width(), storage.width()); + assert_eq!(loaded.height(), storage.height()); + assert!((loaded.resolution() - storage.resolution()).abs() < 0.001); + + // Verify cells + assert_eq!(loaded.get_type(GridCoord::new(5, 5)), CellType::Floor); + assert_eq!(loaded.get_type(GridCoord::new(6, 5)), CellType::Wall); + assert_eq!(loaded.get_type(GridCoord::new(7, 5)), CellType::Cliff); + assert_eq!(loaded.get_type(GridCoord::new(8, 5)), CellType::Bump); + assert_eq!(loaded.get_type(GridCoord::new(0, 0)), CellType::Unknown); + + // Verify swept flag + assert!( + loaded + .get(GridCoord::new(5, 5)) + .map(|c| c.swept) + .unwrap_or(false) + ); + } + + #[test] + fn test_invalid_magic() { + // Create full header with wrong magic + let mut data = Vec::new(); + data.extend_from_slice(b"WRONG"); // Invalid magic (5 bytes) + data.push(1); // version + data.extend([0u8; HEADER_SIZE - 6]); // Rest of header + + let mut cursor = Cursor::new(data); + let result = read_vastu(&mut cursor); + + assert!(matches!(result, Err(IoError::InvalidFormat(_)))); + } + + #[test] + fn test_version_mismatch() { + let mut data = Vec::new(); + data.extend_from_slice(MAGIC); + data.push(99); // Wrong version + data.extend([0u8; HEADER_SIZE - 6]); + + let mut cursor = Cursor::new(data); + let result = read_vastu(&mut cursor); + + assert!(matches!(result, Err(IoError::VersionMismatch { .. }))); + } + + #[test] + fn test_encode_decode_cell() { + for cell_type in [ + CellType::Unknown, + CellType::Floor, + CellType::Wall, + CellType::Cliff, + CellType::Bump, + ] { + for swept in [false, true] { + let original = Cell { + cell_type, + swept, + ..Default::default() + }; + + let encoded = encode_cell(&original); + let decoded = decode_cell(encoded); + + assert_eq!(decoded.cell_type, original.cell_type); + assert_eq!(decoded.swept, original.swept); + } + } + } +} diff --git a/vastu-map/src/lib.rs b/vastu-map/src/lib.rs index ab0ce1a..a7e1739 100644 --- a/vastu-map/src/lib.rs +++ b/vastu-map/src/lib.rs @@ -1,446 +1,426 @@ -#![feature(portable_simd)] - -//! # Vastu-Map: Feature-Based 2D SLAM Library +//! # Vastu-Map //! -//! A high-performance SLAM (Simultaneous Localization and Mapping) library -//! with SIMD-friendly data layouts, designed for indoor robot navigation. +//! Occupancy grid mapping library for robot exploration and navigation. +//! +//! ## Overview +//! +//! Vastu-Map provides a multi-layer occupancy grid with semantic cell types: +//! +//! - **Unknown** - Not yet observed by any sensor +//! - **Floor** - Traversable surface +//! - **Wall** - Lidar-detected obstacles +//! - **Cliff** - Floor drop-offs (stairs, ledges) +//! - **Bump** - Invisible obstacles (glass, mirrors, thin legs) //! //! ## Features //! -//! - **VectorMap Representation**: Uses line segments and corners instead of -//! occupancy grids for memory-efficient mapping -//! - **Full SLAM Pipeline**: Localization and mapping integrated into a clean API -//! - **SIMD Optimized**: Data layouts designed for LLVM auto-vectorization (works on ARM, x86, etc.) -//! - **SoA Data Layout**: Struct-of-Arrays for cache-friendly SIMD operations +//! - **Map Exploration**: Frontier-based autonomous exploration +//! - **Path Finding**: A* on occupancy grid with safety margins +//! - **Coverage Planning**: Full surface sweeping (future) //! //! ## Quick Start //! -//! ```rust,no_run -//! use vastu_map::{VectorMap, VectorMapConfig, Map}; -//! use vastu_map::core::{Pose2D, PointCloud2D, Point2D}; +//! ```rust,ignore +//! use vastu_map::{OccupancyGridMap, MapConfig, Pose2D, LidarScan}; //! //! // Create map with default configuration -//! let config = VectorMapConfig::default(); -//! let mut map = VectorMap::new(config); +//! let config = MapConfig::default(); +//! let mut map = OccupancyGridMap::new(config); //! -//! // Process a scan observation -//! let scan = PointCloud2D::from_points(&[ -//! Point2D::new(1.0, 0.0), -//! Point2D::new(1.0, 0.1), -//! Point2D::new(1.0, 0.2), -//! ]); -//! let odometry = Pose2D::identity(); -//! let result = map.observe(&scan, odometry); -//! println!("Pose: ({:.2}, {:.2}), confidence: {:.2}", -//! result.pose.x, result.pose.y, result.confidence); +//! // Process lidar scan +//! let pose = Pose2D::new(0.0, 0.0, 0.0); +//! let scan = LidarScan::new(ranges, angles, 0.15, 8.0); +//! let result = map.observe_lidar(&scan, pose); +//! +//! println!("Updated {} cells", result.cells_updated); //! ``` //! -//! ## Coordinate Frame +//! ## Coordinate System //! -//! All coordinates follow the ROS REP-103 convention: -//! - **X-forward**: Positive X is in front of the robot -//! - **Y-left**: Positive Y is to the left of the robot -//! - **Z-up**: Positive Z is upward (not used in 2D) -//! - **Rotation**: Counter-clockwise positive -//! -//! ## Architecture -//! -//! The library is organized into modules: -//! -//! - [`core`]: Fundamental types (Point2D, Pose2D, PointCloud2D, etc.) -//! - [`simd`]: SIMD primitives for auto-vectorization -//! - [`config`]: Configuration types -//! - [`features`]: Line and corner feature types -//! - [`extraction`]: Line extraction from point clouds -//! - [`matching`]: Scan-to-map matching algorithms (ICP, RANSAC) -//! - [`integration`]: Map integration and merging -//! - [`query`]: Map queries (raycast, occupancy, frontiers) -//! - [`loop_closure`]: Loop closure detection with keyframes -//! - [`vector_map`]: Main VectorMap implementation -//! -//! ## Data Flow -//! -//! ```text -//! ┌─────────────────┐ -//! │ Lidar Scan │ -//! │ (PolarScan) │ -//! └────────┬────────┘ -//! │ to_cartesian() -//! ▼ -//! ┌─────────────────┐ -//! │ PointCloud2D │ -//! │ (SoA layout) │ -//! └────────┬────────┘ -//! │ -//! ┌────────────────────┼────────────────────┐ -//! │ │ │ -//! ▼ ▼ ▼ -//! ┌────────────────┐ ┌────────────────┐ ┌────────────────┐ -//! │ Extraction │ │ Matching │ │ Loop Closure │ -//! │ (Split-Merge) │ │ (Point-Line │ │ (Keyframes + │ -//! │ │ │ ICP) │ │ Descriptors) │ -//! └───────┬────────┘ └───────┬────────┘ └───────┬────────┘ -//! │ │ │ -//! ▼ ▼ │ -//! ┌────────────────┐ ┌────────────────┐ │ -//! │ Lines/Corners │ │ Matched Pose │ │ -//! │ (FeatureSet) │ │ + Confidence │ │ -//! └───────┬────────┘ └───────┬────────┘ │ -//! │ │ │ -//! └──────────┬─────────┘ │ -//! │ │ -//! ▼ │ -//! ┌────────────────┐ │ -//! │ Integration │◄─────────────────────┘ -//! │ (Merge/Add to │ Loop constraints -//! │ VectorMap) │ -//! └───────┬────────┘ -//! │ -//! ▼ -//! ┌────────────────┐ -//! │ VectorMap │──► Query (raycast, occupancy) -//! │ (Lines + │──► Frontiers (exploration) -//! │ Corners) │──► Path Planning -//! └────────────────┘ -//! ``` -//! -//! ## SIMD and Portability -//! -//! The library uses LLVM auto-vectorization patterns that work across different -//! architectures. No platform-specific intrinsics are used—users can configure -//! compiler flags appropriate for their target platform. +//! Uses ROS REP-103 convention: +//! - X: Forward (positive ahead of robot) +//! - Y: Left (positive to robot's left) +//! - Theta: Rotation in radians, CCW positive from +X axis -pub mod config; +#![warn(missing_docs)] +#![feature(portable_simd)] + +// Core types pub mod core; -pub mod exploration; -pub mod extraction; -pub mod features; -pub mod integration; -pub mod loop_closure; -pub mod matching; -pub mod motion_model; -pub mod odometry; + +// Grid storage and configuration +pub mod grid; + +// Unified configuration +pub mod config; + +// Map queries (frontier detection, etc.) pub mod query; -pub mod vector_map; -// Re-export main types at crate root -pub use motion_model::MotionModel; -pub use vector_map::{ConfigError, VectorMap, VectorMapConfig, YamlConfigError}; +// Path planning +pub mod pathfinding; -// Re-export exploration types -pub use exploration::{ - CollisionEvent, CollisionType, ExplorationConfig, ExplorationController, ExplorationState, - ExplorationStep, PathFollower, VelocityCommand, VirtualWall, +// Exploration controller +pub mod exploration; + +// SLAM (scan matching, loop closure) +pub mod slam; + +// Submap architecture for reversible map updates +pub mod submap; + +// High-level exploration API +pub mod explore; + +// Persistence (save/load) +pub mod io; + +// Re-export commonly used types +pub use core::{ + BumperSensors, Cell, CellType, CliffSensors, GridCoord, LidarScan, Pose2D, SensorObservation, + WorldPoint, }; -// Re-export extensibility traits -pub use extraction::{LineExtractor, SplitMergeExtractor}; -pub use loop_closure::LoopDetector; -pub use matching::ScanMatcher; +pub use grid::{CellCounts, ConfigError, GridConfig, GridStorage, MapConfig, SensorConfig}; -// Re-export scan storage for exploration -pub use integration::{ScanStore, ScanStoreConfig, ScanStoreStats, StoredScan}; +pub use config::{ConfigLoadError, VastuConfig}; -// ───────────────────────────────────────────────────────────────────────────── -// Map Trait and Supporting Types -// ───────────────────────────────────────────────────────────────────────────── +pub use submap::{ + InsertResult, MultiSubmapMatchConfig, MultiSubmapMatchResult, MultiSubmapMatcher, StoredScan, + Submap, SubmapConfig, SubmapCorrection, SubmapGraphConfig, SubmapId, SubmapLoopClosure, + SubmapManager, SubmapPoseGraph, SubmapState, +}; -use core::{Bounds, Point2D, PointCloud2D, Pose2D}; - -/// Point occupancy state. -#[derive(Debug, Clone, Copy, PartialEq, Eq)] -pub enum Occupancy { - /// Point is in navigable free space. - Free, - /// Point is on or inside an obstacle. - Occupied, - /// Point has not been observed yet. - Unknown, -} - -/// Exploration frontier representing an unexplored shadow area. -/// -/// A frontier is a safe observation point from which the robot can -/// scan an unexplored region (gap between unconnected lines). -/// -/// # Key Concept -/// -/// Instead of targeting line endpoints (which are walls), frontiers -/// now represent viewpoints that can observe unexplored gaps: -/// -/// ```text -/// Wall ─────┐ endpoint -/// │ -/// GAP │ ← Unexplored shadow area -/// HERE │ -/// │ -/// Wall ─────┘ endpoint -/// -/// Viewpoint is offset from wall, looking into the gap -/// ``` -#[derive(Debug, Clone)] -pub struct Frontier { - /// Safe observation point where robot should navigate to. - /// This is offset from the wall to ensure the robot stays in free space. - pub viewpoint: Point2D, - - /// Direction towards the unexplored area (unit vector). - /// The robot's lidar will scan in this direction when it reaches the viewpoint. - pub look_direction: Point2D, - - /// Original line endpoint that created this frontier. - /// Used for tracking which gap this frontier observes. - pub endpoint: Point2D, - - /// Index of the source line in the map. - pub line_idx: usize, - - /// Estimated unexplored area visible from this viewpoint (m²). - /// Used for prioritizing larger unexplored regions. - pub estimated_area: f32, -} - -/// A planned path through the map. -#[derive(Debug, Clone)] -pub struct Path { - /// Waypoints along the path. - pub points: Vec, - /// Total path length in meters. - pub length: f32, -} - -impl Path { - /// Create a new empty path. - pub fn new() -> Self { - Self { - points: Vec::new(), - length: 0.0, - } - } - - /// Check if the path is empty. - pub fn is_empty(&self) -> bool { - self.points.is_empty() - } -} - -impl Default for Path { - fn default() -> Self { - Self::new() - } -} - -/// Timing breakdown for a single observation (all times in microseconds). -#[derive(Debug, Clone, Default)] -pub struct TimingBreakdown { - /// Time spent on ICP matching (µs). - pub icp_matching_us: u64, - /// Time spent on line extraction (µs). - pub line_extraction_us: u64, - /// Time spent on corner detection (µs). - pub corner_detection_us: u64, - /// Time spent on line association (µs). - pub association_us: u64, - /// Time spent on line merging (µs). - pub merging_us: u64, - /// Time spent finding new features (µs). - pub new_features_us: u64, - /// Time spent on loop closure detection (µs). - pub loop_closure_us: u64, - /// Total observation time (µs). - pub total_us: u64, -} - -impl TimingBreakdown { - /// Create a new timing breakdown with all zeros. - pub fn new() -> Self { - Self::default() - } - - /// Get total feature extraction time (line + corner). - pub fn extraction_total_us(&self) -> u64 { - self.line_extraction_us + self.corner_detection_us - } - - /// Get total integration time (association + merging + new features). - pub fn integration_total_us(&self) -> u64 { - self.association_us + self.merging_us + self.new_features_us - } -} - -/// Result of observing a scan. -#[derive(Debug, Clone)] +/// Result of processing sensor observations +#[derive(Clone, Debug, Default)] pub struct ObserveResult { - /// Localized pose in world frame. - pub pose: Pose2D, - /// Match confidence (0.0 = used odometry, 1.0 = excellent match). - pub confidence: f32, - /// Number of features extracted from scan. - pub features_extracted: usize, - /// Number of new features added to map. - pub features_added: usize, - /// Number of features merged with existing map features. - pub features_merged: usize, - /// Number of ICP iterations performed (0 if no matching was done). - pub icp_iterations: usize, - /// Whether the ICP algorithm converged. - pub icp_converged: bool, - /// Whether a loop closure was detected. - pub loop_closure_detected: bool, - /// Timing breakdown for this observation. - pub timing: TimingBreakdown, + /// Total cells updated + pub cells_updated: usize, + /// Cells newly marked as Floor + pub cells_floor: usize, + /// Cells marked as Wall (lidar hit) + pub cells_wall: usize, + /// Cells marked as Cliff + pub cells_cliff: usize, + /// Cells marked as Bump (invisible obstacle) + pub cells_bump: usize, + /// Frontiers changed? + pub frontiers_changed: bool, } impl ObserveResult { - /// Create a result with just pose and confidence. - pub fn from_pose(pose: Pose2D, confidence: f32) -> Self { + /// Merge another result into this one + pub fn merge(&mut self, other: &ObserveResult) { + self.cells_updated += other.cells_updated; + self.cells_floor += other.cells_floor; + self.cells_wall += other.cells_wall; + self.cells_cliff += other.cells_cliff; + self.cells_bump += other.cells_bump; + self.frontiers_changed |= other.frontiers_changed; + } +} + +/// Map coverage statistics (cell type breakdown) +#[derive(Clone, Debug, Default)] +pub struct MapCoverageStats { + /// Total known cells (Floor + Wall + Cliff + Bump) + pub known_cells: usize, + /// Floor cells (traversable) + pub floor_cells: usize, + /// Wall cells + pub wall_cells: usize, + /// Cliff cells + pub cliff_cells: usize, + /// Bump cells (invisible obstacles) + pub bump_cells: usize, + /// Unknown cells still in grid + pub unknown_cells: usize, + /// Number of active frontiers + pub frontier_count: usize, + /// Estimated explored area (m²) + pub explored_area_m2: f32, +} + +impl From for MapCoverageStats { + fn from(counts: CellCounts) -> Self { Self { - pose, - confidence, - features_extracted: 0, - features_added: 0, - features_merged: 0, - icp_iterations: 0, - icp_converged: false, - loop_closure_detected: false, - timing: TimingBreakdown::default(), + known_cells: counts.known(), + floor_cells: counts.floor, + wall_cells: counts.wall, + cliff_cells: counts.cliff, + bump_cells: counts.bump, + unknown_cells: counts.unknown, + frontier_count: 0, + explored_area_m2: 0.0, } } } -/// High-level map interface. +/// The main occupancy grid map /// -/// Encapsulates localization, mapping, and query algorithms. -/// All coordinates in meters, all angles in radians. -/// -/// # Usage -/// -/// ```rust,ignore -/// use vastu_map::{Map, core::{PointCloud2D, Pose2D, Point2D}}; -/// -/// fn robot_loop(map: &mut M, get_scan: impl Fn() -> PointCloud2D) { -/// let goal = Point2D::new(5.0, 3.0); -/// -/// loop { -/// let scan = get_scan(); -/// let odom = Pose2D::identity(); // Get from odometry -/// -/// // Observe and localize -/// let result = map.observe(&scan, odom); -/// println!("Position: ({:.2}, {:.2}), confidence: {:.2}", -/// result.pose.x, result.pose.y, result.confidence); -/// -/// // Plan path to goal -/// if let Some(path) = map.get_path(result.pose.position(), goal) { -/// println!("Path found: {} waypoints, {:.2}m", path.points.len(), path.length); -/// } -/// } -/// } -/// ``` -pub trait Map: Send + Sync { - /// Feed sensor observation and get localized pose. +/// This is the primary type for interacting with the map. +pub struct OccupancyGridMap { + /// Grid storage + storage: GridStorage, + /// Configuration + config: MapConfig, +} + +impl OccupancyGridMap { + /// Create a new occupancy grid map + pub fn new(config: MapConfig) -> Self { + let origin = config.grid.effective_origin(); + let storage = GridStorage::new( + config.grid.initial_width, + config.grid.initial_height, + config.grid.resolution, + origin, + ); + + Self { storage, config } + } + + /// Get the grid storage + pub fn storage(&self) -> &GridStorage { + &self.storage + } + + /// Get mutable grid storage + pub fn storage_mut(&mut self) -> &mut GridStorage { + &mut self.storage + } + + /// Get the configuration + pub fn config(&self) -> &MapConfig { + &self.config + } + + /// Grid resolution in meters per cell + pub fn resolution(&self) -> f32 { + self.storage.resolution() + } + + /// Grid dimensions (width, height) in cells + pub fn dimensions(&self) -> (usize, usize) { + (self.storage.width(), self.storage.height()) + } + + /// World bounds: (min_point, max_point) + pub fn bounds(&self) -> (WorldPoint, WorldPoint) { + self.storage.bounds() + } + + /// Convert world coordinates to grid coordinates + pub fn world_to_grid(&self, point: WorldPoint) -> GridCoord { + self.storage.world_to_grid(point) + } + + /// Convert grid coordinates to world coordinates (cell center) + pub fn grid_to_world(&self, coord: GridCoord) -> WorldPoint { + self.storage.grid_to_world(coord) + } + + /// Get cell at world coordinates + pub fn get_cell(&self, point: WorldPoint) -> Cell { + self.storage.get_world(point).unwrap_or_default() + } + + /// Get cell at grid coordinates + pub fn get_cell_grid(&self, coord: GridCoord) -> Cell { + self.storage.get(coord).unwrap_or_default() + } + + /// Get cell type at world coordinates + pub fn get_type(&self, point: WorldPoint) -> CellType { + self.storage.get_type_world(point) + } + + /// Is this point traversable? + pub fn is_traversable(&self, point: WorldPoint) -> bool { + self.get_type(point).is_traversable() + } + + /// Is this point known (not Unknown)? + pub fn is_known(&self, point: WorldPoint) -> bool { + self.get_type(point).is_known() + } + + /// Get map coverage statistics + pub fn coverage_stats(&self) -> MapCoverageStats { + let counts = self.storage.count_by_type(); + let resolution = self.resolution(); + let cell_area = resolution * resolution; + + let mut stats = MapCoverageStats::from(counts); + stats.explored_area_m2 = counts.known() as f32 * cell_area; + stats + } + + /// Clear all cells to Unknown + pub fn clear(&mut self) { + self.storage.clear(); + } + + // ========================================================================= + // SENSOR OBSERVATION METHODS + // ========================================================================= + + /// Process a complete sensor observation. /// - /// This is the main SLAM function. It: - /// 1. Extracts features from the scan - /// 2. Matches features against the map (localization) - /// 3. Integrates new/updated features (mapping) + /// This is the main entry point for updating the map with sensor data. + /// It processes lidar, cliff, and bumper sensors in the correct order + /// (priority: bumper > cliff > lidar). + pub fn observe(&mut self, observation: &SensorObservation) -> ObserveResult { + let mut result = ObserveResult::default(); + + // Process lidar first (lowest priority) + if let Some(ref scan) = observation.lidar { + let lidar_result = self.observe_lidar(scan, observation.pose); + result.merge(&lidar_result); + } + + // Process cliff sensors (higher priority than lidar) + if observation.cliffs.any_triggered() { + let cliff_result = self.observe_cliff(&observation.cliffs, observation.pose); + result.merge(&cliff_result); + } + + // Process bumper sensors (highest priority) + if observation.bumpers.any_triggered() { + let bumper_result = self.observe_bumper(&observation.bumpers, observation.pose); + result.merge(&bumper_result); + } + + result + } + + /// Update the map with a lidar scan. + /// + /// For each valid ray in the scan: + /// - Marks all cells along the ray as Floor (free space) + /// - Marks the endpoint cell as Wall (if range < max_range) /// /// # Arguments - /// * `scan` - Point cloud in robot frame - /// * `odometry` - Odometry estimate (used as initial guess and fallback) + /// * `scan` - The lidar scan data + /// * `pose` - Robot pose when the scan was taken /// /// # Returns - /// Observation result including localized pose and confidence. - /// On match failure, returns odometry pose with low confidence. - fn observe(&mut self, scan: &PointCloud2D, odometry: Pose2D) -> ObserveResult; + /// Statistics about the cells that were updated + pub fn observe_lidar(&mut self, scan: &LidarScan, pose: Pose2D) -> ObserveResult { + grid::lidar_update::update_from_lidar( + &mut self.storage, + scan, + pose, + &self.config.sensor, + &self.config.grid, + ) + } - /// Cast a ray and return distance to first obstacle. + /// Update the map with a lidar scan, using scan-to-map matching. + /// + /// This method first aligns the scan to the current map using + /// correlative matching, then integrates the scan at the refined pose. + /// This helps correct for encoder drift. /// /// # Arguments - /// * `from` - Ray origin in world frame - /// * `direction` - Ray direction (will be normalized internally) - /// * `max_range` - Maximum ray distance + /// * `scan` - The lidar scan data + /// * `encoder_pose` - Initial pose estimate from encoders + /// * `matcher_config` - Configuration for the scan matcher /// /// # Returns - /// Distance to first intersection, or `max_range` if no intersection. - fn raycast(&self, from: Point2D, direction: Point2D, max_range: f32) -> f32; + /// Tuple of (observe_result, refined_pose, match_result) + pub fn observe_lidar_with_matching( + &mut self, + scan: &LidarScan, + encoder_pose: Pose2D, + matcher_config: &slam::CorrelativeMatcherConfig, + ) -> (ObserveResult, Pose2D, slam::ScanMatchResult) { + let matcher = slam::CorrelativeMatcher::new(matcher_config.clone()); + let match_result = matcher.match_scan(scan, encoder_pose, &self.storage); - /// Query point occupancy. - /// - /// Uses contour-based occupancy (like font outlines): - /// - Inside navigable contour → Free - /// - On or outside boundary → Occupied - /// - Not yet mapped → Unknown - fn query(&self, point: Point2D) -> Occupancy; + // Use matched pose if converged, otherwise fall back to encoder pose + let final_pose = if match_result.converged && match_result.score >= matcher_config.min_score + { + match_result.pose + } else { + encoder_pose + }; - /// Get exploration frontiers (unconnected line endpoints). - /// - /// Frontiers are line endpoints that don't connect to other lines, - /// indicating unexplored areas. - fn frontiers(&self) -> Vec; + // Integrate scan at the refined pose + let observe_result = self.observe_lidar(scan, final_pose); - /// Plan path between two points. - /// - /// Uses visibility graph path planning to find the shortest path - /// around obstacles. - /// - /// # Returns - /// Path if one exists, None otherwise. - fn get_path(&self, from: Point2D, to: Point2D) -> Option; + (observe_result, final_pose, match_result) + } - /// Plan path between two points, excluding specific CBVG goal nodes. + /// Update the map with cliff sensor readings. /// - /// This is used for finding alternative approach angles after a collision. - /// When navigation to a frontier causes a collision, the goal node that - /// led to the collision can be excluded, forcing the planner to find a - /// different approach. + /// For each triggered cliff sensor, marks the cell at the sensor + /// position as Cliff. Cliff has higher priority than Floor/Wall. /// /// # Arguments - /// * `from` - Start position - /// * `to` - Goal position - /// * `excluded_goal_nodes` - CBVG node indices to exclude when selecting the goal node + /// * `cliffs` - Cliff sensor states + /// * `pose` - Robot pose when sensors were read /// /// # Returns - /// Tuple of (Path, goal_node_index) if path exists, None otherwise. - fn get_path_excluding_nodes( - &self, - from: Point2D, - to: Point2D, - excluded_goal_nodes: &[usize], - ) -> Option<(Path, usize)>; + /// Statistics about the cells that were updated + pub fn observe_cliff(&mut self, cliffs: &CliffSensors, pose: Pose2D) -> ObserveResult { + grid::cliff_update::update_from_cliff(&mut self.storage, cliffs, pose) + } - /// Find the nearest CBVG node to a target point. + /// Update the map with bumper collision readings. /// - /// This is useful for exploration to find reachable navigation targets - /// near frontiers. CBVG nodes are guaranteed to be in visible, reachable - /// areas with proper wall clearance. + /// For each triggered bumper, estimates the collision position and + /// marks those cells as Bump. Bump has the highest priority. /// /// # Arguments - /// * `target` - The point to find the nearest node to - /// * `max_distance` - Maximum distance to search + /// * `bumpers` - Bumper sensor states + /// * `pose` - Robot pose when collision occurred /// /// # Returns - /// The position of the nearest CBVG node, or None if no node is within range. - fn nearest_cbvg_node(&self, target: Point2D, max_distance: f32) -> Option; + /// Statistics about the cells that were updated + pub fn observe_bumper(&mut self, bumpers: &BumperSensors, pose: Pose2D) -> ObserveResult { + grid::bumper_update::update_from_bumper( + &mut self.storage, + bumpers, + pose, + self.config.sensor.robot_radius, + ) + } - /// Check if a straight-line path between two points is clear. + /// Mark cells as swept by the robot at its current position. /// - /// # Arguments - /// * `from` - Start position - /// * `to` - End position - /// - /// # Returns - /// True if no obstacles block the straight-line path. - fn is_path_clear(&self, from: Point2D, to: Point2D) -> bool; + /// This is used for coverage tracking during cleaning. + pub fn mark_swept(&mut self, pose: Pose2D, radius: f32) { + let center = pose.position(); + let resolution = self.storage.resolution(); + let cells_radius = (radius / resolution).ceil() as i32; + let center_coord = self.storage.world_to_grid(center); - /// Get current map bounds. - /// - /// Returns the axis-aligned bounding box containing all map features. - /// The bounds grow dynamically as the robot explores. - fn bounds(&self) -> Bounds; + for dy in -cells_radius..=cells_radius { + for dx in -cells_radius..=cells_radius { + let coord = center_coord + GridCoord::new(dx, dy); - /// Clear all map data. - fn clear(&mut self); + if !self.storage.is_valid_coord(coord) { + continue; + } + + // Check if within circular radius + let cell_center = self.storage.grid_to_world(coord); + if cell_center.distance(¢er) <= radius + && let Some(cell) = self.storage.get_mut(coord) + { + *cell.swept = 1; + } + } + } + } + + /// Reset swept status for all cells. + pub fn reset_swept(&mut self) { + for (_, cell) in self.storage.iter_mut() { + *cell.swept = 0; + } + } } #[cfg(test)] @@ -448,28 +428,50 @@ mod tests { use super::*; #[test] - fn test_occupancy_debug() { - assert_eq!(format!("{:?}", Occupancy::Free), "Free"); - assert_eq!(format!("{:?}", Occupancy::Occupied), "Occupied"); - assert_eq!(format!("{:?}", Occupancy::Unknown), "Unknown"); + fn test_map_creation() { + let config = MapConfig::default(); + let map = OccupancyGridMap::new(config); + + assert_eq!(map.resolution(), 0.025); + assert_eq!(map.dimensions(), (800, 800)); } #[test] - fn test_path_new() { - let path = Path::new(); - assert!(path.is_empty()); - assert_eq!(path.length, 0.0); + fn test_coordinate_conversion() { + let config = MapConfig::default(); + let map = OccupancyGridMap::new(config); + + // Origin should map to a grid coordinate near center + let origin_coord = map.world_to_grid(WorldPoint::ZERO); + assert!(origin_coord.x > 0); + assert!(origin_coord.y > 0); + + // Round-trip should be close to original + let back_to_world = map.grid_to_world(origin_coord); + assert!((back_to_world.x - 0.0).abs() < map.resolution()); + assert!((back_to_world.y - 0.0).abs() < map.resolution()); } #[test] - fn test_observe_result() { - let result = ObserveResult::from_pose(Pose2D::new(1.0, 2.0, 0.5), 0.8); - assert_eq!(result.pose.x, 1.0); - assert_eq!(result.pose.y, 2.0); - assert_eq!(result.confidence, 0.8); - assert_eq!(result.features_extracted, 0); - assert_eq!(result.icp_iterations, 0); - assert!(!result.icp_converged); - assert!(!result.loop_closure_detected); + fn test_cell_access() { + let config = MapConfig::default(); + let map = OccupancyGridMap::new(config); + + // All cells should initially be Unknown + let cell = map.get_cell(WorldPoint::ZERO); + assert_eq!(cell.cell_type, CellType::Unknown); + assert!(!map.is_traversable(WorldPoint::ZERO)); + assert!(!map.is_known(WorldPoint::ZERO)); + } + + #[test] + fn test_coverage_stats() { + let config = MapConfig::default(); + let map = OccupancyGridMap::new(config); + + let stats = map.coverage_stats(); + assert_eq!(stats.known_cells, 0); + assert_eq!(stats.unknown_cells, 800 * 800); + assert_eq!(stats.explored_area_m2, 0.0); } } diff --git a/vastu-map/src/loop_closure/README.md b/vastu-map/src/loop_closure/README.md deleted file mode 100644 index cefc349..0000000 --- a/vastu-map/src/loop_closure/README.md +++ /dev/null @@ -1,175 +0,0 @@ -# Loop Closure Module - -Keyframe-based loop closure detection using shape context descriptors. - -## Overview - -Loop closure detection identifies when the robot revisits a previously mapped area. This enables: - -- **Drift correction**: Correct accumulated odometry errors -- **Global consistency**: Maintain accurate global map -- **Pose graph optimization**: Provide constraints for optimization - -## Components - -| Type | Description | -|------|-------------| -| `LoopClosureDetector` | Main detector with keyframe storage | -| `LoopClosureConfig` | Configuration parameters | -| `Keyframe` | Snapshot of map state at a pose | -| `LoopClosure` | Detected loop closure constraint | -| `LoopDetector` | Trait for extensibility | - -## Pipeline - -``` -New Observation - │ - ▼ -┌───────────────────┐ -│ Should Create │ ← Check distance from last keyframe -│ Keyframe? │ -└────────┬──────────┘ - │ Yes - ▼ -┌───────────────────┐ -│ Compute Scan │ ← Shape context descriptor -│ Descriptor │ -└────────┬──────────┘ - │ - ▼ -┌───────────────────┐ -│ Find Candidates │ ← Descriptor similarity + min travel distance -└────────┬──────────┘ - │ - ▼ -┌───────────────────┐ -│ Verify with ICP │ ← Point-to-line matching -└────────┬──────────┘ - │ - ▼ - Loop Closure - (if verified) -``` - -## Usage - -```rust -use vastu_map::loop_closure::{LoopClosureDetector, LoopClosureConfig}; -use vastu_map::core::Pose2D; - -let config = LoopClosureConfig::default(); -let mut detector = LoopClosureDetector::new(config); - -// Process observations -for (pose, lines, corners, points) in observations { - if let Some(closure) = detector.process(pose, &lines, &corners, &points) { - println!("Loop closure: {} -> {}", - closure.from_keyframe, closure.to_keyframe); - println!("Confidence: {:.2}", closure.confidence); - - // Use closure for pose graph optimization - optimize_pose_graph(&closure); - } -} -``` - -## Configuration - -```rust -let config = LoopClosureConfig { - // Keyframe creation - keyframe_interval: 2.0, // Distance between keyframes (m) - min_lines_for_keyframe: 3, // Min lines required - min_corners_for_keyframe: 2, // Min corners required - - // Candidate selection - min_travel_distance: 5.0, // Min travel before loop check (m) - max_descriptor_distance: 0.5, // Max descriptor difference - max_candidates_to_verify: 3, // Top candidates to verify - - // Verification - min_verification_confidence: 0.6, // Min ICP confidence - descriptor_radius: 2.0, // Radius for descriptor (m) - - // ICP for verification - icp_config: IcpConfig::default(), -}; -``` - -## Keyframe - -Stores a snapshot for later matching: - -```rust -pub struct Keyframe { - pub id: usize, // Unique identifier - pub pose: Pose2D, // Robot pose when created - pub descriptor: ScanDescriptor, // Shape context descriptor - pub lines: Vec, // Line features - pub corners: Vec, // Corner features - pub points: Vec, // Raw points (for ICP) - pub distance_traveled: f32, // Total travel distance -} -``` - -## Loop Closure Result - -```rust -pub struct LoopClosure { - pub from_keyframe: usize, // Earlier keyframe ID - pub to_keyframe: usize, // Current keyframe ID - pub relative_pose: Pose2D, // Relative transform - pub covariance: [[f32; 3]; 3], // Pose covariance - pub confidence: f32, // Match confidence (0-1) - pub descriptor_distance: f32, // Descriptor similarity -} -``` - -## Descriptor Matching - -Two-stage matching for efficiency: - -1. **Quick Signature**: 64-bit hash for fast pre-filtering -2. **Full Descriptor**: Shape context distance for final matching - -```rust -// Quick pre-filter -let sig_diff = (desc_a.quick_signature() ^ desc_b.quick_signature()).count_ones(); -if sig_diff > 16 { - return false; // Too different -} - -// Full comparison -let distance = desc_a.distance(&desc_b); -if distance <= max_descriptor_distance { - // Candidate for ICP verification -} -``` - -## Extensibility - -Implement `LoopDetector` trait for custom algorithms: - -```rust -pub trait LoopDetector: Send + Sync { - fn detect( - &mut self, - pose: Pose2D, - lines: &[Line2D], - corners: &[Corner2D], - points: &[Point2D], - ) -> Option; - - fn clear(&mut self); - fn can_detect(&self) -> bool; - fn keyframe_count(&self) -> usize; -} -``` - -## Best Practices - -1. **Tune min_travel_distance** to avoid false positives in revisited areas -2. **Verify with ICP** - descriptor matching alone is not sufficient -3. **Use loop closures** for pose graph optimization, not direct correction -4. **Monitor keyframe count** to manage memory usage diff --git a/vastu-map/src/loop_closure/config.rs b/vastu-map/src/loop_closure/config.rs deleted file mode 100644 index 79e6ec8..0000000 --- a/vastu-map/src/loop_closure/config.rs +++ /dev/null @@ -1,76 +0,0 @@ -//! Configuration for loop closure detection. - -use serde::{Deserialize, Serialize}; - -use crate::matching::IcpConfig; - -/// Configuration for loop closure detection. -#[derive(Clone, Debug, Serialize, Deserialize)] -pub struct LoopClosureConfig { - /// Minimum travel distance before considering loop closure (meters). - /// Prevents detecting loops immediately after keyframe creation. - /// Default: 5.0m - pub min_travel_distance: f32, - - /// Maximum descriptor distance for candidate retrieval. - /// Lower values are more selective but may miss valid loops. - /// Default: 0.5 - pub max_descriptor_distance: f32, - - /// Minimum ICP confidence to accept a loop closure. - /// Default: 0.6 - pub min_verification_confidence: f32, - - /// Distance between keyframes (meters). - /// Default: 2.0m - pub keyframe_interval: f32, - - /// Radius for computing corner descriptors (meters). - /// Default: 2.0m - pub descriptor_radius: f32, - - /// Maximum number of candidates to verify with ICP per keyframe. - /// Default: 3 - pub max_candidates_to_verify: usize, - - /// Minimum number of lines required for a valid keyframe. - /// Default: 3 - pub min_lines_for_keyframe: usize, - - /// Minimum number of corners required for a valid keyframe. - /// Default: 2 - pub min_corners_for_keyframe: usize, - - /// ICP configuration for verification. - pub icp_config: IcpConfig, -} - -impl Default for LoopClosureConfig { - fn default() -> Self { - Self { - min_travel_distance: 5.0, - max_descriptor_distance: 0.5, - min_verification_confidence: 0.6, - keyframe_interval: 2.0, - descriptor_radius: 2.0, - max_candidates_to_verify: 3, - min_lines_for_keyframe: 3, - min_corners_for_keyframe: 2, - icp_config: IcpConfig::default(), - } - } -} - -#[cfg(test)] -mod tests { - use super::*; - - #[test] - fn test_config_default() { - let config = LoopClosureConfig::default(); - assert_eq!(config.min_travel_distance, 5.0); - assert_eq!(config.max_descriptor_distance, 0.5); - assert_eq!(config.min_verification_confidence, 0.6); - assert_eq!(config.keyframe_interval, 2.0); - } -} diff --git a/vastu-map/src/loop_closure/detector.rs b/vastu-map/src/loop_closure/detector.rs deleted file mode 100644 index fe55bc5..0000000 --- a/vastu-map/src/loop_closure/detector.rs +++ /dev/null @@ -1,502 +0,0 @@ -//! Loop closure detector implementation. - -use crate::core::{Point2D, Pose2D}; -use crate::features::{Corner2D, Line2D, ScanDescriptor}; -use crate::matching::match_scan; - -use super::config::LoopClosureConfig; -use super::types::{Keyframe, LoopClosure}; - -/// Loop closure detector using keyframe-based matching. -pub struct LoopClosureDetector { - /// Stored keyframes. - keyframes: Vec, - - /// Configuration. - config: LoopClosureConfig, - - /// Last pose where a keyframe was created. - last_keyframe_pose: Option, - - /// Total distance traveled. - total_distance: f32, - - /// Next keyframe ID. - next_keyframe_id: usize, -} - -impl LoopClosureDetector { - /// Create a new loop closure detector. - pub fn new(config: LoopClosureConfig) -> Self { - Self { - keyframes: Vec::new(), - config, - last_keyframe_pose: None, - total_distance: 0.0, - next_keyframe_id: 0, - } - } - - /// Get the configuration. - pub fn config(&self) -> &LoopClosureConfig { - &self.config - } - - /// Get all keyframes. - pub fn keyframes(&self) -> &[Keyframe] { - &self.keyframes - } - - /// Get the number of keyframes. - pub fn keyframe_count(&self) -> usize { - self.keyframes.len() - } - - /// Get the total distance traveled. - pub fn total_distance(&self) -> f32 { - self.total_distance - } - - /// Clear all keyframes and reset state. - pub fn clear(&mut self) { - self.keyframes.clear(); - self.last_keyframe_pose = None; - self.total_distance = 0.0; - self.next_keyframe_id = 0; - } - - /// Process a new observation and check for loop closures. - /// - /// # Arguments - /// * `current_pose` - Current robot pose in world frame - /// * `lines` - Extracted lines in world frame - /// * `corners` - Extracted corners in world frame - /// * `points` - Scan points in robot frame (for ICP verification) - /// - /// # Returns - /// A loop closure if one is detected, None otherwise. - pub fn process( - &mut self, - current_pose: Pose2D, - lines: &[Line2D], - corners: &[Corner2D], - points: &[Point2D], - ) -> Option { - // Update distance traveled - if let Some(last_pose) = self.last_keyframe_pose { - let dx = current_pose.x - last_pose.x; - let dy = current_pose.y - last_pose.y; - self.total_distance += (dx * dx + dy * dy).sqrt(); - } - - // Check if we need to create a keyframe - let should_create_keyframe = self.should_create_keyframe(current_pose, lines, corners); - - let mut loop_closure = None; - - if should_create_keyframe { - // First check for loop closure candidates before adding keyframe - loop_closure = self.find_loop_closure(current_pose, lines, corners, points); - - // Create and store keyframe - let keyframe = Keyframe::new( - self.next_keyframe_id, - current_pose, - lines.to_vec(), - corners.to_vec(), - points.to_vec(), - self.total_distance, - self.config.descriptor_radius, - ); - - self.keyframes.push(keyframe); - self.next_keyframe_id += 1; - self.last_keyframe_pose = Some(current_pose); - } - - loop_closure - } - - /// Check if a new keyframe should be created. - fn should_create_keyframe( - &self, - current_pose: Pose2D, - lines: &[Line2D], - corners: &[Corner2D], - ) -> bool { - // Need minimum features - if lines.len() < self.config.min_lines_for_keyframe { - return false; - } - if corners.len() < self.config.min_corners_for_keyframe { - return false; - } - - // First keyframe - if self.last_keyframe_pose.is_none() { - return true; - } - - // Check distance since last keyframe - let last_pose = self.last_keyframe_pose.unwrap(); - let dx = current_pose.x - last_pose.x; - let dy = current_pose.y - last_pose.y; - let distance = (dx * dx + dy * dy).sqrt(); - - distance >= self.config.keyframe_interval - } - - /// Find loop closure candidates and verify with ICP. - fn find_loop_closure( - &self, - current_pose: Pose2D, - lines: &[Line2D], - corners: &[Corner2D], - points: &[Point2D], - ) -> Option { - // Need enough keyframes for loop closure - if self.keyframes.is_empty() { - return None; - } - - // Compute descriptor for current observation - let current_descriptor = - ScanDescriptor::compute(lines, corners, self.config.descriptor_radius); - - if !current_descriptor.is_valid_for_matching() { - return None; - } - - // Find candidates using descriptor distance - let mut candidates: Vec<(usize, f32)> = self - .keyframes - .iter() - .enumerate() - .filter_map(|(idx, kf)| { - // Skip recent keyframes (min travel distance) - let travel_since_kf = self.total_distance - kf.distance_traveled; - if travel_since_kf < self.config.min_travel_distance { - return None; - } - - // Check descriptor validity - if !kf.descriptor.is_valid_for_matching() { - return None; - } - - // Quick signature pre-filter - let sig_diff = (current_descriptor.quick_signature() - ^ kf.descriptor.quick_signature()) - .count_ones(); - if sig_diff > 16 { - return None; // Too different - } - - // Full descriptor distance - let dist = current_descriptor.distance(&kf.descriptor); - if dist <= self.config.max_descriptor_distance { - Some((idx, dist)) - } else { - None - } - }) - .collect(); - - // Sort by descriptor distance (best first) - candidates.sort_by(|a, b| a.1.partial_cmp(&b.1).unwrap_or(std::cmp::Ordering::Equal)); - - // Verify top candidates with ICP - for (kf_idx, descriptor_dist) in - candidates.iter().take(self.config.max_candidates_to_verify) - { - let keyframe = &self.keyframes[*kf_idx]; - - // Compute initial guess: relative pose from keyframe to current - let initial_guess = keyframe.pose.inverse().compose(current_pose); - - // Transform current points to keyframe frame - let transformed_points: Vec = points - .iter() - .map(|p| { - // Transform from current robot frame to world, then to keyframe frame - let world = current_pose.transform_point(*p); - keyframe.pose.inverse().transform_point(world) - }) - .collect(); - - // Run ICP verification - let match_result = match_scan(&transformed_points, &keyframe.lines, initial_guess); - - if match_result.converged - && match_result.confidence >= self.config.min_verification_confidence - { - // Found a valid loop closure - let relative_pose = keyframe.pose.inverse().compose(current_pose); - - return Some(LoopClosure { - from_keyframe: keyframe.id, - to_keyframe: self.next_keyframe_id, - relative_pose, - covariance: match_result.covariance, - confidence: match_result.confidence, - descriptor_distance: *descriptor_dist, - }); - } - } - - None - } - - /// Force add a keyframe at the current position. - /// Useful for initialization or when loop closure should be checked - /// regardless of distance traveled. - pub fn force_keyframe( - &mut self, - pose: Pose2D, - lines: &[Line2D], - corners: &[Corner2D], - points: &[Point2D], - ) -> usize { - let keyframe = Keyframe::new( - self.next_keyframe_id, - pose, - lines.to_vec(), - corners.to_vec(), - points.to_vec(), - self.total_distance, - self.config.descriptor_radius, - ); - - let id = self.next_keyframe_id; - self.keyframes.push(keyframe); - self.next_keyframe_id += 1; - self.last_keyframe_pose = Some(pose); - - id - } - - /// Get a keyframe by ID. - pub fn get_keyframe(&self, id: usize) -> Option<&Keyframe> { - self.keyframes.iter().find(|kf| kf.id == id) - } - - /// Check if loop closure detection is possible. - /// Returns true if there are enough keyframes and sufficient travel distance. - pub fn can_detect_loop(&self) -> bool { - if self.keyframes.is_empty() { - return false; - } - - // Check if we've traveled enough since any keyframe - self.keyframes.iter().any(|kf| { - let travel_since = self.total_distance - kf.distance_traveled; - travel_since >= self.config.min_travel_distance - }) - } -} - -impl Default for LoopClosureDetector { - fn default() -> Self { - Self::new(LoopClosureConfig::default()) - } -} - -#[cfg(test)] -mod tests { - use super::*; - - fn make_simple_lines() -> Vec { - vec![ - Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(2.0, 0.0)), - Line2D::new(Point2D::new(2.0, 0.0), Point2D::new(2.0, 2.0)), - Line2D::new(Point2D::new(2.0, 2.0), Point2D::new(0.0, 2.0)), - Line2D::new(Point2D::new(0.0, 2.0), Point2D::new(0.0, 0.0)), - ] - } - - fn make_simple_corners() -> Vec { - vec![ - Corner2D::new(Point2D::new(0.0, 0.0), 3, 0, std::f32::consts::FRAC_PI_2), - Corner2D::new(Point2D::new(2.0, 0.0), 0, 1, std::f32::consts::FRAC_PI_2), - Corner2D::new(Point2D::new(2.0, 2.0), 1, 2, std::f32::consts::FRAC_PI_2), - Corner2D::new(Point2D::new(0.0, 2.0), 2, 3, std::f32::consts::FRAC_PI_2), - ] - } - - fn make_simple_points() -> Vec { - let mut points = Vec::new(); - // Wall points - for i in 0..20 { - let t = i as f32 / 19.0 * 2.0; - points.push(Point2D::new(t, 0.0)); - points.push(Point2D::new(t, 2.0)); - points.push(Point2D::new(0.0, t)); - points.push(Point2D::new(2.0, t)); - } - points - } - - #[test] - fn test_detector_new() { - let detector = LoopClosureDetector::default(); - assert_eq!(detector.keyframe_count(), 0); - assert_eq!(detector.total_distance(), 0.0); - assert!(!detector.can_detect_loop()); - } - - #[test] - fn test_first_keyframe_created() { - let mut detector = LoopClosureDetector::default(); - let lines = make_simple_lines(); - let corners = make_simple_corners(); - let points = make_simple_points(); - - let result = detector.process(Pose2D::identity(), &lines, &corners, &points); - - assert!(result.is_none()); // No loop on first observation - assert_eq!(detector.keyframe_count(), 1); - } - - #[test] - fn test_keyframe_interval() { - let config = LoopClosureConfig { - keyframe_interval: 1.0, - ..Default::default() - }; - let mut detector = LoopClosureDetector::new(config); - let lines = make_simple_lines(); - let corners = make_simple_corners(); - let points = make_simple_points(); - - // First keyframe - detector.process(Pose2D::identity(), &lines, &corners, &points); - assert_eq!(detector.keyframe_count(), 1); - - // Too close - no new keyframe - detector.process(Pose2D::new(0.5, 0.0, 0.0), &lines, &corners, &points); - assert_eq!(detector.keyframe_count(), 1); - - // Far enough - new keyframe - detector.process(Pose2D::new(1.5, 0.0, 0.0), &lines, &corners, &points); - assert_eq!(detector.keyframe_count(), 2); - } - - #[test] - fn test_insufficient_features_no_keyframe() { - let mut detector = LoopClosureDetector::default(); - let lines = vec![Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(1.0, 0.0))]; // Only 1 line - let corners = vec![]; // No corners - let points = make_simple_points(); - - detector.process(Pose2D::identity(), &lines, &corners, &points); - - assert_eq!(detector.keyframe_count(), 0); // Not enough features - } - - #[test] - fn test_force_keyframe() { - let mut detector = LoopClosureDetector::default(); - let lines = vec![Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(1.0, 0.0))]; - let corners = vec![]; - let points = make_simple_points(); - - let id = detector.force_keyframe(Pose2D::identity(), &lines, &corners, &points); - - assert_eq!(id, 0); - assert_eq!(detector.keyframe_count(), 1); - } - - #[test] - fn test_get_keyframe() { - let mut detector = LoopClosureDetector::default(); - let lines = make_simple_lines(); - let corners = make_simple_corners(); - let points = make_simple_points(); - - detector.force_keyframe(Pose2D::new(1.0, 2.0, 0.5), &lines, &corners, &points); - - let kf = detector.get_keyframe(0); - assert!(kf.is_some()); - assert_eq!(kf.unwrap().pose.x, 1.0); - assert_eq!(kf.unwrap().pose.y, 2.0); - - assert!(detector.get_keyframe(999).is_none()); - } - - #[test] - fn test_clear() { - let mut detector = LoopClosureDetector::default(); - let lines = make_simple_lines(); - let corners = make_simple_corners(); - let points = make_simple_points(); - - detector.force_keyframe(Pose2D::identity(), &lines, &corners, &points); - assert_eq!(detector.keyframe_count(), 1); - - detector.clear(); - assert_eq!(detector.keyframe_count(), 0); - assert_eq!(detector.total_distance(), 0.0); - } - - #[test] - fn test_can_detect_loop() { - let config = LoopClosureConfig { - min_travel_distance: 2.0, - keyframe_interval: 1.0, - ..Default::default() - }; - let mut detector = LoopClosureDetector::new(config); - let lines = make_simple_lines(); - let corners = make_simple_corners(); - let points = make_simple_points(); - - assert!(!detector.can_detect_loop()); - - // Add first keyframe - detector.process(Pose2D::identity(), &lines, &corners, &points); - assert!(!detector.can_detect_loop()); // Not traveled enough - - // Move far enough - detector.process(Pose2D::new(3.0, 0.0, 0.0), &lines, &corners, &points); - assert!(detector.can_detect_loop()); - } - - #[test] - fn test_total_distance_tracking() { - let config = LoopClosureConfig { - keyframe_interval: 1.0, - ..Default::default() - }; - let mut detector = LoopClosureDetector::new(config); - let lines = make_simple_lines(); - let corners = make_simple_corners(); - let points = make_simple_points(); - - detector.process(Pose2D::identity(), &lines, &corners, &points); - assert_eq!(detector.total_distance(), 0.0); - - // Move 1 meter - detector.process(Pose2D::new(1.0, 0.0, 0.0), &lines, &corners, &points); - assert!((detector.total_distance() - 1.0).abs() < 0.01); - - // Move 1 more meter diagonally - detector.process(Pose2D::new(2.0, 1.0, 0.0), &lines, &corners, &points); - let expected = 1.0 + (2.0_f32).sqrt(); - assert!((detector.total_distance() - expected).abs() < 0.01); - } - - #[test] - fn test_keyframe_descriptor() { - let mut detector = LoopClosureDetector::default(); - let lines = make_simple_lines(); - let corners = make_simple_corners(); - let points = make_simple_points(); - - detector.force_keyframe(Pose2D::identity(), &lines, &corners, &points); - - let kf = detector.get_keyframe(0).unwrap(); - assert!(kf.descriptor.is_valid_for_matching()); - assert_eq!(kf.descriptor.num_lines, 4); - assert_eq!(kf.descriptor.num_corners, 4); - } -} diff --git a/vastu-map/src/loop_closure/mod.rs b/vastu-map/src/loop_closure/mod.rs deleted file mode 100644 index 4a0db97..0000000 --- a/vastu-map/src/loop_closure/mod.rs +++ /dev/null @@ -1,48 +0,0 @@ -//! Loop Closure Detection for VectorMap SLAM. -//! -//! This module provides keyframe-based loop closure detection using -//! shape context descriptors. When the robot revisits a previously -//! mapped area, loop closures are detected and can be used to -//! correct accumulated drift. -//! -//! # Overview -//! -//! The loop closure pipeline: -//! 1. Store keyframes at regular intervals (every `keyframe_interval` meters) -//! 2. Compute scan descriptors for each keyframe -//! 3. When a new keyframe is added, compare against candidates -//! 4. Verify candidates with ICP matching -//! 5. Return loop closures for pose graph optimization -//! -//! # Example -//! -//! ```rust,ignore -//! use vastu_map::loop_closure::{LoopClosureDetector, LoopClosureConfig}; -//! use vastu_map::core::Pose2D; -//! -//! let config = LoopClosureConfig::default(); -//! let mut detector = LoopClosureDetector::new(config); -//! -//! // Process observations -//! if let Some(closure) = detector.process(current_pose, &lines, &corners, &points) { -//! println!("Loop closure detected: {} -> {}", closure.from_keyframe, closure.to_keyframe); -//! } -//! ``` -//! -//! # Module Structure -//! -//! - [`config`]: Configuration for loop closure detection -//! - [`types`]: Core types (Keyframe, LoopClosure) -//! - [`detector`]: LoopClosureDetector implementation -//! - [`traits`]: LoopDetector trait for custom implementations - -mod config; -mod detector; -mod traits; -mod types; - -// Re-export public API -pub use config::LoopClosureConfig; -pub use detector::LoopClosureDetector; -pub use traits::LoopDetector; -pub use types::{Keyframe, LoopClosure}; diff --git a/vastu-map/src/loop_closure/traits.rs b/vastu-map/src/loop_closure/traits.rs deleted file mode 100644 index 84ec908..0000000 --- a/vastu-map/src/loop_closure/traits.rs +++ /dev/null @@ -1,77 +0,0 @@ -//! Trait definitions for loop closure detection. - -use crate::core::{Point2D, Pose2D}; -use crate::features::{Corner2D, Line2D}; - -use super::detector::LoopClosureDetector; -use super::types::LoopClosure; - -/// Trait for loop closure detection algorithms. -/// -/// Implementations detect when the robot revisits a previously mapped area, -/// providing constraints for pose graph optimization. -/// -/// # Example -/// -/// ```rust,ignore -/// use vastu_map::loop_closure::{LoopDetector, LoopClosureDetector, LoopClosureConfig}; -/// use vastu_map::core::Pose2D; -/// -/// let mut detector = LoopClosureDetector::new(LoopClosureConfig::default()); -/// -/// if let Some(closure) = detector.detect(pose, &lines, &corners, &points) { -/// println!("Loop detected with confidence {}", closure.confidence); -/// } -/// ``` -pub trait LoopDetector: Send + Sync { - /// Process an observation and detect potential loop closures. - /// - /// # Arguments - /// * `pose` - Current robot pose in world frame - /// * `lines` - Extracted lines in world frame - /// * `corners` - Extracted corners in world frame - /// * `points` - Scan points in robot frame (for ICP verification) - /// - /// # Returns - /// A loop closure if one is detected, None otherwise. - fn detect( - &mut self, - pose: Pose2D, - lines: &[Line2D], - corners: &[Corner2D], - points: &[Point2D], - ) -> Option; - - /// Clear all stored state (keyframes, history, etc.). - fn clear(&mut self); - - /// Check if loop detection is currently possible. - fn can_detect(&self) -> bool; - - /// Get the number of stored keyframes. - fn keyframe_count(&self) -> usize; -} - -impl LoopDetector for LoopClosureDetector { - fn detect( - &mut self, - pose: Pose2D, - lines: &[Line2D], - corners: &[Corner2D], - points: &[Point2D], - ) -> Option { - self.process(pose, lines, corners, points) - } - - fn clear(&mut self) { - LoopClosureDetector::clear(self) - } - - fn can_detect(&self) -> bool { - self.can_detect_loop() - } - - fn keyframe_count(&self) -> usize { - self.keyframes().len() - } -} diff --git a/vastu-map/src/loop_closure/types.rs b/vastu-map/src/loop_closure/types.rs deleted file mode 100644 index 896e93d..0000000 --- a/vastu-map/src/loop_closure/types.rs +++ /dev/null @@ -1,97 +0,0 @@ -//! Core types for loop closure detection. - -use crate::core::{Point2D, Pose2D}; -use crate::features::{Corner2D, Line2D, ScanDescriptor}; - -/// A keyframe stores a snapshot of the map at a particular pose. -#[derive(Clone, Debug)] -pub struct Keyframe { - /// Unique keyframe identifier. - pub id: usize, - - /// Robot pose when keyframe was created. - pub pose: Pose2D, - - /// Scan descriptor for quick matching. - pub descriptor: ScanDescriptor, - - /// Line features in world frame. - pub lines: Vec, - - /// Corner features in world frame. - pub corners: Vec, - - /// Points in robot frame (for ICP verification). - pub points: Vec, - - /// Distance traveled when keyframe was created. - pub distance_traveled: f32, -} - -impl Keyframe { - /// Create a new keyframe. - pub fn new( - id: usize, - pose: Pose2D, - lines: Vec, - corners: Vec, - points: Vec, - distance_traveled: f32, - descriptor_radius: f32, - ) -> Self { - let descriptor = ScanDescriptor::compute(&lines, &corners, descriptor_radius); - Self { - id, - pose, - descriptor, - lines, - corners, - points, - distance_traveled, - } - } -} - -/// A detected loop closure constraint. -#[derive(Clone, Debug)] -pub struct LoopClosure { - /// Index of the earlier keyframe. - pub from_keyframe: usize, - - /// Index of the later keyframe (current). - pub to_keyframe: usize, - - /// Relative pose from `from_keyframe` to `to_keyframe`. - pub relative_pose: Pose2D, - - /// Covariance matrix of the relative pose estimate. - /// Format: [[σxx, σxy, σxθ], [σxy, σyy, σyθ], [σxθ, σyθ, σθθ]] - pub covariance: [[f32; 3]; 3], - - /// Confidence of the loop closure (0.0 to 1.0). - pub confidence: f32, - - /// Descriptor distance used for candidate retrieval. - pub descriptor_distance: f32, -} - -#[cfg(test)] -mod tests { - use super::*; - - #[test] - fn test_loop_closure_struct() { - let closure = LoopClosure { - from_keyframe: 0, - to_keyframe: 5, - relative_pose: Pose2D::new(1.0, 2.0, 0.5), - covariance: [[0.01, 0.0, 0.0], [0.0, 0.01, 0.0], [0.0, 0.0, 0.001]], - confidence: 0.85, - descriptor_distance: 0.3, - }; - - assert_eq!(closure.from_keyframe, 0); - assert_eq!(closure.to_keyframe, 5); - assert_eq!(closure.confidence, 0.85); - } -} diff --git a/vastu-map/src/matching/README.md b/vastu-map/src/matching/README.md deleted file mode 100644 index f3af32d..0000000 --- a/vastu-map/src/matching/README.md +++ /dev/null @@ -1,182 +0,0 @@ -# Matching Module - -Scan-to-map matching algorithms for robot localization. - -## Components - -| File | Description | -|------|-------------| -| `correspondence.rs` | Point-to-line correspondence types (AoS and SoA layouts) | -| `nearest_neighbor.rs` | Find correspondences by distance | -| `ransac.rs` | Robust pose estimation with outlier rejection | -| `gauss_newton.rs` | Nonlinear least squares optimization (scalar and SIMD) | -| `icp/` | Point-to-Line ICP implementation | -| `scratch.rs` | Pre-allocated buffers for zero-allocation ICP | -| `traits.rs` | `ScanMatcher` trait for extensibility | - -## Algorithms - -### Point-to-Line ICP - -The primary matching algorithm. Aligns scan points to map lines: - -```rust -let result = match_scan(&points, &lines, initial_pose); - -if result.converged { - println!("Pose: ({:.3}, {:.3}, {:.3})", - result.pose.x, result.pose.y, result.pose.theta); - println!("Confidence: {:.2}", result.confidence); -} -``` - -With configuration: - -```rust -let config = IcpConfig { - max_iterations: 20, - convergence_threshold: 0.001, - max_correspondence_distance: 0.5, - outlier_rejection: OutlierRejection::MedianAbsoluteDeviation { k: 2.0 }, - coarse_search: Some(CoarseSearchConfig::default()), - ..Default::default() -}; - -let result = match_scan_with_config(&points, &lines, initial_pose, &config); -``` - -### Zero-Allocation Matching - -For hot paths, reuse scratch space: - -```rust -let mut scratch = IcpScratchSpace::default_capacity(); -let icp = PointToLineIcp::new(); - -for scan in scans { - let result = icp.match_scan_with_scratch(&scan, &map, pose, &mut scratch); -} -``` - -### RANSAC Pose Estimation - -Robust estimation with outlier rejection: - -```rust -let config = RansacConfig { - max_iterations: 100, - inlier_threshold: 0.1, - min_inliers: 10, - confidence: 0.99, -}; - -let result = estimate_pose_ransac(&correspondences, &config); -``` - -### Gauss-Newton Optimization - -Nonlinear least squares for pose refinement: - -```rust -let config = GaussNewtonConfig { - max_iterations: 10, - convergence_threshold: 1e-6, -}; - -// Standard optimization with CorrespondenceSet -let result = optimize_pose_fast(&correspondences, initial_pose, &config); - -// SIMD-optimized with CorrespondenceSoA (f32x4 operations) -let result = optimize_pose_simd(&correspondences_soa, initial_pose, &config); -``` - -## Correspondence Finding - -```rust -// Basic nearest neighbor -let correspondences = find_correspondences(&points, &lines, max_distance); - -// With spatial index (faster for large maps) -let correspondences = find_correspondences_spatial(&points, &lines, &index, max_distance); - -// Batch processing (SIMD-friendly) -let correspondences = find_correspondences_batch(&points, &lines, max_distance); - -// Weighted by confidence -let correspondences = find_correspondences_weighted(&points, &lines, max_distance, weights); - -// With angle constraint -let correspondences = find_correspondences_with_angle( - &points, &lines, max_distance, max_angle_diff -); - -// SoA layout for SIMD Gauss-Newton (inlines line data) -let correspondences_soa = find_correspondences_soa( - &points, &ranges, &line_collection, &noise_model, &config -); -``` - -## Extensibility - -Implement `ScanMatcher` trait for custom matching: - -```rust -pub trait ScanMatcher: Send + Sync { - fn match_scan( - &self, - points: &[Point2D], - lines: &[Line2D], - initial_pose: Pose2D, - ) -> MatchResult; -} -``` - -## ICP Pipeline - -``` -Initial Pose (from odometry) - │ - ▼ -┌───────────────────┐ -│ Coarse Search │ ← Grid search around initial pose -│ (optional) │ -└────────┬──────────┘ - │ - ▼ -┌───────────────────┐ -│ Find │ ← Nearest neighbor in map lines -│ Correspondences │ -└────────┬──────────┘ - │ - ▼ -┌───────────────────┐ -│ Outlier Rejection │ ← MAD or distance threshold -└────────┬──────────┘ - │ - ▼ -┌───────────────────┐ -│ Gauss-Newton │ ← Minimize point-to-line distance -│ Optimization │ -└────────┬──────────┘ - │ - ▼ - Converged? ────No──┐ - │ │ - Yes │ - │ │ - ▼ │ - Matched Pose ◄────┘ -``` - -## Confidence Computation - -Match confidence is based on: -- Inlier ratio (percentage of points with good correspondences) -- Mean residual error (average point-to-line distance) -- Number of correspondences - -```rust -// Confidence ranges from 0.0 to 1.0 -// 0.0 = no match (fallback to odometry) -// 1.0 = perfect match -``` diff --git a/vastu-map/src/matching/correspondence.rs b/vastu-map/src/matching/correspondence.rs deleted file mode 100644 index 34d01ed..0000000 --- a/vastu-map/src/matching/correspondence.rs +++ /dev/null @@ -1,1107 +0,0 @@ -//! Correspondence types for scan matching. -//! -//! A correspondence associates a point from the current scan with -//! a line in the map. -//! -//! # Data Layouts -//! -//! Two layouts are provided: -//! - [`CorrespondenceSet`]: Array-of-Structs (AoS) layout for general use -//! - [`CorrespondenceSoA`]: Struct-of-Arrays (SoA) layout optimized for SIMD Gauss-Newton -//! -//! The SoA layout inlines line data (normals, endpoints) at correspondence creation time, -//! enabling contiguous SIMD loads in the hot loop instead of random indexed access. - -use crate::core::Point2D; -use crate::features::LineCollection; - -/// Raw line data for correspondence creation when LineCollection isn't available. -/// -/// This struct groups all line properties needed for a correspondence, -/// enabling the SoA structure to inline line data without requiring -/// a full LineCollection reference. -#[derive(Clone, Copy, Debug)] -pub struct LineDataRaw { - /// X component of line unit normal. - pub normal_x: f32, - /// Y component of line unit normal. - pub normal_y: f32, - /// X coordinate of line start point. - pub start_x: f32, - /// Y coordinate of line start point. - pub start_y: f32, - /// X coordinate of line end point. - pub end_x: f32, - /// Y coordinate of line end point. - pub end_y: f32, - /// Reciprocal of line length (1.0 / length). - pub inv_length: f32, -} - -/// A correspondence between a scan point and a map line. -#[derive(Clone, Copy, Debug)] -pub struct Correspondence { - /// Index of the point in the scan. - pub point_idx: usize, - /// Index of the line in the map. - pub line_idx: usize, - /// The scan point position (in world frame after initial transform). - pub point: Point2D, - /// Perpendicular distance from point to line. - pub distance: f32, - /// Projection parameter t along the line (0 = start, 1 = end). - pub projection_t: f32, - /// Weight for this correspondence (1/σ² based on measurement uncertainty). - /// Higher weight = more reliable measurement. Default: 1.0 - pub weight: f32, - /// Range from sensor to this point (meters). Used for weight computation. - pub range: f32, -} - -impl Correspondence { - /// Create a new correspondence with default weight (1.0). - #[inline] - pub fn new( - point_idx: usize, - line_idx: usize, - point: Point2D, - distance: f32, - projection_t: f32, - ) -> Self { - Self { - point_idx, - line_idx, - point, - distance, - projection_t, - weight: 1.0, - range: 0.0, - } - } - - /// Create a new correspondence with explicit weight and range. - #[inline] - pub fn with_weight( - point_idx: usize, - line_idx: usize, - point: Point2D, - distance: f32, - projection_t: f32, - weight: f32, - range: f32, - ) -> Self { - Self { - point_idx, - line_idx, - point, - distance, - projection_t, - weight, - range, - } - } - - /// Check if the projection falls within the line segment. - #[inline] - pub fn is_within_segment(&self) -> bool { - self.projection_t >= 0.0 && self.projection_t <= 1.0 - } - - /// Check if this is a valid correspondence (within distance threshold). - #[inline] - pub fn is_valid(&self, max_distance: f32) -> bool { - self.distance <= max_distance - } -} - -/// Collection of correspondences for a scan match. -#[derive(Clone, Debug, Default)] -pub struct CorrespondenceSet { - /// All correspondences found. - pub correspondences: Vec, -} - -impl CorrespondenceSet { - /// Create a new empty correspondence set. - pub fn new() -> Self { - Self::default() - } - - /// Create with capacity. - pub fn with_capacity(capacity: usize) -> Self { - Self { - correspondences: Vec::with_capacity(capacity), - } - } - - /// Add a correspondence. - #[inline] - pub fn push(&mut self, corr: Correspondence) { - self.correspondences.push(corr); - } - - /// Number of correspondences. - #[inline] - pub fn len(&self) -> usize { - self.correspondences.len() - } - - /// Check if empty. - #[inline] - pub fn is_empty(&self) -> bool { - self.correspondences.is_empty() - } - - /// Filter correspondences by maximum distance. - pub fn filter_by_distance(&self, max_distance: f32) -> Self { - Self { - correspondences: self - .correspondences - .iter() - .filter(|c| c.distance <= max_distance) - .copied() - .collect(), - } - } - - /// Filter correspondences by maximum distance in-place (zero-allocation). - /// - /// This is more efficient than `filter_by_distance` when you don't need - /// to preserve the original set. - #[inline] - pub fn retain_by_distance(&mut self, max_distance: f32) { - self.correspondences.retain(|c| c.distance <= max_distance); - } - - /// Filter correspondences to only those within line segments. - pub fn filter_within_segment(&self) -> Self { - Self { - correspondences: self - .correspondences - .iter() - .filter(|c| c.is_within_segment()) - .copied() - .collect(), - } - } - - /// Filter correspondences to only those within line segments in-place (zero-allocation). - #[inline] - pub fn retain_within_segment(&mut self) { - self.correspondences.retain(|c| c.is_within_segment()); - } - - /// Get the mean correspondence distance. - pub fn mean_distance(&self) -> f32 { - if self.is_empty() { - return 0.0; - } - let sum: f32 = self.correspondences.iter().map(|c| c.distance).sum(); - sum / self.len() as f32 - } - - /// Get the RMS (root mean square) correspondence distance. - pub fn rms_distance(&self) -> f32 { - if self.is_empty() { - return 0.0; - } - let sum_sq: f32 = self - .correspondences - .iter() - .map(|c| c.distance * c.distance) - .sum(); - (sum_sq / self.len() as f32).sqrt() - } - - /// Get the weighted RMS correspondence distance. - /// Uses correspondence weights for proper uncertainty-weighted averaging. - pub fn weighted_rms_distance(&self) -> f32 { - if self.is_empty() { - return 0.0; - } - let mut sum_weighted_sq = 0.0f32; - let mut sum_weights = 0.0f32; - for c in &self.correspondences { - sum_weighted_sq += c.weight * c.distance * c.distance; - sum_weights += c.weight; - } - if sum_weights > 0.0 { - (sum_weighted_sq / sum_weights).sqrt() - } else { - self.rms_distance() // fallback to unweighted - } - } - - /// Get the total weight of all correspondences. - pub fn total_weight(&self) -> f32 { - self.correspondences.iter().map(|c| c.weight).sum() - } - - /// Get the maximum correspondence distance. - pub fn max_distance(&self) -> f32 { - self.correspondences - .iter() - .map(|c| c.distance) - .fold(0.0, f32::max) - } - - /// Compute inlier ratio (correspondences within threshold). - pub fn inlier_ratio(&self, threshold: f32) -> f32 { - if self.is_empty() { - return 0.0; - } - let inliers = self - .correspondences - .iter() - .filter(|c| c.distance <= threshold) - .count(); - inliers as f32 / self.len() as f32 - } - - /// Clear all correspondences. - pub fn clear(&mut self) { - self.correspondences.clear(); - } - - /// Iterate over correspondences. - pub fn iter(&self) -> impl Iterator { - self.correspondences.iter() - } - - /// Iterate over correspondences mutably. - pub fn iter_mut(&mut self) -> impl Iterator { - self.correspondences.iter_mut() - } - - /// Apply weights to correspondences based on range using a noise model. - /// - /// The range for each correspondence is computed from the original robot-frame points - /// (magnitude of the point vector). Weights are computed as 1/σ² where σ is the - /// range-dependent measurement uncertainty. - /// - /// # Arguments - /// * `robot_frame_points` - Original scan points in robot frame (for range computation) - /// * `noise_model` - Lidar noise model for weight computation - pub fn apply_weights( - &mut self, - robot_frame_points: &[crate::core::Point2D], - noise_model: &crate::config::LidarNoiseModel, - ) { - for corr in &mut self.correspondences { - if corr.point_idx < robot_frame_points.len() { - let p = robot_frame_points[corr.point_idx]; - let range = (p.x * p.x + p.y * p.y).sqrt(); - corr.range = range; - corr.weight = noise_model.weight(range); - } - } - } - - /// Create a new set with weights applied based on range. - pub fn with_weights( - mut self, - robot_frame_points: &[crate::core::Point2D], - noise_model: &crate::config::LidarNoiseModel, - ) -> Self { - self.apply_weights(robot_frame_points, noise_model); - self - } -} - -// ───────────────────────────────────────────────────────────────────────────── -// CorrespondenceSoA: SIMD-Optimized Struct-of-Arrays Layout -// ───────────────────────────────────────────────────────────────────────────── - -/// SIMD-optimized correspondence collection with Struct-of-Arrays layout. -/// -/// Unlike [`CorrespondenceSet`], this structure inlines line data (normals, -/// endpoints, inverse lengths) at correspondence creation time. This enables -/// contiguous SIMD loads in the Gauss-Newton hot loop instead of random -/// indexed access to line arrays. -/// -/// # Memory Layout -/// -/// **HOT data** (used in Gauss-Newton inner loop): -/// - Point positions (point_xs, point_ys) -/// - Line normals (normal_xs, normal_ys) - INLINED, not indexed -/// - Line endpoints (line_start_xs, line_start_ys, line_end_xs, line_end_ys) -/// - Inverse lengths (line_inv_lengths) - for residual computation -/// - Weights -/// -/// **COLD data** (metadata, not used in optimization): -/// - Point indices, line indices, distances -/// -/// # SIMD Usage -/// -/// Arrays are padded to SIMD width (4) with zero-weight entries via `finalize()`. -/// Use `simd_chunks()` to get the number of 4-element chunks for iteration. -/// -/// # Example -/// -/// ```rust,ignore -/// use vastu_map::matching::CorrespondenceSoA; -/// -/// let mut soa = CorrespondenceSoA::new(); -/// // ... push correspondences ... -/// soa.finalize(); // Pad to SIMD width -/// -/// for i in 0..soa.simd_chunks() { -/// let base = i * 4; -/// let px = f32x4::from_slice(&soa.point_xs[base..]); -/// let nx = f32x4::from_slice(&soa.normal_xs[base..]); -/// // ... SIMD operations ... -/// } -/// ``` -#[derive(Clone, Debug, Default)] -pub struct CorrespondenceSoA { - // === HOT: Contiguous arrays for SIMD (used in Gauss-Newton) === - /// X coordinates of scan points (world frame). - pub point_xs: Vec, - /// Y coordinates of scan points (world frame). - pub point_ys: Vec, - /// X components of line unit normals (INLINED from LineCollection). - pub normal_xs: Vec, - /// Y components of line unit normals (INLINED from LineCollection). - pub normal_ys: Vec, - /// X coordinates of line start points. - pub line_start_xs: Vec, - /// Y coordinates of line start points. - pub line_start_ys: Vec, - /// X coordinates of line end points. - pub line_end_xs: Vec, - /// Y coordinates of line end points. - pub line_end_ys: Vec, - /// Reciprocal of line lengths (1.0 / length). - pub line_inv_lengths: Vec, - /// Correspondence weights (1/σ² from noise model). - pub weights: Vec, - - // === COLD: Metadata (not used in optimization) === - /// Indices of points in original scan. - pub point_indices: Vec, - /// Indices of lines in map. - pub line_indices: Vec, - /// Perpendicular distances from points to lines. - pub distances: Vec, - - /// Number of real correspondences (before padding). - len: usize, -} - -/// SIMD lane width for f32x4. -const SIMD_WIDTH: usize = 4; - -impl CorrespondenceSoA { - /// Create a new empty SoA correspondence collection. - pub fn new() -> Self { - Self::default() - } - - /// Create with capacity hint. - pub fn with_capacity(capacity: usize) -> Self { - Self { - point_xs: Vec::with_capacity(capacity), - point_ys: Vec::with_capacity(capacity), - normal_xs: Vec::with_capacity(capacity), - normal_ys: Vec::with_capacity(capacity), - line_start_xs: Vec::with_capacity(capacity), - line_start_ys: Vec::with_capacity(capacity), - line_end_xs: Vec::with_capacity(capacity), - line_end_ys: Vec::with_capacity(capacity), - line_inv_lengths: Vec::with_capacity(capacity), - weights: Vec::with_capacity(capacity), - point_indices: Vec::with_capacity(capacity), - line_indices: Vec::with_capacity(capacity), - distances: Vec::with_capacity(capacity), - len: 0, - } - } - - /// Add a correspondence, inlining line data from the LineCollection. - /// - /// This copies the line's normal, endpoints, and inverse length into the - /// SoA arrays, eliminating indexed access in the Gauss-Newton hot loop. - #[inline] - pub fn push( - &mut self, - point_idx: usize, - line_idx: usize, - point: Point2D, - distance: f32, - weight: f32, - lines: &LineCollection, - ) { - self.point_xs.push(point.x); - self.point_ys.push(point.y); - - // Inline line data (no indexed access needed in hot loop) - let (normal_xs, normal_ys) = lines.normals(); - self.normal_xs.push(normal_xs[line_idx]); - self.normal_ys.push(normal_ys[line_idx]); - self.line_start_xs.push(lines.start_xs[line_idx]); - self.line_start_ys.push(lines.start_ys[line_idx]); - self.line_end_xs.push(lines.end_xs[line_idx]); - self.line_end_ys.push(lines.end_ys[line_idx]); - self.line_inv_lengths.push(lines.inv_lengths()[line_idx]); - self.weights.push(weight); - - // Metadata - self.point_indices.push(point_idx); - self.line_indices.push(line_idx); - self.distances.push(distance); - - self.len += 1; - } - - /// Add a correspondence with explicit line data (for when LineCollection isn't available). - #[inline] - pub fn push_with_line_data( - &mut self, - point_idx: usize, - line_idx: usize, - point: Point2D, - distance: f32, - weight: f32, - line: LineDataRaw, - ) { - self.point_xs.push(point.x); - self.point_ys.push(point.y); - self.normal_xs.push(line.normal_x); - self.normal_ys.push(line.normal_y); - self.line_start_xs.push(line.start_x); - self.line_start_ys.push(line.start_y); - self.line_end_xs.push(line.end_x); - self.line_end_ys.push(line.end_y); - self.line_inv_lengths.push(line.inv_length); - self.weights.push(weight); - self.point_indices.push(point_idx); - self.line_indices.push(line_idx); - self.distances.push(distance); - self.len += 1; - } - - /// Number of real correspondences (before SIMD padding). - #[inline] - pub fn len(&self) -> usize { - self.len - } - - /// Check if empty. - #[inline] - pub fn is_empty(&self) -> bool { - self.len == 0 - } - - /// Pad arrays to SIMD width (4) with zero-weight entries. - /// - /// Call this after all correspondences have been added and before - /// SIMD iteration. Zero-weight entries don't affect the optimization. - pub fn finalize(&mut self) { - let remainder = self.len % SIMD_WIDTH; - if remainder == 0 { - return; - } - - let padding = SIMD_WIDTH - remainder; - for _ in 0..padding { - self.point_xs.push(0.0); - self.point_ys.push(0.0); - self.normal_xs.push(0.0); - self.normal_ys.push(1.0); // Arbitrary valid normal - self.line_start_xs.push(0.0); - self.line_start_ys.push(0.0); - self.line_end_xs.push(0.0); - self.line_end_ys.push(0.0); - self.line_inv_lengths.push(0.0); - self.weights.push(0.0); // Zero weight = no contribution - self.point_indices.push(0); - self.line_indices.push(0); - self.distances.push(0.0); - } - } - - /// Number of SIMD chunks (4-element groups) for iteration. - /// - /// Call after `finalize()` for correct padding. - #[inline] - pub fn simd_chunks(&self) -> usize { - self.point_xs.len().div_ceil(SIMD_WIDTH) - } - - /// Clear all correspondences (keeps capacity). - pub fn clear(&mut self) { - self.point_xs.clear(); - self.point_ys.clear(); - self.normal_xs.clear(); - self.normal_ys.clear(); - self.line_start_xs.clear(); - self.line_start_ys.clear(); - self.line_end_xs.clear(); - self.line_end_ys.clear(); - self.line_inv_lengths.clear(); - self.weights.clear(); - self.point_indices.clear(); - self.line_indices.clear(); - self.distances.clear(); - self.len = 0; - } - - /// Get the mean correspondence distance. - pub fn mean_distance(&self) -> f32 { - if self.len == 0 { - return 0.0; - } - let sum: f32 = self.distances[..self.len].iter().sum(); - sum / self.len as f32 - } - - /// Get the RMS (root mean square) correspondence distance. - pub fn rms_distance(&self) -> f32 { - if self.len == 0 { - return 0.0; - } - let sum_sq: f32 = self.distances[..self.len].iter().map(|d| d * d).sum(); - (sum_sq / self.len as f32).sqrt() - } - - /// Get the weighted RMS correspondence distance. - pub fn weighted_rms_distance(&self) -> f32 { - if self.len == 0 { - return 0.0; - } - let mut sum_weighted_sq = 0.0f32; - let mut sum_weights = 0.0f32; - for i in 0..self.len { - let w = self.weights[i]; - let d = self.distances[i]; - sum_weighted_sq += w * d * d; - sum_weights += w; - } - if sum_weights > 0.0 { - (sum_weighted_sq / sum_weights).sqrt() - } else { - self.rms_distance() - } - } - - /// Get the total weight of all correspondences. - pub fn total_weight(&self) -> f32 { - self.weights[..self.len].iter().sum() - } - - /// Get the maximum correspondence distance. - pub fn max_distance(&self) -> f32 { - self.distances[..self.len] - .iter() - .copied() - .fold(0.0, f32::max) - } - - /// Compute inlier ratio (correspondences within threshold). - pub fn inlier_ratio(&self, threshold: f32) -> f32 { - if self.len == 0 { - return 0.0; - } - let inliers = self.distances[..self.len] - .iter() - .filter(|&&d| d <= threshold) - .count(); - inliers as f32 / self.len as f32 - } - - /// Convert to CorrespondenceSet (for compatibility with existing code). - pub fn to_correspondence_set(&self) -> CorrespondenceSet { - let mut set = CorrespondenceSet::with_capacity(self.len); - for i in 0..self.len { - set.push(Correspondence::with_weight( - self.point_indices[i], - self.line_indices[i], - Point2D::new(self.point_xs[i], self.point_ys[i]), - self.distances[i], - 0.0, // projection_t not stored in SoA - self.weights[i], - 0.0, // range not stored in SoA - )); - } - set - } -} - -/// A correspondence between a scan point and a map corner. -/// -/// Used for point-to-point constraints in ICP, which provide -/// better angular accuracy than point-to-line constraints alone. -#[derive(Clone, Copy, Debug)] -pub struct CornerCorrespondence { - /// Index of the point in the scan. - pub point_idx: usize, - /// Index of the corner in the map. - pub corner_idx: usize, - /// The scan point position (in world frame after initial transform). - pub point: Point2D, - /// The map corner position. - pub corner: Point2D, - /// Distance from point to corner. - pub distance: f32, - /// Weight for this correspondence (1/σ² based on measurement uncertainty). - pub weight: f32, -} - -impl CornerCorrespondence { - /// Create a new corner correspondence with default weight (1.0). - #[inline] - pub fn new( - point_idx: usize, - corner_idx: usize, - point: Point2D, - corner: Point2D, - distance: f32, - ) -> Self { - Self { - point_idx, - corner_idx, - point, - corner, - distance, - weight: 1.0, - } - } - - /// Create a new corner correspondence with explicit weight. - #[inline] - pub fn with_weight( - point_idx: usize, - corner_idx: usize, - point: Point2D, - corner: Point2D, - distance: f32, - weight: f32, - ) -> Self { - Self { - point_idx, - corner_idx, - point, - corner, - distance, - weight, - } - } - - /// Check if this is a valid correspondence (within distance threshold). - #[inline] - pub fn is_valid(&self, max_distance: f32) -> bool { - self.distance <= max_distance - } -} - -/// Collection of corner correspondences for a scan match. -#[derive(Clone, Debug, Default)] -pub struct CornerCorrespondenceSet { - /// All corner correspondences found. - pub correspondences: Vec, -} - -impl CornerCorrespondenceSet { - /// Create a new empty corner correspondence set. - pub fn new() -> Self { - Self::default() - } - - /// Create with capacity. - pub fn with_capacity(capacity: usize) -> Self { - Self { - correspondences: Vec::with_capacity(capacity), - } - } - - /// Add a correspondence. - #[inline] - pub fn push(&mut self, corr: CornerCorrespondence) { - self.correspondences.push(corr); - } - - /// Number of correspondences. - #[inline] - pub fn len(&self) -> usize { - self.correspondences.len() - } - - /// Check if empty. - #[inline] - pub fn is_empty(&self) -> bool { - self.correspondences.is_empty() - } - - /// Filter correspondences by maximum distance. - pub fn filter_by_distance(&self, max_distance: f32) -> Self { - Self { - correspondences: self - .correspondences - .iter() - .filter(|c| c.distance <= max_distance) - .copied() - .collect(), - } - } - - /// Get the mean correspondence distance. - pub fn mean_distance(&self) -> f32 { - if self.is_empty() { - return 0.0; - } - let sum: f32 = self.correspondences.iter().map(|c| c.distance).sum(); - sum / self.len() as f32 - } - - /// Get the RMS (root mean square) correspondence distance. - pub fn rms_distance(&self) -> f32 { - if self.is_empty() { - return 0.0; - } - let sum_sq: f32 = self - .correspondences - .iter() - .map(|c| c.distance * c.distance) - .sum(); - (sum_sq / self.len() as f32).sqrt() - } - - /// Get the total weight of all correspondences. - pub fn total_weight(&self) -> f32 { - self.correspondences.iter().map(|c| c.weight).sum() - } - - /// Clear all correspondences. - pub fn clear(&mut self) { - self.correspondences.clear(); - } - - /// Iterate over correspondences. - pub fn iter(&self) -> impl Iterator { - self.correspondences.iter() - } - - /// Iterate over correspondences mutably. - pub fn iter_mut(&mut self) -> impl Iterator { - self.correspondences.iter_mut() - } -} - -/// 3x3 covariance matrix for pose uncertainty [x, y, theta]. -pub type PoseCovariance = [[f32; 3]; 3]; - -/// Result of a scan matching operation. -#[derive(Clone, Debug)] -pub struct MatchResult { - /// Estimated pose transformation. - pub pose: crate::core::Pose2D, - /// Correspondences used in the final match. - pub correspondences: CorrespondenceSet, - /// Number of iterations performed. - pub iterations: usize, - /// Whether the matching converged. - pub converged: bool, - /// Final RMS error. - pub rms_error: f32, - /// Match confidence (0.0 to 1.0). - pub confidence: f32, - /// Pose covariance matrix [x, y, theta]. - /// Computed from the Hessian of the Gauss-Newton optimization. - pub covariance: PoseCovariance, - /// Condition number of the Hessian matrix. - /// High values (>100) indicate degenerate geometry (e.g., single wall). - pub condition_number: f32, - /// Whether coarse search was used for initialization. - pub used_coarse_search: bool, -} - -/// Identity covariance matrix (no uncertainty information). -pub const IDENTITY_COVARIANCE: PoseCovariance = [[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]]; - -impl MatchResult { - /// Create a new match result with default covariance (backward compatible). - pub fn new( - pose: crate::core::Pose2D, - correspondences: CorrespondenceSet, - iterations: usize, - converged: bool, - ) -> Self { - let rms_error = correspondences.rms_distance(); - let confidence = Self::compute_confidence(&correspondences, converged); - - Self { - pose, - correspondences, - iterations, - converged, - rms_error, - confidence, - covariance: IDENTITY_COVARIANCE, - condition_number: 1.0, - used_coarse_search: false, - } - } - - /// Create a new match result with full covariance information. - pub fn with_covariance( - pose: crate::core::Pose2D, - correspondences: CorrespondenceSet, - iterations: usize, - converged: bool, - covariance: PoseCovariance, - condition_number: f32, - used_coarse_search: bool, - ) -> Self { - let rms_error = correspondences.rms_distance(); - let confidence = Self::compute_confidence(&correspondences, converged); - - Self { - pose, - correspondences, - iterations, - converged, - rms_error, - confidence, - covariance, - condition_number, - used_coarse_search, - } - } - - /// Compute match confidence based on various factors. - fn compute_confidence(correspondences: &CorrespondenceSet, converged: bool) -> f32 { - if !converged || correspondences.is_empty() { - return 0.0; - } - - // Factors affecting confidence: - // 1. Number of correspondences (more is better) - // 2. Mean distance (lower is better) - // 3. Inlier ratio (higher is better) - - let n = correspondences.len() as f32; - let count_factor = (n / 50.0).min(1.0); // Saturates at 50 correspondences - - let mean_dist = correspondences.mean_distance(); - let dist_factor = (-mean_dist / 0.1).exp(); // Exponential decay, 0.1m = ~37% - - let inlier_ratio = correspondences.inlier_ratio(0.1); - - // Combine factors - (count_factor * dist_factor * inlier_ratio).clamp(0.0, 1.0) - } - - /// Check if this is a good match. - pub fn is_good_match(&self, min_confidence: f32) -> bool { - self.converged && self.confidence >= min_confidence - } - - /// Check if the geometry is degenerate (e.g., single wall). - /// Returns true if condition number exceeds the threshold. - pub fn is_degenerate(&self, threshold: f32) -> bool { - self.condition_number > threshold - } - - /// Get the position uncertainty (sqrt of x,y diagonal covariance elements). - pub fn position_uncertainty(&self) -> f32 { - (self.covariance[0][0] + self.covariance[1][1]).sqrt() - } - - /// Get the orientation uncertainty (sqrt of theta diagonal covariance element). - pub fn orientation_uncertainty(&self) -> f32 { - self.covariance[2][2].sqrt() - } -} - -#[cfg(test)] -mod tests { - use super::*; - use crate::core::Pose2D; - - #[test] - fn test_correspondence_new() { - let corr = Correspondence::new(0, 1, Point2D::new(1.0, 2.0), 0.05, 0.5); - - assert_eq!(corr.point_idx, 0); - assert_eq!(corr.line_idx, 1); - assert_eq!(corr.distance, 0.05); - assert!(corr.is_within_segment()); - } - - #[test] - fn test_correspondence_within_segment() { - let inside = Correspondence::new(0, 0, Point2D::zero(), 0.0, 0.5); - assert!(inside.is_within_segment()); - - let at_start = Correspondence::new(0, 0, Point2D::zero(), 0.0, 0.0); - assert!(at_start.is_within_segment()); - - let at_end = Correspondence::new(0, 0, Point2D::zero(), 0.0, 1.0); - assert!(at_end.is_within_segment()); - - let outside = Correspondence::new(0, 0, Point2D::zero(), 0.0, 1.5); - assert!(!outside.is_within_segment()); - } - - #[test] - fn test_correspondence_set_statistics() { - let mut set = CorrespondenceSet::new(); - set.push(Correspondence::new(0, 0, Point2D::zero(), 0.1, 0.5)); - set.push(Correspondence::new(1, 0, Point2D::zero(), 0.2, 0.5)); - set.push(Correspondence::new(2, 0, Point2D::zero(), 0.3, 0.5)); - - assert_eq!(set.len(), 3); - assert!((set.mean_distance() - 0.2).abs() < 1e-6); - assert_eq!(set.max_distance(), 0.3); - - // RMS = sqrt((0.01 + 0.04 + 0.09) / 3) = sqrt(0.14/3) ≈ 0.216 - assert!((set.rms_distance() - 0.216).abs() < 0.01); - } - - #[test] - fn test_correspondence_set_filter() { - let mut set = CorrespondenceSet::new(); - set.push(Correspondence::new(0, 0, Point2D::zero(), 0.05, 0.5)); - set.push(Correspondence::new(1, 0, Point2D::zero(), 0.15, 0.5)); - set.push(Correspondence::new(2, 0, Point2D::zero(), 0.25, 0.5)); - - let filtered = set.filter_by_distance(0.1); - assert_eq!(filtered.len(), 1); - } - - #[test] - fn test_correspondence_set_inlier_ratio() { - let mut set = CorrespondenceSet::new(); - set.push(Correspondence::new(0, 0, Point2D::zero(), 0.05, 0.5)); - set.push(Correspondence::new(1, 0, Point2D::zero(), 0.05, 0.5)); - set.push(Correspondence::new(2, 0, Point2D::zero(), 0.15, 0.5)); - set.push(Correspondence::new(3, 0, Point2D::zero(), 0.25, 0.5)); - - let ratio = set.inlier_ratio(0.1); - assert!((ratio - 0.5).abs() < 1e-6); // 2 out of 4 - } - - #[test] - fn test_match_result() { - let mut corr = CorrespondenceSet::new(); - for i in 0..50 { - corr.push(Correspondence::new(i, 0, Point2D::zero(), 0.02, 0.5)); - } - - let result = MatchResult::new(Pose2D::identity(), corr, 10, true); - - assert!(result.converged); - // With 50 correspondences at 0.02m: count_factor=1.0, dist_factor≈0.82, inlier_ratio=1.0 - // confidence ≈ 0.82 - assert!(result.confidence > 0.5); - assert!(result.is_good_match(0.3)); - } - - #[test] - fn test_corner_correspondence_new() { - let point = Point2D::new(1.0, 2.0); - let corner = Point2D::new(1.1, 2.05); - let corr = CornerCorrespondence::new(0, 1, point, corner, 0.1); - - assert_eq!(corr.point_idx, 0); - assert_eq!(corr.corner_idx, 1); - assert_eq!(corr.distance, 0.1); - assert_eq!(corr.weight, 1.0); - assert!(corr.is_valid(0.15)); - assert!(!corr.is_valid(0.05)); - } - - #[test] - fn test_corner_correspondence_with_weight() { - let point = Point2D::new(1.0, 2.0); - let corner = Point2D::new(1.1, 2.05); - let corr = CornerCorrespondence::with_weight(0, 1, point, corner, 0.1, 0.5); - - assert_eq!(corr.weight, 0.5); - } - - #[test] - fn test_corner_correspondence_set_statistics() { - let mut set = CornerCorrespondenceSet::new(); - set.push(CornerCorrespondence::new( - 0, - 0, - Point2D::zero(), - Point2D::new(0.1, 0.0), - 0.1, - )); - set.push(CornerCorrespondence::new( - 1, - 1, - Point2D::zero(), - Point2D::new(0.2, 0.0), - 0.2, - )); - set.push(CornerCorrespondence::new( - 2, - 2, - Point2D::zero(), - Point2D::new(0.3, 0.0), - 0.3, - )); - - assert_eq!(set.len(), 3); - assert!(!set.is_empty()); - assert!((set.mean_distance() - 0.2).abs() < 1e-6); - - // RMS = sqrt((0.01 + 0.04 + 0.09) / 3) = sqrt(0.14/3) ≈ 0.216 - assert!((set.rms_distance() - 0.216).abs() < 0.01); - } - - #[test] - fn test_corner_correspondence_set_filter() { - let mut set = CornerCorrespondenceSet::new(); - set.push(CornerCorrespondence::new( - 0, - 0, - Point2D::zero(), - Point2D::new(0.05, 0.0), - 0.05, - )); - set.push(CornerCorrespondence::new( - 1, - 1, - Point2D::zero(), - Point2D::new(0.15, 0.0), - 0.15, - )); - set.push(CornerCorrespondence::new( - 2, - 2, - Point2D::zero(), - Point2D::new(0.25, 0.0), - 0.25, - )); - - let filtered = set.filter_by_distance(0.1); - assert_eq!(filtered.len(), 1); - } - - #[test] - fn test_corner_correspondence_set_total_weight() { - let mut set = CornerCorrespondenceSet::new(); - set.push(CornerCorrespondence::with_weight( - 0, - 0, - Point2D::zero(), - Point2D::new(0.1, 0.0), - 0.1, - 0.5, - )); - set.push(CornerCorrespondence::with_weight( - 1, - 1, - Point2D::zero(), - Point2D::new(0.2, 0.0), - 0.2, - 0.3, - )); - - assert!((set.total_weight() - 0.8).abs() < 1e-6); - } -} diff --git a/vastu-map/src/matching/gauss_newton.rs b/vastu-map/src/matching/gauss_newton.rs deleted file mode 100644 index 3a24473..0000000 --- a/vastu-map/src/matching/gauss_newton.rs +++ /dev/null @@ -1,1233 +0,0 @@ -//! Levenberg-Marquardt solver for point-to-line ICP. -//! -//! Solves the nonlinear least squares problem of finding the pose -//! that minimizes the sum of squared point-to-line distances. -//! -//! Uses Levenberg-Marquardt algorithm for robust convergence. LM blends -//! between Gauss-Newton (fast convergence near solution) and gradient -//! descent (robust far from solution). Set `lm_initial_lambda = 0.0` -//! to disable LM damping and use pure Gauss-Newton. -//! -//! # Performance -//! -//! Three versions are provided: -//! - `optimize_pose`: Takes `&[Line2D]`, computes normals on-the-fly -//! - `optimize_pose_fast`: Takes `&LineCollection` with pre-computed normals -//! - `optimize_pose_simd`: Takes `&CorrespondenceSoA` for full SIMD acceleration -//! -//! Use `optimize_pose_simd` for maximum performance (~30-40% faster than fast). - -use std::simd::{f32x4, num::SimdFloat}; - -use crate::core::{Point2D, Pose2D}; -use crate::features::{Line2D, LineCollection}; - -use super::correspondence::{CorrespondenceSet, CorrespondenceSoA}; - -/// Configuration for Levenberg-Marquardt solver. -/// -/// LM blends between Gauss-Newton and gradient descent for robust convergence. -/// Set `lm_initial_lambda = 0.0` to disable LM damping (pure Gauss-Newton). -#[derive(Clone, Debug)] -pub struct GaussNewtonConfig { - /// Maximum number of iterations. - /// Default: 20 - pub max_iterations: usize, - - /// Convergence threshold for pose change. - /// If ‖Δpose‖ < threshold, iteration stops. - /// Default: 1e-6 - pub convergence_threshold: f32, - - /// Pre-computed squared convergence threshold for efficient comparison. - /// Avoids sqrt() in hot loop. - pub(crate) convergence_threshold_sq: f32, - - /// Minimum eigenvalue ratio for matrix conditioning. - /// If smallest/largest eigenvalue < ratio, add regularization. - /// Default: 1e-6 - pub min_eigenvalue_ratio: f32, - - /// Regularization factor for ill-conditioned systems. - /// Default: 1e-4 - pub regularization: f32, - - /// Initial LM damping factor (λ). - /// Set to 0.0 to disable LM and use pure Gauss-Newton. - /// Default: 0.001 - pub lm_initial_lambda: f32, - - /// Factor to scale λ up (on bad step) or down (on good step). - /// Default: 10.0 - pub lm_lambda_factor: f32, - - /// Minimum λ value (floor after successful steps). - /// Default: 1e-7 - pub lm_min_lambda: f32, - - /// Maximum λ value (triggers failure if exceeded). - /// Default: 1e7 - pub lm_max_lambda: f32, -} - -impl Default for GaussNewtonConfig { - fn default() -> Self { - let convergence_threshold = 1e-6; - Self { - max_iterations: 20, - convergence_threshold, - convergence_threshold_sq: convergence_threshold * convergence_threshold, - min_eigenvalue_ratio: 1e-6, - regularization: 1e-4, - // LM parameters - lm_initial_lambda: 0.001, - lm_lambda_factor: 10.0, - lm_min_lambda: 1e-7, - lm_max_lambda: 1e7, - } - } -} - -impl GaussNewtonConfig { - /// Create a new configuration with default values. - pub fn new() -> Self { - Self::default() - } - - /// Builder-style setter for maximum iterations. - pub fn with_max_iterations(mut self, iterations: usize) -> Self { - self.max_iterations = iterations; - self - } - - /// Builder-style setter for convergence threshold. - pub fn with_convergence_threshold(mut self, threshold: f32) -> Self { - self.convergence_threshold = threshold; - self.convergence_threshold_sq = threshold * threshold; - self - } - - /// Builder-style setter for initial LM lambda. - /// Set to 0.0 to disable LM (pure Gauss-Newton). - pub fn with_lm_lambda(mut self, lambda: f32) -> Self { - self.lm_initial_lambda = lambda; - self - } - - /// Disable LM damping (use pure Gauss-Newton). - pub fn without_lm(mut self) -> Self { - self.lm_initial_lambda = 0.0; - self - } -} - -use super::correspondence::PoseCovariance; - -/// Result of Gauss-Newton optimization. -#[derive(Clone, Debug)] -pub struct GaussNewtonResult { - /// Optimized pose. - pub pose: Pose2D, - /// Number of iterations performed. - pub iterations: usize, - /// Final residual (RMS error). - pub residual: f32, - /// Whether optimization converged. - pub converged: bool, - /// Pose covariance matrix (from inverse Hessian). - /// Captures uncertainty in [x, y, theta]. - pub covariance: PoseCovariance, - /// Condition number of the Hessian matrix. - /// High values (>100) indicate degenerate geometry. - pub condition_number: f32, -} - -/// Identity covariance matrix. -const IDENTITY_COVARIANCE: PoseCovariance = [[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]]; - -/// Optimize pose using Gauss-Newton method for point-to-line ICP. -/// -/// Minimizes: sum_i w_i * (n_i · (R(θ)·p_i + t - q_i))² -/// -/// where: -/// - w_i is the weight of correspondence i (from noise model) -/// - p_i is the i-th scan point -/// - q_i is the closest point on line i -/// - n_i is the normal of line i -/// - R(θ), t is the rotation and translation -/// -/// # Arguments -/// * `correspondences` - Point-to-line correspondences (with weights) -/// * `lines` - Map lines -/// * `initial_pose` - Initial pose estimate -/// * `config` - Solver configuration -/// -/// # Returns -/// Optimization result with final pose, covariance, and convergence info. -pub fn optimize_pose( - correspondences: &CorrespondenceSet, - lines: &[Line2D], - initial_pose: Pose2D, - config: &GaussNewtonConfig, -) -> GaussNewtonResult { - if correspondences.is_empty() { - return GaussNewtonResult { - pose: initial_pose, - iterations: 0, - residual: 0.0, - converged: true, - covariance: IDENTITY_COVARIANCE, - condition_number: 1.0, - }; - } - - let mut pose = initial_pose; - let mut converged = false; - let mut iterations = 0; - let mut final_hessian = [[0.0f32; 3]; 3]; - - for iter in 0..config.max_iterations { - iterations = iter + 1; - - // Build weighted linear system: H·Δx = -g - // H = J^T·W·J (weighted Hessian approximation) - // g = J^T·W·r (weighted gradient) - let (h, g, residual) = build_linear_system_weighted(correspondences, lines, &pose); - final_hessian = h; - - // Solve for Δx using direct 3x3 solve - let Some(delta) = solve_3x3(&h, &g, config.regularization) else { - // System is singular - stop iteration - break; - }; - - // Update pose - pose.x += delta[0]; - pose.y += delta[1]; - pose.theta += delta[2]; - pose.theta = crate::core::math::normalize_angle(pose.theta); - - // Check convergence using squared comparison to avoid sqrt() - let delta_norm_sq = delta[0] * delta[0] + delta[1] * delta[1] + delta[2] * delta[2]; - if delta_norm_sq < config.convergence_threshold_sq { - converged = true; - break; - } - - // Also check if residual is very small - if residual < config.convergence_threshold { - converged = true; - break; - } - } - - // Compute final residual - let final_residual = compute_residual(correspondences, lines, &pose); - - // Compute covariance from inverse Hessian: Cov = σ² × H⁻¹ - // Use residual variance as σ² - let residual_variance = final_residual * final_residual; - let (covariance, condition_number) = - compute_covariance(&final_hessian, residual_variance, config.regularization); - - GaussNewtonResult { - pose, - iterations, - residual: final_residual, - converged, - covariance, - condition_number, - } -} - -/// Build the weighted linear system for Gauss-Newton. -/// -/// Uses correspondence weights for proper uncertainty-weighted least squares. -/// -/// Returns (H, g, residual) where: -/// - H is the 3x3 weighted Hessian approximation (J^T·W·J) -/// - g is the 3-element weighted gradient (J^T·W·r) -/// - residual is the weighted RMS error -fn build_linear_system_weighted( - correspondences: &CorrespondenceSet, - lines: &[Line2D], - pose: &Pose2D, -) -> ([[f32; 3]; 3], [f32; 3], f32) { - let (sin, cos) = pose.theta.sin_cos(); - - // Initialize H and g to zero - let mut h = [[0.0f32; 3]; 3]; - let mut g = [0.0f32; 3]; - let mut sum_weighted_sq_error = 0.0f32; - let mut sum_weights = 0.0f32; - - for corr in correspondences.iter() { - let p = corr.point; - let line = &lines[corr.line_idx]; - let weight = corr.weight; - - // Transform point by current pose - let tx = p.x * cos - p.y * sin + pose.x; - let ty = p.x * sin + p.y * cos + pose.y; - let transformed = Point2D::new(tx, ty); - - // Get line normal - let normal = line.normal(); - - // Compute residual: signed distance from transformed point to line - let residual = line.signed_distance_to_point(transformed); - sum_weighted_sq_error += weight * residual * residual; - sum_weights += weight; - - // Jacobian of residual w.r.t. pose (x, y, theta) - let j0 = normal.x; - let j1 = normal.y; - let j2 = normal.x * (-p.x * sin - p.y * cos) + normal.y * (p.x * cos - p.y * sin); - - let jacobian = [j0, j1, j2]; - - // Accumulate H = J^T·W·J (weighted Hessian) - for i in 0..3 { - for j in 0..3 { - h[i][j] += weight * jacobian[i] * jacobian[j]; - } - } - - // Accumulate g = J^T·W·r (weighted gradient) - for i in 0..3 { - g[i] += weight * jacobian[i] * residual; - } - } - - let rms_error = if sum_weights > 0.0 { - (sum_weighted_sq_error / sum_weights).sqrt() - } else { - 0.0 - }; - - (h, g, rms_error) -} - -/// Compute covariance matrix from inverse Hessian. -/// -/// Cov = σ² × H⁻¹ -/// -/// Also computes condition number for degeneracy detection. -fn compute_covariance( - hessian: &[[f32; 3]; 3], - residual_variance: f32, - regularization: f32, -) -> (PoseCovariance, f32) { - // Add regularization to diagonal - let mut h = *hessian; - h[0][0] += regularization; - h[1][1] += regularization; - h[2][2] += regularization; - - // Compute determinant - let det = h[0][0] * (h[1][1] * h[2][2] - h[1][2] * h[2][1]) - - h[0][1] * (h[1][0] * h[2][2] - h[1][2] * h[2][0]) - + h[0][2] * (h[1][0] * h[2][1] - h[1][1] * h[2][0]); - - if det.abs() < 1e-10 { - // Singular matrix - return identity covariance with high condition number - return (IDENTITY_COVARIANCE, f32::MAX); - } - - let inv_det = 1.0 / det; - - // Compute inverse (H⁻¹) - let inv = [ - [ - (h[1][1] * h[2][2] - h[1][2] * h[2][1]) * inv_det, - (h[0][2] * h[2][1] - h[0][1] * h[2][2]) * inv_det, - (h[0][1] * h[1][2] - h[0][2] * h[1][1]) * inv_det, - ], - [ - (h[1][2] * h[2][0] - h[1][0] * h[2][2]) * inv_det, - (h[0][0] * h[2][2] - h[0][2] * h[2][0]) * inv_det, - (h[0][2] * h[1][0] - h[0][0] * h[1][2]) * inv_det, - ], - [ - (h[1][0] * h[2][1] - h[1][1] * h[2][0]) * inv_det, - (h[0][1] * h[2][0] - h[0][0] * h[2][1]) * inv_det, - (h[0][0] * h[1][1] - h[0][1] * h[1][0]) * inv_det, - ], - ]; - - // Scale by residual variance to get covariance - let covariance = [ - [ - residual_variance * inv[0][0], - residual_variance * inv[0][1], - residual_variance * inv[0][2], - ], - [ - residual_variance * inv[1][0], - residual_variance * inv[1][1], - residual_variance * inv[1][2], - ], - [ - residual_variance * inv[2][0], - residual_variance * inv[2][1], - residual_variance * inv[2][2], - ], - ]; - - // Compute condition number using Frobenius norm approximation - // Condition number ≈ max_eigenvalue / min_eigenvalue - // For 3x3, we can use a simpler approximation based on diagonal elements - let diag_max = h[0][0].max(h[1][1]).max(h[2][2]); - let diag_min = h[0][0].min(h[1][1]).min(h[2][2]); - let condition_number = if diag_min > 1e-10 { - diag_max / diag_min - } else { - f32::MAX - }; - - (covariance, condition_number) -} - -/// Compute covariance matrix with degrees-of-freedom correction. -/// -/// Uses unbiased variance estimate: σ² = Σw·r² / (n - 3) -/// where n is the number of correspondences and 3 is the number of parameters. -fn compute_covariance_with_dof( - hessian: &[[f32; 3]; 3], - sum_weighted_sq_residuals: f32, - num_correspondences: usize, - regularization: f32, -) -> (PoseCovariance, f32) { - // Degrees of freedom correction: n - 3 (3 parameters: x, y, theta) - let n = num_correspondences as f32; - let dof = (n - 3.0).max(1.0); - - // Unbiased variance estimate - let sigma_sq = sum_weighted_sq_residuals / dof; - - // Add regularization to diagonal - let mut h = *hessian; - h[0][0] += regularization; - h[1][1] += regularization; - h[2][2] += regularization; - - // Compute determinant - let det = h[0][0] * (h[1][1] * h[2][2] - h[1][2] * h[2][1]) - - h[0][1] * (h[1][0] * h[2][2] - h[1][2] * h[2][0]) - + h[0][2] * (h[1][0] * h[2][1] - h[1][1] * h[2][0]); - - if det.abs() < 1e-10 { - // Singular matrix - return identity covariance with high condition number - return (IDENTITY_COVARIANCE, f32::MAX); - } - - let inv_det = 1.0 / det; - - // Compute inverse (H⁻¹) - let inv = [ - [ - (h[1][1] * h[2][2] - h[1][2] * h[2][1]) * inv_det, - (h[0][2] * h[2][1] - h[0][1] * h[2][2]) * inv_det, - (h[0][1] * h[1][2] - h[0][2] * h[1][1]) * inv_det, - ], - [ - (h[1][2] * h[2][0] - h[1][0] * h[2][2]) * inv_det, - (h[0][0] * h[2][2] - h[0][2] * h[2][0]) * inv_det, - (h[0][2] * h[1][0] - h[0][0] * h[1][2]) * inv_det, - ], - [ - (h[1][0] * h[2][1] - h[1][1] * h[2][0]) * inv_det, - (h[0][1] * h[2][0] - h[0][0] * h[2][1]) * inv_det, - (h[0][0] * h[1][1] - h[0][1] * h[1][0]) * inv_det, - ], - ]; - - // Scale by unbiased variance to get covariance: Cov = σ² × H⁻¹ - let covariance = [ - [ - sigma_sq * inv[0][0], - sigma_sq * inv[0][1], - sigma_sq * inv[0][2], - ], - [ - sigma_sq * inv[1][0], - sigma_sq * inv[1][1], - sigma_sq * inv[1][2], - ], - [ - sigma_sq * inv[2][0], - sigma_sq * inv[2][1], - sigma_sq * inv[2][2], - ], - ]; - - // Compute condition number - let diag_max = h[0][0].max(h[1][1]).max(h[2][2]); - let diag_min = h[0][0].min(h[1][1]).min(h[2][2]); - let condition_number = if diag_min > 1e-10 { - diag_max / diag_min - } else { - f32::MAX - }; - - (covariance, condition_number) -} - -/// Solve a 3x3 linear system Ax = b using Cramer's rule. -/// -/// Returns None if the matrix is singular. -fn solve_3x3(a: &[[f32; 3]; 3], b: &[f32; 3], regularization: f32) -> Option<[f32; 3]> { - // Add regularization to diagonal - let mut ar = *a; - ar[0][0] += regularization; - ar[1][1] += regularization; - ar[2][2] += regularization; - - // Compute determinant - let det = ar[0][0] * (ar[1][1] * ar[2][2] - ar[1][2] * ar[2][1]) - - ar[0][1] * (ar[1][0] * ar[2][2] - ar[1][2] * ar[2][0]) - + ar[0][2] * (ar[1][0] * ar[2][1] - ar[1][1] * ar[2][0]); - - if det.abs() < 1e-10 { - return None; - } - - let inv_det = 1.0 / det; - - // Compute inverse using cofactors - let inv = [ - [ - (ar[1][1] * ar[2][2] - ar[1][2] * ar[2][1]) * inv_det, - (ar[0][2] * ar[2][1] - ar[0][1] * ar[2][2]) * inv_det, - (ar[0][1] * ar[1][2] - ar[0][2] * ar[1][1]) * inv_det, - ], - [ - (ar[1][2] * ar[2][0] - ar[1][0] * ar[2][2]) * inv_det, - (ar[0][0] * ar[2][2] - ar[0][2] * ar[2][0]) * inv_det, - (ar[0][2] * ar[1][0] - ar[0][0] * ar[1][2]) * inv_det, - ], - [ - (ar[1][0] * ar[2][1] - ar[1][1] * ar[2][0]) * inv_det, - (ar[0][1] * ar[2][0] - ar[0][0] * ar[2][1]) * inv_det, - (ar[0][0] * ar[1][1] - ar[0][1] * ar[1][0]) * inv_det, - ], - ]; - - // x = A^(-1) · (-b) (negative because we want to minimize) - Some([ - -(inv[0][0] * b[0] + inv[0][1] * b[1] + inv[0][2] * b[2]), - -(inv[1][0] * b[0] + inv[1][1] * b[1] + inv[1][2] * b[2]), - -(inv[2][0] * b[0] + inv[2][1] * b[1] + inv[2][2] * b[2]), - ]) -} - -/// Compute RMS residual for current pose. -fn compute_residual(correspondences: &CorrespondenceSet, lines: &[Line2D], pose: &Pose2D) -> f32 { - if correspondences.is_empty() { - return 0.0; - } - - let (sin, cos) = pose.theta.sin_cos(); - let mut sum_sq = 0.0; - - for corr in correspondences.iter() { - let p = corr.point; - let line = &lines[corr.line_idx]; - - // Transform point - let tx = p.x * cos - p.y * sin + pose.x; - let ty = p.x * sin + p.y * cos + pose.y; - let transformed = Point2D::new(tx, ty); - - // Compute distance - let dist = line.distance_to_point(transformed); - sum_sq += dist * dist; - } - - (sum_sq / correspondences.len() as f32).sqrt() -} - -// ============================================================================ -// Fast versions using pre-computed LineCollection normals -// ============================================================================ - -/// Optimize pose using Levenberg-Marquardt with pre-computed normals. -/// -/// This version uses `LineCollection` with pre-computed normals and inverse -/// lengths, avoiding redundant sqrt computations in the inner loop. -/// Uses LM damping for robust convergence from poor initial guesses. -/// -/// # Algorithm -/// -/// LM adds damping (λI) to the Hessian diagonal: -/// - Good step (error decreases): accept step, decrease λ -/// - Bad step (error increases): reject step, increase λ -/// -/// Set `config.lm_initial_lambda = 0.0` to disable LM (pure Gauss-Newton). -/// -/// # Performance -/// -/// ~15-20% faster than `optimize_pose` for typical correspondence sets. -pub fn optimize_pose_fast( - correspondences: &CorrespondenceSet, - lines: &LineCollection, - initial_pose: Pose2D, - config: &GaussNewtonConfig, -) -> GaussNewtonResult { - if correspondences.is_empty() { - return GaussNewtonResult { - pose: initial_pose, - iterations: 0, - residual: 0.0, - converged: true, - covariance: IDENTITY_COVARIANCE, - condition_number: 1.0, - }; - } - - // Pre-fetch normals for all lines used in correspondences - let (normal_xs, normal_ys) = lines.normals(); - let inv_lengths = lines.inv_lengths(); - - let mut pose = initial_pose; - let mut converged = false; - let mut iterations = 0; - let mut final_hessian = [[0.0f32; 3]; 3]; - let mut final_sum_weighted_sq = 0.0f32; - - // LM state - let use_lm = config.lm_initial_lambda > 0.0; - let mut lambda = config.lm_initial_lambda; - let mut prev_residual = if use_lm { - compute_residual_fast(correspondences, lines, inv_lengths, &pose) - } else { - f32::MAX - }; - - for iter in 0..config.max_iterations { - iterations = iter + 1; - - // Build weighted linear system using pre-computed normals - let (h, g, sum_weighted_sq, _sum_weights) = build_linear_system_fast_weighted_extended( - correspondences, - lines, - normal_xs, - normal_ys, - inv_lengths, - &pose, - ); - final_hessian = h; - final_sum_weighted_sq = sum_weighted_sq; - - // Add LM damping to Hessian diagonal - let mut h_damped = h; - if use_lm { - h_damped[0][0] += lambda; - h_damped[1][1] += lambda; - h_damped[2][2] += lambda; - } - - // Solve for Δx using direct 3x3 solve - let Some(delta) = solve_3x3(&h_damped, &g, config.regularization) else { - break; - }; - - // Compute trial pose - let trial_pose = Pose2D::new( - pose.x + delta[0], - pose.y + delta[1], - crate::core::math::normalize_angle(pose.theta + delta[2]), - ); - - if use_lm { - // Evaluate step quality - let trial_residual = - compute_residual_fast(correspondences, lines, inv_lengths, &trial_pose); - - if trial_residual < prev_residual { - // Good step - accept and decrease lambda - pose = trial_pose; - prev_residual = trial_residual; - lambda = (lambda / config.lm_lambda_factor).max(config.lm_min_lambda); - - // Check convergence - let delta_norm_sq = delta[0] * delta[0] + delta[1] * delta[1] + delta[2] * delta[2]; - if delta_norm_sq < config.convergence_threshold_sq { - converged = true; - break; - } - } else { - // Bad step - reject and increase lambda - lambda *= config.lm_lambda_factor; - if lambda > config.lm_max_lambda { - // Failed to converge - break; - } - // Don't update pose, try again with higher damping - continue; - } - } else { - // Pure Gauss-Newton (no LM damping) - pose = trial_pose; - - // Check convergence - let delta_norm_sq = delta[0] * delta[0] + delta[1] * delta[1] + delta[2] * delta[2]; - if delta_norm_sq < config.convergence_threshold_sq { - converged = true; - break; - } - } - } - - let final_residual = compute_residual_fast(correspondences, lines, inv_lengths, &pose); - - // Compute covariance with DOF correction - let (covariance, condition_number) = compute_covariance_with_dof( - &final_hessian, - final_sum_weighted_sq, - correspondences.len(), - config.regularization, - ); - - GaussNewtonResult { - pose, - iterations, - residual: final_residual, - converged, - covariance, - condition_number, - } -} - -/// Build weighted linear system using pre-computed normals. -/// Returns (H, g, sum_weighted_sq_error, sum_weights) for covariance computation. -fn build_linear_system_fast_weighted_extended( - correspondences: &CorrespondenceSet, - lines: &LineCollection, - normal_xs: &[f32], - normal_ys: &[f32], - inv_lengths: &[f32], - pose: &Pose2D, -) -> ([[f32; 3]; 3], [f32; 3], f32, f32) { - let (sin, cos) = pose.theta.sin_cos(); - - // Pre-compute -sin and -cos for Jacobian - let neg_sin = -sin; - let neg_cos = -cos; - - let mut h = [[0.0f32; 3]; 3]; - let mut g = [0.0f32; 3]; - let mut sum_weighted_sq_error = 0.0f32; - let mut sum_weights = 0.0f32; - - for corr in correspondences.iter() { - let p = corr.point; - let line_idx = corr.line_idx; - let weight = corr.weight; - - // Transform point by current pose - let tx = p.x * cos - p.y * sin + pose.x; - let ty = p.x * sin + p.y * cos + pose.y; - - // Get pre-computed normal (no sqrt!) - let nx = normal_xs[line_idx]; - let ny = normal_ys[line_idx]; - - // Compute residual - let sx = lines.start_xs[line_idx]; - let sy = lines.start_ys[line_idx]; - let ex = lines.end_xs[line_idx]; - let ey = lines.end_ys[line_idx]; - - let dx = ex - sx; - let dy = ey - sy; - let to_px = tx - sx; - let to_py = ty - sy; - - let cross = dx * to_py - dy * to_px; - let residual = cross * inv_lengths[line_idx]; - - sum_weighted_sq_error += weight * residual * residual; - sum_weights += weight; - - // Jacobian using pre-computed normal - let j0 = nx; - let j1 = ny; - let j2 = nx * (p.x * neg_sin + p.y * neg_cos) + ny * (p.x * cos + p.y * neg_sin); - - // Accumulate weighted H = J^T·W·J (symmetric) - h[0][0] += weight * j0 * j0; - h[0][1] += weight * j0 * j1; - h[0][2] += weight * j0 * j2; - h[1][1] += weight * j1 * j1; - h[1][2] += weight * j1 * j2; - h[2][2] += weight * j2 * j2; - - // Accumulate weighted g = J^T·W·r - g[0] += weight * j0 * residual; - g[1] += weight * j1 * residual; - g[2] += weight * j2 * residual; - } - - // Fill symmetric entries - h[1][0] = h[0][1]; - h[2][0] = h[0][2]; - h[2][1] = h[1][2]; - - (h, g, sum_weighted_sq_error, sum_weights) -} - -/// Compute RMS residual using pre-computed inverse lengths. -fn compute_residual_fast( - correspondences: &CorrespondenceSet, - lines: &LineCollection, - inv_lengths: &[f32], - pose: &Pose2D, -) -> f32 { - if correspondences.is_empty() { - return 0.0; - } - - let (sin, cos) = pose.theta.sin_cos(); - let mut sum_sq = 0.0; - - for corr in correspondences.iter() { - let p = corr.point; - let line_idx = corr.line_idx; - - // Transform point - let tx = p.x * cos - p.y * sin + pose.x; - let ty = p.x * sin + p.y * cos + pose.y; - - // Compute distance using pre-computed values - let sx = lines.start_xs[line_idx]; - let sy = lines.start_ys[line_idx]; - let ex = lines.end_xs[line_idx]; - let ey = lines.end_ys[line_idx]; - - let dx = ex - sx; - let dy = ey - sy; - let to_px = tx - sx; - let to_py = ty - sy; - - let cross = dx * to_py - dy * to_px; - let dist = cross.abs() * inv_lengths[line_idx]; - - sum_sq += dist * dist; - } - - (sum_sq / correspondences.len() as f32).sqrt() -} - -// ============================================================================ -// SIMD-Optimized Version using CorrespondenceSoA -// ============================================================================ - -/// Optimize pose using SIMD-accelerated Gauss-Newton with SoA correspondences. -/// -/// This is the fastest variant, using [`CorrespondenceSoA`] which inlines all -/// line data for contiguous SIMD loads. Provides ~30-40% speedup over -/// `optimize_pose_fast` by eliminating indexed array access. -/// -/// # Arguments -/// * `correspondences` - SoA correspondences (must be `finalize()`d) -/// * `initial_pose` - Initial pose estimate -/// * `config` - Solver configuration -/// -/// # Example -/// -/// ```rust,ignore -/// let mut soa = CorrespondenceSoA::new(); -/// // ... populate correspondences ... -/// soa.finalize(); -/// -/// let result = optimize_pose_simd(&soa, Pose2D::identity(), &GaussNewtonConfig::default()); -/// ``` -pub fn optimize_pose_simd( - correspondences: &CorrespondenceSoA, - initial_pose: Pose2D, - config: &GaussNewtonConfig, -) -> GaussNewtonResult { - if correspondences.is_empty() { - return GaussNewtonResult { - pose: initial_pose, - iterations: 0, - residual: 0.0, - converged: true, - covariance: IDENTITY_COVARIANCE, - condition_number: 1.0, - }; - } - - let mut pose = initial_pose; - let mut converged = false; - let mut iterations = 0; - let mut final_hessian = [[0.0f32; 3]; 3]; - let mut final_sum_weighted_sq = 0.0f32; - - // LM state - let use_lm = config.lm_initial_lambda > 0.0; - let mut lambda = config.lm_initial_lambda; - let mut prev_residual = if use_lm { - compute_residual_simd(correspondences, &pose) - } else { - f32::MAX - }; - - for iter in 0..config.max_iterations { - iterations = iter + 1; - - // Build weighted linear system using SIMD - let (h, g, sum_weighted_sq, _sum_weights) = - build_linear_system_simd(correspondences, &pose); - final_hessian = h; - final_sum_weighted_sq = sum_weighted_sq; - - // Add LM damping to Hessian diagonal - let mut h_damped = h; - if use_lm { - h_damped[0][0] += lambda; - h_damped[1][1] += lambda; - h_damped[2][2] += lambda; - } - - // Solve for Δx - let Some(delta) = solve_3x3(&h_damped, &g, config.regularization) else { - break; - }; - - // Compute trial pose - let trial_pose = Pose2D::new( - pose.x + delta[0], - pose.y + delta[1], - crate::core::math::normalize_angle(pose.theta + delta[2]), - ); - - if use_lm { - let trial_residual = compute_residual_simd(correspondences, &trial_pose); - - if trial_residual < prev_residual { - pose = trial_pose; - prev_residual = trial_residual; - lambda = (lambda / config.lm_lambda_factor).max(config.lm_min_lambda); - - let delta_norm_sq = delta[0] * delta[0] + delta[1] * delta[1] + delta[2] * delta[2]; - if delta_norm_sq < config.convergence_threshold_sq { - converged = true; - break; - } - } else { - lambda *= config.lm_lambda_factor; - if lambda > config.lm_max_lambda { - break; - } - continue; - } - } else { - pose = trial_pose; - - let delta_norm_sq = delta[0] * delta[0] + delta[1] * delta[1] + delta[2] * delta[2]; - if delta_norm_sq < config.convergence_threshold_sq { - converged = true; - break; - } - } - } - - let final_residual = compute_residual_simd(correspondences, &pose); - - let (covariance, condition_number) = compute_covariance_with_dof( - &final_hessian, - final_sum_weighted_sq, - correspondences.len(), - config.regularization, - ); - - GaussNewtonResult { - pose, - iterations, - residual: final_residual, - converged, - covariance, - condition_number, - } -} - -/// Build weighted linear system using SIMD with SoA correspondences. -/// -/// Processes 4 correspondences per iteration using f32x4 operations. -/// Returns (H, g, sum_weighted_sq_error, sum_weights). -fn build_linear_system_simd( - correspondences: &CorrespondenceSoA, - pose: &Pose2D, -) -> ([[f32; 3]; 3], [f32; 3], f32, f32) { - let (sin, cos) = pose.theta.sin_cos(); - let neg_sin = -sin; - let neg_cos = -cos; - - // SIMD constants - let cos4 = f32x4::splat(cos); - let sin4 = f32x4::splat(sin); - let neg_sin4 = f32x4::splat(neg_sin); - let neg_cos4 = f32x4::splat(neg_cos); - let pose_x4 = f32x4::splat(pose.x); - let pose_y4 = f32x4::splat(pose.y); - - // Accumulators for Hessian (6 unique elements for symmetric 3x3) - let mut h00 = f32x4::splat(0.0); - let mut h01 = f32x4::splat(0.0); - let mut h02 = f32x4::splat(0.0); - let mut h11 = f32x4::splat(0.0); - let mut h12 = f32x4::splat(0.0); - let mut h22 = f32x4::splat(0.0); - - // Accumulators for gradient - let mut g0 = f32x4::splat(0.0); - let mut g1 = f32x4::splat(0.0); - let mut g2 = f32x4::splat(0.0); - - // Error accumulators - let mut sum_wsq4 = f32x4::splat(0.0); - let mut sum_w4 = f32x4::splat(0.0); - - let chunks = correspondences.simd_chunks(); - - for i in 0..chunks { - let base = i * 4; - - // Load 4 points (contiguous SIMD loads!) - let px = f32x4::from_slice(&correspondences.point_xs[base..]); - let py = f32x4::from_slice(&correspondences.point_ys[base..]); - - // Load inlined line data (contiguous!) - let nx = f32x4::from_slice(&correspondences.normal_xs[base..]); - let ny = f32x4::from_slice(&correspondences.normal_ys[base..]); - let sx = f32x4::from_slice(&correspondences.line_start_xs[base..]); - let sy = f32x4::from_slice(&correspondences.line_start_ys[base..]); - let ex = f32x4::from_slice(&correspondences.line_end_xs[base..]); - let ey = f32x4::from_slice(&correspondences.line_end_ys[base..]); - let inv_len = f32x4::from_slice(&correspondences.line_inv_lengths[base..]); - let weight = f32x4::from_slice(&correspondences.weights[base..]); - - // Transform points: tx = px*cos - py*sin + pose.x - let tx = px * cos4 - py * sin4 + pose_x4; - let ty = px * sin4 + py * cos4 + pose_y4; - - // Compute residual: cross / length - let dx = ex - sx; - let dy = ey - sy; - let to_px = tx - sx; - let to_py = ty - sy; - let cross = dx * to_py - dy * to_px; - let residual = cross * inv_len; - - // Accumulate weighted squared error - sum_wsq4 += weight * residual * residual; - sum_w4 += weight; - - // Jacobian components - let j0 = nx; - let j1 = ny; - // j2 = nx * (px * -sin + py * -cos) + ny * (px * cos + py * -sin) - let j2 = nx * (px * neg_sin4 + py * neg_cos4) + ny * (px * cos4 + py * neg_sin4); - - // Weighted Jacobian - let wj0 = weight * j0; - let wj1 = weight * j1; - let wj2 = weight * j2; - - // Accumulate Hessian: H += J^T * W * J - h00 += wj0 * j0; - h01 += wj0 * j1; - h02 += wj0 * j2; - h11 += wj1 * j1; - h12 += wj1 * j2; - h22 += wj2 * j2; - - // Accumulate gradient: g += J^T * W * r - g0 += wj0 * residual; - g1 += wj1 * residual; - g2 += wj2 * residual; - } - - // Horizontal reduction - let h = [ - [h00.reduce_sum(), h01.reduce_sum(), h02.reduce_sum()], - [h01.reduce_sum(), h11.reduce_sum(), h12.reduce_sum()], - [h02.reduce_sum(), h12.reduce_sum(), h22.reduce_sum()], - ]; - - let g = [g0.reduce_sum(), g1.reduce_sum(), g2.reduce_sum()]; - - (h, g, sum_wsq4.reduce_sum(), sum_w4.reduce_sum()) -} - -/// Compute RMS residual using SIMD with SoA correspondences. -fn compute_residual_simd(correspondences: &CorrespondenceSoA, pose: &Pose2D) -> f32 { - if correspondences.is_empty() { - return 0.0; - } - - let (sin, cos) = pose.theta.sin_cos(); - let cos4 = f32x4::splat(cos); - let sin4 = f32x4::splat(sin); - let pose_x4 = f32x4::splat(pose.x); - let pose_y4 = f32x4::splat(pose.y); - - let mut sum_sq4 = f32x4::splat(0.0); - let chunks = correspondences.simd_chunks(); - - for i in 0..chunks { - let base = i * 4; - - let px = f32x4::from_slice(&correspondences.point_xs[base..]); - let py = f32x4::from_slice(&correspondences.point_ys[base..]); - let sx = f32x4::from_slice(&correspondences.line_start_xs[base..]); - let sy = f32x4::from_slice(&correspondences.line_start_ys[base..]); - let ex = f32x4::from_slice(&correspondences.line_end_xs[base..]); - let ey = f32x4::from_slice(&correspondences.line_end_ys[base..]); - let inv_len = f32x4::from_slice(&correspondences.line_inv_lengths[base..]); - let weight = f32x4::from_slice(&correspondences.weights[base..]); - - // Transform point - let tx = px * cos4 - py * sin4 + pose_x4; - let ty = px * sin4 + py * cos4 + pose_y4; - - // Compute distance - let dx = ex - sx; - let dy = ey - sy; - let to_px = tx - sx; - let to_py = ty - sy; - let cross = dx * to_py - dy * to_px; - let dist = cross.abs() * inv_len; - - // Only count non-zero weight entries (padding has weight=0) - let mask = weight; // Use weight as mask (0 for padding) - sum_sq4 += mask * dist * dist; - } - - (sum_sq4.reduce_sum() / correspondences.len() as f32).sqrt() -} - -#[cfg(test)] -mod tests { - use super::*; - use crate::core::Point2D; - use crate::matching::correspondence::Correspondence; - use approx::assert_relative_eq; - - #[test] - fn test_optimize_translation_only() { - // Single horizontal line at y=0 - let lines = vec![Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(10.0, 0.0))]; - - // Points that are offset from the line - let mut corrs = CorrespondenceSet::new(); - corrs.push(Correspondence::new(0, 0, Point2D::new(1.0, 0.5), 0.5, 0.1)); - corrs.push(Correspondence::new(1, 0, Point2D::new(3.0, 0.5), 0.5, 0.3)); - corrs.push(Correspondence::new(2, 0, Point2D::new(5.0, 0.5), 0.5, 0.5)); - - let config = GaussNewtonConfig::default(); - let result = optimize_pose(&corrs, &lines, Pose2D::identity(), &config); - - // Should find translation to move points down to the line - assert!(result.converged); - assert_relative_eq!(result.pose.y, -0.5, epsilon = 0.1); - assert_relative_eq!(result.pose.x, 0.0, epsilon = 0.1); - } - - #[test] - fn test_optimize_two_lines() { - // Two perpendicular lines - let lines = vec![ - Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(10.0, 0.0)), // Horizontal - Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(0.0, 10.0)), // Vertical - ]; - - // Points offset from both lines - let mut corrs = CorrespondenceSet::new(); - corrs.push(Correspondence::new(0, 0, Point2D::new(2.0, 0.3), 0.3, 0.2)); - corrs.push(Correspondence::new(1, 0, Point2D::new(5.0, 0.3), 0.3, 0.5)); - corrs.push(Correspondence::new(2, 1, Point2D::new(0.3, 2.0), 0.3, 0.2)); - corrs.push(Correspondence::new(3, 1, Point2D::new(0.3, 5.0), 0.3, 0.5)); - - let config = GaussNewtonConfig::default(); - let result = optimize_pose(&corrs, &lines, Pose2D::identity(), &config); - - assert!(result.converged); - // Should translate points toward both lines - assert!(result.pose.x < 0.0); // Move left - assert!(result.pose.y < 0.0); // Move down - } - - #[test] - fn test_optimize_empty_correspondences() { - let lines = vec![Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(10.0, 0.0))]; - let corrs = CorrespondenceSet::new(); - - let initial = Pose2D::new(1.0, 2.0, 0.5); - let config = GaussNewtonConfig::default(); - let result = optimize_pose(&corrs, &lines, initial, &config); - - // Should return initial pose unchanged - assert_eq!(result.pose.x, initial.x); - assert_eq!(result.pose.y, initial.y); - assert_eq!(result.pose.theta, initial.theta); - assert!(result.converged); - } - - #[test] - fn test_solve_3x3() { - // Simple identity system - let a = [[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]]; - let b = [1.0, 2.0, 3.0]; - - let x = solve_3x3(&a, &b, 0.0).unwrap(); - - assert_relative_eq!(x[0], -1.0, epsilon = 1e-6); - assert_relative_eq!(x[1], -2.0, epsilon = 1e-6); - assert_relative_eq!(x[2], -3.0, epsilon = 1e-6); - } - - #[test] - fn test_solve_3x3_with_regularization() { - // Nearly singular matrix - let a = [[1e-8, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]]; - let b = [1.0, 2.0, 3.0]; - - // Without regularization might fail - let x = solve_3x3(&a, &b, 1e-4); - assert!(x.is_some()); - } - - #[test] - fn test_compute_residual() { - let lines = vec![Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(10.0, 0.0))]; - - let mut corrs = CorrespondenceSet::new(); - corrs.push(Correspondence::new(0, 0, Point2D::new(1.0, 0.1), 0.1, 0.1)); - corrs.push(Correspondence::new(1, 0, Point2D::new(5.0, 0.1), 0.1, 0.5)); - - let residual = compute_residual(&corrs, &lines, &Pose2D::identity()); - - // RMS of [0.1, 0.1] = sqrt(0.02/2) = 0.1 - assert_relative_eq!(residual, 0.1, epsilon = 0.01); - } - - #[test] - fn test_convergence() { - let lines = vec![Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(10.0, 0.0))]; - - // Points already on the line - should converge immediately - let mut corrs = CorrespondenceSet::new(); - corrs.push(Correspondence::new(0, 0, Point2D::new(1.0, 0.0), 0.0, 0.1)); - corrs.push(Correspondence::new(1, 0, Point2D::new(5.0, 0.0), 0.0, 0.5)); - - let config = GaussNewtonConfig::default(); - let result = optimize_pose(&corrs, &lines, Pose2D::identity(), &config); - - assert!(result.converged); - assert!(result.iterations <= 2); // Should converge quickly - assert!(result.residual < 1e-5); - } -} diff --git a/vastu-map/src/matching/icp/config.rs b/vastu-map/src/matching/icp/config.rs deleted file mode 100644 index 6532104..0000000 --- a/vastu-map/src/matching/icp/config.rs +++ /dev/null @@ -1,428 +0,0 @@ -//! Configuration for Point-to-Line ICP algorithm. - -use serde::{Deserialize, Serialize}; - -use crate::config::LidarNoiseModel; - -use super::super::robust::RobustCostFunction; - -/// Configuration for Point-to-Line ICP. -#[derive(Clone, Debug, Serialize, Deserialize)] -pub struct IcpConfig { - /// Maximum number of ICP iterations. - /// Default: 30 - pub max_iterations: usize, - - /// Convergence threshold for pose change. - /// If ‖Δpose‖ < threshold, ICP stops. - /// Default: 1e-4 - pub convergence_threshold: f32, - - /// Pre-computed squared convergence threshold for efficient comparison. - /// Avoids sqrt() in hot loop by comparing squared distances. - #[serde(skip)] - pub(crate) convergence_threshold_sq: f32, - - /// Maximum correspondence distance (meters). - /// Points farther than this from all lines are rejected. - /// Default: 0.5m - pub max_correspondence_distance: f32, - - /// Robust cost function for outlier handling. - /// Uses M-estimators to down-weight outliers based on residual magnitude. - /// Default: Huber with delta = 0.03 (3cm) - pub robust_cost: RobustCostFunction, - - /// Enable Iteratively Reweighted Least Squares (IRLS). - /// When true, correspondence weights are updated each iteration - /// based on residuals and the robust cost function. - /// Default: true - pub use_irls: bool, - - /// Minimum number of correspondences required. - /// Default: 10 - pub min_correspondences: usize, - - /// Minimum match confidence to consider successful. - /// Default: 0.3 - pub min_confidence: f32, - - /// Whether to use RANSAC for initial pose estimate. - /// Default: true - pub use_ransac_init: bool, - - /// Whether to use batch (SIMD) correspondence search. - /// Default: true - pub use_batch_search: bool, - - /// Lidar noise model for correspondence weighting. - /// Used to weight measurements by their estimated uncertainty. - pub noise_model: LidarNoiseModel, - - /// Whether to use coarse search when odometry is unreliable. - /// Default: false - pub use_coarse_search: bool, - - /// Confidence threshold below which coarse search is triggered. - /// Only used if use_coarse_search is true. - /// Default: 0.5 - pub coarse_search_confidence_threshold: f32, - - /// Multi-resolution ICP configuration (coarse-to-fine matching). - /// When enabled, matching starts with subsampled points and refines - /// with progressively more points for better convergence. - pub multi_resolution: MultiResolutionConfig, -} - -impl Default for IcpConfig { - fn default() -> Self { - let convergence_threshold = 1e-4; - Self { - max_iterations: 30, - convergence_threshold, - convergence_threshold_sq: convergence_threshold * convergence_threshold, - max_correspondence_distance: 0.5, - robust_cost: RobustCostFunction::default(), // Huber with delta = 0.03 - use_irls: true, - min_correspondences: 10, - min_confidence: 0.3, - use_ransac_init: true, - use_batch_search: true, - noise_model: LidarNoiseModel::default(), - use_coarse_search: true, // Enabled: triggers when previous match confidence < threshold - coarse_search_confidence_threshold: 0.5, - multi_resolution: MultiResolutionConfig::default(), - } - } -} - -impl IcpConfig { - /// Create a new configuration with default values. - pub fn new() -> Self { - Self::default() - } - - /// Recompute derived fields after deserialization. - /// - /// Call this after loading from YAML to ensure computed fields are correct. - pub fn finalize(&mut self) { - self.convergence_threshold_sq = self.convergence_threshold * self.convergence_threshold; - } - - // ===== Preset Configurations ===== - - /// Fast preset: Optimized for speed at the cost of some accuracy. - /// - /// Good for: - /// - Real-time applications with tight timing constraints - /// - Situations where odometry is reliable - /// - High-frequency (>10Hz) scan matching - /// - /// Trade-offs: - /// - Fewer iterations may not converge for poor initial guesses - /// - No multi-resolution (faster but less robust) - /// - Larger correspondence distance (fewer false negatives) - pub fn fast() -> Self { - let convergence_threshold = 5e-4; - Self { - max_iterations: 15, - convergence_threshold, - convergence_threshold_sq: convergence_threshold * convergence_threshold, - max_correspondence_distance: 0.6, - robust_cost: RobustCostFunction::Huber { delta: 0.05 }, - use_irls: false, // Skip IRLS for speed - min_correspondences: 8, - min_confidence: 0.25, - use_ransac_init: false, // Trust odometry - use_batch_search: true, - noise_model: LidarNoiseModel::default(), - use_coarse_search: false, - coarse_search_confidence_threshold: 0.5, - multi_resolution: MultiResolutionConfig { - enabled: false, - ..Default::default() - }, - } - } - - /// Accurate preset: Optimized for accuracy at the cost of speed. - /// - /// Good for: - /// - Mapping applications where precision matters - /// - Situations with poor odometry - /// - Loop closure verification - /// - /// Trade-offs: - /// - More iterations = longer compute time - /// - Tighter thresholds may cause false rejections - pub fn accurate() -> Self { - let convergence_threshold = 1e-5; - Self { - max_iterations: 50, - convergence_threshold, - convergence_threshold_sq: convergence_threshold * convergence_threshold, - max_correspondence_distance: 0.4, - robust_cost: RobustCostFunction::Tukey { c: 0.02 }, // Aggressive outlier rejection - use_irls: true, - min_correspondences: 15, - min_confidence: 0.4, - use_ransac_init: true, - use_batch_search: true, - noise_model: LidarNoiseModel::default(), - use_coarse_search: true, - coarse_search_confidence_threshold: 0.6, - multi_resolution: MultiResolutionConfig { - enabled: true, - levels: 3, - subsample_factor: 2, - iterations_per_level: vec![5, 10, 15], - }, - } - } - - /// Balanced preset: Good trade-off between speed and accuracy. - /// - /// This is the same as `default()` but with a more descriptive name. - /// Suitable for most indoor navigation applications. - pub fn balanced() -> Self { - Self::default() - } - - /// Robust preset: Optimized for challenging environments with outliers. - /// - /// Good for: - /// - Environments with dynamic obstacles - /// - Cluttered or partially occluded scenes - /// - Glass walls or reflective surfaces - /// - /// Uses aggressive outlier rejection but maintains accuracy. - pub fn robust() -> Self { - let convergence_threshold = 1e-4; - Self { - max_iterations: 40, - convergence_threshold, - convergence_threshold_sq: convergence_threshold * convergence_threshold, - max_correspondence_distance: 0.5, - robust_cost: RobustCostFunction::GemanMcClure { scale: 0.02 }, // Heavy outlier rejection - use_irls: true, - min_correspondences: 12, - min_confidence: 0.35, - use_ransac_init: true, - use_batch_search: true, - noise_model: LidarNoiseModel::default(), - use_coarse_search: true, - coarse_search_confidence_threshold: 0.5, - multi_resolution: MultiResolutionConfig::default(), - } - } - - /// Builder-style setter for maximum iterations. - pub fn with_max_iterations(mut self, iterations: usize) -> Self { - self.max_iterations = iterations; - self - } - - /// Builder-style setter for convergence threshold. - pub fn with_convergence_threshold(mut self, threshold: f32) -> Self { - self.convergence_threshold = threshold; - self.convergence_threshold_sq = threshold * threshold; - self - } - - /// Builder-style setter for maximum correspondence distance. - pub fn with_max_correspondence_distance(mut self, meters: f32) -> Self { - self.max_correspondence_distance = meters; - self - } - - /// Builder-style setter for robust cost function. - pub fn with_robust_cost(mut self, cost: RobustCostFunction) -> Self { - self.robust_cost = cost; - self - } - - /// Builder-style setter for IRLS (Iteratively Reweighted Least Squares). - pub fn with_irls(mut self, enabled: bool) -> Self { - self.use_irls = enabled; - self - } - - /// Builder-style setter for batch search. - pub fn with_batch_search(mut self, enabled: bool) -> Self { - self.use_batch_search = enabled; - self - } - - /// Builder-style setter for minimum correspondences. - pub fn with_min_correspondences(mut self, count: usize) -> Self { - self.min_correspondences = count; - self - } - - /// Builder-style setter for RANSAC initialization. - pub fn with_ransac_init(mut self, enabled: bool) -> Self { - self.use_ransac_init = enabled; - self - } - - /// Builder-style setter for lidar noise model. - pub fn with_noise_model(mut self, model: LidarNoiseModel) -> Self { - self.noise_model = model; - self - } - - /// Builder-style setter for coarse search. - pub fn with_coarse_search(mut self, enabled: bool) -> Self { - self.use_coarse_search = enabled; - self - } - - /// Builder-style setter for coarse search confidence threshold. - pub fn with_coarse_search_threshold(mut self, threshold: f32) -> Self { - self.coarse_search_confidence_threshold = threshold; - self - } - - /// Builder-style setter for multi-resolution configuration. - pub fn with_multi_resolution(mut self, config: MultiResolutionConfig) -> Self { - self.multi_resolution = config; - self - } - - /// Disable multi-resolution matching. - pub fn without_multi_resolution(mut self) -> Self { - self.multi_resolution.enabled = false; - self - } -} - -/// Configuration for coarse search initialization. -#[derive(Clone, Debug, Serialize, Deserialize)] -pub struct CoarseSearchConfig { - /// Maximum translation search range (meters). - /// Search from -range to +range in both x and y. - /// Default: 0.3m - pub translation_range: f32, - - /// Translation step size (meters). - /// Default: 0.05m (5cm) - pub translation_step: f32, - - /// Maximum rotation search range (radians). - /// Search from -range to +range. - /// Default: 0.262 rad (~15°) - pub rotation_range: f32, - - /// Rotation step size (radians). - /// Default: 0.052 rad (~3°) - pub rotation_step: f32, - - /// Point subsampling rate for fast scoring. - /// Only use every Nth point for initial evaluation. - /// Default: 4 - pub subsample_rate: usize, -} - -impl Default for CoarseSearchConfig { - fn default() -> Self { - Self { - translation_range: 0.3, - translation_step: 0.05, - rotation_range: 0.262, // ~15° - rotation_step: 0.052, // ~3° - subsample_rate: 4, - } - } -} - -/// Configuration for multi-resolution ICP (coarse-to-fine matching). -/// -/// Multi-resolution ICP improves convergence by starting with subsampled -/// points (coarse) and progressively refining with more points (fine). -/// This helps escape local minima when the initial guess is poor. -#[derive(Clone, Debug, Serialize, Deserialize)] -pub struct MultiResolutionConfig { - /// Enable multi-resolution matching. - /// Default: true - pub enabled: bool, - - /// Number of pyramid levels. - /// Level 0 is coarsest, level (levels-1) is finest (full resolution). - /// Default: 3 - pub levels: usize, - - /// Subsampling factor between levels. - /// Points at level i use every (subsample_factor^(levels-1-i)) points. - /// Example with levels=3, factor=2: coarse=1/4, medium=1/2, fine=full - /// Default: 2 - pub subsample_factor: usize, - - /// ICP iterations at each level (from coarsest to finest). - /// Length should match `levels`. If shorter, last value is repeated. - /// Default: [3, 5, 10] - pub iterations_per_level: Vec, -} - -impl Default for MultiResolutionConfig { - fn default() -> Self { - Self { - enabled: true, - levels: 3, - subsample_factor: 2, - iterations_per_level: vec![3, 5, 10], - } - } -} - -impl MultiResolutionConfig { - /// Create a new multi-resolution config with default values. - pub fn new() -> Self { - Self::default() - } - - /// Create an enabled config with default pyramid settings. - pub fn enabled() -> Self { - Self { - enabled: true, - ..Self::default() - } - } - - /// Builder-style setter for number of levels. - pub fn with_levels(mut self, levels: usize) -> Self { - self.levels = levels.max(1); - self - } - - /// Builder-style setter for subsample factor. - pub fn with_subsample_factor(mut self, factor: usize) -> Self { - self.subsample_factor = factor.max(1); - self - } - - /// Builder-style setter for iterations per level. - pub fn with_iterations(mut self, iterations: Vec) -> Self { - self.iterations_per_level = iterations; - self - } - - /// Get iterations for a given level (0 = coarsest). - pub fn iterations_for_level(&self, level: usize) -> usize { - if level < self.iterations_per_level.len() { - self.iterations_per_level[level] - } else { - // Use last value if not enough entries - *self.iterations_per_level.last().unwrap_or(&10) - } - } - - /// Get subsample rate for a given level (0 = coarsest). - /// - /// Returns the step size for point sampling at this level. - pub fn subsample_for_level(&self, level: usize) -> usize { - if level >= self.levels { - return 1; - } - self.subsample_factor.pow((self.levels - 1 - level) as u32) - } -} diff --git a/vastu-map/src/matching/icp/matcher.rs b/vastu-map/src/matching/icp/matcher.rs deleted file mode 100644 index 45ed09e..0000000 --- a/vastu-map/src/matching/icp/matcher.rs +++ /dev/null @@ -1,1324 +0,0 @@ -//! Point-to-Line ICP matcher implementation. - -use crate::core::{Point2D, Pose2D}; -use crate::features::{Line2D, LineCollection, LineSpatialIndex, should_use_spatial_index}; - -use super::super::correspondence::{ - CorrespondenceSet, IDENTITY_COVARIANCE, MatchResult, PoseCovariance, -}; -use super::super::gauss_newton::{GaussNewtonConfig, optimize_pose, optimize_pose_fast}; -use super::super::nearest_neighbor::NearestNeighborConfig; -use super::super::ransac::RansacConfig; -use super::super::ransac::estimate_pose_ransac; -use super::super::scratch::IcpScratchSpace; -use super::config::{CoarseSearchConfig, IcpConfig}; - -/// Point-to-Line ICP matcher. -/// -/// Holds configuration and provides scan matching functionality. -/// Includes internal scratch space for zero-allocation matching. -pub struct PointToLineIcp { - /// ICP configuration. - config: IcpConfig, - /// Nearest neighbor configuration. - nn_config: NearestNeighborConfig, - /// Gauss-Newton configuration. - gn_config: GaussNewtonConfig, - /// Coarse search configuration. - coarse_config: CoarseSearchConfig, - /// Last match confidence for adaptive coarse search. - last_confidence: f32, - /// Pre-allocated scratch space for zero-allocation matching. - scratch: IcpScratchSpace, -} - -impl PointToLineIcp { - /// Create a new ICP matcher with default configuration. - pub fn new() -> Self { - Self::with_config(IcpConfig::default()) - } - - /// Create a new ICP matcher with custom configuration. - pub fn with_config(config: IcpConfig) -> Self { - let nn_config = NearestNeighborConfig::default() - .with_max_distance(config.max_correspondence_distance) - .with_unique_per_point(true); - - let gn_config = GaussNewtonConfig::default() - .with_max_iterations(10) - .with_convergence_threshold(config.convergence_threshold / 10.0); - - Self { - config, - nn_config, - gn_config, - coarse_config: CoarseSearchConfig::default(), - last_confidence: 1.0, // Start with high confidence (no coarse search first time) - scratch: IcpScratchSpace::default_capacity(), // 360 points, 200 lines - } - } - - /// Update the last match confidence (for adaptive coarse search). - pub fn update_last_confidence(&mut self, confidence: f32) { - self.last_confidence = confidence; - } - - /// Check if coarse search should be used. - fn should_use_coarse_search(&self) -> bool { - self.config.use_coarse_search - && self.last_confidence < self.config.coarse_search_confidence_threshold - } - - /// Perform coarse search to find a better initial pose. - /// - /// Searches a grid of poses around the initial pose and returns - /// the one with the lowest total point-to-line distance. - fn coarse_search( - &self, - points: &[Point2D], - lines: &LineCollection, - initial_pose: Pose2D, - ) -> Pose2D { - if lines.is_empty() || points.is_empty() { - return initial_pose; - } - - let cfg = &self.coarse_config; - - // Compute number of steps in each dimension - let t_steps = ((2.0 * cfg.translation_range / cfg.translation_step) as usize).max(1); - let r_steps = ((2.0 * cfg.rotation_range / cfg.rotation_step) as usize).max(1); - - // Subsample points for fast evaluation - let subsampled_points: Vec<&Point2D> = - points.iter().step_by(cfg.subsample_rate.max(1)).collect(); - - let mut best_pose = initial_pose; - let mut best_score = f32::MAX; - - for tx_i in 0..=t_steps { - let tx = initial_pose.x - cfg.translation_range + (tx_i as f32 * cfg.translation_step); - - for ty_i in 0..=t_steps { - let ty = - initial_pose.y - cfg.translation_range + (ty_i as f32 * cfg.translation_step); - - for r_i in 0..=r_steps { - let theta = - initial_pose.theta - cfg.rotation_range + (r_i as f32 * cfg.rotation_step); - - let test_pose = Pose2D::new(tx, ty, theta); - let score = self.score_pose(&subsampled_points, lines, &test_pose); - - if score < best_score { - best_score = score; - best_pose = test_pose; - } - } - } - } - - best_pose - } - - /// Score a pose hypothesis by summing squared point-to-line distances. - fn score_pose(&self, points: &[&Point2D], lines: &LineCollection, pose: &Pose2D) -> f32 { - let (sin, cos) = pose.theta.sin_cos(); - let mut sum_sq = 0.0f32; - - for &point in points { - // Transform point by pose - let tx = point.x * cos - point.y * sin + pose.x; - let ty = point.x * sin + point.y * cos + pose.y; - - // Find minimum distance to any line - let min_dist_sq = lines - .distances_to_point(Point2D::new(tx, ty)) - .into_iter() - .map(|d| d * d) - .fold(f32::MAX, f32::min); - - // Clamp to avoid outliers dominating - sum_sq += min_dist_sq.min(1.0); - } - - sum_sq - } - - /// Match scan points against map lines. - /// - /// # Arguments - /// * `points` - Scan points in world frame (after initial transform) - /// * `lines` - Map lines - /// * `initial_pose` - Initial pose estimate (typically from odometry) - /// - /// # Returns - /// Match result with optimized pose, covariance, and confidence. - pub fn match_scan( - &mut self, - points: &[Point2D], - lines: &[Line2D], - initial_pose: Pose2D, - ) -> MatchResult { - if points.is_empty() || lines.is_empty() { - return MatchResult::new(initial_pose, CorrespondenceSet::new(), 0, false); - } - - // Convert lines to collection for batch operations - let line_collection = LineCollection::from_lines(lines); - - // Only use spatial index for large maps - let use_spatial = should_use_spatial_index(&line_collection); - let spatial_index = if use_spatial { - Some( - line_collection - .build_spatial_index_for_radius(self.config.max_correspondence_distance), - ) - } else { - None - }; - - let mut pose = initial_pose; - let mut prev_pose = pose; - let mut final_correspondences = CorrespondenceSet::new(); - let mut converged = false; - let mut used_coarse_search = false; - let mut final_covariance: PoseCovariance = IDENTITY_COVARIANCE; - let mut final_condition_number = 1.0f32; - - // Optional coarse search when confidence is low - if self.should_use_coarse_search() { - pose = self.coarse_search(points, &line_collection, initial_pose); - used_coarse_search = true; - } - - // Optional RANSAC initialization (after coarse search if used) - if self.config.use_ransac_init { - let initial_corrs = self.find_correspondences_for_pose( - points, - &line_collection, - spatial_index.as_ref(), - &pose, - ); - - if initial_corrs.len() >= self.config.min_correspondences { - let ransac_config = RansacConfig::default() - .with_max_iterations(50) - .with_inlier_threshold(0.15); - - if let Some(ransac_result) = - estimate_pose_ransac(&initial_corrs, lines, &ransac_config) - { - pose = pose.compose(ransac_result.pose); - } - } - } - - // Main ICP loop - let mut final_iteration = 0; - for iteration in 0..self.config.max_iterations { - final_iteration = iteration + 1; - - // 1. Find correspondences at current pose - let correspondences = self.find_correspondences_for_pose( - points, - &line_collection, - spatial_index.as_ref(), - &pose, - ); - - if correspondences.len() < self.config.min_correspondences { - // Not enough correspondences - return with low confidence - return MatchResult::with_covariance( - pose, - correspondences, - final_iteration, - false, - final_covariance, - final_condition_number, - used_coarse_search, - ); - } - - // 2. Filter by max distance and apply IRLS weights - let mut filtered = self.filter_by_max_distance(&correspondences); - self.apply_irls_weights(&mut filtered); - - if filtered.len() < self.config.min_correspondences { - return MatchResult::with_covariance( - pose, - filtered, - final_iteration, - false, - final_covariance, - final_condition_number, - used_coarse_search, - ); - } - - // 3. Optimize pose using Gauss-Newton with weighted correspondences - let gn_result = optimize_pose(&filtered, lines, Pose2D::identity(), &self.gn_config); - final_covariance = gn_result.covariance; - final_condition_number = gn_result.condition_number; - - // 4. Update pose (compose with delta) - pose = pose.compose(gn_result.pose); - - // 5. Check convergence - // Using squared distance comparison to avoid sqrt() - let dx = pose.x - prev_pose.x; - let dy = pose.y - prev_pose.y; - let delta_trans_sq = dx * dx + dy * dy; - let delta_rot = crate::core::math::angle_diff(pose.theta, prev_pose.theta).abs(); - - if delta_trans_sq < self.config.convergence_threshold_sq - && delta_rot < self.config.convergence_threshold - { - converged = true; - final_correspondences = filtered; - break; - } - - prev_pose = pose; - final_correspondences = filtered; - } - - MatchResult::with_covariance( - pose, - final_correspondences, - final_iteration, - converged, - final_covariance, - final_condition_number, - used_coarse_search, - ) - } - - /// Match scan against line collection (optimized for large maps). - pub fn match_scan_batch( - &mut self, - points: &[Point2D], - lines: &LineCollection, - initial_pose: Pose2D, - ) -> MatchResult { - let lines_vec = lines.to_lines(); - self.match_scan(points, &lines_vec, initial_pose) - } - - /// Match scan with range-based weighted correspondences. - /// - /// This method takes points in robot frame and applies proper uncertainty-based - /// weighting using the configured noise model. Close-range measurements get - /// higher weight than far-range ones. - /// - /// # Arguments - /// * `robot_frame_points` - Scan points in robot frame (for weight computation) - /// * `lines` - Map lines in world frame - /// * `initial_pose` - Initial pose estimate in world frame (from odometry) - /// - /// # Returns - /// Match result with optimized pose, covariance, and confidence. - pub fn match_scan_robot_frame( - &mut self, - robot_frame_points: &[Point2D], - lines: &[Line2D], - initial_pose: Pose2D, - ) -> MatchResult { - if robot_frame_points.is_empty() || lines.is_empty() { - return MatchResult::new(initial_pose, CorrespondenceSet::new(), 0, false); - } - - // Use multi-resolution ICP if enabled - if self.config.multi_resolution.enabled { - return match_scan_multiresolution( - robot_frame_points, - lines, - initial_pose, - &self.config.multi_resolution, - &self.config, - ); - } - - // Convert lines to collection for batch operations - let line_collection = LineCollection::from_lines(lines); - self.match_scan_robot_frame_impl(robot_frame_points, lines, &line_collection, initial_pose) - } - - /// Match scan with range-based weighted correspondences using cached LineCollection. - /// - /// This is the preferred method when the LineCollection is already cached - /// (e.g., in LineStore), avoiding re-computation of the SoA structure. - /// - /// # Arguments - /// * `robot_frame_points` - Scan points in robot frame (for weight computation) - /// * `line_collection` - Cached LineCollection from LineStore - /// * `initial_pose` - Initial pose estimate in world frame (from odometry) - /// - /// # Returns - /// Match result with optimized pose, covariance, and confidence. - pub fn match_scan_robot_frame_collection( - &mut self, - robot_frame_points: &[Point2D], - line_collection: &LineCollection, - initial_pose: Pose2D, - ) -> MatchResult { - if robot_frame_points.is_empty() || line_collection.is_empty() { - return MatchResult::new(initial_pose, CorrespondenceSet::new(), 0, false); - } - - // Convert to lines for RANSAC/GN (they require line endpoints) - let lines = line_collection.to_lines(); - - // Use multi-resolution ICP if enabled - if self.config.multi_resolution.enabled { - return match_scan_multiresolution( - robot_frame_points, - &lines, - initial_pose, - &self.config.multi_resolution, - &self.config, - ); - } - - self.match_scan_robot_frame_impl(robot_frame_points, &lines, line_collection, initial_pose) - } - - /// Internal implementation for robot-frame matching. - fn match_scan_robot_frame_impl( - &mut self, - robot_frame_points: &[Point2D], - lines: &[Line2D], - line_collection: &LineCollection, - initial_pose: Pose2D, - ) -> MatchResult { - // Only use spatial index for large maps - let use_spatial = should_use_spatial_index(line_collection); - let spatial_index = if use_spatial { - Some( - line_collection - .build_spatial_index_for_radius(self.config.max_correspondence_distance), - ) - } else { - None - }; - - let mut pose = initial_pose; - let mut prev_pose = pose; - let mut final_correspondences = CorrespondenceSet::new(); - let mut converged = false; - let mut used_coarse_search = false; - let mut final_covariance: PoseCovariance = IDENTITY_COVARIANCE; - let mut final_condition_number = 1.0f32; - - // Keep points in robot frame - find_correspondences_for_pose will transform them - // using the current pose estimate during each ICP iteration - - // Optional coarse search when confidence is low - // Note: coarse_search expects robot-frame points and transforms them internally - if self.should_use_coarse_search() { - pose = self.coarse_search(robot_frame_points, line_collection, initial_pose); - used_coarse_search = true; - } - - // Optional RANSAC initialization - if self.config.use_ransac_init { - let mut initial_corrs = self.find_correspondences_for_pose( - robot_frame_points, - line_collection, - spatial_index.as_ref(), - &pose, - ); - // Apply weights based on robot-frame ranges - initial_corrs.apply_weights(robot_frame_points, &self.config.noise_model); - - if initial_corrs.len() >= self.config.min_correspondences { - let ransac_config = RansacConfig::default() - .with_max_iterations(50) - .with_inlier_threshold(0.15); - - if let Some(ransac_result) = - estimate_pose_ransac(&initial_corrs, lines, &ransac_config) - { - pose = pose.compose(ransac_result.pose); - } - } - } - - // Main ICP loop - let mut final_iteration = 0; - for iteration in 0..self.config.max_iterations { - final_iteration = iteration + 1; - - // 1. Find correspondences at current pose - // Pass robot-frame points - find_correspondences_for_pose transforms them internally - let mut correspondences = self.find_correspondences_for_pose( - robot_frame_points, - line_collection, - spatial_index.as_ref(), - &pose, - ); - - // Apply weights based on robot-frame ranges - correspondences.apply_weights(robot_frame_points, &self.config.noise_model); - - if correspondences.len() < self.config.min_correspondences { - return MatchResult::with_covariance( - pose, - correspondences, - final_iteration, - false, - final_covariance, - final_condition_number, - used_coarse_search, - ); - } - - // 2. Filter by max distance and apply IRLS weights - // Note: Range-based weights already applied above via apply_weights() - let mut filtered = self.filter_by_max_distance(&correspondences); - self.apply_irls_weights(&mut filtered); - - if filtered.len() < self.config.min_correspondences { - return MatchResult::with_covariance( - pose, - filtered, - final_iteration, - false, - final_covariance, - final_condition_number, - used_coarse_search, - ); - } - - // 3. Optimize pose using Gauss-Newton with weighted correspondences - let gn_result = optimize_pose(&filtered, lines, Pose2D::identity(), &self.gn_config); - final_covariance = gn_result.covariance; - final_condition_number = gn_result.condition_number; - - // 4. Update pose (compose with delta) - pose = pose.compose(gn_result.pose); - - // 5. Check convergence - // Using squared distance comparison to avoid sqrt() - let dx = pose.x - prev_pose.x; - let dy = pose.y - prev_pose.y; - let delta_trans_sq = dx * dx + dy * dy; - let delta_rot = crate::core::math::angle_diff(pose.theta, prev_pose.theta).abs(); - - if delta_trans_sq < self.config.convergence_threshold_sq - && delta_rot < self.config.convergence_threshold - { - converged = true; - final_correspondences = filtered; - break; - } - - prev_pose = pose; - final_correspondences = filtered; - } - - MatchResult::with_covariance( - pose, - final_correspondences, - final_iteration, - converged, - final_covariance, - final_condition_number, - used_coarse_search, - ) - } - - /// Match scan using pre-allocated scratch space (zero-allocation hot path). - /// - /// This method automatically uses spatial indexing for O(n×k) correspondence - /// search instead of O(n×m) brute force, providing significant speedup for - /// maps with many lines. - /// - /// # Arguments - /// * `points` - Scan points in robot frame - /// * `lines` - Map lines as LineCollection (for SIMD operations) - /// * `initial_pose` - Initial pose estimate (typically from odometry) - /// * `scratch` - Pre-allocated scratch space for buffer reuse - /// - /// # Returns - /// Match result with optimized pose and confidence. - /// - /// # Example - /// ```rust,ignore - /// let mut scratch = IcpScratchSpace::default_capacity(); - /// let mut icp = PointToLineIcp::new(); - /// - /// // Reuse scratch across multiple calls - /// for (scan, odom_pose) in scans.iter().zip(odometry.iter()) { - /// let result = icp.match_scan_with_scratch(&scan, &map_lines, *odom_pose, &mut scratch); - /// if result.converged { - /// // Use result.pose - /// } - /// } - /// ``` - pub fn match_scan_with_scratch( - &self, - points: &[Point2D], - lines: &LineCollection, - initial_pose: Pose2D, - scratch: &mut IcpScratchSpace, - ) -> MatchResult { - if points.is_empty() || lines.is_empty() { - return MatchResult::new(initial_pose, CorrespondenceSet::new(), 0, false); - } - - // Only use spatial index for large maps (> 30 lines and > 8m extent) - // For small rooms, brute-force is faster due to lower overhead - let use_spatial = should_use_spatial_index(lines); - let spatial_index = if use_spatial { - Some(lines.build_spatial_index_for_radius(self.config.max_correspondence_distance)) - } else { - None - }; - - let mut pose = initial_pose; - let mut prev_pose = pose; - let mut final_correspondences = CorrespondenceSet::new(); - let mut converged = false; - let mut used_coarse_search = false; - let mut final_covariance: PoseCovariance = IDENTITY_COVARIANCE; - let mut final_condition_number = 1.0f32; - - // Optional coarse search when confidence is low - if self.should_use_coarse_search() { - pose = self.coarse_search(points, lines, initial_pose); - used_coarse_search = true; - } - - // Main ICP loop - let mut final_iteration = 0; - for iteration in 0..self.config.max_iterations { - final_iteration = iteration + 1; - - // 1. Transform points and find correspondences - let (sin, cos) = pose.theta.sin_cos(); - scratch.transform_points(points, sin, cos, pose.x, pose.y); - - // Use spatial index for large maps, brute force for small maps - if let Some(ref index) = spatial_index { - scratch.find_correspondences_indexed(lines, index, &self.nn_config); - } else { - scratch.find_correspondences(lines, &self.nn_config); - } - - if scratch.num_correspondences() < self.config.min_correspondences { - // Not enough correspondences - return with low confidence - return MatchResult::with_covariance( - pose, - scratch.take_correspondences(), - final_iteration, - false, - final_covariance, - final_condition_number, - used_coarse_search, - ); - } - - // 2. Get correspondences, filter by distance, and apply IRLS weights - let correspondences = scratch.correspondences(); - let mut filtered = self.filter_by_max_distance(correspondences); - self.apply_irls_weights(&mut filtered); - - if filtered.len() < self.config.min_correspondences { - return MatchResult::with_covariance( - pose, - filtered, - final_iteration, - false, - final_covariance, - final_condition_number, - used_coarse_search, - ); - } - - // 3. Optimize pose using fast Gauss-Newton with weighted correspondences - let gn_result = - optimize_pose_fast(&filtered, lines, Pose2D::identity(), &self.gn_config); - final_covariance = gn_result.covariance; - final_condition_number = gn_result.condition_number; - - // 4. Update pose (compose with delta) - pose = pose.compose(gn_result.pose); - - // 5. Check convergence - // Using squared distance comparison to avoid sqrt() - let dx = pose.x - prev_pose.x; - let dy = pose.y - prev_pose.y; - let delta_trans_sq = dx * dx + dy * dy; - let delta_rot = crate::core::math::angle_diff(pose.theta, prev_pose.theta).abs(); - - if delta_trans_sq < self.config.convergence_threshold_sq - && delta_rot < self.config.convergence_threshold - { - converged = true; - final_correspondences = filtered; - break; - } - - prev_pose = pose; - final_correspondences = filtered; - } - - MatchResult::with_covariance( - pose, - final_correspondences, - final_iteration, - converged, - final_covariance, - final_condition_number, - used_coarse_search, - ) - } - - /// Match scan using spatial index for O(n×k) correspondence search. - /// - /// This is the fastest matching method for large maps, using spatial hashing - /// to reduce correspondence search from O(n×m) to O(n×k) where k << m. - /// - /// # Arguments - /// * `points` - Scan points in robot frame - /// * `lines` - Map lines as LineCollection (for SIMD operations) - /// * `spatial_index` - Pre-built spatial index for fast line lookup - /// * `initial_pose` - Initial pose estimate (typically from odometry) - /// * `scratch` - Pre-allocated scratch space for buffer reuse - /// - /// # Performance - /// For a map with 100 lines and 360 points: - /// - Standard ICP: ~36,000 distance calculations per iteration - /// - With spatial index: ~3,600 (assuming ~10 candidates per point) - /// - /// # Example - /// ```rust,ignore - /// let mut scratch = IcpScratchSpace::default_capacity(); - /// let mut icp = PointToLineIcp::new(); - /// - /// // Build spatial index once when map changes - /// let spatial_index = map_lines.build_spatial_index_for_radius(0.5); - /// - /// // Reuse across multiple scans - /// for (scan, odom_pose) in scans.iter().zip(odometry.iter()) { - /// let result = icp.match_scan_indexed( - /// &scan, &map_lines, &spatial_index, *odom_pose, &mut scratch - /// ); - /// } - /// ``` - pub fn match_scan_indexed( - &self, - points: &[Point2D], - lines: &LineCollection, - spatial_index: &LineSpatialIndex, - initial_pose: Pose2D, - scratch: &mut IcpScratchSpace, - ) -> MatchResult { - if points.is_empty() || lines.is_empty() { - return MatchResult::new(initial_pose, CorrespondenceSet::new(), 0, false); - } - - let mut pose = initial_pose; - let mut prev_pose = pose; - let mut final_correspondences = CorrespondenceSet::new(); - let mut converged = false; - let used_coarse_search = false; // Coarse search not supported with spatial index yet - let mut final_covariance: PoseCovariance = IDENTITY_COVARIANCE; - let mut final_condition_number = 1.0f32; - - // Main ICP loop with spatial-indexed correspondence search - let mut final_iteration = 0; - for iteration in 0..self.config.max_iterations { - final_iteration = iteration + 1; - - // 1. Transform points and find correspondences using spatial index - let (sin, cos) = pose.theta.sin_cos(); - scratch.transform_points(points, sin, cos, pose.x, pose.y); - scratch.find_correspondences_indexed(lines, spatial_index, &self.nn_config); - - if scratch.num_correspondences() < self.config.min_correspondences { - return MatchResult::with_covariance( - pose, - scratch.take_correspondences(), - final_iteration, - false, - final_covariance, - final_condition_number, - used_coarse_search, - ); - } - - // 2. Get correspondences, filter by distance, and apply IRLS weights - let correspondences = scratch.correspondences(); - let mut filtered = self.filter_by_max_distance(correspondences); - self.apply_irls_weights(&mut filtered); - - if filtered.len() < self.config.min_correspondences { - return MatchResult::with_covariance( - pose, - filtered, - final_iteration, - false, - final_covariance, - final_condition_number, - used_coarse_search, - ); - } - - // 3. Optimize pose using fast Gauss-Newton with weighted correspondences - let gn_result = - optimize_pose_fast(&filtered, lines, Pose2D::identity(), &self.gn_config); - final_covariance = gn_result.covariance; - final_condition_number = gn_result.condition_number; - - // 4. Update pose (compose with delta) - pose = pose.compose(gn_result.pose); - - // 5. Check convergence - let dx = pose.x - prev_pose.x; - let dy = pose.y - prev_pose.y; - let delta_trans_sq = dx * dx + dy * dy; - let delta_rot = crate::core::math::angle_diff(pose.theta, prev_pose.theta).abs(); - - if delta_trans_sq < self.config.convergence_threshold_sq - && delta_rot < self.config.convergence_threshold - { - converged = true; - final_correspondences = filtered; - break; - } - - prev_pose = pose; - final_correspondences = filtered; - } - - MatchResult::with_covariance( - pose, - final_correspondences, - final_iteration, - converged, - final_covariance, - final_condition_number, - used_coarse_search, - ) - } - - /// Find correspondences for points at a given pose using scratch space. - /// - /// Uses internal scratch space for zero-allocation transformation. - /// Uses spatial indexing if provided, otherwise falls back to brute force. - fn find_correspondences_for_pose( - &mut self, - points: &[Point2D], - line_collection: &LineCollection, - spatial_index: Option<&LineSpatialIndex>, - pose: &Pose2D, - ) -> CorrespondenceSet { - // Transform points by pose into scratch space (zero-allocation) - let (sin, cos) = pose.theta.sin_cos(); - self.scratch - .transform_points(points, sin, cos, pose.x, pose.y); - - // Find correspondences - use spatial index if available, else brute force - if let Some(index) = spatial_index { - self.scratch - .find_correspondences_indexed(line_collection, index, &self.nn_config); - } else { - self.scratch - .find_correspondences(line_collection, &self.nn_config); - } - - // Return a clone of the correspondences (scratch needs to be reusable) - self.scratch.correspondences().clone() - } - - /// Apply IRLS robust weights to correspondences. - /// - /// Multiplies each correspondence weight by a robust weight computed - /// from its residual (distance). This down-weights outliers softly - /// rather than removing them entirely. - fn apply_irls_weights(&self, correspondences: &mut CorrespondenceSet) { - if !self.config.use_irls || self.config.robust_cost.is_none() { - return; - } - - for corr in correspondences.iter_mut() { - let robust_weight = self.config.robust_cost.weight(corr.distance); - corr.weight *= robust_weight; // Multiply range-based weight with robust weight - } - } - - /// Filter correspondences by max correspondence distance. - /// - /// This is a hard filter applied before IRLS weighting to remove - /// obviously bad correspondences (beyond max search distance). - fn filter_by_max_distance(&self, correspondences: &CorrespondenceSet) -> CorrespondenceSet { - correspondences.filter_by_distance(self.config.max_correspondence_distance) - } -} - -impl Default for PointToLineIcp { - fn default() -> Self { - Self::new() - } -} - -/// Convenience function for single-shot scan matching. -/// -/// # Arguments -/// * `points` - Scan points in robot frame -/// * `lines` - Map lines in world frame -/// * `initial_pose` - Initial robot pose in world frame -/// -/// # Returns -/// Match result with optimized pose. -pub fn match_scan(points: &[Point2D], lines: &[Line2D], initial_pose: Pose2D) -> MatchResult { - let mut icp = PointToLineIcp::new(); - icp.match_scan(points, lines, initial_pose) -} - -/// Match scan with custom configuration. -pub fn match_scan_with_config( - points: &[Point2D], - lines: &[Line2D], - initial_pose: Pose2D, - config: IcpConfig, -) -> MatchResult { - let mut icp = PointToLineIcp::with_config(config); - icp.match_scan(points, lines, initial_pose) -} - -/// Match scan using multi-resolution (coarse-to-fine) ICP. -/// -/// This method improves convergence by starting with subsampled points -/// (coarse level) and progressively refining with more points (fine level). -/// Useful when the initial pose guess may be poor. -/// -/// # Arguments -/// * `points` - Scan points in robot frame -/// * `lines` - Map lines in world frame -/// * `initial_pose` - Initial pose estimate -/// * `multi_config` - Multi-resolution pyramid configuration -/// * `icp_config` - Base ICP configuration (iterations overridden per level) -/// -/// # Returns -/// Match result with optimized pose from finest level. -/// -/// # Example -/// ```rust,ignore -/// use vastu_map::matching::{match_scan_multiresolution, MultiResolutionConfig, IcpConfig}; -/// -/// let multi_config = MultiResolutionConfig::enabled() -/// .with_levels(3) -/// .with_iterations(vec![3, 5, 10]); -/// let icp_config = IcpConfig::default(); -/// -/// let result = match_scan_multiresolution( -/// &points, &lines, initial_pose, &multi_config, &icp_config -/// ); -/// ``` -pub fn match_scan_multiresolution( - points: &[Point2D], - lines: &[Line2D], - initial_pose: Pose2D, - multi_config: &super::config::MultiResolutionConfig, - icp_config: &IcpConfig, -) -> MatchResult { - if points.is_empty() || lines.is_empty() { - return MatchResult::new(initial_pose, CorrespondenceSet::new(), 0, false); - } - - // If multi-resolution disabled, fall back to standard ICP - if !multi_config.enabled || multi_config.levels == 0 { - return match_scan_with_config(points, lines, initial_pose, icp_config.clone()); - } - - let mut pose = initial_pose; - let mut total_iterations = 0; - let mut final_result = None; - - // Process from coarsest (level 0) to finest (level N-1) - for level in 0..multi_config.levels { - let subsample = multi_config.subsample_for_level(level); - let level_iterations = multi_config.iterations_for_level(level); - - // Subsample points deterministically using step_by - let subsampled: Vec = points.iter().step_by(subsample.max(1)).copied().collect(); - - // Skip if too few points after subsampling - if subsampled.len() < icp_config.min_correspondences { - continue; - } - - // Configure ICP for this level - let mut level_config = icp_config.clone(); - level_config.max_iterations = level_iterations; - - // Run ICP at this level - let mut icp = PointToLineIcp::with_config(level_config); - let result = icp.match_scan(&subsampled, lines, pose); - - // Update pose for next level - pose = result.pose; - total_iterations += result.iterations; - final_result = Some(result); - } - - // Return result from finest level (or initial if no levels ran) - match final_result { - Some(mut result) => { - result.iterations = total_iterations; - result - } - None => MatchResult::new(pose, CorrespondenceSet::new(), 0, false), - } -} - -#[cfg(test)] -mod tests { - use super::*; - use crate::core::Point2D; - use crate::matching::RobustCostFunction; - use approx::assert_relative_eq; - - fn make_room_lines() -> Vec { - // 4m x 4m room centered at origin - vec![ - Line2D::new(Point2D::new(-2.0, -2.0), Point2D::new(2.0, -2.0)), // Bottom - Line2D::new(Point2D::new(2.0, -2.0), Point2D::new(2.0, 2.0)), // Right - Line2D::new(Point2D::new(2.0, 2.0), Point2D::new(-2.0, 2.0)), // Top - Line2D::new(Point2D::new(-2.0, 2.0), Point2D::new(-2.0, -2.0)), // Left - ] - } - - fn make_scan_points_at_origin() -> Vec { - // Points as if robot is at origin, looking at walls - let mut points = Vec::new(); - - // Points seeing the four walls - for i in -5..=5 { - let x = i as f32 * 0.3; - // Bottom wall (y = -2) - points.push(Point2D::new(x, -2.0)); - // Top wall (y = 2) - points.push(Point2D::new(x, 2.0)); - } - for i in -5..=5 { - let y = i as f32 * 0.3; - // Left wall (x = -2) - points.push(Point2D::new(-2.0, y)); - // Right wall (x = 2) - points.push(Point2D::new(2.0, y)); - } - - points - } - - #[test] - fn test_icp_identity() { - let lines = make_room_lines(); - let points = make_scan_points_at_origin(); - - let mut icp = PointToLineIcp::new(); - let result = icp.match_scan(&points, &lines, Pose2D::identity()); - - assert!(result.converged); - assert_relative_eq!(result.pose.x, 0.0, epsilon = 0.1); - assert_relative_eq!(result.pose.y, 0.0, epsilon = 0.1); - assert_relative_eq!(result.pose.theta.abs(), 0.0, epsilon = 0.1); - } - - #[test] - fn test_icp_with_offset() { - let lines = make_room_lines(); - - // Create scan points slightly offset from walls (simulating sensor noise) - let mut points = Vec::new(); - for i in -5..=5 { - let x = i as f32 * 0.3; - // Points near bottom wall with small offset - points.push(Point2D::new(x, -1.95)); - // Points near top wall with small offset - points.push(Point2D::new(x, 1.95)); - } - for i in -5..=5 { - let y = i as f32 * 0.3; - // Points near left wall with small offset - points.push(Point2D::new(-1.95, y)); - // Points near right wall with small offset - points.push(Point2D::new(1.95, y)); - } - - let config = IcpConfig::default() - .with_max_iterations(50) - .with_max_correspondence_distance(0.5); - - let mut icp = PointToLineIcp::with_config(config); - let result = icp.match_scan(&points, &lines, Pose2D::identity()); - - // Should find correspondences - assert!(!result.correspondences.is_empty()); - // RMS error should be small (points are close to lines) - assert!( - result.rms_error < 0.2, - "RMS error too high: {}", - result.rms_error - ); - } - - #[test] - fn test_icp_insufficient_points() { - let lines = make_room_lines(); - let points = vec![Point2D::new(0.0, -2.0)]; // Only one point - - let config = IcpConfig::default().with_min_correspondences(5); - let mut icp = PointToLineIcp::with_config(config); - let result = icp.match_scan(&points, &lines, Pose2D::identity()); - - assert!(!result.converged); - } - - #[test] - fn test_icp_no_lines() { - let lines: Vec = vec![]; - let points = make_scan_points_at_origin(); - - let mut icp = PointToLineIcp::new(); - let result = icp.match_scan(&points, &lines, Pose2D::identity()); - - assert!(!result.converged); - assert!(result.correspondences.is_empty()); - } - - #[test] - fn test_icp_with_tukey_robust_cost() { - let lines = make_room_lines(); - let mut points = make_scan_points_at_origin(); - - // Add some outliers - points.push(Point2D::new(10.0, 10.0)); - points.push(Point2D::new(-10.0, -10.0)); - points.push(Point2D::new(5.0, -5.0)); - - // Use Tukey for aggressive outlier rejection (zero weight beyond cutoff) - let config = IcpConfig::default() - .with_robust_cost(RobustCostFunction::Tukey { c: 0.1 }) - .with_irls(true); - - let mut icp = PointToLineIcp::with_config(config); - let result = icp.match_scan(&points, &lines, Pose2D::identity()); - - // Should still converge despite outliers (they get zero weight) - assert!(result.converged); - } - - #[test] - fn test_icp_with_huber_robust_cost() { - let lines = make_room_lines(); - let mut points = make_scan_points_at_origin(); - - // Add outliers far from lines - points.push(Point2D::new(0.0, 0.0)); // Center - far from all walls - - // Use Huber for soft outlier down-weighting - let config = IcpConfig::default() - .with_robust_cost(RobustCostFunction::Huber { delta: 0.03 }) - .with_irls(true); - - let mut icp = PointToLineIcp::with_config(config); - let result = icp.match_scan(&points, &lines, Pose2D::identity()); - - assert!(result.converged); - // Outlier should have low weight but still be included - assert!(!result.correspondences.is_empty()); - } - - #[test] - fn test_match_result_confidence() { - let lines = make_room_lines(); - let points = make_scan_points_at_origin(); - - let result = match_scan(&points, &lines, Pose2D::identity()); - - assert!(result.converged); - assert!(result.confidence > 0.0); - assert!(result.is_good_match(0.1)); - } - - #[test] - fn test_convenience_function() { - let lines = make_room_lines(); - let points = make_scan_points_at_origin(); - - let result = match_scan(&points, &lines, Pose2D::identity()); - - assert!(result.converged); - } - - #[test] - fn test_icp_collinear_points() { - // Test with all points along a single line (degenerate case) - // This can cause issues with the Jacobian being rank-deficient - - // Map has only a horizontal wall - let lines = vec![Line2D::new(Point2D::new(-5.0, 0.0), Point2D::new(5.0, 0.0))]; - - // All points on a line parallel to the wall - let points: Vec = (-10..=10) - .map(|i| Point2D::new(i as f32 * 0.5, 0.1)) - .collect(); - - let config = IcpConfig::default().with_min_correspondences(3); - let mut icp = PointToLineIcp::with_config(config); - let result = icp.match_scan(&points, &lines, Pose2D::identity()); - - // Should handle gracefully (may or may not converge, but shouldn't crash) - // The key is no panics or NaN values - assert!(!result.pose.x.is_nan()); - assert!(!result.pose.y.is_nan()); - assert!(!result.pose.theta.is_nan()); - } - - #[test] - fn test_icp_max_iterations_reached() { - // Create a scenario where ICP struggles to converge - - // Small room - let lines = vec![ - Line2D::new(Point2D::new(-1.0, -1.0), Point2D::new(1.0, -1.0)), - Line2D::new(Point2D::new(1.0, -1.0), Point2D::new(1.0, 1.0)), - ]; - - // Points with large initial offset - let points: Vec = (-5..=5) - .map(|i| Point2D::new(i as f32 * 0.1 + 2.0, -0.9)) - .collect(); - - // Very low max iterations to force hitting the limit - let config = IcpConfig::default() - .with_max_iterations(2) - .with_convergence_threshold(1e-10) // Very strict to prevent early convergence - .with_min_correspondences(3); - - let mut icp = PointToLineIcp::with_config(config); - let result = icp.match_scan(&points, &lines, Pose2D::identity()); - - // Should hit max iterations (may not converge) - // The important check is that iterations <= max_iterations - assert!(result.iterations <= 2); - } - - #[test] - fn test_icp_iterations_tracked() { - let lines = make_room_lines(); - let points = make_scan_points_at_origin(); - - let result = match_scan(&points, &lines, Pose2D::identity()); - - // If converged, should have used some iterations - if result.converged { - assert!( - result.iterations > 0, - "Converged result should track iterations" - ); - assert!( - result.iterations <= 30, - "Iterations should not exceed max (30)" - ); - } - } - - #[test] - fn test_icp_symmetric_room() { - // Test with a symmetric room where correspondences might be ambiguous - - // Square room (all walls equal length) - let lines = vec![ - Line2D::new(Point2D::new(-2.0, -2.0), Point2D::new(2.0, -2.0)), // Bottom - Line2D::new(Point2D::new(2.0, -2.0), Point2D::new(2.0, 2.0)), // Right - Line2D::new(Point2D::new(2.0, 2.0), Point2D::new(-2.0, 2.0)), // Top - Line2D::new(Point2D::new(-2.0, 2.0), Point2D::new(-2.0, -2.0)), // Left - ]; - - // Symmetric scan from center - let mut points = Vec::new(); - for i in -3..=3 { - let offset = i as f32 * 0.5; - points.push(Point2D::new(offset, -2.0)); // Bottom - points.push(Point2D::new(offset, 2.0)); // Top - points.push(Point2D::new(-2.0, offset)); // Left - points.push(Point2D::new(2.0, offset)); // Right - } - - let result = match_scan(&points, &lines, Pose2D::identity()); - - // Should converge despite symmetry - assert!(result.converged); - // Pose should be near identity (we're at center looking at symmetric walls) - assert!(result.pose.x.abs() < 0.5); - assert!(result.pose.y.abs() < 0.5); - } - - #[test] - fn test_icp_indexed_matches_standard() { - // Verify that indexed ICP produces same results as standard ICP - let lines = make_room_lines(); - let points = make_scan_points_at_origin(); - - // Standard ICP - let line_collection = LineCollection::from_lines(&lines); - let mut scratch_std = IcpScratchSpace::default_capacity(); - let icp = PointToLineIcp::new(); - let result_std = icp.match_scan_with_scratch( - &points, - &line_collection, - Pose2D::identity(), - &mut scratch_std, - ); - - // Indexed ICP - let spatial_index = line_collection.build_spatial_index_for_radius(0.5); - let mut scratch_idx = IcpScratchSpace::default_capacity(); - let result_idx = icp.match_scan_indexed( - &points, - &line_collection, - &spatial_index, - Pose2D::identity(), - &mut scratch_idx, - ); - - // Both should converge - assert!(result_std.converged, "Standard ICP should converge"); - assert!(result_idx.converged, "Indexed ICP should converge"); - - // Poses should be very close - assert_relative_eq!(result_std.pose.x, result_idx.pose.x, epsilon = 0.05); - assert_relative_eq!(result_std.pose.y, result_idx.pose.y, epsilon = 0.05); - assert_relative_eq!(result_std.pose.theta, result_idx.pose.theta, epsilon = 0.05); - } - - #[test] - fn test_icp_indexed_identity() { - let lines = make_room_lines(); - let points = make_scan_points_at_origin(); - - let line_collection = LineCollection::from_lines(&lines); - let spatial_index = line_collection.build_spatial_index_for_radius(0.5); - let mut scratch = IcpScratchSpace::default_capacity(); - - let icp = PointToLineIcp::new(); - let result = icp.match_scan_indexed( - &points, - &line_collection, - &spatial_index, - Pose2D::identity(), - &mut scratch, - ); - - assert!(result.converged); - assert_relative_eq!(result.pose.x, 0.0, epsilon = 0.1); - assert_relative_eq!(result.pose.y, 0.0, epsilon = 0.1); - assert_relative_eq!(result.pose.theta.abs(), 0.0, epsilon = 0.1); - } -} diff --git a/vastu-map/src/matching/icp/mod.rs b/vastu-map/src/matching/icp/mod.rs deleted file mode 100644 index 08db54c..0000000 --- a/vastu-map/src/matching/icp/mod.rs +++ /dev/null @@ -1,30 +0,0 @@ -//! Point-to-Line Iterative Closest Point (ICP) algorithm. -//! -//! This module implements the full scan-to-map matching pipeline: -//! -//! 1. **Correspondence Search**: Find nearest map lines for each scan point -//! 2. **Robust Weighting**: Apply M-estimator weights via IRLS to down-weight outliers -//! 3. **Pose Optimization**: Levenberg-Marquardt solver for weighted point-to-line error -//! 4. **Iteration**: Repeat until convergence -//! -//! # Point-to-Line vs Point-to-Point -//! -//! Point-to-Line ICP converges faster than Point-to-Point because: -//! - Points can slide along lines in the tangent direction -//! - Only the perpendicular distance matters -//! - Better handles structured environments (walls, edges) -//! -//! # Robust Cost Functions -//! -//! Use `RobustCostFunction` with IRLS for soft outlier rejection: -//! - **Huber** (default): Good balance between robustness and efficiency -//! - **Cauchy**: Heavy-tailed, good for cluttered environments -//! - **Tukey**: Zero weight for large residuals (aggressive rejection) -//! - **Geman-McClure**: Redescending M-estimator for multimodal errors - -mod config; -mod matcher; - -// Re-export public API -pub use config::{CoarseSearchConfig, IcpConfig, MultiResolutionConfig}; -pub use matcher::{PointToLineIcp, match_scan, match_scan_multiresolution, match_scan_with_config}; diff --git a/vastu-map/src/matching/mod.rs b/vastu-map/src/matching/mod.rs deleted file mode 100644 index 451e3b6..0000000 --- a/vastu-map/src/matching/mod.rs +++ /dev/null @@ -1,92 +0,0 @@ -//! Scan-to-map matching algorithms. -//! -//! This module provides algorithms for matching lidar scans against a map -//! of line features. The primary algorithm is Point-to-Line ICP. -//! -//! # Components -//! -//! - **Correspondence**: Types for point-to-line associations -//! - **Nearest Neighbor**: Find correspondences by distance -//! - **RANSAC**: Robust pose estimation with outlier rejection -//! - **Gauss-Newton/Levenberg-Marquardt**: Nonlinear least squares optimization -//! - **Robust Cost Functions**: M-estimators for outlier rejection (Huber, Cauchy, Tukey) -//! - **Point-to-Line ICP**: Full scan matching pipeline with IRLS support -//! - **Scratch Space**: Pre-allocated buffers for zero-allocation ICP -//! -//! # Usage -//! -//! ```rust,ignore -//! use vastu_map::matching::{match_scan, IcpConfig}; -//! use vastu_map::core::{Point2D, Pose2D}; -//! use vastu_map::features::Line2D; -//! -//! // Map lines -//! let lines = vec![ -//! Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(5.0, 0.0)), -//! Line2D::new(Point2D::new(5.0, 0.0), Point2D::new(5.0, 5.0)), -//! ]; -//! -//! // Scan points (in robot frame) -//! let points = vec![ -//! Point2D::new(1.0, 0.05), // Near first line -//! Point2D::new(2.0, -0.02), // Near first line -//! Point2D::new(4.95, 2.0), // Near second line -//! ]; -//! -//! // Match scan against map -//! let initial_pose = Pose2D::new(0.0, 0.0, 0.0); // From odometry -//! let result = match_scan(&points, &lines, initial_pose); -//! -//! if result.converged { -//! println!("Matched pose: ({:.3}, {:.3}, {:.3})", -//! result.pose.x, result.pose.y, result.pose.theta); -//! println!("Confidence: {:.2}", result.confidence); -//! } -//! ``` -//! -//! # Performance: Zero-Allocation Matching -//! -//! For best performance in hot paths, use `IcpScratchSpace` to reuse buffers: -//! -//! ```rust,ignore -//! use vastu_map::matching::{IcpScratchSpace, PointToLineIcp}; -//! -//! let mut scratch = IcpScratchSpace::default_capacity(); -//! let icp = PointToLineIcp::new(); -//! -//! for scan in scans { -//! let result = icp.match_scan_with_scratch(&scan, &map, pose, &mut scratch); -//! } -//! ``` - -pub mod correspondence; -pub mod gauss_newton; -pub mod icp; -pub mod nearest_neighbor; -pub mod ransac; -pub mod robust; -pub mod scratch; -pub mod traits; - -// Re-export main types -pub use correspondence::{ - CornerCorrespondence, CornerCorrespondenceSet, Correspondence, CorrespondenceSet, - CorrespondenceSoA, IDENTITY_COVARIANCE, LineDataRaw, MatchResult, PoseCovariance, -}; -pub use gauss_newton::{ - GaussNewtonConfig, GaussNewtonResult, optimize_pose_fast, optimize_pose_simd, -}; -pub use nearest_neighbor::{ - CorrespondenceCandidate, NearestNeighborConfig, find_correspondences, - validate_correspondence_candidate, -}; -// Re-export from icp submodule -pub use icp::{ - CoarseSearchConfig, IcpConfig, MultiResolutionConfig, PointToLineIcp, match_scan, - match_scan_multiresolution, match_scan_with_config, -}; -pub use ransac::{RansacConfig, RansacResult, RansacWorkspace, estimate_pose_ransac}; -pub use robust::RobustCostFunction; -pub use scratch::IcpScratchSpace; -// Re-export traits -pub use traits::ScanMatcher; diff --git a/vastu-map/src/matching/nearest_neighbor.rs b/vastu-map/src/matching/nearest_neighbor.rs deleted file mode 100644 index d6041ff..0000000 --- a/vastu-map/src/matching/nearest_neighbor.rs +++ /dev/null @@ -1,542 +0,0 @@ -//! Nearest neighbor matching for scan-to-map correspondence. -//! -//! Finds correspondences between scan points and map lines using -//! distance-based matching with geometric constraints. -//! -//! Note: For ICP hot paths, use `IcpScratchSpace::find_correspondences()` -//! which is zero-allocation. This module's `find_correspondences()` function -//! is provided for testing and simple use cases. - -use crate::core::Point2D; -use crate::features::Line2D; - -use super::correspondence::{Correspondence, CorrespondenceSet}; - -// ============================================================================ -// Correspondence Validation Helper -// ============================================================================ - -/// Result of validating a potential correspondence. -/// -/// Contains the projection parameter and perpendicular distance needed -/// to create a `Correspondence` if the validation passes. -#[derive(Clone, Copy, Debug)] -pub struct CorrespondenceCandidate { - /// Projection parameter along the line (0 = start, 1 = end). - pub t: f32, - /// Perpendicular distance from point to line. - pub distance: f32, -} - -/// Validate a potential correspondence between a point and a line. -/// -/// Checks: -/// 1. Projection bounds: point projects within the line segment (with extension tolerance) -/// 2. Distance threshold: perpendicular distance is within the maximum allowed -/// -/// # Arguments -/// * `point` - The scan point to validate -/// * `line` - The map line to match against -/// * `max_distance` - Maximum allowed perpendicular distance (meters) -/// * `max_projection_extension` - Tolerance for projection beyond line endpoints -/// -/// # Returns -/// `Some(CorrespondenceCandidate)` if valid, `None` if rejected. -/// -/// # Example -/// ```rust,ignore -/// let line = Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(5.0, 0.0)); -/// let point = Point2D::new(2.5, 0.1); // Near middle of line -/// -/// if let Some(candidate) = validate_correspondence_candidate(point, &line, 0.5, 0.2) { -/// // Point is valid: distance=0.1, t=0.5 -/// let corr = Correspondence::new(point_idx, line_idx, point, candidate.distance, candidate.t); -/// } -/// ``` -#[inline] -pub fn validate_correspondence_candidate( - point: Point2D, - line: &Line2D, - max_distance: f32, - max_projection_extension: f32, -) -> Option { - // Compute projection parameter - let t = line.project_point(point); - - // Check projection bounds using Line2D helper - if !line.contains_projection_extended(t, max_projection_extension) { - return None; - } - - // Compute perpendicular distance - let distance = line.distance_to_point(point); - - // Check distance threshold - if distance > max_distance { - return None; - } - - Some(CorrespondenceCandidate { t, distance }) -} - -/// Configuration for nearest neighbor matching. -/// -/// # Geometric Concepts -/// -/// ## Projection Parameter (t) -/// -/// When a point P is matched against a line segment from A to B, we compute -/// the projection parameter `t` where `t=0` means P projects onto A, and -/// `t=1` means P projects onto B: -/// -/// ```text -/// P (point) -/// | -/// v (perpendicular distance) -/// A----+----B (line segment) -/// t=0 t=0.5 t=1 -/// ``` -/// -/// ## Projection Extension -/// -/// The `max_projection_extension` parameter allows matching points that -/// project slightly beyond the line endpoints. With extension=0.2: -/// -/// ```text -/// Valid projection range: [-0.2, 1.2] -/// -/// |<-- 0.2 -->|<---- line ---->|<-- 0.2 -->| -/// ---+===========A================B===========+--- -/// t=-0.2 t=0 t=1 t=1.2 -/// ``` -/// -/// This is useful because lidar points near corners may project just past -/// the extracted line segment. -#[derive(Clone, Debug)] -pub struct NearestNeighborConfig { - /// Maximum perpendicular distance for a valid correspondence (meters). - /// Points farther than this from all lines are rejected. - /// Default: 0.5m - pub max_distance: f32, - - /// Maximum projection extension beyond line endpoints. - /// - /// The projection parameter `t` normally ranges from 0 (at line start) - /// to 1 (at line end). This setting allows points to project up to - /// `extension` units beyond the endpoints: - /// - Valid range becomes `[-extension, 1+extension]` - /// - Value of 0.2 allows 20% extension beyond endpoints - /// - Value of 0.0 requires projection to fall exactly on the segment - /// - /// Default: 0.2 - pub max_projection_extension: f32, - - /// Whether to only keep the best correspondence per point. - /// If false, a point may correspond to multiple lines. - /// Default: true - pub unique_per_point: bool, -} - -impl Default for NearestNeighborConfig { - fn default() -> Self { - Self { - max_distance: 0.5, - max_projection_extension: 0.2, - unique_per_point: true, - } - } -} - -impl NearestNeighborConfig { - /// Create a new configuration with default values. - pub fn new() -> Self { - Self::default() - } - - /// Builder-style setter for maximum distance. - pub fn with_max_distance(mut self, meters: f32) -> Self { - self.max_distance = meters; - self - } - - /// Builder-style setter for maximum projection extension. - pub fn with_max_projection_extension(mut self, extension: f32) -> Self { - self.max_projection_extension = extension; - self - } - - /// Builder-style setter for unique per point. - pub fn with_unique_per_point(mut self, unique: bool) -> Self { - self.unique_per_point = unique; - self - } -} - -/// Find correspondences between scan points and map lines. -/// -/// For each point, finds the nearest line within the distance threshold. -/// Uses geometric constraints (projection bounds) to filter invalid matches. -/// -/// # Arguments -/// * `points` - Scan points in world frame -/// * `lines` - Map lines to match against -/// * `config` - Matching configuration -/// -/// # Returns -/// Set of valid correspondences. -pub fn find_correspondences( - points: &[Point2D], - lines: &[Line2D], - config: &NearestNeighborConfig, -) -> CorrespondenceSet { - if points.is_empty() || lines.is_empty() { - return CorrespondenceSet::new(); - } - - let mut result = CorrespondenceSet::with_capacity(points.len()); - - for (point_idx, &point) in points.iter().enumerate() { - let mut best_distance = config.max_distance; - let mut best_corr: Option = None; - - for (line_idx, line) in lines.iter().enumerate() { - // Use validation helper to check projection and distance constraints - let Some(candidate) = validate_correspondence_candidate( - point, - line, - config.max_distance, - config.max_projection_extension, - ) else { - continue; - }; - - if config.unique_per_point { - // Keep only the best match - if candidate.distance < best_distance { - best_distance = candidate.distance; - best_corr = Some(Correspondence::new( - point_idx, - line_idx, - point, - candidate.distance, - candidate.t, - )); - } - } else { - // Add all valid matches - result.push(Correspondence::new( - point_idx, - line_idx, - point, - candidate.distance, - candidate.t, - )); - } - } - - if config.unique_per_point - && let Some(corr) = best_corr - { - result.push(corr); - } - } - - result -} - -#[cfg(test)] -mod tests { - use super::*; - - fn make_horizontal_line() -> Line2D { - Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(2.0, 0.0)) - } - - fn make_vertical_line() -> Line2D { - Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(0.0, 2.0)) - } - - // === Tests for validate_correspondence_candidate helper === - - #[test] - fn test_validate_correspondence_candidate_valid() { - let line = make_horizontal_line(); // (0,0) to (2,0) - let point = Point2D::new(1.0, 0.1); // Above middle of line - - let result = validate_correspondence_candidate(point, &line, 0.5, 0.2); - - assert!(result.is_some()); - let candidate = result.unwrap(); - assert!((candidate.t - 0.5).abs() < 0.01); // Projects to middle - assert!((candidate.distance - 0.1).abs() < 0.01); // 0.1m perpendicular - } - - #[test] - fn test_validate_correspondence_candidate_too_far() { - let line = make_horizontal_line(); - let point = Point2D::new(1.0, 1.0); // 1m away from line - - let result = validate_correspondence_candidate(point, &line, 0.5, 0.2); - - assert!(result.is_none()); // Rejected: distance > 0.5 - } - - #[test] - fn test_validate_correspondence_candidate_outside_projection() { - let line = make_horizontal_line(); // (0,0) to (2,0) - let point = Point2D::new(-1.0, 0.1); // Projects before line start - - // Strict: no extension - let result_strict = validate_correspondence_candidate(point, &line, 0.5, 0.0); - assert!(result_strict.is_none()); - - // Relaxed: 0.5 extension (projects at t=-0.5, within [-0.5, 1.5]) - let result_relaxed = validate_correspondence_candidate(point, &line, 0.5, 0.6); - assert!(result_relaxed.is_some()); - } - - #[test] - fn test_validate_correspondence_candidate_at_endpoints() { - let line = make_horizontal_line(); // (0,0) to (2,0) - - // At start - let at_start = validate_correspondence_candidate(Point2D::new(0.0, 0.1), &line, 0.5, 0.0); - assert!(at_start.is_some()); - assert!((at_start.unwrap().t - 0.0).abs() < 0.01); - - // At end - let at_end = validate_correspondence_candidate(Point2D::new(2.0, 0.1), &line, 0.5, 0.0); - assert!(at_end.is_some()); - assert!((at_end.unwrap().t - 1.0).abs() < 0.01); - } - - #[test] - fn test_find_correspondences_single_point() { - let lines = vec![make_horizontal_line()]; - let points = vec![Point2D::new(1.0, 0.1)]; // Close to middle of line - - let config = NearestNeighborConfig::default(); - let corrs = find_correspondences(&points, &lines, &config); - - assert_eq!(corrs.len(), 1); - assert_eq!(corrs.correspondences[0].point_idx, 0); - assert_eq!(corrs.correspondences[0].line_idx, 0); - assert!((corrs.correspondences[0].distance - 0.1).abs() < 0.01); - } - - #[test] - fn test_find_correspondences_multiple_lines() { - let lines = vec![make_horizontal_line(), make_vertical_line()]; - let points = vec![Point2D::new(0.05, 0.1)]; // Closer to vertical line - - let config = NearestNeighborConfig::default(); - let corrs = find_correspondences(&points, &lines, &config); - - assert_eq!(corrs.len(), 1); - // Should match vertical line (distance 0.05) not horizontal (distance 0.1) - assert_eq!(corrs.correspondences[0].line_idx, 1); - assert!((corrs.correspondences[0].distance - 0.05).abs() < 0.01); - } - - #[test] - fn test_find_correspondences_no_match_distance() { - let lines = vec![make_horizontal_line()]; - let points = vec![Point2D::new(1.0, 1.0)]; // Too far from line - - let config = NearestNeighborConfig::default().with_max_distance(0.5); - let corrs = find_correspondences(&points, &lines, &config); - - assert!(corrs.is_empty()); - } - - #[test] - fn test_find_correspondences_no_match_projection() { - let lines = vec![make_horizontal_line()]; // From (0,0) to (2,0) - let points = vec![Point2D::new(-1.0, 0.1)]; // Projects before line start - - let config = NearestNeighborConfig::default().with_max_projection_extension(0.0); - let corrs = find_correspondences(&points, &lines, &config); - - assert!(corrs.is_empty()); - } - - #[test] - fn test_find_correspondences_projection_extension() { - let lines = vec![make_horizontal_line()]; // From (0,0) to (2,0) - let points = vec![Point2D::new(-0.2, 0.1)]; // Projects slightly before start - - // Without extension: no match - let config_strict = NearestNeighborConfig::default().with_max_projection_extension(0.0); - let corrs_strict = find_correspondences(&points, &lines, &config_strict); - assert!(corrs_strict.is_empty()); - - // With extension: match - let config_relaxed = NearestNeighborConfig::default().with_max_projection_extension(0.2); - let corrs_relaxed = find_correspondences(&points, &lines, &config_relaxed); - assert_eq!(corrs_relaxed.len(), 1); - } - - #[test] - fn test_find_correspondences_multiple_per_point() { - let lines = vec![ - make_horizontal_line(), - Line2D::new(Point2D::new(0.0, 0.2), Point2D::new(2.0, 0.2)), // Parallel line - ]; - let points = vec![Point2D::new(1.0, 0.1)]; // Between the two lines - - // Unique: only best match - let config_unique = NearestNeighborConfig::default().with_unique_per_point(true); - let corrs_unique = find_correspondences(&points, &lines, &config_unique); - assert_eq!(corrs_unique.len(), 1); - - // Non-unique: both matches - let config_multi = NearestNeighborConfig::default().with_unique_per_point(false); - let corrs_multi = find_correspondences(&points, &lines, &config_multi); - assert_eq!(corrs_multi.len(), 2); - } - - #[test] - fn test_find_correspondences_empty_inputs() { - let config = NearestNeighborConfig::default(); - - // Empty points - let corrs1 = find_correspondences(&[], &[make_horizontal_line()], &config); - assert!(corrs1.is_empty()); - - // Empty lines - let corrs2 = find_correspondences(&[Point2D::new(0.0, 0.0)], &[], &config); - assert!(corrs2.is_empty()); - } - - #[test] - fn test_projection_accuracy() { - let line = make_horizontal_line(); // From (0,0) to (2,0) - let points = vec![ - Point2D::new(0.0, 0.1), // At start, t ≈ 0 - Point2D::new(1.0, 0.1), // At middle, t ≈ 0.5 - Point2D::new(2.0, 0.1), // At end, t ≈ 1 - ]; - - let config = NearestNeighborConfig::default(); - let corrs = find_correspondences(&points, &[line], &config); - - assert_eq!(corrs.len(), 3); - assert!((corrs.correspondences[0].projection_t - 0.0).abs() < 0.01); - assert!((corrs.correspondences[1].projection_t - 0.5).abs() < 0.01); - assert!((corrs.correspondences[2].projection_t - 1.0).abs() < 0.01); - } - - // === Additional edge case tests === - - #[test] - fn test_very_close_parallel_lines() { - // Two parallel lines very close together - test tie-breaking - let lines = vec![ - Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(5.0, 0.0)), - Line2D::new(Point2D::new(0.0, 0.02), Point2D::new(5.0, 0.02)), // 2cm apart - ]; - - // Point exactly between the two lines - let points = vec![Point2D::new(2.5, 0.01)]; - - let config = NearestNeighborConfig::default().with_unique_per_point(true); - let corrs = find_correspondences(&points, &lines, &config); - - // Should match one line (the closest one) - assert_eq!(corrs.len(), 1); - // Distance should be ~0.01m to either line - assert!(corrs.correspondences[0].distance < 0.015); - } - - #[test] - fn test_large_line_collection() { - // Test with many lines to verify performance doesn't degrade - let mut lines = Vec::new(); - - // Create a grid of lines (100 lines) - for i in 0..10 { - // Horizontal lines - lines.push(Line2D::new( - Point2D::new(0.0, i as f32 * 0.5), - Point2D::new(5.0, i as f32 * 0.5), - )); - // Vertical lines - lines.push(Line2D::new( - Point2D::new(i as f32 * 0.5, 0.0), - Point2D::new(i as f32 * 0.5, 5.0), - )); - } - - // Many points - let points: Vec = (0..50) - .map(|i| Point2D::new((i % 10) as f32 * 0.5 + 0.1, (i / 10) as f32 * 0.5 + 0.1)) - .collect(); - - let config = NearestNeighborConfig::default() - .with_max_distance(0.5) - .with_unique_per_point(true); - - let corrs = find_correspondences(&points, &lines, &config); - - // Should find correspondences for most points - assert!( - corrs.len() > 30, - "Expected >30 matches, got {}", - corrs.len() - ); - } - - #[test] - fn test_config_default_values() { - let config = NearestNeighborConfig::default(); - - assert_eq!(config.max_distance, 0.5); - assert_eq!(config.max_projection_extension, 0.2); - assert!(config.unique_per_point); - } - - #[test] - fn test_multiple_points_same_line() { - // Multiple points all matching the same line - let lines = vec![Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(10.0, 0.0))]; - - let points = vec![ - Point2D::new(1.0, 0.05), - Point2D::new(3.0, 0.08), - Point2D::new(5.0, 0.03), - Point2D::new(7.0, 0.06), - Point2D::new(9.0, 0.04), - ]; - - let config = NearestNeighborConfig::default(); - let corrs = find_correspondences(&points, &lines, &config); - - // All points should match the same line - assert_eq!(corrs.len(), 5); - for corr in corrs.iter() { - assert_eq!(corr.line_idx, 0); - } - } - - #[test] - fn test_non_unique_mode_with_many_lines() { - // Test non-unique mode returns all valid matches - let lines = vec![ - Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(5.0, 0.0)), - Line2D::new(Point2D::new(0.0, 0.1), Point2D::new(5.0, 0.1)), - Line2D::new(Point2D::new(0.0, 0.2), Point2D::new(5.0, 0.2)), - ]; - - // Point between all three lines - let points = vec![Point2D::new(2.5, 0.1)]; - - let config = NearestNeighborConfig::default() - .with_max_distance(0.2) - .with_unique_per_point(false); - - let corrs = find_correspondences(&points, &lines, &config); - - // Should match all three lines (distances: 0.1, 0.0, 0.1) - assert_eq!(corrs.len(), 3, "Should match all 3 parallel lines"); - } -} diff --git a/vastu-map/src/matching/ransac.rs b/vastu-map/src/matching/ransac.rs deleted file mode 100644 index 79109a0..0000000 --- a/vastu-map/src/matching/ransac.rs +++ /dev/null @@ -1,853 +0,0 @@ -//! RANSAC-based robust pose estimation. -//! -//! Uses Random Sample Consensus to find the best pose transformation -//! from a set of point-to-line correspondences, robust to outliers. - -use rand::Rng; - -use crate::core::{Point2D, Pose2D}; -use crate::features::Line2D; - -use super::correspondence::{Correspondence, CorrespondenceSet}; - -/// Configuration for RANSAC pose estimation. -#[derive(Clone, Debug)] -pub struct RansacConfig { - /// Maximum number of iterations. - /// Default: 100 - pub max_iterations: usize, - - /// Inlier distance threshold (meters). - /// Correspondences with distance below this are considered inliers. - /// Default: 0.1m - pub inlier_threshold: f32, - - /// Minimum inlier ratio required for a valid model. - /// If the best model has fewer inliers, the estimation fails. - /// Default: 0.3 (30%) - pub min_inlier_ratio: f32, - - /// Early termination if inlier ratio exceeds this. - /// Default: 0.9 (90%) - pub early_termination_ratio: f32, - - /// Minimum number of correspondences for estimation. - /// Default: 3 - pub min_correspondences: usize, -} - -impl Default for RansacConfig { - fn default() -> Self { - Self { - max_iterations: 100, - inlier_threshold: 0.1, - min_inlier_ratio: 0.3, - early_termination_ratio: 0.9, - min_correspondences: 3, - } - } -} - -impl RansacConfig { - /// Create a new configuration with default values. - pub fn new() -> Self { - Self::default() - } - - /// Builder-style setter for maximum iterations. - pub fn with_max_iterations(mut self, iterations: usize) -> Self { - self.max_iterations = iterations; - self - } - - /// Builder-style setter for inlier threshold. - pub fn with_inlier_threshold(mut self, meters: f32) -> Self { - self.inlier_threshold = meters; - self - } - - /// Builder-style setter for minimum inlier ratio. - pub fn with_min_inlier_ratio(mut self, ratio: f32) -> Self { - self.min_inlier_ratio = ratio; - self - } - - /// Builder-style setter for minimum correspondences. - pub fn with_min_correspondences(mut self, count: usize) -> Self { - self.min_correspondences = count; - self - } - - /// Builder-style setter for early termination ratio. - pub fn with_early_termination_ratio(mut self, ratio: f32) -> Self { - self.early_termination_ratio = ratio; - self - } -} - -/// Result of RANSAC pose estimation. -#[derive(Clone, Debug)] -pub struct RansacResult { - /// Estimated pose transformation. - pub pose: Pose2D, - /// Inlier correspondences. - pub inliers: CorrespondenceSet, - /// Inlier ratio (inliers / total correspondences). - pub inlier_ratio: f32, - /// Number of iterations performed. - pub iterations: usize, -} - -/// Pre-allocated workspace for RANSAC operations. -/// -/// RANSAC allocates ~140KB per call for inlier tracking. This workspace -/// pre-allocates buffers that can be reused across multiple RANSAC calls. -/// -/// # Memory Savings -/// -/// Without workspace, each RANSAC call allocates: -/// - Remaining indices buffer: ~3KB (for 400 correspondences) -/// - Inlier mask: ~400 bytes -/// - Best inliers buffer: ~10KB -/// - Per-iteration inlier set: ~10KB × iterations -/// -/// With workspace, these are allocated once and reused. -/// -/// # Example -/// -/// ```rust,ignore -/// use vastu_map::matching::{RansacWorkspace, estimate_pose_ransac_with_workspace}; -/// -/// let mut workspace = RansacWorkspace::new(400); -/// -/// for correspondences in scan_correspondences { -/// let result = estimate_pose_ransac_with_workspace( -/// &correspondences, &lines, &config, &mut workspace -/// ); -/// } -/// ``` -#[derive(Clone, Debug)] -pub struct RansacWorkspace { - /// Buffer for tracking which correspondence indices remain. - pub remaining_indices: Vec, - /// Bitmask for inlier detection. - pub inlier_mask: Vec, - /// Buffer for storing best inlier indices. - pub best_inliers: Vec, - /// Reusable inlier correspondence set. - pub inliers: CorrespondenceSet, -} - -impl RansacWorkspace { - /// Create a new workspace with capacity hint. - /// - /// # Arguments - /// * `max_correspondences` - Expected maximum number of correspondences - pub fn new(max_correspondences: usize) -> Self { - Self { - remaining_indices: Vec::with_capacity(max_correspondences), - inlier_mask: Vec::with_capacity(max_correspondences), - best_inliers: Vec::with_capacity(max_correspondences), - inliers: CorrespondenceSet::with_capacity(max_correspondences), - } - } - - /// Create with default capacity (400 correspondences). - pub fn default_capacity() -> Self { - Self::new(400) - } - - /// Clear all buffers for reuse (keeps capacity). - pub fn clear(&mut self) { - self.remaining_indices.clear(); - self.inlier_mask.clear(); - self.best_inliers.clear(); - self.inliers.clear(); - } -} - -impl Default for RansacWorkspace { - fn default() -> Self { - Self::default_capacity() - } -} - -/// Estimate pose transformation using RANSAC. -/// -/// Samples random subsets of correspondences, computes pose hypotheses, -/// and finds the pose with the most inliers. -/// -/// # Arguments -/// * `correspondences` - Point-to-line correspondences -/// * `lines` - Map lines -/// * `config` - RANSAC configuration -/// -/// # Returns -/// RANSAC result if successful, None if no valid model found. -pub fn estimate_pose_ransac( - correspondences: &CorrespondenceSet, - lines: &[Line2D], - config: &RansacConfig, -) -> Option { - let n = correspondences.len(); - if n < config.min_correspondences { - return None; - } - - let mut rng = rand::rng(); - let mut best_inliers = CorrespondenceSet::new(); - let mut best_pose = Pose2D::identity(); - let mut best_count = 0; - - for iteration in 0..config.max_iterations { - // Sample 2 random correspondences for minimal set - let idx1 = rng.random_range(0..n); - let mut idx2 = rng.random_range(0..n); - while idx2 == idx1 { - idx2 = rng.random_range(0..n); - } - - let corr1 = &correspondences.correspondences[idx1]; - let corr2 = &correspondences.correspondences[idx2]; - - // Get the corresponding lines - let line1 = &lines[corr1.line_idx]; - let line2 = &lines[corr2.line_idx]; - - // Compute pose hypothesis from 2 correspondences - let Some(pose) = estimate_pose_from_two_correspondences(corr1, corr2, line1, line2) else { - continue; - }; - - // Count inliers - let (inlier_count, inliers) = - count_inliers(correspondences, lines, &pose, config.inlier_threshold); - - if inlier_count > best_count { - best_count = inlier_count; - best_inliers = inliers; - best_pose = pose; - - // Early termination check - let ratio = inlier_count as f32 / n as f32; - if ratio >= config.early_termination_ratio { - return Some(RansacResult { - pose: best_pose, - inliers: best_inliers, - inlier_ratio: ratio, - iterations: iteration + 1, - }); - } - } - } - - // Check minimum inlier ratio - let inlier_ratio = best_count as f32 / n as f32; - if inlier_ratio < config.min_inlier_ratio { - return None; - } - - Some(RansacResult { - pose: best_pose, - inliers: best_inliers, - inlier_ratio, - iterations: config.max_iterations, - }) -} - -/// Estimate pose from two point-to-line correspondences. -/// -/// Uses the constraint that each point must lie on its corresponding line -/// after transformation. -fn estimate_pose_from_two_correspondences( - corr1: &Correspondence, - corr2: &Correspondence, - line1: &Line2D, - line2: &Line2D, -) -> Option { - let p1 = corr1.point; - let p2 = corr2.point; - - // Get projection points on lines (where points should map to) - let target1 = line1.point_at(corr1.projection_t); - let target2 = line2.point_at(corr2.projection_t); - - // Compute the transformation that maps p1->target1 and p2->target2 - // This is a rigid body transformation (rotation + translation) - - // Vector between source points - let src_vec = p2 - p1; - let src_len = src_vec.length(); - - if src_len < 1e-6 { - // Points too close together - return None; - } - - // Vector between target points - let dst_vec = target2 - target1; - let dst_len = dst_vec.length(); - - if dst_len < 1e-6 { - // Targets too close together - return None; - } - - // Check that lengths are approximately equal (rigid transformation) - let length_ratio = dst_len / src_len; - if (length_ratio - 1.0).abs() > 0.3 { - // Too much scale difference - invalid correspondences - return None; - } - - // Compute rotation angle - let src_angle = src_vec.y.atan2(src_vec.x); - let dst_angle = dst_vec.y.atan2(dst_vec.x); - let theta = dst_angle - src_angle; - - // Normalize angle - let theta = crate::core::math::normalize_angle(theta); - - // Compute translation using centroid method - let src_centroid = (p1 + p2) * 0.5; - let dst_centroid = (target1 + target2) * 0.5; - - // Rotate source centroid - let (sin, cos) = theta.sin_cos(); - let rotated_centroid = Point2D::new( - src_centroid.x * cos - src_centroid.y * sin, - src_centroid.x * sin + src_centroid.y * cos, - ); - - // Translation is the difference - let tx = dst_centroid.x - rotated_centroid.x; - let ty = dst_centroid.y - rotated_centroid.y; - - Some(Pose2D::new(tx, ty, theta)) -} - -/// Count inliers for a given pose hypothesis. -fn count_inliers( - correspondences: &CorrespondenceSet, - lines: &[Line2D], - pose: &Pose2D, - threshold: f32, -) -> (usize, CorrespondenceSet) { - let mut inliers = CorrespondenceSet::with_capacity(correspondences.len()); - let (sin, cos) = pose.theta.sin_cos(); - - for corr in correspondences.iter() { - // Transform the point - let p = corr.point; - let transformed = Point2D::new( - p.x * cos - p.y * sin + pose.x, - p.x * sin + p.y * cos + pose.y, - ); - - // Compute distance to corresponding line - let line = &lines[corr.line_idx]; - let distance = line.distance_to_point(transformed); - - if distance <= threshold { - // Create updated correspondence with new position and distance - inliers.push(Correspondence::new( - corr.point_idx, - corr.line_idx, - transformed, - distance, - line.project_point(transformed), - )); - } - } - - (inliers.len(), inliers) -} - -/// Refine pose estimate using all inliers (least squares). -/// -/// After RANSAC finds a good initial pose, this function refines it -/// using all inliers for better accuracy. -pub fn refine_pose_with_inliers( - inliers: &CorrespondenceSet, - lines: &[Line2D], - initial_pose: Pose2D, - iterations: usize, -) -> Pose2D { - if inliers.is_empty() { - return initial_pose; - } - - let mut pose = initial_pose; - - for _ in 0..iterations { - // Accumulate gradients - let mut sum_dx = 0.0; - let mut sum_dy = 0.0; - let mut sum_dtheta = 0.0; - let mut count = 0.0; - - let (sin, cos) = pose.theta.sin_cos(); - - for corr in inliers.iter() { - let p = corr.point; - - // Transform point by current pose - let tx = p.x * cos - p.y * sin + pose.x; - let ty = p.x * sin + p.y * cos + pose.y; - let transformed = Point2D::new(tx, ty); - - let line = &lines[corr.line_idx]; - - // Get line normal (perpendicular to line direction) - let normal = line.normal(); - - // Signed distance (positive = left of line) - let signed_dist = line.signed_distance_to_point(transformed); - - // Gradient of distance w.r.t. pose parameters - // d(dist)/d(tx) = normal.x - // d(dist)/d(ty) = normal.y - // d(dist)/d(theta) ≈ cross(p_rotated, normal) - let p_rotated = Point2D::new(tx - pose.x, ty - pose.y); - - sum_dx += signed_dist * normal.x; - sum_dy += signed_dist * normal.y; - sum_dtheta += signed_dist * (p_rotated.x * normal.y - p_rotated.y * normal.x); - count += 1.0; - } - - if count < 1.0 { - break; - } - - // Apply gradient descent step - let step = 0.5; - pose.x -= step * sum_dx / count; - pose.y -= step * sum_dy / count; - pose.theta -= step * sum_dtheta / count; - pose.theta = crate::core::math::normalize_angle(pose.theta); - } - - pose -} - -#[cfg(test)] -mod tests { - use super::*; - use crate::core::Point2D; - use approx::assert_relative_eq; - - fn make_test_lines() -> Vec { - vec![ - // Horizontal line at y=0 - Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(5.0, 0.0)), - // Vertical line at x=5 - Line2D::new(Point2D::new(5.0, 0.0), Point2D::new(5.0, 5.0)), - // Horizontal line at y=5 - Line2D::new(Point2D::new(5.0, 5.0), Point2D::new(0.0, 5.0)), - // Vertical line at x=0 - Line2D::new(Point2D::new(0.0, 5.0), Point2D::new(0.0, 0.0)), - ] - } - - fn make_test_correspondences(offset: Point2D) -> CorrespondenceSet { - let mut corrs = CorrespondenceSet::new(); - - // Points that should match to the square walls - // After applying translation, these should land on the lines - corrs.push(Correspondence::new( - 0, - 0, - Point2D::new(1.0, 0.0) - offset, - 0.0, - 0.2, - )); - corrs.push(Correspondence::new( - 1, - 0, - Point2D::new(2.0, 0.0) - offset, - 0.0, - 0.4, - )); - corrs.push(Correspondence::new( - 2, - 1, - Point2D::new(5.0, 1.0) - offset, - 0.0, - 0.2, - )); - corrs.push(Correspondence::new( - 3, - 1, - Point2D::new(5.0, 2.0) - offset, - 0.0, - 0.4, - )); - corrs.push(Correspondence::new( - 4, - 2, - Point2D::new(4.0, 5.0) - offset, - 0.0, - 0.2, - )); - corrs.push(Correspondence::new( - 5, - 3, - Point2D::new(0.0, 3.0) - offset, - 0.0, - 0.6, - )); - - corrs - } - - #[test] - fn test_ransac_translation() { - let lines = make_test_lines(); - let offset = Point2D::new(0.5, 0.3); - let corrs = make_test_correspondences(offset); - - let config = RansacConfig::default() - .with_max_iterations(200) - .with_inlier_threshold(0.2); - - let result = estimate_pose_ransac(&corrs, &lines, &config); - - assert!(result.is_some(), "RANSAC should find a solution"); - let result = result.unwrap(); - - // The pose should approximately recover the offset - assert_relative_eq!(result.pose.x, offset.x, epsilon = 0.3); - assert_relative_eq!(result.pose.y, offset.y, epsilon = 0.3); - assert!(result.inlier_ratio >= 0.3); - } - - #[test] - fn test_ransac_insufficient_correspondences() { - let lines = make_test_lines(); - let mut corrs = CorrespondenceSet::new(); - corrs.push(Correspondence::new(0, 0, Point2D::new(1.0, 0.0), 0.0, 0.2)); - - let config = RansacConfig::default().with_min_correspondences(3); - let result = estimate_pose_ransac(&corrs, &lines, &config); - - assert!(result.is_none()); - } - - #[test] - fn test_pose_from_two_correspondences() { - // Two points on a horizontal line - let line1 = Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(10.0, 0.0)); - let line2 = Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(10.0, 0.0)); - - // Points that are offset from where they should be - let corr1 = Correspondence::new(0, 0, Point2D::new(1.0, 1.0), 1.0, 0.1); - let corr2 = Correspondence::new(1, 0, Point2D::new(5.0, 1.0), 1.0, 0.5); - - let pose = estimate_pose_from_two_correspondences(&corr1, &corr2, &line1, &line2); - - assert!(pose.is_some()); - let pose = pose.unwrap(); - - // Should translate down by approximately 1 - assert_relative_eq!(pose.y, -1.0, epsilon = 0.1); - assert_relative_eq!(pose.theta.abs(), 0.0, epsilon = 0.1); - } - - #[test] - fn test_count_inliers() { - let lines = vec![Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(10.0, 0.0))]; - - let mut corrs = CorrespondenceSet::new(); - // Point at y=0.05 - should be inlier - corrs.push(Correspondence::new( - 0, - 0, - Point2D::new(1.0, 0.05), - 0.05, - 0.1, - )); - // Point at y=0.5 - should be outlier with threshold 0.1 - corrs.push(Correspondence::new(1, 0, Point2D::new(2.0, 0.5), 0.5, 0.2)); - - let pose = Pose2D::identity(); - let (count, inliers) = count_inliers(&corrs, &lines, &pose, 0.1); - - assert_eq!(count, 1); - assert_eq!(inliers.len(), 1); - } - - #[test] - fn test_refine_pose() { - let lines = vec![ - Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(10.0, 0.0)), - Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(0.0, 10.0)), - ]; - - let mut corrs = CorrespondenceSet::new(); - // Points slightly off the lines - corrs.push(Correspondence::new(0, 0, Point2D::new(1.0, 0.1), 0.1, 0.1)); - corrs.push(Correspondence::new(1, 0, Point2D::new(5.0, 0.1), 0.1, 0.5)); - corrs.push(Correspondence::new(2, 1, Point2D::new(0.1, 3.0), 0.1, 0.3)); - - let initial = Pose2D::identity(); - let refined = refine_pose_with_inliers(&corrs, &lines, initial, 10); - - // Pose should shift to better align points with lines - // Points have positive y offset, so pose.y should be negative to compensate - assert!(refined.y < 0.0 || refined.x < 0.0); - } - - // === Additional edge case tests === - - #[test] - fn test_ransac_high_outlier_ratio() { - // Test with >50% outliers - RANSAC should still find a valid solution - // if the inlier threshold is set appropriately - let lines = make_test_lines(); - let offset = Point2D::new(0.3, 0.2); - - let mut corrs = CorrespondenceSet::new(); - - // Add 3 inliers (points that will land on lines after translation) - corrs.push(Correspondence::new( - 0, - 0, - Point2D::new(1.0, 0.0) - offset, - 0.0, - 0.2, - )); - corrs.push(Correspondence::new( - 1, - 0, - Point2D::new(3.0, 0.0) - offset, - 0.0, - 0.6, - )); - corrs.push(Correspondence::new( - 2, - 1, - Point2D::new(5.0, 2.0) - offset, - 0.0, - 0.4, - )); - - // Add 7 outliers (points far from any line) - for i in 0..7 { - corrs.push(Correspondence::new( - 3 + i, - 0, - Point2D::new(2.5 + i as f32 * 0.1, 2.5), // Center of square, far from walls - 2.5, - 0.5, - )); - } - - let config = RansacConfig::default() - .with_max_iterations(500) - .with_inlier_threshold(0.3) - .with_min_inlier_ratio(0.2); // Lower threshold since we have many outliers - - let result = estimate_pose_ransac(&corrs, &lines, &config); - - // Should find a solution despite high outlier ratio - assert!( - result.is_some(), - "RANSAC should find solution with 70% outliers" - ); - let result = result.unwrap(); - assert!(result.inlier_ratio >= 0.2); - } - - #[test] - fn test_ransac_all_outliers() { - // Test with all outliers - RANSAC should return None - let lines = make_test_lines(); - - let mut corrs = CorrespondenceSet::new(); - - // All points are far from any line (center of the square) - for i in 0..10 { - corrs.push(Correspondence::new( - i, - 0, - Point2D::new(2.5 + i as f32 * 0.05, 2.5 + i as f32 * 0.05), - 2.5, - 0.5, - )); - } - - let config = RansacConfig::default() - .with_max_iterations(100) - .with_inlier_threshold(0.1) - .with_min_inlier_ratio(0.5); - - let result = estimate_pose_ransac(&corrs, &lines, &config); - - // Should fail - no valid model with >50% inliers - assert!(result.is_none(), "RANSAC should fail with all outliers"); - } - - #[test] - fn test_ransac_early_termination() { - // Test that early termination works when we have very good inliers - let lines = make_test_lines(); - let offset = Point2D::new(0.2, 0.1); - - // Create correspondences where all points are inliers - let mut corrs = CorrespondenceSet::new(); - corrs.push(Correspondence::new( - 0, - 0, - Point2D::new(1.0, 0.0) - offset, - 0.0, - 0.2, - )); - corrs.push(Correspondence::new( - 1, - 0, - Point2D::new(2.0, 0.0) - offset, - 0.0, - 0.4, - )); - corrs.push(Correspondence::new( - 2, - 0, - Point2D::new(3.0, 0.0) - offset, - 0.0, - 0.6, - )); - corrs.push(Correspondence::new( - 3, - 0, - Point2D::new(4.0, 0.0) - offset, - 0.0, - 0.8, - )); - - let config = RansacConfig::default() - .with_max_iterations(1000) // Many iterations - .with_inlier_threshold(0.3) - .with_early_termination_ratio(0.8); // Should terminate early - - let result = estimate_pose_ransac(&corrs, &lines, &config); - - assert!(result.is_some()); - let result = result.unwrap(); - - // Should have terminated early (before max_iterations) - assert!( - result.iterations < 1000, - "Should terminate early, but used {} iterations", - result.iterations - ); - assert!(result.inlier_ratio >= 0.8); - } - - #[test] - fn test_ransac_degenerate_geometry_collinear() { - // Test with degenerate case: all correspondences point to same line - // This should still work as long as we can compute a valid transformation - let lines = vec![Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(10.0, 0.0))]; - - let offset = Point2D::new(0.0, 0.5); // Only Y offset - - let mut corrs = CorrespondenceSet::new(); - // All points on same line but with Y offset - corrs.push(Correspondence::new( - 0, - 0, - Point2D::new(1.0, 0.0) - offset, - 0.5, - 0.1, - )); - corrs.push(Correspondence::new( - 1, - 0, - Point2D::new(3.0, 0.0) - offset, - 0.5, - 0.3, - )); - corrs.push(Correspondence::new( - 2, - 0, - Point2D::new(5.0, 0.0) - offset, - 0.5, - 0.5, - )); - corrs.push(Correspondence::new( - 3, - 0, - Point2D::new(7.0, 0.0) - offset, - 0.5, - 0.7, - )); - - let config = RansacConfig::default() - .with_max_iterations(200) - .with_inlier_threshold(0.3); - - let result = estimate_pose_ransac(&corrs, &lines, &config); - - // Should find a solution (pure translation along Y) - assert!(result.is_some()); - let result = result.unwrap(); - assert_relative_eq!(result.pose.y, offset.y, epsilon = 0.3); - } - - #[test] - fn test_ransac_points_too_close() { - // Test with points that are too close together - let line1 = Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(10.0, 0.0)); - let line2 = Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(10.0, 0.0)); - - // Two points that are extremely close (< 1e-6 apart) - let corr1 = Correspondence::new(0, 0, Point2D::new(1.0, 0.5), 0.5, 0.1); - let corr2 = Correspondence::new(1, 0, Point2D::new(1.0 + 1e-7, 0.5), 0.5, 0.1); - - let pose = estimate_pose_from_two_correspondences(&corr1, &corr2, &line1, &line2); - - // Should return None because points are too close - assert!(pose.is_none(), "Should fail with points too close together"); - } - - #[test] - fn test_ransac_config_builders() { - // Test all config builder methods - let config = RansacConfig::new() - .with_max_iterations(50) - .with_inlier_threshold(0.2) - .with_min_inlier_ratio(0.4) - .with_min_correspondences(5); - - assert_eq!(config.max_iterations, 50); - assert_eq!(config.inlier_threshold, 0.2); - assert_eq!(config.min_inlier_ratio, 0.4); - assert_eq!(config.min_correspondences, 5); - } - - #[test] - fn test_ransac_empty_correspondences() { - let lines = make_test_lines(); - let corrs = CorrespondenceSet::new(); - - let config = RansacConfig::default(); - let result = estimate_pose_ransac(&corrs, &lines, &config); - - assert!(result.is_none()); - } - - #[test] - fn test_refine_pose_empty_inliers() { - let lines = make_test_lines(); - let corrs = CorrespondenceSet::new(); - let initial = Pose2D::new(1.0, 2.0, 0.5); - - let refined = refine_pose_with_inliers(&corrs, &lines, initial, 10); - - // Should return initial pose unchanged - assert_relative_eq!(refined.x, initial.x); - assert_relative_eq!(refined.y, initial.y); - assert_relative_eq!(refined.theta, initial.theta); - } -} diff --git a/vastu-map/src/matching/robust.rs b/vastu-map/src/matching/robust.rs deleted file mode 100644 index c2ffc7a..0000000 --- a/vastu-map/src/matching/robust.rs +++ /dev/null @@ -1,300 +0,0 @@ -//! Robust cost functions for outlier rejection in ICP. -//! -//! These M-estimators compute weights that down-weight outliers based on -//! residual magnitude, enabling soft outlier rejection without hard thresholds. -//! -//! # Usage -//! -//! ```rust,ignore -//! use vastu_map::matching::RobustCostFunction; -//! -//! // Huber is recommended for most cases -//! let cost = RobustCostFunction::Huber { delta: 0.03 }; -//! -//! // Compute weight for a residual -//! let weight = cost.weight(0.05); // Returns ~0.6 (down-weighted) -//! ``` -//! -//! # M-Estimator Comparison -//! -//! | Estimator | Behavior | Best For | -//! |-----------|----------|----------| -//! | Huber | Linear beyond delta | General use, balanced | -//! | Cauchy | Heavy-tailed | Cluttered environments | -//! | Tukey | Zero weight for outliers | Aggressive rejection | -//! | Geman-McClure | Redescending | Multimodal errors | - -use serde::{Deserialize, Serialize}; - -/// Robust cost function for weighted least squares (IRLS). -/// -/// Computes weights that down-weight outliers based on residual magnitude. -/// Use with ICP to improve accuracy in presence of noise and clutter. -#[derive(Clone, Debug, PartialEq, Serialize, Deserialize)] -#[serde(tag = "type")] -pub enum RobustCostFunction { - /// No robust weighting (standard least squares). - /// Weight is always 1.0 regardless of residual. - None, - - /// Huber loss: quadratic near zero, linear beyond delta. - /// - /// Good balance between robustness and efficiency. - /// Recommended as default choice. - /// - /// Weight: 1.0 if |r| ≤ δ, else δ/|r| - Huber { - /// Threshold where loss transitions from quadratic to linear. - /// Default: 0.03 (3cm) - delta: f32, - }, - - /// Cauchy loss (Lorentzian): heavy-tailed distribution. - /// - /// More aggressive outlier rejection than Huber. - /// Good for cluttered environments with many outliers. - /// - /// Weight: 1 / (1 + (r/scale)²) - Cauchy { - /// Scale parameter controlling tail heaviness. - /// Default: 0.05 - scale: f32, - }, - - /// Tukey biweight: zero weight for large residuals. - /// - /// Most aggressive - completely ignores outliers beyond threshold. - /// Use when outliers are clearly distinguishable. - /// - /// Weight: (1 - (r/c)²)² if |r| ≤ c, else 0 - Tukey { - /// Cutoff threshold. Residuals beyond this get zero weight. - /// Default: 0.1 - c: f32, - }, - - /// Geman-McClure: redescending M-estimator. - /// - /// Good for multimodal error distributions. - /// Weights decrease rapidly for large residuals. - /// - /// Weight: 1 / (1 + (r/scale)²)² - GemanMcClure { - /// Scale parameter. - /// Default: 0.05 - scale: f32, - }, -} - -impl RobustCostFunction { - /// Compute weight for IRLS (Iteratively Reweighted Least Squares). - /// - /// Returns a weight in [0, 1] that should multiply the correspondence weight. - /// Small residuals get weight ≈ 1, large residuals get down-weighted. - /// - /// # Arguments - /// * `residual` - The point-to-line distance (can be negative for signed distance) - /// - /// # Returns - /// Weight in [0, 1] to apply to this correspondence - #[inline] - pub fn weight(&self, residual: f32) -> f32 { - let r = residual.abs(); - match self { - Self::None => 1.0, - Self::Huber { delta } => { - if r <= *delta { - 1.0 - } else { - *delta / r - } - } - Self::Cauchy { scale } => { - let u = r / scale; - 1.0 / (1.0 + u * u) - } - Self::Tukey { c } => { - if r <= *c { - let u = r / c; - let v = 1.0 - u * u; - v * v - } else { - 0.0 - } - } - Self::GemanMcClure { scale } => { - let u = r / scale; - let v = 1.0 + u * u; - 1.0 / (v * v) - } - } - } - - /// Create Huber cost with default threshold (3cm). - #[inline] - pub fn default_huber() -> Self { - Self::Huber { delta: 0.03 } - } - - /// Create Cauchy cost with default scale (5cm). - #[inline] - pub fn default_cauchy() -> Self { - Self::Cauchy { scale: 0.05 } - } - - /// Create Tukey cost with default cutoff (10cm). - #[inline] - pub fn default_tukey() -> Self { - Self::Tukey { c: 0.1 } - } - - /// Create Geman-McClure cost with default scale (5cm). - #[inline] - pub fn default_geman_mcclure() -> Self { - Self::GemanMcClure { scale: 0.05 } - } - - /// Check if this is the None variant (no robust weighting). - #[inline] - pub fn is_none(&self) -> bool { - matches!(self, Self::None) - } - - /// Check if this cost function is enabled (not None). - #[inline] - pub fn is_enabled(&self) -> bool { - !self.is_none() - } -} - -impl Default for RobustCostFunction { - /// Default to Huber with 3cm threshold. - fn default() -> Self { - Self::default_huber() - } -} - -#[cfg(test)] -mod tests { - use super::*; - use approx::assert_relative_eq; - - #[test] - fn test_none_always_returns_one() { - let cost = RobustCostFunction::None; - assert_eq!(cost.weight(0.0), 1.0); - assert_eq!(cost.weight(0.1), 1.0); - assert_eq!(cost.weight(1.0), 1.0); - assert_eq!(cost.weight(-0.5), 1.0); - } - - #[test] - fn test_huber_within_delta() { - let cost = RobustCostFunction::Huber { delta: 0.05 }; - assert_eq!(cost.weight(0.0), 1.0); - assert_eq!(cost.weight(0.03), 1.0); - assert_eq!(cost.weight(0.05), 1.0); - assert_eq!(cost.weight(-0.03), 1.0); - } - - #[test] - fn test_huber_beyond_delta() { - let cost = RobustCostFunction::Huber { delta: 0.05 }; - // At r = 0.1, weight = 0.05 / 0.1 = 0.5 - assert_relative_eq!(cost.weight(0.1), 0.5, epsilon = 1e-6); - // At r = 0.2, weight = 0.05 / 0.2 = 0.25 - assert_relative_eq!(cost.weight(0.2), 0.25, epsilon = 1e-6); - } - - #[test] - fn test_cauchy() { - let cost = RobustCostFunction::Cauchy { scale: 0.05 }; - // At r = 0, weight = 1 - assert_eq!(cost.weight(0.0), 1.0); - // At r = scale, weight = 1 / (1 + 1) = 0.5 - assert_relative_eq!(cost.weight(0.05), 0.5, epsilon = 1e-6); - // At r = 2*scale, weight = 1 / (1 + 4) = 0.2 - assert_relative_eq!(cost.weight(0.1), 0.2, epsilon = 1e-6); - } - - #[test] - fn test_tukey_within_cutoff() { - let cost = RobustCostFunction::Tukey { c: 0.1 }; - // At r = 0, weight = 1 - assert_eq!(cost.weight(0.0), 1.0); - // At r = c/2, weight = (1 - 0.25)^2 = 0.5625 - assert_relative_eq!(cost.weight(0.05), 0.5625, epsilon = 1e-6); - } - - #[test] - fn test_tukey_beyond_cutoff() { - let cost = RobustCostFunction::Tukey { c: 0.1 }; - // Beyond cutoff, weight = 0 - assert_eq!(cost.weight(0.1), 0.0); - assert_eq!(cost.weight(0.2), 0.0); - assert_eq!(cost.weight(1.0), 0.0); - } - - #[test] - fn test_geman_mcclure() { - let cost = RobustCostFunction::GemanMcClure { scale: 0.05 }; - // At r = 0, weight = 1 - assert_eq!(cost.weight(0.0), 1.0); - // At r = scale, weight = 1 / (1 + 1)^2 = 0.25 - assert_relative_eq!(cost.weight(0.05), 0.25, epsilon = 1e-6); - } - - #[test] - fn test_default_is_huber() { - let cost = RobustCostFunction::default(); - assert!(matches!(cost, RobustCostFunction::Huber { delta } if (delta - 0.03).abs() < 1e-6)); - } - - #[test] - fn test_is_none() { - assert!(RobustCostFunction::None.is_none()); - assert!(!RobustCostFunction::default_huber().is_none()); - } - - #[test] - fn test_is_enabled() { - assert!(!RobustCostFunction::None.is_enabled()); - assert!(RobustCostFunction::default_huber().is_enabled()); - } - - #[test] - fn test_weights_are_symmetric() { - let costs = [ - RobustCostFunction::None, - RobustCostFunction::default_huber(), - RobustCostFunction::default_cauchy(), - RobustCostFunction::default_tukey(), - RobustCostFunction::default_geman_mcclure(), - ]; - - for cost in &costs { - assert_eq!(cost.weight(0.03), cost.weight(-0.03)); - assert_eq!(cost.weight(0.1), cost.weight(-0.1)); - } - } - - #[test] - fn test_weights_decrease_with_residual() { - let costs = [ - RobustCostFunction::default_huber(), - RobustCostFunction::default_cauchy(), - RobustCostFunction::default_geman_mcclure(), - ]; - - for cost in &costs { - let w0 = cost.weight(0.0); - let w1 = cost.weight(0.05); - let w2 = cost.weight(0.1); - let w3 = cost.weight(0.2); - - assert!(w0 >= w1, "Weight should decrease: {} >= {}", w0, w1); - assert!(w1 >= w2, "Weight should decrease: {} >= {}", w1, w2); - assert!(w2 >= w3, "Weight should decrease: {} >= {}", w2, w3); - } - } -} diff --git a/vastu-map/src/matching/scratch.rs b/vastu-map/src/matching/scratch.rs deleted file mode 100644 index b7c79ca..0000000 --- a/vastu-map/src/matching/scratch.rs +++ /dev/null @@ -1,962 +0,0 @@ -//! Pre-allocated scratch space for ICP hot paths. -//! -//! This module provides buffer reuse to eliminate allocations during the -//! ICP inner loop, significantly improving performance on embedded systems. -//! -//! # Performance Impact -//! -//! Without scratch space, each ICP iteration allocates: -//! - Transformed points buffer (~360 points × 8 bytes = ~3KB) -//! - Distance buffer (~100 lines × 4 bytes × 360 points = ~140KB) -//! - Projection buffer (same as distance) -//! - Correspondence set (~360 × 24 bytes = ~9KB) -//! -//! With scratch space, these allocations happen once and are reused. - -use std::simd::f32x4; - -use crate::config::LidarNoiseModel; -use crate::core::Point2D; -use crate::features::{LineCollection, LineSpatialIndex}; - -use super::correspondence::{Correspondence, CorrespondenceSet, CorrespondenceSoA}; -use super::nearest_neighbor::NearestNeighborConfig; - -/// Pre-allocated scratch space for ICP operations. -/// -/// Reuse this struct across multiple `match_scan` calls to avoid -/// per-iteration allocations. Create once, use many times. -/// -/// # Example -/// ```rust,ignore -/// use vastu_map::matching::{IcpScratchSpace, PointToLineIcp}; -/// -/// // Create scratch space sized for typical scans -/// let mut scratch = IcpScratchSpace::new(400, 200); -/// -/// let icp = PointToLineIcp::new(); -/// for scan in scans { -/// let result = icp.match_scan_with_scratch(&scan, &map, pose, &mut scratch); -/// } -/// ``` -#[derive(Clone, Debug)] -pub struct IcpScratchSpace { - /// Buffer for transformed points. - pub(crate) transformed_xs: Vec, - pub(crate) transformed_ys: Vec, - - /// Buffer for distances to all lines (per point). - pub(crate) distances: Vec, - - /// Buffer for projection parameters (per point). - pub(crate) projections: Vec, - - /// Reusable correspondence set (AoS layout). - pub(crate) correspondences: CorrespondenceSet, - - /// Reusable correspondence set (SoA layout for SIMD). - pub(crate) correspondences_soa: CorrespondenceSoA, - - /// Buffer for ranges (distances from sensor). - pub(crate) ranges: Vec, - - // ========================================================================= - // Multi-resolution / Coarse search buffers - // ========================================================================= - /// Buffer for subsampled point X coordinates. - /// Used by multi-resolution ICP and coarse search to avoid allocations. - pub(crate) subsampled_xs: Vec, - - /// Buffer for subsampled point Y coordinates. - pub(crate) subsampled_ys: Vec, - - // ========================================================================= - // Spatial index query buffer - // ========================================================================= - /// Buffer for spatial index query candidates. - /// Reused across point queries to avoid allocations. - pub(crate) spatial_candidates: Vec, -} - -impl IcpScratchSpace { - /// Create new scratch space with capacity hints. - /// - /// # Arguments - /// * `max_points` - Expected maximum scan points (e.g., 360 for a typical lidar) - /// * `max_lines` - Expected maximum map lines (e.g., 100-200 for indoor) - pub fn new(max_points: usize, max_lines: usize) -> Self { - Self { - transformed_xs: Vec::with_capacity(max_points), - transformed_ys: Vec::with_capacity(max_points), - distances: Vec::with_capacity(max_lines), - projections: Vec::with_capacity(max_lines), - correspondences: CorrespondenceSet::with_capacity(max_points), - correspondences_soa: CorrespondenceSoA::with_capacity(max_points), - ranges: Vec::with_capacity(max_points), - subsampled_xs: Vec::with_capacity(max_points), - subsampled_ys: Vec::with_capacity(max_points), - spatial_candidates: Vec::with_capacity(max_lines / 4), // Typical: ~25% of lines per query - } - } - - /// Create with default capacity (360 points, 200 lines). - pub fn default_capacity() -> Self { - Self::new(360, 200) - } - - /// Clear buffers for next iteration (keeps capacity). - #[inline] - pub fn clear(&mut self) { - self.transformed_xs.clear(); - self.transformed_ys.clear(); - self.distances.clear(); - self.projections.clear(); - self.correspondences.clear(); - self.correspondences_soa.clear(); - self.ranges.clear(); - self.subsampled_xs.clear(); - self.subsampled_ys.clear(); - self.spatial_candidates.clear(); - } - - // ========================================================================= - // Subsampling methods for multi-resolution ICP - // ========================================================================= - - /// Subsample points into internal buffer using step_by. - /// - /// This is the zero-allocation version of subsampling for multi-resolution ICP. - /// After calling this, use `subsampled_point()` or `num_subsampled()` to access. - /// - /// # Arguments - /// * `points` - Source points to subsample - /// * `step` - Step size (1 = no subsampling, 2 = every other point, etc.) - #[inline] - pub fn subsample_points(&mut self, points: &[Point2D], step: usize) { - self.subsampled_xs.clear(); - self.subsampled_ys.clear(); - - let step = step.max(1); - let expected_len = points.len().div_ceil(step); - self.subsampled_xs.reserve(expected_len); - self.subsampled_ys.reserve(expected_len); - - for p in points.iter().step_by(step) { - self.subsampled_xs.push(p.x); - self.subsampled_ys.push(p.y); - } - } - - /// Subsample from SoA arrays into internal buffer. - /// - /// Useful when points are already in SoA format (e.g., from PointCloud2D). - #[inline] - pub fn subsample_from_soa(&mut self, xs: &[f32], ys: &[f32], step: usize) { - self.subsampled_xs.clear(); - self.subsampled_ys.clear(); - - let step = step.max(1); - let len = xs.len().min(ys.len()); - let expected_len = len.div_ceil(step); - self.subsampled_xs.reserve(expected_len); - self.subsampled_ys.reserve(expected_len); - - for i in (0..len).step_by(step) { - self.subsampled_xs.push(xs[i]); - self.subsampled_ys.push(ys[i]); - } - } - - /// Get a subsampled point by index. - #[inline] - pub fn subsampled_point(&self, index: usize) -> Point2D { - Point2D::new(self.subsampled_xs[index], self.subsampled_ys[index]) - } - - /// Get number of subsampled points. - #[inline] - pub fn num_subsampled(&self) -> usize { - self.subsampled_xs.len() - } - - /// Get slice of subsampled X coordinates. - #[inline] - pub fn subsampled_xs(&self) -> &[f32] { - &self.subsampled_xs - } - - /// Get slice of subsampled Y coordinates. - #[inline] - pub fn subsampled_ys(&self) -> &[f32] { - &self.subsampled_ys - } - - /// Transform points by pose into internal buffer. - /// - /// SIMD-optimized: processes 4 points per iteration for better performance. - /// After calling this, `transformed_point()` can be used to access results. - #[inline] - pub fn transform_points(&mut self, points: &[Point2D], sin: f32, cos: f32, tx: f32, ty: f32) { - let n = points.len(); - - // Resize buffers (keeps capacity, avoids reallocation) - self.transformed_xs.clear(); - self.transformed_ys.clear(); - self.transformed_xs.resize(n, 0.0); - self.transformed_ys.resize(n, 0.0); - - if n == 0 { - return; - } - - // SIMD constants - let cos4 = f32x4::splat(cos); - let sin4 = f32x4::splat(sin); - let tx4 = f32x4::splat(tx); - let ty4 = f32x4::splat(ty); - - // Process 4 points at a time - // Note: Point2D is stored as AoS, so we need to gather/scatter - let chunks = n / 4; - for i in 0..chunks { - let base = i * 4; - - // Gather 4 points into separate x and y vectors - let px = f32x4::from_array([ - points[base].x, - points[base + 1].x, - points[base + 2].x, - points[base + 3].x, - ]); - let py = f32x4::from_array([ - points[base].y, - points[base + 1].y, - points[base + 2].y, - points[base + 3].y, - ]); - - // Transform: new_x = x*cos - y*sin + tx - // new_y = x*sin + y*cos + ty - let new_x = px * cos4 - py * sin4 + tx4; - let new_y = px * sin4 + py * cos4 + ty4; - - // Scatter to output buffers - self.transformed_xs[base..base + 4].copy_from_slice(&new_x.to_array()); - self.transformed_ys[base..base + 4].copy_from_slice(&new_y.to_array()); - } - - // Handle remainder (scalar) - let remainder = chunks * 4; - for (p, (out_x, out_y)) in points[remainder..n].iter().zip( - self.transformed_xs[remainder..n] - .iter_mut() - .zip(&mut self.transformed_ys[remainder..n]), - ) { - *out_x = p.x * cos - p.y * sin + tx; - *out_y = p.x * sin + p.y * cos + ty; - } - } - - /// Get a transformed point by index. - #[inline] - pub fn transformed_point(&self, index: usize) -> Point2D { - Point2D::new(self.transformed_xs[index], self.transformed_ys[index]) - } - - /// Get number of transformed points. - #[inline] - pub fn num_transformed(&self) -> usize { - self.transformed_xs.len() - } - - /// Find correspondences using pre-allocated buffers. - /// - /// This is the zero-allocation hot path for ICP iterations. - pub fn find_correspondences(&mut self, lines: &LineCollection, config: &NearestNeighborConfig) { - self.correspondences.clear(); - - if self.transformed_xs.is_empty() || lines.is_empty() { - return; - } - - let min_t = -config.max_projection_extension; - let max_t = 1.0 + config.max_projection_extension; - let n_lines = lines.len(); - let n_points = self.transformed_xs.len(); - - // Ensure buffers are large enough - self.distances.resize(n_lines, 0.0); - self.projections.resize(n_lines, 0.0); - - for point_idx in 0..n_points { - let point = Point2D::new( - self.transformed_xs[point_idx], - self.transformed_ys[point_idx], - ); - - // Compute distances and projections in a single pass (cache-friendly) - lines.distances_and_projections_into(point, &mut self.distances, &mut self.projections); - - let mut best_distance = config.max_distance; - let mut best_corr: Option = None; - - for line_idx in 0..n_lines { - let t = self.projections[line_idx]; - let distance = self.distances[line_idx]; - - // Check projection bounds - if t < min_t || t > max_t { - continue; - } - - // Check distance threshold - if distance > config.max_distance { - continue; - } - - if config.unique_per_point { - if distance < best_distance { - best_distance = distance; - best_corr = - Some(Correspondence::new(point_idx, line_idx, point, distance, t)); - } - } else { - self.correspondences - .push(Correspondence::new(point_idx, line_idx, point, distance, t)); - } - } - - if config.unique_per_point - && let Some(corr) = best_corr - { - self.correspondences.push(corr); - } - } - } - - /// Find correspondences using a spatial index for O(n×k) instead of O(n×m). - /// - /// This is the accelerated hot path for ICP iterations when the map has many lines. - /// Uses spatial hashing to only check nearby lines for each point. - /// - /// # Arguments - /// * `lines` - Line collection (for distance/projection calculations) - /// * `spatial_index` - Pre-built spatial index for the lines - /// * `config` - Matching configuration - /// - /// # Performance - /// For a map with 100 lines and 360 points: - /// - Brute force: 36,000 distance calculations - /// - With spatial index: ~3,600 (assuming ~10 candidates per point) - pub fn find_correspondences_indexed( - &mut self, - lines: &LineCollection, - spatial_index: &LineSpatialIndex, - config: &NearestNeighborConfig, - ) { - self.correspondences.clear(); - - if self.transformed_xs.is_empty() || lines.is_empty() { - return; - } - - let min_t = -config.max_projection_extension; - let max_t = 1.0 + config.max_projection_extension; - let n_points = self.transformed_xs.len(); - - for point_idx in 0..n_points { - let point = Point2D::new( - self.transformed_xs[point_idx], - self.transformed_ys[point_idx], - ); - - // Query spatial index for candidate lines (O(k) instead of O(m)) - spatial_index.query_radius_into( - point, - config.max_distance, - &mut self.spatial_candidates, - ); - - if self.spatial_candidates.is_empty() { - continue; - } - - let mut best_distance = config.max_distance; - let mut best_corr: Option = None; - - // Only check candidate lines from spatial index - for &line_idx in &self.spatial_candidates { - // Compute distance and projection for this line - let sx = lines.start_xs[line_idx]; - let sy = lines.start_ys[line_idx]; - let ex = lines.end_xs[line_idx]; - let ey = lines.end_ys[line_idx]; - let inv_len = lines.inv_lengths()[line_idx]; - - let dx = ex - sx; - let dy = ey - sy; - let to_px = point.x - sx; - let to_py = point.y - sy; - - // Perpendicular distance - let cross = to_px * dy - to_py * dx; - let distance = cross.abs() * inv_len; - - // Projection parameter - let len_sq = dx * dx + dy * dy; - let t = if len_sq > f32::EPSILON { - (to_px * dx + to_py * dy) / len_sq - } else { - 0.5 - }; - - // Check projection bounds - if t < min_t || t > max_t { - continue; - } - - // Check distance threshold - if distance > config.max_distance { - continue; - } - - if config.unique_per_point { - if distance < best_distance { - best_distance = distance; - best_corr = - Some(Correspondence::new(point_idx, line_idx, point, distance, t)); - } - } else { - self.correspondences - .push(Correspondence::new(point_idx, line_idx, point, distance, t)); - } - } - - if config.unique_per_point - && let Some(corr) = best_corr - { - self.correspondences.push(corr); - } - } - } - - /// Take the correspondences out of the scratch space. - /// - /// This moves the correspondence set, leaving an empty one in the scratch space. - /// Use `clear()` to reset for the next iteration instead if you don't need ownership. - #[inline] - pub fn take_correspondences(&mut self) -> CorrespondenceSet { - std::mem::take(&mut self.correspondences) - } - - /// Get a reference to the current correspondences. - #[inline] - pub fn correspondences(&self) -> &CorrespondenceSet { - &self.correspondences - } - - /// Get the number of correspondences found. - #[inline] - pub fn num_correspondences(&self) -> usize { - self.correspondences.len() - } - - // ========================================================================= - // SIMD-optimized SoA correspondence finding - // ========================================================================= - - /// Store ranges for later use in SoA correspondence finding. - #[inline] - pub fn store_ranges(&mut self, points: &[Point2D]) { - self.ranges.clear(); - self.ranges.reserve(points.len()); - for p in points { - self.ranges.push((p.x * p.x + p.y * p.y).sqrt()); - } - } - - /// Find correspondences using pre-allocated buffers, producing SoA layout. - /// - /// This is the SIMD-optimized hot path for ICP iterations. - /// The result is stored in `self.correspondences_soa` and is ready for - /// use with `optimize_pose_simd()`. - /// - /// # Arguments - /// * `lines` - Map lines in SoA format - /// * `noise_model` - Lidar noise model for weight computation - /// * `config` - Matching configuration - pub fn find_correspondences_soa( - &mut self, - lines: &LineCollection, - noise_model: &LidarNoiseModel, - config: &NearestNeighborConfig, - ) { - self.correspondences_soa.clear(); - - if self.transformed_xs.is_empty() || lines.is_empty() { - return; - } - - let min_t = -config.max_projection_extension; - let max_t = 1.0 + config.max_projection_extension; - let n_lines = lines.len(); - let n_points = self.transformed_xs.len(); - - // Ensure buffers are large enough - self.distances.resize(n_lines, 0.0); - self.projections.resize(n_lines, 0.0); - - for point_idx in 0..n_points { - let point = Point2D::new( - self.transformed_xs[point_idx], - self.transformed_ys[point_idx], - ); - - // Compute distances and projections in a single pass (cache-friendly) - lines.distances_and_projections_into(point, &mut self.distances, &mut self.projections); - - // Compute weight for this point based on its range - let range = if point_idx < self.ranges.len() { - self.ranges[point_idx] - } else { - 1.0 // Default weight factor - }; - let weight = noise_model.weight(range); - - let mut best_distance = config.max_distance; - let mut best_line_idx: Option = None; - - for line_idx in 0..n_lines { - let t = self.projections[line_idx]; - let distance = self.distances[line_idx]; - - // Check projection bounds - if t < min_t || t > max_t { - continue; - } - - // Check distance threshold - if distance > config.max_distance { - continue; - } - - if config.unique_per_point { - if distance < best_distance { - best_distance = distance; - best_line_idx = Some(line_idx); - } - } else { - self.correspondences_soa - .push(point_idx, line_idx, point, distance, weight, lines); - } - } - - if config.unique_per_point - && let Some(line_idx) = best_line_idx - { - self.correspondences_soa.push( - point_idx, - line_idx, - point, - best_distance, - weight, - lines, - ); - } - } - - // Finalize with SIMD padding - self.correspondences_soa.finalize(); - } - - /// Get a reference to the current SoA correspondences. - #[inline] - pub fn correspondences_soa(&self) -> &CorrespondenceSoA { - &self.correspondences_soa - } - - /// Get the number of SoA correspondences found. - #[inline] - pub fn num_correspondences_soa(&self) -> usize { - self.correspondences_soa.len() - } - - /// Take the SoA correspondences out of the scratch space. - #[inline] - pub fn take_correspondences_soa(&mut self) -> CorrespondenceSoA { - std::mem::take(&mut self.correspondences_soa) - } -} - -impl Default for IcpScratchSpace { - fn default() -> Self { - Self::default_capacity() - } -} - -#[cfg(test)] -mod tests { - use super::*; - use crate::features::Line2D; - - fn make_room_lines() -> LineCollection { - let lines = vec![ - Line2D::new(Point2D::new(-2.0, -2.0), Point2D::new(2.0, -2.0)), // Bottom - Line2D::new(Point2D::new(2.0, -2.0), Point2D::new(2.0, 2.0)), // Right - Line2D::new(Point2D::new(2.0, 2.0), Point2D::new(-2.0, 2.0)), // Top - Line2D::new(Point2D::new(-2.0, 2.0), Point2D::new(-2.0, -2.0)), // Left - ]; - LineCollection::from_lines(&lines) - } - - #[test] - fn test_scratch_space_creation() { - let scratch = IcpScratchSpace::new(100, 50); - // Verify buffers have the expected capacity - assert!(scratch.transformed_xs.capacity() >= 100); - assert!(scratch.distances.capacity() >= 50); - } - - #[test] - fn test_transform_points() { - let mut scratch = IcpScratchSpace::new(10, 10); - let points = vec![Point2D::new(1.0, 0.0), Point2D::new(0.0, 1.0)]; - - // Identity transform - scratch.transform_points(&points, 0.0, 1.0, 0.0, 0.0); - - assert_eq!(scratch.num_transformed(), 2); - let p0 = scratch.transformed_point(0); - let p1 = scratch.transformed_point(1); - - assert!((p0.x - 1.0).abs() < 1e-6); - assert!((p0.y - 0.0).abs() < 1e-6); - assert!((p1.x - 0.0).abs() < 1e-6); - assert!((p1.y - 1.0).abs() < 1e-6); - } - - #[test] - fn test_transform_with_rotation() { - let mut scratch = IcpScratchSpace::new(10, 10); - let points = vec![Point2D::new(1.0, 0.0)]; - - // 90 degree rotation - let (sin, cos) = std::f32::consts::FRAC_PI_2.sin_cos(); - scratch.transform_points(&points, sin, cos, 0.0, 0.0); - - let p = scratch.transformed_point(0); - assert!((p.x - 0.0).abs() < 1e-6); - assert!((p.y - 1.0).abs() < 1e-6); - } - - #[test] - fn test_find_correspondences_basic() { - let mut scratch = IcpScratchSpace::new(100, 10); - let lines = make_room_lines(); - - // Points near the walls - let points = vec![ - Point2D::new(0.0, -1.95), // Near bottom wall - Point2D::new(1.95, 0.0), // Near right wall - Point2D::new(0.0, 1.95), // Near top wall - Point2D::new(-1.95, 0.0), // Near left wall - ]; - - // Identity transform - scratch.transform_points(&points, 0.0, 1.0, 0.0, 0.0); - - let config = NearestNeighborConfig::default(); - scratch.find_correspondences(&lines, &config); - - assert_eq!(scratch.num_correspondences(), 4); - } - - #[test] - fn test_find_correspondences_with_threshold() { - let mut scratch = IcpScratchSpace::new(100, 10); - let lines = make_room_lines(); - - // One point near wall, one far from walls - let points = vec![ - Point2D::new(0.0, -1.95), // Near bottom wall (0.05m away) - Point2D::new(0.0, 0.0), // Center (2.0m from all walls) - ]; - - scratch.transform_points(&points, 0.0, 1.0, 0.0, 0.0); - - let config = NearestNeighborConfig::default().with_max_distance(0.5); - scratch.find_correspondences(&lines, &config); - - // Only the first point should match - assert_eq!(scratch.num_correspondences(), 1); - } - - #[test] - fn test_clear_reuse() { - let mut scratch = IcpScratchSpace::new(100, 10); - let lines = make_room_lines(); - let points = vec![Point2D::new(0.0, -1.95)]; - - // First iteration - scratch.transform_points(&points, 0.0, 1.0, 0.0, 0.0); - let config = NearestNeighborConfig::default(); - scratch.find_correspondences(&lines, &config); - assert_eq!(scratch.num_correspondences(), 1); - - // Clear and reuse - scratch.clear(); - assert_eq!(scratch.num_correspondences(), 0); - assert_eq!(scratch.num_transformed(), 0); - - // Second iteration - scratch.transform_points(&points, 0.0, 1.0, 0.0, 0.0); - scratch.find_correspondences(&lines, &config); - assert_eq!(scratch.num_correspondences(), 1); - } - - #[test] - fn test_take_correspondences() { - let mut scratch = IcpScratchSpace::new(100, 10); - let lines = make_room_lines(); - let points = vec![Point2D::new(0.0, -1.95)]; - - scratch.transform_points(&points, 0.0, 1.0, 0.0, 0.0); - let config = NearestNeighborConfig::default(); - scratch.find_correspondences(&lines, &config); - - let corrs = scratch.take_correspondences(); - assert_eq!(corrs.len(), 1); - assert_eq!(scratch.num_correspondences(), 0); // Should be empty after take - } - - #[test] - fn test_empty_inputs() { - let mut scratch = IcpScratchSpace::new(100, 10); - let lines = LineCollection::new(); - let config = NearestNeighborConfig::default(); - - // Empty points - scratch.transform_points(&[], 0.0, 1.0, 0.0, 0.0); - scratch.find_correspondences(&lines, &config); - assert_eq!(scratch.num_correspondences(), 0); - - // Empty lines - let points = vec![Point2D::new(0.0, 0.0)]; - scratch.transform_points(&points, 0.0, 1.0, 0.0, 0.0); - scratch.find_correspondences(&lines, &config); - assert_eq!(scratch.num_correspondences(), 0); - } - - // ========================================================================= - // Subsample tests - // ========================================================================= - - #[test] - fn test_subsample_no_subsampling() { - let mut scratch = IcpScratchSpace::new(100, 10); - let points: Vec = (0..10).map(|i| Point2D::new(i as f32, 0.0)).collect(); - - scratch.subsample_points(&points, 1); - - assert_eq!(scratch.num_subsampled(), 10); - for i in 0..10 { - let p = scratch.subsampled_point(i); - assert!((p.x - i as f32).abs() < 1e-6); - assert!((p.y - 0.0).abs() < 1e-6); - } - } - - #[test] - fn test_subsample_every_other() { - let mut scratch = IcpScratchSpace::new(100, 10); - let points: Vec = (0..10).map(|i| Point2D::new(i as f32, 0.0)).collect(); - - scratch.subsample_points(&points, 2); - - assert_eq!(scratch.num_subsampled(), 5); - // Should have 0, 2, 4, 6, 8 - assert!((scratch.subsampled_point(0).x - 0.0).abs() < 1e-6); - assert!((scratch.subsampled_point(1).x - 2.0).abs() < 1e-6); - assert!((scratch.subsampled_point(2).x - 4.0).abs() < 1e-6); - assert!((scratch.subsampled_point(3).x - 6.0).abs() < 1e-6); - assert!((scratch.subsampled_point(4).x - 8.0).abs() < 1e-6); - } - - #[test] - fn test_subsample_step_3() { - let mut scratch = IcpScratchSpace::new(100, 10); - let points: Vec = (0..12).map(|i| Point2D::new(i as f32, 0.0)).collect(); - - scratch.subsample_points(&points, 3); - - assert_eq!(scratch.num_subsampled(), 4); - // Should have 0, 3, 6, 9 - assert!((scratch.subsampled_point(0).x - 0.0).abs() < 1e-6); - assert!((scratch.subsampled_point(1).x - 3.0).abs() < 1e-6); - assert!((scratch.subsampled_point(2).x - 6.0).abs() < 1e-6); - assert!((scratch.subsampled_point(3).x - 9.0).abs() < 1e-6); - } - - #[test] - fn test_subsample_soa() { - let mut scratch = IcpScratchSpace::new(100, 10); - let xs: Vec = (0..10).map(|i| i as f32).collect(); - let ys: Vec = (0..10).map(|i| (i as f32) * 2.0).collect(); - - scratch.subsample_from_soa(&xs, &ys, 2); - - assert_eq!(scratch.num_subsampled(), 5); - assert!((scratch.subsampled_xs()[0] - 0.0).abs() < 1e-6); - assert!((scratch.subsampled_xs()[1] - 2.0).abs() < 1e-6); - assert!((scratch.subsampled_ys()[0] - 0.0).abs() < 1e-6); - assert!((scratch.subsampled_ys()[1] - 4.0).abs() < 1e-6); - } - - #[test] - fn test_subsample_reuse() { - let mut scratch = IcpScratchSpace::new(100, 10); - - // First subsample - let points1: Vec = (0..10).map(|i| Point2D::new(i as f32, 0.0)).collect(); - scratch.subsample_points(&points1, 2); - assert_eq!(scratch.num_subsampled(), 5); - - // Second subsample (different data, reusing scratch) - let points2: Vec = (0..6).map(|i| Point2D::new(i as f32 * 10.0, 0.0)).collect(); - scratch.subsample_points(&points2, 3); - assert_eq!(scratch.num_subsampled(), 2); - // Should have 0.0, 30.0 - assert!((scratch.subsampled_point(0).x - 0.0).abs() < 1e-6); - assert!((scratch.subsampled_point(1).x - 30.0).abs() < 1e-6); - } - - #[test] - fn test_subsample_empty() { - let mut scratch = IcpScratchSpace::new(100, 10); - let points: Vec = vec![]; - - scratch.subsample_points(&points, 2); - - assert_eq!(scratch.num_subsampled(), 0); - } - - #[test] - fn test_subsample_step_zero_treated_as_one() { - let mut scratch = IcpScratchSpace::new(100, 10); - let points: Vec = (0..5).map(|i| Point2D::new(i as f32, 0.0)).collect(); - - // Step of 0 should be treated as 1 (no subsampling) - scratch.subsample_points(&points, 0); - - assert_eq!(scratch.num_subsampled(), 5); - } - - // ========================================================================= - // Spatial index tests - // ========================================================================= - - #[test] - fn test_find_correspondences_indexed_basic() { - let mut scratch = IcpScratchSpace::new(100, 10); - let lines = make_room_lines(); - let spatial_index = lines.build_spatial_index_for_radius(0.5); - - // Points near the walls - let points = vec![ - Point2D::new(0.0, -1.95), // Near bottom wall - Point2D::new(1.95, 0.0), // Near right wall - Point2D::new(0.0, 1.95), // Near top wall - Point2D::new(-1.95, 0.0), // Near left wall - ]; - - // Identity transform - scratch.transform_points(&points, 0.0, 1.0, 0.0, 0.0); - - let config = NearestNeighborConfig::default(); - scratch.find_correspondences_indexed(&lines, &spatial_index, &config); - - assert_eq!(scratch.num_correspondences(), 4); - } - - #[test] - fn test_find_correspondences_indexed_matches_brute_force() { - let mut scratch = IcpScratchSpace::new(100, 10); - let lines = make_room_lines(); - let spatial_index = lines.build_spatial_index_for_radius(0.5); - - let points = vec![Point2D::new(0.0, -1.95), Point2D::new(1.95, 0.0)]; - - scratch.transform_points(&points, 0.0, 1.0, 0.0, 0.0); - - let config = NearestNeighborConfig::default(); - - // Get results from brute force - scratch.find_correspondences(&lines, &config); - let brute_force_count = scratch.num_correspondences(); - let brute_force_corrs = scratch.take_correspondences(); - - // Get results from indexed - scratch.transform_points(&points, 0.0, 1.0, 0.0, 0.0); - scratch.find_correspondences_indexed(&lines, &spatial_index, &config); - let indexed_count = scratch.num_correspondences(); - - // Should find the same number of correspondences - assert_eq!(brute_force_count, indexed_count); - - // Verify the correspondences match - let indexed_corrs = scratch.correspondences(); - for bf_corr in brute_force_corrs.iter() { - let found = indexed_corrs.iter().any(|ic| { - ic.point_idx == bf_corr.point_idx - && ic.line_idx == bf_corr.line_idx - && (ic.distance - bf_corr.distance).abs() < 1e-6 - }); - assert!( - found, - "Brute force correspondence not found in indexed results" - ); - } - } - - #[test] - fn test_find_correspondences_indexed_with_threshold() { - let mut scratch = IcpScratchSpace::new(100, 10); - let lines = make_room_lines(); - let spatial_index = lines.build_spatial_index_for_radius(0.5); - - // One point near wall, one far from walls - let points = vec![ - Point2D::new(0.0, -1.95), // Near bottom wall (0.05m away) - Point2D::new(0.0, 0.0), // Center (2.0m from all walls) - ]; - - scratch.transform_points(&points, 0.0, 1.0, 0.0, 0.0); - - let config = NearestNeighborConfig::default().with_max_distance(0.5); - scratch.find_correspondences_indexed(&lines, &spatial_index, &config); - - // Only the first point should match - assert_eq!(scratch.num_correspondences(), 1); - } - - #[test] - fn test_find_correspondences_indexed_empty() { - let mut scratch = IcpScratchSpace::new(100, 10); - let lines = LineCollection::new(); - let spatial_index = lines.build_spatial_index(0.5); - let config = NearestNeighborConfig::default(); - - // Empty points - scratch.transform_points(&[], 0.0, 1.0, 0.0, 0.0); - scratch.find_correspondences_indexed(&lines, &spatial_index, &config); - assert_eq!(scratch.num_correspondences(), 0); - - // Non-empty points but empty lines - let lines = LineCollection::new(); - let spatial_index = lines.build_spatial_index(0.5); - let points = vec![Point2D::new(0.0, 0.0)]; - scratch.transform_points(&points, 0.0, 1.0, 0.0, 0.0); - scratch.find_correspondences_indexed(&lines, &spatial_index, &config); - assert_eq!(scratch.num_correspondences(), 0); - } -} diff --git a/vastu-map/src/matching/traits.rs b/vastu-map/src/matching/traits.rs deleted file mode 100644 index 168318a..0000000 --- a/vastu-map/src/matching/traits.rs +++ /dev/null @@ -1,150 +0,0 @@ -//! Traits for scan matching algorithms. -//! -//! This module defines the `ScanMatcher` trait which allows different -//! scan matching algorithms to be used interchangeably. - -use crate::core::{Point2D, Pose2D}; -use crate::features::{Line2D, LineCollection}; - -use super::correspondence::MatchResult; - -/// Trait for scan matching algorithms. -/// -/// Implementations match scan points against a map of line features -/// to estimate the robot's pose. -/// -/// # Example -/// -/// ```rust,ignore -/// use vastu_map::matching::{ScanMatcher, PointToLineIcp, IcpConfig}; -/// use vastu_map::core::{Point2D, Pose2D}; -/// use vastu_map::features::Line2D; -/// -/// let mut matcher = PointToLineIcp::with_config(IcpConfig::default()); -/// let points = vec![Point2D::new(1.0, 0.1)]; -/// let lines = vec![Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(5.0, 0.0))]; -/// -/// let result = matcher.match_scan(&points, &lines, Pose2D::identity()); -/// ``` -pub trait ScanMatcher: Send + Sync { - /// Match scan points against map lines. - /// - /// # Arguments - /// * `points` - Scan points (in robot frame or world frame depending on implementation) - /// * `lines` - Map lines in world frame - /// * `initial_pose` - Initial pose estimate (typically from odometry) - /// - /// # Returns - /// Match result with optimized pose, covariance, and confidence. - fn match_scan( - &mut self, - points: &[Point2D], - lines: &[Line2D], - initial_pose: Pose2D, - ) -> MatchResult; - - /// Match scan against a LineCollection (SoA format for SIMD). - /// - /// Default implementation converts to Line2D slice and calls `match_scan`. - fn match_scan_collection( - &mut self, - points: &[Point2D], - lines: &LineCollection, - initial_pose: Pose2D, - ) -> MatchResult { - let lines_vec = lines.to_lines(); - self.match_scan(points, &lines_vec, initial_pose) - } -} - -// PointToLineIcp implements ScanMatcher via its inherent methods -// We can't add a blanket impl here due to Rust's orphan rules, -// so we implement it directly on the type. - -impl ScanMatcher for super::PointToLineIcp { - fn match_scan( - &mut self, - points: &[Point2D], - lines: &[Line2D], - initial_pose: Pose2D, - ) -> MatchResult { - // Use the inherent match_scan method - super::PointToLineIcp::match_scan(self, points, lines, initial_pose) - } - - fn match_scan_collection( - &mut self, - points: &[Point2D], - lines: &LineCollection, - initial_pose: Pose2D, - ) -> MatchResult { - self.match_scan_batch(points, lines, initial_pose) - } -} - -#[cfg(test)] -mod tests { - use super::*; - use crate::matching::IcpConfig; - - fn make_room_lines() -> Vec { - vec![ - Line2D::new(Point2D::new(-2.0, -2.0), Point2D::new(2.0, -2.0)), - Line2D::new(Point2D::new(2.0, -2.0), Point2D::new(2.0, 2.0)), - Line2D::new(Point2D::new(2.0, 2.0), Point2D::new(-2.0, 2.0)), - Line2D::new(Point2D::new(-2.0, 2.0), Point2D::new(-2.0, -2.0)), - ] - } - - fn make_scan_points() -> Vec { - let mut points = Vec::new(); - for i in -5..=5 { - let x = i as f32 * 0.3; - points.push(Point2D::new(x, -2.0)); - points.push(Point2D::new(x, 2.0)); - } - for i in -5..=5 { - let y = i as f32 * 0.3; - points.push(Point2D::new(-2.0, y)); - points.push(Point2D::new(2.0, y)); - } - points - } - - #[test] - fn test_scan_matcher_trait() { - let mut matcher = super::super::PointToLineIcp::with_config(IcpConfig::default()); - let lines = make_room_lines(); - let points = make_scan_points(); - - let result = matcher.match_scan(&points, &lines, Pose2D::identity()); - - assert!(result.converged); - } - - #[test] - fn test_trait_object() { - // Verify trait can be used as a trait object - let mut matcher: Box = Box::new( - super::super::PointToLineIcp::with_config(IcpConfig::default()), - ); - - let lines = make_room_lines(); - let points = make_scan_points(); - - let result = matcher.match_scan(&points, &lines, Pose2D::identity()); - assert!(result.converged); - } - - #[test] - fn test_collection_method() { - let mut matcher = super::super::PointToLineIcp::with_config(IcpConfig::default()); - let lines = make_room_lines(); - let collection = LineCollection::from_lines(&lines); - let points = make_scan_points(); - - let result = matcher.match_scan_collection(&points, &collection, Pose2D::identity()); - - assert!(result.converged); - } -} diff --git a/vastu-map/src/motion_model.rs b/vastu-map/src/motion_model.rs deleted file mode 100644 index 53d9002..0000000 --- a/vastu-map/src/motion_model.rs +++ /dev/null @@ -1,290 +0,0 @@ -//! Motion model for pose prediction. -//! -//! This is a utility struct for callers (e.g., dhruva-slam) to use when -//! computing initial pose estimates for ICP when odometry is unavailable -//! or unreliable. -//! -//! # Usage -//! -//! ```rust,ignore -//! use vastu_map::MotionModel; -//! -//! let mut motion = MotionModel::new(); -//! -//! // After each successful ICP: -//! motion.update(icp_result.pose, current_time); -//! -//! // For next scan, if odometry unavailable: -//! let predicted = motion.predict(next_time); -//! let result = icp.match_scan(&points, &lines, predicted, &config); -//! ``` - -use crate::core::{Pose2D, math::angle_diff}; - -/// Constant-velocity motion model for pose prediction. -/// -/// Maintains an exponentially-smoothed velocity estimate based on -/// observed pose changes, and uses this to predict future poses. -#[derive(Clone, Debug)] -pub struct MotionModel { - /// Last observed pose. - last_pose: Pose2D, - /// Smoothed velocity estimate (vx, vy, omega). - velocity: (f32, f32, f32), - /// Timestamp of last observation. - last_time: f64, - /// Smoothing factor for velocity updates (0-1). - /// Higher = more smoothing (slower response to changes). - smoothing: f32, - /// Whether the model has been initialized with at least one observation. - initialized: bool, -} - -impl MotionModel { - /// Create a new motion model with default smoothing (0.7). - pub fn new() -> Self { - Self { - last_pose: Pose2D::identity(), - velocity: (0.0, 0.0, 0.0), - last_time: 0.0, - smoothing: 0.7, - initialized: false, - } - } - - /// Create a motion model with custom smoothing factor. - /// - /// # Arguments - /// * `smoothing` - Smoothing factor in [0, 1). - /// - 0.0 = no smoothing (velocity tracks instantly) - /// - 0.9 = high smoothing (slow response to velocity changes) - /// Default is 0.7 for a balance of responsiveness and stability. - pub fn with_smoothing(smoothing: f32) -> Self { - Self { - smoothing: smoothing.clamp(0.0, 0.99), - ..Self::new() - } - } - - /// Predict pose at a given time based on current velocity estimate. - /// - /// Returns identity pose if not yet initialized. - /// - /// # Arguments - /// * `current_time` - Time at which to predict (seconds) - pub fn predict(&self, current_time: f64) -> Pose2D { - if !self.initialized { - return Pose2D::identity(); - } - - let dt = (current_time - self.last_time) as f32; - if dt <= 0.0 { - return self.last_pose; - } - - Pose2D::new( - self.last_pose.x + self.velocity.0 * dt, - self.last_pose.y + self.velocity.1 * dt, - self.last_pose.theta + self.velocity.2 * dt, - ) - } - - /// Update the motion model with a new pose observation. - /// - /// This updates the internal velocity estimate using exponential smoothing. - /// - /// # Arguments - /// * `new_pose` - Observed pose (typically from ICP result) - /// * `current_time` - Time of observation (seconds) - pub fn update(&mut self, new_pose: Pose2D, current_time: f64) { - if self.initialized { - let dt = (current_time - self.last_time) as f32; - if dt > 0.001 { - // Compute instantaneous velocity - let new_vx = (new_pose.x - self.last_pose.x) / dt; - let new_vy = (new_pose.y - self.last_pose.y) / dt; - let new_omega = angle_diff(self.last_pose.theta, new_pose.theta) / dt; - - // Apply exponential smoothing - let s = self.smoothing; - self.velocity.0 = s * self.velocity.0 + (1.0 - s) * new_vx; - self.velocity.1 = s * self.velocity.1 + (1.0 - s) * new_vy; - self.velocity.2 = s * self.velocity.2 + (1.0 - s) * new_omega; - } - } - - self.last_pose = new_pose; - self.last_time = current_time; - self.initialized = true; - } - - /// Reset the motion model to uninitialized state. - pub fn reset(&mut self) { - *self = Self::with_smoothing(self.smoothing); - } - - /// Get the current velocity estimate (vx, vy, omega). - /// - /// Returns (0, 0, 0) if not initialized. - pub fn velocity(&self) -> (f32, f32, f32) { - if self.initialized { - self.velocity - } else { - (0.0, 0.0, 0.0) - } - } - - /// Get the linear speed (magnitude of translational velocity). - pub fn speed(&self) -> f32 { - let (vx, vy, _) = self.velocity(); - (vx * vx + vy * vy).sqrt() - } - - /// Get the angular velocity (omega). - pub fn angular_velocity(&self) -> f32 { - self.velocity().2 - } - - /// Check if the model has been initialized. - pub fn is_initialized(&self) -> bool { - self.initialized - } - - /// Get the last observed pose. - pub fn last_pose(&self) -> Pose2D { - self.last_pose - } - - /// Get the timestamp of the last observation. - pub fn last_time(&self) -> f64 { - self.last_time - } - - /// Set the smoothing factor. - pub fn set_smoothing(&mut self, smoothing: f32) { - self.smoothing = smoothing.clamp(0.0, 0.99); - } -} - -impl Default for MotionModel { - fn default() -> Self { - Self::new() - } -} - -#[cfg(test)] -mod tests { - use super::*; - use approx::assert_relative_eq; - - #[test] - fn test_new_model_not_initialized() { - let model = MotionModel::new(); - assert!(!model.is_initialized()); - assert_eq!(model.velocity(), (0.0, 0.0, 0.0)); - } - - #[test] - fn test_predict_uninitialized_returns_identity() { - let model = MotionModel::new(); - let predicted = model.predict(1.0); - assert_relative_eq!(predicted.x, 0.0); - assert_relative_eq!(predicted.y, 0.0); - assert_relative_eq!(predicted.theta, 0.0); - } - - #[test] - fn test_first_update_initializes() { - let mut model = MotionModel::new(); - model.update(Pose2D::new(1.0, 2.0, 0.5), 0.0); - - assert!(model.is_initialized()); - assert_relative_eq!(model.last_pose().x, 1.0); - assert_relative_eq!(model.last_pose().y, 2.0); - // First update has no velocity estimate - assert_eq!(model.velocity(), (0.0, 0.0, 0.0)); - } - - #[test] - fn test_velocity_estimation() { - let mut model = MotionModel::with_smoothing(0.0); // No smoothing for predictable test - - // First observation - model.update(Pose2D::new(0.0, 0.0, 0.0), 0.0); - - // Second observation: moved 1m in x over 1 second - model.update(Pose2D::new(1.0, 0.0, 0.0), 1.0); - - let (vx, vy, omega) = model.velocity(); - assert_relative_eq!(vx, 1.0, epsilon = 0.01); - assert_relative_eq!(vy, 0.0, epsilon = 0.01); - assert_relative_eq!(omega, 0.0, epsilon = 0.01); - } - - #[test] - fn test_prediction() { - let mut model = MotionModel::with_smoothing(0.0); - - model.update(Pose2D::new(0.0, 0.0, 0.0), 0.0); - model.update(Pose2D::new(1.0, 0.0, 0.0), 1.0); - - // Predict 1 second ahead - let predicted = model.predict(2.0); - assert_relative_eq!(predicted.x, 2.0, epsilon = 0.01); - assert_relative_eq!(predicted.y, 0.0, epsilon = 0.01); - } - - #[test] - fn test_smoothing_effect() { - let mut model = MotionModel::with_smoothing(0.5); - - model.update(Pose2D::new(0.0, 0.0, 0.0), 0.0); - model.update(Pose2D::new(1.0, 0.0, 0.0), 1.0); // vx = 1.0 - - // With smoothing=0.5: new_vx = 0.5 * 0 + 0.5 * 1.0 = 0.5 - let (vx, _, _) = model.velocity(); - assert_relative_eq!(vx, 0.5, epsilon = 0.01); - - // Another update with same velocity - model.update(Pose2D::new(2.0, 0.0, 0.0), 2.0); // instantaneous vx = 1.0 - - // new_vx = 0.5 * 0.5 + 0.5 * 1.0 = 0.75 - let (vx, _, _) = model.velocity(); - assert_relative_eq!(vx, 0.75, epsilon = 0.01); - } - - #[test] - fn test_reset() { - let mut model = MotionModel::with_smoothing(0.8); - model.update(Pose2D::new(1.0, 1.0, 1.0), 1.0); - model.update(Pose2D::new(2.0, 2.0, 2.0), 2.0); - - model.reset(); - - assert!(!model.is_initialized()); - assert_eq!(model.velocity(), (0.0, 0.0, 0.0)); - // Smoothing should be preserved - assert_relative_eq!(model.smoothing, 0.8); - } - - #[test] - fn test_speed() { - let mut model = MotionModel::with_smoothing(0.0); - - model.update(Pose2D::new(0.0, 0.0, 0.0), 0.0); - model.update(Pose2D::new(3.0, 4.0, 0.0), 1.0); - - // Speed should be sqrt(3^2 + 4^2) = 5 - assert_relative_eq!(model.speed(), 5.0, epsilon = 0.01); - } - - #[test] - fn test_angular_velocity() { - let mut model = MotionModel::with_smoothing(0.0); - - model.update(Pose2D::new(0.0, 0.0, 0.0), 0.0); - model.update(Pose2D::new(0.0, 0.0, 0.5), 1.0); - - assert_relative_eq!(model.angular_velocity(), 0.5, epsilon = 0.01); - } -} diff --git a/vastu-map/src/odometry.rs b/vastu-map/src/odometry.rs deleted file mode 100644 index fa2540b..0000000 --- a/vastu-map/src/odometry.rs +++ /dev/null @@ -1,405 +0,0 @@ -//! Wheel odometry for differential drive robots. -//! -//! Computes robot pose changes from wheel encoder ticks using -//! standard differential drive kinematics with arc-based integration. -//! -//! # Example -//! -//! ```rust -//! use vastu_map::odometry::WheelOdometry; -//! -//! // Create odometry tracker (0.3m wheel base, 1000 ticks/meter) -//! let mut odom = WheelOdometry::new(0.3, 1000.0); -//! -//! // Process encoder readings -//! let delta = odom.update(100, 100); // Straight forward -//! println!("Moved: ({:.3}, {:.3}, {:.3})", delta.x, delta.y, delta.theta); -//! -//! // Get cumulative pose -//! let total = odom.cumulative(); -//! println!("Total: ({:.3}, {:.3}, {:.3})", total.x, total.y, total.theta); -//! ``` - -use crate::core::Pose2D; - -/// Differential drive wheel odometry. -/// -/// Tracks robot pose from wheel encoder ticks. Uses arc-based motion model -/// for accurate integration, especially during turns. -/// -/// # Coordinate Frame -/// -/// Follows ROS REP-103 convention: -/// - X-forward (positive X is in front of robot) -/// - Y-left (positive Y is to the left) -/// - Theta counter-clockwise positive -#[derive(Clone, Debug)] -pub struct WheelOdometry { - /// Distance between wheel centers (meters). - wheel_base: f32, - - /// Encoder ticks per meter of wheel travel. - ticks_per_meter: f32, - - /// Last left encoder reading (ticks). - last_left: i32, - - /// Last right encoder reading (ticks). - last_right: i32, - - /// Cumulative pose from origin. - cumulative_pose: Pose2D, - - /// Whether the odometry has been initialized with first reading. - initialized: bool, -} - -impl WheelOdometry { - /// Create a new wheel odometry tracker. - /// - /// # Arguments - /// - /// * `wheel_base` - Distance between wheel centers in meters - /// * `ticks_per_meter` - Encoder ticks per meter of wheel travel - /// - /// # Panics - /// - /// Panics if `wheel_base` or `ticks_per_meter` is not positive. - pub fn new(wheel_base: f32, ticks_per_meter: f32) -> Self { - assert!(wheel_base > 0.0, "wheel_base must be positive"); - assert!(ticks_per_meter > 0.0, "ticks_per_meter must be positive"); - - Self { - wheel_base, - ticks_per_meter, - last_left: 0, - last_right: 0, - cumulative_pose: Pose2D::identity(), - initialized: false, - } - } - - /// Process encoder readings and return pose delta since last update. - /// - /// On the first call, initializes the encoder baselines and returns identity pose. - /// Subsequent calls compute the pose change using arc-based motion model. - /// - /// # Arguments - /// - /// * `left_ticks` - Current left wheel encoder reading (ticks) - /// * `right_ticks` - Current right wheel encoder reading (ticks) - /// - /// # Returns - /// - /// Pose delta in robot frame (how the robot moved since last update). - pub fn update(&mut self, left_ticks: i32, right_ticks: i32) -> Pose2D { - if !self.initialized { - // First reading - just store baselines - self.last_left = left_ticks; - self.last_right = right_ticks; - self.initialized = true; - return Pose2D::identity(); - } - - // Compute tick deltas - let delta_left_ticks = left_ticks - self.last_left; - let delta_right_ticks = right_ticks - self.last_right; - - // Update stored values - self.last_left = left_ticks; - self.last_right = right_ticks; - - // Convert to meters - let delta_left = delta_left_ticks as f32 / self.ticks_per_meter; - let delta_right = delta_right_ticks as f32 / self.ticks_per_meter; - - // Differential drive kinematics - let delta_theta = (delta_right - delta_left) / self.wheel_base; - let delta_linear = (delta_left + delta_right) / 2.0; - - // Robot-frame delta (for return value, used by SLAM's compose()) - let (dx_robot, dy_robot) = if delta_theta.abs() < 1e-6 { - // Straight motion: forward is +X in robot frame - (delta_linear, 0.0) - } else { - // Arc motion in robot's starting frame - let radius = delta_linear / delta_theta; - ( - radius * delta_theta.sin(), // Forward displacement - radius * (1.0 - delta_theta.cos()), // Lateral displacement (left +) - ) - }; - - // World-frame delta (for cumulative pose tracking) - let theta = self.cumulative_pose.theta; - let (sin_t, cos_t) = theta.sin_cos(); - let dx_world = dx_robot * cos_t - dy_robot * sin_t; - let dy_world = dx_robot * sin_t + dy_robot * cos_t; - - // Create delta pose (in robot frame for compose()) - let delta = Pose2D::new(dx_robot, dy_robot, delta_theta); - - // Update cumulative pose (world frame) - self.cumulative_pose = Pose2D::new( - self.cumulative_pose.x + dx_world, - self.cumulative_pose.y + dy_world, - crate::core::math::normalize_angle(self.cumulative_pose.theta + delta_theta), - ); - - delta - } - - /// Get the cumulative pose from origin. - /// - /// This is the total pose accumulated since creation or last reset. - #[inline] - pub fn cumulative(&self) -> Pose2D { - self.cumulative_pose - } - - /// Reset odometry to origin. - /// - /// Clears the cumulative pose but keeps the current encoder readings - /// as the new baseline. - pub fn reset(&mut self) { - self.cumulative_pose = Pose2D::identity(); - } - - /// Reset odometry completely. - /// - /// Clears both cumulative pose and encoder baselines. - /// The next `update()` call will re-initialize. - pub fn reset_all(&mut self) { - self.cumulative_pose = Pose2D::identity(); - self.last_left = 0; - self.last_right = 0; - self.initialized = false; - } - - /// Get the wheel base (distance between wheels). - #[inline] - pub fn wheel_base(&self) -> f32 { - self.wheel_base - } - - /// Get the ticks per meter conversion factor. - #[inline] - pub fn ticks_per_meter(&self) -> f32 { - self.ticks_per_meter - } - - /// Check if odometry has been initialized. - #[inline] - pub fn is_initialized(&self) -> bool { - self.initialized - } -} - -#[cfg(test)] -mod tests { - use super::*; - use approx::assert_relative_eq; - use std::f32::consts::PI; - - #[test] - fn test_new() { - let odom = WheelOdometry::new(0.3, 1000.0); - assert_eq!(odom.wheel_base(), 0.3); - assert_eq!(odom.ticks_per_meter(), 1000.0); - assert!(!odom.is_initialized()); - } - - #[test] - #[should_panic(expected = "wheel_base must be positive")] - fn test_new_invalid_wheel_base() { - WheelOdometry::new(0.0, 1000.0); - } - - #[test] - #[should_panic(expected = "ticks_per_meter must be positive")] - fn test_new_invalid_ticks_per_meter() { - WheelOdometry::new(0.3, 0.0); - } - - #[test] - fn test_first_update_initializes() { - let mut odom = WheelOdometry::new(0.3, 1000.0); - - let delta = odom.update(100, 100); - - assert!(odom.is_initialized()); - assert_eq!(delta, Pose2D::identity()); - } - - #[test] - fn test_straight_forward() { - let mut odom = WheelOdometry::new(0.3, 1000.0); - - // Initialize - odom.update(0, 0); - - // Move 1 meter forward (1000 ticks on each wheel) - let delta = odom.update(1000, 1000); - - assert_relative_eq!(delta.x, 1.0, epsilon = 0.001); - assert_relative_eq!(delta.y, 0.0, epsilon = 0.001); - assert_relative_eq!(delta.theta, 0.0, epsilon = 0.001); - - let cum = odom.cumulative(); - assert_relative_eq!(cum.x, 1.0, epsilon = 0.001); - assert_relative_eq!(cum.y, 0.0, epsilon = 0.001); - } - - #[test] - fn test_straight_backward() { - let mut odom = WheelOdometry::new(0.3, 1000.0); - - odom.update(0, 0); - - // Move 0.5 meter backward - let delta = odom.update(-500, -500); - - assert_relative_eq!(delta.x, -0.5, epsilon = 0.001); - assert_relative_eq!(delta.y, 0.0, epsilon = 0.001); - assert_relative_eq!(delta.theta, 0.0, epsilon = 0.001); - } - - #[test] - fn test_rotate_in_place_ccw() { - let mut odom = WheelOdometry::new(0.3, 1000.0); - - odom.update(0, 0); - - // Rotate counter-clockwise: left wheel back, right wheel forward - // For 90° rotation: arc = wheel_base * theta / 2 for each wheel - // theta = PI/2, arc = 0.3 * PI/2 / 2 = 0.2356m per wheel - // ticks = 0.2356 * 1000 = 235.6 - let ticks = (0.3 * PI / 4.0 * 1000.0) as i32; // ~236 - let delta = odom.update(-ticks, ticks); - - // Should rotate ~90° with minimal translation - assert_relative_eq!(delta.theta, PI / 2.0, epsilon = 0.01); - // Translation should be minimal for pure rotation - assert!(delta.x.abs() < 0.01); - } - - #[test] - fn test_arc_motion() { - let mut odom = WheelOdometry::new(0.3, 1000.0); - - odom.update(0, 0); - - // Arc motion: right wheel travels further than left - // This should result in a left turn - let delta = odom.update(800, 1200); - - // Should have forward motion and counter-clockwise rotation - assert!(delta.x > 0.0); - assert!(delta.theta > 0.0); - } - - #[test] - fn test_cumulative_multiple_updates() { - let mut odom = WheelOdometry::new(0.3, 1000.0); - - odom.update(0, 0); - - // Move forward 1m - odom.update(1000, 1000); - // Move forward another 1m - odom.update(2000, 2000); - - let cum = odom.cumulative(); - assert_relative_eq!(cum.x, 2.0, epsilon = 0.001); - assert_relative_eq!(cum.y, 0.0, epsilon = 0.001); - } - - #[test] - fn test_reset() { - let mut odom = WheelOdometry::new(0.3, 1000.0); - - odom.update(0, 0); - odom.update(1000, 1000); - - assert!(odom.cumulative().x > 0.0); - - odom.reset(); - - let cum = odom.cumulative(); - assert_eq!(cum.x, 0.0); - assert_eq!(cum.y, 0.0); - assert_eq!(cum.theta, 0.0); - - // Should still be initialized - assert!(odom.is_initialized()); - } - - #[test] - fn test_reset_all() { - let mut odom = WheelOdometry::new(0.3, 1000.0); - - odom.update(0, 0); - odom.update(1000, 1000); - - odom.reset_all(); - - assert!(!odom.is_initialized()); - - // Next update should re-initialize - let delta = odom.update(500, 500); - assert_eq!(delta, Pose2D::identity()); - assert!(odom.is_initialized()); - } - - #[test] - fn test_encoder_overflow_handling() { - let mut odom = WheelOdometry::new(0.3, 1000.0); - - // Start near max i32 - odom.update(i32::MAX - 500, i32::MAX - 500); - - // Overflow (in real hardware, this would wrap) - // For now, just ensure it doesn't panic - let delta = odom.update(i32::MAX - 400, i32::MAX - 400); - - // Should show forward motion of 100 ticks = 0.1m - assert_relative_eq!(delta.x, 0.1, epsilon = 0.001); - } - - #[test] - fn test_delta_is_robot_frame_after_rotation() { - // This test verifies that update() returns robot-frame delta, - // not world-frame. After rotating 90°, moving forward should - // give delta.x > 0 (forward in robot frame), not delta.y > 0. - let mut odom = WheelOdometry::new(0.3, 1000.0); - - // Initialize - odom.update(0, 0); - - // Rotate 90° CCW (in-place rotation) - // arc = wheel_base * theta / 2 = 0.3 * π/2 / 2 = 0.2356m per wheel - let ticks = (0.3 * PI / 4.0 * 1000.0) as i32; // ~236 ticks - let left = -ticks; - let right = ticks; - odom.update(left, right); - - // Verify we're now at ~90° heading - let cum = odom.cumulative(); - assert_relative_eq!(cum.theta, PI / 2.0, epsilon = 0.02); - - // Now move forward 0.5m (equal wheel ticks) - let forward_ticks = 500; - let delta = odom.update(left + forward_ticks, right + forward_ticks); - - // Delta should be in ROBOT frame: forward is +X regardless of world heading - // If bug existed (world frame), delta would be (0, 0.5) since robot faces +Y - assert_relative_eq!(delta.x, 0.5, epsilon = 0.01); // Forward in robot frame - assert_relative_eq!(delta.y, 0.0, epsilon = 0.01); // No lateral movement - assert_relative_eq!(delta.theta, 0.0, epsilon = 0.01); // No rotation - - // But cumulative should be in WORLD frame: robot at ~(0, 0.5) - let cum2 = odom.cumulative(); - assert_relative_eq!(cum2.x, 0.0, epsilon = 0.05); // Started near origin - assert_relative_eq!(cum2.y, 0.5, epsilon = 0.05); // Moved in +Y direction - } -} diff --git a/vastu-map/src/pathfinding/astar.rs b/vastu-map/src/pathfinding/astar.rs new file mode 100644 index 0000000..51b1c62 --- /dev/null +++ b/vastu-map/src/pathfinding/astar.rs @@ -0,0 +1,822 @@ +//! A* pathfinding algorithm. +//! +//! Implements A* search on the occupancy grid with support for: +//! - Robot footprint collision checking +//! - Safety margin around obstacles +//! - 8-connected grid movement + +use crate::core::{GridCoord, WorldPoint}; +use crate::grid::GridStorage; +use crate::query::{RobotFootprint, TraversabilityChecker}; +use log::{debug, trace}; +use std::cmp::Ordering; +use std::collections::{BinaryHeap, HashMap, HashSet}; + +/// A node in the A* search +#[derive(Clone, Debug)] +struct AStarNode { + coord: GridCoord, + g_cost: f32, // Cost from start + f_cost: f32, // g_cost + heuristic +} + +impl Eq for AStarNode {} + +impl PartialEq for AStarNode { + fn eq(&self, other: &Self) -> bool { + self.coord == other.coord + } +} + +impl Ord for AStarNode { + fn cmp(&self, other: &Self) -> Ordering { + // Reverse ordering for min-heap behavior + other + .f_cost + .partial_cmp(&self.f_cost) + .unwrap_or(Ordering::Equal) + } +} + +impl PartialOrd for AStarNode { + fn partial_cmp(&self, other: &Self) -> Option { + Some(self.cmp(other)) + } +} + +/// A* pathfinding configuration +#[derive(Clone, Debug)] +pub struct AStarConfig { + /// Robot footprint for collision checking + pub footprint: RobotFootprint, + /// Allow diagonal movement (8-connected vs 4-connected) + pub allow_diagonal: bool, + /// Diagonal movement cost multiplier (sqrt(2) ≈ 1.414) + pub diagonal_cost: f32, + /// Maximum number of nodes to expand before giving up + pub max_iterations: usize, + /// Prefer paths with more clearance from obstacles + pub clearance_weight: f32, + /// Exploration mode: allow Unknown cells in footprint + /// (assumes unknown areas are likely free) + pub exploration_mode: bool, +} + +impl Default for AStarConfig { + fn default() -> Self { + Self { + footprint: RobotFootprint::default(), + allow_diagonal: true, + diagonal_cost: std::f32::consts::SQRT_2, + max_iterations: 100_000, + clearance_weight: 0.0, // Disabled by default for performance + exploration_mode: false, + } + } +} + +impl AStarConfig { + /// Create with custom robot footprint + pub fn with_footprint(footprint: RobotFootprint) -> Self { + Self { + footprint, + ..Default::default() + } + } + + /// Enable clearance-weighted pathfinding + pub fn with_clearance_weight(mut self, weight: f32) -> Self { + self.clearance_weight = weight; + self + } +} + +/// Result of A* pathfinding +#[derive(Clone, Debug)] +pub struct PathResult { + /// Path as grid coordinates (empty if no path found) + pub path_grid: Vec, + /// Path as world coordinates + pub path_world: Vec, + /// Total path cost + pub cost: f32, + /// Number of nodes expanded during search + pub nodes_expanded: usize, + /// Whether a path was found + pub success: bool, + /// Reason for failure (if any) + pub failure_reason: Option, +} + +impl PathResult { + /// Create a failed result + fn failed(reason: PathFailure, nodes_expanded: usize) -> Self { + Self { + path_grid: Vec::new(), + path_world: Vec::new(), + cost: f32::INFINITY, + nodes_expanded, + success: false, + failure_reason: Some(reason), + } + } + + /// Path length in cells + pub fn length_cells(&self) -> usize { + self.path_grid.len() + } + + /// Path length in meters (approximate) + pub fn length_meters(&self) -> f32 { + if self.path_world.len() < 2 { + return 0.0; + } + + let mut total = 0.0; + for i in 1..self.path_world.len() { + total += self.path_world[i - 1].distance(&self.path_world[i]); + } + total + } +} + +/// Reason for path failure +#[derive(Clone, Debug, PartialEq, Eq)] +pub enum PathFailure { + /// Start position is not traversable + StartBlocked, + /// Goal position is not traversable + GoalBlocked, + /// No path exists between start and goal + NoPath, + /// Maximum iterations exceeded + MaxIterationsExceeded, + /// Start or goal is out of bounds + OutOfBounds, +} + +/// A* pathfinder +pub struct AStarPlanner<'a> { + storage: &'a GridStorage, + config: AStarConfig, +} + +impl<'a> AStarPlanner<'a> { + /// Create a new A* planner + pub fn new(storage: &'a GridStorage, config: AStarConfig) -> Self { + Self { storage, config } + } + + /// Create with default configuration + pub fn with_defaults(storage: &'a GridStorage) -> Self { + Self::new(storage, AStarConfig::default()) + } + + /// Find a path from start to goal (grid coordinates) + pub fn find_path(&self, start: GridCoord, goal: GridCoord) -> PathResult { + trace!( + "[AStar] find_path: start=({},{}) goal=({},{})", + start.x, start.y, goal.x, goal.y + ); + + // Check bounds + if !self.storage.is_valid_coord(start) || !self.storage.is_valid_coord(goal) { + debug!("[AStar] FAILED: OutOfBounds - start or goal outside grid"); + return PathResult::failed(PathFailure::OutOfBounds, 0); + } + + // Check start and goal traversability using footprint + let checker = TraversabilityChecker::new(self.storage, self.config.footprint.clone()); + + let start_world = self.storage.grid_to_world(start); + let goal_world = self.storage.grid_to_world(goal); + + if !checker.is_position_safe_mode(start_world, self.config.exploration_mode) { + debug!( + "[AStar] FAILED: StartBlocked at ({:.2},{:.2}), exploration_mode={}", + start_world.x, start_world.y, self.config.exploration_mode + ); + return PathResult::failed(PathFailure::StartBlocked, 0); + } + if !checker.is_position_safe_mode(goal_world, self.config.exploration_mode) { + debug!( + "[AStar] FAILED: GoalBlocked at ({:.2},{:.2}), exploration_mode={}", + goal_world.x, goal_world.y, self.config.exploration_mode + ); + return PathResult::failed(PathFailure::GoalBlocked, 0); + } + + // A* search + let mut open_set = BinaryHeap::new(); + let mut closed_set = HashSet::new(); + let mut came_from: HashMap = HashMap::new(); + let mut g_scores: HashMap = HashMap::new(); + + let h_start = self.heuristic(start, goal); + open_set.push(AStarNode { + coord: start, + g_cost: 0.0, + f_cost: h_start, + }); + g_scores.insert(start, 0.0); + + let mut nodes_expanded = 0; + + while let Some(current) = open_set.pop() { + nodes_expanded += 1; + + if nodes_expanded > self.config.max_iterations { + debug!( + "[AStar] FAILED: MaxIterationsExceeded ({} nodes)", + nodes_expanded + ); + return PathResult::failed(PathFailure::MaxIterationsExceeded, nodes_expanded); + } + + // Goal reached + if current.coord == goal { + return self.reconstruct_path(came_from, goal, current.g_cost, nodes_expanded); + } + + if closed_set.contains(¤t.coord) { + continue; + } + closed_set.insert(current.coord); + + // Explore neighbors + let neighbors = if self.config.allow_diagonal { + current.coord.neighbors_8().to_vec() + } else { + current.coord.neighbors_4().to_vec() + }; + + for (i, neighbor) in neighbors.iter().enumerate() { + if closed_set.contains(neighbor) { + continue; + } + + if !self.storage.is_valid_coord(*neighbor) { + continue; + } + + // Check traversability with footprint + let neighbor_world = self.storage.grid_to_world(*neighbor); + if !checker.is_position_safe_mode(neighbor_world, self.config.exploration_mode) { + continue; + } + + // Calculate movement cost + let is_diagonal = self.config.allow_diagonal && i >= 4; + let move_cost = if is_diagonal { + self.config.diagonal_cost + } else { + 1.0 + }; + + // Add clearance cost if enabled + let clearance_cost = if self.config.clearance_weight > 0.0 { + let clearance = checker.clearance(neighbor_world, 1.0); + // Penalize positions close to obstacles + self.config.clearance_weight / (clearance + 0.1) + } else { + 0.0 + }; + + let tentative_g = g_scores[¤t.coord] + move_cost + clearance_cost; + + let current_g = g_scores.get(neighbor).copied().unwrap_or(f32::INFINITY); + if tentative_g < current_g { + came_from.insert(*neighbor, current.coord); + g_scores.insert(*neighbor, tentative_g); + + let h = self.heuristic(*neighbor, goal); + open_set.push(AStarNode { + coord: *neighbor, + g_cost: tentative_g, + f_cost: tentative_g + h, + }); + } + } + } + + debug!( + "[AStar] FAILED: NoPath after expanding {} nodes", + nodes_expanded + ); + PathResult::failed(PathFailure::NoPath, nodes_expanded) + } + + /// Find a path from start to goal (world coordinates) + pub fn find_path_world(&self, start: WorldPoint, goal: WorldPoint) -> PathResult { + let start_grid = self.storage.world_to_grid(start); + let goal_grid = self.storage.world_to_grid(goal); + self.find_path(start_grid, goal_grid) + } + + /// Heuristic function (octile distance for 8-connected grid) + fn heuristic(&self, from: GridCoord, to: GridCoord) -> f32 { + let dx = (from.x - to.x).abs() as f32; + let dy = (from.y - to.y).abs() as f32; + + if self.config.allow_diagonal { + // Octile distance + let min = dx.min(dy); + let max = dx.max(dy); + min * self.config.diagonal_cost + (max - min) + } else { + // Manhattan distance + dx + dy + } + } + + /// Reconstruct the path from came_from map + fn reconstruct_path( + &self, + came_from: HashMap, + goal: GridCoord, + cost: f32, + nodes_expanded: usize, + ) -> PathResult { + let mut path_grid = Vec::new(); + let mut current = goal; + + while let Some(&prev) = came_from.get(¤t) { + path_grid.push(current); + current = prev; + } + path_grid.push(current); // Add start + path_grid.reverse(); + + // Convert to world coordinates + let path_world: Vec = path_grid + .iter() + .map(|c| self.storage.grid_to_world(*c)) + .collect(); + + trace!( + "[AStar] SUCCESS: path length={} cells, cost={:.2}, nodes_expanded={}", + path_grid.len(), + cost, + nodes_expanded + ); + + PathResult { + path_grid, + path_world, + cost, + nodes_expanded, + success: true, + failure_reason: None, + } + } +} + +/// Quick path finding with default configuration +pub fn find_path(storage: &GridStorage, start: WorldPoint, goal: WorldPoint) -> PathResult { + let planner = AStarPlanner::with_defaults(storage); + planner.find_path_world(start, goal) +} + +/// Check if a path exists (faster than full path computation) +pub fn path_exists(storage: &GridStorage, start: WorldPoint, goal: WorldPoint) -> bool { + find_path(storage, start, goal).success +} + +/// A* planner with pre-computed traversability cache. +/// +/// This is faster for multiple path queries on the same grid state, +/// as it pre-computes traversability for all cells once. +/// +/// # Example +/// ```rust,ignore +/// let cached = CachedAStarPlanner::new(storage, config); +/// let path1 = cached.find_path(start1, goal1); +/// let path2 = cached.find_path(start2, goal2); // Uses cached traversability +/// ``` +pub struct CachedAStarPlanner<'a> { + storage: &'a GridStorage, + config: AStarConfig, + /// Pre-computed traversability for each cell (true = safe to traverse) + traversability_cache: Vec, +} + +impl<'a> CachedAStarPlanner<'a> { + /// Create a new cached A* planner. + /// + /// Pre-computes traversability for all cells based on the robot footprint. + /// This has O(width × height × footprint_area) upfront cost but makes + /// each pathfinding query O(1) for traversability checks. + pub fn new(storage: &'a GridStorage, config: AStarConfig) -> Self { + let checker = TraversabilityChecker::new(storage, config.footprint.clone()); + let width = storage.width(); + let height = storage.height(); + let exploration_mode = config.exploration_mode; + + // Pre-compute traversability for all cells + let mut cache = vec![false; width * height]; + for y in 0..height { + for x in 0..width { + let coord = GridCoord::new(x as i32, y as i32); + let world = storage.grid_to_world(coord); + cache[y * width + x] = checker.is_position_safe_mode(world, exploration_mode); + } + } + + Self { + storage, + config, + traversability_cache: cache, + } + } + + /// Create with default configuration + pub fn with_defaults(storage: &'a GridStorage) -> Self { + Self::new(storage, AStarConfig::default()) + } + + /// Check if a cell is traversable using the cache + #[inline] + fn is_traversable(&self, coord: GridCoord) -> bool { + if !self.storage.is_valid_coord(coord) { + return false; + } + let idx = coord.y as usize * self.storage.width() + coord.x as usize; + self.traversability_cache[idx] + } + + /// Find a path from start to goal (grid coordinates) + pub fn find_path(&self, start: GridCoord, goal: GridCoord) -> PathResult { + trace!( + "[CachedAStar] find_path: start=({},{}) goal=({},{})", + start.x, start.y, goal.x, goal.y + ); + + // Check bounds + if !self.storage.is_valid_coord(start) || !self.storage.is_valid_coord(goal) { + debug!("[CachedAStar] FAILED: OutOfBounds - start or goal outside grid"); + return PathResult::failed(PathFailure::OutOfBounds, 0); + } + + // Check start and goal traversability using cache + if !self.is_traversable(start) { + debug!("[CachedAStar] FAILED: StartBlocked"); + return PathResult::failed(PathFailure::StartBlocked, 0); + } + if !self.is_traversable(goal) { + debug!("[CachedAStar] FAILED: GoalBlocked"); + return PathResult::failed(PathFailure::GoalBlocked, 0); + } + + // A* search + let mut open_set = BinaryHeap::new(); + let mut closed_set = HashSet::new(); + let mut came_from: HashMap = HashMap::new(); + let mut g_scores: HashMap = HashMap::new(); + + let h_start = self.heuristic(start, goal); + open_set.push(AStarNode { + coord: start, + g_cost: 0.0, + f_cost: h_start, + }); + g_scores.insert(start, 0.0); + + let mut nodes_expanded = 0; + + while let Some(current) = open_set.pop() { + nodes_expanded += 1; + + if nodes_expanded > self.config.max_iterations { + debug!( + "[CachedAStar] FAILED: MaxIterationsExceeded ({} nodes)", + nodes_expanded + ); + return PathResult::failed(PathFailure::MaxIterationsExceeded, nodes_expanded); + } + + // Goal reached + if current.coord == goal { + return self.reconstruct_path(came_from, goal, current.g_cost, nodes_expanded); + } + + if closed_set.contains(¤t.coord) { + continue; + } + closed_set.insert(current.coord); + + // Explore neighbors + let neighbors = if self.config.allow_diagonal { + current.coord.neighbors_8().to_vec() + } else { + current.coord.neighbors_4().to_vec() + }; + + for (i, neighbor) in neighbors.iter().enumerate() { + if closed_set.contains(neighbor) { + continue; + } + + // Check traversability using cache (O(1) lookup) + if !self.is_traversable(*neighbor) { + continue; + } + + // Calculate movement cost + let is_diagonal = self.config.allow_diagonal && i >= 4; + let move_cost = if is_diagonal { + self.config.diagonal_cost + } else { + 1.0 + }; + + let tentative_g = g_scores[¤t.coord] + move_cost; + + let current_g = g_scores.get(neighbor).copied().unwrap_or(f32::INFINITY); + if tentative_g < current_g { + came_from.insert(*neighbor, current.coord); + g_scores.insert(*neighbor, tentative_g); + + let h = self.heuristic(*neighbor, goal); + open_set.push(AStarNode { + coord: *neighbor, + g_cost: tentative_g, + f_cost: tentative_g + h, + }); + } + } + } + + debug!( + "[CachedAStar] FAILED: NoPath after expanding {} nodes", + nodes_expanded + ); + PathResult::failed(PathFailure::NoPath, nodes_expanded) + } + + /// Find a path from start to goal (world coordinates) + pub fn find_path_world(&self, start: WorldPoint, goal: WorldPoint) -> PathResult { + let start_grid = self.storage.world_to_grid(start); + let goal_grid = self.storage.world_to_grid(goal); + self.find_path(start_grid, goal_grid) + } + + /// Heuristic function (octile distance for 8-connected grid) + fn heuristic(&self, from: GridCoord, to: GridCoord) -> f32 { + let dx = (from.x - to.x).abs() as f32; + let dy = (from.y - to.y).abs() as f32; + + if self.config.allow_diagonal { + let min = dx.min(dy); + let max = dx.max(dy); + min * self.config.diagonal_cost + (max - min) + } else { + dx + dy + } + } + + /// Reconstruct the path from came_from map + fn reconstruct_path( + &self, + came_from: HashMap, + goal: GridCoord, + cost: f32, + nodes_expanded: usize, + ) -> PathResult { + let mut path_grid = Vec::new(); + let mut current = goal; + + while let Some(&prev) = came_from.get(¤t) { + path_grid.push(current); + current = prev; + } + path_grid.push(current); // Add start + path_grid.reverse(); + + let path_world: Vec = path_grid + .iter() + .map(|c| self.storage.grid_to_world(*c)) + .collect(); + + trace!( + "[CachedAStar] SUCCESS: path length={} cells, cost={:.2}, nodes_expanded={}", + path_grid.len(), + cost, + nodes_expanded + ); + + PathResult { + path_grid, + path_world, + cost, + nodes_expanded, + success: true, + failure_reason: None, + } + } +} + +#[cfg(test)] +mod tests { + use super::*; + use crate::core::CellType; + + fn create_test_storage() -> GridStorage { + GridStorage::centered(50, 50, 0.1) // 5m x 5m at 10cm resolution + } + + fn fill_floor(storage: &mut GridStorage) { + for x in 0..50 { + for y in 0..50 { + storage.set_type(GridCoord::new(x, y), CellType::Floor); + } + } + } + + #[test] + fn test_simple_path() { + let mut storage = create_test_storage(); + fill_floor(&mut storage); + + // Small footprint for this test + let config = AStarConfig::with_footprint(RobotFootprint::new(0.05, 0.01)); + let planner = AStarPlanner::new(&storage, config); + + let start = GridCoord::new(10, 25); + let goal = GridCoord::new(40, 25); + + let result = planner.find_path(start, goal); + + assert!(result.success); + assert!(!result.path_grid.is_empty()); + assert_eq!(result.path_grid[0], start); + assert_eq!(*result.path_grid.last().unwrap(), goal); + } + + #[test] + fn test_path_around_obstacle() { + let mut storage = create_test_storage(); + fill_floor(&mut storage); + + // Add a wall across the middle + for y in 15..35 { + storage.set_type_with_priority(GridCoord::new(25, y), CellType::Wall); + } + + let config = AStarConfig::with_footprint(RobotFootprint::new(0.05, 0.01)); + let planner = AStarPlanner::new(&storage, config); + + let start = GridCoord::new(10, 25); + let goal = GridCoord::new(40, 25); + + let result = planner.find_path(start, goal); + + assert!(result.success); + // Path should go around the wall + assert!(result.path_grid.len() > 30); + } + + #[test] + fn test_no_path() { + let mut storage = create_test_storage(); + fill_floor(&mut storage); + + // Add a complete wall barrier + for y in 0..50 { + storage.set_type_with_priority(GridCoord::new(25, y), CellType::Wall); + } + + let config = AStarConfig::with_footprint(RobotFootprint::new(0.05, 0.01)); + let planner = AStarPlanner::new(&storage, config); + + let start = GridCoord::new(10, 25); + let goal = GridCoord::new(40, 25); + + let result = planner.find_path(start, goal); + + assert!(!result.success); + assert_eq!(result.failure_reason, Some(PathFailure::NoPath)); + } + + #[test] + fn test_start_blocked() { + let mut storage = create_test_storage(); + fill_floor(&mut storage); + + // Block start position + storage.set_type_with_priority(GridCoord::new(10, 25), CellType::Wall); + + let config = AStarConfig::with_footprint(RobotFootprint::new(0.05, 0.01)); + let planner = AStarPlanner::new(&storage, config); + + let start = GridCoord::new(10, 25); + let goal = GridCoord::new(40, 25); + + let result = planner.find_path(start, goal); + + assert!(!result.success); + assert_eq!(result.failure_reason, Some(PathFailure::StartBlocked)); + } + + #[test] + fn test_goal_blocked() { + let mut storage = create_test_storage(); + fill_floor(&mut storage); + + // Block goal position + storage.set_type_with_priority(GridCoord::new(40, 25), CellType::Wall); + + let config = AStarConfig::with_footprint(RobotFootprint::new(0.05, 0.01)); + let planner = AStarPlanner::new(&storage, config); + + let start = GridCoord::new(10, 25); + let goal = GridCoord::new(40, 25); + + let result = planner.find_path(start, goal); + + assert!(!result.success); + assert_eq!(result.failure_reason, Some(PathFailure::GoalBlocked)); + } + + #[test] + fn test_diagonal_path() { + let mut storage = create_test_storage(); + fill_floor(&mut storage); + + let config = AStarConfig { + footprint: RobotFootprint::new(0.05, 0.01), + allow_diagonal: true, + ..Default::default() + }; + let planner = AStarPlanner::new(&storage, config); + + let start = GridCoord::new(10, 10); + let goal = GridCoord::new(40, 40); + + let result = planner.find_path(start, goal); + + assert!(result.success); + // Diagonal path should be shorter than manhattan + // For a 30x30 diagonal, manhattan would be 60, octile should be ~42 + assert!(result.path_grid.len() < 50); + } + + #[test] + fn test_4_connected_path() { + let mut storage = create_test_storage(); + fill_floor(&mut storage); + + let config = AStarConfig { + footprint: RobotFootprint::new(0.05, 0.01), + allow_diagonal: false, + ..Default::default() + }; + let planner = AStarPlanner::new(&storage, config); + + let start = GridCoord::new(10, 10); + let goal = GridCoord::new(40, 40); + + let result = planner.find_path(start, goal); + + assert!(result.success); + // 4-connected path should follow manhattan distance + // Path length should be approximately dx + dy = 60 + assert!(result.path_grid.len() >= 60); + } + + #[test] + fn test_world_coordinates() { + let mut storage = create_test_storage(); + fill_floor(&mut storage); + + let config = AStarConfig::with_footprint(RobotFootprint::new(0.05, 0.01)); + let planner = AStarPlanner::new(&storage, config); + + let start = WorldPoint::new(-1.0, 0.0); + let goal = WorldPoint::new(1.0, 0.0); + + let result = planner.find_path_world(start, goal); + + assert!(result.success); + assert!(result.path_world.len() > 0); + } + + #[test] + fn test_path_length_meters() { + let mut storage = create_test_storage(); + fill_floor(&mut storage); + + let config = AStarConfig::with_footprint(RobotFootprint::new(0.05, 0.01)); + let planner = AStarPlanner::new(&storage, config); + + // Path of 2 meters (20 cells at 10cm resolution) + let start = WorldPoint::new(-1.0, 0.0); + let goal = WorldPoint::new(1.0, 0.0); + + let result = planner.find_path_world(start, goal); + + assert!(result.success); + // Path length should be approximately 2.0 meters + let length = result.length_meters(); + assert!(length > 1.5 && length < 2.5); + } +} diff --git a/vastu-map/src/pathfinding/mod.rs b/vastu-map/src/pathfinding/mod.rs new file mode 100644 index 0000000..5a7c326 --- /dev/null +++ b/vastu-map/src/pathfinding/mod.rs @@ -0,0 +1,38 @@ +//! Path planning algorithms. +//! +//! This module provides path planning on the occupancy grid: +//! +//! - **A* Search**: Find shortest collision-free paths +//! - **Path Smoothing**: Make paths more natural for robot motion +//! +//! ## A* Pathfinding +//! +//! ```rust,ignore +//! use vastu_map::pathfinding::{AStarPlanner, AStarConfig}; +//! +//! let config = AStarConfig::default(); +//! let planner = AStarPlanner::new(map.storage(), config); +//! +//! let result = planner.find_path_world(start, goal); +//! if result.success { +//! println!("Path found with {} waypoints", result.path_world.len()); +//! } +//! ``` +//! +//! ## Path Smoothing +//! +//! ```rust,ignore +//! use vastu_map::pathfinding::{PathSmoother, SmoothingConfig}; +//! +//! let smoother = PathSmoother::with_defaults(map.storage()); +//! let smooth_path = smoother.smooth(&result.path_world); +//! ``` + +pub mod astar; +pub mod smoothing; + +pub use astar::{ + AStarConfig, AStarPlanner, CachedAStarPlanner, PathFailure, PathResult, find_path, path_exists, +}; + +pub use smoothing::{PathSmoother, SmoothingConfig, path_length, resample_path, simplify_path}; diff --git a/vastu-map/src/pathfinding/smoothing.rs b/vastu-map/src/pathfinding/smoothing.rs new file mode 100644 index 0000000..998ebf2 --- /dev/null +++ b/vastu-map/src/pathfinding/smoothing.rs @@ -0,0 +1,400 @@ +//! Path smoothing algorithms. +//! +//! Smooths A* paths for more natural robot movement: +//! - Line-of-sight shortcutting +//! - Bezier curve smoothing +//! - Waypoint reduction + +use crate::core::{GridCoord, WorldPoint}; +use crate::grid::GridStorage; +use crate::query::{RobotFootprint, TraversabilityChecker}; + +/// Path smoothing configuration +#[derive(Clone, Debug)] +pub struct SmoothingConfig { + /// Robot footprint for collision checking + pub footprint: RobotFootprint, + /// Step size for line-of-sight checks (in meters) + pub los_step_size: f32, + /// Maximum iterations for iterative smoothing + pub max_iterations: usize, +} + +impl Default for SmoothingConfig { + fn default() -> Self { + Self { + footprint: RobotFootprint::default(), + los_step_size: 0.05, // 5cm steps + max_iterations: 100, + } + } +} + +/// Path smoother +pub struct PathSmoother<'a> { + storage: &'a GridStorage, + config: SmoothingConfig, +} + +impl<'a> PathSmoother<'a> { + /// Create a new path smoother + pub fn new(storage: &'a GridStorage, config: SmoothingConfig) -> Self { + Self { storage, config } + } + + /// Create with default configuration + pub fn with_defaults(storage: &'a GridStorage) -> Self { + Self::new(storage, SmoothingConfig::default()) + } + + /// Smooth a path using line-of-sight shortcuts + /// + /// Removes unnecessary waypoints by checking if direct paths exist + /// between non-adjacent waypoints. + pub fn smooth_los(&self, path: &[WorldPoint]) -> Vec { + if path.len() <= 2 { + return path.to_vec(); + } + + let checker = TraversabilityChecker::new(self.storage, self.config.footprint.clone()); + let mut smoothed = vec![path[0]]; + let mut i = 0; + + while i < path.len() - 1 { + // Find the furthest reachable point from current + let mut furthest = i + 1; + + for j in (i + 2)..path.len() { + let check_result = + checker.check_path(path[i], path[j], Some(self.config.los_step_size)); + + if check_result.is_clear { + furthest = j; + } + } + + smoothed.push(path[furthest]); + i = furthest; + } + + smoothed + } + + /// Smooth using iterative midpoint displacement + /// + /// Moves each waypoint towards the line connecting its neighbors + /// while maintaining collision-free path. + pub fn smooth_midpoint(&self, path: &[WorldPoint]) -> Vec { + if path.len() <= 2 { + return path.to_vec(); + } + + let checker = TraversabilityChecker::new(self.storage, self.config.footprint.clone()); + let mut smoothed = path.to_vec(); + let alpha = 0.5; // Smoothing factor + + for _ in 0..self.config.max_iterations { + let mut changed = false; + let mut new_path = smoothed.clone(); + + // Skip first and last points + for i in 1..smoothed.len() - 1 { + let prev = smoothed[i - 1]; + let curr = smoothed[i]; + let next = smoothed[i + 1]; + + // Target is midpoint of prev-next line + let target = WorldPoint::new((prev.x + next.x) / 2.0, (prev.y + next.y) / 2.0); + + // Move towards target + let new_point = WorldPoint::new( + curr.x + alpha * (target.x - curr.x), + curr.y + alpha * (target.y - curr.y), + ); + + // Check if new position is safe + if checker.is_position_safe(new_point) { + // Check paths to neighbors + let to_prev = + checker.check_path(new_point, prev, Some(self.config.los_step_size)); + let to_next = + checker.check_path(new_point, next, Some(self.config.los_step_size)); + + if to_prev.is_clear + && to_next.is_clear + && ((new_point.x - curr.x).abs() > 0.001 + || (new_point.y - curr.y).abs() > 0.001) + { + new_path[i] = new_point; + changed = true; + } + } + } + + smoothed = new_path; + if !changed { + break; + } + } + + smoothed + } + + /// Full smoothing: LOS shortcuts + midpoint smoothing + pub fn smooth(&self, path: &[WorldPoint]) -> Vec { + let los_smoothed = self.smooth_los(path); + self.smooth_midpoint(&los_smoothed) + } + + /// Convert grid path to world path with smoothing + pub fn smooth_grid_path(&self, path: &[GridCoord]) -> Vec { + let world_path: Vec = path + .iter() + .map(|c| self.storage.grid_to_world(*c)) + .collect(); + + self.smooth(&world_path) + } +} + +/// Simplify a path by removing collinear points +pub fn simplify_path(path: &[WorldPoint], tolerance: f32) -> Vec { + if path.len() <= 2 { + return path.to_vec(); + } + + let mut result = vec![path[0]]; + + for i in 1..path.len() - 1 { + let prev = result.last().unwrap(); + let curr = &path[i]; + let next = &path[i + 1]; + + // Check if current point is far enough from the line prev->next + let distance = point_to_line_distance(curr, prev, next); + if distance > tolerance { + result.push(*curr); + } + } + + result.push(*path.last().unwrap()); + result +} + +/// Calculate perpendicular distance from point to line +fn point_to_line_distance( + point: &WorldPoint, + line_start: &WorldPoint, + line_end: &WorldPoint, +) -> f32 { + let dx = line_end.x - line_start.x; + let dy = line_end.y - line_start.y; + let line_length_sq = dx * dx + dy * dy; + + if line_length_sq < 0.0001 { + // Line is a point + return point.distance(line_start); + } + + // Project point onto line + let t = ((point.x - line_start.x) * dx + (point.y - line_start.y) * dy) / line_length_sq; + let t_clamped = t.clamp(0.0, 1.0); + + let proj_x = line_start.x + t_clamped * dx; + let proj_y = line_start.y + t_clamped * dy; + + let distance_x = point.x - proj_x; + let distance_y = point.y - proj_y; + + (distance_x * distance_x + distance_y * distance_y).sqrt() +} + +/// Calculate total path length +pub fn path_length(path: &[WorldPoint]) -> f32 { + if path.len() < 2 { + return 0.0; + } + + path.windows(2).map(|w| w[0].distance(&w[1])).sum() +} + +/// Interpolate along path to get evenly spaced waypoints +pub fn resample_path(path: &[WorldPoint], spacing: f32) -> Vec { + if path.is_empty() { + return Vec::new(); + } + if path.len() == 1 { + return path.to_vec(); + } + + let total_length = path_length(path); + if total_length < spacing { + return vec![path[0], *path.last().unwrap()]; + } + + let mut result = vec![path[0]]; + let mut accumulated = 0.0; + let mut segment_idx = 0; + let mut segment_progress = 0.0; + + while accumulated < total_length - spacing * 0.5 { + accumulated += spacing; + + // Find the segment containing this distance + let mut distance_along = 0.0; + while segment_idx < path.len() - 1 { + let segment_length = path[segment_idx].distance(&path[segment_idx + 1]); + + if distance_along + segment_length >= accumulated { + // Point is in this segment + segment_progress = (accumulated - distance_along) / segment_length; + break; + } + + distance_along += segment_length; + segment_idx += 1; + } + + if segment_idx < path.len() - 1 { + let p1 = &path[segment_idx]; + let p2 = &path[segment_idx + 1]; + let point = WorldPoint::new( + p1.x + segment_progress * (p2.x - p1.x), + p1.y + segment_progress * (p2.y - p1.y), + ); + result.push(point); + } + } + + // Always include the end point + result.push(*path.last().unwrap()); + result +} + +#[cfg(test)] +mod tests { + use super::*; + use crate::core::CellType; + + fn create_test_storage() -> GridStorage { + GridStorage::centered(50, 50, 0.1) // 5m x 5m at 10cm resolution + } + + fn fill_floor(storage: &mut GridStorage) { + for x in 0..50 { + for y in 0..50 { + storage.set_type(GridCoord::new(x, y), CellType::Floor); + } + } + } + + #[test] + fn test_los_smoothing_straight_line() { + let mut storage = create_test_storage(); + fill_floor(&mut storage); + + let config = SmoothingConfig { + footprint: RobotFootprint::new(0.05, 0.01), + ..Default::default() + }; + let smoother = PathSmoother::new(&storage, config); + + // Jagged path that should become straight + let path = vec![ + WorldPoint::new(0.0, 0.0), + WorldPoint::new(0.1, 0.01), + WorldPoint::new(0.2, -0.01), + WorldPoint::new(0.3, 0.02), + WorldPoint::new(0.4, 0.0), + WorldPoint::new(0.5, 0.0), + ]; + + let smoothed = smoother.smooth_los(&path); + + // Should reduce to just start and end + assert!(smoothed.len() <= 3); + assert_eq!(smoothed[0], path[0]); + assert_eq!(*smoothed.last().unwrap(), *path.last().unwrap()); + } + + #[test] + fn test_los_preserves_corners() { + let mut storage = create_test_storage(); + fill_floor(&mut storage); + + // Add wall + for y in -10..10 { + storage.set_type_with_priority( + storage.world_to_grid(WorldPoint::new(0.5, y as f32 * 0.1)), + CellType::Wall, + ); + } + + let config = SmoothingConfig { + footprint: RobotFootprint::new(0.05, 0.01), + ..Default::default() + }; + let smoother = PathSmoother::new(&storage, config); + + // Path that goes around the wall + let path = vec![ + WorldPoint::new(0.0, 0.0), + WorldPoint::new(0.3, 0.0), + WorldPoint::new(0.3, 1.2), // Go up + WorldPoint::new(0.7, 1.2), // Go right (around wall) + WorldPoint::new(0.7, 0.0), // Go down + WorldPoint::new(1.0, 0.0), + ]; + + let smoothed = smoother.smooth_los(&path); + + // Should preserve some waypoints to go around wall + assert!(smoothed.len() >= 3); + } + + #[test] + fn test_simplify_path() { + // Path with a clear corner (not collinear) + let path = vec![ + WorldPoint::new(0.0, 0.0), + WorldPoint::new(0.1, 0.001), // Nearly collinear with start->end of segment 1 + WorldPoint::new(0.2, 0.0), // End of first segment + WorldPoint::new(0.3, 0.1), // True corner - offset from line + WorldPoint::new(0.3, 0.2), + ]; + + let simplified = simplify_path(&path, 0.01); + + // Should have fewer points than original + // start, corner at (0.2, 0), corner at (0.3, 0.1), end + assert!(simplified.len() <= path.len()); + + // First and last points should always be preserved + assert_eq!(simplified[0], path[0]); + assert_eq!(*simplified.last().unwrap(), *path.last().unwrap()); + } + + #[test] + fn test_path_length() { + let path = vec![ + WorldPoint::new(0.0, 0.0), + WorldPoint::new(1.0, 0.0), + WorldPoint::new(1.0, 1.0), + ]; + + let length = path_length(&path); + assert!((length - 2.0).abs() < 0.01); + } + + #[test] + fn test_resample_path() { + let path = vec![WorldPoint::new(0.0, 0.0), WorldPoint::new(1.0, 0.0)]; + + let resampled = resample_path(&path, 0.25); + + // Should have points at 0, 0.25, 0.5, 0.75, 1.0 + assert!(resampled.len() >= 4); + assert_eq!(resampled[0], path[0]); + assert_eq!(*resampled.last().unwrap(), *path.last().unwrap()); + } +} diff --git a/vastu-map/src/query/README.md b/vastu-map/src/query/README.md deleted file mode 100644 index fa2efd4..0000000 --- a/vastu-map/src/query/README.md +++ /dev/null @@ -1,153 +0,0 @@ -# Query Module - -Map query operations for navigation and exploration. - -## Components - -| File | Description | -|------|-------------| -| `raycast.rs` | Ray casting to find obstacles | -| `occupancy.rs` | Point occupancy queries (free/occupied/unknown) | -| `frontier.rs` | Frontier detection for exploration | - -## Raycast - -Find distance to obstacles in a given direction: - -```rust -// Basic raycast -let distance = raycast(origin, direction, max_range, &lines); - -// With spatial index (faster) -let distance = raycast_indexed(origin, direction, max_range, &index); - -// Detailed result with hit information -let result = raycast_detailed(origin, direction, max_range, &lines); -if let Some(hit) = result.hit { - println!("Hit at {:?}, line index: {}", hit.point, hit.line_idx); -} - -// Batch raycast (multiple rays at once) -let distances = raycast_batch(&origins, &directions, max_range, &lines); - -// 360-degree sweep -let distances = raycast_360(origin, num_rays, max_range, &lines); - -// Sweep over angle range -let distances = raycast_sweep(origin, start_angle, end_angle, num_rays, max_range, &lines); -``` - -## Occupancy - -Query whether points are free, occupied, or unknown: - -```rust -let config = OccupancyConfig { - robot_radius: 0.15, // Robot footprint radius - clearance: 0.05, // Extra safety margin - unknown_threshold: 0.5, // Distance threshold for "unknown" -}; - -// Single point query -let occupancy = query_occupancy(point, &lines, Some(&index), &config); - -match occupancy { - Occupancy::Free => println!("Navigable space"), - Occupancy::Occupied => println!("Obstacle"), - Occupancy::Unknown => println!("Not yet mapped"), -} - -// Batch query (SIMD-friendly) -let results = query_occupancy_batch(&points, &lines, Some(&index), &config); - -// Path clearance check -let clear = is_path_clear(from, to, &lines, &config); - -// Region clearance check -let clear = is_region_clear(center, radius, &lines, &config); -``` - -## Frontier Detection - -Find unexplored boundaries for exploration: - -```rust -let config = FrontierConfig { - min_length: 0.3, // Min frontier length - cluster_distance: 0.5, // Distance for clustering - min_cluster_size: 3, // Min points per cluster -}; - -// Detect all frontiers -let frontiers = detect_frontiers(&lines, &config); - -// Detect frontiers relative to robot position -let frontiers = detect_frontiers_from_robot(&lines, robot_pose, &config); - -// Cluster nearby frontiers -let clusters = cluster_frontiers(&frontiers, cluster_distance); - -// Get cluster centroid -let centroid = cluster_centroid(&cluster); - -// Rank frontiers by exploration value -let ranked = rank_frontiers(&frontiers, robot_pose, &config); - -// Get best frontier for exploration -if let Some(best) = get_best_frontier(&frontiers, robot_pose, &config) { - println!("Explore: {:?}", best.point); -} -``` - -## Frontier Types - -Frontiers are line endpoints that don't connect to other lines: - -``` - ═══════════════╗ - ║ - Frontier → • ║ ← Connected - ║ - ═══════════════╝ - - Unconnected endpoints indicate unexplored areas -``` - -## Use Cases - -### Navigation - -```rust -// Check if goal is reachable -if is_path_clear(robot_pos, goal, &lines, &config) { - // Plan direct path -} - -// Virtual lidar for local planning -let distances = raycast_360(robot_pos, 360, 3.0, &lines); -``` - -### Exploration - -```rust -// Find next exploration target -let frontiers = detect_frontiers(&lines, &config); -let ranked = rank_frontiers(&frontiers, robot_pose, &config); - -if let Some(target) = ranked.first() { - println!("Next target: {:?}", target.point); -} -``` - -### Collision Checking - -```rust -// Check robot footprint -let clear = is_region_clear(robot_pos, robot_radius, &lines, &config); - -// Check planned path -let waypoints = [p1, p2, p3, p4]; -let all_clear = waypoints.windows(2).all(|pair| { - is_path_clear(pair[0], pair[1], &lines, &config) -}); -``` diff --git a/vastu-map/src/query/cbvg/clearance.rs b/vastu-map/src/query/cbvg/clearance.rs deleted file mode 100644 index f7cf1c3..0000000 --- a/vastu-map/src/query/cbvg/clearance.rs +++ /dev/null @@ -1,302 +0,0 @@ -//! Clearance computation utilities for CBVG. -//! -//! Provides functions to compute distances from points to walls -//! and check path clearance. - -use crate::core::Point2D; -use crate::features::Line2D; - -/// Compute minimum distance from a point to any wall segment. -/// -/// # Arguments -/// * `point` - The query point -/// * `lines` - Wall segments in the map -/// -/// # Returns -/// Minimum distance to any wall, or `f32::INFINITY` if no walls exist. -#[inline] -pub fn min_distance_to_walls(point: Point2D, lines: &[Line2D]) -> f32 { - lines - .iter() - .map(|line| line.distance_to_point_segment(point)) - .min_by(|a, b| a.partial_cmp(b).unwrap_or(std::cmp::Ordering::Equal)) - .unwrap_or(f32::INFINITY) -} - -/// Find the nearest wall to a point. -/// -/// # Arguments -/// * `point` - The query point -/// * `lines` - Wall segments in the map -/// -/// # Returns -/// Tuple of (distance, wall_index), or None if no walls exist. -pub fn nearest_wall(point: Point2D, lines: &[Line2D]) -> Option<(f32, usize)> { - lines - .iter() - .enumerate() - .map(|(idx, line)| (line.distance_to_point_segment(point), idx)) - .min_by(|(a, _), (b, _)| a.partial_cmp(b).unwrap_or(std::cmp::Ordering::Equal)) -} - -/// Check if a straight path between two points has sufficient clearance. -/// -/// Samples points along the path and verifies each has minimum clearance. -/// -/// # Arguments -/// * `from` - Start position -/// * `to` - End position -/// * `lines` - Wall segments -/// * `min_clearance` - Required minimum distance from walls -/// * `sample_step` - Distance between sample points -/// -/// # Returns -/// True if the entire path has sufficient clearance. -pub fn is_path_clear_with_clearance( - from: Point2D, - to: Point2D, - lines: &[Line2D], - min_clearance: f32, - sample_step: f32, -) -> bool { - let segment = Line2D::new(from, to); - let length = segment.length(); - - if length < 0.001 { - // Zero-length path - just check the point - return min_distance_to_walls(from, lines) >= min_clearance; - } - - // Sample at regular intervals - let step = sample_step.min(min_clearance * 0.5); - let num_samples = (length / step).ceil() as usize; - let num_samples = num_samples.max(2); // At least check start and end - - for k in 0..=num_samples { - let t = k as f32 / num_samples as f32; - let sample = segment.point_at(t); - - if min_distance_to_walls(sample, lines) < min_clearance { - return false; - } - } - - true -} - -/// Check if an edge between two nodes is safe for robot traversal. -/// -/// Uses adaptive clearance: full clearance in open areas, -/// reduced clearance in narrow passages. -/// -/// # Arguments -/// * `from` - Start position -/// * `to` - End position -/// * `lines` - Wall segments -/// * `min_clearance` - Full clearance for open areas -/// * `min_clearance_narrow` - Reduced clearance for narrow passages -/// * `sample_step` - Distance between sample points -/// -/// # Returns -/// True if the edge is safe for traversal. -pub fn is_edge_safe( - from: Point2D, - to: Point2D, - lines: &[Line2D], - min_clearance: f32, - min_clearance_narrow: f32, - sample_step: f32, -) -> bool { - let segment = Line2D::new(from, to); - let length = segment.length(); - - if length < 0.001 { - return min_distance_to_walls(from, lines) >= min_clearance_narrow; - } - - let num_samples = (length / sample_step).ceil() as usize; - let num_samples = num_samples.max(2); - - for k in 0..=num_samples { - let t = k as f32 / num_samples as f32; - let sample = segment.point_at(t); - - let clearance = min_distance_to_walls(sample, lines); - - // Adaptive clearance: use narrow clearance if we're in a tight spot - let required = if clearance < min_clearance { - min_clearance_narrow - } else { - min_clearance - }; - - if clearance < required { - return false; - } - } - - true -} - -/// Sample points along a line segment at regular intervals. -/// -/// # Arguments -/// * `line` - The line segment to sample -/// * `step` - Distance between samples -/// -/// # Returns -/// Vector of sampled points including start and end. -pub fn sample_line(line: &Line2D, step: f32) -> Vec { - let length = line.length(); - - if length < step { - return vec![line.start, line.end]; - } - - let num_samples = (length / step).ceil() as usize; - let mut points = Vec::with_capacity(num_samples + 1); - - for i in 0..=num_samples { - let t = i as f32 / num_samples as f32; - points.push(line.point_at(t)); - } - - points -} - -/// Find the closest point on any wall to the given point. -/// -/// # Arguments -/// * `point` - The query point -/// * `lines` - Wall segments -/// -/// # Returns -/// The closest point on any wall, or the input point if no walls exist. -pub fn closest_point_on_walls(point: Point2D, lines: &[Line2D]) -> Point2D { - lines - .iter() - .map(|line| { - let t = line.project_point(point).clamp(0.0, 1.0); - let projected = line.point_at(t); - let dist = point.distance(projected); - (projected, dist) - }) - .min_by(|(_, a), (_, b)| a.partial_cmp(b).unwrap_or(std::cmp::Ordering::Equal)) - .map(|(p, _)| p) - .unwrap_or(point) -} - -#[cfg(test)] -mod tests { - use super::*; - - fn make_test_walls() -> Vec { - vec![ - // Horizontal wall at y=2 - Line2D::new(Point2D::new(-5.0, 2.0), Point2D::new(5.0, 2.0)), - // Vertical wall at x=3 - Line2D::new(Point2D::new(3.0, -2.0), Point2D::new(3.0, 2.0)), - ] - } - - #[test] - fn test_min_distance_to_walls() { - let walls = make_test_walls(); - - // Point at origin - let dist = min_distance_to_walls(Point2D::new(0.0, 0.0), &walls); - assert!((dist - 2.0).abs() < 0.01); // 2m from horizontal wall - - // Point near vertical wall - let dist = min_distance_to_walls(Point2D::new(2.5, 0.0), &walls); - assert!((dist - 0.5).abs() < 0.01); // 0.5m from vertical wall - - // Empty walls - let dist = min_distance_to_walls(Point2D::new(0.0, 0.0), &[]); - assert!(dist.is_infinite()); - } - - #[test] - fn test_nearest_wall() { - let walls = make_test_walls(); - - let result = nearest_wall(Point2D::new(0.0, 0.0), &walls); - assert!(result.is_some()); - let (dist, idx) = result.unwrap(); - assert!((dist - 2.0).abs() < 0.01); - assert_eq!(idx, 0); // Horizontal wall is closer - - let result = nearest_wall(Point2D::new(0.0, 0.0), &[]); - assert!(result.is_none()); - } - - #[test] - fn test_is_path_clear_with_clearance() { - let walls = make_test_walls(); - - // Path in open area (far from walls) - assert!(is_path_clear_with_clearance( - Point2D::new(-2.0, 0.0), - Point2D::new(0.0, 0.0), - &walls, - 0.3, - 0.1 - )); - - // Path too close to wall - assert!(!is_path_clear_with_clearance( - Point2D::new(-2.0, 1.9), - Point2D::new(2.0, 1.9), - &walls, - 0.3, - 0.1 - )); - } - - #[test] - fn test_is_edge_safe() { - let walls = make_test_walls(); - - // Safe edge in open area - assert!(is_edge_safe( - Point2D::new(-2.0, 0.0), - Point2D::new(0.0, 0.0), - &walls, - 0.3, - 0.18, - 0.05 - )); - - // Edge too close to wall - assert!(!is_edge_safe( - Point2D::new(-2.0, 1.95), - Point2D::new(2.0, 1.95), - &walls, - 0.3, - 0.18, - 0.05 - )); - } - - #[test] - fn test_sample_line() { - let line = Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(1.0, 0.0)); - - let samples = sample_line(&line, 0.25); - assert_eq!(samples.len(), 5); // 0, 0.25, 0.5, 0.75, 1.0 - - // Short line - let short = Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(0.1, 0.0)); - let samples = sample_line(&short, 0.5); - assert_eq!(samples.len(), 2); // Just start and end - } - - #[test] - fn test_closest_point_on_walls() { - let walls = make_test_walls(); - - let closest = closest_point_on_walls(Point2D::new(0.0, 3.0), &walls); - assert!((closest.x - 0.0).abs() < 0.01); - assert!((closest.y - 2.0).abs() < 0.01); // Closest to horizontal wall - } -} diff --git a/vastu-map/src/query/cbvg/config.rs b/vastu-map/src/query/cbvg/config.rs deleted file mode 100644 index 0b52d25..0000000 --- a/vastu-map/src/query/cbvg/config.rs +++ /dev/null @@ -1,227 +0,0 @@ -//! Configuration for Clearance-Based Visibility Graph. - -use serde::{Deserialize, Serialize}; - -/// Configuration for clearance-based visibility graph path planning. -/// -/// The CBVG places nodes along the medial axis of free space, ensuring -/// all waypoints are at a safe distance from obstacles. -#[derive(Clone, Debug, Serialize, Deserialize)] -pub struct CBVGConfig { - /// Minimum clearance from walls in open areas (meters). - /// Nodes will be placed at least this distance from any wall. - /// Default: 0.3 - pub min_clearance: f32, - - /// Minimum clearance in narrow passages (meters). - /// Used when corridor is too narrow for full clearance. - /// Default: 0.18 (robot_radius + 0.02) - pub min_clearance_narrow: f32, - - /// Sample resolution along Voronoi edges (meters). - /// Controls density of nodes along the medial axis. - /// Default: 0.1 - pub voronoi_sample_step: f32, - - /// Maximum edge length between nodes (meters). - /// Longer edges are split with intermediate nodes. - /// Default: 0.8 - pub max_edge_length: f32, - - /// Interval for placing coverage nodes in open areas (meters). - /// Ensures nodes exist even in areas without medial axis. - /// Default: 0.5 - pub coverage_node_interval: f32, - - /// Distance threshold for merging nearby nodes (meters). - /// Nodes closer than this are merged to reduce graph size. - /// Default: 0.15 - pub node_merge_distance: f32, - - /// Maximum nodes in the graph. - /// Limits computation for very complex maps. - /// Default: 500 - pub max_nodes: usize, - - /// Clearance sample step for edge validation (meters). - /// Points are sampled along edges to verify clearance. - /// Default: 0.05 - pub edge_clearance_step: f32, - - /// Wall sampling step for Voronoi approximation (meters). - /// Points are sampled along walls for Voronoi computation. - /// Default: 0.1 - pub wall_sample_step: f32, - - /// Minimum clique size for merging during optimization. - /// Cliques (mutually visible node groups) smaller than this are kept separate. - /// Set to 3 to merge triangles and larger clusters. - /// Default: 3 - pub min_clique_size_for_merge: usize, - - /// Number of lidar scans between clique optimization runs. - /// Higher values reduce CPU but may leave redundant nodes longer. - /// Set to 1 to optimize after every scan. - /// Default: 1 - pub optimize_interval: usize, - - /// Maximum lidar range (meters). - /// When a scan point is at or near this distance, it indicates no wall was hit - /// (open space). Nodes are still added along the ray up to this distance. - /// Default: 8.0 - pub max_lidar_range: f32, -} - -impl Default for CBVGConfig { - fn default() -> Self { - Self { - min_clearance: 0.25, // Reduced since robot radius is accounted separately - min_clearance_narrow: 0.18, - voronoi_sample_step: 0.1, - max_edge_length: 0.8, // Reduced for tighter connectivity - coverage_node_interval: 0.4, // Reasonable node spacing - node_merge_distance: 0.4, // Minimum distance between nodes - max_nodes: 5000, // Support large multi-room layouts - edge_clearance_step: 0.05, - wall_sample_step: 0.1, - min_clique_size_for_merge: 3, - optimize_interval: 1, - max_lidar_range: 8.0, - } - } -} - -impl CBVGConfig { - /// Create a new configuration with default values. - pub fn new() -> Self { - Self::default() - } - - /// Builder-style setter for minimum clearance. - pub fn with_min_clearance(mut self, clearance: f32) -> Self { - self.min_clearance = clearance; - self - } - - /// Builder-style setter for narrow passage clearance. - pub fn with_min_clearance_narrow(mut self, clearance: f32) -> Self { - self.min_clearance_narrow = clearance; - self - } - - /// Builder-style setter for Voronoi sample step. - pub fn with_voronoi_sample_step(mut self, step: f32) -> Self { - self.voronoi_sample_step = step; - self - } - - /// Builder-style setter for maximum edge length. - pub fn with_max_edge_length(mut self, length: f32) -> Self { - self.max_edge_length = length; - self - } - - /// Builder-style setter for coverage node interval. - pub fn with_coverage_node_interval(mut self, interval: f32) -> Self { - self.coverage_node_interval = interval; - self - } - - /// Builder-style setter for node merge distance. - pub fn with_node_merge_distance(mut self, distance: f32) -> Self { - self.node_merge_distance = distance; - self - } - - /// Builder-style setter for maximum nodes. - pub fn with_max_nodes(mut self, max: usize) -> Self { - self.max_nodes = max; - self - } - - /// Builder-style setter for edge clearance step. - pub fn with_edge_clearance_step(mut self, step: f32) -> Self { - self.edge_clearance_step = step; - self - } - - /// Builder-style setter for wall sample step. - pub fn with_wall_sample_step(mut self, step: f32) -> Self { - self.wall_sample_step = step; - self - } - - /// Builder-style setter for minimum clique size for merging. - pub fn with_min_clique_size_for_merge(mut self, size: usize) -> Self { - self.min_clique_size_for_merge = size; - self - } - - /// Builder-style setter for optimization interval. - pub fn with_optimize_interval(mut self, interval: usize) -> Self { - self.optimize_interval = interval; - self - } - - /// Builder-style setter for maximum lidar range. - pub fn with_max_lidar_range(mut self, range: f32) -> Self { - self.max_lidar_range = range; - self - } - - /// Get the effective clearance for a given measured clearance. - /// - /// Returns `min_clearance` for open areas, or `min_clearance_narrow` - /// for narrow passages where full clearance isn't achievable. - #[inline] - pub fn effective_clearance(&self, measured_clearance: f32) -> f32 { - if measured_clearance >= self.min_clearance { - self.min_clearance - } else { - self.min_clearance_narrow - } - } -} - -#[cfg(test)] -mod tests { - use super::*; - - #[test] - fn test_default_config() { - let config = CBVGConfig::default(); - assert_eq!(config.min_clearance, 0.25); - assert_eq!(config.min_clearance_narrow, 0.18); - assert_eq!(config.voronoi_sample_step, 0.1); - assert_eq!(config.max_edge_length, 0.8); - assert_eq!(config.coverage_node_interval, 0.4); - assert_eq!(config.node_merge_distance, 0.4); - assert_eq!(config.max_nodes, 5000); - assert_eq!(config.edge_clearance_step, 0.05); - assert_eq!(config.wall_sample_step, 0.1); - assert_eq!(config.min_clique_size_for_merge, 3); - assert_eq!(config.optimize_interval, 1); - assert_eq!(config.max_lidar_range, 8.0); - } - - #[test] - fn test_builder_pattern() { - let config = CBVGConfig::new() - .with_min_clearance(0.25) - .with_max_nodes(1000); - - assert_eq!(config.min_clearance, 0.25); - assert_eq!(config.max_nodes, 1000); - } - - #[test] - fn test_effective_clearance() { - let config = CBVGConfig::default(); - - // Open area - use full clearance - assert_eq!(config.effective_clearance(0.5), 0.25); - - // Narrow passage - use reduced clearance - assert_eq!(config.effective_clearance(0.2), 0.18); - } -} diff --git a/vastu-map/src/query/cbvg/dijkstra.rs b/vastu-map/src/query/cbvg/dijkstra.rs deleted file mode 100644 index d463b0e..0000000 --- a/vastu-map/src/query/cbvg/dijkstra.rs +++ /dev/null @@ -1,360 +0,0 @@ -//! Dijkstra's shortest path algorithm for CBVG. - -use std::cmp::Ordering; -use std::collections::BinaryHeap; - -/// State for Dijkstra's algorithm priority queue. -#[derive(Clone, Copy, Debug)] -pub struct DijkstraState { - /// Current path cost (distance). - pub cost: f32, - /// Current node index. - pub node: usize, -} - -impl PartialEq for DijkstraState { - fn eq(&self, other: &Self) -> bool { - self.cost == other.cost && self.node == other.node - } -} - -impl Eq for DijkstraState {} - -impl Ord for DijkstraState { - fn cmp(&self, other: &Self) -> Ordering { - // Reverse ordering for min-heap (BinaryHeap is max-heap by default) - other - .cost - .partial_cmp(&self.cost) - .unwrap_or(Ordering::Equal) - } -} - -impl PartialOrd for DijkstraState { - fn partial_cmp(&self, other: &Self) -> Option { - Some(self.cmp(other)) - } -} - -/// Result of a Dijkstra search. -#[derive(Clone, Debug)] -pub struct DijkstraResult { - /// Sequence of node indices from start to goal. - pub path: Vec, - /// Total path distance. - pub distance: f32, -} - -/// Find shortest path using Dijkstra's algorithm. -/// -/// # Arguments -/// * `edges` - Adjacency list: edges[i] = [(neighbor_idx, distance), ...] -/// * `start` - Start node index -/// * `goal` - Goal node index -/// -/// # Returns -/// DijkstraResult with path and distance, or None if no path exists. -pub fn dijkstra(edges: &[Vec<(usize, f32)>], start: usize, goal: usize) -> Option { - let n = edges.len(); - - if n == 0 || start >= n || goal >= n { - return None; - } - - // Handle same start and goal - if start == goal { - return Some(DijkstraResult { - path: vec![start], - distance: 0.0, - }); - } - - // Distance to each node - let mut dist: Vec = vec![f32::INFINITY; n]; - let mut prev: Vec> = vec![None; n]; - dist[start] = 0.0; - - // Priority queue - let mut heap = BinaryHeap::new(); - heap.push(DijkstraState { - cost: 0.0, - node: start, - }); - - while let Some(DijkstraState { cost, node }) = heap.pop() { - // Skip if we've found a better path - if cost > dist[node] { - continue; - } - - // Found goal - if node == goal { - break; - } - - // Explore neighbors - for &(neighbor, edge_dist) in &edges[node] { - let new_dist = dist[node] + edge_dist; - if new_dist < dist[neighbor] { - dist[neighbor] = new_dist; - prev[neighbor] = Some(node); - heap.push(DijkstraState { - cost: new_dist, - node: neighbor, - }); - } - } - } - - // Check if goal is reachable - if prev[goal].is_none() && start != goal { - return None; - } - - // Reconstruct path - let mut path = Vec::new(); - let mut current = goal; - - while current != start { - path.push(current); - match prev[current] { - Some(p) => current = p, - None => return None, // No path - } - } - path.push(start); - path.reverse(); - - Some(DijkstraResult { - path, - distance: dist[goal], - }) -} - -/// Find shortest path with early termination if goal is found within max_distance. -/// -/// Same as `dijkstra` but stops early if the goal is found. -pub fn dijkstra_with_limit( - edges: &[Vec<(usize, f32)>], - start: usize, - goal: usize, - max_distance: f32, -) -> Option { - let n = edges.len(); - - if n == 0 || start >= n || goal >= n { - return None; - } - - if start == goal { - return Some(DijkstraResult { - path: vec![start], - distance: 0.0, - }); - } - - let mut dist: Vec = vec![f32::INFINITY; n]; - let mut prev: Vec> = vec![None; n]; - dist[start] = 0.0; - - let mut heap = BinaryHeap::new(); - heap.push(DijkstraState { - cost: 0.0, - node: start, - }); - - while let Some(DijkstraState { cost, node }) = heap.pop() { - // Early termination if exceeded max distance - if cost > max_distance { - break; - } - - if cost > dist[node] { - continue; - } - - if node == goal { - // Reconstruct path - let mut path = Vec::new(); - let mut current = goal; - while current != start { - path.push(current); - current = prev[current]?; - } - path.push(start); - path.reverse(); - - return Some(DijkstraResult { - path, - distance: cost, - }); - } - - for &(neighbor, edge_dist) in &edges[node] { - let new_dist = dist[node] + edge_dist; - if new_dist < dist[neighbor] && new_dist <= max_distance { - dist[neighbor] = new_dist; - prev[neighbor] = Some(node); - heap.push(DijkstraState { - cost: new_dist, - node: neighbor, - }); - } - } - } - - None -} - -/// Compute distances from a source node to all reachable nodes. -/// -/// # Arguments -/// * `edges` - Adjacency list -/// * `source` - Source node index -/// -/// # Returns -/// Vector of distances (f32::INFINITY for unreachable nodes). -pub fn dijkstra_distances(edges: &[Vec<(usize, f32)>], source: usize) -> Vec { - let n = edges.len(); - - if n == 0 || source >= n { - return vec![f32::INFINITY; n]; - } - - let mut dist: Vec = vec![f32::INFINITY; n]; - dist[source] = 0.0; - - let mut heap = BinaryHeap::new(); - heap.push(DijkstraState { - cost: 0.0, - node: source, - }); - - while let Some(DijkstraState { cost, node }) = heap.pop() { - if cost > dist[node] { - continue; - } - - for &(neighbor, edge_dist) in &edges[node] { - let new_dist = dist[node] + edge_dist; - if new_dist < dist[neighbor] { - dist[neighbor] = new_dist; - heap.push(DijkstraState { - cost: new_dist, - node: neighbor, - }); - } - } - } - - dist -} - -#[cfg(test)] -mod tests { - use super::*; - - fn make_simple_graph() -> Vec> { - // Graph: - // 0 --1.0-- 1 --1.0-- 2 - // | | - // 2.0 1.5 - // | | - // 3 --1.0-- 4 - vec![ - vec![(1, 1.0), (3, 2.0)], // Node 0 - vec![(0, 1.0), (2, 1.0), (4, 1.5)], // Node 1 - vec![(1, 1.0)], // Node 2 - vec![(0, 2.0), (4, 1.0)], // Node 3 - vec![(1, 1.5), (3, 1.0)], // Node 4 - ] - } - - #[test] - fn test_dijkstra_simple() { - let edges = make_simple_graph(); - - // Direct path - let result = dijkstra(&edges, 0, 1).unwrap(); - assert_eq!(result.path, vec![0, 1]); - assert!((result.distance - 1.0).abs() < 0.01); - - // Path through intermediate node - let result = dijkstra(&edges, 0, 2).unwrap(); - assert_eq!(result.path, vec![0, 1, 2]); - assert!((result.distance - 2.0).abs() < 0.01); - } - - #[test] - fn test_dijkstra_same_node() { - let edges = make_simple_graph(); - let result = dijkstra(&edges, 0, 0).unwrap(); - assert_eq!(result.path, vec![0]); - assert_eq!(result.distance, 0.0); - } - - #[test] - fn test_dijkstra_no_path() { - // Disconnected graph - let edges: Vec> = vec![ - vec![(1, 1.0)], // Node 0 -> 1 - vec![(0, 1.0)], // Node 1 -> 0 - vec![], // Node 2 (isolated) - ]; - - let result = dijkstra(&edges, 0, 2); - assert!(result.is_none()); - } - - #[test] - fn test_dijkstra_with_limit() { - let edges = make_simple_graph(); - - // Within limit - let result = dijkstra_with_limit(&edges, 0, 1, 5.0).unwrap(); - assert_eq!(result.path, vec![0, 1]); - - // Exceeds limit - let result = dijkstra_with_limit(&edges, 0, 2, 1.5); - assert!(result.is_none()); - } - - #[test] - fn test_dijkstra_distances() { - let edges = make_simple_graph(); - let dist = dijkstra_distances(&edges, 0); - - assert!((dist[0] - 0.0).abs() < 0.01); - assert!((dist[1] - 1.0).abs() < 0.01); - assert!((dist[2] - 2.0).abs() < 0.01); - assert!((dist[3] - 2.0).abs() < 0.01); - assert!((dist[4] - 2.5).abs() < 0.01); - } - - #[test] - fn test_dijkstra_shortest_path() { - let edges = make_simple_graph(); - - // 0 -> 4: Can go 0->1->4 (2.5) or 0->3->4 (3.0) - let result = dijkstra(&edges, 0, 4).unwrap(); - assert_eq!(result.path, vec![0, 1, 4]); - assert!((result.distance - 2.5).abs() < 0.01); - } - - #[test] - fn test_dijkstra_empty_graph() { - let edges: Vec> = vec![]; - let result = dijkstra(&edges, 0, 1); - assert!(result.is_none()); - } - - #[test] - fn test_dijkstra_state_ordering() { - // Test that lower cost has higher priority - let state1 = DijkstraState { cost: 1.0, node: 0 }; - let state2 = DijkstraState { cost: 2.0, node: 1 }; - - // state1 should come before state2 (lower cost = higher priority) - assert!(state1 > state2); - } -} diff --git a/vastu-map/src/query/cbvg/graph.rs b/vastu-map/src/query/cbvg/graph.rs deleted file mode 100644 index e0118cd..0000000 --- a/vastu-map/src/query/cbvg/graph.rs +++ /dev/null @@ -1,1923 +0,0 @@ -//! Clearance-Based Visibility Graph implementation. -//! -//! The main graph structure that maintains nodes along the medial axis -//! and edges connecting them for path planning. - -use std::collections::HashSet; -use std::f32::consts::PI; - -use crate::core::{Bounds, Point2D, Pose2D}; -use crate::features::Line2D; -use crate::{Frontier, Path}; - -use super::clearance::{is_edge_safe, is_path_clear_with_clearance, min_distance_to_walls}; -use super::config::CBVGConfig; -use super::dijkstra::{DijkstraResult, dijkstra}; -use super::node::CBVGNode; -use super::visibility::VisibilityRegion; -use super::voronoi::generate_grid_nodes; - -/// Clearance-Based Visibility Graph for safe robot navigation. -/// -/// The CBVG places nodes along the medial axis of free space, ensuring -/// all waypoints are at a safe distance from obstacles. This provides -/// safer paths compared to traditional visibility graphs that place -/// nodes at obstacle corners. -/// -/// # Example -/// -/// ```rust,ignore -/// use vastu_map::query::cbvg::{ClearanceVisibilityGraph, CBVGConfig}; -/// use vastu_map::core::Point2D; -/// -/// let config = CBVGConfig::default(); -/// let mut graph = ClearanceVisibilityGraph::new(config); -/// -/// // Build graph from walls -/// graph.build(&walls, Some(&bounds)); -/// -/// // Find path -/// if let Some(path) = graph.find_path(start, goal, &walls) { -/// println!("Path found: {} waypoints", path.points.len()); -/// } -/// ``` -#[derive(Clone, Debug)] -pub struct ClearanceVisibilityGraph { - /// All nodes in the graph. - nodes: Vec, - - /// Adjacency list: edges[i] = [(neighbor_idx, distance), ...] - edges: Vec>, - - /// Configuration. - config: CBVGConfig, - - /// Counter for periodic clique optimization (currently disabled). - #[allow(dead_code)] - scans_since_optimize: usize, - - /// Visibility region tracking (currently unused - using ray-based approach). - #[allow(dead_code)] - visibility: VisibilityRegion, -} - -impl ClearanceVisibilityGraph { - /// Create a new empty graph with the given configuration. - pub fn new(config: CBVGConfig) -> Self { - // Use coverage_node_interval as visibility grid resolution so each cell - // corresponds to one potential node position - let visibility = VisibilityRegion::new(config.coverage_node_interval); - Self { - nodes: Vec::new(), - edges: Vec::new(), - config, - scans_since_optimize: 0, - visibility, - } - } - - /// Build the graph from wall segments. - /// - /// This clears any existing nodes and rebuilds the entire graph - /// by computing the medial axis of free space. Automatically optimizes - /// the graph by merging visibility cliques into centroids. - /// - /// # Arguments - /// * `lines` - Wall segments - /// * `bounds` - Optional map bounds (computed from lines if not provided) - pub fn build(&mut self, lines: &[Line2D], bounds: Option<&Bounds>) { - self.nodes.clear(); - self.edges.clear(); - - if lines.is_empty() { - return; - } - - // Generate nodes on a uniform grid with clearance checks - self.nodes = generate_grid_nodes(lines, bounds, &self.config); - - // Build edges between nodes - self.build_edges(lines); - - // Optimize by merging visibility cliques into centroids - self.optimize_nodes(lines); - } - - /// Build edges between all nodes that can be safely connected. - fn build_edges(&mut self, lines: &[Line2D]) { - let n = self.nodes.len(); - self.edges = vec![Vec::new(); n]; - - for i in 0..n { - for j in (i + 1)..n { - let dist = self.nodes[i].position.distance(self.nodes[j].position); - - // Skip if too far - if dist > self.config.max_edge_length { - continue; - } - - // Check if edge is safe - if is_edge_safe( - self.nodes[i].position, - self.nodes[j].position, - lines, - self.config.min_clearance, - self.config.min_clearance_narrow, - self.config.edge_clearance_step, - ) { - self.edges[i].push((j, dist)); - self.edges[j].push((i, dist)); - } - } - } - } - - /// Update the graph after new walls are added. - /// - /// This performs an incremental update: - /// 1. Invalidates nodes too close to new walls - /// 2. Generates new nodes in the updated space - /// 3. Rebuilds affected edges - /// - /// # Arguments - /// * `all_lines` - All wall segments in the map - /// * `new_lines` - Only the newly added wall segments - pub fn update(&mut self, all_lines: &[Line2D], new_lines: &[Line2D]) { - if new_lines.is_empty() { - return; - } - - // 1. Remove nodes too close to ANY wall (using min_clearance, not min_clearance_narrow) - // This catches nodes that were valid when created but are now too close - // due to newly discovered walls OR were created with looser constraints - let original_count = self.nodes.len(); - self.nodes.retain(|node| { - min_distance_to_walls(node.position, all_lines) >= self.config.min_clearance - }); - - let removed = original_count - self.nodes.len(); - if removed > 0 { - log::debug!("update: removed {} nodes too close to walls", removed); - } - - // 2. Generate new nodes - let new_nodes = generate_grid_nodes(all_lines, None, &self.config); - - // 3. Add new nodes, avoiding duplicates - for node in new_nodes { - if !self.has_nearby_node(node.position) { - self.add_node(node, all_lines); - } - } - } - - /// Add nodes from a single lidar scan using visibility-filtered grid sampling. - /// - /// This method provides uniform node distribution in visible areas: - /// 1. Updates the visibility region with the new scan - /// 2. Gets newly visible cells from the visibility grid - /// 3. Places nodes at cell centers with deterministic jitter - /// 4. Runs clique optimization periodically - /// - /// The visibility grid resolution matches `coverage_node_interval`, so each - /// visible cell can generate at most one node, ensuring uniform spacing. - /// - /// # Arguments - /// * `robot_pose` - Current robot pose (position and heading) - /// * `scan_points` - Lidar scan endpoints in world coordinates - /// * `lines` - Wall segments for clearance computation - pub fn add_nodes_from_scan( - &mut self, - robot_pose: Pose2D, - scan_points: &[Point2D], - lines: &[Line2D], - ) { - if scan_points.is_empty() { - return; - } - - let robot_pos = robot_pose.position(); - - // First, add a node at the robot position if it has good clearance. - // This ensures the robot's current position is connected to the graph. - if !self.has_nearby_node(robot_pos) && self.nodes.len() < self.config.max_nodes { - // Check clearance from existing wall lines - let clearance_from_lines = if lines.is_empty() { - f32::INFINITY - } else { - min_distance_to_walls(robot_pos, lines) - }; - - // Check clearance from ALL scan points (current scan's wall positions) - let clearance_from_scan = scan_points - .iter() - .map(|p| robot_pos.distance(*p)) - .min_by(|a, b| a.partial_cmp(b).unwrap_or(std::cmp::Ordering::Equal)) - .unwrap_or(f32::INFINITY); - - // Use the minimum clearance from both sources - let clearance = clearance_from_lines.min(clearance_from_scan); - - // Node must be at least min_clearance from any wall - if clearance >= self.config.min_clearance { - let node = CBVGNode::medial_axis(robot_pos, clearance, false); - self.add_node(node, lines); - } - } - - // Add nodes along rays from robot to each scan point. - // This ensures nodes are only placed in FREE SPACE (before the wall), - // never at or beyond the scan point which represents the wall location. - // We pass all scan_points so we can check distance to ALL walls (scan points), - // not just the one we're tracing along. - for scan_point in scan_points { - self.add_nodes_along_ray(robot_pos, *scan_point, lines, scan_points); - } - } - - /// Add nodes along a ray from robot position toward scan endpoint. - /// - /// The scan endpoint represents where the lidar hit a wall, so nodes - /// are only placed in the FREE SPACE before the wall, maintaining - /// minimum clearance from ALL walls (both existing lines and scan points). - /// - /// # Arguments - /// * `robot_pos` - Current robot position - /// * `scan_point` - Lidar scan endpoint (wall location) in world coordinates - /// * `lines` - Existing wall segments for clearance computation - /// * `all_scan_points` - All scan points from current scan (represent walls not yet in lines) - fn add_nodes_along_ray( - &mut self, - robot_pos: Point2D, - scan_point: Point2D, - lines: &[Line2D], - all_scan_points: &[Point2D], - ) { - // Cap ray length at max_lidar_range for node creation - // (wall detection still uses full scan range) - let actual_distance = robot_pos.distance(scan_point); - let ray_length = actual_distance.min(self.config.max_lidar_range); - - // Calculate unit direction for node placement - let dir_x = (scan_point.x - robot_pos.x) / actual_distance; - let dir_y = (scan_point.y - robot_pos.y) / actual_distance; - - // Only maintain clearance from wall if wall is within our node range - let safe_length = if actual_distance <= self.config.max_lidar_range { - ray_length - self.config.min_clearance // Stop before wall - } else { - ray_length // No wall in range, use full node range - }; - - if safe_length < self.config.coverage_node_interval * 0.5 { - return; // Not enough room for any nodes - } - - // Limit total nodes - if self.nodes.len() >= self.config.max_nodes { - return; - } - - // Sample points along the ray at coverage_node_interval spacing - // Only sample up to safe_length - let num_samples = (safe_length / self.config.coverage_node_interval).floor() as usize; - if num_samples == 0 { - return; - } - - for i in 1..=num_samples { - let distance = i as f32 * self.config.coverage_node_interval; - - // Safety check: never place nodes within min_clearance of a nearby wall - if actual_distance <= self.config.max_lidar_range { - let dist_to_wall = actual_distance - distance; - if dist_to_wall < self.config.min_clearance { - break; - } - } - - // Place node at distance along ray direction - let candidate = Point2D::new( - robot_pos.x + distance * dir_x, - robot_pos.y + distance * dir_y, - ); - - // Check for duplicates first (fast check) - if self.has_nearby_node(candidate) { - continue; - } - - // Check clearance from existing wall lines - let clearance_from_lines = if lines.is_empty() { - f32::INFINITY - } else { - min_distance_to_walls(candidate, lines) - }; - - // Check clearance from ALL scan points (current scan's wall positions) - let clearance_from_scan = all_scan_points - .iter() - .map(|p| candidate.distance(*p)) - .min_by(|a, b| a.partial_cmp(b).unwrap_or(std::cmp::Ordering::Equal)) - .unwrap_or(f32::INFINITY); - - // Use the minimum clearance from both sources - let clearance = clearance_from_lines.min(clearance_from_scan); - - if clearance < self.config.min_clearance { - continue; // Too close to wall - } - - // Add node (all nodes now have good clearance, not narrow) - let node = CBVGNode::medial_axis(candidate, clearance, false); - self.add_node(node, lines); - - // Limit total nodes - if self.nodes.len() >= self.config.max_nodes { - break; - } - } - } - - /// Remove a node from the graph. - fn remove_node(&mut self, idx: usize) { - if idx >= self.nodes.len() { - return; - } - - // Remove from nodes - self.nodes.remove(idx); - - // Remove from edges and update indices - self.edges.remove(idx); - for neighbors in &mut self.edges { - neighbors.retain(|(neighbor_idx, _)| *neighbor_idx != idx); - for (neighbor_idx, _) in neighbors.iter_mut() { - if *neighbor_idx > idx { - *neighbor_idx -= 1; - } - } - } - } - - /// Add a node to the graph and connect it to nearby nodes. - fn add_node(&mut self, node: CBVGNode, lines: &[Line2D]) { - let new_idx = self.nodes.len(); - self.nodes.push(node); - self.edges.push(Vec::new()); - - // Connect to existing nodes - for i in 0..new_idx { - let dist = self.nodes[i] - .position - .distance(self.nodes[new_idx].position); - - if dist <= self.config.max_edge_length - && is_edge_safe( - self.nodes[i].position, - self.nodes[new_idx].position, - lines, - self.config.min_clearance, - self.config.min_clearance_narrow, - self.config.edge_clearance_step, - ) - { - self.edges[i].push((new_idx, dist)); - self.edges[new_idx].push((i, dist)); - } - } - } - - /// Check if there's a node near the given position. - fn has_nearby_node(&self, position: Point2D) -> bool { - self.nodes - .iter() - .any(|n| n.position.distance(position) < self.config.node_merge_distance) - } - - /// Find a path from start to goal. - /// - /// Always routes through CBVG nodes for safe navigation with proper - /// wall clearance. This ensures predictable paths that follow the - /// medial axis of free space. - /// - /// # Arguments - /// * `from` - Start position - /// * `to` - Goal position - /// * `lines` - Wall segments (for clearance checks) - /// - /// # Returns - /// Path if one exists, None otherwise. - pub fn find_path(&self, from: Point2D, to: Point2D, lines: &[Line2D]) -> Option { - // Always route through CBVG nodes for safe navigation - - // 1. Find nearest reachable node to start - let start_node = match self.find_nearest_reachable(from, lines) { - Some(idx) => idx, - None => { - log::debug!( - "CBVG: No reachable node from start ({:.2}, {:.2}). Graph has {} nodes, {} edges", - from.x, - from.y, - self.nodes.len(), - self.edge_count() - ); - return None; - } - }; - - // 2. Find nearest reachable node to goal - let goal_node = match self.find_nearest_reachable(to, lines) { - Some(idx) => idx, - None => { - log::debug!( - "CBVG: No reachable node from goal ({:.2}, {:.2}). Graph has {} nodes, {} edges", - to.x, - to.y, - self.nodes.len(), - self.edge_count() - ); - return None; - } - }; - - // 3. If same node, just go from -> node -> to - if start_node == goal_node { - let node_pos = self.nodes[start_node].position; - let len1 = from.distance(node_pos); - let len2 = node_pos.distance(to); - return Some(Path { - points: vec![from, node_pos, to], - length: len1 + len2, - }); - } - - // 4. Dijkstra search through CBVG nodes - let result = match dijkstra(&self.edges, start_node, goal_node) { - Some(r) => r, - None => { - log::debug!( - "CBVG: Dijkstra failed from node {} to node {}. Graph may be disconnected ({} nodes, {} edges)", - start_node, - goal_node, - self.nodes.len(), - self.edge_count() - ); - return None; - } - }; - - // 5. Build full path - self.build_path(from, to, &result) - } - - /// Find the nearest node reachable from a point. - fn find_nearest_reachable(&self, point: Point2D, lines: &[Line2D]) -> Option { - self.find_nearest_reachable_excluding(point, lines, &[]) - } - - /// Find the nearest node reachable from a point, excluding specific node indices. - /// Used to find alternative approach angles after collision. - fn find_nearest_reachable_excluding( - &self, - point: Point2D, - lines: &[Line2D], - excluded: &[usize], - ) -> Option { - self.nodes - .iter() - .enumerate() - .filter(|(idx, _)| !excluded.contains(idx)) - .filter(|(_, node)| { - is_path_clear_with_clearance( - point, - node.position, - lines, - self.config.min_clearance_narrow, - self.config.edge_clearance_step, - ) - }) - .min_by(|(_, a), (_, b)| { - point - .distance(a.position) - .partial_cmp(&point.distance(b.position)) - .unwrap_or(std::cmp::Ordering::Equal) - }) - .map(|(idx, _)| idx) - } - - /// Find a path from start to goal, excluding specific goal node indices. - /// - /// This is used to find alternative approach angles after a collision. - /// When a path to a frontier causes a collision, the goal node that led - /// to the collision can be excluded, forcing the planner to find an - /// alternative node (and thus a different approach angle). - /// - /// # Arguments - /// * `from` - Start position - /// * `to` - Goal position - /// * `lines` - Wall segments (for clearance checks) - /// * `excluded_goal_nodes` - Node indices to exclude when finding the goal node - /// - /// # Returns - /// Tuple of (Path, goal_node_index) if path exists, None otherwise. - pub fn find_path_excluding_nodes( - &self, - from: Point2D, - to: Point2D, - lines: &[Line2D], - excluded_goal_nodes: &[usize], - ) -> Option<(Path, usize)> { - // 1. Find nearest reachable node to start (not excluded - we need to start somewhere) - let start_node = match self.find_nearest_reachable(from, lines) { - Some(idx) => idx, - None => { - log::debug!( - "CBVG: No reachable node from start ({:.2}, {:.2})", - from.x, - from.y - ); - return None; - } - }; - - // 2. Find nearest reachable node to goal, EXCLUDING specified nodes - let goal_node = match self.find_nearest_reachable_excluding(to, lines, excluded_goal_nodes) - { - Some(idx) => idx, - None => { - log::debug!( - "CBVG: No reachable goal node from ({:.2}, {:.2}) after excluding {} nodes", - to.x, - to.y, - excluded_goal_nodes.len() - ); - return None; - } - }; - - // 3. If same node, just go from -> node -> to - if start_node == goal_node { - let node_pos = self.nodes[start_node].position; - let len1 = from.distance(node_pos); - let len2 = node_pos.distance(to); - return Some(( - Path { - points: vec![from, node_pos, to], - length: len1 + len2, - }, - goal_node, - )); - } - - // 4. Dijkstra search through CBVG nodes - let result = match dijkstra(&self.edges, start_node, goal_node) { - Some(r) => r, - None => { - log::debug!( - "CBVG: Dijkstra failed from node {} to node {}", - start_node, - goal_node - ); - return None; - } - }; - - // 5. Build full path and return with goal node index - self.build_path(from, to, &result) - .map(|path| (path, goal_node)) - } - - /// Build the final path from Dijkstra result. - fn build_path(&self, from: Point2D, to: Point2D, result: &DijkstraResult) -> Option { - let mut points = Vec::with_capacity(result.path.len() + 2); - let mut length = 0.0; - - points.push(from); - - for (i, &idx) in result.path.iter().enumerate() { - let node_pos = self.nodes[idx].position; - - if i == 0 { - length += from.distance(node_pos); - } else { - length += points.last()?.distance(node_pos); - } - - points.push(node_pos); - } - - length += points.last()?.distance(to); - points.push(to); - - Some(Path { points, length }) - } - - /// Get all nodes in the graph. - pub fn nodes(&self) -> &[CBVGNode] { - &self.nodes - } - - /// Get the number of nodes. - pub fn node_count(&self) -> usize { - self.nodes.len() - } - - /// Get the number of edges. - pub fn edge_count(&self) -> usize { - self.edges.iter().map(|e| e.len()).sum::() / 2 - } - - /// Get the adjacency list. - pub fn edges(&self) -> &[Vec<(usize, f32)>] { - &self.edges - } - - /// Get the configuration. - pub fn config(&self) -> &CBVGConfig { - &self.config - } - - /// Check if the graph is empty. - pub fn is_empty(&self) -> bool { - self.nodes.is_empty() - } - - /// Find the nearest node to a target point. - /// - /// Returns the node position if found within max_distance. - /// This is useful for exploration to find reachable navigation targets. - /// - /// # Arguments - /// * `target` - The point to find the nearest node to - /// * `max_distance` - Maximum distance to search (nodes farther are ignored) - /// - /// # Returns - /// The position of the nearest node, or None if no node is within range. - pub fn nearest_node(&self, target: Point2D, max_distance: f32) -> Option { - self.nodes - .iter() - .filter(|n| n.position.distance(target) <= max_distance) - .min_by(|a, b| { - a.position - .distance(target) - .partial_cmp(&b.position.distance(target)) - .unwrap_or(std::cmp::Ordering::Equal) - }) - .map(|n| n.position) - } - - /// Clear all nodes and edges. - pub fn clear(&mut self) { - self.nodes.clear(); - self.edges.clear(); - } - - // ───────────────────────────────────────────────────────────────────────── - // Frontier Detection - // ───────────────────────────────────────────────────────────────────────── - - /// Detect frontier nodes - nodes with uncovered angular gaps. - /// - /// A node is a frontier if it has a gap of at least `min_frontier_angle` degrees - /// where there is neither a neighboring node nor a wall. - /// - /// # Arguments - /// * `lines` - Wall segments for checking wall coverage - /// * `min_frontier_angle` - Minimum uncovered angle to be a frontier (radians) - /// * `max_wall_distance` - Maximum distance to check for walls (meters) - /// - /// # Returns - /// Tuple of (frontiers, frontier_node_indices) for visualization - pub fn detect_frontiers( - &self, - lines: &[Line2D], - min_frontier_angle: f32, - max_wall_distance: f32, - ) -> (Vec, HashSet) { - let mut frontiers = Vec::new(); - let mut frontier_indices = HashSet::new(); - - for (node_idx, node) in self.nodes.iter().enumerate() { - // Skip temporary nodes (robot/goal) - if node.is_temporary() { - continue; - } - - if let Some((frontier, _gap_angle)) = - self.check_node_frontier(node_idx, lines, min_frontier_angle, max_wall_distance) - { - frontiers.push(frontier); - frontier_indices.insert(node_idx); - } - } - - (frontiers, frontier_indices) - } - - /// Check if a single node is a frontier. - /// - /// Returns the frontier and the gap angle if this node is a frontier. - fn check_node_frontier( - &self, - node_idx: usize, - lines: &[Line2D], - min_frontier_angle: f32, - max_wall_distance: f32, - ) -> Option<(Frontier, f32)> { - let node = &self.nodes[node_idx]; - let mut covered_arcs: Vec<(f32, f32)> = Vec::new(); - - // Add arcs covered by neighboring nodes - for &(neighbor_idx, distance) in &self.edges[node_idx] { - let neighbor = &self.nodes[neighbor_idx]; - let dx = neighbor.position.x - node.position.x; - let dy = neighbor.position.y - node.position.y; - let angle = dy.atan2(dx); - - // Each neighbor covers a cone - wider for closer neighbors - // half_cone = atan(node_spacing / 2 / distance) - let half_cone = (self.config.node_merge_distance / 2.0 / distance) - .atan() - .max(0.15); // At least ~8 degrees - covered_arcs.push((angle - half_cone, angle + half_cone)); - } - - // Add arcs covered by nearby walls - for line in lines { - let distance = line.distance_to_point(node.position); - - if distance < max_wall_distance { - // Compute angles to line endpoints - let dx1 = line.start.x - node.position.x; - let dy1 = line.start.y - node.position.y; - let dx2 = line.end.x - node.position.x; - let dy2 = line.end.y - node.position.y; - - let angle1 = dy1.atan2(dx1); - let angle2 = dy2.atan2(dx2); - - // Handle angle wrapping - find the shorter arc - let (start, end) = normalize_arc(angle1, angle2); - covered_arcs.push((start, end)); - } - } - - // Find the largest uncovered gap - if let Some((gap_start, gap_end, gap_size)) = find_largest_gap(&covered_arcs) - && gap_size >= min_frontier_angle - { - // This node is a frontier! - let frontier_angle = normalize_angle((gap_start + gap_end) / 2.0); - let look_direction = Point2D::new(frontier_angle.cos(), frontier_angle.sin()); - - return Some(( - Frontier { - viewpoint: node.position, - look_direction, - endpoint: node.position, - line_idx: 0, - estimated_area: gap_size * 2.0, // Rough estimate - }, - gap_size, - )); - } - - None - } - - /// Export graph to SVG for visualization. - /// - /// # Arguments - /// * `lines` - Wall segments to draw - /// * `path` - Optional path to highlight - /// - /// # Returns - /// SVG string that can be written to a file. - pub fn to_svg(&self, lines: &[Line2D], path: Option<&Path>) -> String { - // Calculate bounds - let (min_x, min_y, max_x, max_y) = self.calculate_svg_bounds(lines); - - let width = max_x - min_x; - let height = max_y - min_y; - let margin = 0.5; - - let view_min_x = min_x - margin; - let view_min_y = min_y - margin; - let view_width = width + 2.0 * margin; - let view_height = height + 2.0 * margin; - - let mut svg = format!( - r#" - - - -"#, - view_min_x, - -view_min_y - view_height, - view_width, - view_height, - view_min_x, - -view_min_y - view_height, - view_width, - view_height, - 2.0 * view_min_y + view_height - ); - - // Draw walls (black) - svg.push_str(" \n"); - for line in lines { - svg.push_str(&format!( - r#" "#, - line.start.x, line.start.y, line.end.x, line.end.y - )); - svg.push('\n'); - } - - // Draw edges (light gray) - svg.push_str(" \n"); - for (i, neighbors) in self.edges.iter().enumerate() { - for (j, _) in neighbors { - if i < *j { - let p1 = self.nodes[i].position; - let p2 = self.nodes[*j].position; - svg.push_str(&format!( - " \n", - p1.x, p1.y, p2.x, p2.y - )); - } - } - } - - // Draw path if provided (thick green) - if let Some(path) = path { - svg.push_str(" \n"); - for i in 0..path.points.len().saturating_sub(1) { - let p1 = path.points[i]; - let p2 = path.points[i + 1]; - svg.push_str(&format!( - r#" "#, - p1.x, p1.y, p2.x, p2.y - )); - svg.push('\n'); - } - } - - // Draw nodes - svg.push_str(" \n"); - for node in &self.nodes { - let color = node.svg_color(); - let radius = if node.is_temporary() { 0.08 } else { 0.05 }; - svg.push_str(&format!( - r#" "#, - node.position.x, node.position.y, radius, color - )); - svg.push('\n'); - } - - svg.push_str(" \n\n"); - svg - } - - /// Export graph to SVG with frontier nodes highlighted. - /// - /// Frontier nodes are shown in magenta with direction arrows. - /// - /// # Arguments - /// * `lines` - Wall segments to draw - /// * `path` - Optional path to highlight - /// * `frontiers` - Frontier data with directions - /// * `frontier_indices` - Set of node indices that are frontiers - pub fn to_svg_with_frontiers( - &self, - lines: &[Line2D], - path: Option<&Path>, - frontiers: &[Frontier], - frontier_indices: &HashSet, - ) -> String { - // Calculate bounds - let (min_x, min_y, max_x, max_y) = self.calculate_svg_bounds(lines); - - let width = max_x - min_x; - let height = max_y - min_y; - let margin = 0.5; - - let view_min_x = min_x - margin; - let view_min_y = min_y - margin; - let view_width = width + 2.0 * margin; - let view_height = height + 2.0 * margin; - - let mut svg = format!( - r#" - - - -"#, - view_min_x, - -view_min_y - view_height, - view_width, - view_height, - view_min_x, - -view_min_y - view_height, - view_width, - view_height, - 2.0 * view_min_y + view_height - ); - - // Draw walls (black) - svg.push_str(" \n"); - for line in lines { - svg.push_str(&format!( - r#" "#, - line.start.x, line.start.y, line.end.x, line.end.y - )); - svg.push('\n'); - } - - // Draw edges (light gray) - svg.push_str(" \n"); - for (i, neighbors) in self.edges.iter().enumerate() { - for (j, _) in neighbors { - if i < *j { - let p1 = self.nodes[i].position; - let p2 = self.nodes[*j].position; - svg.push_str(&format!( - " \n", - p1.x, p1.y, p2.x, p2.y - )); - } - } - } - - // Draw path if provided (thick green) - if let Some(path) = path { - svg.push_str(" \n"); - for i in 0..path.points.len().saturating_sub(1) { - let p1 = path.points[i]; - let p2 = path.points[i + 1]; - svg.push_str(&format!( - r#" "#, - p1.x, p1.y, p2.x, p2.y - )); - svg.push('\n'); - } - } - - // Draw regular nodes (blue/orange) - svg.push_str(" \n"); - for (idx, node) in self.nodes.iter().enumerate() { - if frontier_indices.contains(&idx) { - continue; // Draw frontiers separately - } - let color = node.svg_color(); - let radius = if node.is_temporary() { 0.08 } else { 0.05 }; - svg.push_str(&format!( - r#" "#, - node.position.x, node.position.y, radius, color - )); - svg.push('\n'); - } - - // Draw frontier nodes (magenta) with direction arrows - svg.push_str(" \n"); - for frontier in frontiers { - let pos = frontier.viewpoint; - let dir = frontier.look_direction; - - // Draw node circle (magenta) - svg.push_str(&format!( - r#" "#, - pos.x, pos.y - )); - svg.push('\n'); - - // Draw direction arrow - let arrow_len = 0.3; - let arrow_end_x = pos.x + dir.x * arrow_len; - let arrow_end_y = pos.y + dir.y * arrow_len; - svg.push_str(&format!( - r#" "#, - pos.x, pos.y, arrow_end_x, arrow_end_y - )); - svg.push('\n'); - } - - svg.push_str(" \n\n"); - svg - } - - /// Calculate bounds for SVG export. - fn calculate_svg_bounds(&self, lines: &[Line2D]) -> (f32, f32, f32, f32) { - let mut min_x = f32::INFINITY; - let mut min_y = f32::INFINITY; - let mut max_x = f32::NEG_INFINITY; - let mut max_y = f32::NEG_INFINITY; - - for line in lines { - min_x = min_x.min(line.start.x).min(line.end.x); - min_y = min_y.min(line.start.y).min(line.end.y); - max_x = max_x.max(line.start.x).max(line.end.x); - max_y = max_y.max(line.start.y).max(line.end.y); - } - - for node in &self.nodes { - min_x = min_x.min(node.position.x); - min_y = min_y.min(node.position.y); - max_x = max_x.max(node.position.x); - max_y = max_y.max(node.position.y); - } - - if min_x.is_infinite() { - (0.0, 0.0, 1.0, 1.0) - } else { - (min_x, min_y, max_x, max_y) - } - } - - /// Add a temporary node for the robot's current position. - /// - /// Returns the index of the added node. - pub fn add_robot_node(&mut self, position: Point2D, lines: &[Line2D]) -> usize { - let clearance = min_distance_to_walls(position, lines); - let node = CBVGNode::robot(position, clearance); - self.add_node(node, lines); - self.nodes.len() - 1 - } - - /// Add a temporary node for a goal position. - /// - /// Returns the index of the added node. - pub fn add_goal_node(&mut self, position: Point2D, lines: &[Line2D]) -> usize { - let clearance = min_distance_to_walls(position, lines); - let node = CBVGNode::goal(position, clearance); - self.add_node(node, lines); - self.nodes.len() - 1 - } - - /// Remove all temporary nodes (robot and goal nodes). - pub fn remove_temporary_nodes(&mut self) { - let mut indices_to_remove: Vec = self - .nodes - .iter() - .enumerate() - .filter(|(_, n)| n.is_temporary()) - .map(|(i, _)| i) - .collect(); - - // Remove in reverse order - indices_to_remove.sort_by(|a, b| b.cmp(a)); - for idx in indices_to_remove { - self.remove_node(idx); - } - } - - /// Remove nodes that are too close to scan endpoints. - /// - /// This catches nodes that were valid when created but are now - /// too close due to newly discovered obstacles from subsequent scans. - /// Unlike wall-line-based checks, this uses raw scan points which are - /// available immediately before line fitting occurs. - /// - /// # Arguments - /// * `scan_points` - World-frame scan endpoints to check against - /// * `lines` - Wall segments for rebuilding edges after cleanup - pub fn cleanup_nodes_near_scan_points(&mut self, scan_points: &[Point2D], lines: &[Line2D]) { - if scan_points.is_empty() { - return; - } - - let original_count = self.nodes.len(); - self.nodes.retain(|node| { - // Find minimum distance to any scan point - let min_dist = scan_points - .iter() - .map(|p| node.position.distance(*p)) - .min_by(|a, b| a.partial_cmp(b).unwrap_or(std::cmp::Ordering::Equal)) - .unwrap_or(f32::INFINITY); - - min_dist >= self.config.min_clearance - }); - - let removed = original_count - self.nodes.len(); - if removed > 0 { - log::debug!( - "cleanup_nodes_near_scan_points: removed {} nodes too close to scan points", - removed - ); - // Rebuild edges after removing nodes (indices are invalidated) - self.build_edges(lines); - } - } - - /// Remove edges that pass too close to scan endpoints. - /// - /// An edge between two nodes may pass through or near an obstacle even if - /// both nodes have sufficient clearance. This method removes edges where - /// any scan point has a perpendicular distance to the edge less than - /// `min_clearance`. - /// - /// # Arguments - /// * `scan_points` - World-frame scan endpoints to check against - pub fn cleanup_edges_near_scan_points(&mut self, scan_points: &[Point2D]) { - if scan_points.is_empty() || self.nodes.is_empty() { - return; - } - - let mut removed_count = 0; - - // Process each node's edge list - for i in 0..self.edges.len() { - let node_i_pos = self.nodes[i].position; - - // Filter edges that are too close to scan points - self.edges[i].retain(|(j, _dist)| { - let node_j_pos = self.nodes[*j].position; - - // Create a line segment representing the edge - let edge_line = Line2D::new(node_i_pos, node_j_pos); - - // Find minimum perpendicular distance from any scan point to the edge - let min_dist_to_edge = scan_points - .iter() - .map(|p| edge_line.distance_to_point_segment(*p)) - .min_by(|a, b| a.partial_cmp(b).unwrap_or(std::cmp::Ordering::Equal)) - .unwrap_or(f32::INFINITY); - - if min_dist_to_edge < self.config.min_clearance { - removed_count += 1; - false // Remove this edge - } else { - true // Keep this edge - } - }); - } - - // Note: removed_count counts removals from one direction only (i -> j) - // The j -> i edge will be removed when we process node j's edges - if removed_count > 0 { - log::debug!( - "cleanup_edges_near_scan_points: removed {} edge references too close to scan points", - removed_count - ); - } - } - - /// Optimize the graph by merging clusters of mutually visible nodes. - /// - /// This finds cliques (fully connected subgraphs where all nodes can see each other) - /// and merges them into single centroid nodes. This reduces redundant path options - /// and simplifies the graph. - /// - /// # Arguments - /// * `lines` - Wall segments for visibility checks - pub fn optimize_nodes(&mut self, lines: &[Line2D]) { - // Step 0: Remove nodes that are too close to walls - // This catches nodes that were valid when created but are now too close - // due to newly discovered walls from subsequent scans - let original_count = self.nodes.len(); - self.nodes.retain(|node| { - let clearance = min_distance_to_walls(node.position, lines); - clearance >= self.config.min_clearance - }); - let removed = original_count - self.nodes.len(); - if removed > 0 { - log::debug!( - "optimize_nodes: removed {} nodes too close to walls", - removed - ); - } - - let n = self.nodes.len(); - if n < self.config.min_clique_size_for_merge { - // Rebuild edges after removal even if we don't do clique merging - if removed > 0 { - self.build_edges(lines); - } - return; // Not enough nodes to form a clique worth merging - } - - // Step 1: Build visibility matrix (which nodes can see each other) - let visible = self.build_visibility_matrix(lines); - - // Step 2: Find greedy maximal cliques - let cliques = Self::find_greedy_cliques(&visible); - - // Step 3: Merge cliques into centroids - let mut new_nodes: Vec = Vec::new(); - - for clique in cliques { - if clique.len() >= self.config.min_clique_size_for_merge { - // Merge clique to centroid - let centroid_pos = Self::compute_centroid(&clique, &self.nodes); - let centroid_clearance = min_distance_to_walls(centroid_pos, lines); - - if centroid_clearance >= self.config.min_clearance { - // Valid centroid - create merged node - new_nodes.push(CBVGNode::medial_axis( - centroid_pos, - centroid_clearance, - false, - )); - } else { - // Centroid too close to wall - keep original nodes - for &idx in &clique { - new_nodes.push(self.nodes[idx].clone()); - } - } - } else { - // Clique too small to merge - keep original nodes - for &idx in &clique { - new_nodes.push(self.nodes[idx].clone()); - } - } - } - - // Step 4: Replace nodes and rebuild edges - self.nodes = new_nodes; - self.build_edges(lines); - } - - /// Build a visibility matrix showing which nodes can see each other. - /// Uses the same constraints as build_edges() to ensure consistency. - fn build_visibility_matrix(&self, lines: &[Line2D]) -> Vec> { - let n = self.nodes.len(); - let mut visible = vec![vec![false; n]; n]; - - for i in 0..n { - for j in (i + 1)..n { - let dist = self.nodes[i].position.distance(self.nodes[j].position); - - // Must be within max_edge_length (same as build_edges) - if dist > self.config.max_edge_length { - continue; - } - - // Check if nodes can see each other with safe clearance - if is_edge_safe( - self.nodes[i].position, - self.nodes[j].position, - lines, - self.config.min_clearance, - self.config.min_clearance_narrow, - self.config.edge_clearance_step, - ) { - visible[i][j] = true; - visible[j][i] = true; - } - } - } - - visible - } - - /// Find greedy maximal cliques from a visibility matrix. - /// - /// Uses a greedy algorithm that picks the node with most visible neighbors - /// as seed, then greedily adds nodes that can see all clique members. - fn find_greedy_cliques(visible: &[Vec]) -> Vec> { - // Safety limit: max iterations = 2x the number of nodes (each iteration removes at least 1 node) - const MAX_ITERATIONS: usize = 10000; - - let n = visible.len(); - let mut unassigned: std::collections::HashSet = (0..n).collect(); - let mut cliques: Vec> = Vec::new(); - let mut iterations = 0; - - while !unassigned.is_empty() { - iterations += 1; - if iterations > MAX_ITERATIONS { - log::warn!( - "find_greedy_cliques: exceeded {} iterations with {} nodes remaining, assigning as singletons", - MAX_ITERATIONS, - unassigned.len() - ); - // Assign remaining nodes as singleton cliques - for node in unassigned { - cliques.push(vec![node]); - } - break; - } - - // Find seed with most visible neighbors among unassigned nodes - let seed = *unassigned - .iter() - .max_by_key(|&&i| unassigned.iter().filter(|&&j| visible[i][j]).count()) - .unwrap(); - - let mut clique = vec![seed]; - - // Get candidates: unassigned nodes visible to seed, with their visibility counts - let mut candidates: Vec<(usize, usize)> = unassigned - .iter() - .filter(|&&j| j != seed && visible[seed][j]) - .map(|&j| { - // Count how many other candidates this node can see - let vis_count = unassigned.iter().filter(|&&k| visible[j][k]).count(); - (j, vis_count) - }) - .collect(); - - // Sort by visibility count descending (greedy heuristic - prefer well-connected nodes) - candidates.sort_by_key(|&(_, count)| std::cmp::Reverse(count)); - - // Greedily add nodes that can see ALL current clique members - for (candidate, _) in candidates { - if clique.iter().all(|&m| visible[candidate][m]) { - clique.push(candidate); - } - } - - // Remove clique nodes from unassigned - for &node in &clique { - unassigned.remove(&node); - } - - cliques.push(clique); - } - - cliques - } - - /// Compute the centroid (average position) of a set of nodes. - fn compute_centroid(indices: &[usize], nodes: &[CBVGNode]) -> Point2D { - let sum_x: f32 = indices.iter().map(|&i| nodes[i].position.x).sum(); - let sum_y: f32 = indices.iter().map(|&i| nodes[i].position.y).sum(); - let count = indices.len() as f32; - Point2D::new(sum_x / count, sum_y / count) - } -} - -impl Default for ClearanceVisibilityGraph { - fn default() -> Self { - Self::new(CBVGConfig::default()) - } -} - -// ───────────────────────────────────────────────────────────────────────────── -// Helper functions for angular gap detection -// ───────────────────────────────────────────────────────────────────────────── - -/// Normalize an angle to the range [-π, π]. -/// Uses modular arithmetic to avoid potential infinite loops with extreme values. -#[inline] -fn normalize_angle(angle: f32) -> f32 { - if !angle.is_finite() { - return 0.0; - } - let mut a = angle % (2.0 * PI); - if a > PI { - a -= 2.0 * PI; - } else if a < -PI { - a += 2.0 * PI; - } - a -} - -/// Normalize an arc to ensure start < end and handle wrapping. -/// Returns (start, end) where the arc goes from start to end counter-clockwise. -fn normalize_arc(angle1: f32, angle2: f32) -> (f32, f32) { - let a1 = normalize_angle(angle1); - let a2 = normalize_angle(angle2); - - // Find the shorter arc between the two angles - let diff = normalize_angle(a2 - a1); - - if diff >= 0.0 { - (a1, a1 + diff) - } else { - (a2, a2 - diff) - } -} - -/// Find the largest uncovered gap in a set of angular arcs. -/// -/// # Arguments -/// * `arcs` - List of (start_angle, end_angle) arcs that are "covered" -/// -/// # Returns -/// The largest uncovered gap as (start_angle, end_angle, size_in_radians), -/// or None if the entire circle is covered. -fn find_largest_gap(arcs: &[(f32, f32)]) -> Option<(f32, f32, f32)> { - if arcs.is_empty() { - // No coverage at all - entire circle is a gap - return Some((-PI, PI, 2.0 * PI)); - } - - // Convert all arcs to a common representation and collect edge events - // Each arc contributes a "start coverage" and "end coverage" event - let mut events: Vec<(f32, i32)> = Vec::with_capacity(arcs.len() * 2); - - for &(start, end) in arcs { - let s = normalize_angle(start); - let e = normalize_angle(end); - - if s <= e { - // Normal arc - events.push((s, 1)); // Start coverage - events.push((e, -1)); // End coverage - } else { - // Arc wraps around -π/π boundary - events.push((s, 1)); - events.push((PI, -1)); - events.push((-PI, 1)); - events.push((e, -1)); - } - } - - // Sort events by angle - events.sort_by(|a, b| a.0.partial_cmp(&b.0).unwrap_or(std::cmp::Ordering::Equal)); - - // Sweep through events to find uncovered regions - let mut coverage = 0; - let mut largest_gap: Option<(f32, f32, f32)> = None; - let mut gap_start: Option = None; - - // Check initial state at -π - for &(angle, delta) in &events { - if angle > -PI + 0.001 { - break; - } - coverage += delta; - } - - if coverage == 0 { - gap_start = Some(-PI); - } - - // Process all events - for &(angle, delta) in &events { - let was_covered = coverage > 0; - coverage += delta; - let now_covered = coverage > 0; - - if was_covered && !now_covered { - // Gap starts here - gap_start = Some(angle); - } else if !was_covered && now_covered { - // Gap ends here - if let Some(start) = gap_start { - let size = normalize_angle(angle - start); - let size = if size < 0.0 { size + 2.0 * PI } else { size }; - - if largest_gap.is_none() || size > largest_gap.unwrap().2 { - largest_gap = Some((start, angle, size)); - } - gap_start = None; - } - } - } - - // Check if there's a gap that wraps around - if let Some(start) = gap_start { - // Gap extends to +π and possibly wraps to -π - let end = if coverage == 0 { - // Find first coverage after -π - events - .iter() - .find(|&&(_, d)| d > 0) - .map(|&(a, _)| a) - .unwrap_or(start) - } else { - PI - }; - - let size = if end > start { - end - start - } else { - (PI - start) + (end + PI) - }; - - if largest_gap.is_none() || size > largest_gap.unwrap().2 { - largest_gap = Some((start, end, size)); - } - } - - largest_gap -} - -#[cfg(test)] -mod tests { - use super::*; - - fn make_room() -> Vec { - vec![ - Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(4.0, 0.0)), // Bottom - Line2D::new(Point2D::new(4.0, 0.0), Point2D::new(4.0, 3.0)), // Right - Line2D::new(Point2D::new(4.0, 3.0), Point2D::new(0.0, 3.0)), // Top - Line2D::new(Point2D::new(0.0, 3.0), Point2D::new(0.0, 0.0)), // Left - ] - } - - fn make_room_with_obstacle() -> Vec { - let mut lines = make_room(); - // Add a vertical wall in the middle - lines.push(Line2D::new(Point2D::new(2.0, 0.0), Point2D::new(2.0, 2.0))); - lines - } - - #[test] - fn test_build_empty() { - let mut graph = ClearanceVisibilityGraph::default(); - graph.build(&[], None); - assert!(graph.is_empty()); - } - - #[test] - fn test_build_simple_room() { - let walls = make_room(); - let config = CBVGConfig::default() - .with_voronoi_sample_step(0.2) - .with_min_clearance(0.3) - .with_min_clearance_narrow(0.18); - - let mut graph = ClearanceVisibilityGraph::new(config); - graph.build(&walls, None); - - // Should have some nodes - assert!(!graph.is_empty()); - - // All nodes should have sufficient clearance - for node in graph.nodes() { - assert!(node.clearance >= 0.18); - } - } - - #[test] - fn test_find_path_direct() { - let walls = make_room(); - let config = CBVGConfig::default() - .with_voronoi_sample_step(0.2) - .with_min_clearance(0.2) - .with_min_clearance_narrow(0.1); - - let mut graph = ClearanceVisibilityGraph::new(config); - graph.build(&walls, None); - - // Direct path in open space - let path = graph.find_path(Point2D::new(1.0, 1.5), Point2D::new(3.0, 1.5), &walls); - - assert!(path.is_some()); - let path = path.unwrap(); - assert!(!path.is_empty()); - assert!(path.length > 0.0); - } - - #[test] - fn test_find_path_around_obstacle() { - let walls = make_room_with_obstacle(); - let config = CBVGConfig::default() - .with_voronoi_sample_step(0.15) - .with_min_clearance(0.2) - .with_min_clearance_narrow(0.1); - - let mut graph = ClearanceVisibilityGraph::new(config); - graph.build(&walls, None); - - // Path that must go around obstacle - let path = graph.find_path(Point2D::new(1.0, 1.0), Point2D::new(3.0, 1.0), &walls); - - // Path should exist and go around - if let Some(path) = path { - assert!(path.points.len() >= 2); - // Path should be longer than direct - assert!(path.length > 2.0); - } - } - - #[test] - fn test_add_remove_temporary_nodes() { - let walls = make_room(); - let mut graph = ClearanceVisibilityGraph::default(); - graph.build(&walls, None); - - let initial_count = graph.node_count(); - - // Add robot node - let robot_idx = graph.add_robot_node(Point2D::new(1.0, 1.0), &walls); - assert_eq!(graph.node_count(), initial_count + 1); - assert!(graph.nodes()[robot_idx].is_temporary()); - - // Add goal node - graph.add_goal_node(Point2D::new(3.0, 2.0), &walls); - assert_eq!(graph.node_count(), initial_count + 2); - - // Remove temporary nodes - graph.remove_temporary_nodes(); - assert_eq!(graph.node_count(), initial_count); - } - - #[test] - fn test_svg_export() { - let walls = make_room(); - let mut graph = ClearanceVisibilityGraph::default(); - graph.build(&walls, None); - - let svg = graph.to_svg(&walls, None); - assert!(svg.contains("")); - assert!(svg.contains("circle")); // Nodes - assert!(svg.contains("line")); // Walls/edges - } - - #[test] - fn test_update_incremental() { - let walls = make_room(); - let config = CBVGConfig::default().with_voronoi_sample_step(0.2); - - let mut graph = ClearanceVisibilityGraph::new(config); - graph.build(&walls, None); - - let _initial_count = graph.node_count(); - - // Add a new wall - let new_wall = vec![Line2D::new(Point2D::new(2.0, 0.0), Point2D::new(2.0, 1.5))]; - - let mut all_walls = walls.clone(); - all_walls.extend(new_wall.iter().cloned()); - - graph.update(&all_walls, &new_wall); - - // Graph should be updated (nodes may change) - // Just verify it doesn't crash and has nodes - assert!(graph.node_count() > 0); - } - - #[test] - fn test_optimize_nodes_merges_cliques() { - // Create a large open room where many nodes will be mutually visible - let walls = vec![ - Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(10.0, 0.0)), // Bottom - Line2D::new(Point2D::new(10.0, 0.0), Point2D::new(10.0, 10.0)), // Right - Line2D::new(Point2D::new(10.0, 10.0), Point2D::new(0.0, 10.0)), // Top - Line2D::new(Point2D::new(0.0, 10.0), Point2D::new(0.0, 0.0)), // Left - ]; - - // Use config that generates many nodes - let config = CBVGConfig::default() - .with_coverage_node_interval(0.5) - .with_max_edge_length(2.0) // Increase edge length for larger room - .with_min_clearance(0.3) - .with_min_clearance_narrow(0.18) - .with_min_clique_size_for_merge(3); - - let mut graph = ClearanceVisibilityGraph::new(config); - - // First build without optimization to count original nodes - graph.nodes = generate_grid_nodes(&walls, None, &graph.config); - graph.build_edges(&walls); - let nodes_before = graph.node_count(); - - // Now optimize - graph.optimize_nodes(&walls); - let nodes_after = graph.node_count(); - - // Optimization should reduce node count in open room with clusters - // or at minimum keep the same count (if no cliques >= 3) - assert!( - nodes_after <= nodes_before, - "Optimization should not increase node count: before={}, after={}", - nodes_before, - nodes_after - ); - - // Graph should still be valid (has nodes and can find paths) - assert!(!graph.is_empty()); - - // Verify path finding still works after optimization - let path = graph.find_path(Point2D::new(2.0, 2.0), Point2D::new(8.0, 8.0), &walls); - assert!( - path.is_some(), - "Path should still be findable after optimization" - ); - } - - #[test] - fn test_optimize_nodes_preserves_linear_chain() { - // Create a narrow corridor where nodes form a linear chain (no cliques) - let walls = vec![ - Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(10.0, 0.0)), // Bottom - Line2D::new(Point2D::new(0.0, 1.0), Point2D::new(10.0, 1.0)), // Top - ]; - - let config = CBVGConfig::default() - .with_voronoi_sample_step(0.2) - .with_min_clearance(0.3) - .with_min_clearance_narrow(0.18) - .with_min_clique_size_for_merge(3); - - let mut graph = ClearanceVisibilityGraph::new(config); - - // Build and let optimization run - graph.build(&walls, None); - - // In a narrow corridor, most nodes should be preserved as they form - // a linear chain (each node only sees neighbors, not a full clique) - // Graph should still be valid - assert!(!graph.is_empty()); - } - - #[test] - fn test_find_greedy_cliques() { - // Create a simple visibility matrix for testing - // 4 nodes: 0-1-2 form a clique, 3 is isolated - let visible = vec![ - vec![false, true, true, false], // Node 0 sees 1, 2 - vec![true, false, true, false], // Node 1 sees 0, 2 - vec![true, true, false, false], // Node 2 sees 0, 1 - vec![false, false, false, false], // Node 3 sees nobody - ]; - - let cliques = ClearanceVisibilityGraph::find_greedy_cliques(&visible); - - // Should have 2 cliques: one with {0,1,2} and one with {3} - assert_eq!(cliques.len(), 2); - - // One clique should have 3 nodes - let has_large_clique = cliques.iter().any(|c| c.len() == 3); - assert!(has_large_clique, "Should find the 3-node clique"); - - // One clique should have 1 node (isolated) - let has_singleton = cliques.iter().any(|c| c.len() == 1); - assert!( - has_singleton, - "Should have isolated node as singleton clique" - ); - } - - #[test] - fn test_compute_centroid() { - let nodes = vec![ - CBVGNode::medial_axis(Point2D::new(0.0, 0.0), 1.0, false), - CBVGNode::medial_axis(Point2D::new(2.0, 0.0), 1.0, false), - CBVGNode::medial_axis(Point2D::new(1.0, 2.0), 1.0, false), - ]; - - let indices = vec![0, 1, 2]; - let centroid = ClearanceVisibilityGraph::compute_centroid(&indices, &nodes); - - // Centroid of triangle (0,0), (2,0), (1,2) is (1, 0.667) - assert!((centroid.x - 1.0).abs() < 0.01); - assert!((centroid.y - 0.666).abs() < 0.01); - } - - #[test] - fn test_add_nodes_from_scan_basic() { - let walls = make_room(); - let config = CBVGConfig::default() - .with_coverage_node_interval(0.5) - .with_min_clearance(0.3) - .with_min_clearance_narrow(0.18); - - let mut graph = ClearanceVisibilityGraph::new(config); - - // Simulate a scan from the center of the room - let robot_pose = Pose2D::new(2.0, 1.5, 0.0); - let robot_pos = robot_pose.position(); - let scan_points: Vec = (0..36) - .map(|i| { - let angle = i as f32 * std::f32::consts::PI / 18.0; - let dist = 1.5; - Point2D::new( - robot_pos.x + dist * angle.cos(), - robot_pos.y + dist * angle.sin(), - ) - }) - .collect(); - - // Add nodes from the scan - graph.add_nodes_from_scan(robot_pose, &scan_points, &walls); - - // Should have some nodes - assert!(!graph.is_empty(), "Graph should have nodes after scan"); - - // All nodes should have sufficient clearance - for node in graph.nodes() { - assert!( - node.clearance >= 0.18, - "Node clearance {} is below minimum", - node.clearance - ); - } - } - - #[test] - fn test_add_nodes_from_scan_incremental() { - let walls = make_room(); - let config = CBVGConfig::default() - .with_coverage_node_interval(0.5) - .with_min_clearance(0.3) - .with_min_clearance_narrow(0.18) - .with_optimize_interval(100); // High value so optimization doesn't trigger - - let mut graph = ClearanceVisibilityGraph::new(config); - - // First scan from one corner - let pose1 = Pose2D::new(1.0, 1.0, 0.0); - let pos1 = pose1.position(); - let scan1: Vec = (0..36) - .map(|i| { - let angle = i as f32 * std::f32::consts::PI / 18.0; - Point2D::new(pos1.x + 1.0 * angle.cos(), pos1.y + 1.0 * angle.sin()) - }) - .collect(); - - graph.add_nodes_from_scan(pose1, &scan1, &walls); - let count1 = graph.node_count(); - - // Second scan from another corner - let pose2 = Pose2D::new(3.0, 2.0, 0.0); - let pos2 = pose2.position(); - let scan2: Vec = (0..36) - .map(|i| { - let angle = i as f32 * std::f32::consts::PI / 18.0; - Point2D::new(pos2.x + 1.0 * angle.cos(), pos2.y + 1.0 * angle.sin()) - }) - .collect(); - - graph.add_nodes_from_scan(pose2, &scan2, &walls); - let count2 = graph.node_count(); - - // Should have more nodes after second scan - assert!( - count2 >= count1, - "Node count should not decrease: {} -> {}", - count1, - count2 - ); - } - - #[test] - fn test_add_nodes_from_scan_no_duplicates() { - let walls = make_room(); - let config = CBVGConfig::default() - .with_coverage_node_interval(0.5) - .with_min_clearance(0.3) - .with_node_merge_distance(0.15) - .with_optimize_interval(100); - - let mut graph = ClearanceVisibilityGraph::new(config); - - // Same scan position twice - let robot_pose = Pose2D::new(2.0, 1.5, 0.0); - let robot_pos = robot_pose.position(); - let scan_points: Vec = (0..36) - .map(|i| { - let angle = i as f32 * std::f32::consts::PI / 18.0; - Point2D::new( - robot_pos.x + 1.5 * angle.cos(), - robot_pos.y + 1.5 * angle.sin(), - ) - }) - .collect(); - - graph.add_nodes_from_scan(robot_pose, &scan_points, &walls); - let count1 = graph.node_count(); - - // Add same scan again - graph.add_nodes_from_scan(robot_pose, &scan_points, &walls); - let count2 = graph.node_count(); - - // Should not add duplicate nodes - assert_eq!( - count1, count2, - "Adding same scan twice should not add duplicates: {} vs {}", - count1, count2 - ); - } - - #[test] - fn test_add_nodes_from_scan_optimization_trigger() { - let walls = make_room(); - let config = CBVGConfig::default() - .with_coverage_node_interval(0.3) - .with_min_clearance(0.3) - .with_min_clearance_narrow(0.18) - .with_optimize_interval(2) // Low value to trigger optimization - .with_min_clique_size_for_merge(3); - - let mut graph = ClearanceVisibilityGraph::new(config); - - // Add multiple scans to trigger optimization - for i in 0..5 { - let x = 1.0 + i as f32 * 0.5; - let pose = Pose2D::new(x, 1.5, 0.0); - let pos = pose.position(); - let scan: Vec = (0..36) - .map(|j| { - let angle = j as f32 * std::f32::consts::PI / 18.0; - Point2D::new(pos.x + 1.0 * angle.cos(), pos.y + 1.0 * angle.sin()) - }) - .collect(); - - graph.add_nodes_from_scan(pose, &scan, &walls); - } - - // Should have nodes and path finding should work - assert!(!graph.is_empty()); - - let path = graph.find_path(Point2D::new(1.0, 1.0), Point2D::new(3.0, 2.0), &walls); - // Path may or may not exist depending on node placement, but it shouldn't crash - let _ = path; - } -} diff --git a/vastu-map/src/query/cbvg/mod.rs b/vastu-map/src/query/cbvg/mod.rs deleted file mode 100644 index c687a5b..0000000 --- a/vastu-map/src/query/cbvg/mod.rs +++ /dev/null @@ -1,83 +0,0 @@ -//! Clearance-Based Visibility Graph (CBVG) path planning. -//! -//! This module implements a path planning algorithm that places graph nodes -//! along the **medial axis** (centerline) of free space, ensuring the robot -//! navigates through safe, traversable positions rather than wall corners. -//! -//! # Key Features -//! -//! - **Safe waypoints**: All nodes are at minimum distance from obstacles -//! - **Medial axis**: Nodes follow the centerline of corridors -//! - **Adaptive clearance**: Reduced clearance in narrow passages -//! - **Incremental updates**: Graph updates as map grows -//! - **SVG visualization**: Export graph for debugging -//! -//! # Example -//! -//! ```rust,ignore -//! use vastu_map::query::cbvg::{ClearanceVisibilityGraph, CBVGConfig}; -//! use vastu_map::core::Point2D; -//! -//! // Create and configure graph -//! let config = CBVGConfig::default() -//! .with_min_clearance(0.3) -//! .with_min_clearance_narrow(0.18); -//! -//! let mut graph = ClearanceVisibilityGraph::new(config); -//! -//! // Build from wall segments -//! graph.build(&walls, Some(&bounds)); -//! -//! // Find path -//! let start = Point2D::new(1.0, 1.0); -//! let goal = Point2D::new(5.0, 3.0); -//! -//! if let Some(path) = graph.find_path(start, goal, &walls) { -//! println!("Path found: {} waypoints, {:.2}m", path.points.len(), path.length); -//! } -//! -//! // Export for visualization -//! let svg = graph.to_svg(&walls, Some(&path)); -//! std::fs::write("path.svg", svg).unwrap(); -//! ``` -//! -//! # Algorithm Overview -//! -//! 1. **Node generation**: Sample points along the medial axis (Voronoi-based) -//! 2. **Edge construction**: Connect nodes with clearance-checked edges -//! 3. **Path planning**: Dijkstra's algorithm on the graph -//! 4. **Final approach**: Direct navigation from last node to goal -//! -//! # Configuration -//! -//! Key parameters in [`CBVGConfig`]: -//! -//! - `min_clearance`: Minimum distance from walls (default: 0.3m) -//! - `min_clearance_narrow`: Reduced clearance for tight passages (default: 0.18m) -//! - `max_edge_length`: Maximum distance between connected nodes (default: 2.0m) -//! - `voronoi_sample_step`: Density of medial axis sampling (default: 0.1m) - -pub mod clearance; -pub mod config; -pub mod dijkstra; -pub mod graph; -pub mod node; -pub mod visibility; -pub mod voronoi; - -// Re-export main types -pub use clearance::{ - closest_point_on_walls, is_edge_safe, is_path_clear_with_clearance, min_distance_to_walls, - nearest_wall, sample_line, -}; -pub use config::CBVGConfig; -pub use dijkstra::{ - DijkstraResult, DijkstraState, dijkstra, dijkstra_distances, dijkstra_with_limit, -}; -pub use graph::ClearanceVisibilityGraph; -pub use node::{CBVGNode, NodeType}; -pub use visibility::VisibilityRegion; -pub use voronoi::{ - VoronoiEdge, compute_voronoi_edges, generate_grid_nodes, sample_medial_axis_nodes, - sample_wall_points, -}; diff --git a/vastu-map/src/query/cbvg/node.rs b/vastu-map/src/query/cbvg/node.rs deleted file mode 100644 index d685d87..0000000 --- a/vastu-map/src/query/cbvg/node.rs +++ /dev/null @@ -1,167 +0,0 @@ -//! Node types for Clearance-Based Visibility Graph. - -use crate::core::Point2D; - -/// Type of CBVG node for debugging and visualization. -#[derive(Clone, Copy, Debug, PartialEq, Eq)] -pub enum NodeType { - /// Node on Voronoi edge (medial axis of free space). - MedialAxis, - /// Node at Voronoi vertex (corridor junction). - Junction, - /// Temporary node for robot's current position. - Robot, - /// Temporary node for goal position. - Goal, -} - -/// A node in the clearance-based visibility graph. -/// -/// Each node represents a safe waypoint that the robot can travel through, -/// with guaranteed minimum clearance from all obstacles. -#[derive(Clone, Debug)] -pub struct CBVGNode { - /// Position in map coordinates (meters). - pub position: Point2D, - - /// Clearance from nearest wall (meters). - /// This is the distance to the closest obstacle. - pub clearance: f32, - - /// Node type for debugging and visualization. - pub node_type: NodeType, - - /// Whether this node is in a narrow passage. - /// True if clearance < min_clearance but >= min_clearance_narrow. - pub is_narrow_passage: bool, -} - -impl CBVGNode { - /// Create a new CBVG node. - #[inline] - pub fn new(position: Point2D, clearance: f32, node_type: NodeType) -> Self { - Self { - position, - clearance, - node_type, - is_narrow_passage: false, - } - } - - /// Create a medial axis node. - #[inline] - pub fn medial_axis(position: Point2D, clearance: f32, is_narrow: bool) -> Self { - Self { - position, - clearance, - node_type: NodeType::MedialAxis, - is_narrow_passage: is_narrow, - } - } - - /// Create a junction node. - #[inline] - pub fn junction(position: Point2D, clearance: f32) -> Self { - Self { - position, - clearance, - node_type: NodeType::Junction, - is_narrow_passage: false, - } - } - - /// Create a robot position node. - #[inline] - pub fn robot(position: Point2D, clearance: f32) -> Self { - Self { - position, - clearance, - node_type: NodeType::Robot, - is_narrow_passage: false, - } - } - - /// Create a goal position node. - #[inline] - pub fn goal(position: Point2D, clearance: f32) -> Self { - Self { - position, - clearance, - node_type: NodeType::Goal, - is_narrow_passage: false, - } - } - - /// Check if this is a temporary node (robot or goal). - #[inline] - pub fn is_temporary(&self) -> bool { - matches!(self.node_type, NodeType::Robot | NodeType::Goal) - } - - /// Get the color for SVG visualization. - pub fn svg_color(&self) -> &'static str { - match self.node_type { - NodeType::MedialAxis => { - if self.is_narrow_passage { - "orange" - } else { - "blue" - } - } - NodeType::Junction => "green", - NodeType::Robot => "red", - NodeType::Goal => "purple", - } - } -} - -#[cfg(test)] -mod tests { - use super::*; - - #[test] - fn test_node_creation() { - let node = CBVGNode::new(Point2D::new(1.0, 2.0), 0.5, NodeType::MedialAxis); - assert_eq!(node.position.x, 1.0); - assert_eq!(node.position.y, 2.0); - assert_eq!(node.clearance, 0.5); - assert_eq!(node.node_type, NodeType::MedialAxis); - assert!(!node.is_narrow_passage); - } - - #[test] - fn test_medial_axis_node() { - let node = CBVGNode::medial_axis(Point2D::new(0.0, 0.0), 0.2, true); - assert_eq!(node.node_type, NodeType::MedialAxis); - assert!(node.is_narrow_passage); - } - - #[test] - fn test_temporary_nodes() { - let robot = CBVGNode::robot(Point2D::zero(), 0.3); - let goal = CBVGNode::goal(Point2D::zero(), 0.3); - let axis = CBVGNode::medial_axis(Point2D::zero(), 0.3, false); - - assert!(robot.is_temporary()); - assert!(goal.is_temporary()); - assert!(!axis.is_temporary()); - } - - #[test] - fn test_svg_colors() { - assert_eq!( - CBVGNode::medial_axis(Point2D::zero(), 0.3, false).svg_color(), - "blue" - ); - assert_eq!( - CBVGNode::medial_axis(Point2D::zero(), 0.2, true).svg_color(), - "orange" - ); - assert_eq!( - CBVGNode::junction(Point2D::zero(), 0.3).svg_color(), - "green" - ); - assert_eq!(CBVGNode::robot(Point2D::zero(), 0.3).svg_color(), "red"); - assert_eq!(CBVGNode::goal(Point2D::zero(), 0.3).svg_color(), "purple"); - } -} diff --git a/vastu-map/src/query/cbvg/visibility.rs b/vastu-map/src/query/cbvg/visibility.rs deleted file mode 100644 index 0a28202..0000000 --- a/vastu-map/src/query/cbvg/visibility.rs +++ /dev/null @@ -1,597 +0,0 @@ -//! Visibility region tracking for scan-based node generation. -//! -//! This module tracks areas that have been observed by the lidar scanner, -//! ensuring CBVG nodes are only placed in regions the robot has actually seen. -//! -//! # Algorithm -//! -//! For each lidar scan: -//! 1. For each ray from robot position to scan point -//! 2. Mark all grid cells along the ray as visible -//! 3. The endpoint (scan point) marks the boundary -//! -//! This creates a conservative estimate of free space - areas the lidar -//! has actually "seen through". - -use crate::core::{Bounds, Point2D, Pose2D}; - -/// Grid-based visibility tracking. -/// -/// Tracks which areas of the map have been observed by lidar scans. -/// Uses a grid representation for efficient lookup. -#[derive(Clone, Debug)] -pub struct VisibilityRegion { - /// 2D grid of visibility: true = visible, false = unknown. - /// Indexed as grid[y_cell][x_cell]. - grid: Vec>, - - /// 2D grid of processed cells: true = already processed for node generation. - /// Used to track which visible cells have been returned by get_newly_visible_cells(). - processed: Vec>, - - /// Resolution of the visibility grid (meters per cell). - resolution: f32, - - /// Origin of the grid in world coordinates (min corner). - origin: Point2D, - - /// Grid dimensions. - width: usize, - height: usize, - - /// Current bounds of visible area. - bounds: Bounds, -} - -impl VisibilityRegion { - /// Create a new visibility region with the given resolution. - /// - /// # Arguments - /// * `resolution` - Grid cell size in meters (default: 0.1) - pub fn new(resolution: f32) -> Self { - Self { - grid: Vec::new(), - processed: Vec::new(), - resolution, - origin: Point2D::zero(), - width: 0, - height: 0, - bounds: Bounds::empty(), - } - } - - /// Create a visibility region with default resolution (0.1m). - pub fn default_resolution() -> Self { - Self::new(0.1) - } - - /// Get the current bounds of the visible region. - pub fn visible_bounds(&self) -> Bounds { - self.bounds - } - - /// Check if the visibility region is empty (no observations yet). - pub fn is_empty(&self) -> bool { - self.grid.is_empty() - } - - /// Check if a point is in a visible (scanned) area. - /// - /// Returns true if the point has been observed by lidar. - pub fn is_visible(&self, point: Point2D) -> bool { - if self.grid.is_empty() { - return false; - } - - // Convert to grid coordinates - let (x_cell, y_cell) = self.world_to_grid(point); - - // Check bounds - if x_cell < 0 || y_cell < 0 { - return false; - } - let x_cell = x_cell as usize; - let y_cell = y_cell as usize; - - if x_cell >= self.width || y_cell >= self.height { - return false; - } - - self.grid[y_cell][x_cell] - } - - /// Update visibility from a lidar scan. - /// - /// Marks all cells in the scanned area as visible by filling triangular - /// sectors between adjacent lidar rays. This ensures the entire interior - /// of the scanned region is marked visible, not just the rays themselves. - /// - /// # Arguments - /// * `robot_pose` - Current robot pose (position and heading) - /// * `scan_points` - Lidar scan points in world coordinates - pub fn update_from_scan(&mut self, robot_pose: Pose2D, scan_points: &[Point2D]) { - if scan_points.is_empty() { - return; - } - - let robot_pos = robot_pose.position(); - - // First, ensure grid covers all scan points plus robot position - self.ensure_coverage(robot_pos, scan_points); - - // Sort scan points by angle from robot for proper sector filling - let mut sorted_points: Vec = scan_points.to_vec(); - sorted_points.sort_by(|a, b| { - let angle_a = (a.y - robot_pos.y).atan2(a.x - robot_pos.x); - let angle_b = (b.y - robot_pos.y).atan2(b.x - robot_pos.x); - angle_a - .partial_cmp(&angle_b) - .unwrap_or(std::cmp::Ordering::Equal) - }); - - // Fill triangular sectors between adjacent scan points - // This marks the entire interior as visible, not just the rays - for i in 0..sorted_points.len() { - let p1 = sorted_points[i]; - let p2 = sorted_points[(i + 1) % sorted_points.len()]; - - // Check if the angle between rays is small enough to fill - // (avoid filling across large gaps like behind the robot) - let angle1 = (p1.y - robot_pos.y).atan2(p1.x - robot_pos.x); - let angle2 = (p2.y - robot_pos.y).atan2(p2.x - robot_pos.x); - let mut angle_diff = (angle2 - angle1).abs(); - if angle_diff > std::f32::consts::PI { - angle_diff = 2.0 * std::f32::consts::PI - angle_diff; - } - - // Only fill sectors with small angular gaps (< 15 degrees) - if angle_diff < 0.26 { - self.fill_triangle(robot_pos, p1, p2); - } else { - // For larger gaps, just mark the rays - self.mark_ray_visible(robot_pos, p1); - } - } - } - - /// Update visibility from scan points already in robot-local frame. - /// - /// # Arguments - /// * `robot_pose` - Current robot pose for transforming points - /// * `local_points` - Points in robot-local frame - pub fn update_from_local_scan(&mut self, robot_pose: Pose2D, local_points: &[Point2D]) { - if local_points.is_empty() { - return; - } - - // Transform points to world frame - let world_points: Vec = local_points - .iter() - .map(|&p| robot_pose.transform_point(p)) - .collect(); - - self.update_from_scan(robot_pose, &world_points); - } - - /// Ensure the grid covers all given points. - fn ensure_coverage(&mut self, robot_pos: Point2D, scan_points: &[Point2D]) { - // Calculate required bounds - let mut required_bounds = Bounds::from_point(robot_pos); - for &point in scan_points { - required_bounds.expand_to_include(point); - } - - // Add margin (one cell) - required_bounds = required_bounds.expand(self.resolution); - - if self.grid.is_empty() { - // Initialize grid - self.initialize_grid(required_bounds); - } else if !self.bounds_contain(&required_bounds) { - // Expand grid - self.expand_grid(required_bounds); - } - - // Update visible bounds - self.bounds = self.bounds.union(&required_bounds); - } - - /// Check if current grid bounds contain the required bounds. - fn bounds_contain(&self, required: &Bounds) -> bool { - let grid_min = self.origin; - let grid_max = Point2D::new( - self.origin.x + self.width as f32 * self.resolution, - self.origin.y + self.height as f32 * self.resolution, - ); - - required.min.x >= grid_min.x - && required.min.y >= grid_min.y - && required.max.x <= grid_max.x - && required.max.y <= grid_max.y - } - - /// Initialize the grid with the given bounds. - fn initialize_grid(&mut self, bounds: Bounds) { - self.origin = bounds.min; - self.width = ((bounds.width() / self.resolution).ceil() as usize).max(1); - self.height = ((bounds.height() / self.resolution).ceil() as usize).max(1); - - // Create grids filled with false (not visible, not processed) - self.grid = vec![vec![false; self.width]; self.height]; - self.processed = vec![vec![false; self.width]; self.height]; - } - - /// Expand the grid to cover new bounds while preserving existing data. - fn expand_grid(&mut self, new_bounds: Bounds) { - // Calculate new grid bounds (union of current and required) - let current_max = Point2D::new( - self.origin.x + self.width as f32 * self.resolution, - self.origin.y + self.height as f32 * self.resolution, - ); - let current_bounds = Bounds::new(self.origin, current_max); - let expanded = current_bounds.union(&new_bounds); - - let new_origin = expanded.min; - let new_width = ((expanded.width() / self.resolution).ceil() as usize).max(1); - let new_height = ((expanded.height() / self.resolution).ceil() as usize).max(1); - - // Create new grids - let mut new_grid = vec![vec![false; new_width]; new_height]; - let mut new_processed = vec![vec![false; new_width]; new_height]; - - // Copy existing data - let offset_x = ((self.origin.x - new_origin.x) / self.resolution).round() as isize; - let offset_y = ((self.origin.y - new_origin.y) / self.resolution).round() as isize; - - for old_y in 0..self.height { - let new_y = old_y as isize + offset_y; - if new_y >= 0 && (new_y as usize) < new_height { - for old_x in 0..self.width { - let new_x = old_x as isize + offset_x; - if new_x >= 0 && (new_x as usize) < new_width { - let nx = new_x as usize; - let ny = new_y as usize; - if self.grid[old_y][old_x] { - new_grid[ny][nx] = true; - } - if self.processed[old_y][old_x] { - new_processed[ny][nx] = true; - } - } - } - } - } - - // Update state - self.origin = new_origin; - self.width = new_width; - self.height = new_height; - self.grid = new_grid; - self.processed = new_processed; - } - - /// Mark all cells along a ray from start to end as visible. - fn mark_ray_visible(&mut self, start: Point2D, end: Point2D) { - // Use Bresenham-like line drawing - let (x0, y0) = self.world_to_grid(start); - let (x1, y1) = self.world_to_grid(end); - - // Simple DDA line algorithm - let dx = (x1 - x0).abs(); - let dy = (y1 - y0).abs(); - let steps = dx.max(dy); - - if steps == 0 { - self.mark_cell_visible(x0, y0); - return; - } - - let x_step = (x1 - x0) as f32 / steps as f32; - let y_step = (y1 - y0) as f32 / steps as f32; - - let mut x = x0 as f32; - let mut y = y0 as f32; - - for _ in 0..=steps { - self.mark_cell_visible(x.round() as i32, y.round() as i32); - x += x_step; - y += y_step; - } - } - - /// Mark a single cell as visible. - fn mark_cell_visible(&mut self, x_cell: i32, y_cell: i32) { - if x_cell < 0 || y_cell < 0 { - return; - } - let x_cell = x_cell as usize; - let y_cell = y_cell as usize; - - if x_cell < self.width && y_cell < self.height { - self.grid[y_cell][x_cell] = true; - } - } - - /// Fill a triangular region as visible. - /// - /// Uses a scanline algorithm to fill the triangle formed by three points. - fn fill_triangle(&mut self, p0: Point2D, p1: Point2D, p2: Point2D) { - // Convert to grid coordinates - let (x0, y0) = self.world_to_grid(p0); - let (x1, y1) = self.world_to_grid(p1); - let (x2, y2) = self.world_to_grid(p2); - - // Find bounding box - let min_y = y0.min(y1).min(y2).max(0); - let max_y = y0.max(y1).max(y2).min(self.height as i32 - 1); - let min_x = x0.min(x1).min(x2).max(0); - let max_x = x0.max(x1).max(x2).min(self.width as i32 - 1); - - // Convert to f32 for edge calculations - let (x0, y0) = (x0 as f32, y0 as f32); - let (x1, y1) = (x1 as f32, y1 as f32); - let (x2, y2) = (x2 as f32, y2 as f32); - - // Use barycentric coordinates to check if points are inside triangle - let area = 0.5 * (-y1 * x2 + y0 * (-x1 + x2) + x0 * (y1 - y2) + x1 * y2); - if area.abs() < 0.001 { - // Degenerate triangle, just mark the edges - self.mark_ray_visible(p0, p1); - self.mark_ray_visible(p0, p2); - return; - } - - let sign = if area < 0.0 { -1.0 } else { 1.0 }; - - // Scanline fill - for py in min_y..=max_y { - for px in min_x..=max_x { - let px_f = px as f32; - let py_f = py as f32; - - // Calculate barycentric coordinates - let s = sign * (y0 * x2 - x0 * y2 + (y2 - y0) * px_f + (x0 - x2) * py_f); - let t = sign * (x0 * y1 - y0 * x1 + (y0 - y1) * px_f + (x1 - x0) * py_f); - - if s >= 0.0 && t >= 0.0 && (s + t) <= 2.0 * area.abs() * sign { - self.mark_cell_visible(px, py); - } - } - } - } - - /// Convert world coordinates to grid cell indices. - fn world_to_grid(&self, point: Point2D) -> (i32, i32) { - let x = ((point.x - self.origin.x) / self.resolution).floor() as i32; - let y = ((point.y - self.origin.y) / self.resolution).floor() as i32; - (x, y) - } - - /// Get the grid resolution in meters. - pub fn resolution(&self) -> f32 { - self.resolution - } - - /// Get grid dimensions (width, height) in cells. - pub fn grid_size(&self) -> (usize, usize) { - (self.width, self.height) - } - - /// Count the number of visible cells. - pub fn visible_cell_count(&self) -> usize { - self.grid - .iter() - .map(|row| row.iter().filter(|&&v| v).count()) - .sum() - } - - /// Get total cell count. - pub fn total_cell_count(&self) -> usize { - self.width * self.height - } - - /// Get the visibility ratio (visible cells / total cells). - pub fn visibility_ratio(&self) -> f32 { - if self.total_cell_count() == 0 { - return 0.0; - } - self.visible_cell_count() as f32 / self.total_cell_count() as f32 - } - - /// Get newly visible cells that haven't been processed yet. - /// - /// Returns the center points of cells that are visible but not yet processed, - /// and marks them as processed. This is used for incremental node generation - /// in the CBVG graph. - /// - /// # Returns - /// Vector of cell center points in world coordinates. - pub fn get_newly_visible_cells(&mut self) -> Vec { - let mut new_cells = Vec::new(); - - for y in 0..self.height { - for x in 0..self.width { - if self.grid[y][x] && !self.processed[y][x] { - new_cells.push(self.grid_to_world_center(x, y)); - self.processed[y][x] = true; - } - } - } - - new_cells - } - - /// Reset the processed state for all cells. - /// - /// This allows regenerating nodes in all visible areas. - pub fn reset_processed(&mut self) { - for row in &mut self.processed { - for cell in row { - *cell = false; - } - } - } - - /// Iterator over all visible cell center points. - pub fn visible_points(&self) -> impl Iterator + '_ { - self.grid.iter().enumerate().flat_map(move |(y, row)| { - row.iter() - .enumerate() - .filter(|&(_, visible)| *visible) - .map(move |(x, _)| self.grid_to_world_center(x, y)) - }) - } - - /// Convert grid cell to world coordinates (cell center). - fn grid_to_world_center(&self, x_cell: usize, y_cell: usize) -> Point2D { - Point2D::new( - self.origin.x + (x_cell as f32 + 0.5) * self.resolution, - self.origin.y + (y_cell as f32 + 0.5) * self.resolution, - ) - } -} - -impl Default for VisibilityRegion { - fn default() -> Self { - Self::default_resolution() - } -} - -#[cfg(test)] -mod tests { - use super::*; - use std::f32::consts::PI; - - #[test] - fn test_new_visibility_region() { - let vis = VisibilityRegion::new(0.1); - assert!(vis.is_empty()); - assert_eq!(vis.resolution(), 0.1); - assert!(vis.visible_bounds().is_empty()); - } - - #[test] - fn test_empty_scan() { - let mut vis = VisibilityRegion::new(0.1); - let pose = Pose2D::identity(); - vis.update_from_scan(pose, &[]); - assert!(vis.is_empty()); - } - - #[test] - fn test_single_point_scan() { - let mut vis = VisibilityRegion::new(0.1); - let pose = Pose2D::identity(); - let points = vec![Point2D::new(1.0, 0.0)]; - - vis.update_from_scan(pose, &points); - - // Robot position should be visible - assert!(vis.is_visible(Point2D::new(0.0, 0.0))); - // Point along ray should be visible - assert!(vis.is_visible(Point2D::new(0.5, 0.0))); - // Scan point should be visible - assert!(vis.is_visible(Point2D::new(1.0, 0.0))); - // Point beyond scan should not be visible - assert!(!vis.is_visible(Point2D::new(2.0, 0.0))); - // Point off to the side should not be visible - assert!(!vis.is_visible(Point2D::new(0.5, 1.0))); - } - - #[test] - fn test_circular_scan() { - let mut vis = VisibilityRegion::new(0.1); - let pose = Pose2D::identity(); - - // Create a circular scan (8 points at 1m radius) - let points: Vec = (0..8) - .map(|i| { - let angle = i as f32 * PI / 4.0; - Point2D::from_polar(angle, 1.0) - }) - .collect(); - - vis.update_from_scan(pose, &points); - - // Center should be visible - assert!(vis.is_visible(Point2D::zero())); - // Points halfway should be visible - assert!(vis.is_visible(Point2D::new(0.5, 0.0))); - assert!(vis.is_visible(Point2D::new(0.0, 0.5))); - } - - #[test] - fn test_incremental_scans() { - let mut vis = VisibilityRegion::new(0.1); - - // First scan from origin - let pose1 = Pose2D::identity(); - let points1 = vec![Point2D::new(1.0, 0.0)]; - vis.update_from_scan(pose1, &points1); - - // Second scan from different position - let pose2 = Pose2D::new(1.0, 0.0, PI / 2.0); - let points2 = vec![Point2D::new(1.0, 1.0)]; - vis.update_from_scan(pose2, &points2); - - // Both scan areas should be visible - assert!(vis.is_visible(Point2D::new(0.5, 0.0))); - assert!(vis.is_visible(Point2D::new(1.0, 0.5))); - } - - #[test] - fn test_visibility_bounds() { - let mut vis = VisibilityRegion::new(0.1); - let pose = Pose2D::identity(); - let points = vec![ - Point2D::new(1.0, 0.0), - Point2D::new(-1.0, 0.0), - Point2D::new(0.0, 1.0), - Point2D::new(0.0, -1.0), - ]; - - vis.update_from_scan(pose, &points); - - let bounds = vis.visible_bounds(); - assert!(!bounds.is_empty()); - assert!(bounds.contains(Point2D::zero())); - } - - #[test] - fn test_grid_expansion() { - let mut vis = VisibilityRegion::new(0.1); - - // First small scan - let pose1 = Pose2D::identity(); - let points1 = vec![Point2D::new(0.5, 0.0)]; - vis.update_from_scan(pose1, &points1); - let initial_size = vis.grid_size(); - - // Larger scan requiring expansion - let pose2 = Pose2D::identity(); - let points2 = vec![Point2D::new(5.0, 5.0)]; - vis.update_from_scan(pose2, &points2); - let expanded_size = vis.grid_size(); - - // Grid should have grown - assert!(expanded_size.0 > initial_size.0); - assert!(expanded_size.1 > initial_size.1); - - // Original visibility should be preserved - assert!(vis.is_visible(Point2D::new(0.25, 0.0))); - } - - #[test] - fn test_visibility_statistics() { - let mut vis = VisibilityRegion::new(0.1); - let pose = Pose2D::identity(); - let points = vec![Point2D::new(1.0, 0.0)]; - - vis.update_from_scan(pose, &points); - - assert!(vis.visible_cell_count() > 0); - assert!(vis.total_cell_count() >= vis.visible_cell_count()); - let ratio = vis.visibility_ratio(); - assert!(ratio > 0.0 && ratio <= 1.0); - } -} diff --git a/vastu-map/src/query/cbvg/voronoi.rs b/vastu-map/src/query/cbvg/voronoi.rs deleted file mode 100644 index 11eeacd..0000000 --- a/vastu-map/src/query/cbvg/voronoi.rs +++ /dev/null @@ -1,467 +0,0 @@ -//! Voronoi diagram approximation for medial axis computation. -//! -//! Uses point sampling along walls and computes Voronoi diagram -//! to approximate the medial axis of free space. - -use crate::core::{Bounds, Point2D}; -use crate::features::Line2D; - -use super::clearance::{min_distance_to_walls, sample_line}; -use super::config::CBVGConfig; -use super::node::CBVGNode; - -/// A Voronoi edge connecting two points in free space. -#[derive(Clone, Debug)] -pub struct VoronoiEdge { - /// Start point of the edge. - pub start: Point2D, - /// End point of the edge. - pub end: Point2D, - /// Indices of the two generating wall points. - pub generators: (usize, usize), -} - -impl VoronoiEdge { - /// Create a new Voronoi edge. - pub fn new(start: Point2D, end: Point2D, gen1: usize, gen2: usize) -> Self { - Self { - start, - end, - generators: (gen1, gen2), - } - } - - /// Get the length of this edge. - #[inline] - pub fn length(&self) -> f32 { - self.start.distance(self.end) - } - - /// Get a point along the edge at parameter t (0=start, 1=end). - #[inline] - pub fn point_at(&self, t: f32) -> Point2D { - self.start.lerp(self.end, t) - } -} - -/// Sample points along all wall segments. -/// -/// # Arguments -/// * `lines` - Wall segments -/// * `sample_step` - Distance between samples -/// -/// # Returns -/// Vector of sampled wall points. -pub fn sample_wall_points(lines: &[Line2D], sample_step: f32) -> Vec { - lines - .iter() - .flat_map(|line| sample_line(line, sample_step)) - .collect() -} - -/// Compute approximate Voronoi edges using distance field approach. -/// -/// This is a simplified implementation that: -/// 1. Samples points along walls -/// 2. Finds points equidistant from two walls (approximate Voronoi vertices) -/// 3. Connects them to form Voronoi edges -/// -/// # Arguments -/// * `lines` - Wall segments -/// * `bounds` - Map bounds for grid sampling -/// * `config` - CBVG configuration -/// -/// # Returns -/// Vector of Voronoi edges in free space. -pub fn compute_voronoi_edges( - lines: &[Line2D], - bounds: Option<&Bounds>, - config: &CBVGConfig, -) -> Vec { - if lines.is_empty() { - return Vec::new(); - } - - // Get bounds for sampling - let bounds = match bounds { - Some(b) => *b, - None => compute_bounds_from_lines(lines), - }; - - // Expand bounds slightly to ensure coverage - let margin = config.min_clearance * 2.0; - let expanded_bounds = Bounds { - min: Point2D::new(bounds.min.x - margin, bounds.min.y - margin), - max: Point2D::new(bounds.max.x + margin, bounds.max.y + margin), - }; - - // Use grid-based approach to find medial axis points - let step = config.voronoi_sample_step; - let mut medial_points: Vec = Vec::new(); - - let x_steps = ((expanded_bounds.max.x - expanded_bounds.min.x) / step).ceil() as usize; - let y_steps = ((expanded_bounds.max.y - expanded_bounds.min.y) / step).ceil() as usize; - - for ix in 0..=x_steps { - for iy in 0..=y_steps { - let x = expanded_bounds.min.x + ix as f32 * step; - let y = expanded_bounds.min.y + iy as f32 * step; - let point = Point2D::new(x, y); - - // Check if this point is on the medial axis - if is_near_medial_axis(point, lines, config) { - let clearance = min_distance_to_walls(point, lines); - if clearance >= config.min_clearance_narrow { - medial_points.push(point); - } - } - } - } - - // Connect nearby medial points to form edges - connect_medial_points(&medial_points, lines, config) -} - -/// Check if a point is near the medial axis (equidistant from multiple walls). -fn is_near_medial_axis(point: Point2D, lines: &[Line2D], config: &CBVGConfig) -> bool { - if lines.len() < 2 { - return false; - } - - let mut distances: Vec<(f32, usize)> = lines - .iter() - .enumerate() - .map(|(idx, line)| (line.distance_to_point_segment(point), idx)) - .collect(); - - distances.sort_by(|a, b| a.0.partial_cmp(&b.0).unwrap_or(std::cmp::Ordering::Equal)); - - if distances.len() < 2 { - return false; - } - - let d1 = distances[0].0; - let d2 = distances[1].0; - - if d1 < config.min_clearance_narrow { - return false; - } - - let tolerance = config.voronoi_sample_step * 1.5; - let diff = (d1 - d2).abs(); - diff < tolerance -} - -/// Connect medial axis points to form edges. -fn connect_medial_points( - points: &[Point2D], - lines: &[Line2D], - config: &CBVGConfig, -) -> Vec { - let mut edges = Vec::new(); - let max_connect_dist = config.voronoi_sample_step * 2.0; - - for i in 0..points.len() { - for j in (i + 1)..points.len() { - let dist = points[i].distance(points[j]); - - // Only connect nearby points - if dist <= max_connect_dist { - // Verify the connection is valid (no walls in between) - let midpoint = points[i].lerp(points[j], 0.5); - let mid_clearance = min_distance_to_walls(midpoint, lines); - - if mid_clearance >= config.min_clearance_narrow { - edges.push(VoronoiEdge::new(points[i], points[j], i, j)); - } - } - } - } - - edges -} - -/// Sample nodes along Voronoi edges (medial axis). -/// -/// # Arguments -/// * `edges` - Voronoi edges -/// * `lines` - Wall segments (for clearance computation) -/// * `config` - CBVG configuration -/// -/// # Returns -/// Vector of CBVG nodes sampled along the medial axis. -pub fn sample_medial_axis_nodes( - edges: &[VoronoiEdge], - lines: &[Line2D], - config: &CBVGConfig, -) -> Vec { - let mut nodes = Vec::new(); - let mut seen_positions: Vec = Vec::new(); - - for edge in edges { - let edge_length = edge.length(); - let num_samples = (edge_length / config.voronoi_sample_step).ceil() as usize; - let num_samples = num_samples.max(1); - - for i in 0..=num_samples { - let t = i as f32 / num_samples as f32; - let position = edge.point_at(t); - - // Skip if too close to existing node - let is_duplicate = seen_positions - .iter() - .any(|p| p.distance(position) < config.node_merge_distance); - - if is_duplicate { - continue; - } - - let clearance = min_distance_to_walls(position, lines); - - // Skip if insufficient clearance - if clearance < config.min_clearance_narrow { - continue; - } - - let is_narrow = clearance < config.min_clearance; - - nodes.push(CBVGNode::medial_axis(position, clearance, is_narrow)); - seen_positions.push(position); - - // Limit total nodes - if nodes.len() >= config.max_nodes { - return nodes; - } - } - } - - nodes -} - -/// Generate nodes on a uniform grid with clearance checks. -/// -/// This replaces medial axis generation with simple grid sampling: -/// 1. Sample points on a uniform grid (coverage_node_interval spacing) -/// 2. Keep only points with sufficient clearance from walls -/// 3. Apply deterministic jitter for better distribution -/// -/// The graph's optimize_nodes() should be called after to reduce density -/// by merging visibility cliques into centroids. -/// -/// # Arguments -/// * `lines` - Wall segments -/// * `bounds` - Map bounds -/// * `config` - CBVG configuration -/// -/// # Returns -/// Vector of CBVG nodes uniformly distributed in free space. -pub fn generate_grid_nodes( - lines: &[Line2D], - bounds: Option<&Bounds>, - config: &CBVGConfig, -) -> Vec { - if lines.is_empty() { - return Vec::new(); - } - - let bounds = match bounds { - Some(b) => *b, - None => compute_bounds_from_lines(lines), - }; - - // Expand bounds slightly to ensure edge coverage - let margin = config.min_clearance; - let expanded_bounds = Bounds { - min: Point2D::new(bounds.min.x - margin, bounds.min.y - margin), - max: Point2D::new(bounds.max.x + margin, bounds.max.y + margin), - }; - - let step = config.coverage_node_interval; - let jitter_range = step * 0.3; // ±30% jitter for better distribution - let mut nodes: Vec = Vec::new(); - - let x_steps = ((expanded_bounds.max.x - expanded_bounds.min.x) / step).ceil() as usize; - let y_steps = ((expanded_bounds.max.y - expanded_bounds.min.y) / step).ceil() as usize; - - for ix in 0..=x_steps { - for iy in 0..=y_steps { - // Base grid position with deterministic jitter - let base_x = expanded_bounds.min.x + ix as f32 * step; - let base_y = expanded_bounds.min.y + iy as f32 * step; - let (jitter_x, jitter_y) = deterministic_jitter(ix, iy, jitter_range); - let point = Point2D::new(base_x + jitter_x, base_y + jitter_y); - - // Check clearance from walls - let clearance = min_distance_to_walls(point, lines); - if clearance < config.min_clearance_narrow { - continue; - } - - // Skip duplicates - let is_duplicate = nodes - .iter() - .any(|n| n.position.distance(point) < config.node_merge_distance); - - if is_duplicate { - continue; - } - - let is_narrow = clearance < config.min_clearance; - nodes.push(CBVGNode::medial_axis(point, clearance, is_narrow)); - - if nodes.len() >= config.max_nodes { - return nodes; - } - } - } - - nodes -} - -/// Compute bounds from line segments. -fn compute_bounds_from_lines(lines: &[Line2D]) -> Bounds { - if lines.is_empty() { - return Bounds::empty(); - } - - let mut min = Point2D::new(f32::INFINITY, f32::INFINITY); - let mut max = Point2D::new(f32::NEG_INFINITY, f32::NEG_INFINITY); - - for line in lines { - min = min.min(line.start).min(line.end); - max = max.max(line.start).max(line.end); - } - - Bounds { min, max } -} - -/// Generate deterministic jitter for a grid cell. -/// -/// Uses a simple hash function to create pseudo-random but reproducible -/// offsets for each grid cell. This ensures the same grid always produces -/// the same node positions. -/// -/// # Arguments -/// * `ix` - Grid x index -/// * `iy` - Grid y index -/// * `range` - Maximum jitter magnitude (±range) -/// -/// # Returns -/// Tuple of (x_offset, y_offset) in range [-range, +range] -pub fn deterministic_jitter(ix: usize, iy: usize, range: f32) -> (f32, f32) { - // Simple hash combining grid indices - // Using prime multipliers for better distribution - let hash1 = ((ix.wrapping_mul(73856093)) ^ (iy.wrapping_mul(19349663))) as u32; - let hash2 = ((ix.wrapping_mul(83492791)) ^ (iy.wrapping_mul(41729867))) as u32; - - // Convert to [-1, 1] range using the hash bits - let norm1 = (hash1 % 10000) as f32 / 5000.0 - 1.0; - let norm2 = (hash2 % 10000) as f32 / 5000.0 - 1.0; - - (norm1 * range, norm2 * range) -} - -#[cfg(test)] -mod tests { - use super::*; - - fn make_corridor() -> Vec { - // Simple corridor: two parallel walls - vec![ - Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(5.0, 0.0)), // Bottom wall - Line2D::new(Point2D::new(0.0, 1.0), Point2D::new(5.0, 1.0)), // Top wall - ] - } - - fn make_room() -> Vec { - // Simple room: 4 walls - vec![ - Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(4.0, 0.0)), // Bottom - Line2D::new(Point2D::new(4.0, 0.0), Point2D::new(4.0, 3.0)), // Right - Line2D::new(Point2D::new(4.0, 3.0), Point2D::new(0.0, 3.0)), // Top - Line2D::new(Point2D::new(0.0, 3.0), Point2D::new(0.0, 0.0)), // Left - ] - } - - #[test] - fn test_deterministic_jitter() { - let range = 0.15; // ±0.15m jitter - - // Test that jitter is deterministic (same input = same output) - let (j1x, j1y) = deterministic_jitter(5, 10, range); - let (j2x, j2y) = deterministic_jitter(5, 10, range); - assert_eq!(j1x, j2x); - assert_eq!(j1y, j2y); - - // Test that different cells get different jitter - let (j3x, j3y) = deterministic_jitter(5, 11, range); - assert!( - j1x != j3x || j1y != j3y, - "Adjacent cells should have different jitter" - ); - - // Test that jitter is within bounds - for ix in 0..20 { - for iy in 0..20 { - let (jx, jy) = deterministic_jitter(ix, iy, range); - assert!(jx.abs() <= range, "X jitter {} exceeds range {}", jx, range); - assert!(jy.abs() <= range, "Y jitter {} exceeds range {}", jy, range); - } - } - } - - #[test] - fn test_sample_wall_points() { - let walls = make_corridor(); - let points = sample_wall_points(&walls, 1.0); - - // Should have points from both walls - assert!(!points.is_empty()); - assert!(points.len() >= 10); // At least 5 per wall - } - - #[test] - fn test_generate_grid_nodes() { - let walls = make_room(); - let config = CBVGConfig::default() - .with_coverage_node_interval(0.3) - .with_min_clearance(0.3) - .with_min_clearance_narrow(0.18); - - let nodes = generate_grid_nodes(&walls, None, &config); - - // Should have nodes in the room - assert!(!nodes.is_empty()); - - // All nodes should have sufficient clearance - for node in &nodes { - assert!(node.clearance >= config.min_clearance_narrow); - } - - // Should have nodes near the center of the room - let center = Point2D::new(2.0, 1.5); - let near_center: usize = nodes - .iter() - .filter(|n| n.position.distance(center) < 1.5) - .count(); - assert!(near_center > 0); - } - - #[test] - fn test_compute_bounds_from_lines() { - let walls = make_room(); - let bounds = compute_bounds_from_lines(&walls); - - assert!((bounds.min.x - 0.0).abs() < 0.01); - assert!((bounds.min.y - 0.0).abs() < 0.01); - assert!((bounds.max.x - 4.0).abs() < 0.01); - assert!((bounds.max.y - 3.0).abs() < 0.01); - } - - #[test] - fn test_empty_walls() { - let config = CBVGConfig::default(); - let nodes = generate_grid_nodes(&[], None, &config); - assert!(nodes.is_empty()); - } -} diff --git a/vastu-map/src/query/frontier.rs b/vastu-map/src/query/frontier.rs index 1ce3858..56cdb38 100644 --- a/vastu-map/src/query/frontier.rs +++ b/vastu-map/src/query/frontier.rs @@ -1,1018 +1,781 @@ -//! Gap-based frontier detection for exploration. +//! Frontier detection for exploration. //! -//! Frontiers represent **gaps between unconnected wall endpoints**. The robot -//! navigates to a viewpoint (a reachable position) from which it can observe -//! the unexplored area through the gap. -//! -//! # Key Concept -//! -//! ```text -//! Wall A ─────┐ ┌───── Wall B -//! │ │ -//! endpoint1 endpoint2 -//! │ GAP │ -//! └──────────────┘ -//! ↑ -//! Frontier is HERE -//! (midpoint of gap) -//! -//! Viewpoint = nearest reachable position that can see the gap -//! ``` -//! -//! # Algorithm -//! -//! 1. Find all **unconnected endpoints** (wall ends not connected to other walls) -//! 2. Find **pairs of endpoints** that form a gap: -//! - Close enough to each other (within max_gap_width) -//! - Wide enough for robot to pass (min_gap_width) -//! - No wall blocking between them -//! 3. For each gap, find a **viewpoint**: -//! - Must be in known free space -//! - Must have line-of-sight to the gap -//! - Prefer positions closer to the robot -//! 4. Also handle **single endpoints** (walls ending in open space) -//! -//! # Zero-Allocation Detection -//! -//! For repeated frontier detection (e.g., during SLAM), use `FrontierDetector` -//! which maintains persistent buffers to avoid per-call allocations: -//! -//! ```rust,ignore -//! use vastu_map::query::frontier::{FrontierDetector, FrontierConfig}; -//! -//! let mut detector = FrontierDetector::new(); -//! let config = FrontierConfig::default(); -//! -//! // First detection - allocates buffers -//! let frontiers = detector.detect(&lines, robot_pos, &config); -//! -//! // Subsequent calls reuse buffers (zero allocation) -//! let frontiers = detector.detect(&new_lines, robot_pos, &config); -//! ``` +//! A frontier is a boundary between known (Floor) and unknown cells. +//! This module detects frontiers and clusters them for exploration targeting. -use serde::{Deserialize, Serialize}; -use std::collections::HashMap; +use crate::core::{CellType, GridCoord, WorldPoint}; +use crate::grid::GridStorage; +use log::{debug, trace}; +use std::collections::{HashSet, VecDeque}; +use std::simd::{cmp::SimdPartialEq, u8x16}; -use crate::Frontier; -use crate::core::Point2D; -use crate::features::Line2D; - -/// Spatial hash grid for endpoint proximity queries. -/// Maps (cell_x, cell_y) -> Vec<(point, line_index, is_start_endpoint)> -type EndpointGrid = HashMap<(i32, i32), Vec<(Point2D, usize, bool)>>; - -/// An unconnected endpoint with metadata. -#[derive(Clone, Debug)] -struct UnconnectedEndpoint { - /// The endpoint position. - point: Point2D, - /// Index of the line this endpoint belongs to. - line_idx: usize, - /// Outward direction (away from the line). - outward_dir: Point2D, +/// A single frontier cell +#[derive(Clone, Copy, Debug, PartialEq, Eq, Hash)] +pub struct FrontierCell { + /// Grid coordinate of the frontier cell + pub coord: GridCoord, + /// Number of adjacent unknown cells (1-8) + pub unknown_neighbors: u8, } -/// A detected gap between two unconnected endpoints. -#[derive(Clone, Debug)] -struct Gap { - /// Center of the gap. - center: Point2D, - /// Direction from endpoint1 to endpoint2 (normalized). - direction: Point2D, - /// Width of the gap. - width: f32, +impl FrontierCell { + /// Create a new frontier cell + pub fn new(coord: GridCoord, unknown_neighbors: u8) -> Self { + Self { + coord, + unknown_neighbors, + } + } } -/// Persistent frontier detector with reusable buffers. -/// -/// Uses gap-based detection: frontiers are gaps between unconnected -/// wall endpoints. The viewpoint is found by looking for reachable -/// positions with line-of-sight to the gap. -#[derive(Clone, Debug, Default)] +/// A cluster of connected frontier cells +#[derive(Clone, Debug)] +pub struct Frontier { + /// All cells in this frontier + pub cells: Vec, + /// Centroid in grid coordinates + pub centroid_grid: GridCoord, + /// Centroid in world coordinates + pub centroid_world: WorldPoint, + /// Approximate size (number of cells) + pub size: usize, + /// Sum of unknown neighbors (exploration potential) + pub unknown_exposure: usize, +} + +impl Frontier { + /// Create a new frontier from a set of cells + pub fn from_cells(cells: Vec, storage: &GridStorage) -> Self { + let size = cells.len(); + + // Calculate centroid + let sum_x: i32 = cells.iter().map(|c| c.coord.x).sum(); + let sum_y: i32 = cells.iter().map(|c| c.coord.y).sum(); + let centroid_grid = GridCoord::new(sum_x / size as i32, sum_y / size as i32); + let centroid_world = storage.grid_to_world(centroid_grid); + + // Calculate total unknown exposure + let unknown_exposure: usize = cells.iter().map(|c| c.unknown_neighbors as usize).sum(); + + Self { + cells, + centroid_grid, + centroid_world, + size, + unknown_exposure, + } + } + + /// Exploration score (higher = more valuable to explore) + /// Considers both size and unknown exposure + pub fn exploration_score(&self) -> f32 { + // Balance between size and exposure + // Larger frontiers with more unknown cells are more valuable + self.size as f32 * 0.5 + self.unknown_exposure as f32 * 0.5 + } + + /// Get the closest cell in this frontier to a given position + pub fn closest_cell_to(&self, target: GridCoord) -> Option<&FrontierCell> { + self.cells + .iter() + .min_by_key(|c| c.coord.chebyshev_distance(&target)) + } + + /// Length of the frontier in cells (approximate) + pub fn length(&self) -> usize { + self.size + } +} + +/// Frontier detector pub struct FrontierDetector { - /// Reusable spatial hash grid. - endpoint_grid: EndpointGrid, + /// Minimum frontier size (smaller clusters are filtered out) + pub min_frontier_size: usize, + /// Use 8-connectivity for frontier detection (vs 4-connectivity) + pub use_8_connectivity: bool, +} - /// Reusable output buffer for detected frontiers. - frontiers: Vec, - - /// Reusable buffer for ranking frontiers by value. - ranked_frontiers: Vec<(Frontier, f32)>, - - /// Cells that were used in the last detection (for efficient clearing). - used_cells: Vec<(i32, i32)>, - - /// Reusable buffer for unconnected endpoints. - unconnected: Vec, - - /// Reusable buffer for detected gaps. - gaps: Vec, - - /// Reusable buffer for visibility graph nodes. - graph_nodes: Vec, +impl Default for FrontierDetector { + fn default() -> Self { + Self { + min_frontier_size: 3, + use_8_connectivity: true, + } + } } impl FrontierDetector { - /// Create a new frontier detector with default capacity. - pub fn new() -> Self { - Self::with_capacity(100, 50) - } - - /// Create a frontier detector with capacity hints. - pub fn with_capacity(expected_lines: usize, expected_frontiers: usize) -> Self { - Self { - endpoint_grid: HashMap::with_capacity(expected_lines * 2), - frontiers: Vec::with_capacity(expected_frontiers), - ranked_frontiers: Vec::with_capacity(expected_frontiers), - used_cells: Vec::with_capacity(expected_lines * 2), - unconnected: Vec::with_capacity(expected_lines * 2), - gaps: Vec::with_capacity(expected_frontiers), - graph_nodes: Vec::with_capacity(expected_lines * 2 + 2), - } - } - - /// Clear grid by clearing each used cell's vector (retains capacity). - #[inline] - fn clear_grid(&mut self) { - for cell in &self.used_cells { - if let Some(vec) = self.endpoint_grid.get_mut(cell) { - vec.clear(); - } - } - self.used_cells.clear(); - } - - /// Insert an endpoint into the grid. - #[inline] - fn insert_endpoint(&mut self, point: Point2D, line_idx: usize, is_start: bool, cell_size: f32) { - let cell = point_to_cell(point, cell_size); - - if let std::collections::hash_map::Entry::Vacant(e) = self.endpoint_grid.entry(cell) { - e.insert(Vec::with_capacity(4)); - self.used_cells.push(cell); - } else if self.endpoint_grid.get(&cell).is_none_or(|v| v.is_empty()) { - self.used_cells.push(cell); - } - - self.endpoint_grid - .get_mut(&cell) - .unwrap() - .push((point, line_idx, is_start)); - } - - /// Detect frontiers from map lines. - /// - /// Returns viewpoints that can observe unexplored gaps between lines. - /// Uses gap-based detection: finds pairs of unconnected endpoints that - /// form passable gaps, then finds reachable viewpoints for each gap. - pub fn detect( - &mut self, - lines: &[Line2D], - robot_position: Point2D, - config: &FrontierConfig, - ) -> &[Frontier] { - self.frontiers.clear(); - self.unconnected.clear(); - self.gaps.clear(); - - if lines.is_empty() { - return &self.frontiers; - } - - let cell_size = config.connection_distance; - - // Step 1: Build spatial hash grid and find unconnected endpoints - self.clear_grid(); - - for (idx, line) in lines.iter().enumerate() { - for (point, is_start) in [(line.start, true), (line.end, false)] { - self.insert_endpoint(point, idx, is_start, cell_size); - } - } - - // Find unconnected endpoints with their outward directions - for (idx, line) in lines.iter().enumerate() { - for (point, is_start) in [(line.start, true), (line.end, false)] { - let cell = point_to_cell(point, cell_size); - if !is_connected_in_grid( - &self.endpoint_grid, - cell, - point, - idx, - config.connection_distance, - ) { - // Compute outward direction (away from the line) - let outward_dir = if is_start { - -line.unit_direction() - } else { - line.unit_direction() - }; - self.unconnected.push(UnconnectedEndpoint { - point, - line_idx: idx, - outward_dir, - }); - } - } - } - - // Step 2: Find pairs of unconnected endpoints that form gaps - let mut used_endpoints = vec![false; self.unconnected.len()]; - - for i in 0..self.unconnected.len() { - if used_endpoints[i] { - continue; - } - - let ep1 = &self.unconnected[i]; - - // Find the closest unconnected endpoint that forms a valid gap - let mut best_pair: Option<(usize, f32)> = None; - - for (j, (used, ep2)) in used_endpoints - .iter() - .zip(self.unconnected.iter()) - .enumerate() - .skip(i + 1) - { - if *used { - continue; - } - let distance = ep1.point.distance(ep2.point); - - // Check if this pair forms a valid gap - if distance < config.min_gap_width || distance > config.max_gap_width { - continue; - } - - // Check that there's no wall blocking between them - if is_path_blocked(ep1.point, ep2.point, lines) { - continue; - } - - // Check if endpoints are facing each other (outward dirs point toward each other) - let to_ep2 = (ep2.point - ep1.point).normalized(); - let to_ep1 = (ep1.point - ep2.point).normalized(); - let facing1 = ep1.outward_dir.dot(to_ep2); - let facing2 = ep2.outward_dir.dot(to_ep1); - - // At least one endpoint should be facing the other - if facing1 <= -0.5 && facing2 <= -0.5 { - continue; - } - - if best_pair.is_none() || distance < best_pair.unwrap().1 { - best_pair = Some((j, distance)); - } - } - - if let Some((j, width)) = best_pair { - let ep2 = &self.unconnected[j]; - let center = Point2D::new( - (ep1.point.x + ep2.point.x) / 2.0, - (ep1.point.y + ep2.point.y) / 2.0, - ); - let direction = (ep2.point - ep1.point).normalized(); - - self.gaps.push(Gap { - center, - direction, - width, - }); - - used_endpoints[i] = true; - used_endpoints[j] = true; - } - } - - // Step 3: Build visibility graph nodes (all line endpoints + robot position) - self.graph_nodes.clear(); - for line in lines { - self.graph_nodes.push(line.start); - self.graph_nodes.push(line.end); - } - self.graph_nodes.push(robot_position); - - // Step 4: For each gap, find the nearest reachable graph node as target - for gap in &self.gaps.clone() { - if let Some(target) = find_nearest_reachable_node( - gap.center, - &self.graph_nodes, - robot_position, - lines, - config, - ) && target.distance(robot_position) >= config.min_distance_from_robot - { - // Look direction is perpendicular to gap direction - let look_direction = Point2D::new(-gap.direction.y, gap.direction.x); - self.frontiers.push(Frontier { - viewpoint: target, - look_direction, - endpoint: gap.center, - line_idx: 0, - estimated_area: gap.width * config.gap_probe_distance, - }); - } - } - - // Step 5: Handle single unconnected endpoints (no pair found) - for (i, ep) in self.unconnected.iter().enumerate() { - if used_endpoints[i] { - continue; // Already part of a gap - } - - // Find nearest reachable node toward this endpoint - if let Some(target) = find_nearest_reachable_node( - ep.point, - &self.graph_nodes, - robot_position, - lines, - config, - ) && target.distance(robot_position) >= config.min_distance_from_robot - { - self.frontiers.push(Frontier { - viewpoint: target, - look_direction: ep.outward_dir, - endpoint: ep.point, - line_idx: ep.line_idx, - estimated_area: config.gap_probe_distance * config.gap_probe_distance * 0.5, - }); - } - } - - // Debug logging - log::debug!( - "Frontier detection: {} lines, {} unconnected, {} gaps, {} single, {} frontiers", - lines.len(), - self.unconnected.len(), - self.gaps.len(), - self.unconnected - .iter() - .enumerate() - .filter(|(i, _)| !used_endpoints[*i]) - .count(), - self.frontiers.len() - ); - - // Step 6: Sort by estimated_area (largest first) - self.frontiers.sort_by(|a, b| { - b.estimated_area - .partial_cmp(&a.estimated_area) - .unwrap_or(std::cmp::Ordering::Equal) - }); - - &self.frontiers - } - - /// Rank the currently detected frontiers by exploration value. - /// - /// Value considers distance from robot and estimated area. - pub fn rank(&mut self, robot_position: Point2D) -> &[(Frontier, f32)] { - self.ranked_frontiers.clear(); - - for f in &self.frontiers { - let distance = f.viewpoint.distance(robot_position); - - // Distance factor: 1.0 at 0m, decaying to ~0.1 at 10m - let distance_factor = 1.0 / (1.0 + distance * 0.1); - - // Area factor: normalize by expected room size (~25m²) - let area_factor = (f.estimated_area / 25.0).min(1.0); - - // Value: prefer nearby frontiers with large unexplored areas - let value = distance_factor * 0.3 + area_factor * 0.7; - - self.ranked_frontiers.push((f.clone(), value)); - } - - // Sort by value (descending) - self.ranked_frontiers - .sort_by(|a, b| b.1.partial_cmp(&a.1).unwrap_or(std::cmp::Ordering::Equal)); - - &self.ranked_frontiers - } - - /// Get the best frontier for exploration. - pub fn get_best( - &mut self, - lines: &[Line2D], - robot_position: Point2D, - config: &FrontierConfig, - ) -> Option { - self.detect(lines, robot_position, config); - - if self.frontiers.is_empty() { - return None; - } - - self.rank(robot_position); - self.ranked_frontiers.first().map(|(f, _)| f.clone()) - } - - /// Get the currently detected frontiers (from last detect call). - pub fn frontiers(&self) -> &[Frontier] { - &self.frontiers - } - - /// Get the currently ranked frontiers (from last rank call). - pub fn ranked_frontiers(&self) -> &[(Frontier, f32)] { - &self.ranked_frontiers - } -} - -/// Configuration for frontier detection. -#[derive(Clone, Debug, Serialize, Deserialize)] -pub struct FrontierConfig { - /// Maximum distance for endpoints to be considered "connected" (meters). - /// Endpoints closer than this are not frontiers. - /// Default: 0.3m - pub connection_distance: f32, - - /// Minimum distance from robot for a valid frontier (meters). - /// Frontiers too close are ignored. - /// Default: 0.5m - pub min_distance_from_robot: f32, - - /// Safety margin: viewpoint offset from wall (meters). - /// The viewpoint is placed this far from the line endpoint. - /// Default: 0.4m - pub safety_margin: f32, - - /// Gap probe distance: how far to probe for unexplored gaps (meters). - /// Used for estimating unexplored area size. - /// Default: 2.0m - pub gap_probe_distance: f32, - - /// Robot radius for viewpoint validation (meters). - /// Viewpoints must be clear of walls by this margin. - /// Default: 0.15m - pub robot_radius: f32, - - /// Maximum iterations for viewpoint adjustment. - /// If viewpoint is blocked, move away from wall up to this many times. - /// Default: 5 - pub max_viewpoint_iterations: usize, - - /// Viewpoint adjustment step size (meters). - /// How far to move the viewpoint per iteration if blocked. - /// Default: 0.1m - pub viewpoint_step: f32, - - /// Minimum gap width for safe robot passage (meters). - /// Gaps narrower than this are ignored (robot can't fit). - /// Default: 0.4m (robot diameter + safety margin) - pub min_gap_width: f32, - - /// Maximum gap width to consider as a single gap (meters). - /// Larger distances are treated as separate areas, not a gap. - /// Default: 3.0m - pub max_gap_width: f32, - - // ───────────────────────────────────────────────────────────────────────── - // CBVG-based frontier detection parameters - // ───────────────────────────────────────────────────────────────────────── - /// Minimum uncovered angle for a CBVG node to be a frontier (radians). - /// Nodes with a gap of at least this size (no neighbors or walls) are frontiers. - /// Default: 1.75 rad (100 degrees) - pub min_frontier_angle: f32, - - /// Maximum distance to check for walls when detecting frontiers (meters). - /// Walls closer than this block the direction from being a frontier. - /// Default: 0.5m - pub max_wall_distance: f32, -} - -impl Default for FrontierConfig { - fn default() -> Self { - Self { - connection_distance: 0.3, - min_distance_from_robot: 0.5, - safety_margin: 0.4, - gap_probe_distance: 2.0, - robot_radius: 0.15, - max_viewpoint_iterations: 5, - viewpoint_step: 0.1, - min_gap_width: 0.4, // robot diameter (0.3m) + margin - max_gap_width: 10.0, // allow gaps across rooms - // CBVG-based parameters - min_frontier_angle: 1.75, // 100 degrees in radians - max_wall_distance: 0.5, - } - } -} - -impl FrontierConfig { - /// Create a new configuration with default values. + /// Create a new frontier detector with default settings pub fn new() -> Self { Self::default() } - /// Builder-style setter for connection distance. - pub fn with_connection_distance(mut self, meters: f32) -> Self { - self.connection_distance = meters; - self + /// Create with custom minimum frontier size + pub fn with_min_size(min_size: usize) -> Self { + Self { + min_frontier_size: min_size, + ..Default::default() + } } - /// Builder-style setter for minimum distance from robot. - pub fn with_min_distance_from_robot(mut self, meters: f32) -> Self { - self.min_distance_from_robot = meters; - self - } + /// Detect all frontier cells in the grid (SIMD-optimized). + /// + /// Uses SIMD to quickly filter Floor cells (16 at a time), then + /// performs scalar neighbor counting for each candidate. + pub fn detect_frontier_cells(&self, storage: &GridStorage) -> Vec { + let width = storage.width(); + let height = storage.height(); + let cell_types = storage.cell_types_raw(); - /// Builder-style setter for safety margin. - pub fn with_safety_margin(mut self, meters: f32) -> Self { - self.safety_margin = meters; - self - } + let floor_val = CellType::Floor as u8; + let floor_vec = u8x16::splat(floor_val); - /// Builder-style setter for gap probe distance. - pub fn with_gap_probe_distance(mut self, meters: f32) -> Self { - self.gap_probe_distance = meters; - self - } + let mut frontiers = Vec::new(); - /// Builder-style setter for robot radius. - pub fn with_robot_radius(mut self, meters: f32) -> Self { - self.robot_radius = meters; - self - } + // Process 16 cells at a time using SIMD + let chunks = cell_types.chunks_exact(16); + let remainder = chunks.remainder(); - /// Builder-style setter for minimum gap width. - pub fn with_min_gap_width(mut self, meters: f32) -> Self { - self.min_gap_width = meters; - self - } + for (chunk_idx, chunk) in chunks.enumerate() { + let data = u8x16::from_slice(chunk); + let is_floor = data.simd_eq(floor_vec); + let mask = is_floor.to_bitmask(); - /// Builder-style setter for maximum gap width. - pub fn with_max_gap_width(mut self, meters: f32) -> Self { - self.max_gap_width = meters; - self - } -} + if mask == 0 { + continue; // No Floor cells in this chunk - skip entirely + } -/// Convert a point to a grid cell coordinate. -#[inline] -fn point_to_cell(point: Point2D, cell_size: f32) -> (i32, i32) { - ( - (point.x / cell_size).floor() as i32, - (point.y / cell_size).floor() as i32, - ) -} + // Process each Floor cell found in this chunk + let base_idx = chunk_idx * 16; + for bit in 0..16 { + if (mask & (1 << bit)) != 0 { + let idx = base_idx + bit; + let x = (idx % width) as i32; + let y = (idx / width) as i32; + let coord = GridCoord::new(x, y); -/// Check if a point is connected to any other endpoint in the 3x3 neighborhood. -#[inline] -fn is_connected_in_grid( - grid: &EndpointGrid, - cell: (i32, i32), - point: Point2D, - line_idx: usize, - connection_distance: f32, -) -> bool { - for dx in -1..=1 { - for dy in -1..=1 { - let neighbor_cell = (cell.0 + dx, cell.1 + dy); - if let Some(neighbors) = grid.get(&neighbor_cell) { - for &(other_point, other_idx, _) in neighbors { - if other_idx == line_idx { - continue; - } - if point.distance(other_point) < connection_distance { - return true; + // Count unknown neighbors (scalar - depends on grid topology) + let unknown_count = + self.count_unknown_neighbors(storage, coord, width as i32, height as i32); + if unknown_count > 0 { + frontiers.push(FrontierCell::new(coord, unknown_count)); } } } } - } - false -} -/// Find a target point that makes progress toward the frontier. -/// -/// Simple approach: -/// 1. If robot can see the frontier directly, move toward it -/// 2. Otherwise, find any reachable graph node that is closer to frontier -/// 3. If no such node, the frontier is not reachable from current position -fn find_nearest_reachable_node( - frontier_point: Point2D, - graph_nodes: &[Point2D], - robot_position: Point2D, - lines: &[Line2D], - config: &FrontierConfig, -) -> Option { - let robot_dist_to_frontier = robot_position.distance(frontier_point); + // Handle remainder (< 16 cells) with scalar fallback + let base_idx = (cell_types.len() / 16) * 16; + for (i, &cell_type) in remainder.iter().enumerate() { + if cell_type == floor_val { + let idx = base_idx + i; + let x = (idx % width) as i32; + let y = (idx / width) as i32; + let coord = GridCoord::new(x, y); - // Skip if frontier is too close - if robot_dist_to_frontier < config.min_distance_from_robot { - return None; + let unknown_count = + self.count_unknown_neighbors(storage, coord, width as i32, height as i32); + if unknown_count > 0 { + frontiers.push(FrontierCell::new(coord, unknown_count)); + } + } + } + + trace!("[Frontier] Detected {} frontier cells", frontiers.len()); + frontiers } - // First, check if robot can directly see the frontier point - if !is_path_blocked(robot_position, frontier_point, lines) { - // Direct line of sight - move toward frontier - let to_frontier = frontier_point - robot_position; - let move_dist = - (robot_dist_to_frontier - config.safety_margin).max(config.min_distance_from_robot); - let target = robot_position + to_frontier.normalized() * move_dist; + /// Count unknown neighbors for a cell (helper for SIMD detection). + #[inline] + fn count_unknown_neighbors( + &self, + storage: &GridStorage, + coord: GridCoord, + width: i32, + height: i32, + ) -> u8 { + let neighbors = if self.use_8_connectivity { + coord.neighbors_8() + } else { + let n4 = coord.neighbors_4(); + [n4[0], n4[1], n4[2], n4[3], n4[0], n4[1], n4[2], n4[3]] + }; - // Verify target is not too close to walls - if !is_point_too_close_to_lines(target, lines, config.robot_radius) { - return Some(target); + let limit = if self.use_8_connectivity { 8 } else { 4 }; + let mut count = 0u8; + + for n in neighbors.iter().take(limit) { + if n.x >= 0 + && n.x < width + && n.y >= 0 + && n.y < height + && storage.get_type(*n) == CellType::Unknown + { + count += 1; + } } - // Even if computed target is too close to walls, try moving there anyway - // (path planner will find an alternative) - return Some(target); + + count } - // No direct line of sight - find the best intermediate node - let mut best_node: Option<(Point2D, f32)> = None; + /// Detect and cluster frontiers + pub fn detect_frontiers(&self, storage: &GridStorage) -> Vec { + let frontier_cells = self.detect_frontier_cells(storage); - for &node in graph_nodes { - // Skip robot position - if node.distance(robot_position) < 0.1 { - continue; + if frontier_cells.is_empty() { + debug!("[Frontier] No frontier cells found (no Floor cells adjacent to Unknown)"); + return Vec::new(); } - // Skip nodes too close to frontier (would be blocked by walls near frontier) - if node.distance(frontier_point) < config.safety_margin { - continue; - } + // Build a set for fast lookup + let frontier_set: HashSet = frontier_cells.iter().map(|f| f.coord).collect(); - // Must be reachable from robot (direct line of sight) - if is_path_blocked(robot_position, node, lines) { - continue; - } + // Map coord to FrontierCell + let cell_map: std::collections::HashMap = + frontier_cells.iter().map(|f| (f.coord, *f)).collect(); - // Prefer nodes closer to frontier (makes progress) - let node_dist_to_frontier = node.distance(frontier_point); + // Cluster using BFS + let mut visited = HashSet::new(); + let mut frontiers = Vec::new(); + let mut clusters_before_filter = 0; + let mut filtered_clusters = 0; - if best_node.is_none() || node_dist_to_frontier < best_node.unwrap().1 { - best_node = Some((node, node_dist_to_frontier)); - } - } - - best_node.map(|(node, _)| node) -} - -/// Check if the straight-line path between two points is blocked by any line. -/// Excludes intersections that occur exactly at the path endpoints (from/to). -fn is_path_blocked(from: Point2D, to: Point2D, lines: &[Line2D]) -> bool { - let epsilon = 0.01; // 1cm tolerance for endpoint matching - - for line in lines { - if segments_intersect(from, to, line.start, line.end) { - // Check if the intersection is at one of the path endpoints - // If the line endpoint is essentially the same as our path endpoint, - // it's not a blocking intersection - it's expected. - let from_matches_start = from.distance(line.start) < epsilon; - let from_matches_end = from.distance(line.end) < epsilon; - let to_matches_start = to.distance(line.start) < epsilon; - let to_matches_end = to.distance(line.end) < epsilon; - - // If the intersection is only at endpoints, it's not truly blocking - if from_matches_start || from_matches_end || to_matches_start || to_matches_end { + for cell in &frontier_cells { + if visited.contains(&cell.coord) { continue; } - return true; + + // BFS to find all connected frontier cells + let mut cluster = Vec::new(); + let mut queue = VecDeque::new(); + queue.push_back(cell.coord); + visited.insert(cell.coord); + + while let Some(current) = queue.pop_front() { + if let Some(&fc) = cell_map.get(¤t) { + cluster.push(fc); + } + + // Check neighbors + let neighbors = if self.use_8_connectivity { + current.neighbors_8().to_vec() + } else { + current.neighbors_4().to_vec() + }; + + for neighbor in neighbors { + if !visited.contains(&neighbor) && frontier_set.contains(&neighbor) { + visited.insert(neighbor); + queue.push_back(neighbor); + } + } + } + + clusters_before_filter += 1; + + // Only keep clusters above minimum size + if cluster.len() >= self.min_frontier_size { + frontiers.push(Frontier::from_cells(cluster, storage)); + } else { + filtered_clusters += 1; + trace!( + "[Frontier] Filtered cluster with {} cells (min_size={})", + cluster.len(), + self.min_frontier_size + ); + } } - } - false -} -/// Check if a point is too close to any line (within robot radius). -fn is_point_too_close_to_lines(point: Point2D, lines: &[Line2D], min_distance: f32) -> bool { - for line in lines { - if line.distance_to_point(point) < min_distance { - return true; + debug!( + "[Frontier] Clustering: {} cells → {} clusters, {} valid (filtered {} < {} cells)", + frontier_cells.len(), + clusters_before_filter, + frontiers.len(), + filtered_clusters, + self.min_frontier_size + ); + + frontiers + } + + /// Get frontiers sorted by exploration score (highest first) + pub fn detect_frontiers_sorted(&self, storage: &GridStorage) -> Vec { + let mut frontiers = self.detect_frontiers(storage); + frontiers.sort_by(|a, b| b.exploration_score().total_cmp(&a.exploration_score())); + frontiers + } + + /// Find the best frontier to explore from a given position + pub fn best_frontier_from( + &self, + storage: &GridStorage, + robot_pos: GridCoord, + ) -> Option { + let frontiers = self.detect_frontiers(storage); + + if frontiers.is_empty() { + return None; } - } - false -} -/// Check if two line segments intersect. -fn segments_intersect(a1: Point2D, a2: Point2D, b1: Point2D, b2: Point2D) -> bool { - let d1 = cross_product(b2 - b1, a1 - b1); - let d2 = cross_product(b2 - b1, a2 - b1); - let d3 = cross_product(a2 - a1, b1 - a1); - let d4 = cross_product(a2 - a1, b2 - a1); - - if ((d1 > 0.0 && d2 < 0.0) || (d1 < 0.0 && d2 > 0.0)) - && ((d3 > 0.0 && d4 < 0.0) || (d3 < 0.0 && d4 > 0.0)) - { - return true; - } - - // Check collinear cases - let epsilon = 1e-6; - if d1.abs() < epsilon && point_on_segment(a1, b1, b2) { - return true; - } - if d2.abs() < epsilon && point_on_segment(a2, b1, b2) { - return true; - } - if d3.abs() < epsilon && point_on_segment(b1, a1, a2) { - return true; - } - if d4.abs() < epsilon && point_on_segment(b2, a1, a2) { - return true; - } - - false -} - -/// 2D cross product of vectors. -#[inline] -fn cross_product(a: Point2D, b: Point2D) -> f32 { - a.x * b.y - a.y * b.x -} - -/// Check if point p lies on segment (a, b). -#[inline] -fn point_on_segment(p: Point2D, a: Point2D, b: Point2D) -> bool { - p.x >= a.x.min(b.x) && p.x <= a.x.max(b.x) && p.y >= a.y.min(b.y) && p.y <= a.y.max(b.y) -} - -// ───────────────────────────────────────────────────────────────────────────── -// Convenience functions (for backward compatibility) -// ───────────────────────────────────────────────────────────────────────────── - -/// Detect frontiers from map lines. -pub fn detect_frontiers( - lines: &[Line2D], - robot_position: Point2D, - config: &FrontierConfig, -) -> Vec { - let mut detector = FrontierDetector::new(); - detector.detect(lines, robot_position, config).to_vec() -} - -/// Rank frontiers by exploration value. -pub fn rank_frontiers(frontiers: &[Frontier], robot_position: Point2D) -> Vec<(Frontier, f32)> { - frontiers - .iter() - .map(|f| { - let distance = f.viewpoint.distance(robot_position); - let distance_factor = 1.0 / (1.0 + distance * 0.1); - let area_factor = (f.estimated_area / 25.0).min(1.0); - let value = distance_factor * 0.3 + area_factor * 0.7; - (f.clone(), value) + // Score combines exploration value and distance + // Closer frontiers with higher exploration potential are preferred + frontiers.into_iter().max_by(|a, b| { + let score_a = self.score_frontier(a, robot_pos); + let score_b = self.score_frontier(b, robot_pos); + score_a.total_cmp(&score_b) }) - .collect() -} - -/// Get the best frontier for exploration. -pub fn get_best_frontier( - lines: &[Line2D], - robot_position: Point2D, - config: &FrontierConfig, -) -> Option { - let mut detector = FrontierDetector::new(); - detector.get_best(lines, robot_position, config) -} - -/// Cluster nearby frontiers into frontier regions. -pub fn cluster_frontiers(frontiers: &[Frontier], cluster_distance: f32) -> Vec> { - if frontiers.is_empty() { - return Vec::new(); } - let mut clusters: Vec> = Vec::new(); - let mut assigned = vec![false; frontiers.len()]; + /// Score a frontier considering distance from robot + fn score_frontier(&self, frontier: &Frontier, robot_pos: GridCoord) -> f32 { + let distance = robot_pos.manhattan_distance(&frontier.centroid_grid) as f32; + let exploration_score = frontier.exploration_score(); - for i in 0..frontiers.len() { - if assigned[i] { - continue; + // Penalize distance, but not too heavily + // We want to explore nearby frontiers first, but not ignore large distant ones + if distance > 0.0 { + exploration_score / (1.0 + distance * 0.1) + } else { + exploration_score * 2.0 // Bonus for being at the frontier } - - let mut cluster = vec![frontiers[i].clone()]; - assigned[i] = true; - - for j in (i + 1)..frontiers.len() { - if assigned[j] { - continue; - } - - let close = cluster - .iter() - .any(|f| f.viewpoint.distance(frontiers[j].viewpoint) < cluster_distance); - - if close { - cluster.push(frontiers[j].clone()); - assigned[j] = true; - } - } - - clusters.push(cluster); } - - clusters } -/// Get the centroid of a frontier cluster. -pub fn cluster_centroid(cluster: &[Frontier]) -> Option { - if cluster.is_empty() { - return None; - } +/// Quick frontier detection without clustering (for performance) +pub fn count_frontier_cells(storage: &GridStorage) -> usize { + let detector = FrontierDetector::new(); + detector.detect_frontier_cells(storage).len() +} - let sum_x: f32 = cluster.iter().map(|f| f.viewpoint.x).sum(); - let sum_y: f32 = cluster.iter().map(|f| f.viewpoint.y).sum(); - let n = cluster.len() as f32; - - Some(Point2D::new(sum_x / n, sum_y / n)) +/// Check if any frontiers exist in the map +pub fn has_frontiers(storage: &GridStorage) -> bool { + let detector = FrontierDetector::new(); + !detector.detect_frontier_cells(storage).is_empty() } #[cfg(test)] mod tests { use super::*; + use std::simd::{cmp::SimdPartialEq, u8x16}; - fn make_partial_room() -> Vec { - // Room with one side open (no top wall) - vec![ - Line2D::new(Point2D::new(-5.0, -5.0), Point2D::new(5.0, -5.0)), // Bottom - Line2D::new(Point2D::new(5.0, -5.0), Point2D::new(5.0, 5.0)), // Right - Line2D::new(Point2D::new(-5.0, 5.0), Point2D::new(-5.0, -5.0)), // Left - ] + fn create_test_storage() -> GridStorage { + GridStorage::centered(20, 20, 0.1) // 2m x 2m at 10cm resolution } - fn make_complete_room() -> Vec { - vec![ - Line2D::new(Point2D::new(-5.0, -5.0), Point2D::new(5.0, -5.0)), // Bottom - Line2D::new(Point2D::new(5.0, -5.0), Point2D::new(5.0, 5.0)), // Right - Line2D::new(Point2D::new(5.0, 5.0), Point2D::new(-5.0, 5.0)), // Top - Line2D::new(Point2D::new(-5.0, 5.0), Point2D::new(-5.0, -5.0)), // Left - ] + // ========================================================================= + // SIMD-OPTIMIZED TEST HELPERS + // These are test-only implementations to verify SIMD correctness + // ========================================================================= + + /// Detect all frontier cells using SIMD-accelerated Floor cell detection. + fn detect_frontier_cells_simd( + detector: &FrontierDetector, + storage: &GridStorage, + ) -> Vec { + let width = storage.width(); + let height = storage.height(); + let cell_types = storage.cell_types_raw(); + + let floor_val = CellType::Floor as u8; + let floor_vec = u8x16::splat(floor_val); + + let mut frontiers = Vec::new(); + + // Process 16 cells at a time using SIMD + let chunks = cell_types.chunks_exact(16); + let remainder = chunks.remainder(); + + for (chunk_idx, chunk) in chunks.enumerate() { + let data = u8x16::from_slice(chunk); + let is_floor = data.simd_eq(floor_vec); + let mask = is_floor.to_bitmask(); + + if mask == 0 { + continue; // No Floor cells in this chunk + } + + // Process each Floor cell found + let base_idx = chunk_idx * 16; + for bit in 0..16 { + if (mask & (1 << bit)) != 0 { + let idx = base_idx + bit; + let x = (idx % width) as i32; + let y = (idx / width) as i32; + let coord = GridCoord::new(x, y); + + // Count unknown neighbors (scalar) + let unknown_count = count_unknown_neighbors( + detector, + storage, + coord, + width as i32, + height as i32, + ); + if unknown_count > 0 { + frontiers.push(FrontierCell::new(coord, unknown_count)); + } + } + } + } + + // Handle remainder (< 16 cells) + let base_idx = (cell_types.len() / 16) * 16; + for (i, &cell_type) in remainder.iter().enumerate() { + if cell_type == floor_val { + let idx = base_idx + i; + let x = (idx % width) as i32; + let y = (idx / width) as i32; + let coord = GridCoord::new(x, y); + + let unknown_count = + count_unknown_neighbors(detector, storage, coord, width as i32, height as i32); + if unknown_count > 0 { + frontiers.push(FrontierCell::new(coord, unknown_count)); + } + } + } + + frontiers + } + + /// Count unknown neighbors for a given cell + fn count_unknown_neighbors( + detector: &FrontierDetector, + storage: &GridStorage, + coord: GridCoord, + width: i32, + height: i32, + ) -> u8 { + let neighbors = if detector.use_8_connectivity { + coord.neighbors_8() + } else { + let n4 = coord.neighbors_4(); + [n4[0], n4[1], n4[2], n4[3], n4[0], n4[1], n4[2], n4[3]] + }; + + let limit = if detector.use_8_connectivity { 8 } else { 4 }; + let mut count = 0u8; + + for n in neighbors.iter().take(limit) { + if n.x >= 0 + && n.x < width + && n.y >= 0 + && n.y < height + && storage.get_type(*n) == CellType::Unknown + { + count += 1; + } + } + + count + } + + /// Detect and cluster frontiers using SIMD-optimized cell detection. + fn detect_frontiers_simd(detector: &FrontierDetector, storage: &GridStorage) -> Vec { + let frontier_cells = detect_frontier_cells_simd(detector, storage); + + if frontier_cells.is_empty() { + return Vec::new(); + } + + cluster_frontier_cells(detector, frontier_cells, storage) + } + + /// Cluster frontier cells into connected frontiers. + fn cluster_frontier_cells( + detector: &FrontierDetector, + frontier_cells: Vec, + storage: &GridStorage, + ) -> Vec { + // Build a set for fast lookup + let frontier_set: HashSet = frontier_cells.iter().map(|f| f.coord).collect(); + + // Map coord to FrontierCell + let cell_map: std::collections::HashMap = + frontier_cells.iter().map(|f| (f.coord, *f)).collect(); + + // Cluster using BFS + let mut visited = HashSet::new(); + let mut frontiers = Vec::new(); + + for cell in &frontier_cells { + if visited.contains(&cell.coord) { + continue; + } + + // BFS to find all connected frontier cells + let mut cluster = Vec::new(); + let mut queue = VecDeque::new(); + queue.push_back(cell.coord); + visited.insert(cell.coord); + + while let Some(current) = queue.pop_front() { + if let Some(&fc) = cell_map.get(¤t) { + cluster.push(fc); + } + + // Check neighbors + let neighbors = if detector.use_8_connectivity { + current.neighbors_8().to_vec() + } else { + current.neighbors_4().to_vec() + }; + + for neighbor in neighbors { + if !visited.contains(&neighbor) && frontier_set.contains(&neighbor) { + visited.insert(neighbor); + queue.push_back(neighbor); + } + } + } + + // Only keep clusters above minimum size + if cluster.len() >= detector.min_frontier_size { + frontiers.push(Frontier::from_cells(cluster, storage)); + } + } + + frontiers } #[test] - fn test_detect_frontiers_partial_room() { - let lines = make_partial_room(); - let config = FrontierConfig::default(); - let robot = Point2D::zero(); + fn test_no_frontiers_on_empty_grid() { + let storage = create_test_storage(); + let detector = FrontierDetector::new(); - let frontiers = detect_frontiers(&lines, robot, &config); + let frontiers = detector.detect_frontiers(&storage); + assert!(frontiers.is_empty()); + } + + #[test] + fn test_frontier_detection_basic() { + let mut storage = create_test_storage(); + + // Create a small floor area + for x in 8..12 { + for y in 8..12 { + storage.set_type(GridCoord::new(x, y), CellType::Floor); + } + } + + let detector = FrontierDetector::new(); + let cells = detector.detect_frontier_cells(&storage); + + // All edge cells should be frontiers + assert!(cells.len() > 0); + + // Center cell should NOT be a frontier + let center = GridCoord::new(10, 10); + assert!(!cells.iter().any(|f| f.coord == center)); + } + + #[test] + fn test_frontier_clustering() { + let mut storage = create_test_storage(); + + // Create two separate floor regions + // Region 1 + for x in 2..5 { + for y in 2..5 { + storage.set_type(GridCoord::new(x, y), CellType::Floor); + } + } + // Region 2 (separated by unknown) + for x in 15..18 { + for y in 15..18 { + storage.set_type(GridCoord::new(x, y), CellType::Floor); + } + } + + let detector = FrontierDetector::new(); + let frontiers = detector.detect_frontiers(&storage); + + // Should have 2 frontier clusters + assert_eq!(frontiers.len(), 2); + } + + #[test] + fn test_frontier_centroid() { + let mut storage = create_test_storage(); + + // Create a symmetric floor region + for x in 5..15 { + for y in 5..15 { + storage.set_type(GridCoord::new(x, y), CellType::Floor); + } + } + + let detector = FrontierDetector::new(); + let frontiers = detector.detect_frontiers(&storage); + + assert_eq!(frontiers.len(), 1); + + // Centroid should be roughly in the middle of the frontier ring + let frontier = &frontiers[0]; + // The frontier is on the edge, so centroid should be near edges + assert!(frontier.centroid_grid.x >= 5 && frontier.centroid_grid.x <= 14); + assert!(frontier.centroid_grid.y >= 5 && frontier.centroid_grid.y <= 14); + } + + #[test] + fn test_min_frontier_size() { + let mut storage = create_test_storage(); + + // Create a very small floor area (just 1 cell) + storage.set_type(GridCoord::new(10, 10), CellType::Floor); + + let detector = FrontierDetector::with_min_size(5); + let frontiers = detector.detect_frontiers(&storage); + + // Single cell frontier should be filtered out + assert!(frontiers.is_empty()); + } + + #[test] + fn test_wall_blocks_frontier() { + let mut storage = create_test_storage(); + + // Floor surrounded by walls on one side + for x in 8..12 { + for y in 8..12 { + storage.set_type(GridCoord::new(x, y), CellType::Floor); + } + } + // Add wall on the right + for y in 7..13 { + storage.set_type(GridCoord::new(12, y), CellType::Wall); + } + + let detector = FrontierDetector::new(); + let cells = detector.detect_frontier_cells(&storage); + + // Cells next to wall should not have frontiers on that side + let right_edge = GridCoord::new(11, 10); + let frontier_cell = cells.iter().find(|f| f.coord == right_edge); + + // The right edge cell should have fewer unknown neighbors + // (wall blocks the right side from being unknown) + if let Some(fc) = frontier_cell { + // Should be < 8 unknown neighbors due to wall + assert!(fc.unknown_neighbors < 8); + } + } + + #[test] + fn test_exploration_score() { + let mut storage = create_test_storage(); + + // Large floor area + for x in 2..18 { + for y in 2..18 { + storage.set_type(GridCoord::new(x, y), CellType::Floor); + } + } + + let detector = FrontierDetector::new(); + let frontiers = detector.detect_frontiers(&storage); - // Should have frontiers at the open top assert!(!frontiers.is_empty()); - // Viewpoints should be offset from walls - for f in &frontiers { - let min_dist = lines - .iter() - .map(|l| l.distance_to_point(f.viewpoint)) - .fold(f32::MAX, f32::min); - assert!( - min_dist >= config.robot_radius * 0.9, - "Viewpoint too close to wall: {:.3}m", - min_dist - ); + // Score should be positive + assert!(frontiers[0].exploration_score() > 0.0); + } + + #[test] + fn test_best_frontier_from() { + let mut storage = create_test_storage(); + + // Create floor area + for x in 5..15 { + for y in 5..15 { + storage.set_type(GridCoord::new(x, y), CellType::Floor); + } } + + let detector = FrontierDetector::new(); + let robot_pos = GridCoord::new(10, 10); + + let best = detector.best_frontier_from(&storage, robot_pos); + assert!(best.is_some()); } #[test] - fn test_detect_frontiers_complete_room() { - let lines = make_complete_room(); - let config = FrontierConfig::default(); - let robot = Point2D::zero(); + fn test_simd_frontier_matches_scalar() { + let mut storage = create_test_storage(); - let frontiers = detect_frontiers(&lines, robot, &config); - - // Complete room - all corners should be connected (no frontiers) - assert_eq!(frontiers.len(), 0); - } - - #[test] - fn test_detect_frontiers_single_line() { - let lines = vec![Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(5.0, 0.0))]; - let config = FrontierConfig::default(); - let robot = Point2D::new(2.5, 2.0); - - let frontiers = detect_frontiers(&lines, robot, &config); - - // Both endpoints are unconnected, but viewpoints should be valid - assert!(frontiers.len() <= 2); - - // Check viewpoints are not on the line - for f in &frontiers { - assert!(f.viewpoint.distance(f.endpoint) > 0.1); + // Create floor area + for x in 5..15 { + for y in 5..15 { + storage.set_type(GridCoord::new(x, y), CellType::Floor); + } } + + let detector = FrontierDetector::new(); + + // Get scalar frontier cells + let scalar_cells = detector.detect_frontier_cells(&storage); + + // Get SIMD frontier cells + let simd_cells = detect_frontier_cells_simd(&detector, &storage); + + // Should have same count + assert_eq!( + scalar_cells.len(), + simd_cells.len(), + "Cell count mismatch: scalar={}, simd={}", + scalar_cells.len(), + simd_cells.len() + ); + + // Verify same cells are found (order may differ) + let scalar_set: std::collections::HashSet<_> = scalar_cells + .iter() + .map(|c| (c.coord, c.unknown_neighbors)) + .collect(); + let simd_set: std::collections::HashSet<_> = simd_cells + .iter() + .map(|c| (c.coord, c.unknown_neighbors)) + .collect(); + + assert_eq!(scalar_set, simd_set, "Different frontier cells detected"); } #[test] - fn test_viewpoint_not_on_wall() { - let lines = vec![Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(5.0, 0.0))]; - let config = FrontierConfig::default(); - let robot = Point2D::new(2.5, 2.0); + fn test_simd_frontier_clustering_matches_scalar() { + let mut storage = create_test_storage(); - let frontiers = detect_frontiers(&lines, robot, &config); - - for f in &frontiers { - // Viewpoint should be at least safety_margin away from the endpoint - let dist_to_endpoint = f.viewpoint.distance(f.endpoint); - assert!( - dist_to_endpoint >= config.safety_margin * 0.9, - "Viewpoint {:.2}m from endpoint (expected >= {:.2}m)", - dist_to_endpoint, - config.safety_margin - ); + // Create two separate floor regions + for x in 2..5 { + for y in 2..5 { + storage.set_type(GridCoord::new(x, y), CellType::Floor); + } } - } - - #[test] - fn test_frontier_has_look_direction() { - let lines = vec![Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(5.0, 0.0))]; - let config = FrontierConfig::default(); - let robot = Point2D::new(2.5, 2.0); - - let frontiers = detect_frontiers(&lines, robot, &config); - - for f in &frontiers { - // Look direction should be a unit vector (or close to it) - let len = (f.look_direction.x.powi(2) + f.look_direction.y.powi(2)).sqrt(); - assert!( - (len - 1.0).abs() < 0.01, - "Look direction not unit: len={:.3}", - len - ); + for x in 15..18 { + for y in 15..18 { + storage.set_type(GridCoord::new(x, y), CellType::Floor); + } } - } - #[test] - fn test_detector_reuse_buffers() { - let mut detector = FrontierDetector::new(); - let config = FrontierConfig::default(); - let robot = Point2D::zero(); + let detector = FrontierDetector::new(); - // First detection - let lines1 = vec![Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(5.0, 0.0))]; - let frontiers1 = detector.detect(&lines1, robot, &config); - let count1 = frontiers1.len(); + let scalar_frontiers = detector.detect_frontiers(&storage); + let simd_frontiers = detect_frontiers_simd(&detector, &storage); - // Second detection with different lines - let lines2 = make_complete_room(); - let frontiers2 = detector.detect(&lines2, robot, &config); - assert_eq!(frontiers2.len(), 0); + // Should have same number of clusters + assert_eq!( + scalar_frontiers.len(), + simd_frontiers.len(), + "Frontier count mismatch: scalar={}, simd={}", + scalar_frontiers.len(), + simd_frontiers.len() + ); - // Third detection - back to single line - let frontiers3 = detector.detect(&lines1, robot, &config); - assert_eq!(frontiers3.len(), count1); - } + // Each cluster should have same size + let mut scalar_sizes: Vec<_> = scalar_frontiers.iter().map(|f| f.size).collect(); + let mut simd_sizes: Vec<_> = simd_frontiers.iter().map(|f| f.size).collect(); + scalar_sizes.sort(); + simd_sizes.sort(); - #[test] - fn test_cluster_frontiers_uses_viewpoint() { - let frontiers = vec![ - Frontier { - viewpoint: Point2D::new(0.0, 0.0), - look_direction: Point2D::new(1.0, 0.0), - endpoint: Point2D::new(-0.4, 0.0), - line_idx: 0, - estimated_area: 4.0, - }, - Frontier { - viewpoint: Point2D::new(0.1, 0.0), - look_direction: Point2D::new(1.0, 0.0), - endpoint: Point2D::new(-0.3, 0.0), - line_idx: 1, - estimated_area: 4.0, - }, - Frontier { - viewpoint: Point2D::new(10.0, 10.0), - look_direction: Point2D::new(1.0, 0.0), - endpoint: Point2D::new(9.6, 10.0), - line_idx: 2, - estimated_area: 4.0, - }, - ]; - - let clusters = cluster_frontiers(&frontiers, 0.5); - - // Should have 2 clusters based on viewpoint distance - assert_eq!(clusters.len(), 2); - } - - #[test] - fn test_cluster_centroid_uses_viewpoint() { - let frontiers = vec![ - Frontier { - viewpoint: Point2D::new(0.0, 0.0), - look_direction: Point2D::new(1.0, 0.0), - endpoint: Point2D::new(-0.4, 0.0), - line_idx: 0, - estimated_area: 4.0, - }, - Frontier { - viewpoint: Point2D::new(2.0, 0.0), - look_direction: Point2D::new(1.0, 0.0), - endpoint: Point2D::new(1.6, 0.0), - line_idx: 1, - estimated_area: 4.0, - }, - ]; - - let centroid = cluster_centroid(&frontiers); - - assert!(centroid.is_some()); - let c = centroid.unwrap(); - // Centroid should be at (1.0, 0.0) - average of viewpoints - assert!((c.x - 1.0).abs() < 0.01); - assert!((c.y - 0.0).abs() < 0.01); + assert_eq!(scalar_sizes, simd_sizes, "Cluster sizes differ"); } } diff --git a/vastu-map/src/query/mod.rs b/vastu-map/src/query/mod.rs index 652826c..5cc41ac 100644 --- a/vastu-map/src/query/mod.rs +++ b/vastu-map/src/query/mod.rs @@ -1,50 +1,47 @@ -//! Map query operations for VectorMap. +//! Map query operations. //! -//! Provides spatial queries against the map: -//! - **Raycast**: Find distance to obstacles in a direction -//! - **Occupancy**: Query whether a point is free, occupied, or unknown -//! - **Frontier**: Detect unexplored boundaries for exploration -//! - **CBVG**: Clearance-based visibility graph for safe path planning +//! This module provides query operations on the occupancy grid: //! -//! # Example +//! - **Frontier detection**: Find boundaries between known and unknown areas +//! - **Traversability**: Check if positions and paths are safe for the robot +//! +//! ## Frontier Detection +//! +//! Frontiers are floor cells adjacent to unknown cells. They represent the +//! boundary of explored space and are used as targets for exploration. //! //! ```rust,ignore -//! use vastu_map::query::{raycast, query_occupancy, detect_frontiers}; -//! use vastu_map::query::{ClearanceVisibilityGraph, CBVGConfig}; -//! use vastu_map::core::Point2D; +//! use vastu_map::query::{FrontierDetector, Frontier}; //! -//! // Raycast to find wall distance -//! let origin = Point2D::new(0.0, 0.0); -//! let direction = Point2D::new(1.0, 0.0); -//! let distance = raycast(origin, direction, 10.0, &map_lines); +//! let detector = FrontierDetector::new(); +//! let frontiers = detector.detect_frontiers(map.storage()); //! -//! // Check if a point is free -//! let occupancy = query_occupancy(Point2D::new(1.0, 1.0), &map_lines, None, &config); +//! for frontier in frontiers { +//! println!("Frontier at {:?} with {} cells", +//! frontier.centroid_world, frontier.size); +//! } +//! ``` //! -//! // Find exploration frontiers -//! let frontiers = detect_frontiers(&map_lines, &frontier_config); +//! ## Traversability //! -//! // Plan a path using CBVG -//! let config = CBVGConfig::default(); -//! let mut graph = ClearanceVisibilityGraph::new(config); -//! graph.build(&lines, None); -//! let path = graph.find_path(start, goal, &lines); +//! Check if positions and paths are safe for robot navigation. +//! +//! ```rust,ignore +//! use vastu_map::query::{TraversabilityChecker, RobotFootprint}; +//! +//! let footprint = RobotFootprint::new(0.17, 0.05); +//! let checker = TraversabilityChecker::new(map.storage(), footprint); +//! +//! if checker.is_position_safe(target_position) { +//! // Safe to navigate to +//! } //! ``` -pub mod cbvg; pub mod frontier; -pub mod occupancy; -pub mod path_planning; -pub mod raycast; +pub mod traversability; -// Re-export main types and functions -pub use frontier::{ - FrontierConfig, FrontierDetector, cluster_centroid, cluster_frontiers, detect_frontiers, - get_best_frontier, rank_frontiers, +pub use frontier::{Frontier, FrontierCell, FrontierDetector, count_frontier_cells, has_frontiers}; + +pub use traversability::{ + PathCheckResult, RobotFootprint, TraversabilityChecker, is_safe, is_traversable, }; -pub use occupancy::{OccupancyConfig, is_path_clear, is_region_clear, query_occupancy}; -pub use path_planning::is_straight_path_clear; -pub use raycast::{RaycastResult, raycast, raycast_detailed}; - -// Re-export CBVG types -pub use cbvg::{CBVGConfig, CBVGNode, ClearanceVisibilityGraph, NodeType}; diff --git a/vastu-map/src/query/occupancy.rs b/vastu-map/src/query/occupancy.rs deleted file mode 100644 index 2b020a0..0000000 --- a/vastu-map/src/query/occupancy.rs +++ /dev/null @@ -1,315 +0,0 @@ -//! Point occupancy queries for VectorMap. -//! -//! Determines whether a point is in free space, occupied, or unknown -//! based on the map's line features. - -use serde::{Deserialize, Serialize}; - -use crate::Occupancy; -use crate::core::{Bounds, Point2D}; -use crate::features::Line2D; - -/// Configuration for occupancy queries. -#[derive(Clone, Debug, Serialize, Deserialize)] -pub struct OccupancyConfig { - /// Distance threshold for considering a point "on" a line (meters). - /// Points within this distance of a line are considered occupied. - /// Default: 0.1m - pub obstacle_distance: f32, - - /// Distance threshold for "near" checks (meters). - /// Points must be within this distance of some map feature to be known. - /// Default: 2.0m - pub known_distance: f32, -} - -impl Default for OccupancyConfig { - fn default() -> Self { - Self { - obstacle_distance: 0.1, - known_distance: 2.0, - } - } -} - -impl OccupancyConfig { - /// Create a new configuration with default values. - pub fn new() -> Self { - Self::default() - } - - /// Builder-style setter for obstacle distance. - pub fn with_obstacle_distance(mut self, meters: f32) -> Self { - self.obstacle_distance = meters; - self - } - - /// Builder-style setter for known distance. - pub fn with_known_distance(mut self, meters: f32) -> Self { - self.known_distance = meters; - self - } -} - -/// Query point occupancy. -/// -/// # Arguments -/// * `point` - Point to query -/// * `lines` - Map lines -/// * `bounds` - Map bounds (for unknown check) -/// * `config` - Occupancy configuration -/// -/// # Returns -/// Occupancy state of the point. -pub fn query_occupancy( - point: Point2D, - lines: &[Line2D], - bounds: Option<&Bounds>, - config: &OccupancyConfig, -) -> Occupancy { - // First check if point is outside map bounds - if let Some(b) = bounds - && !b.contains(point) - { - return Occupancy::Unknown; - } - - if lines.is_empty() { - return Occupancy::Unknown; - } - - // Find minimum distance to any line - let mut min_distance = f32::MAX; - let mut has_nearby_line = false; - - for line in lines { - let dist = line.distance_to_point(point); - if dist < min_distance { - min_distance = dist; - } - if dist < config.known_distance { - has_nearby_line = true; - } - } - - // Point is very close to a line - occupied - if min_distance < config.obstacle_distance { - return Occupancy::Occupied; - } - - // Point is within known region - free - if has_nearby_line { - return Occupancy::Free; - } - - // Point is far from all lines - unknown - Occupancy::Unknown -} - -/// Check if a path is collision-free. -/// -/// # Arguments -/// * `start` - Path start point -/// * `end` - Path end point -/// * `lines` - Map lines -/// * `config` - Occupancy configuration -/// -/// # Returns -/// True if the path doesn't collide with any obstacles. -pub fn is_path_clear( - start: Point2D, - end: Point2D, - lines: &[Line2D], - config: &OccupancyConfig, -) -> bool { - // Create a line segment representing the path - let path = Line2D::new(start, end); - let path_len = path.length(); - - if path_len < f32::EPSILON { - // Zero-length path - just check the point - return query_occupancy(start, lines, None, config) != Occupancy::Occupied; - } - - // Check for intersections with map lines - for line in lines { - if path.intersection(line).is_some() { - return false; - } - - // Also check if path passes too close to any line - // Sample points along the path - let num_samples = (path_len / config.obstacle_distance).ceil() as usize; - let num_samples = num_samples.max(2); - - for i in 0..=num_samples { - let t = i as f32 / num_samples as f32; - let point = path.point_at(t); - let dist = line.distance_to_point(point); - - if dist < config.obstacle_distance { - return false; - } - } - } - - true -} - -/// Check if a circular region is collision-free. -/// -/// # Arguments -/// * `center` - Circle center -/// * `radius` - Circle radius -/// * `lines` - Map lines -/// -/// # Returns -/// True if no obstacles within the circle. -pub fn is_region_clear(center: Point2D, radius: f32, lines: &[Line2D]) -> bool { - for line in lines { - let dist = line.distance_to_point(center); - if dist < radius { - return false; - } - } - true -} - -#[cfg(test)] -mod tests { - use super::*; - - fn make_room() -> Vec { - vec![ - Line2D::new(Point2D::new(-5.0, -5.0), Point2D::new(5.0, -5.0)), // Bottom - Line2D::new(Point2D::new(5.0, -5.0), Point2D::new(5.0, 5.0)), // Right - Line2D::new(Point2D::new(5.0, 5.0), Point2D::new(-5.0, 5.0)), // Top - Line2D::new(Point2D::new(-5.0, 5.0), Point2D::new(-5.0, -5.0)), // Left - ] - } - - #[test] - fn test_query_center_is_free() { - let lines = make_room(); - // Center is 5m from walls, so need larger known_distance - let config = OccupancyConfig::default().with_known_distance(6.0); - - let result = query_occupancy(Point2D::zero(), &lines, None, &config); - - assert_eq!(result, Occupancy::Free); - } - - #[test] - fn test_query_on_wall_is_occupied() { - let lines = make_room(); - let config = OccupancyConfig::default(); - - // Point on the bottom wall - let result = query_occupancy(Point2D::new(0.0, -5.0), &lines, None, &config); - - assert_eq!(result, Occupancy::Occupied); - } - - #[test] - fn test_query_near_wall_is_occupied() { - let lines = make_room(); - let config = OccupancyConfig::default().with_obstacle_distance(0.2); - - // Point very close to wall - let result = query_occupancy(Point2D::new(0.0, -4.9), &lines, None, &config); - - assert_eq!(result, Occupancy::Occupied); - } - - #[test] - fn test_query_outside_bounds_is_unknown() { - let lines = make_room(); - let bounds = Bounds::new(Point2D::new(-6.0, -6.0), Point2D::new(6.0, 6.0)); - let config = OccupancyConfig::default(); - - // Point outside bounds - let result = query_occupancy(Point2D::new(10.0, 10.0), &lines, Some(&bounds), &config); - - assert_eq!(result, Occupancy::Unknown); - } - - #[test] - fn test_query_far_from_lines_is_unknown() { - let lines = vec![Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(1.0, 0.0))]; - let config = OccupancyConfig::default().with_known_distance(1.0); - - // Point far from the only line - let result = query_occupancy(Point2D::new(10.0, 10.0), &lines, None, &config); - - assert_eq!(result, Occupancy::Unknown); - } - - #[test] - fn test_query_empty_map_is_unknown() { - let lines: Vec = vec![]; - let config = OccupancyConfig::default(); - - let result = query_occupancy(Point2D::zero(), &lines, None, &config); - - assert_eq!(result, Occupancy::Unknown); - } - - #[test] - fn test_is_path_clear_straight() { - let lines = make_room(); - let config = OccupancyConfig::default(); - - // Path through center - should be clear - let clear = is_path_clear( - Point2D::new(-2.0, 0.0), - Point2D::new(2.0, 0.0), - &lines, - &config, - ); - - assert!(clear); - } - - #[test] - fn test_is_path_blocked() { - let lines = make_room(); - let config = OccupancyConfig::default(); - - // Path through wall - should be blocked - let clear = is_path_clear( - Point2D::new(0.0, 0.0), - Point2D::new(10.0, 0.0), // Goes through right wall - &lines, - &config, - ); - - assert!(!clear); - } - - #[test] - fn test_is_path_too_close() { - let lines = vec![Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(10.0, 0.0))]; - let config = OccupancyConfig::default().with_obstacle_distance(0.2); - - // Path very close to line - let clear = is_path_clear( - Point2D::new(0.0, 0.1), - Point2D::new(10.0, 0.1), - &lines, - &config, - ); - - assert!(!clear); - } - - #[test] - fn test_is_region_clear() { - let lines = make_room(); - - // Center region should be clear - assert!(is_region_clear(Point2D::zero(), 2.0, &lines)); - - // Region touching wall should not be clear - assert!(!is_region_clear(Point2D::new(4.0, 0.0), 2.0, &lines)); - } -} diff --git a/vastu-map/src/query/path_planning.rs b/vastu-map/src/query/path_planning.rs deleted file mode 100644 index 9b13de7..0000000 --- a/vastu-map/src/query/path_planning.rs +++ /dev/null @@ -1,138 +0,0 @@ -//! Path clearance checking utilities. -//! -//! Provides functions to check if a straight-line path is clear of obstacles. - -use crate::core::Point2D; -use crate::features::Line2D; - -/// Check if a straight-line path between two points is clear of obstacles. -/// -/// This performs collision checking by: -/// 1. Checking for direct line-line intersections -/// 2. Sampling points along the path and checking clearance -/// -/// # Arguments -/// * `from` - Start position -/// * `to` - End position -/// * `lines` - Map lines (obstacles) -/// * `robot_radius` - Robot radius for clearance -/// -/// # Returns -/// True if the straight path is clear (no obstacles within robot_radius). -/// -/// # Example -/// -/// ```rust,ignore -/// use vastu_map::query::is_straight_path_clear; -/// use vastu_map::core::Point2D; -/// -/// let clear = is_straight_path_clear( -/// Point2D::new(0.0, 0.0), -/// Point2D::new(1.0, 1.0), -/// &walls, -/// 0.15, // robot radius -/// ); -/// ``` -pub fn is_straight_path_clear( - from: Point2D, - to: Point2D, - lines: &[Line2D], - robot_radius: f32, -) -> bool { - let test_line = Line2D::new(from, to); - let test_length = test_line.length(); - - if test_length < 0.001 { - return true; // Zero-length path is always clear - } - - for line in lines { - // Check for direct intersection - if test_line.intersection(line).is_some() { - return false; - } - - // Check clearance along the path (use segment distance, not infinite line) - if robot_radius > 0.0 { - let num_samples = (test_length / robot_radius).ceil() as usize; - let num_samples = num_samples.max(3); - - for k in 0..=num_samples { - let t = k as f32 / num_samples as f32; - let sample = test_line.point_at(t); - // Use distance_to_point_segment for accurate segment distance - let dist = line.distance_to_point_segment(sample); - - if dist < robot_radius { - return false; - } - } - } - } - - true -} - -#[cfg(test)] -mod tests { - use super::*; - - #[test] - fn test_is_straight_path_clear_empty() { - let lines: Vec = vec![]; - assert!(is_straight_path_clear( - Point2D::new(0.0, 0.0), - Point2D::new(1.0, 1.0), - &lines, - 0.1 - )); - } - - #[test] - fn test_is_straight_path_clear_blocked() { - let lines = vec![Line2D::new(Point2D::new(0.5, -1.0), Point2D::new(0.5, 1.0))]; - assert!(!is_straight_path_clear( - Point2D::new(0.0, 0.0), - Point2D::new(1.0, 0.0), - &lines, - 0.0 - )); - } - - #[test] - fn test_is_straight_path_clear_with_radius() { - // Path goes close to wall but doesn't intersect - let lines = vec![Line2D::new(Point2D::new(0.5, 0.15), Point2D::new(0.5, 1.0))]; - // With radius 0.2, should be blocked (too close) - assert!(!is_straight_path_clear( - Point2D::new(0.0, 0.0), - Point2D::new(1.0, 0.0), - &lines, - 0.2 - )); - } - - #[test] - fn test_is_straight_path_clear_with_clearance() { - // Wall is far enough away - let lines = vec![Line2D::new(Point2D::new(0.5, 0.5), Point2D::new(0.5, 1.0))]; - assert!(is_straight_path_clear( - Point2D::new(0.0, 0.0), - Point2D::new(1.0, 0.0), - &lines, - 0.1 - )); - } - - #[test] - fn test_zero_length_path() { - let lines = vec![Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(1.0, 0.0))]; - // Zero-length path is always clear - assert!(is_straight_path_clear( - Point2D::new(0.0, 0.0), - Point2D::new(0.0, 0.0), - &lines, - 0.1 - )); - } -} diff --git a/vastu-map/src/query/raycast.rs b/vastu-map/src/query/raycast.rs deleted file mode 100644 index f48ae38..0000000 --- a/vastu-map/src/query/raycast.rs +++ /dev/null @@ -1,218 +0,0 @@ -//! Ray casting for obstacle detection. -//! -//! Computes the distance from a point in a given direction to the -//! first obstacle (line segment) in the map. - -use crate::core::Point2D; -use crate::features::Line2D; - -/// Result of a raycast operation. -#[derive(Clone, Debug)] -pub struct RaycastResult { - /// Distance to the hit point, or max_range if no hit. - pub distance: f32, - /// Index of the line that was hit, if any. - pub line_idx: Option, - /// The hit point in world coordinates. - pub hit_point: Option, - /// Whether a hit occurred. - pub hit: bool, -} - -impl RaycastResult { - /// Create a result indicating no hit. - pub fn miss(max_range: f32) -> Self { - Self { - distance: max_range, - line_idx: None, - hit_point: None, - hit: false, - } - } - - /// Create a result indicating a hit. - pub fn hit(distance: f32, line_idx: usize, hit_point: Point2D) -> Self { - Self { - distance, - line_idx: Some(line_idx), - hit_point: Some(hit_point), - hit: true, - } - } -} - -/// Cast a ray and find the first intersection with map lines. -/// -/// # Arguments -/// * `origin` - Ray origin in world frame -/// * `direction` - Ray direction (will be normalized) -/// * `max_range` - Maximum ray distance -/// * `lines` - Map line segments -/// -/// # Returns -/// Distance to first intersection, or max_range if none. -pub fn raycast(origin: Point2D, direction: Point2D, max_range: f32, lines: &[Line2D]) -> f32 { - raycast_detailed(origin, direction, max_range, lines).distance -} - -/// Cast a ray with detailed hit information. -/// -/// # Arguments -/// * `origin` - Ray origin in world frame -/// * `direction` - Ray direction (will be normalized) -/// * `max_range` - Maximum ray distance -/// * `lines` - Map line segments -/// -/// # Returns -/// Detailed raycast result. -pub fn raycast_detailed( - origin: Point2D, - direction: Point2D, - max_range: f32, - lines: &[Line2D], -) -> RaycastResult { - if lines.is_empty() { - return RaycastResult::miss(max_range); - } - - let dir = direction.normalized(); - let mut closest_dist = max_range; - let mut closest_line: Option = None; - let mut closest_point: Option = None; - - for (idx, line) in lines.iter().enumerate() { - if let Some(t) = line.ray_intersection(origin, dir) - && t > 0.0 - && t < closest_dist - { - closest_dist = t; - closest_line = Some(idx); - closest_point = Some(origin + dir * t); - } - } - - match closest_line { - Some(idx) => RaycastResult::hit(closest_dist, idx, closest_point.unwrap()), - None => RaycastResult::miss(max_range), - } -} - -#[cfg(test)] -mod tests { - use super::*; - use crate::core::Point2D; - use approx::assert_relative_eq; - - fn make_room() -> Vec { - vec![ - Line2D::new(Point2D::new(-5.0, -5.0), Point2D::new(5.0, -5.0)), // Bottom - Line2D::new(Point2D::new(5.0, -5.0), Point2D::new(5.0, 5.0)), // Right - Line2D::new(Point2D::new(5.0, 5.0), Point2D::new(-5.0, 5.0)), // Top - Line2D::new(Point2D::new(-5.0, 5.0), Point2D::new(-5.0, -5.0)), // Left - ] - } - - #[test] - fn test_raycast_hit() { - let lines = make_room(); - let origin = Point2D::zero(); - let direction = Point2D::new(1.0, 0.0); // East - - let dist = raycast(origin, direction, 100.0, &lines); - - // Should hit right wall at x=5 - assert_relative_eq!(dist, 5.0, epsilon = 0.01); - } - - #[test] - fn test_raycast_miss() { - let lines = vec![Line2D::new( - Point2D::new(10.0, 0.0), - Point2D::new(10.0, 10.0), - )]; - let origin = Point2D::zero(); - let direction = Point2D::new(-1.0, 0.0); // West (away from line) - - let dist = raycast(origin, direction, 100.0, &lines); - - assert_eq!(dist, 100.0); // Max range - } - - #[test] - fn test_raycast_detailed() { - let lines = make_room(); - let origin = Point2D::zero(); - let direction = Point2D::new(0.0, -1.0); // South - - let result = raycast_detailed(origin, direction, 100.0, &lines); - - assert!(result.hit); - assert_relative_eq!(result.distance, 5.0, epsilon = 0.01); - assert_eq!(result.line_idx, Some(0)); // Bottom wall - assert!(result.hit_point.is_some()); - assert_relative_eq!(result.hit_point.unwrap().y, -5.0, epsilon = 0.01); - } - - #[test] - fn test_raycast_diagonal() { - let lines = make_room(); - let origin = Point2D::zero(); - let direction = Point2D::new(1.0, 1.0).normalized(); // Northeast - - let dist = raycast(origin, direction, 100.0, &lines); - - // Should hit corner at (5, 5), distance = sqrt(50) ≈ 7.07 - assert_relative_eq!(dist, 5.0 * 2.0_f32.sqrt(), epsilon = 0.1); - } - - #[test] - fn test_raycast_empty_lines() { - let lines: Vec = vec![]; - let origin = Point2D::zero(); - let direction = Point2D::new(1.0, 0.0); - - let dist = raycast(origin, direction, 10.0, &lines); - - assert_eq!(dist, 10.0); - } - - #[test] - fn test_raycast_from_edge() { - let lines = make_room(); - // Origin near left wall - let origin = Point2D::new(-4.9, 0.0); - let direction = Point2D::new(-1.0, 0.0); // Toward wall - - let dist = raycast(origin, direction, 100.0, &lines); - - // Should hit wall very close - assert_relative_eq!(dist, 0.1, epsilon = 0.01); - } - - #[test] - fn test_raycast_parallel_to_wall() { - let lines = vec![Line2D::new(Point2D::new(0.0, 5.0), Point2D::new(10.0, 5.0))]; - let origin = Point2D::zero(); - let direction = Point2D::new(1.0, 0.0); // Parallel to line - - let dist = raycast(origin, direction, 100.0, &lines); - - // Should not hit (parallel) - assert_eq!(dist, 100.0); - } - - #[test] - fn test_raycast_behind_origin() { - let lines = vec![Line2D::new( - Point2D::new(-5.0, -1.0), - Point2D::new(-5.0, 1.0), - )]; - let origin = Point2D::zero(); - let direction = Point2D::new(1.0, 0.0); // Away from line - - let dist = raycast(origin, direction, 100.0, &lines); - - // Line is behind, should not hit - assert_eq!(dist, 100.0); - } -} diff --git a/vastu-map/src/query/traversability.rs b/vastu-map/src/query/traversability.rs new file mode 100644 index 0000000..514be4f --- /dev/null +++ b/vastu-map/src/query/traversability.rs @@ -0,0 +1,911 @@ +//! Traversability queries for path planning and navigation. +//! +//! Provides methods to check if positions and paths are safe for robot traversal. + +use serde::{Deserialize, Serialize}; +use std::simd::{cmp::SimdPartialOrd, u8x16}; + +use crate::core::{CellType, GridCoord, WorldPoint}; +use crate::grid::GridStorage; + +/// Robot footprint for collision checking +#[derive(Clone, Debug, Serialize, Deserialize)] +pub struct RobotFootprint { + /// Robot radius in meters + pub radius: f32, + /// Safety margin in meters + pub safety_margin: f32, +} + +impl Default for RobotFootprint { + fn default() -> Self { + Self { + radius: 0.17, // CRL-200S robot radius + safety_margin: 0.05, // 5cm safety margin + } + } +} + +impl RobotFootprint { + /// Create a new robot footprint + pub fn new(radius: f32, safety_margin: f32) -> Self { + Self { + radius, + safety_margin, + } + } + + /// Total radius including safety margin + pub fn total_radius(&self) -> f32 { + self.radius + self.safety_margin + } +} + +/// Traversability checker +pub struct TraversabilityChecker<'a> { + storage: &'a GridStorage, + footprint: RobotFootprint, +} + +impl<'a> TraversabilityChecker<'a> { + /// Create a new traversability checker + pub fn new(storage: &'a GridStorage, footprint: RobotFootprint) -> Self { + Self { storage, footprint } + } + + /// Create with default robot footprint + pub fn with_default_footprint(storage: &'a GridStorage) -> Self { + Self::new(storage, RobotFootprint::default()) + } + + /// Check if a single cell is traversable + pub fn is_cell_traversable(&self, coord: GridCoord) -> bool { + if !self.storage.is_valid_coord(coord) { + return false; + } + self.storage.get_type(coord).is_traversable() + } + + /// Check if a world point is traversable (single cell) + pub fn is_point_traversable(&self, point: WorldPoint) -> bool { + let coord = self.storage.world_to_grid(point); + self.is_cell_traversable(coord) + } + + /// Check if a position is safe for the robot (considering footprint) + pub fn is_position_safe(&self, position: WorldPoint) -> bool { + self.is_position_safe_mode(position, false) + } + + /// Check if a position is safe, with option to allow Unknown cells (SIMD-optimized). + /// + /// In exploration mode, Unknown cells are allowed in the footprint + /// (they're assumed to be likely free). Only actual obstacles block. + /// + /// Uses SIMD to quickly check the bounding box for obstacles before + /// performing the circular footprint check. + pub fn is_position_safe_mode(&self, position: WorldPoint, allow_unknown: bool) -> bool { + let center = self.storage.world_to_grid(position); + let resolution = self.storage.resolution(); + let cells_radius = (self.footprint.total_radius() / resolution).ceil() as i32; + + // Fast path: SIMD check for obstacles in bounding box + if !self.is_rect_obstacle_free(center, cells_radius, cells_radius) { + return false; + } + + // If allow_unknown is true, we're done (no obstacles found via SIMD) + if allow_unknown { + // Still need to check bounds validity + return self.is_footprint_in_bounds(center, cells_radius, position); + } + + // Strict mode: Need to check for Unknown cells in circular footprint + self.is_footprint_traversable(center, cells_radius, position) + } + + /// Check if a rectangular region contains any obstacle cells (SIMD-optimized). + /// + /// Obstacles are Wall, Cliff, or Bump (cell_type >= 2). + /// Returns true if no obstacles found. + #[inline] + fn is_rect_obstacle_free(&self, center: GridCoord, half_width: i32, half_height: i32) -> bool { + let width = self.storage.width() as i32; + let height = self.storage.height() as i32; + + let min_x = (center.x - half_width).max(0) as usize; + let max_x = ((center.x + half_width).min(width - 1) as usize).min(self.storage.width() - 1); + let min_y = (center.y - half_height).max(0) as usize; + let max_y = + ((center.y + half_height).min(height - 1) as usize).min(self.storage.height() - 1); + + if min_x > max_x || min_y > max_y { + return false; // Out of bounds + } + + let cell_types = self.storage.cell_types_raw(); + let storage_width = self.storage.width(); + + // Wall, Cliff, Bump are >= 2 (Floor=1, Unknown=0) + let wall_val = CellType::Wall as u8; + let wall_vec = u8x16::splat(wall_val); + + for y in min_y..=max_y { + let row_start = y * storage_width + min_x; + let row_end = y * storage_width + max_x + 1; + let row_slice = &cell_types[row_start..row_end]; + + // Process 16 cells at a time + let chunks = row_slice.chunks_exact(16); + let remainder = chunks.remainder(); + + for chunk in chunks { + let data = u8x16::from_slice(chunk); + // Check if any cell >= Wall (2) + let is_obstacle = data.simd_ge(wall_vec); + if is_obstacle.to_bitmask() != 0 { + return false; + } + } + + // Scalar remainder + for &cell_type in remainder { + if cell_type >= wall_val { + return false; + } + } + } + + true + } + + /// Check if all cells in the circular footprint are within bounds. + #[inline] + fn is_footprint_in_bounds( + &self, + center: GridCoord, + cells_radius: i32, + position: WorldPoint, + ) -> bool { + for dy in -cells_radius..=cells_radius { + for dx in -cells_radius..=cells_radius { + let coord = center + GridCoord::new(dx, dy); + + if !self.storage.is_valid_coord(coord) { + // Check if this cell is within the circular footprint + let cell_world = self.storage.grid_to_world(coord); + let dist = cell_world.distance(&position); + if dist <= self.footprint.total_radius() { + return false; // Out of bounds within footprint + } + } + } + } + true + } + + /// Check if all cells in the circular footprint are traversable (Floor). + #[inline] + fn is_footprint_traversable( + &self, + center: GridCoord, + cells_radius: i32, + position: WorldPoint, + ) -> bool { + for dy in -cells_radius..=cells_radius { + for dx in -cells_radius..=cells_radius { + let coord = center + GridCoord::new(dx, dy); + + if !self.storage.is_valid_coord(coord) { + // Check if this cell is within the circular footprint + let cell_world = self.storage.grid_to_world(coord); + let dist = cell_world.distance(&position); + if dist <= self.footprint.total_radius() { + return false; // Out of bounds within footprint + } + continue; + } + + // Check if within circular footprint + let cell_world = self.storage.grid_to_world(coord); + let dist = cell_world.distance(&position); + + if dist <= self.footprint.total_radius() { + let cell_type = self.storage.get_type(coord); + if !cell_type.is_traversable() { + return false; + } + } + } + } + true + } + + /// Check if a position is known (all cells within footprint are known) + pub fn is_position_known(&self, position: WorldPoint) -> bool { + let center = self.storage.world_to_grid(position); + let resolution = self.storage.resolution(); + let cells_radius = (self.footprint.total_radius() / resolution).ceil() as i32; + + for dy in -cells_radius..=cells_radius { + for dx in -cells_radius..=cells_radius { + let coord = center + GridCoord::new(dx, dy); + + if !self.storage.is_valid_coord(coord) { + return false; + } + + let cell_world = self.storage.grid_to_world(coord); + let dist = cell_world.distance(&position); + + if dist <= self.footprint.total_radius() && !self.storage.get_type(coord).is_known() + { + return false; + } + } + } + + true + } + + /// Check if a straight-line path is traversable + /// Returns the furthest traversable point and whether the full path is clear + pub fn check_path( + &self, + start: WorldPoint, + end: WorldPoint, + step_size: Option, + ) -> PathCheckResult { + let step = step_size.unwrap_or(self.storage.resolution() * 0.5); + let total_dist = start.distance(&end); + + if total_dist < step { + // Very short path - just check endpoints + let start_safe = self.is_position_safe(start); + let end_safe = self.is_position_safe(end); + + return PathCheckResult { + is_clear: start_safe && end_safe, + blocked_at: if !start_safe { + Some(start) + } else if !end_safe { + Some(end) + } else { + None + }, + furthest_safe: if start_safe { Some(end) } else { None }, + distance_traversable: if start_safe && end_safe { + total_dist + } else { + 0.0 + }, + }; + } + + let dir_x = (end.x - start.x) / total_dist; + let dir_y = (end.y - start.y) / total_dist; + + let mut furthest_safe = start; + let mut distance_traversable = 0.0; + + // Check start position first + if !self.is_position_safe(start) { + return PathCheckResult { + is_clear: false, + blocked_at: Some(start), + furthest_safe: None, + distance_traversable: 0.0, + }; + } + + let mut dist = step; + while dist < total_dist { + let point = WorldPoint::new(start.x + dir_x * dist, start.y + dir_y * dist); + + if self.is_position_safe(point) { + furthest_safe = point; + distance_traversable = dist; + } else { + return PathCheckResult { + is_clear: false, + blocked_at: Some(point), + furthest_safe: Some(furthest_safe), + distance_traversable, + }; + } + + dist += step; + } + + // Check endpoint + if self.is_position_safe(end) { + PathCheckResult { + is_clear: true, + blocked_at: None, + furthest_safe: Some(end), + distance_traversable: total_dist, + } + } else { + PathCheckResult { + is_clear: false, + blocked_at: Some(end), + furthest_safe: Some(furthest_safe), + distance_traversable, + } + } + } + + /// Check if movement from current position in a direction is safe + pub fn check_direction( + &self, + position: WorldPoint, + angle: f32, + distance: f32, + ) -> PathCheckResult { + let end = WorldPoint::new( + position.x + distance * angle.cos(), + position.y + distance * angle.sin(), + ); + self.check_path(position, end, None) + } + + /// Find the nearest obstacle from a position + pub fn nearest_obstacle(&self, position: WorldPoint, max_distance: f32) -> Option { + let center = self.storage.world_to_grid(position); + let resolution = self.storage.resolution(); + let max_cells = (max_distance / resolution).ceil() as i32; + + let mut nearest: Option<(WorldPoint, f32)> = None; + + for dy in -max_cells..=max_cells { + for dx in -max_cells..=max_cells { + let coord = center + GridCoord::new(dx, dy); + + if !self.storage.is_valid_coord(coord) { + continue; + } + + let cell_type = self.storage.get_type(coord); + if cell_type.is_obstacle() { + let obstacle_pos = self.storage.grid_to_world(coord); + let dist = position.distance(&obstacle_pos); + + if dist <= max_distance { + match nearest { + None => nearest = Some((obstacle_pos, dist)), + Some((_, d)) if dist < d => nearest = Some((obstacle_pos, dist)), + _ => {} + } + } + } + } + } + + nearest.map(|(p, _)| p) + } + + /// Get clearance (distance to nearest obstacle) at a position + pub fn clearance(&self, position: WorldPoint, max_distance: f32) -> f32 { + match self.nearest_obstacle(position, max_distance) { + Some(obstacle) => position.distance(&obstacle), + None => max_distance, + } + } +} + +/// Result of path traversability check +#[derive(Clone, Debug)] +pub struct PathCheckResult { + /// Is the entire path clear? + pub is_clear: bool, + /// Position where the path is blocked (if blocked) + pub blocked_at: Option, + /// Furthest safe position along the path + pub furthest_safe: Option, + /// Distance that is traversable + pub distance_traversable: f32, +} + +impl PathCheckResult { + /// Percentage of path that is traversable (0.0 to 1.0) + pub fn traversable_ratio(&self, total_length: f32) -> f32 { + if total_length <= 0.0 { + return 0.0; + } + (self.distance_traversable / total_length).min(1.0) + } +} + +/// Quick check if a point is traversable +pub fn is_traversable(storage: &GridStorage, point: WorldPoint) -> bool { + let checker = TraversabilityChecker::with_default_footprint(storage); + checker.is_point_traversable(point) +} + +/// Quick check if a point is safe for the robot +pub fn is_safe(storage: &GridStorage, point: WorldPoint) -> bool { + let checker = TraversabilityChecker::with_default_footprint(storage); + checker.is_position_safe(point) +} + +#[cfg(test)] +mod tests { + use super::*; + use crate::core::CellType; + use std::simd::{cmp::SimdPartialOrd, u8x16}; + + fn create_test_storage() -> GridStorage { + GridStorage::centered(100, 100, 0.05) // 5m x 5m at 5cm resolution + } + + // ========================================================================= + // SIMD-OPTIMIZED TEST HELPERS + // These are test-only implementations to verify SIMD correctness + // ========================================================================= + + /// Check if a rectangular region contains any obstacle cells (SIMD-optimized). + fn is_rect_obstacle_free( + checker: &TraversabilityChecker, + center: GridCoord, + half_width: i32, + half_height: i32, + ) -> bool { + let width = checker.storage.width() as i32; + let height = checker.storage.height() as i32; + + let min_x = (center.x - half_width).max(0) as usize; + let max_x = + ((center.x + half_width).min(width - 1) as usize).min(checker.storage.width() - 1); + let min_y = (center.y - half_height).max(0) as usize; + let max_y = + ((center.y + half_height).min(height - 1) as usize).min(checker.storage.height() - 1); + + if min_x > max_x || min_y > max_y { + return false; // Out of bounds + } + + let cell_types = checker.storage.cell_types_raw(); + let storage_width = checker.storage.width(); + + let wall_val = CellType::Wall as u8; + let wall_vec = u8x16::splat(wall_val); + + for y in min_y..=max_y { + let row_start = y * storage_width + min_x; + let row_end = y * storage_width + max_x + 1; + let row_slice = &cell_types[row_start..row_end]; + + // Process 16 cells at a time + let chunks = row_slice.chunks_exact(16); + let remainder = chunks.remainder(); + + for chunk in chunks { + let data = u8x16::from_slice(chunk); + let is_obstacle = data.simd_ge(wall_vec); + if is_obstacle.to_bitmask() != 0 { + return false; + } + } + + for &cell_type in remainder { + if cell_type >= wall_val { + return false; + } + } + } + + true + } + + /// Check if a position is safe using SIMD for bounding box pre-check. + fn is_position_safe_simd(checker: &TraversabilityChecker, position: WorldPoint) -> bool { + let center = checker.storage.world_to_grid(position); + let resolution = checker.storage.resolution(); + let cells_radius = (checker.footprint.total_radius() / resolution).ceil() as i32; + + if !is_rect_obstacle_free(checker, center, cells_radius, cells_radius) { + return false; + } + + is_position_safe_check_unknown(checker, position, center, cells_radius) + } + + /// Check for Unknown cells within the footprint (helper for SIMD version) + fn is_position_safe_check_unknown( + checker: &TraversabilityChecker, + position: WorldPoint, + center: GridCoord, + cells_radius: i32, + ) -> bool { + for dy in -cells_radius..=cells_radius { + for dx in -cells_radius..=cells_radius { + let coord = center + GridCoord::new(dx, dy); + + if !checker.storage.is_valid_coord(coord) { + return false; + } + + let cell_world = checker.storage.grid_to_world(coord); + let dist = cell_world.distance(&position); + + if dist <= checker.footprint.total_radius() { + let cell_type = checker.storage.get_type(coord); + if !cell_type.is_traversable() { + return false; + } + } + } + } + + true + } + + /// Find nearest obstacle using SIMD for batch distance calculations. + fn nearest_obstacle_simd( + checker: &TraversabilityChecker, + position: WorldPoint, + max_distance: f32, + ) -> Option { + let center = checker.storage.world_to_grid(position); + let resolution = checker.storage.resolution(); + let max_cells = (max_distance / resolution).ceil() as i32; + + let width = checker.storage.width() as i32; + let height = checker.storage.height() as i32; + + let cell_types = checker.storage.cell_types_raw(); + let storage_width = checker.storage.width(); + + let wall_val = CellType::Wall as u8; + let wall_vec = u8x16::splat(wall_val); + + let mut nearest: Option<(WorldPoint, f32)> = None; + + let min_y = (center.y - max_cells).max(0) as usize; + let max_y = + ((center.y + max_cells).min(height - 1) as usize).min(checker.storage.height() - 1); + let min_x = (center.x - max_cells).max(0) as usize; + let max_x = + ((center.x + max_cells).min(width - 1) as usize).min(checker.storage.width() - 1); + + for y in min_y..=max_y { + let row_start = y * storage_width + min_x; + let row_end = y * storage_width + max_x + 1; + let row_slice = &cell_types[row_start..row_end]; + + let chunks = row_slice.chunks_exact(16); + let remainder = chunks.remainder(); + + for (chunk_idx, chunk) in chunks.enumerate() { + let data = u8x16::from_slice(chunk); + let is_obstacle = data.simd_ge(wall_vec); + let mask = is_obstacle.to_bitmask(); + + if mask != 0 { + let base_x = min_x + chunk_idx * 16; + for bit in 0..16 { + if (mask & (1 << bit)) != 0 { + let x = base_x + bit; + let coord = GridCoord::new(x as i32, y as i32); + let obstacle_pos = checker.storage.grid_to_world(coord); + let dist = position.distance(&obstacle_pos); + + if dist <= max_distance { + match nearest { + None => nearest = Some((obstacle_pos, dist)), + Some((_, d)) if dist < d => { + nearest = Some((obstacle_pos, dist)) + } + _ => {} + } + } + } + } + } + } + + let remainder_start = min_x + (row_slice.len() / 16) * 16; + for (i, &cell_type) in remainder.iter().enumerate() { + if cell_type >= wall_val { + let x = remainder_start + i; + let coord = GridCoord::new(x as i32, y as i32); + let obstacle_pos = checker.storage.grid_to_world(coord); + let dist = position.distance(&obstacle_pos); + + if dist <= max_distance { + match nearest { + None => nearest = Some((obstacle_pos, dist)), + Some((_, d)) if dist < d => nearest = Some((obstacle_pos, dist)), + _ => {} + } + } + } + } + } + + nearest.map(|(p, _)| p) + } + + #[test] + fn test_traversable_unknown() { + let storage = create_test_storage(); + let checker = TraversabilityChecker::with_default_footprint(&storage); + + // Unknown cells are not traversable + assert!(!checker.is_point_traversable(WorldPoint::new(0.0, 0.0))); + } + + #[test] + fn test_traversable_floor() { + let mut storage = create_test_storage(); + + // Mark center as floor + let center = storage.world_to_grid(WorldPoint::new(0.0, 0.0)); + storage.set_type(center, CellType::Floor); + + let checker = TraversabilityChecker::with_default_footprint(&storage); + + // Floor is traversable + assert!(checker.is_cell_traversable(center)); + } + + #[test] + fn test_position_safe_requires_footprint() { + let mut storage = create_test_storage(); + let center = WorldPoint::new(0.0, 0.0); + + // Mark a large floor area + for dx in -10..=10 { + for dy in -10..=10 { + let coord = storage.world_to_grid(center) + GridCoord::new(dx, dy); + storage.set_type(coord, CellType::Floor); + } + } + + let footprint = RobotFootprint::new(0.1, 0.05); + let checker = TraversabilityChecker::new(&storage, footprint); + + // Position should be safe with floor all around + assert!(checker.is_position_safe(center)); + } + + #[test] + fn test_position_unsafe_near_wall() { + let mut storage = create_test_storage(); + let center = WorldPoint::new(0.0, 0.0); + + // Mark floor area + let center_coord = storage.world_to_grid(center); + for dx in -10..=10 { + for dy in -10..=10 { + let coord = center_coord + GridCoord::new(dx, dy); + storage.set_type(coord, CellType::Floor); + } + } + + // Put a wall very close (within robot radius) + let wall_coord = center_coord + GridCoord::new(2, 0); // ~10cm away + storage.set_type_with_priority(wall_coord, CellType::Wall); + + let footprint = RobotFootprint::new(0.17, 0.05); // ~22cm total radius + let checker = TraversabilityChecker::new(&storage, footprint); + + // Position should be unsafe due to wall within footprint + assert!(!checker.is_position_safe(center)); + } + + #[test] + fn test_path_check_clear() { + let mut storage = create_test_storage(); + + // Create a floor corridor + for x in -20..=20 { + for y in -5..=5 { + let coord = storage.world_to_grid(WorldPoint::ZERO) + GridCoord::new(x, y); + storage.set_type(coord, CellType::Floor); + } + } + + let footprint = RobotFootprint::new(0.1, 0.02); + let checker = TraversabilityChecker::new(&storage, footprint); + + let start = WorldPoint::new(-0.5, 0.0); + let end = WorldPoint::new(0.5, 0.0); + + let result = checker.check_path(start, end, None); + + assert!(result.is_clear); + assert!(result.blocked_at.is_none()); + } + + #[test] + fn test_path_check_blocked() { + let mut storage = create_test_storage(); + + // Create floor + for x in -20..=20 { + for y in -5..=5 { + let coord = storage.world_to_grid(WorldPoint::ZERO) + GridCoord::new(x, y); + storage.set_type(coord, CellType::Floor); + } + } + + // Add wall in the middle + let wall_coord = storage.world_to_grid(WorldPoint::new(0.0, 0.0)); + storage.set_type_with_priority(wall_coord, CellType::Wall); + + let footprint = RobotFootprint::new(0.1, 0.02); + let checker = TraversabilityChecker::new(&storage, footprint); + + let start = WorldPoint::new(-0.5, 0.0); + let end = WorldPoint::new(0.5, 0.0); + + let result = checker.check_path(start, end, None); + + assert!(!result.is_clear); + assert!(result.blocked_at.is_some()); + assert!(result.furthest_safe.is_some()); + } + + #[test] + fn test_clearance() { + let mut storage = create_test_storage(); + + // Create floor + for x in -20..=20 { + for y in -10..=10 { + let coord = storage.world_to_grid(WorldPoint::ZERO) + GridCoord::new(x, y); + storage.set_type(coord, CellType::Floor); + } + } + + // Add wall + let wall_pos = WorldPoint::new(0.3, 0.0); + let wall_coord = storage.world_to_grid(wall_pos); + storage.set_type_with_priority(wall_coord, CellType::Wall); + + let checker = TraversabilityChecker::with_default_footprint(&storage); + let robot_pos = WorldPoint::new(0.0, 0.0); + + let clearance = checker.clearance(robot_pos, 1.0); + + // Clearance should be approximately the distance to the wall + assert!(clearance > 0.0); + assert!(clearance < 0.5); + } + + #[test] + fn test_nearest_obstacle() { + let mut storage = create_test_storage(); + + // Create floor + for x in -20..=20 { + for y in -20..=20 { + let coord = storage.world_to_grid(WorldPoint::ZERO) + GridCoord::new(x, y); + storage.set_type(coord, CellType::Floor); + } + } + + // Add walls + let wall1_pos = WorldPoint::new(0.2, 0.0); + let wall2_pos = WorldPoint::new(-0.5, 0.0); + storage.set_type_with_priority(storage.world_to_grid(wall1_pos), CellType::Wall); + storage.set_type_with_priority(storage.world_to_grid(wall2_pos), CellType::Wall); + + let checker = TraversabilityChecker::with_default_footprint(&storage); + let robot_pos = WorldPoint::new(0.0, 0.0); + + let nearest = checker.nearest_obstacle(robot_pos, 1.0); + + assert!(nearest.is_some()); + // Nearest should be the closer wall + let obstacle = nearest.unwrap(); + assert!(obstacle.distance(&robot_pos) < 0.3); + } + + #[test] + fn test_simd_position_safe_matches_scalar() { + let mut storage = create_test_storage(); + let center = WorldPoint::new(0.0, 0.0); + + // Mark a large floor area + for dx in -10..=10 { + for dy in -10..=10 { + let coord = storage.world_to_grid(center) + GridCoord::new(dx, dy); + storage.set_type(coord, CellType::Floor); + } + } + + let footprint = RobotFootprint::new(0.1, 0.05); + let checker = TraversabilityChecker::new(&storage, footprint); + + // Test positions + let positions = [ + WorldPoint::new(0.0, 0.0), + WorldPoint::new(0.1, 0.1), + WorldPoint::new(-0.1, 0.1), + ]; + + for pos in positions { + let scalar = checker.is_position_safe(pos); + let simd = is_position_safe_simd(&checker, pos); + assert_eq!(scalar, simd, "Mismatch at {:?}", pos); + } + } + + #[test] + fn test_simd_rect_obstacle_free() { + let mut storage = create_test_storage(); + let center = storage.world_to_grid(WorldPoint::ZERO); + + // Mark floor area + for dx in -10..=10 { + for dy in -10..=10 { + let coord = center + GridCoord::new(dx, dy); + storage.set_type(coord, CellType::Floor); + } + } + + // First check: should be obstacle-free + { + let checker = TraversabilityChecker::with_default_footprint(&storage); + assert!(is_rect_obstacle_free(&checker, center, 5, 5)); + } + + // Add a wall + storage.set_type_with_priority(center + GridCoord::new(2, 2), CellType::Wall); + + // Second check: should now detect obstacle + { + let checker = TraversabilityChecker::with_default_footprint(&storage); + assert!(!is_rect_obstacle_free(&checker, center, 5, 5)); + + // But smaller rect not including wall should be clear + assert!(is_rect_obstacle_free( + &checker, + center + GridCoord::new(-5, 0), + 2, + 2 + )); + } + } + + #[test] + fn test_simd_nearest_obstacle_matches_scalar() { + let mut storage = create_test_storage(); + + // Create floor + for x in -20..=20 { + for y in -20..=20 { + let coord = storage.world_to_grid(WorldPoint::ZERO) + GridCoord::new(x, y); + storage.set_type(coord, CellType::Floor); + } + } + + // Add walls + let wall1_pos = WorldPoint::new(0.2, 0.0); + let wall2_pos = WorldPoint::new(-0.5, 0.0); + storage.set_type_with_priority(storage.world_to_grid(wall1_pos), CellType::Wall); + storage.set_type_with_priority(storage.world_to_grid(wall2_pos), CellType::Wall); + + let checker = TraversabilityChecker::with_default_footprint(&storage); + let robot_pos = WorldPoint::new(0.0, 0.0); + + let scalar_nearest = checker.nearest_obstacle(robot_pos, 1.0); + let simd_nearest = nearest_obstacle_simd(&checker, robot_pos, 1.0); + + assert_eq!(scalar_nearest.is_some(), simd_nearest.is_some()); + + if let (Some(s), Some(simd)) = (scalar_nearest, simd_nearest) { + // Should find the same nearest obstacle (within grid resolution tolerance) + let scalar_dist = s.distance(&robot_pos); + let simd_dist = simd.distance(&robot_pos); + assert!( + (scalar_dist - simd_dist).abs() < storage.resolution(), + "Distance mismatch: scalar={}, simd={}", + scalar_dist, + simd_dist + ); + } + } +} diff --git a/vastu-map/src/slam/config.rs b/vastu-map/src/slam/config.rs new file mode 100644 index 0000000..654973f --- /dev/null +++ b/vastu-map/src/slam/config.rs @@ -0,0 +1,264 @@ +//! SLAM configuration types. + +use serde::{Deserialize, Serialize}; + +/// Configuration for the correlative scan matcher. +/// +/// The correlative matcher performs a brute-force search over a 3D space +/// (x, y, theta) to find the best alignment of a lidar scan to the current map. +#[derive(Clone, Debug, Serialize, Deserialize)] +pub struct CorrelativeMatcherConfig { + /// Search window in X direction (meters). + /// The matcher will search from -search_x to +search_x relative to prior pose. + #[serde(default = "default_search_xy")] + pub search_x: f32, + + /// Search window in Y direction (meters). + /// The matcher will search from -search_y to +search_y relative to prior pose. + #[serde(default = "default_search_xy")] + pub search_y: f32, + + /// Search window in theta (radians). + /// The matcher will search from -search_theta to +search_theta relative to prior pose. + #[serde(default = "default_search_theta")] + pub search_theta: f32, + + /// Linear search resolution (meters). + /// Step size for X and Y search dimensions. + #[serde(default = "default_linear_resolution")] + pub linear_resolution: f32, + + /// Angular search resolution (radians). + /// Step size for theta search dimension. + #[serde(default = "default_angular_resolution")] + pub angular_resolution: f32, + + /// Minimum acceptable match score (0.0 to 1.0). + /// Matches below this threshold will be rejected. + #[serde(default = "default_min_score")] + pub min_score: f32, + + /// Whether to use multi-resolution search. + /// First does coarse search, then refines around best candidate. + #[serde(default = "default_multi_resolution")] + pub multi_resolution: bool, + + /// Coarse resolution multiplier for multi-resolution search. + /// The coarse search uses resolution * coarse_factor. + #[serde(default = "default_coarse_factor")] + pub coarse_factor: f32, + + /// Maximum number of scan points to use. + /// Subsamples if scan has more points. 0 = use all. + #[serde(default)] + pub max_points: usize, + + /// Whether matching is enabled. + /// If false, matcher returns prior pose unchanged. + #[serde(default = "default_true")] + pub enabled: bool, + + /// Sensor offset from robot center in robot frame (x, y). + /// This offset is applied when transforming scan points to world frame. + /// Typically the lidar is mounted behind the robot center (negative x). + #[serde(default)] + pub sensor_offset: (f32, f32), + + /// Whether to use Gaussian scoring based on distance field. + /// If true, uses continuous distance-to-wall scoring instead of binary cell type. + /// This provides better gradients for small pose changes. + #[serde(default = "default_true")] + pub use_gaussian_scoring: bool, + + /// Standard deviation for Gaussian scoring (meters). + /// Controls how quickly score drops off with distance from wall. + /// - sigma=0.05 (5cm): Very tight, requires near-exact alignment + /// - sigma=0.10 (10cm): Good balance for 5cm grid resolution + /// - sigma=0.15 (15cm): More forgiving, smoother gradients + #[serde(default = "default_gaussian_sigma")] + pub gaussian_sigma: f32, + + // === Gauss-Newton Refinement === + /// Maximum iterations for Gauss-Newton refinement. + /// Typically converges in 3-5 iterations. + #[serde(default = "default_gn_max_iterations")] + pub gn_max_iterations: usize, + + /// Convergence threshold for Gauss-Newton (meters). + /// Stop when pose update magnitude is below this. + #[serde(default = "default_gn_convergence")] + pub gn_convergence_threshold: f32, + + /// Levenberg-Marquardt damping factor for Gauss-Newton. + /// Higher values make optimization more stable but slower. + /// 0.0 = pure Gauss-Newton, 0.001-0.01 = typical LM damping. + #[serde(default = "default_gn_damping")] + pub gn_damping: f32, + + /// Translation weight for Gauss-Newton prior constraint. + /// Penalizes deviation from the correlative search result. + /// Higher = trust prior more, lower = trust scan matching more. + /// Cartographer uses ~1e5 for indoor environments. + #[serde(default = "default_gn_translation_weight")] + pub gn_translation_weight: f32, + + /// Rotation weight for Gauss-Newton prior constraint. + /// Penalizes angular deviation from the correlative search result. + #[serde(default = "default_gn_rotation_weight")] + pub gn_rotation_weight: f32, +} + +fn default_search_xy() -> f32 { + 0.3 // 30cm search window +} + +fn default_search_theta() -> f32 { + 0.15 // ~8.6 degrees - appropriate for inter-scan corrections +} + +fn default_linear_resolution() -> f32 { + 0.02 // 2cm steps +} + +fn default_angular_resolution() -> f32 { + 0.02 // ~1.1 degrees +} + +fn default_min_score() -> f32 { + 0.5 +} + +fn default_true() -> bool { + true +} + +fn default_multi_resolution() -> bool { + true // Multi-resolution for efficiency +} + +fn default_coarse_factor() -> f32 { + 2.0 // Reduced from 4.0 to improve accuracy (0.04m coarse steps instead of 0.08m) +} + +fn default_gaussian_sigma() -> f32 { + 0.10 // 10cm - good balance for 5cm grid resolution +} + +fn default_gn_max_iterations() -> usize { + 10 // Typically converges in 3-5 iterations +} + +fn default_gn_convergence() -> f32 { + 1e-5 // Sub-millimeter convergence +} + +fn default_gn_damping() -> f32 { + 1e-3 // Light LM damping for stability +} + +fn default_gn_translation_weight() -> f32 { + 500.0 // Very strong translation prior - encoders are typically reliable for distance +} + +fn default_gn_rotation_weight() -> f32 { + 20.0 // Lower rotation weight - allow scan matching to correct heading drift +} + +impl Default for CorrelativeMatcherConfig { + fn default() -> Self { + Self { + search_x: default_search_xy(), + search_y: default_search_xy(), + search_theta: default_search_theta(), + linear_resolution: default_linear_resolution(), + angular_resolution: default_angular_resolution(), + min_score: default_min_score(), + multi_resolution: default_multi_resolution(), + coarse_factor: default_coarse_factor(), + max_points: 0, + enabled: true, + sensor_offset: (0.0, 0.0), + use_gaussian_scoring: true, + gaussian_sigma: default_gaussian_sigma(), + gn_max_iterations: default_gn_max_iterations(), + gn_convergence_threshold: default_gn_convergence(), + gn_damping: default_gn_damping(), + gn_translation_weight: default_gn_translation_weight(), + gn_rotation_weight: default_gn_rotation_weight(), + } + } +} + +impl CorrelativeMatcherConfig { + /// Create a fast configuration with reduced search space. + /// Good for real-time applications with accurate odometry. + pub fn fast() -> Self { + Self { + search_x: 0.15, + search_y: 0.15, + search_theta: 0.25, + linear_resolution: 0.03, + angular_resolution: 0.03, + max_points: 180, + ..Default::default() + } + } + + /// Create a thorough configuration with larger search space. + /// Good for situations with poor odometry or after long gaps. + pub fn thorough() -> Self { + Self { + search_x: 0.5, + search_y: 0.5, + search_theta: 0.8, + linear_resolution: 0.01, + angular_resolution: 0.01, + ..Default::default() + } + } + + /// Create a disabled matcher that just passes through poses. + pub fn disabled() -> Self { + Self { + enabled: false, + ..Default::default() + } + } + + /// Calculate the number of search poses to evaluate. + pub fn search_space_size(&self) -> usize { + let x_steps = (2.0 * self.search_x / self.linear_resolution) as usize + 1; + let y_steps = (2.0 * self.search_y / self.linear_resolution) as usize + 1; + let theta_steps = (2.0 * self.search_theta / self.angular_resolution) as usize + 1; + x_steps * y_steps * theta_steps + } +} + +#[cfg(test)] +mod tests { + use super::*; + + #[test] + fn test_default_config() { + let config = CorrelativeMatcherConfig::default(); + assert!((config.search_x - 0.3).abs() < 0.001); + assert!(config.enabled); + assert!(config.multi_resolution); + } + + #[test] + fn test_search_space_size() { + let config = CorrelativeMatcherConfig::default(); + let size = config.search_space_size(); + // With defaults: 31 x 31 x 51 = 49,011 poses + assert!(size > 10000); + assert!(size < 100000); + } + + #[test] + fn test_fast_config() { + let config = CorrelativeMatcherConfig::fast(); + let default_config = CorrelativeMatcherConfig::default(); + assert!(config.search_space_size() < default_config.search_space_size()); + } +} diff --git a/vastu-map/src/slam/loop_closure/descriptor.rs b/vastu-map/src/slam/loop_closure/descriptor.rs new file mode 100644 index 0000000..6eff90b --- /dev/null +++ b/vastu-map/src/slam/loop_closure/descriptor.rs @@ -0,0 +1,282 @@ +//! LiDAR-IRIS descriptor for loop closure detection. +//! +//! A compact 80-byte binary descriptor that encodes the structure of a lidar scan +//! in a rotation-invariant way for efficient place recognition. + +use crate::core::LidarScan; + +/// Number of radial rings in the descriptor. +const NUM_RINGS: usize = 8; + +/// Number of angular sectors in the descriptor. +const NUM_SECTORS: usize = 80; + +/// Total descriptor size in bytes (8 rings × 80 sectors = 640 bits = 80 bytes). +const DESCRIPTOR_SIZE: usize = NUM_RINGS * NUM_SECTORS / 8; + +/// LiDAR-IRIS binary descriptor. +/// +/// Encodes lidar scan structure in a compact 80-byte binary format. +/// Designed for fast Hamming distance computation for loop closure detection. +#[derive(Clone, Debug)] +pub struct LidarIris { + /// Binary descriptor data (80 bytes = 640 bits). + data: [u8; DESCRIPTOR_SIZE], + /// Maximum range used when creating descriptor. + max_range: f32, +} + +impl LidarIris { + /// Create a new descriptor from a lidar scan. + /// + /// The descriptor encodes occupancy in a polar grid: + /// - 8 radial rings (from center outward) + /// - 80 angular sectors (around 360°) + /// + /// Each cell is 1 if there's a lidar hit in that cell, 0 otherwise. + pub fn from_scan(scan: &LidarScan) -> Self { + let max_range = scan.range_max; + let ring_width = max_range / NUM_RINGS as f32; + let sector_width = std::f32::consts::TAU / NUM_SECTORS as f32; + + let mut data = [0u8; DESCRIPTOR_SIZE]; + + // Process each valid scan point + for (angle, range) in scan.valid_points() { + // Determine which ring (0 = closest to robot) + let ring = ((range / ring_width) as usize).min(NUM_RINGS - 1); + + // Determine which sector (normalize angle to 0..TAU) + let normalized_angle = if angle < 0.0 { + angle + std::f32::consts::TAU + } else { + angle + }; + let sector = ((normalized_angle / sector_width) as usize).min(NUM_SECTORS - 1); + + // Calculate bit position + // Layout: ring 0 sectors 0-79, ring 1 sectors 0-79, etc. + let bit_index = ring * NUM_SECTORS + sector; + let byte_index = bit_index / 8; + let bit_offset = bit_index % 8; + + // Set the bit + data[byte_index] |= 1 << bit_offset; + } + + Self { data, max_range } + } + + /// Create an empty descriptor. + pub fn empty() -> Self { + Self { + data: [0u8; DESCRIPTOR_SIZE], + max_range: 10.0, + } + } + + /// Get the raw descriptor data. + pub fn data(&self) -> &[u8; DESCRIPTOR_SIZE] { + &self.data + } + + /// Calculate Hamming distance to another descriptor. + /// + /// Returns the number of differing bits (0 to 640). + /// Lower distance = more similar scans. + pub fn distance(&self, other: &LidarIris) -> u32 { + let mut dist = 0u32; + for i in 0..DESCRIPTOR_SIZE { + dist += (self.data[i] ^ other.data[i]).count_ones(); + } + dist + } + + /// Calculate normalized similarity (0.0 to 1.0). + /// + /// 1.0 = identical, 0.0 = completely different. + pub fn similarity(&self, other: &LidarIris) -> f32 { + let dist = self.distance(other); + 1.0 - (dist as f32 / (DESCRIPTOR_SIZE * 8) as f32) + } + + /// Count the number of set bits (occupied cells). + pub fn popcount(&self) -> u32 { + self.data.iter().map(|b| b.count_ones()).sum() + } + + /// Check if descriptor is mostly empty (low occupancy). + pub fn is_sparse(&self) -> bool { + self.popcount() < 20 + } + + /// Rotate the descriptor by a number of sectors. + /// + /// This is useful for rotation-invariant matching. + /// Positive rotation = counterclockwise. + pub fn rotated(&self, sectors: i32) -> Self { + let mut rotated_data = [0u8; DESCRIPTOR_SIZE]; + + for ring in 0..NUM_RINGS { + for sector in 0..NUM_SECTORS { + // Calculate source sector (with wrapping) + let src_sector = + ((sector as i32 - sectors).rem_euclid(NUM_SECTORS as i32)) as usize; + + // Copy bit from source to destination + let src_bit_index = ring * NUM_SECTORS + src_sector; + let src_byte = src_bit_index / 8; + let src_offset = src_bit_index % 8; + + let dst_bit_index = ring * NUM_SECTORS + sector; + let dst_byte = dst_bit_index / 8; + let dst_offset = dst_bit_index % 8; + + if (self.data[src_byte] >> src_offset) & 1 != 0 { + rotated_data[dst_byte] |= 1 << dst_offset; + } + } + } + + Self { + data: rotated_data, + max_range: self.max_range, + } + } + + /// Find the best rotation-invariant match. + /// + /// Tests all rotations and returns the minimum distance + /// along with the rotation offset that achieved it. + pub fn best_match(&self, other: &LidarIris) -> (u32, i32) { + let mut best_dist = self.distance(other); + let mut best_rotation = 0; + + // Test rotations at coarse resolution first + let coarse_step = 4; + for i in (coarse_step..NUM_SECTORS as i32).step_by(coarse_step as usize) { + let rotated = self.rotated(i); + let dist = rotated.distance(other); + if dist < best_dist { + best_dist = dist; + best_rotation = i; + } + } + + // Fine search around best coarse rotation + for offset in -coarse_step..=coarse_step { + let rot = best_rotation + offset; + if rot == best_rotation { + continue; + } + let rotated = self.rotated(rot); + let dist = rotated.distance(other); + if dist < best_dist { + best_dist = dist; + best_rotation = rot; + } + } + + (best_dist, best_rotation) + } +} + +impl Default for LidarIris { + fn default() -> Self { + Self::empty() + } +} + +impl PartialEq for LidarIris { + fn eq(&self, other: &Self) -> bool { + self.data == other.data + } +} + +impl Eq for LidarIris {} + +#[cfg(test)] +mod tests { + use super::*; + + fn create_test_scan(ranges: &[f32]) -> LidarScan { + let num_points = ranges.len(); + let angles: Vec = (0..num_points) + .map(|i| -std::f32::consts::PI + i as f32 * std::f32::consts::TAU / num_points as f32) + .collect(); + LidarScan::new(ranges.to_vec(), angles, 0.1, 10.0) + } + + #[test] + fn test_empty_descriptor() { + let desc = LidarIris::empty(); + assert_eq!(desc.popcount(), 0); + assert!(desc.is_sparse()); + } + + #[test] + fn test_from_scan() { + // Create a scan with some points + let ranges = vec![2.0; 360]; + let scan = create_test_scan(&ranges); + + let desc = LidarIris::from_scan(&scan); + assert!(desc.popcount() > 0); + } + + #[test] + fn test_identical_distance() { + let ranges = vec![2.0; 360]; + let scan = create_test_scan(&ranges); + + let desc1 = LidarIris::from_scan(&scan); + let desc2 = LidarIris::from_scan(&scan); + + assert_eq!(desc1.distance(&desc2), 0); + assert!((desc1.similarity(&desc2) - 1.0).abs() < 0.001); + } + + #[test] + fn test_different_scans() { + let scan1 = create_test_scan(&vec![2.0; 360]); + let scan2 = create_test_scan(&vec![5.0; 360]); // Different range + + let desc1 = LidarIris::from_scan(&scan1); + let desc2 = LidarIris::from_scan(&scan2); + + // Should be different (different rings) + assert!(desc1.distance(&desc2) > 0); + } + + #[test] + fn test_rotation() { + let ranges = vec![2.0; 360]; + let scan = create_test_scan(&ranges); + let desc = LidarIris::from_scan(&scan); + + // Rotating by 0 should give same descriptor + let rotated = desc.rotated(0); + assert_eq!(desc.distance(&rotated), 0); + + // Full rotation should also give same descriptor + let full_rotation = desc.rotated(NUM_SECTORS as i32); + assert_eq!(desc.distance(&full_rotation), 0); + } + + #[test] + fn test_best_match() { + let ranges = vec![2.0; 360]; + let scan = create_test_scan(&ranges); + let desc = LidarIris::from_scan(&scan); + + // Match with itself should have distance 0 + let (dist, _) = desc.best_match(&desc); + assert_eq!(dist, 0); + } + + #[test] + fn test_descriptor_size() { + assert_eq!(DESCRIPTOR_SIZE, 80); + assert_eq!(NUM_RINGS * NUM_SECTORS, 640); + } +} diff --git a/vastu-map/src/slam/loop_closure/detector.rs b/vastu-map/src/slam/loop_closure/detector.rs new file mode 100644 index 0000000..efb1ba7 --- /dev/null +++ b/vastu-map/src/slam/loop_closure/detector.rs @@ -0,0 +1,507 @@ +//! Loop closure detection using LiDAR-IRIS descriptors. +//! +//! Two-stage detection (like Cartographer): +//! 1. Fast candidate detection using LiDAR-IRIS descriptors +//! 2. Verification via correlative scan matching with Gauss-Newton refinement + +use serde::{Deserialize, Serialize}; + +use crate::core::{LidarScan, Pose2D}; +use crate::grid::GridStorage; + +use super::descriptor::LidarIris; +use crate::slam::{CorrelativeMatcher, CorrelativeMatcherConfig}; + +/// Configuration for loop closure detection. +#[derive(Clone, Debug, Serialize, Deserialize)] +pub struct LoopClosureConfig { + /// Minimum distance traveled between keyframes (meters). + #[serde(default = "default_keyframe_distance")] + pub keyframe_distance: f32, + + /// Minimum rotation between keyframes (radians). + #[serde(default = "default_keyframe_rotation")] + pub keyframe_rotation: f32, + + /// Maximum descriptor distance for loop closure candidate. + /// Lower = stricter matching (0-640). + #[serde(default = "default_max_descriptor_distance")] + pub max_descriptor_distance: u32, + + /// Minimum pose graph index gap for loop closure. + /// Prevents detecting "loops" between consecutive scans. + #[serde(default = "default_min_loop_gap")] + pub min_loop_gap: usize, + + /// Maximum distance between poses for loop closure (meters). + /// Prevents matching places that are too far apart. + #[serde(default = "default_max_loop_distance")] + pub max_loop_distance: f32, + + /// Whether loop closure is enabled. + #[serde(default = "default_true")] + pub enabled: bool, + + // === Scan-Matching Verification === + /// Minimum scan matching score for loop closure verification. + /// Candidates below this score are rejected as false positives. + /// Range: 0.0 to 1.0 (0.6 = 60% of points match walls) + #[serde(default = "default_min_verification_score")] + pub min_verification_score: f32, + + /// Search window for verification scan matching (meters). + /// Should be small since descriptor already gives approximate alignment. + #[serde(default = "default_verification_search")] + pub verification_search_window: f32, + + /// Angular search window for verification (radians). + #[serde(default = "default_verification_angular")] + pub verification_angular_window: f32, +} + +fn default_keyframe_distance() -> f32 { + 0.5 // 50cm between keyframes +} + +fn default_keyframe_rotation() -> f32 { + 0.5 // ~28 degrees +} + +fn default_max_descriptor_distance() -> u32 { + 100 // Out of 640 bits +} + +fn default_min_loop_gap() -> usize { + 20 // At least 20 poses between loop endpoints +} + +fn default_max_loop_distance() -> f32 { + 5.0 // 5 meters +} + +fn default_true() -> bool { + true +} + +fn default_min_verification_score() -> f32 { + 0.6 // 60% of points must match walls +} + +fn default_verification_search() -> f32 { + 0.2 // 20cm search window (descriptor gives rough alignment) +} + +fn default_verification_angular() -> f32 { + 0.2 // ~11 degrees angular search +} + +impl Default for LoopClosureConfig { + fn default() -> Self { + Self { + keyframe_distance: default_keyframe_distance(), + keyframe_rotation: default_keyframe_rotation(), + max_descriptor_distance: default_max_descriptor_distance(), + min_loop_gap: default_min_loop_gap(), + max_loop_distance: default_max_loop_distance(), + enabled: true, + min_verification_score: default_min_verification_score(), + verification_search_window: default_verification_search(), + verification_angular_window: default_verification_angular(), + } + } +} + +/// A detected loop closure. +#[derive(Clone, Debug)] +pub struct LoopClosure { + /// Index of the current pose (loop start). + pub from_idx: usize, + /// Index of the matched keyframe pose (loop end). + pub to_idx: usize, + /// Relative pose from `from_idx` to `to_idx`. + /// This is the constraint to add to the pose graph. + pub relative_pose: Pose2D, + /// Confidence in the loop closure (0.0 to 1.0). + pub confidence: f32, + /// Descriptor distance (lower = better match). + pub descriptor_distance: u32, + /// Rotation offset used for best match. + pub rotation_offset: i32, +} + +/// Keyframe data stored for loop closure detection. +#[derive(Clone, Debug)] +struct Keyframe { + /// Pose graph index. + pose_idx: usize, + /// World pose at this keyframe. + pose: Pose2D, + /// LiDAR-IRIS descriptor. + descriptor: LidarIris, + /// Stored scan for potential scan-to-scan verification. + /// Currently using scan-to-map, but kept for future use. + #[allow(dead_code)] + scan: Option, +} + +/// Result of loop closure verification. +#[derive(Clone, Debug)] +pub struct VerifiedLoopClosure { + /// The original loop closure candidate. + pub closure: LoopClosure, + /// Refined relative pose from scan matching. + pub refined_relative_pose: Pose2D, + /// Scan matching score (0.0 to 1.0). + pub match_score: f32, + /// Whether verification passed. + pub verified: bool, +} + +/// Loop closure detector. +/// +/// Maintains a database of keyframes and detects when the robot +/// revisits a previously seen location. +/// +/// Two-stage detection (Cartographer-style): +/// 1. Fast candidate detection using LiDAR-IRIS descriptors +/// 2. Verification via correlative scan matching with Gauss-Newton refinement +pub struct LoopClosureDetector { + /// Configuration. + config: LoopClosureConfig, + /// Stored keyframes. + keyframes: Vec, + /// Last keyframe pose (for distance check). + last_keyframe_pose: Option, + /// Current pose index. + current_idx: usize, + /// Matcher for verification. + verification_matcher: CorrelativeMatcher, +} + +impl LoopClosureDetector { + /// Create a new loop closure detector. + pub fn new(config: LoopClosureConfig) -> Self { + // Create verification matcher with tight search window + let matcher_config = CorrelativeMatcherConfig { + search_x: config.verification_search_window, + search_y: config.verification_search_window, + search_theta: config.verification_angular_window, + linear_resolution: 0.01, // 1cm fine resolution + angular_resolution: 0.01, + multi_resolution: true, + ..CorrelativeMatcherConfig::default() + }; + let verification_matcher = CorrelativeMatcher::new(matcher_config); + + Self { + config, + keyframes: Vec::new(), + last_keyframe_pose: None, + current_idx: 0, + verification_matcher, + } + } + + /// Create with default configuration. + pub fn with_defaults() -> Self { + Self::new(LoopClosureConfig::default()) + } + + /// Get configuration. + pub fn config(&self) -> &LoopClosureConfig { + &self.config + } + + /// Get number of stored keyframes. + pub fn keyframe_count(&self) -> usize { + self.keyframes.len() + } + + /// Process a new scan and pose. + /// + /// Returns a loop closure if detected, otherwise None. + /// + /// # Arguments + /// * `scan` - The current lidar scan + /// * `pose` - The current robot pose in world frame + /// + /// # Returns + /// Some(LoopClosure) if a loop is detected, None otherwise. + pub fn add_scan(&mut self, scan: &LidarScan, pose: Pose2D) -> Option { + if !self.config.enabled { + return None; + } + + let pose_idx = self.current_idx; + self.current_idx += 1; + + // Check if we should create a keyframe + if !self.should_create_keyframe(pose) { + return None; + } + + // Create descriptor for current scan + let descriptor = LidarIris::from_scan(scan); + + // Skip sparse descriptors (e.g., in open areas) + if descriptor.is_sparse() { + return None; + } + + // Search for loop closure candidates + let loop_closure = self.find_loop_closure(pose_idx, pose, &descriptor); + + // Add as new keyframe (store scan for verification) + self.keyframes.push(Keyframe { + pose_idx, + pose, + descriptor, + scan: Some(scan.clone()), + }); + self.last_keyframe_pose = Some(pose); + + loop_closure + } + + /// Verify a loop closure candidate using scan matching. + /// + /// This is the second stage of Cartographer-style loop closure: + /// after LiDAR-IRIS finds a candidate, verify it with actual scan matching. + /// + /// # Arguments + /// * `candidate` - The loop closure candidate from `add_scan` + /// * `current_scan` - The current lidar scan + /// * `storage` - The occupancy grid map + /// + /// # Returns + /// Verification result with refined pose and match score. + pub fn verify_loop_closure( + &self, + candidate: &LoopClosure, + current_scan: &LidarScan, + storage: &GridStorage, + ) -> VerifiedLoopClosure { + // Find the target keyframe + let target_keyframe = self + .keyframes + .iter() + .find(|kf| kf.pose_idx == candidate.to_idx); + + let (refined_pose, match_score) = match target_keyframe { + Some(kf) => { + // Use the candidate's estimated pose as prior for scan matching + // The target keyframe pose transformed by the relative pose + let estimated_current_pose = Pose2D::new( + kf.pose.x - candidate.relative_pose.x, + kf.pose.y - candidate.relative_pose.y, + normalize_angle(kf.pose.theta - candidate.relative_pose.theta), + ); + + // Run scan matching to verify and refine + let match_result = self.verification_matcher.match_scan( + current_scan, + estimated_current_pose, + storage, + ); + + // Compute refined relative pose from match result + let refined_relative = Pose2D::new( + kf.pose.x - match_result.pose.x, + kf.pose.y - match_result.pose.y, + normalize_angle(kf.pose.theta - match_result.pose.theta), + ); + + (refined_relative, match_result.score) + } + None => { + // Keyframe not found, return original with zero score + (candidate.relative_pose, 0.0) + } + }; + + let verified = match_score >= self.config.min_verification_score; + + VerifiedLoopClosure { + closure: candidate.clone(), + refined_relative_pose: refined_pose, + match_score, + verified, + } + } + + /// Check if we should create a keyframe at current pose. + fn should_create_keyframe(&self, pose: Pose2D) -> bool { + match &self.last_keyframe_pose { + None => true, // First keyframe + Some(last) => { + let distance = pose.position().distance(&last.position()); + let rotation = (pose.theta - last.theta).abs(); + + distance >= self.config.keyframe_distance + || rotation >= self.config.keyframe_rotation + } + } + } + + /// Search for loop closure candidates. + fn find_loop_closure( + &self, + current_idx: usize, + current_pose: Pose2D, + current_descriptor: &LidarIris, + ) -> Option { + let mut best_candidate: Option = None; + let mut best_distance = self.config.max_descriptor_distance; + + for keyframe in &self.keyframes { + // Check pose index gap + if current_idx.saturating_sub(keyframe.pose_idx) < self.config.min_loop_gap { + continue; + } + + // Check spatial distance + let spatial_distance = current_pose.position().distance(&keyframe.pose.position()); + if spatial_distance > self.config.max_loop_distance { + continue; + } + + // Compare descriptors with rotation-invariant matching + let (desc_distance, rotation_offset) = + current_descriptor.best_match(&keyframe.descriptor); + + if desc_distance < best_distance { + best_distance = desc_distance; + + // Calculate relative pose (simple approximation) + // In a full implementation, you'd use ICP or similar for refinement + let dx = keyframe.pose.x - current_pose.x; + let dy = keyframe.pose.y - current_pose.y; + let dtheta = keyframe.pose.theta - current_pose.theta; + + // Convert rotation offset to radians + let rotation_rad = rotation_offset as f32 * std::f32::consts::TAU / 80.0; + + let relative_pose = Pose2D::new(dx, dy, dtheta + rotation_rad); + + // Confidence based on descriptor similarity + let confidence = 1.0 - (desc_distance as f32 / 640.0); + + best_candidate = Some(LoopClosure { + from_idx: current_idx, + to_idx: keyframe.pose_idx, + relative_pose, + confidence, + descriptor_distance: desc_distance, + rotation_offset, + }); + } + } + + best_candidate + } + + /// Clear all stored keyframes. + pub fn reset(&mut self) { + self.keyframes.clear(); + self.last_keyframe_pose = None; + self.current_idx = 0; + } + + /// Get all keyframe poses (for visualization). + pub fn keyframe_poses(&self) -> Vec { + self.keyframes.iter().map(|kf| kf.pose).collect() + } +} + +/// Normalize angle to [-π, π). +fn normalize_angle(angle: f32) -> f32 { + let mut a = angle % std::f32::consts::TAU; + if a > std::f32::consts::PI { + a -= std::f32::consts::TAU; + } else if a < -std::f32::consts::PI { + a += std::f32::consts::TAU; + } + a +} + +#[cfg(test)] +mod tests { + use super::*; + + fn create_test_scan(ranges: &[f32]) -> LidarScan { + let num_points = ranges.len(); + let angles: Vec = (0..num_points) + .map(|i| -std::f32::consts::PI + i as f32 * std::f32::consts::TAU / num_points as f32) + .collect(); + LidarScan::new(ranges.to_vec(), angles, 0.1, 10.0) + } + + #[test] + fn test_detector_creation() { + let detector = LoopClosureDetector::with_defaults(); + assert_eq!(detector.keyframe_count(), 0); + } + + #[test] + fn test_add_first_keyframe() { + let mut detector = LoopClosureDetector::with_defaults(); + let scan = create_test_scan(&vec![2.0; 360]); + let pose = Pose2D::new(0.0, 0.0, 0.0); + + let result = detector.add_scan(&scan, pose); + assert!(result.is_none()); // No loop on first scan + assert_eq!(detector.keyframe_count(), 1); + } + + #[test] + fn test_keyframe_distance() { + let mut detector = LoopClosureDetector::with_defaults(); + let scan = create_test_scan(&vec![2.0; 360]); + + // First keyframe + detector.add_scan(&scan, Pose2D::new(0.0, 0.0, 0.0)); + assert_eq!(detector.keyframe_count(), 1); + + // Too close - should not create keyframe + detector.add_scan(&scan, Pose2D::new(0.1, 0.0, 0.0)); + assert_eq!(detector.keyframe_count(), 1); + + // Far enough - should create keyframe + detector.add_scan(&scan, Pose2D::new(1.0, 0.0, 0.0)); + assert_eq!(detector.keyframe_count(), 2); + } + + #[test] + fn test_disabled_detector() { + let config = LoopClosureConfig { + enabled: false, + ..Default::default() + }; + let mut detector = LoopClosureDetector::new(config); + let scan = create_test_scan(&vec![2.0; 360]); + + let result = detector.add_scan(&scan, Pose2D::new(0.0, 0.0, 0.0)); + assert!(result.is_none()); + assert_eq!(detector.keyframe_count(), 0); + } + + #[test] + fn test_reset() { + let mut detector = LoopClosureDetector::with_defaults(); + let scan = create_test_scan(&vec![2.0; 360]); + + detector.add_scan(&scan, Pose2D::new(0.0, 0.0, 0.0)); + detector.add_scan(&scan, Pose2D::new(1.0, 0.0, 0.0)); + assert_eq!(detector.keyframe_count(), 2); + + detector.reset(); + assert_eq!(detector.keyframe_count(), 0); + } + + #[test] + fn test_loop_closure_config() { + let config = LoopClosureConfig::default(); + assert!((config.keyframe_distance - 0.5).abs() < 0.001); + assert!(config.enabled); + } +} diff --git a/vastu-map/src/slam/loop_closure/graph.rs b/vastu-map/src/slam/loop_closure/graph.rs new file mode 100644 index 0000000..5932135 --- /dev/null +++ b/vastu-map/src/slam/loop_closure/graph.rs @@ -0,0 +1,562 @@ +//! Pose graph for SLAM with Gauss-Newton optimization. + +use serde::{Deserialize, Serialize}; + +use crate::core::Pose2D; + +use super::detector::LoopClosure; + +/// Configuration for pose graph optimization. +#[derive(Clone, Debug, Serialize, Deserialize)] +pub struct PoseGraphConfig { + /// Maximum optimization iterations. + #[serde(default = "default_max_iterations")] + pub max_iterations: usize, + + /// Convergence threshold for pose change. + #[serde(default = "default_convergence_threshold")] + pub convergence_threshold: f32, + + /// Levenberg-Marquardt damping factor. + #[serde(default = "default_damping")] + pub damping: f32, + + /// Information matrix weight for odometry edges. + #[serde(default = "default_odom_weight")] + pub odometry_weight: f32, + + /// Information matrix weight for loop closure edges. + #[serde(default = "default_loop_weight")] + pub loop_weight: f32, + + /// Huber loss scale parameter (meters). + /// Residuals larger than this are downweighted. + /// Typical: 0.1 (10cm) - effectively ignores constraints with >10cm error. + #[serde(default = "default_huber_scale")] + pub huber_scale: f32, +} + +fn default_max_iterations() -> usize { + 20 +} + +fn default_convergence_threshold() -> f32 { + 1e-4 +} + +fn default_damping() -> f32 { + 1e-3 +} + +fn default_odom_weight() -> f32 { + 100.0 +} + +fn default_loop_weight() -> f32 { + 50.0 +} + +fn default_huber_scale() -> f32 { + 0.1 // 10cm - downweights constraints with error > 10cm +} + +impl Default for PoseGraphConfig { + fn default() -> Self { + Self { + max_iterations: default_max_iterations(), + convergence_threshold: default_convergence_threshold(), + damping: default_damping(), + odometry_weight: default_odom_weight(), + loop_weight: default_loop_weight(), + huber_scale: default_huber_scale(), + } + } +} + +/// Odometry edge (constraint between consecutive poses). +#[derive(Clone, Debug)] +pub struct OdometryEdge { + /// Index of the "from" pose. + pub from_idx: usize, + /// Index of the "to" pose. + pub to_idx: usize, + /// Measured relative pose (from → to). + pub measurement: Pose2D, + /// Information weight (inverse covariance). + pub weight: f32, +} + +/// Loop closure edge (constraint between non-consecutive poses). +#[derive(Clone, Debug)] +pub struct LoopEdge { + /// Index of the "from" pose. + pub from_idx: usize, + /// Index of the "to" pose. + pub to_idx: usize, + /// Measured relative pose (from → to). + pub measurement: Pose2D, + /// Information weight (inverse covariance). + pub weight: f32, + /// Confidence in this loop closure. + pub confidence: f32, +} + +/// Pose graph for SLAM. +/// +/// Stores robot poses and constraints (odometry + loop closures). +/// Supports optimization to correct accumulated drift. +pub struct PoseGraph { + /// Configuration. + config: PoseGraphConfig, + /// Poses (nodes). + poses: Vec, + /// Odometry edges (sequential constraints). + odometry_edges: Vec, + /// Loop closure edges. + loop_edges: Vec, +} + +impl PoseGraph { + /// Create a new pose graph. + pub fn new(config: PoseGraphConfig) -> Self { + Self { + config, + poses: Vec::new(), + odometry_edges: Vec::new(), + loop_edges: Vec::new(), + } + } + + /// Create with default configuration. + pub fn with_defaults() -> Self { + Self::new(PoseGraphConfig::default()) + } + + /// Get number of poses in the graph. + pub fn len(&self) -> usize { + self.poses.len() + } + + /// Check if graph is empty. + pub fn is_empty(&self) -> bool { + self.poses.is_empty() + } + + /// Get pose at index. + pub fn get_pose(&self, idx: usize) -> Option { + self.poses.get(idx).copied() + } + + /// Get all poses. + pub fn poses(&self) -> &[Pose2D] { + &self.poses + } + + /// Get odometry edges. + pub fn odometry_edges(&self) -> &[OdometryEdge] { + &self.odometry_edges + } + + /// Get loop closure edges. + pub fn loop_edges(&self) -> &[LoopEdge] { + &self.loop_edges + } + + /// Add a new pose with odometry constraint from previous pose. + /// + /// # Arguments + /// * `pose` - The new pose in world frame + /// * `odom_delta` - Relative pose from previous to this pose + /// + /// # Returns + /// Index of the new pose. + pub fn add_pose(&mut self, pose: Pose2D, odom_delta: Option) -> usize { + let new_idx = self.poses.len(); + self.poses.push(pose); + + // Add odometry edge if not first pose + if let Some(delta) = odom_delta + && new_idx > 0 + { + self.odometry_edges.push(OdometryEdge { + from_idx: new_idx - 1, + to_idx: new_idx, + measurement: delta, + weight: self.config.odometry_weight, + }); + } + + new_idx + } + + /// Add a loop closure constraint. + pub fn add_loop_closure(&mut self, closure: LoopClosure) { + self.loop_edges.push(LoopEdge { + from_idx: closure.from_idx, + to_idx: closure.to_idx, + measurement: closure.relative_pose, + weight: self.config.loop_weight * closure.confidence, + confidence: closure.confidence, + }); + } + + /// Check if graph has any loop closures. + pub fn has_loops(&self) -> bool { + !self.loop_edges.is_empty() + } + + /// Optimize the pose graph using Gauss-Newton. + /// + /// Returns the number of iterations performed. + pub fn optimize(&mut self) -> usize { + if self.poses.len() < 2 { + return 0; + } + + for iteration in 0..self.config.max_iterations { + let max_update = self.optimization_step(); + + if max_update < self.config.convergence_threshold { + return iteration + 1; + } + } + + self.config.max_iterations + } + + /// Perform one optimization step. + /// + /// Returns the maximum pose update magnitude. + fn optimization_step(&mut self) -> f32 { + let n = self.poses.len(); + if n < 2 { + return 0.0; + } + + // Build linear system Hx = b + // For simplicity, we use a coordinate descent approach: + // Update each pose based on its constraints + + let mut max_update = 0.0f32; + + // Skip pose 0 (anchor - fixed at origin) + for i in 1..n { + let (dx, dy, dtheta) = self.compute_pose_update(i); + + // Apply update with damping + let damping = self.config.damping; + let update_mag = (dx * dx + dy * dy + dtheta * dtheta).sqrt(); + + self.poses[i].x += dx / (1.0 + damping); + self.poses[i].y += dy / (1.0 + damping); + self.poses[i].theta += dtheta / (1.0 + damping); + + // Normalize theta + self.poses[i].theta = normalize_angle(self.poses[i].theta); + + max_update = max_update.max(update_mag); + } + + max_update + } + + /// Compute update for a single pose. + /// + /// Uses Huber loss weighting for loop closure edges to make + /// optimization robust to outliers (false loop closures). + fn compute_pose_update(&self, pose_idx: usize) -> (f32, f32, f32) { + let mut sum_x = 0.0f32; + let mut sum_y = 0.0f32; + let mut sum_theta = 0.0f32; + let mut sum_weight = 0.0f32; + + let huber_scale = self.config.huber_scale; + + // Contribution from odometry edges (no Huber - odometry is reliable) + for edge in &self.odometry_edges { + if edge.to_idx == pose_idx { + // This pose is the "to" end + let from_pose = self.poses[edge.from_idx]; + let expected = self.transform_pose(from_pose, edge.measurement); + + let error_x = expected.x - self.poses[pose_idx].x; + let error_y = expected.y - self.poses[pose_idx].y; + let error_theta = normalize_angle(expected.theta - self.poses[pose_idx].theta); + + sum_x += edge.weight * error_x; + sum_y += edge.weight * error_y; + sum_theta += edge.weight * error_theta; + sum_weight += edge.weight; + } + + if edge.from_idx == pose_idx { + // This pose is the "from" end + let to_pose = self.poses[edge.to_idx]; + let expected = self.inverse_transform_pose(to_pose, edge.measurement); + + let error_x = expected.x - self.poses[pose_idx].x; + let error_y = expected.y - self.poses[pose_idx].y; + let error_theta = normalize_angle(expected.theta - self.poses[pose_idx].theta); + + sum_x += edge.weight * error_x; + sum_y += edge.weight * error_y; + sum_theta += edge.weight * error_theta; + sum_weight += edge.weight; + } + } + + // Contribution from loop closure edges (with Huber loss) + for edge in &self.loop_edges { + if edge.to_idx == pose_idx { + let from_pose = self.poses[edge.from_idx]; + let expected = self.transform_pose(from_pose, edge.measurement); + + let error_x = expected.x - self.poses[pose_idx].x; + let error_y = expected.y - self.poses[pose_idx].y; + let error_theta = normalize_angle(expected.theta - self.poses[pose_idx].theta); + + // Compute Huber weight based on translation error magnitude + let error_mag = (error_x * error_x + error_y * error_y).sqrt(); + let huber_w = huber_weight(error_mag, huber_scale); + let effective_weight = edge.weight * huber_w; + + sum_x += effective_weight * error_x; + sum_y += effective_weight * error_y; + sum_theta += effective_weight * error_theta; + sum_weight += effective_weight; + } + + if edge.from_idx == pose_idx { + let to_pose = self.poses[edge.to_idx]; + let expected = self.inverse_transform_pose(to_pose, edge.measurement); + + let error_x = expected.x - self.poses[pose_idx].x; + let error_y = expected.y - self.poses[pose_idx].y; + let error_theta = normalize_angle(expected.theta - self.poses[pose_idx].theta); + + // Compute Huber weight based on translation error magnitude + let error_mag = (error_x * error_x + error_y * error_y).sqrt(); + let huber_w = huber_weight(error_mag, huber_scale); + let effective_weight = edge.weight * huber_w; + + sum_x += effective_weight * error_x; + sum_y += effective_weight * error_y; + sum_theta += effective_weight * error_theta; + sum_weight += effective_weight; + } + } + + if sum_weight > 0.0 { + ( + sum_x / sum_weight, + sum_y / sum_weight, + sum_theta / sum_weight, + ) + } else { + (0.0, 0.0, 0.0) + } + } + + /// Transform a pose by a relative pose. + fn transform_pose(&self, base: Pose2D, delta: Pose2D) -> Pose2D { + let cos_theta = base.theta.cos(); + let sin_theta = base.theta.sin(); + + Pose2D::new( + base.x + delta.x * cos_theta - delta.y * sin_theta, + base.y + delta.x * sin_theta + delta.y * cos_theta, + normalize_angle(base.theta + delta.theta), + ) + } + + /// Inverse transform: given result and delta, find base. + fn inverse_transform_pose(&self, result: Pose2D, delta: Pose2D) -> Pose2D { + let base_theta = normalize_angle(result.theta - delta.theta); + let cos_theta = base_theta.cos(); + let sin_theta = base_theta.sin(); + + Pose2D::new( + result.x - (delta.x * cos_theta - delta.y * sin_theta), + result.y - (delta.x * sin_theta + delta.y * cos_theta), + base_theta, + ) + } + + /// Get the total error (sum of squared residuals). + pub fn total_error(&self) -> f32 { + let mut error = 0.0f32; + + for edge in &self.odometry_edges { + let from_pose = self.poses[edge.from_idx]; + let to_pose = self.poses[edge.to_idx]; + let expected = self.transform_pose(from_pose, edge.measurement); + + let dx = expected.x - to_pose.x; + let dy = expected.y - to_pose.y; + let dtheta = normalize_angle(expected.theta - to_pose.theta); + + error += edge.weight * (dx * dx + dy * dy + dtheta * dtheta); + } + + for edge in &self.loop_edges { + let from_pose = self.poses[edge.from_idx]; + let to_pose = self.poses[edge.to_idx]; + let expected = self.transform_pose(from_pose, edge.measurement); + + let dx = expected.x - to_pose.x; + let dy = expected.y - to_pose.y; + let dtheta = normalize_angle(expected.theta - to_pose.theta); + + error += edge.weight * (dx * dx + dy * dy + dtheta * dtheta); + } + + error + } + + /// Clear the graph. + pub fn reset(&mut self) { + self.poses.clear(); + self.odometry_edges.clear(); + self.loop_edges.clear(); + } +} + +/// Huber weight function for robust optimization. +/// +/// Returns a weight that is: +/// - 1.0 when |residual| <= delta (quadratic region) +/// - delta / |residual| when |residual| > delta (linear region) +/// +/// This downweights large residuals (outliers) to prevent them +/// from dominating the optimization. +#[inline] +fn huber_weight(residual: f32, delta: f32) -> f32 { + let abs_r = residual.abs(); + if abs_r <= delta { 1.0 } else { delta / abs_r } +} + +/// Normalize angle to [-π, π). +fn normalize_angle(angle: f32) -> f32 { + let mut a = angle % std::f32::consts::TAU; + if a > std::f32::consts::PI { + a -= std::f32::consts::TAU; + } else if a < -std::f32::consts::PI { + a += std::f32::consts::TAU; + } + a +} + +#[cfg(test)] +mod tests { + use super::*; + + #[test] + fn test_graph_creation() { + let graph = PoseGraph::with_defaults(); + assert!(graph.is_empty()); + } + + #[test] + fn test_add_poses() { + let mut graph = PoseGraph::with_defaults(); + + // Add first pose (no odometry) + let idx0 = graph.add_pose(Pose2D::new(0.0, 0.0, 0.0), None); + assert_eq!(idx0, 0); + assert_eq!(graph.len(), 1); + + // Add second pose with odometry + let delta = Pose2D::new(1.0, 0.0, 0.1); + let idx1 = graph.add_pose(Pose2D::new(1.0, 0.0, 0.1), Some(delta)); + assert_eq!(idx1, 1); + assert_eq!(graph.len(), 2); + assert_eq!(graph.odometry_edges().len(), 1); + } + + #[test] + fn test_get_pose() { + let mut graph = PoseGraph::with_defaults(); + let pose = Pose2D::new(1.0, 2.0, 0.5); + graph.add_pose(pose, None); + + let retrieved = graph.get_pose(0).unwrap(); + assert!((retrieved.x - 1.0).abs() < 0.001); + assert!((retrieved.y - 2.0).abs() < 0.001); + } + + #[test] + fn test_normalize_angle() { + assert!((normalize_angle(0.0) - 0.0).abs() < 0.001); + assert!((normalize_angle(std::f32::consts::PI * 2.0) - 0.0).abs() < 0.001); + assert!((normalize_angle(std::f32::consts::PI * 3.0) - std::f32::consts::PI).abs() < 0.001); + } + + #[test] + fn test_optimization_no_loops() { + let mut graph = PoseGraph::with_defaults(); + + // Add a chain of poses + graph.add_pose(Pose2D::new(0.0, 0.0, 0.0), None); + graph.add_pose(Pose2D::new(1.0, 0.0, 0.0), Some(Pose2D::new(1.0, 0.0, 0.0))); + graph.add_pose(Pose2D::new(2.0, 0.0, 0.0), Some(Pose2D::new(1.0, 0.0, 0.0))); + + let iterations = graph.optimize(); + + // Should converge quickly with consistent odometry + assert!(iterations < 5); + } + + #[test] + fn test_loop_closure() { + let mut graph = PoseGraph::with_defaults(); + + // Create a square path + graph.add_pose(Pose2D::new(0.0, 0.0, 0.0), None); + graph.add_pose( + Pose2D::new(1.0, 0.0, std::f32::consts::FRAC_PI_2), + Some(Pose2D::new(1.0, 0.0, std::f32::consts::FRAC_PI_2)), + ); + graph.add_pose( + Pose2D::new(1.0, 1.0, std::f32::consts::PI), + Some(Pose2D::new(1.0, 0.0, std::f32::consts::FRAC_PI_2)), + ); + graph.add_pose( + Pose2D::new(0.0, 1.0, -std::f32::consts::FRAC_PI_2), + Some(Pose2D::new(1.0, 0.0, std::f32::consts::FRAC_PI_2)), + ); + + // Add drift (accumulated error) + graph.poses[3].x = 0.1; // Should be 0.0 + + // Add loop closure + let closure = LoopClosure { + from_idx: 3, + to_idx: 0, + relative_pose: Pose2D::new(0.0, -1.0, std::f32::consts::FRAC_PI_2), + confidence: 1.0, + descriptor_distance: 10, + rotation_offset: 0, + }; + graph.add_loop_closure(closure); + + assert!(graph.has_loops()); + + let error_before = graph.total_error(); + graph.optimize(); + let error_after = graph.total_error(); + + // Error should decrease after optimization + assert!(error_after <= error_before); + } + + #[test] + fn test_reset() { + let mut graph = PoseGraph::with_defaults(); + graph.add_pose(Pose2D::new(0.0, 0.0, 0.0), None); + graph.add_pose(Pose2D::new(1.0, 0.0, 0.0), Some(Pose2D::new(1.0, 0.0, 0.0))); + + graph.reset(); + assert!(graph.is_empty()); + assert!(graph.odometry_edges().is_empty()); + } +} diff --git a/vastu-map/src/slam/loop_closure/mod.rs b/vastu-map/src/slam/loop_closure/mod.rs new file mode 100644 index 0000000..2e9b2ea --- /dev/null +++ b/vastu-map/src/slam/loop_closure/mod.rs @@ -0,0 +1,14 @@ +//! Loop closure detection and pose graph optimization. +//! +//! This module provides: +//! - **LiDAR-IRIS**: Compact binary descriptor for place recognition +//! - **LoopClosureDetector**: Detects when robot revisits a location +//! - **PoseGraph**: Stores poses and constraints, with optimization + +mod descriptor; +mod detector; +mod graph; + +pub use descriptor::LidarIris; +pub use detector::{LoopClosure, LoopClosureConfig, LoopClosureDetector}; +pub use graph::{LoopEdge, OdometryEdge, PoseGraph, PoseGraphConfig}; diff --git a/vastu-map/src/slam/matcher.rs b/vastu-map/src/slam/matcher.rs new file mode 100644 index 0000000..fe0c0a7 --- /dev/null +++ b/vastu-map/src/slam/matcher.rs @@ -0,0 +1,1012 @@ +//! Correlative scan-to-map matcher. +//! +//! Performs brute-force search over a 3D pose space (x, y, theta) to find +//! the best alignment of a lidar scan to the current occupancy grid map. + +use crate::core::{CellType, LidarScan, Pose2D, WorldPoint}; +use crate::grid::GridStorage; + +use crate::core::simd::{PointBatch, RotationMatrix4, transform_points_simd4, world_to_grid_simd4}; + +use super::config::CorrelativeMatcherConfig; +use super::types::ScanMatchResult; + +/// Correlative scan-to-map matcher. +/// +/// Uses a brute-force search with optional multi-resolution refinement +/// to find the best alignment of a lidar scan to the map. +pub struct CorrelativeMatcher { + config: CorrelativeMatcherConfig, +} + +/// Scratch buffers for SIMD operations to avoid per-call allocations. +pub struct ScratchBuffers { + /// Transformed world X coordinates + world_xs: Vec, + /// Transformed world Y coordinates + world_ys: Vec, + /// Grid X coordinates + grid_xs: Vec, + /// Grid Y coordinates + grid_ys: Vec, +} + +impl ScratchBuffers { + /// Create scratch buffers for a given number of points. + pub fn new(capacity: usize) -> Self { + Self { + world_xs: vec![0.0; capacity], + world_ys: vec![0.0; capacity], + grid_xs: vec![0; capacity], + grid_ys: vec![0; capacity], + } + } + + /// Resize buffers if needed. + pub fn resize(&mut self, n: usize) { + if self.world_xs.len() < n { + self.world_xs.resize(n, 0.0); + self.world_ys.resize(n, 0.0); + self.grid_xs.resize(n, 0); + self.grid_ys.resize(n, 0); + } + } +} + +impl CorrelativeMatcher { + /// Create a new correlative matcher. + pub fn new(config: CorrelativeMatcherConfig) -> Self { + Self { config } + } + + /// Create with default configuration. + pub fn with_defaults() -> Self { + Self::new(CorrelativeMatcherConfig::default()) + } + + /// Get configuration. + pub fn config(&self) -> &CorrelativeMatcherConfig { + &self.config + } + + /// Match a lidar scan against the occupancy grid map. + /// + /// # Arguments + /// * `scan` - The lidar scan to match + /// * `prior_pose` - Initial pose estimate (e.g., from encoders) + /// * `storage` - The occupancy grid map + /// + /// # Returns + /// The best matching pose and quality metrics. + pub fn match_scan( + &self, + scan: &LidarScan, + prior_pose: Pose2D, + storage: &GridStorage, + ) -> ScanMatchResult { + if !self.config.enabled { + return ScanMatchResult::new(prior_pose, 1.0, 0, 0); + } + + // Extract valid scan points (in sensor frame) + let points = self.extract_scan_points(scan); + if points.is_empty() { + return ScanMatchResult::failed(prior_pose); + } + + if self.config.multi_resolution { + self.match_multi_resolution(&points, prior_pose, storage) + } else { + self.match_single_resolution(&points, prior_pose, storage) + } + } + + /// Extract valid points from scan, optionally subsampling. + fn extract_scan_points(&self, scan: &LidarScan) -> Vec<(f32, f32)> { + let mut points: Vec<(f32, f32)> = scan + .valid_points() + .map(|(angle, range)| { + let x = range * angle.cos(); + let y = range * angle.sin(); + (x, y) + }) + .collect(); + + // Subsample if needed + if self.config.max_points > 0 && points.len() > self.config.max_points { + let step = points.len() / self.config.max_points; + points = points.into_iter().step_by(step).collect(); + } + + points + } + + /// Single resolution search. + fn match_single_resolution( + &self, + points: &[(f32, f32)], + prior_pose: Pose2D, + storage: &GridStorage, + ) -> ScanMatchResult { + self.search_poses( + points, + prior_pose, + prior_pose, // center and prior are the same for single resolution + storage, + self.config.linear_resolution, + self.config.angular_resolution, + ) + } + + /// Multi-resolution search: coarse first, then refine with Gauss-Newton. + fn match_multi_resolution( + &self, + points: &[(f32, f32)], + prior_pose: Pose2D, + storage: &GridStorage, + ) -> ScanMatchResult { + // Coarse search + let coarse_linear = self.config.linear_resolution * self.config.coarse_factor; + let coarse_angular = self.config.angular_resolution * self.config.coarse_factor; + + let coarse_result = self.search_poses( + points, + prior_pose, + prior_pose, + storage, + coarse_linear, + coarse_angular, + ); + + if !coarse_result.converged { + return coarse_result; + } + + // Fine search around coarse result (with tighter window) + // NOTE: We still penalize based on original prior_pose, not coarse_result + let fine_config = CorrelativeMatcherConfig { + search_x: coarse_linear * 1.5, + search_y: coarse_linear * 1.5, + search_theta: coarse_angular * 1.5, + ..self.config.clone() + }; + + let fine_matcher = CorrelativeMatcher::new(fine_config); + let fine_result = fine_matcher.search_poses( + points, + coarse_result.pose, // center on coarse result + prior_pose, // but penalize based on original prior + storage, + self.config.linear_resolution, + self.config.angular_resolution, + ); + + // Gauss-Newton refinement for sub-pixel accuracy + self.refine_with_gauss_newton(points, fine_result, storage) + } + + /// Refine a scan match result using Gauss-Newton optimization. + /// + /// This provides sub-pixel/sub-degree accuracy by minimizing the sum of + /// squared distances from scan points to the nearest walls. + /// + /// Uses Levenberg-Marquardt damping for stability and a prior constraint + /// (like Cartographer's CeresScanMatcher) to prevent over-correction. + fn refine_with_gauss_newton( + &self, + points: &[(f32, f32)], + initial_result: ScanMatchResult, + storage: &GridStorage, + ) -> ScanMatchResult { + if points.is_empty() || self.config.gn_max_iterations == 0 { + return initial_result; + } + + let mut pose = initial_result.pose; + let initial_pose = initial_result.pose; // Save for prior constraint + let (offset_x, offset_y) = self.config.sensor_offset; + + // Prior constraint weights (Cartographer-style) + // These penalize deviation from the correlative search result + let trans_weight = self.config.gn_translation_weight; + let rot_weight = self.config.gn_rotation_weight; + + for _iter in 0..self.config.gn_max_iterations { + // Build normal equations: H * delta = -g + // where H ≈ J^T * J, g = J^T * r + let mut h = [[0.0f32; 3]; 3]; // 3x3 Hessian approximation + let mut g = [0.0f32; 3]; // Gradient + + let cos_theta = pose.theta.cos(); + let sin_theta = pose.theta.sin(); + + // Sensor position in world frame + let sensor_x = pose.x + offset_x * cos_theta - offset_y * sin_theta; + let sensor_y = pose.y + offset_x * sin_theta + offset_y * cos_theta; + + for &(px, py) in points { + // Transform point from sensor frame to world frame + let world_x = sensor_x + px * cos_theta - py * sin_theta; + let world_y = sensor_y + px * sin_theta + py * cos_theta; + + // Get interpolated distance and gradient + let (dist, grad_x, grad_y) = + storage.get_distance_interpolated(WorldPoint::new(world_x, world_y)); + + // Skip points with no gradient info (far from walls) + if dist >= storage.resolution() * 20.0 { + continue; + } + + // Jacobian row: [∂d/∂x, ∂d/∂y, ∂d/∂θ] + // ∂d/∂x = grad_x (direct translation effect) + // ∂d/∂y = grad_y (direct translation effect) + // ∂d/∂θ = grad_x * ∂world_x/∂θ + grad_y * ∂world_y/∂θ + // = grad_x * (-px*sin - py*cos) + grad_y * (px*cos - py*sin) + let dworld_x_dtheta = -px * sin_theta - py * cos_theta; + let dworld_y_dtheta = px * cos_theta - py * sin_theta; + let j_theta = grad_x * dworld_x_dtheta + grad_y * dworld_y_dtheta; + + let j = [grad_x, grad_y, j_theta]; + + // Accumulate H = J^T * J and g = J^T * r + // Using outer product: H += j * j^T, g += j * dist + for i in 0..3 { + g[i] += j[i] * dist; + for k in 0..3 { + h[i][k] += j[i] * j[k]; + } + } + } + + // Add prior constraint (Cartographer-style CeresScanMatcher) + // This penalizes deviation from the correlative search result + // E_prior = trans_weight * ||p - p_init||² + rot_weight * (θ - θ_init)² + // Adds to H diagonal and g vector + if trans_weight > 0.0 { + h[0][0] += trans_weight; + h[1][1] += trans_weight; + g[0] += trans_weight * (pose.x - initial_pose.x); + g[1] += trans_weight * (pose.y - initial_pose.y); + } + if rot_weight > 0.0 { + h[2][2] += rot_weight; + let angle_error = normalize_angle(pose.theta - initial_pose.theta); + g[2] += rot_weight * angle_error; + } + + // Add Levenberg-Marquardt damping to diagonal + let damping = self.config.gn_damping; + for i in 0..3 { + h[i][i] += damping * (1.0 + h[i][i]); + } + + // Solve 3x3 system using Cramer's rule + let delta = solve_3x3(&h, &[-g[0], -g[1], -g[2]]); + + // Check for numerical issues + if delta.iter().any(|&d| !d.is_finite()) { + break; + } + + // Update pose + pose.x += delta[0]; + pose.y += delta[1]; + pose.theta += delta[2]; + + // Normalize theta to [-π, π] + pose.theta = normalize_angle(pose.theta); + + // Check convergence + let update_mag = (delta[0] * delta[0] + delta[1] * delta[1]).sqrt(); + if update_mag < self.config.gn_convergence_threshold { + break; + } + } + + // Recompute score at refined pose + let (_raw_score, hits) = self.score_pose(points, pose, storage); + let normalized_score = if points.is_empty() { + 0.0 + } else { + (hits as f32 / points.len() as f32).min(1.0) + }; + + ScanMatchResult::new(pose, normalized_score, hits, points.len()) + } + + /// Search over pose space to find best match. + /// + /// # Arguments + /// * `center_pose` - The pose to center the search around + /// * `prior_pose` - The original prior pose used for penalty calculation + /// (may differ from center_pose in multi-resolution search) + fn search_poses( + &self, + points: &[(f32, f32)], + center_pose: Pose2D, + prior_pose: Pose2D, + storage: &GridStorage, + linear_step: f32, + angular_step: f32, + ) -> ScanMatchResult { + let mut best_pose = center_pose; + let mut best_score = f32::NEG_INFINITY; + let mut best_hits = 0usize; + + // Generate search poses + let x_steps = (self.config.search_x / linear_step).ceil() as i32; + let y_steps = (self.config.search_y / linear_step).ceil() as i32; + let theta_steps = (self.config.search_theta / angular_step).ceil() as i32; + + for dx in -x_steps..=x_steps { + let x = center_pose.x + dx as f32 * linear_step; + + for dy in -y_steps..=y_steps { + let y = center_pose.y + dy as f32 * linear_step; + + for dtheta in -theta_steps..=theta_steps { + let theta = center_pose.theta + dtheta as f32 * angular_step; + let candidate = Pose2D::new(x, y, theta); + + let (score, hits) = self.score_pose(points, candidate, storage); + + // Add tie-breaking penalty for distance from ORIGINAL prior pose + // This prefers poses closer to odometry when scores are similar + // Position: Higher weight since encoders are typically reliable for distance + // Angular: Lower weight to allow scan matching to correct heading drift + let dist_to_prior = ((candidate.x - prior_pose.x).powi(2) + + (candidate.y - prior_pose.y).powi(2)) + .sqrt(); + let angle_dist = (candidate.theta - prior_pose.theta).abs(); + let adjusted_score = score - dist_to_prior * 300.0 - angle_dist * 30.0; + + if adjusted_score > best_score { + best_score = adjusted_score; + best_pose = candidate; + best_hits = hits; + } + } + } + } + + // Normalize score to 0-1 range + let normalized_score = if points.is_empty() { + 0.0 + } else { + (best_hits as f32 / points.len() as f32).min(1.0) + }; + + ScanMatchResult::new(best_pose, normalized_score, best_hits, points.len()) + } + + /// Score how well a pose aligns the scan points with the map. + /// + /// Returns (raw_score, hit_count). + fn score_pose( + &self, + points: &[(f32, f32)], + pose: Pose2D, + storage: &GridStorage, + ) -> (f32, usize) { + let cos_theta = pose.theta.cos(); + let sin_theta = pose.theta.sin(); + + // Calculate sensor position in world frame (applying sensor offset) + let (offset_x, offset_y) = self.config.sensor_offset; + let sensor_x = pose.x + offset_x * cos_theta - offset_y * sin_theta; + let sensor_y = pose.y + offset_x * sin_theta + offset_y * cos_theta; + + let mut hits = 0usize; + let mut score = 0.0f32; + + // Precompute Gaussian constants + let use_gaussian = self.config.use_gaussian_scoring; + let sigma = self.config.gaussian_sigma; + let two_sigma_sq = 2.0 * sigma * sigma; + // Hit threshold: within 1 sigma of wall + let hit_threshold = sigma; + + for &(px, py) in points { + // Transform point from sensor frame to world frame + let world_x = sensor_x + px * cos_theta - py * sin_theta; + let world_y = sensor_y + px * sin_theta + py * cos_theta; + + let world_point = WorldPoint::new(world_x, world_y); + let grid_coord = storage.world_to_grid(world_point); + + // Check if point is within map bounds + if !storage.is_valid_coord(grid_coord) { + continue; + } + + if use_gaussian { + // Gaussian scoring based on distance field + let distance = storage.get_distance(grid_coord); + + if distance < f32::MAX { + // Gaussian score: exp(-d²/(2σ²)) + // Distance 0 → score 1.0 + // Distance sigma → score ~0.6 + // Distance 2*sigma → score ~0.14 + let point_score = (-distance * distance / two_sigma_sq).exp(); + score += point_score; + + if distance <= hit_threshold { + hits += 1; + } + } else { + // No wall nearby (unknown area) - small bonus for exploration + score += 0.1; + } + } else { + // Binary scoring (original behavior) + match storage.get_type(grid_coord) { + CellType::Wall => { + hits += 1; + score += 1.0; + } + CellType::Floor => { + score -= 1.0; + } + CellType::Unknown => { + score += 0.1; + } + _ => {} + } + } + } + + (score, hits) + } + + // ========================================================================= + // SIMD-OPTIMIZED METHODS + // ========================================================================= + + /// Match a lidar scan using SIMD-optimized batch operations. + /// + /// This method pre-converts scan points to SoA format and uses SIMD + /// for point transformations. Reusable scratch buffers avoid allocations. + pub fn match_scan_simd( + &self, + scan: &LidarScan, + prior_pose: Pose2D, + storage: &GridStorage, + scratch: &mut ScratchBuffers, + ) -> ScanMatchResult { + if !self.config.enabled { + return ScanMatchResult::new(prior_pose, 1.0, 0, 0); + } + + // Extract valid scan points in SoA format + let points = self.extract_scan_points_simd(scan); + if points.is_empty() { + return ScanMatchResult::failed(prior_pose); + } + + // Ensure scratch buffers are large enough + scratch.resize(points.len()); + + if self.config.multi_resolution { + self.match_multi_resolution_simd(&points, prior_pose, storage, scratch) + } else { + self.match_single_resolution_simd(&points, prior_pose, storage, scratch) + } + } + + /// Extract valid points from scan in SoA format for SIMD processing. + fn extract_scan_points_simd(&self, scan: &LidarScan) -> PointBatch { + let mut xs = Vec::with_capacity(scan.ranges.len()); + let mut ys = Vec::with_capacity(scan.ranges.len()); + + for (angle, range) in scan.valid_points() { + xs.push(range * angle.cos()); + ys.push(range * angle.sin()); + } + + // Subsample if needed + if self.config.max_points > 0 && xs.len() > self.config.max_points { + let step = xs.len() / self.config.max_points; + xs = xs.into_iter().step_by(step).collect(); + ys = ys.into_iter().step_by(step).collect(); + } + + PointBatch { xs, ys } + } + + /// Single resolution search with SIMD. + fn match_single_resolution_simd( + &self, + points: &PointBatch, + prior_pose: Pose2D, + storage: &GridStorage, + scratch: &mut ScratchBuffers, + ) -> ScanMatchResult { + self.search_poses_simd( + points, + prior_pose, + prior_pose, // center and prior are the same for single resolution + storage, + self.config.linear_resolution, + self.config.angular_resolution, + scratch, + ) + } + + /// Multi-resolution search with SIMD: coarse first, then refine with Gauss-Newton. + fn match_multi_resolution_simd( + &self, + points: &PointBatch, + prior_pose: Pose2D, + storage: &GridStorage, + scratch: &mut ScratchBuffers, + ) -> ScanMatchResult { + // Coarse search + let coarse_linear = self.config.linear_resolution * self.config.coarse_factor; + let coarse_angular = self.config.angular_resolution * self.config.coarse_factor; + + let coarse_result = self.search_poses_simd( + points, + prior_pose, + prior_pose, + storage, + coarse_linear, + coarse_angular, + scratch, + ); + + if !coarse_result.converged { + return coarse_result; + } + + // Fine search around coarse result (with tighter window) + // NOTE: We still penalize based on original prior_pose, not coarse_result + let fine_config = CorrelativeMatcherConfig { + search_x: coarse_linear * 1.5, + search_y: coarse_linear * 1.5, + search_theta: coarse_angular * 1.5, + ..self.config.clone() + }; + + let fine_matcher = CorrelativeMatcher::new(fine_config); + let fine_result = fine_matcher.search_poses_simd( + points, + coarse_result.pose, // center on coarse result + prior_pose, // but penalize based on original prior + storage, + self.config.linear_resolution, + self.config.angular_resolution, + scratch, + ); + + // Gauss-Newton refinement for sub-pixel accuracy + // Convert PointBatch to Vec<(f32, f32)> for refinement + let points_vec: Vec<(f32, f32)> = points + .xs + .iter() + .zip(points.ys.iter()) + .map(|(&x, &y)| (x, y)) + .collect(); + self.refine_with_gauss_newton(&points_vec, fine_result, storage) + } + + /// Search over pose space to find best match using SIMD. + fn search_poses_simd( + &self, + points: &PointBatch, + center_pose: Pose2D, + prior_pose: Pose2D, + storage: &GridStorage, + linear_step: f32, + angular_step: f32, + scratch: &mut ScratchBuffers, + ) -> ScanMatchResult { + let mut best_pose = center_pose; + let mut best_score = f32::NEG_INFINITY; + let mut best_hits = 0usize; + + // Generate search poses + let x_steps = (self.config.search_x / linear_step).ceil() as i32; + let y_steps = (self.config.search_y / linear_step).ceil() as i32; + let theta_steps = (self.config.search_theta / angular_step).ceil() as i32; + + for dx in -x_steps..=x_steps { + let x = center_pose.x + dx as f32 * linear_step; + + for dy in -y_steps..=y_steps { + let y = center_pose.y + dy as f32 * linear_step; + + for dtheta in -theta_steps..=theta_steps { + let theta = center_pose.theta + dtheta as f32 * angular_step; + let candidate = Pose2D::new(x, y, theta); + + let (score, hits) = self.score_pose_simd(points, candidate, storage, scratch); + + // Add tie-breaking penalty for distance from ORIGINAL prior pose + // This prefers poses closer to odometry when scores are similar + // Position: Higher weight since encoders are typically reliable for distance + // Angular: Lower weight to allow scan matching to correct heading drift + let dist_to_prior = ((candidate.x - prior_pose.x).powi(2) + + (candidate.y - prior_pose.y).powi(2)) + .sqrt(); + let angle_dist = (candidate.theta - prior_pose.theta).abs(); + let adjusted_score = score - dist_to_prior * 300.0 - angle_dist * 30.0; + + if adjusted_score > best_score { + best_score = adjusted_score; + best_pose = candidate; + best_hits = hits; + } + } + } + } + + // Normalize score to 0-1 range + let normalized_score = if points.is_empty() { + 0.0 + } else { + (best_hits as f32 / points.len() as f32).min(1.0) + }; + + ScanMatchResult::new(best_pose, normalized_score, best_hits, points.len()) + } + + /// Score how well a pose aligns the scan points with the map using SIMD. + /// + /// Uses SIMD for batch point transformation and world-to-grid conversion. + /// The final scoring loop is scalar due to random memory access patterns. + fn score_pose_simd( + &self, + points: &PointBatch, + pose: Pose2D, + storage: &GridStorage, + scratch: &mut ScratchBuffers, + ) -> (f32, usize) { + let n = points.len(); + if n == 0 { + return (0.0, 0); + } + + // Calculate sensor position in world frame (applying sensor offset) + let (offset_x, offset_y) = self.config.sensor_offset; + let cos_theta = pose.theta.cos(); + let sin_theta = pose.theta.sin(); + let sensor_x = pose.x + offset_x * cos_theta - offset_y * sin_theta; + let sensor_y = pose.y + offset_x * sin_theta + offset_y * cos_theta; + + // Create a pose with sensor position for SIMD transform + let sensor_pose = Pose2D::new(sensor_x, sensor_y, pose.theta); + + // 1. Batch transform points from sensor frame to world frame (SIMD) + let rot = RotationMatrix4::from_pose(&sensor_pose); + transform_points_simd4( + points, + &rot, + &mut scratch.world_xs[..n], + &mut scratch.world_ys[..n], + ); + + // 2. Batch convert world to grid coordinates (SIMD) + let inv_resolution = 1.0 / storage.resolution(); + world_to_grid_simd4( + &scratch.world_xs[..n], + &scratch.world_ys[..n], + storage.origin().x, + storage.origin().y, + inv_resolution, + &mut scratch.grid_xs[..n], + &mut scratch.grid_ys[..n], + ); + + // 3. Score each point (scalar - random memory access) + let mut hits = 0usize; + let mut score = 0.0f32; + + let width = storage.width() as i32; + let height = storage.height() as i32; + + // Precompute Gaussian constants + let use_gaussian = self.config.use_gaussian_scoring; + let sigma = self.config.gaussian_sigma; + let two_sigma_sq = 2.0 * sigma * sigma; + let hit_threshold = sigma; + + for i in 0..n { + let gx = scratch.grid_xs[i]; + let gy = scratch.grid_ys[i]; + + // Check if point is within map bounds + if gx < 0 || gx >= width || gy < 0 || gy >= height { + continue; + } + + let coord = crate::core::GridCoord::new(gx, gy); + + if use_gaussian { + // Gaussian scoring based on distance field + let distance = storage.get_distance(coord); + + if distance < f32::MAX { + let point_score = (-distance * distance / two_sigma_sq).exp(); + score += point_score; + + if distance <= hit_threshold { + hits += 1; + } + } else { + score += 0.1; + } + } else { + // Binary scoring (original behavior) + match storage.get_type(coord) { + CellType::Wall => { + hits += 1; + score += 1.0; + } + CellType::Floor => { + score -= 1.0; + } + CellType::Unknown => { + score += 0.1; + } + _ => {} + } + } + } + + (score, hits) + } +} + +/// Solve a 3x3 linear system Ax = b using Cramer's rule. +/// +/// Returns [0, 0, 0] if the matrix is singular. +fn solve_3x3(a: &[[f32; 3]; 3], b: &[f32; 3]) -> [f32; 3] { + // Compute determinant of A + let det = a[0][0] * (a[1][1] * a[2][2] - a[1][2] * a[2][1]) + - a[0][1] * (a[1][0] * a[2][2] - a[1][2] * a[2][0]) + + a[0][2] * (a[1][0] * a[2][1] - a[1][1] * a[2][0]); + + if det.abs() < 1e-10 { + return [0.0, 0.0, 0.0]; + } + + let inv_det = 1.0 / det; + + // Cramer's rule: x_i = det(A_i) / det(A) + // where A_i is A with column i replaced by b + let x0 = (b[0] * (a[1][1] * a[2][2] - a[1][2] * a[2][1]) + - a[0][1] * (b[1] * a[2][2] - a[1][2] * b[2]) + + a[0][2] * (b[1] * a[2][1] - a[1][1] * b[2])) + * inv_det; + + let x1 = (a[0][0] * (b[1] * a[2][2] - a[1][2] * b[2]) + - b[0] * (a[1][0] * a[2][2] - a[1][2] * a[2][0]) + + a[0][2] * (a[1][0] * b[2] - b[1] * a[2][0])) + * inv_det; + + let x2 = (a[0][0] * (a[1][1] * b[2] - b[1] * a[2][1]) + - a[0][1] * (a[1][0] * b[2] - b[1] * a[2][0]) + + b[0] * (a[1][0] * a[2][1] - a[1][1] * a[2][0])) + * inv_det; + + [x0, x1, x2] +} + +/// Normalize angle to [-π, π). +fn normalize_angle(angle: f32) -> f32 { + let mut a = angle % std::f32::consts::TAU; + if a > std::f32::consts::PI { + a -= std::f32::consts::TAU; + } else if a < -std::f32::consts::PI { + a += std::f32::consts::TAU; + } + a +} + +#[cfg(test)] +mod tests { + use super::*; + use crate::core::GridCoord; + + fn create_test_storage() -> GridStorage { + GridStorage::centered(100, 100, 0.05) // 5m x 5m at 5cm resolution + } + + fn create_wall_in_front(storage: &mut GridStorage, pose: Pose2D, distance: f32) { + // Create a wall in front of the pose + let wall_x = pose.x + distance * pose.theta.cos(); + let wall_y = pose.y + distance * pose.theta.sin(); + + // Create a line of wall cells + for offset in -10..=10 { + let perp_angle = pose.theta + std::f32::consts::FRAC_PI_2; + let wx = wall_x + offset as f32 * 0.05 * perp_angle.cos(); + let wy = wall_y + offset as f32 * 0.05 * perp_angle.sin(); + let coord = storage.world_to_grid(WorldPoint::new(wx, wy)); + if storage.is_valid_coord(coord) { + storage.set_type(coord, CellType::Wall); + } + } + + // Update distance field for Gaussian scoring + storage.recompute_distance_field(); + } + + fn create_simple_scan(distance: f32, num_points: usize) -> LidarScan { + // Create a scan with points at uniform angles, all at same distance + let angle_min = -std::f32::consts::PI; + let angle_max = std::f32::consts::PI; + let angle_increment = (angle_max - angle_min) / num_points as f32; + + let ranges = vec![distance; num_points]; + let angles: Vec = (0..num_points) + .map(|i| angle_min + i as f32 * angle_increment) + .collect(); + + LidarScan::new(ranges, angles, 0.1, 10.0) + } + + #[test] + fn test_matcher_creation() { + let matcher = CorrelativeMatcher::with_defaults(); + assert!(matcher.config().enabled); + } + + #[test] + fn test_disabled_matcher() { + let config = CorrelativeMatcherConfig::disabled(); + let matcher = CorrelativeMatcher::new(config); + + let storage = create_test_storage(); + let scan = create_simple_scan(2.0, 360); + let pose = Pose2D::new(0.0, 0.0, 0.0); + + let result = matcher.match_scan(&scan, pose, &storage); + + // Disabled matcher should return input pose unchanged + assert!((result.pose.x - pose.x).abs() < 0.001); + assert!((result.pose.y - pose.y).abs() < 0.001); + } + + #[test] + fn test_matching_returns_result() { + let mut storage = create_test_storage(); + let true_pose = Pose2D::new(0.0, 0.0, 0.0); + + // Create a wall 2m in front + create_wall_in_front(&mut storage, true_pose, 2.0); + + // Use fast config for speed + let config = CorrelativeMatcherConfig::fast(); + let matcher = CorrelativeMatcher::new(config); + + // Test with prior very close to true pose + let prior = Pose2D::new(0.01, 0.01, 0.01); + + // Create a scan that simulates hitting the wall from the prior pose + let angles: Vec = (-10..=10).map(|i| i as f32 * 0.05).collect(); + let ranges: Vec = vec![2.0; angles.len()]; + let scan = LidarScan::new(ranges, angles, 0.1, 10.0); + + let result = matcher.match_scan(&scan, prior, &storage); + + // Matcher should return a result (may or may not converge depending on map state) + // At minimum, it should return a valid pose + assert!(result.pose.x.is_finite()); + assert!(result.pose.y.is_finite()); + assert!(result.pose.theta.is_finite()); + } + + #[test] + fn test_score_increases_with_hits() { + let mut storage = create_test_storage(); + + // Fill some cells with walls + for x in 40..60 { + for y in 40..60 { + storage.set_type(GridCoord::new(x, y), CellType::Wall); + } + } + + // Update distance field for Gaussian scoring + storage.recompute_distance_field(); + + let matcher = CorrelativeMatcher::with_defaults(); + let points: Vec<(f32, f32)> = vec![(0.0, 0.0), (0.05, 0.0), (0.1, 0.0)]; + + // Pose in wall area should score higher + let pose_in_wall = Pose2D::new(0.0, 0.0, 0.0); + let pose_in_floor = Pose2D::new(-2.0, -2.0, 0.0); + + let (score_wall, hits_wall) = matcher.score_pose(&points, pose_in_wall, &storage); + let (score_floor, hits_floor) = matcher.score_pose(&points, pose_in_floor, &storage); + + assert!(hits_wall > hits_floor); + assert!(score_wall > score_floor); + } + + #[test] + fn test_simd_matches_scalar() { + let mut storage = create_test_storage(); + let true_pose = Pose2D::new(0.0, 0.0, 0.0); + + // Create a wall 2m in front + create_wall_in_front(&mut storage, true_pose, 2.0); + + // Use fast config for speed + let config = CorrelativeMatcherConfig::fast(); + let matcher = CorrelativeMatcher::new(config); + + // Create a scan + let angles: Vec = (-10..=10).map(|i| i as f32 * 0.05).collect(); + let ranges: Vec = vec![2.0; angles.len()]; + let scan = LidarScan::new(ranges, angles, 0.1, 10.0); + + let prior = Pose2D::new(0.01, 0.01, 0.01); + + // Get scalar result + let scalar_result = matcher.match_scan(&scan, prior, &storage); + + // Get SIMD result + let mut scratch = ScratchBuffers::new(scan.ranges.len()); + let simd_result = matcher.match_scan_simd(&scan, prior, &storage, &mut scratch); + + // Results should match + assert!((scalar_result.pose.x - simd_result.pose.x).abs() < 0.001); + assert!((scalar_result.pose.y - simd_result.pose.y).abs() < 0.001); + assert!((scalar_result.pose.theta - simd_result.pose.theta).abs() < 0.001); + assert!((scalar_result.score - simd_result.score).abs() < 0.001); + assert_eq!(scalar_result.hits, simd_result.hits); + } + + #[test] + fn test_simd_score_matches_scalar() { + let mut storage = create_test_storage(); + + // Fill some cells with walls + for x in 40..60 { + for y in 40..60 { + storage.set_type(GridCoord::new(x, y), CellType::Wall); + } + } + + // Update distance field for Gaussian scoring + storage.recompute_distance_field(); + + let matcher = CorrelativeMatcher::with_defaults(); + + // Create points in AoS format for scalar + let points: Vec<(f32, f32)> = vec![(0.0, 0.0), (0.05, 0.0), (0.1, 0.0), (0.15, 0.0)]; + + // Convert to SoA format for SIMD + let points_soa = PointBatch { + xs: points.iter().map(|(x, _)| *x).collect(), + ys: points.iter().map(|(_, y)| *y).collect(), + }; + + let pose = Pose2D::new(0.0, 0.0, 0.0); + let mut scratch = ScratchBuffers::new(points.len()); + + // Get scalar score + let (scalar_score, scalar_hits) = matcher.score_pose(&points, pose, &storage); + + // Get SIMD score + let (simd_score, simd_hits) = + matcher.score_pose_simd(&points_soa, pose, &storage, &mut scratch); + + // Results should match + assert!( + (scalar_score - simd_score).abs() < 0.001, + "score mismatch: {} vs {}", + scalar_score, + simd_score + ); + assert_eq!( + scalar_hits, simd_hits, + "hits mismatch: {} vs {}", + scalar_hits, simd_hits + ); + } +} diff --git a/vastu-map/src/slam/mod.rs b/vastu-map/src/slam/mod.rs new file mode 100644 index 0000000..d074cab --- /dev/null +++ b/vastu-map/src/slam/mod.rs @@ -0,0 +1,36 @@ +//! SLAM (Simultaneous Localization and Mapping) module. +//! +//! This module provides scan-to-map matching and loop closure capabilities +//! for correcting encoder drift during autonomous exploration. +//! +//! # Components +//! +//! - **Correlative Matcher**: Brute-force search for best scan alignment +//! - **Loop Closure**: Detects revisited locations using LiDAR-IRIS descriptors +//! - **Pose Graph**: Stores and optimizes pose constraints +//! +//! # Usage +//! +//! ```ignore +//! use vastu_map::slam::{CorrelativeMatcher, CorrelativeMatcherConfig}; +//! +//! let config = CorrelativeMatcherConfig::default(); +//! let matcher = CorrelativeMatcher::new(config); +//! +//! // Match scan against current map +//! let result = matcher.match_scan(&scan, encoder_pose, &storage); +//! if result.converged { +//! // Use refined pose +//! let corrected_pose = result.pose; +//! } +//! ``` + +mod config; +mod matcher; +mod types; + +pub mod loop_closure; + +pub use config::CorrelativeMatcherConfig; +pub use matcher::CorrelativeMatcher; +pub use types::{MatchQuality, ScanMatchResult}; diff --git a/vastu-map/src/slam/types.rs b/vastu-map/src/slam/types.rs new file mode 100644 index 0000000..6ee3bc0 --- /dev/null +++ b/vastu-map/src/slam/types.rs @@ -0,0 +1,109 @@ +//! SLAM type definitions. + +use crate::core::Pose2D; + +/// Result of a scan-to-map matching operation. +#[derive(Clone, Debug)] +pub struct ScanMatchResult { + /// The refined pose after matching. + pub pose: Pose2D, + /// Match quality score (0.0 to 1.0). + /// Higher is better alignment. + pub score: f32, + /// Whether the matching converged to a good solution. + pub converged: bool, + /// Quality classification of the match. + pub quality: MatchQuality, + /// Number of scan points that hit occupied cells. + pub hits: usize, + /// Total number of valid scan points used. + pub total_points: usize, +} + +impl ScanMatchResult { + /// Create a new match result. + pub fn new(pose: Pose2D, score: f32, hits: usize, total_points: usize) -> Self { + let quality = MatchQuality::from_score(score); + // Accept Marginal quality during exploration when map is sparse + let converged = matches!( + quality, + MatchQuality::Good | MatchQuality::Excellent | MatchQuality::Marginal + ); + + Self { + pose, + score, + converged, + quality, + hits, + total_points, + } + } + + /// Create a failed match result (no valid match found). + pub fn failed(prior_pose: Pose2D) -> Self { + Self { + pose: prior_pose, + score: 0.0, + converged: false, + quality: MatchQuality::Failed, + hits: 0, + total_points: 0, + } + } + + /// Hit ratio (0.0 to 1.0). + pub fn hit_ratio(&self) -> f32 { + if self.total_points == 0 { + 0.0 + } else { + self.hits as f32 / self.total_points as f32 + } + } +} + +impl Default for ScanMatchResult { + fn default() -> Self { + Self::failed(Pose2D::default()) + } +} + +/// Quality classification of a scan match. +#[derive(Clone, Copy, Debug, PartialEq, Eq)] +pub enum MatchQuality { + /// Excellent match (score > 0.8). + Excellent, + /// Good match (score > 0.6). + Good, + /// Marginal match (score > 0.4). + Marginal, + /// Poor match (score > 0.2). + Poor, + /// Match failed (score <= 0.2). + Failed, +} + +impl MatchQuality { + /// Classify a match score. + pub fn from_score(score: f32) -> Self { + if score > 0.8 { + MatchQuality::Excellent + } else if score > 0.6 { + MatchQuality::Good + } else if score > 0.4 { + MatchQuality::Marginal + } else if score > 0.2 { + MatchQuality::Poor + } else { + MatchQuality::Failed + } + } + + /// Check if this quality is acceptable for pose correction. + pub fn is_acceptable(&self) -> bool { + matches!( + self, + MatchQuality::Excellent | MatchQuality::Good | MatchQuality::Marginal + ) + } +} diff --git a/vastu-map/src/submap/config.rs b/vastu-map/src/submap/config.rs new file mode 100644 index 0000000..35dd88c --- /dev/null +++ b/vastu-map/src/submap/config.rs @@ -0,0 +1,216 @@ +//! Submap configuration. + +use serde::{Deserialize, Serialize}; + +/// Configuration for submap management. +#[derive(Clone, Debug, Serialize, Deserialize)] +pub struct SubmapConfig { + /// Number of scans before transitioning to Filling state. + /// Default: 50 + pub scans_per_submap: usize, + + /// Additional scans during Filling state (overlap period). + /// These scans are added to both the filling submap and the new active submap. + /// Default: 10 + pub overlap_scans: usize, + + /// Local grid size in cells (square). + /// Default: 200 (5m × 5m at 2.5cm resolution) + pub local_grid_size: usize, + + /// Grid resolution in meters per cell. + /// Should match the global grid resolution. + /// Default: 0.025 (2.5cm) + pub resolution: f32, + + /// Distance traveled threshold for creating new submap (meters). + /// If robot travels more than this within a submap, trigger new submap. + /// Default: 3.0 + pub max_distance_per_submap: f32, + + /// Maximum number of submaps to keep in memory. + /// Oldest finalized submaps may be evicted if exceeded. + /// Default: 100 (sufficient for ~100m² home) + pub max_submaps: usize, + + /// Minimum gap (in submap IDs) before considering loop closure. + /// Prevents false positives from recent submaps. + /// Default: 3 + pub min_loop_closure_gap: usize, + + /// Maximum distance (meters) for loop closure candidates. + /// Submaps further than this are not considered for loop closure. + /// Default: 10.0 + pub max_loop_closure_distance: f32, +} + +impl Default for SubmapConfig { + fn default() -> Self { + Self { + scans_per_submap: 50, + overlap_scans: 10, + local_grid_size: 200, + resolution: 0.025, + max_distance_per_submap: 3.0, + max_submaps: 100, + min_loop_closure_gap: 3, + max_loop_closure_distance: 10.0, + } + } +} + +impl SubmapConfig { + /// Create a config optimized for small rooms (< 20m²). + pub fn small_room() -> Self { + Self { + scans_per_submap: 30, + overlap_scans: 5, + local_grid_size: 160, // 4m × 4m + max_distance_per_submap: 2.0, + max_submaps: 50, + ..Default::default() + } + } + + /// Create a config optimized for large spaces (> 50m²). + pub fn large_space() -> Self { + Self { + scans_per_submap: 80, + overlap_scans: 15, + local_grid_size: 300, // 7.5m × 7.5m + max_distance_per_submap: 5.0, + max_submaps: 200, + ..Default::default() + } + } + + /// Validate the configuration. + pub fn validate(&self) -> Result<(), ConfigValidationError> { + if self.scans_per_submap == 0 { + return Err(ConfigValidationError::InvalidValue( + "scans_per_submap must be > 0".to_string(), + )); + } + + if self.local_grid_size < 50 { + return Err(ConfigValidationError::InvalidValue( + "local_grid_size must be >= 50".to_string(), + )); + } + + if self.resolution <= 0.0 { + return Err(ConfigValidationError::InvalidValue( + "resolution must be > 0".to_string(), + )); + } + + if self.max_distance_per_submap <= 0.0 { + return Err(ConfigValidationError::InvalidValue( + "max_distance_per_submap must be > 0".to_string(), + )); + } + + // Check that local grid can fit the distance + let grid_extent = self.local_grid_size as f32 * self.resolution; + if self.max_distance_per_submap > grid_extent { + return Err(ConfigValidationError::InvalidValue(format!( + "max_distance_per_submap ({:.1}m) exceeds local grid extent ({:.1}m)", + self.max_distance_per_submap, grid_extent + ))); + } + + Ok(()) + } + + /// Get the local grid extent in meters. + #[inline] + pub fn local_grid_extent(&self) -> f32 { + self.local_grid_size as f32 * self.resolution + } + + /// Get approximate memory per submap in bytes. + pub fn estimated_memory_per_submap(&self) -> usize { + // Grid: cells × ~6 bytes per cell (type, confidence, etc.) + let grid_bytes = self.local_grid_size * self.local_grid_size * 6; + + // Scans: ~1.4KB per scan (360 rays × 4 bytes each for ranges + angles) + let scans_bytes = (self.scans_per_submap + self.overlap_scans) * 1400; + + grid_bytes + scans_bytes + } + + /// Get total estimated memory for max submaps. + pub fn estimated_total_memory(&self) -> usize { + self.estimated_memory_per_submap() * self.max_submaps + } +} + +/// Error during configuration validation. +#[derive(Clone, Debug)] +pub enum ConfigValidationError { + /// Invalid configuration value. + InvalidValue(String), +} + +impl std::fmt::Display for ConfigValidationError { + fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result { + match self { + ConfigValidationError::InvalidValue(msg) => write!(f, "Invalid config: {}", msg), + } + } +} + +impl std::error::Error for ConfigValidationError {} + +#[cfg(test)] +mod tests { + use super::*; + + #[test] + fn test_default_config() { + let config = SubmapConfig::default(); + assert_eq!(config.scans_per_submap, 50); + assert_eq!(config.overlap_scans, 10); + assert_eq!(config.local_grid_size, 200); + assert!((config.resolution - 0.025).abs() < 1e-6); + } + + #[test] + fn test_config_validation() { + let config = SubmapConfig::default(); + assert!(config.validate().is_ok()); + + let mut bad_config = SubmapConfig::default(); + bad_config.scans_per_submap = 0; + assert!(bad_config.validate().is_err()); + + let mut bad_config = SubmapConfig::default(); + bad_config.resolution = -1.0; + assert!(bad_config.validate().is_err()); + } + + #[test] + fn test_local_grid_extent() { + let config = SubmapConfig::default(); + let extent = config.local_grid_extent(); + // 200 cells × 0.025m = 5.0m + assert!((extent - 5.0).abs() < 1e-6); + } + + #[test] + fn test_memory_estimation() { + let config = SubmapConfig::default(); + let per_submap = config.estimated_memory_per_submap(); + + // Grid: 200 × 200 × 6 = 240KB + // Scans: 60 × 1.4KB = 84KB + // Total: ~324KB + assert!(per_submap > 300_000); + assert!(per_submap < 400_000); + + // Total for 100 submaps: ~32MB + let total = config.estimated_total_memory(); + assert!(total > 30_000_000); + assert!(total < 40_000_000); + } +} diff --git a/vastu-map/src/submap/graph.rs b/vastu-map/src/submap/graph.rs new file mode 100644 index 0000000..8750071 --- /dev/null +++ b/vastu-map/src/submap/graph.rs @@ -0,0 +1,557 @@ +//! Submap-level pose graph for SLAM optimization. +//! +//! Unlike the scan-level pose graph, this graph operates on submap origins, +//! resulting in ~50× fewer nodes for the same trajectory length. + +use serde::{Deserialize, Serialize}; + +use crate::core::Pose2D; + +use super::manager::SubmapCorrection; +use super::types::SubmapId; + +/// Configuration for submap pose graph optimization. +#[derive(Clone, Debug, Serialize, Deserialize)] +pub struct SubmapGraphConfig { + /// Maximum optimization iterations. + pub max_iterations: usize, + + /// Convergence threshold for pose change (meters). + pub convergence_threshold: f32, + + /// Levenberg-Marquardt damping factor. + pub damping: f32, + + /// Information weight for odometry edges (between consecutive submaps). + pub odometry_weight: f32, + + /// Information weight for loop closure edges. + pub loop_weight: f32, + + /// Huber loss scale parameter (meters). + /// Residuals larger than this are downweighted. + pub huber_scale: f32, +} + +impl Default for SubmapGraphConfig { + fn default() -> Self { + Self { + max_iterations: 50, // More iterations since fewer nodes + convergence_threshold: 1e-4, + damping: 1e-3, + odometry_weight: 100.0, + loop_weight: 50.0, + huber_scale: 0.1, // 10cm + } + } +} + +/// Node in the submap pose graph. +#[derive(Clone, Debug)] +pub struct SubmapNode { + /// Submap identifier. + pub id: SubmapId, + + /// Current pose estimate (optimizable). + pub pose: Pose2D, + + /// Whether this node is fixed (anchor). + pub fixed: bool, +} + +/// Odometry edge between consecutive submaps. +#[derive(Clone, Debug)] +pub struct SubmapOdometryEdge { + /// From submap ID. + pub from_id: SubmapId, + + /// To submap ID. + pub to_id: SubmapId, + + /// Measured relative pose (accumulated from scans within submaps). + pub measurement: Pose2D, + + /// Information weight. + pub weight: f32, +} + +/// Loop closure edge between non-consecutive submaps. +#[derive(Clone, Debug)] +pub struct SubmapLoopEdge { + /// From submap ID. + pub from_id: SubmapId, + + /// To submap ID. + pub to_id: SubmapId, + + /// Measured relative pose from scan matching. + pub measurement: Pose2D, + + /// Information weight. + pub weight: f32, + + /// Confidence in this loop closure. + pub confidence: f32, +} + +/// Loop closure detected between submaps. +#[derive(Clone, Debug)] +pub struct SubmapLoopClosure { + /// Submap where loop was detected. + pub from_submap: SubmapId, + + /// Matched submap (older). + pub to_submap: SubmapId, + + /// Relative pose from scan matching. + pub relative_pose: Pose2D, + + /// Match confidence (0-1). + pub confidence: f32, +} + +/// Submap-level pose graph. +/// +/// Operates on submap origins instead of individual scan poses. +/// This results in much faster optimization with similar accuracy. +pub struct SubmapPoseGraph { + /// Configuration. + config: SubmapGraphConfig, + + /// Submap nodes. + nodes: Vec, + + /// Odometry edges (between consecutive submaps). + odometry_edges: Vec, + + /// Loop closure edges. + loop_edges: Vec, +} + +impl SubmapPoseGraph { + /// Create a new submap pose graph. + pub fn new(config: SubmapGraphConfig) -> Self { + Self { + config, + nodes: Vec::new(), + odometry_edges: Vec::new(), + loop_edges: Vec::new(), + } + } + + /// Create with default configuration. + pub fn with_defaults() -> Self { + Self::new(SubmapGraphConfig::default()) + } + + /// Get number of nodes. + pub fn node_count(&self) -> usize { + self.nodes.len() + } + + /// Check if graph is empty. + pub fn is_empty(&self) -> bool { + self.nodes.is_empty() + } + + /// Get all nodes. + pub fn nodes(&self) -> &[SubmapNode] { + &self.nodes + } + + /// Get odometry edges. + pub fn odometry_edges(&self) -> &[SubmapOdometryEdge] { + &self.odometry_edges + } + + /// Get loop edges. + pub fn loop_edges(&self) -> &[SubmapLoopEdge] { + &self.loop_edges + } + + /// Get node by submap ID. + pub fn get_node(&self, id: SubmapId) -> Option<&SubmapNode> { + self.nodes.iter().find(|n| n.id == id) + } + + /// Get node index by submap ID. + #[allow(dead_code)] + fn node_index(&self, id: SubmapId) -> Option { + self.nodes.iter().position(|n| n.id == id) + } + + /// Add a new submap node. + /// + /// If this is not the first node, also creates an odometry edge + /// from the previous submap. + pub fn add_submap(&mut self, id: SubmapId, origin: Pose2D, odom_from_previous: Option) { + let is_first = self.nodes.is_empty(); + + self.nodes.push(SubmapNode { + id, + pose: origin, + fixed: is_first, // First node is anchor + }); + + // Add odometry edge from previous submap + if let Some(delta) = odom_from_previous + && !is_first + { + let prev_id = self.nodes[self.nodes.len() - 2].id; + self.odometry_edges.push(SubmapOdometryEdge { + from_id: prev_id, + to_id: id, + measurement: delta, + weight: self.config.odometry_weight, + }); + } + } + + /// Add a loop closure constraint. + pub fn add_loop_closure(&mut self, closure: SubmapLoopClosure) { + self.loop_edges.push(SubmapLoopEdge { + from_id: closure.from_submap, + to_id: closure.to_submap, + measurement: closure.relative_pose, + weight: self.config.loop_weight * closure.confidence, + confidence: closure.confidence, + }); + } + + /// Check if graph has any loop closures. + pub fn has_loops(&self) -> bool { + !self.loop_edges.is_empty() + } + + /// Optimize the pose graph. + /// + /// Returns (iterations, corrections) where corrections can be applied + /// to the SubmapManager. + pub fn optimize(&mut self) -> (usize, Vec) { + if self.nodes.len() < 2 { + return (0, Vec::new()); + } + + let mut iterations = 0; + for i in 0..self.config.max_iterations { + iterations = i + 1; + let max_update = self.optimization_step(); + + if max_update < self.config.convergence_threshold { + break; + } + } + + // Generate corrections + let corrections = self + .nodes + .iter() + .filter(|n| !n.fixed) + .map(|n| SubmapCorrection { + submap_id: n.id, + new_origin: n.pose, + }) + .collect(); + + (iterations, corrections) + } + + /// Perform one optimization step using coordinate descent. + /// + /// Returns the maximum pose update magnitude. + fn optimization_step(&mut self) -> f32 { + let mut max_update = 0.0f32; + + // Update each non-fixed node + for i in 0..self.nodes.len() { + if self.nodes[i].fixed { + continue; + } + + let node_id = self.nodes[i].id; + let (dx, dy, dtheta) = self.compute_node_update(node_id); + + // Apply update with damping + let damping = self.config.damping; + let update_mag = (dx * dx + dy * dy + dtheta * dtheta).sqrt(); + + self.nodes[i].pose.x += dx / (1.0 + damping); + self.nodes[i].pose.y += dy / (1.0 + damping); + self.nodes[i].pose.theta = + normalize_angle(self.nodes[i].pose.theta + dtheta / (1.0 + damping)); + + max_update = max_update.max(update_mag); + } + + max_update + } + + /// Compute update for a single node. + fn compute_node_update(&self, node_id: SubmapId) -> (f32, f32, f32) { + let node_pose = self.get_node(node_id).unwrap().pose; + let huber_scale = self.config.huber_scale; + + let mut sum_x = 0.0f32; + let mut sum_y = 0.0f32; + let mut sum_theta = 0.0f32; + let mut sum_weight = 0.0f32; + + // Contribution from odometry edges + for edge in &self.odometry_edges { + if edge.to_id == node_id + && let Some(from_node) = self.get_node(edge.from_id) + { + let expected = transform_pose(from_node.pose, edge.measurement); + let (ex, ey, et) = compute_error(expected, node_pose); + + sum_x += edge.weight * ex; + sum_y += edge.weight * ey; + sum_theta += edge.weight * et; + sum_weight += edge.weight; + } + + if edge.from_id == node_id + && let Some(to_node) = self.get_node(edge.to_id) + { + let expected = inverse_transform_pose(to_node.pose, edge.measurement); + let (ex, ey, et) = compute_error(expected, node_pose); + + sum_x += edge.weight * ex; + sum_y += edge.weight * ey; + sum_theta += edge.weight * et; + sum_weight += edge.weight; + } + } + + // Contribution from loop closure edges (with Huber loss) + for edge in &self.loop_edges { + if edge.to_id == node_id + && let Some(from_node) = self.get_node(edge.from_id) + { + let expected = transform_pose(from_node.pose, edge.measurement); + let (ex, ey, et) = compute_error(expected, node_pose); + + let error_mag = (ex * ex + ey * ey).sqrt(); + let huber_w = huber_weight(error_mag, huber_scale); + let effective_weight = edge.weight * huber_w; + + sum_x += effective_weight * ex; + sum_y += effective_weight * ey; + sum_theta += effective_weight * et; + sum_weight += effective_weight; + } + + if edge.from_id == node_id + && let Some(to_node) = self.get_node(edge.to_id) + { + let expected = inverse_transform_pose(to_node.pose, edge.measurement); + let (ex, ey, et) = compute_error(expected, node_pose); + + let error_mag = (ex * ex + ey * ey).sqrt(); + let huber_w = huber_weight(error_mag, huber_scale); + let effective_weight = edge.weight * huber_w; + + sum_x += effective_weight * ex; + sum_y += effective_weight * ey; + sum_theta += effective_weight * et; + sum_weight += effective_weight; + } + } + + if sum_weight > 0.0 { + ( + sum_x / sum_weight, + sum_y / sum_weight, + sum_theta / sum_weight, + ) + } else { + (0.0, 0.0, 0.0) + } + } + + /// Get total error (sum of squared residuals). + pub fn total_error(&self) -> f32 { + let mut error = 0.0f32; + + for edge in &self.odometry_edges { + if let (Some(from), Some(to)) = (self.get_node(edge.from_id), self.get_node(edge.to_id)) + { + let expected = transform_pose(from.pose, edge.measurement); + let (dx, dy, dt) = compute_error(expected, to.pose); + error += edge.weight * (dx * dx + dy * dy + dt * dt); + } + } + + for edge in &self.loop_edges { + if let (Some(from), Some(to)) = (self.get_node(edge.from_id), self.get_node(edge.to_id)) + { + let expected = transform_pose(from.pose, edge.measurement); + let (dx, dy, dt) = compute_error(expected, to.pose); + error += edge.weight * (dx * dx + dy * dy + dt * dt); + } + } + + error + } + + /// Clear the graph. + pub fn clear(&mut self) { + self.nodes.clear(); + self.odometry_edges.clear(); + self.loop_edges.clear(); + } +} + +/// Transform pose by relative pose. +fn transform_pose(base: Pose2D, delta: Pose2D) -> Pose2D { + let cos_t = base.theta.cos(); + let sin_t = base.theta.sin(); + + Pose2D::new( + base.x + delta.x * cos_t - delta.y * sin_t, + base.y + delta.x * sin_t + delta.y * cos_t, + normalize_angle(base.theta + delta.theta), + ) +} + +/// Inverse transform: given result and delta, find base. +fn inverse_transform_pose(result: Pose2D, delta: Pose2D) -> Pose2D { + let base_theta = normalize_angle(result.theta - delta.theta); + let cos_t = base_theta.cos(); + let sin_t = base_theta.sin(); + + Pose2D::new( + result.x - (delta.x * cos_t - delta.y * sin_t), + result.y - (delta.x * sin_t + delta.y * cos_t), + base_theta, + ) +} + +/// Compute error between expected and actual pose. +fn compute_error(expected: Pose2D, actual: Pose2D) -> (f32, f32, f32) { + ( + expected.x - actual.x, + expected.y - actual.y, + normalize_angle(expected.theta - actual.theta), + ) +} + +/// Huber weight for robust optimization. +#[inline] +fn huber_weight(residual: f32, delta: f32) -> f32 { + let abs_r = residual.abs(); + if abs_r <= delta { 1.0 } else { delta / abs_r } +} + +/// Normalize angle to [-π, π). +fn normalize_angle(angle: f32) -> f32 { + let mut a = angle % std::f32::consts::TAU; + if a > std::f32::consts::PI { + a -= std::f32::consts::TAU; + } else if a < -std::f32::consts::PI { + a += std::f32::consts::TAU; + } + a +} + +#[cfg(test)] +mod tests { + use super::*; + + #[test] + fn test_graph_creation() { + let graph = SubmapPoseGraph::with_defaults(); + assert!(graph.is_empty()); + } + + #[test] + fn test_add_submaps() { + let mut graph = SubmapPoseGraph::with_defaults(); + + // Add first submap (anchor) + graph.add_submap(SubmapId::new(0), Pose2D::new(0.0, 0.0, 0.0), None); + assert_eq!(graph.node_count(), 1); + assert!(graph.nodes[0].fixed); + + // Add second submap with odometry + let delta = Pose2D::new(1.0, 0.0, 0.1); + graph.add_submap(SubmapId::new(1), Pose2D::new(1.0, 0.0, 0.1), Some(delta)); + assert_eq!(graph.node_count(), 2); + assert!(!graph.nodes[1].fixed); + assert_eq!(graph.odometry_edges().len(), 1); + } + + #[test] + fn test_loop_closure() { + let mut graph = SubmapPoseGraph::with_defaults(); + + // Create a simple chain with drift that loop closure should correct + // Submap 0 at origin (anchor) + graph.add_submap(SubmapId::new(0), Pose2D::new(0.0, 0.0, 0.0), None); + + // Submap 1: move 1m forward + graph.add_submap( + SubmapId::new(1), + Pose2D::new(1.0, 0.0, 0.0), + Some(Pose2D::new(1.0, 0.0, 0.0)), + ); + + // Submap 2: move another 1m forward, but with 0.1m drift + // Actual pose is (2.1, 0, 0) but should be (2.0, 0, 0) + graph.add_submap( + SubmapId::new(2), + Pose2D::new(2.1, 0.0, 0.0), + Some(Pose2D::new(1.0, 0.0, 0.0)), // Odometry says 1m + ); + + // No loops yet + assert!(!graph.has_loops()); + assert_eq!(graph.loop_edges().len(), 0); + + // Add loop closure that says submap 2 should be at (2.0, 0, 0) relative to submap 0 + graph.add_loop_closure(SubmapLoopClosure { + from_submap: SubmapId::new(2), + to_submap: SubmapId::new(0), + relative_pose: Pose2D::new(-2.0, 0.0, 0.0), // From 2 back to 0 + confidence: 1.0, + }); + + assert!(graph.has_loops()); + assert_eq!(graph.loop_edges().len(), 1); + + let (iterations, corrections) = graph.optimize(); + + // Optimization should run + assert!(iterations > 0); + + // Should have corrections for non-fixed nodes (1 and 2) + assert_eq!(corrections.len(), 2); + + // Node 2's x should move closer to 2.0 (from 2.1) + let node2 = graph.get_node(SubmapId::new(2)).unwrap(); + // After optimization, should be closer to 2.0 than the original 2.1 + assert!( + (node2.pose.x - 2.0).abs() < 0.15, + "Expected node2.x closer to 2.0, got {}", + node2.pose.x + ); + } + + #[test] + fn test_corrections_output() { + let mut graph = SubmapPoseGraph::with_defaults(); + + graph.add_submap(SubmapId::new(0), Pose2D::new(0.0, 0.0, 0.0), None); + graph.add_submap( + SubmapId::new(1), + Pose2D::new(1.0, 0.0, 0.0), + Some(Pose2D::new(1.0, 0.0, 0.0)), + ); + + let (_, corrections) = graph.optimize(); + + // Should have correction for non-fixed node (submap 1) + assert_eq!(corrections.len(), 1); + assert_eq!(corrections[0].submap_id, SubmapId::new(1)); + } +} diff --git a/vastu-map/src/submap/manager.rs b/vastu-map/src/submap/manager.rs new file mode 100644 index 0000000..4aea3fe --- /dev/null +++ b/vastu-map/src/submap/manager.rs @@ -0,0 +1,624 @@ +//! Submap manager - orchestrates submap lifecycle and global grid composition. + +use super::config::SubmapConfig; +use super::types::{Submap, SubmapId}; +use crate::core::{LidarScan, Pose2D, WorldPoint}; +use crate::grid::{GridConfig, GridStorage, MapConfig, SensorConfig}; + +/// Correction to apply to a submap's origin after pose graph optimization. +#[derive(Clone, Debug)] +pub struct SubmapCorrection { + /// ID of the submap to correct. + pub submap_id: SubmapId, + /// New origin pose in world frame. + pub new_origin: Pose2D, +} + +/// Cached global grid with version tracking. +pub struct CachedGlobalGrid { + /// The composed global grid. + pub grid: GridStorage, + /// Submap versions used to create this grid (id -> origin). + /// Used to detect if regeneration is needed. + _submap_versions: Vec<(SubmapId, Pose2D)>, +} + +/// Result of inserting a scan into the submap manager. +#[derive(Clone, Debug)] +pub struct InsertResult { + /// The submap(s) the scan was inserted into. + pub submaps: Vec, + /// The local pose in the active submap. + pub local_pose: Pose2D, + /// Whether a new submap was created. + pub new_submap_created: bool, + /// Whether any submap was finalized. + pub submap_finalized: bool, +} + +/// Manages submaps and global grid composition. +/// +/// The SubmapManager is the central orchestrator for the submap architecture. +/// It handles: +/// - Submap lifecycle (Active → Filling → Finalized) +/// - Scan insertion with automatic transitions +/// - Global grid composition with lazy regeneration +/// - Applying pose corrections from optimizer +pub struct SubmapManager { + /// All submaps (completed + active). + submaps: Vec, + + /// Next submap ID to assign. + next_id: u32, + + /// Index of the currently active submap. + active_idx: Option, + + /// Index of the filling submap (if any). + filling_idx: Option, + + /// Configuration. + config: SubmapConfig, + + /// Sensor configuration for grid updates. + sensor_config: SensorConfig, + + /// Grid configuration. + grid_config: GridConfig, + + /// Cached global grid (regenerated on demand). + global_grid: Option, + + /// Whether the global grid needs regeneration. + global_dirty: bool, +} + +impl SubmapManager { + /// Create a new submap manager. + pub fn new(config: SubmapConfig, map_config: &MapConfig) -> Self { + Self { + submaps: Vec::new(), + next_id: 0, + active_idx: None, + filling_idx: None, + config, + sensor_config: map_config.sensor.clone(), + grid_config: map_config.grid.clone(), + global_grid: None, + global_dirty: true, + } + } + + /// Get the configuration. + pub fn config(&self) -> &SubmapConfig { + &self.config + } + + /// Get all submaps. + pub fn submaps(&self) -> &[Submap] { + &self.submaps + } + + /// Get the active submap (if any). + pub fn active_submap(&self) -> Option<&Submap> { + self.active_idx.map(|i| &self.submaps[i]) + } + + /// Get the active submap mutably. + pub fn active_submap_mut(&mut self) -> Option<&mut Submap> { + self.active_idx.map(|i| &mut self.submaps[i]) + } + + /// Get a submap by ID. + pub fn get_submap(&self, id: SubmapId) -> Option<&Submap> { + self.submaps.iter().find(|s| s.id == id) + } + + /// Get a submap by ID mutably. + pub fn get_submap_mut(&mut self, id: SubmapId) -> Option<&mut Submap> { + self.submaps.iter_mut().find(|s| s.id == id) + } + + /// Get all finalized submaps (eligible for loop closure). + pub fn finalized_submaps(&self) -> impl Iterator { + self.submaps.iter().filter(|s| s.is_finalized()) + } + + /// Number of submaps. + pub fn submap_count(&self) -> usize { + self.submaps.len() + } + + /// Total memory usage in bytes. + pub fn memory_bytes(&self) -> usize { + let submaps_bytes: usize = self.submaps.iter().map(|s| s.memory_bytes()).sum(); + let global_bytes = self + .global_grid + .as_ref() + .map(|g| g.grid.width() * g.grid.height() * 6) + .unwrap_or(0); + submaps_bytes + global_bytes + } + + /// Insert a scan at the given world pose. + /// + /// This method handles: + /// - Creating the first submap if none exists + /// - Inserting into active (and filling) submaps + /// - Triggering state transitions based on scan count and distance + /// - Creating new submaps when thresholds are exceeded + pub fn insert_scan( + &mut self, + scan: &LidarScan, + world_pose: Pose2D, + timestamp_us: u64, + ) -> InsertResult { + let mut result = InsertResult { + submaps: Vec::new(), + local_pose: Pose2D::default(), + new_submap_created: false, + submap_finalized: false, + }; + + // Create first submap if needed + if self.active_idx.is_none() { + self.create_new_submap(world_pose); + result.new_submap_created = true; + } + + // Check if we need to create a new submap before inserting + let should_transition = self.should_create_new_submap(world_pose); + + if should_transition && self.filling_idx.is_none() { + // Start transition: active -> filling, create new active + self.start_submap_transition(world_pose); + result.new_submap_created = true; + } + + // Insert into active submap + let active_submap = &mut self.submaps[self.active_idx.unwrap()]; + active_submap.insert_scan( + scan, + world_pose, + timestamp_us, + &self.sensor_config, + &self.grid_config, + ); + result.submaps.push(active_submap.id); + result.local_pose = active_submap.origin.inverse().compose(&world_pose); + + // Insert into filling submap if exists (overlap period) + if let Some(filling_idx) = self.filling_idx { + let filling_submap = &mut self.submaps[filling_idx]; + filling_submap.insert_scan( + scan, + world_pose, + timestamp_us, + &self.sensor_config, + &self.grid_config, + ); + result.submaps.push(filling_submap.id); + + // Check if filling submap should be finalized + if filling_submap.overlap_count >= self.config.overlap_scans { + filling_submap.finalize(); + self.filling_idx = None; + result.submap_finalized = true; + } + } + + // Mark global grid as dirty + self.global_dirty = true; + + result + } + + /// Check if we should create a new submap. + fn should_create_new_submap(&self, world_pose: Pose2D) -> bool { + let Some(active_idx) = self.active_idx else { + return true; + }; + + let active = &self.submaps[active_idx]; + + // Check scan count threshold + if active.scan_count >= self.config.scans_per_submap { + return true; + } + + // Check distance threshold + if active.distance_traveled >= self.config.max_distance_per_submap { + return true; + } + + // Check if robot is near edge of local grid + if !active.contains_world_point(world_pose.position()) { + return true; + } + + false + } + + /// Start the transition from active to filling, create new active. + fn start_submap_transition(&mut self, new_origin: Pose2D) { + if let Some(active_idx) = self.active_idx { + // Current active becomes filling + self.submaps[active_idx].start_filling(); + self.filling_idx = Some(active_idx); + } + + // Create new active submap + self.create_new_submap(new_origin); + } + + /// Create a new submap at the given origin. + fn create_new_submap(&mut self, origin: Pose2D) { + let id = SubmapId::new(self.next_id); + self.next_id += 1; + + let submap = Submap::new( + id, + origin, + self.config.local_grid_size, + self.config.resolution, + ); + + self.submaps.push(submap); + self.active_idx = Some(self.submaps.len() - 1); + } + + /// Get or regenerate the global grid. + /// + /// Uses lazy regeneration - only regenerates when dirty. + pub fn global_grid(&mut self) -> &GridStorage { + if self.global_dirty || self.global_grid.is_none() { + self.regenerate_global_grid(); + } + &self.global_grid.as_ref().unwrap().grid + } + + /// Force regeneration of the global grid. + pub fn regenerate_global_grid(&mut self) { + let bounds = self.compute_global_bounds(); + + // Create new grid covering all submaps + let width = ((bounds.1.x - bounds.0.x) / self.config.resolution).ceil() as usize + 1; + let height = ((bounds.1.y - bounds.0.y) / self.config.resolution).ceil() as usize + 1; + + // Clamp to reasonable size + let width = width.min(self.grid_config.max_width).max(100); + let height = height.min(self.grid_config.max_height).max(100); + + let mut grid = GridStorage::new(width, height, self.config.resolution, bounds.0); + + // Insert all scans from all submaps + for submap in &self.submaps { + for stored in &submap.scans { + let world_pose = submap.origin.compose(&stored.local_pose); + crate::grid::lidar_update::update_from_lidar( + &mut grid, + &stored.scan, + world_pose, + &self.sensor_config, + &self.grid_config, + ); + } + } + + // Cache version info + let versions: Vec<_> = self.submaps.iter().map(|s| (s.id, s.origin)).collect(); + + self.global_grid = Some(CachedGlobalGrid { + grid, + _submap_versions: versions, + }); + self.global_dirty = false; + } + + /// Compute the bounding box of all submaps in world coordinates. + fn compute_global_bounds(&self) -> (WorldPoint, WorldPoint) { + if self.submaps.is_empty() { + // Default bounds centered at origin + let half = (self.config.local_grid_size as f32 * self.config.resolution) / 2.0; + return (WorldPoint::new(-half, -half), WorldPoint::new(half, half)); + } + + let mut min_x = f32::INFINITY; + let mut min_y = f32::INFINITY; + let mut max_x = f32::NEG_INFINITY; + let mut max_y = f32::NEG_INFINITY; + + for submap in &self.submaps { + let (sub_min, sub_max) = submap.world_bounds(); + min_x = min_x.min(sub_min.x); + min_y = min_y.min(sub_min.y); + max_x = max_x.max(sub_max.x); + max_y = max_y.max(sub_max.y); + } + + // Add small margin + let margin = self.config.resolution * 10.0; + ( + WorldPoint::new(min_x - margin, min_y - margin), + WorldPoint::new(max_x + margin, max_y + margin), + ) + } + + /// Apply pose corrections from optimizer. + /// + /// After pose graph optimization, call this method to update submap origins. + /// The global grid will be regenerated on next access. + pub fn apply_corrections(&mut self, corrections: &[SubmapCorrection]) { + for correction in corrections { + if let Some(submap) = self.get_submap_mut(correction.submap_id) { + submap.origin = correction.new_origin; + } + } + + // Mark global grid as needing regeneration + self.global_dirty = true; + } + + /// Apply corrections and immediately regenerate affected submap grids. + /// + /// Use this when you need immediate consistency (e.g., for scan matching). + pub fn apply_corrections_and_regenerate(&mut self, corrections: &[SubmapCorrection]) { + // First, apply all corrections + for correction in corrections { + if let Some(submap) = self + .submaps + .iter_mut() + .find(|s| s.id == correction.submap_id) + { + submap.origin = correction.new_origin; + } + } + + // Then regenerate all affected submaps + // Clone configs to avoid borrow conflict + let sensor_config = self.sensor_config.clone(); + let grid_config = self.grid_config.clone(); + + for correction in corrections { + if let Some(submap) = self + .submaps + .iter_mut() + .find(|s| s.id == correction.submap_id) + { + submap.regenerate_grid(&sensor_config, &grid_config); + } + } + + self.global_dirty = true; + } + + /// Get submaps that overlap with a world point. + /// + /// Useful for scan matching against multiple submaps. + pub fn submaps_containing(&self, point: WorldPoint) -> Vec<&Submap> { + self.submaps + .iter() + .filter(|s| s.contains_world_point(point)) + .collect() + } + + /// Get submaps within a distance of a world point. + pub fn submaps_near(&self, point: WorldPoint, max_distance: f32) -> Vec<&Submap> { + self.submaps + .iter() + .filter(|s| s.origin.position().distance(&point) <= max_distance) + .collect() + } + + /// Get finalized submaps suitable for loop closure with the active submap. + /// + /// Returns submaps that are: + /// - Finalized + /// - Have sufficient ID gap (not recent) + /// - Within maximum distance + pub fn loop_closure_candidates(&self, current_pose: Pose2D) -> Vec<&Submap> { + let Some(active) = self.active_submap() else { + return Vec::new(); + }; + + let min_id = active + .id + .value() + .saturating_sub(self.config.min_loop_closure_gap as u32); + + self.submaps + .iter() + .filter(|s| s.is_finalized()) + .filter(|s| s.id.value() < min_id) + .filter(|s| { + s.origin.position().distance(¤t_pose.position()) + <= self.config.max_loop_closure_distance + }) + .collect() + } + + /// Check if global grid is dirty (needs regeneration). + pub fn is_global_dirty(&self) -> bool { + self.global_dirty + } + + /// Clear all submaps and reset state. + pub fn clear(&mut self) { + self.submaps.clear(); + self.next_id = 0; + self.active_idx = None; + self.filling_idx = None; + self.global_grid = None; + self.global_dirty = true; + } +} + +#[cfg(test)] +mod tests { + use super::super::types::SubmapState; + use super::*; + + fn make_test_scan() -> LidarScan { + // Simple scan with a few points + let angles: Vec = (0..36).map(|i| (i as f32) * 0.1745).collect(); // 0 to ~360° + let ranges: Vec = angles.iter().map(|_| 2.0).collect(); // 2m range + LidarScan::new(ranges, angles, 0.15, 8.0) + } + + #[test] + fn test_manager_creation() { + let config = SubmapConfig::default(); + let map_config = MapConfig::default(); + let manager = SubmapManager::new(config, &map_config); + + assert_eq!(manager.submap_count(), 0); + assert!(manager.active_submap().is_none()); + } + + #[test] + fn test_first_scan_creates_submap() { + let config = SubmapConfig::default(); + let map_config = MapConfig::default(); + let mut manager = SubmapManager::new(config, &map_config); + + let scan = make_test_scan(); + let pose = Pose2D::new(0.0, 0.0, 0.0); + let result = manager.insert_scan(&scan, pose, 0); + + assert!(result.new_submap_created); + assert_eq!(result.submaps.len(), 1); + assert_eq!(manager.submap_count(), 1); + assert!(manager.active_submap().is_some()); + } + + #[test] + fn test_submap_transition() { + let mut config = SubmapConfig::default(); + config.scans_per_submap = 3; // Small for testing + config.overlap_scans = 2; + + let map_config = MapConfig::default(); + let mut manager = SubmapManager::new(config, &map_config); + + let scan = make_test_scan(); + + // Insert scans until transition + for i in 0..5 { + let pose = Pose2D::new(i as f32 * 0.1, 0.0, 0.0); + let result = manager.insert_scan(&scan, pose, i as u64); + + if i == 3 { + // After threshold, new submap should be created + assert!(result.new_submap_created); + } + } + + // Should have 2 submaps now + assert_eq!(manager.submap_count(), 2); + + // First submap should be filling or finalized + let first = manager.get_submap(SubmapId::new(0)).unwrap(); + assert!(first.state != SubmapState::Active); + } + + #[test] + fn test_submap_finalization() { + let mut config = SubmapConfig::default(); + config.scans_per_submap = 2; + config.overlap_scans = 2; + + let map_config = MapConfig::default(); + let mut manager = SubmapManager::new(config, &map_config); + + let scan = make_test_scan(); + + // Insert enough scans to finalize first submap + for i in 0..6 { + let pose = Pose2D::new(i as f32 * 0.1, 0.0, 0.0); + manager.insert_scan(&scan, pose, i as u64); + } + + // First submap should be finalized + let first = manager.get_submap(SubmapId::new(0)).unwrap(); + assert!(first.is_finalized()); + } + + #[test] + fn test_global_grid_regeneration() { + let config = SubmapConfig::default(); + let map_config = MapConfig::default(); + let mut manager = SubmapManager::new(config, &map_config); + + let scan = make_test_scan(); + let pose = Pose2D::new(0.0, 0.0, 0.0); + manager.insert_scan(&scan, pose, 0); + + // Should be dirty + assert!(manager.is_global_dirty()); + + // Access global grid (triggers regeneration) + let _grid = manager.global_grid(); + + // Should no longer be dirty + assert!(!manager.is_global_dirty()); + } + + #[test] + fn test_apply_corrections() { + let config = SubmapConfig::default(); + let map_config = MapConfig::default(); + let mut manager = SubmapManager::new(config, &map_config); + + let scan = make_test_scan(); + manager.insert_scan(&scan, Pose2D::new(0.0, 0.0, 0.0), 0); + + // Get grid to clear dirty flag + let _ = manager.global_grid(); + assert!(!manager.is_global_dirty()); + + // Apply correction + let correction = SubmapCorrection { + submap_id: SubmapId::new(0), + new_origin: Pose2D::new(1.0, 1.0, 0.5), + }; + manager.apply_corrections(&[correction]); + + // Should be dirty again + assert!(manager.is_global_dirty()); + + // Origin should be updated + let submap = manager.get_submap(SubmapId::new(0)).unwrap(); + assert!((submap.origin.x - 1.0).abs() < 1e-6); + assert!((submap.origin.y - 1.0).abs() < 1e-6); + } + + #[test] + fn test_loop_closure_candidates() { + let mut config = SubmapConfig::default(); + config.scans_per_submap = 2; + config.overlap_scans = 1; + config.min_loop_closure_gap = 2; + config.max_loop_closure_distance = 10.0; + + let map_config = MapConfig::default(); + let mut manager = SubmapManager::new(config, &map_config); + + let scan = make_test_scan(); + + // Create several submaps + for i in 0..10 { + let pose = Pose2D::new(i as f32 * 0.2, 0.0, 0.0); + manager.insert_scan(&scan, pose, i as u64); + } + + // Get candidates from current position + let current = Pose2D::new(0.0, 0.0, 0.0); // Back at origin + let candidates = manager.loop_closure_candidates(current); + + // Should find finalized submaps with sufficient gap + assert!(!candidates.is_empty()); + for c in &candidates { + assert!(c.is_finalized()); + } + } +} diff --git a/vastu-map/src/submap/matching.rs b/vastu-map/src/submap/matching.rs new file mode 100644 index 0000000..a7d64a8 --- /dev/null +++ b/vastu-map/src/submap/matching.rs @@ -0,0 +1,352 @@ +//! Multi-submap scan matching. +//! +//! Provides scan matching against the active submap and overlapping finalized submaps. +//! This enables robust matching when the robot returns to previously mapped areas. + +use crate::core::{LidarScan, Pose2D}; +use crate::slam::{CorrelativeMatcher, CorrelativeMatcherConfig, ScanMatchResult}; + +use super::manager::SubmapManager; +use super::types::{Submap, SubmapId}; + +/// Result of matching against multiple submaps. +#[derive(Clone, Debug)] +pub struct MultiSubmapMatchResult { + /// The best match result across all submaps. + pub best_match: ScanMatchResult, + + /// The submap that produced the best match. + pub matched_submap_id: SubmapId, + + /// The local pose within the matched submap. + pub local_pose: Pose2D, + + /// Number of submaps that were searched. + pub submaps_searched: usize, + + /// All match results (for debugging/visualization). + pub all_results: Vec<(SubmapId, ScanMatchResult)>, +} + +impl MultiSubmapMatchResult { + /// Create a failed result. + pub fn failed(prior_pose: Pose2D) -> Self { + Self { + best_match: ScanMatchResult::failed(prior_pose), + matched_submap_id: SubmapId::new(0), + local_pose: Pose2D::default(), + submaps_searched: 0, + all_results: Vec::new(), + } + } + + /// Did matching converge? + pub fn converged(&self) -> bool { + self.best_match.converged + } + + /// Get the matched world pose. + pub fn world_pose(&self) -> Pose2D { + self.best_match.pose + } + + /// Get the match score. + pub fn score(&self) -> f32 { + self.best_match.score + } +} + +/// Configuration for multi-submap matching. +#[derive(Clone, Debug)] +pub struct MultiSubmapMatchConfig { + /// Maximum distance to consider overlapping submaps. + pub max_overlap_distance: f32, + + /// Include active submap in search. + pub include_active: bool, + + /// Include finalized submaps in search. + pub include_finalized: bool, + + /// Maximum number of submaps to search. + pub max_submaps: usize, + + /// Minimum score improvement to prefer a different submap. + /// Prevents jumping between submaps with similar scores. + pub score_hysteresis: f32, +} + +impl Default for MultiSubmapMatchConfig { + fn default() -> Self { + Self { + max_overlap_distance: 3.0, // 3m + include_active: true, + include_finalized: true, + max_submaps: 5, + score_hysteresis: 0.05, + } + } +} + +/// Matcher that can search across multiple submaps. +pub struct MultiSubmapMatcher { + /// Underlying correlative matcher. + matcher: CorrelativeMatcher, + + /// Configuration. + config: MultiSubmapMatchConfig, +} + +impl MultiSubmapMatcher { + /// Create a new multi-submap matcher. + pub fn new( + matcher_config: CorrelativeMatcherConfig, + multi_config: MultiSubmapMatchConfig, + ) -> Self { + Self { + matcher: CorrelativeMatcher::new(matcher_config), + config: multi_config, + } + } + + /// Create with default configurations. + pub fn with_defaults() -> Self { + Self::new( + CorrelativeMatcherConfig::default(), + MultiSubmapMatchConfig::default(), + ) + } + + /// Match a scan against the active submap only. + /// + /// This is the fast path for normal operation. + pub fn match_active_only( + &self, + scan: &LidarScan, + prior_world_pose: Pose2D, + manager: &SubmapManager, + ) -> MultiSubmapMatchResult { + let Some(active) = manager.active_submap() else { + return MultiSubmapMatchResult::failed(prior_world_pose); + }; + + let result = self.match_against_submap(scan, prior_world_pose, active); + + MultiSubmapMatchResult { + best_match: result.clone(), + matched_submap_id: active.id, + local_pose: active.origin.inverse().compose(&result.pose), + submaps_searched: 1, + all_results: vec![(active.id, result)], + } + } + + /// Match a scan against multiple submaps. + /// + /// Searches the active submap and overlapping finalized submaps, + /// returning the best match. + pub fn match_multi( + &self, + scan: &LidarScan, + prior_world_pose: Pose2D, + manager: &SubmapManager, + ) -> MultiSubmapMatchResult { + let mut candidates: Vec<&Submap> = Vec::new(); + + // Add active submap + if self.config.include_active + && let Some(active) = manager.active_submap() + { + candidates.push(active); + } + + // Add overlapping finalized submaps + if self.config.include_finalized { + let finalized: Vec<_> = manager + .submaps() + .iter() + .filter(|s| s.is_finalized()) + .filter(|s| { + s.origin.position().distance(&prior_world_pose.position()) + <= self.config.max_overlap_distance + }) + .collect(); + + candidates.extend(finalized); + } + + // Limit candidates + if candidates.len() > self.config.max_submaps { + // Sort by distance to prior pose, take closest + candidates.sort_by(|a, b| { + let dist_a = a.origin.position().distance(&prior_world_pose.position()); + let dist_b = b.origin.position().distance(&prior_world_pose.position()); + dist_a.partial_cmp(&dist_b).unwrap() + }); + candidates.truncate(self.config.max_submaps); + } + + if candidates.is_empty() { + return MultiSubmapMatchResult::failed(prior_world_pose); + } + + // Match against each candidate + let mut all_results: Vec<(SubmapId, ScanMatchResult)> = Vec::new(); + let mut best_score = f32::NEG_INFINITY; + let mut best_idx = 0; + + for (i, submap) in candidates.iter().enumerate() { + let result = self.match_against_submap(scan, prior_world_pose, submap); + all_results.push((submap.id, result.clone())); + + // Use hysteresis: prefer current best unless new is significantly better + let adjusted_score = if i == 0 { + result.score + } else { + result.score - self.config.score_hysteresis + }; + + if adjusted_score > best_score && result.converged { + best_score = adjusted_score; + best_idx = i; + } + } + + let (best_submap_id, best_match) = all_results[best_idx].clone(); + let best_submap = candidates[best_idx]; + + MultiSubmapMatchResult { + local_pose: best_submap.origin.inverse().compose(&best_match.pose), + best_match, + matched_submap_id: best_submap_id, + submaps_searched: all_results.len(), + all_results, + } + } + + /// Match a scan against a single submap. + fn match_against_submap( + &self, + scan: &LidarScan, + prior_world_pose: Pose2D, + submap: &Submap, + ) -> ScanMatchResult { + // Convert world pose to local pose for matching + let prior_local_pose = submap.origin.inverse().compose(&prior_world_pose); + + // Match against the submap's local grid + let local_result = self + .matcher + .match_scan(scan, prior_local_pose, &submap.grid); + + // Convert result back to world frame + let world_pose = submap.origin.compose(&local_result.pose); + + ScanMatchResult::new( + world_pose, + local_result.score, + local_result.hits, + local_result.total_points, + ) + } +} + +impl SubmapManager { + /// Match a scan against the active submap. + /// + /// This is the fast path for normal SLAM operation. + pub fn match_scan( + &self, + scan: &LidarScan, + prior_world_pose: Pose2D, + matcher_config: &CorrelativeMatcherConfig, + ) -> MultiSubmapMatchResult { + let matcher = MultiSubmapMatcher::new( + matcher_config.clone(), + MultiSubmapMatchConfig { + include_finalized: false, + ..Default::default() + }, + ); + matcher.match_active_only(scan, prior_world_pose, self) + } + + /// Match a scan against multiple submaps (active + overlapping). + /// + /// Use this when the robot may have returned to a previously mapped area. + pub fn match_scan_multi( + &self, + scan: &LidarScan, + prior_world_pose: Pose2D, + matcher_config: &CorrelativeMatcherConfig, + multi_config: &MultiSubmapMatchConfig, + ) -> MultiSubmapMatchResult { + let matcher = MultiSubmapMatcher::new(matcher_config.clone(), multi_config.clone()); + matcher.match_multi(scan, prior_world_pose, self) + } +} + +#[cfg(test)] +mod tests { + use super::*; + use crate::grid::MapConfig; + use crate::submap::SubmapConfig; + + fn make_test_scan() -> LidarScan { + let angles: Vec = (0..36).map(|i| (i as f32) * 0.1745).collect(); + let ranges: Vec = angles.iter().map(|_| 2.0).collect(); + LidarScan::new(ranges, angles, 0.15, 8.0) + } + + #[test] + fn test_match_active_only() { + let config = SubmapConfig::default(); + let map_config = MapConfig::default(); + let mut manager = SubmapManager::new(config, &map_config); + + let scan = make_test_scan(); + let pose = Pose2D::new(0.0, 0.0, 0.0); + + // Insert a scan to create a submap + manager.insert_scan(&scan, pose, 0); + + // Match against active + let matcher_config = CorrelativeMatcherConfig::fast(); + let result = manager.match_scan(&scan, pose, &matcher_config); + + assert_eq!(result.submaps_searched, 1); + assert!(result.world_pose().x.is_finite()); + } + + #[test] + fn test_match_multi() { + let mut submap_config = SubmapConfig::default(); + submap_config.scans_per_submap = 2; + submap_config.overlap_scans = 1; + + let map_config = MapConfig::default(); + let mut manager = SubmapManager::new(submap_config, &map_config); + + let scan = make_test_scan(); + + // Insert several scans to create multiple submaps + for i in 0..6 { + let pose = Pose2D::new(i as f32 * 0.2, 0.0, 0.0); + manager.insert_scan(&scan, pose, i as u64); + } + + // Match with multi-submap search + let matcher_config = CorrelativeMatcherConfig::fast(); + let multi_config = MultiSubmapMatchConfig { + max_overlap_distance: 5.0, + ..Default::default() + }; + + let pose = Pose2D::new(0.0, 0.0, 0.0); + let result = manager.match_scan_multi(&scan, pose, &matcher_config, &multi_config); + + // Should have searched multiple submaps + assert!(result.submaps_searched >= 1); + assert!(result.world_pose().x.is_finite()); + } +} diff --git a/vastu-map/src/submap/mod.rs b/vastu-map/src/submap/mod.rs new file mode 100644 index 0000000..6e000cd --- /dev/null +++ b/vastu-map/src/submap/mod.rs @@ -0,0 +1,47 @@ +//! Submap-based SLAM architecture for reversible map updates. +//! +//! This module implements a submap architecture inspired by Google Cartographer, +//! enabling proper loop closure correction by storing raw scans and regenerating +//! the global map when submap origins are adjusted. +//! +//! ## Key Concepts +//! +//! - **Submap**: A locally-consistent map fragment with an adjustable origin +//! - **StoredScan**: Raw lidar scan stored for later regeneration +//! - **SubmapManager**: Orchestrates submap lifecycle and global grid composition +//! +//! ## Architecture +//! +//! ```text +//! ┌─────────────────────────────────────────────────────────────────┐ +//! │ World Frame │ +//! │ │ +//! │ Submap 0 Submap 1 Submap 2 │ +//! │ ┌─────────┐ ┌─────────┐ ┌─────────┐ │ +//! │ │ Local │ ──T01──▶ │ Local │ ──T12──▶ │ Local │ │ +//! │ │ Grid │ │ Grid │ │ Grid │ │ +//! │ └────┬────┘ └────┬────┘ └────┬────┘ │ +//! │ │ │ │ │ +//! │ ▼ ▼ ▼ │ +//! │ origin_0 (fixed) origin_1 (opt) origin_2 (opt) │ +//! │ │ +//! └─────────────────────────────────────────────────────────────────┘ +//! ``` +//! +//! After pose graph optimization, only submap origins change - local grids +//! are regenerated from stored scans at new positions. + +mod config; +mod graph; +mod manager; +mod matching; +mod types; + +pub use config::SubmapConfig; +pub use graph::{ + SubmapGraphConfig, SubmapLoopClosure, SubmapLoopEdge, SubmapNode, SubmapOdometryEdge, + SubmapPoseGraph, +}; +pub use manager::{CachedGlobalGrid, InsertResult, SubmapCorrection, SubmapManager}; +pub use matching::{MultiSubmapMatchConfig, MultiSubmapMatchResult, MultiSubmapMatcher}; +pub use types::{StoredScan, Submap, SubmapId, SubmapState}; diff --git a/vastu-map/src/submap/types.rs b/vastu-map/src/submap/types.rs new file mode 100644 index 0000000..a2c8153 --- /dev/null +++ b/vastu-map/src/submap/types.rs @@ -0,0 +1,375 @@ +//! Core submap data types. + +use crate::core::{LidarScan, Pose2D, WorldPoint}; +use crate::grid::GridStorage; + +/// Unique identifier for a submap. +#[derive(Clone, Copy, Debug, PartialEq, Eq, Hash)] +pub struct SubmapId(pub u32); + +impl SubmapId { + /// Create a new submap ID. + #[inline] + pub fn new(id: u32) -> Self { + Self(id) + } + + /// Get the numeric value. + #[inline] + pub fn value(&self) -> u32 { + self.0 + } +} + +impl std::fmt::Display for SubmapId { + fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result { + write!(f, "Submap({})", self.0) + } +} + +/// State of a submap in its lifecycle. +#[derive(Clone, Copy, Debug, PartialEq, Eq)] +pub enum SubmapState { + /// Actively receiving scans. + /// Only one submap can be Active at a time. + Active, + + /// Full but still receiving overlap scans. + /// A new Active submap has been created in parallel. + Filling, + + /// Finalized - no more scan updates allowed. + /// Eligible for loop closure matching. + /// Origin can be adjusted by pose graph optimizer. + Finalized, +} + +impl SubmapState { + /// Can this submap receive new scans? + #[inline] + pub fn accepts_scans(&self) -> bool { + matches!(self, SubmapState::Active | SubmapState::Filling) + } + + /// Is this submap eligible for loop closure matching? + #[inline] + pub fn is_matchable(&self) -> bool { + matches!(self, SubmapState::Finalized) + } +} + +/// A stored lidar scan with its local pose. +/// +/// Scans are stored in polar format (original `LidarScan`) for memory efficiency. +/// The pose is relative to the submap origin, not world frame. +#[derive(Clone, Debug)] +pub struct StoredScan { + /// Pose relative to submap origin (in submap frame). + pub local_pose: Pose2D, + + /// Timestamp in microseconds since epoch. + pub timestamp_us: u64, + + /// Original lidar scan data (polar coordinates). + /// Stored as-is for perfect regeneration fidelity. + pub scan: LidarScan, +} + +impl StoredScan { + /// Create a new stored scan. + pub fn new(local_pose: Pose2D, timestamp_us: u64, scan: LidarScan) -> Self { + Self { + local_pose, + timestamp_us, + scan, + } + } + + /// Get the world pose given the submap origin. + #[inline] + pub fn world_pose(&self, submap_origin: Pose2D) -> Pose2D { + submap_origin.compose(&self.local_pose) + } + + /// Approximate memory usage in bytes. + pub fn memory_bytes(&self) -> usize { + // Pose2D (12 bytes) + u64 (8 bytes) + LidarScan overhead + data + 20 + std::mem::size_of::() + + self.scan.ranges.len() * 4 + + self.scan.angles.len() * 4 + } +} + +/// A locally-consistent map fragment with adjustable origin. +/// +/// Submaps store both a local occupancy grid (for scan matching) and +/// raw scans (for regeneration after pose graph optimization). +pub struct Submap { + /// Unique identifier. + pub id: SubmapId, + + /// Origin pose in world frame. + /// This is what gets adjusted during pose graph optimization. + pub origin: Pose2D, + + /// Local occupancy grid (origin at local 0,0). + /// Uses the same resolution as global grid. + pub grid: GridStorage, + + /// Raw scans with poses relative to submap origin. + /// Used for regeneration after optimization. + pub scans: Vec, + + /// Current lifecycle state. + pub state: SubmapState, + + /// Number of scans inserted (including overlap). + pub scan_count: usize, + + /// Number of overlap scans (received during Filling state). + pub overlap_count: usize, + + /// Distance traveled within this submap (for transition trigger). + pub distance_traveled: f32, + + /// Last pose in local frame (for distance tracking). + last_local_pose: Option, +} + +impl Submap { + /// Create a new active submap at the given world origin. + pub fn new(id: SubmapId, origin: Pose2D, grid_size: usize, resolution: f32) -> Self { + // Local grid is centered at (0, 0) in local frame + // Origin is at center of grid + let half_size = (grid_size as f32 * resolution) / 2.0; + let local_origin = WorldPoint::new(-half_size, -half_size); + + let grid = GridStorage::new(grid_size, grid_size, resolution, local_origin); + + Self { + id, + origin, + grid, + scans: Vec::with_capacity(60), // Typical 50 + 10 overlap + state: SubmapState::Active, + scan_count: 0, + overlap_count: 0, + distance_traveled: 0.0, + last_local_pose: None, + } + } + + /// Check if the submap can accept new scans. + #[inline] + pub fn accepts_scans(&self) -> bool { + self.state.accepts_scans() + } + + /// Check if the submap is finalized and matchable. + #[inline] + pub fn is_finalized(&self) -> bool { + self.state == SubmapState::Finalized + } + + /// Insert a scan into this submap. + /// + /// # Arguments + /// * `scan` - The lidar scan to insert + /// * `world_pose` - Robot pose in world frame when scan was taken + /// * `timestamp_us` - Timestamp in microseconds + /// * `sensor_config` - Sensor configuration for grid update + /// * `grid_config` - Grid configuration + /// + /// # Returns + /// `true` if scan was accepted, `false` if submap doesn't accept scans + pub fn insert_scan( + &mut self, + scan: &LidarScan, + world_pose: Pose2D, + timestamp_us: u64, + sensor_config: &crate::grid::SensorConfig, + grid_config: &crate::grid::GridConfig, + ) -> bool { + if !self.accepts_scans() { + return false; + } + + // Convert world pose to local pose + let local_pose = self.origin.inverse().compose(&world_pose); + + // Update distance traveled + if let Some(last) = self.last_local_pose { + self.distance_traveled += local_pose.distance(&last); + } + self.last_local_pose = Some(local_pose); + + // Store raw scan for regeneration + self.scans + .push(StoredScan::new(local_pose, timestamp_us, scan.clone())); + + // Update local grid + crate::grid::lidar_update::update_from_lidar( + &mut self.grid, + scan, + local_pose, + sensor_config, + grid_config, + ); + + self.scan_count += 1; + if self.state == SubmapState::Filling { + self.overlap_count += 1; + } + + true + } + + /// Transition to Filling state. + /// Called when scan count threshold is reached. + pub fn start_filling(&mut self) { + if self.state == SubmapState::Active { + self.state = SubmapState::Filling; + } + } + + /// Finalize the submap - no more updates allowed. + pub fn finalize(&mut self) { + self.state = SubmapState::Finalized; + } + + /// Regenerate the local grid from stored scans. + /// Called after the submap origin has been adjusted. + pub fn regenerate_grid( + &mut self, + sensor_config: &crate::grid::SensorConfig, + grid_config: &crate::grid::GridConfig, + ) { + // Clear the grid + self.grid.clear(); + + // Re-insert all scans at their local poses + for stored in &self.scans { + crate::grid::lidar_update::update_from_lidar( + &mut self.grid, + &stored.scan, + stored.local_pose, + sensor_config, + grid_config, + ); + } + } + + /// Get the bounds of this submap in world coordinates. + pub fn world_bounds(&self) -> (WorldPoint, WorldPoint) { + let (local_min, local_max) = self.grid.bounds(); + + // Transform corners to world frame + let corners = [ + self.origin.transform_point(local_min), + self.origin + .transform_point(WorldPoint::new(local_max.x, local_min.y)), + self.origin.transform_point(local_max), + self.origin + .transform_point(WorldPoint::new(local_min.x, local_max.y)), + ]; + + let min_x = corners.iter().map(|p| p.x).fold(f32::INFINITY, f32::min); + let min_y = corners.iter().map(|p| p.y).fold(f32::INFINITY, f32::min); + let max_x = corners + .iter() + .map(|p| p.x) + .fold(f32::NEG_INFINITY, f32::max); + let max_y = corners + .iter() + .map(|p| p.y) + .fold(f32::NEG_INFINITY, f32::max); + + (WorldPoint::new(min_x, min_y), WorldPoint::new(max_x, max_y)) + } + + /// Check if a world point is within this submap's local grid bounds. + pub fn contains_world_point(&self, world_point: WorldPoint) -> bool { + let local_point = self.origin.inverse_transform_point(world_point); + let coord = self.grid.world_to_grid(local_point); + self.grid.is_valid_coord(coord) + } + + /// Approximate memory usage in bytes. + pub fn memory_bytes(&self) -> usize { + let grid_bytes = self.grid.width() * self.grid.height() * 6; // ~6 bytes per cell + let scans_bytes: usize = self.scans.iter().map(|s| s.memory_bytes()).sum(); + grid_bytes + scans_bytes + std::mem::size_of::() + } +} + +#[cfg(test)] +mod tests { + use super::*; + + #[test] + fn test_submap_id() { + let id = SubmapId::new(42); + assert_eq!(id.value(), 42); + assert_eq!(format!("{}", id), "Submap(42)"); + } + + #[test] + fn test_submap_state_transitions() { + let state = SubmapState::Active; + assert!(state.accepts_scans()); + assert!(!state.is_matchable()); + + let state = SubmapState::Filling; + assert!(state.accepts_scans()); + assert!(!state.is_matchable()); + + let state = SubmapState::Finalized; + assert!(!state.accepts_scans()); + assert!(state.is_matchable()); + } + + #[test] + fn test_submap_creation() { + let origin = Pose2D::new(1.0, 2.0, 0.5); + let submap = Submap::new(SubmapId::new(0), origin, 200, 0.025); + + assert_eq!(submap.id, SubmapId::new(0)); + assert_eq!(submap.origin, origin); + assert_eq!(submap.state, SubmapState::Active); + assert_eq!(submap.scan_count, 0); + assert!(submap.accepts_scans()); + } + + #[test] + fn test_stored_scan_world_pose() { + let submap_origin = Pose2D::new(1.0, 0.0, std::f32::consts::FRAC_PI_2); + let local_pose = Pose2D::new(1.0, 0.0, 0.0); + + let scan = LidarScan::new(vec![1.0], vec![0.0], 0.15, 8.0); + let stored = StoredScan::new(local_pose, 0, scan); + + let world_pose = stored.world_pose(submap_origin); + + // At origin (1, 0) facing +Y, moving 1m forward in local frame + // should put us at (1, 1) in world frame + assert!((world_pose.x - 1.0).abs() < 1e-5); + assert!((world_pose.y - 1.0).abs() < 1e-5); + } + + #[test] + fn test_submap_lifecycle() { + let origin = Pose2D::new(0.0, 0.0, 0.0); + let mut submap = Submap::new(SubmapId::new(0), origin, 200, 0.025); + + assert_eq!(submap.state, SubmapState::Active); + + submap.start_filling(); + assert_eq!(submap.state, SubmapState::Filling); + assert!(submap.accepts_scans()); + + submap.finalize(); + assert_eq!(submap.state, SubmapState::Finalized); + assert!(!submap.accepts_scans()); + assert!(submap.is_finalized()); + } +} diff --git a/vastu-map/src/vector_map/README.md b/vastu-map/src/vector_map/README.md deleted file mode 100644 index 0763d18..0000000 --- a/vastu-map/src/vector_map/README.md +++ /dev/null @@ -1,179 +0,0 @@ -# VectorMap Module - -The main SLAM map implementation using line and corner features. - -## Components - -| File | Description | -|------|-------------| -| `map.rs` | `VectorMap` - Main SLAM map implementing the `Map` trait | -| `config.rs` | `VectorMapConfig` - Configuration for all SLAM components | -| `line_store.rs` | `LineStore` - Line storage with synchronized auxiliary structures | - -## Overview - -VectorMap is the primary SLAM implementation. It: - -1. **Extracts** features from lidar scans (lines and corners) -2. **Matches** scans against the map (localization via ICP) -3. **Integrates** new features (mapping) -4. **Queries** the map (raycast, occupancy, frontiers) - -## Quick Start - -```rust -use vastu_map::{VectorMap, VectorMapConfig, Map}; -use vastu_map::core::{Pose2D, PointCloud2D, Point2D}; - -let config = VectorMapConfig::default(); -let mut map = VectorMap::new(config); - -// Process scans -let scan = PointCloud2D::from_points(&points); -let odometry = Pose2D::identity(); -let result = map.observe(&scan, odometry); - -println!("Pose: ({:.2}, {:.2}), confidence: {:.2}", - result.pose.x, result.pose.y, result.confidence); -``` - -## Map Trait - -VectorMap implements the `Map` trait: - -```rust -pub trait Map: Send + Sync { - fn observe(&mut self, scan: &PointCloud2D, odometry: Pose2D) -> ObserveResult; - fn raycast(&self, from: Point2D, direction: Point2D, max_range: f32) -> f32; - fn query(&self, point: Point2D) -> Occupancy; - fn frontiers(&self) -> Vec; - fn get_path(&self, from: Point2D, to: Point2D) -> Option; - fn bounds(&self) -> Bounds; - fn clear(&mut self); -} -``` - -## Configuration - -```rust -let config = VectorMapConfig { - // Feature extraction - extraction: SplitMergeConfig { - split_threshold: 0.03, - min_segment_points: 5, - min_segment_length: 0.2, - ..Default::default() - }, - - // Scan matching - matching: IcpConfig { - max_iterations: 20, - convergence_threshold: 0.001, - max_correspondence_distance: 0.5, - ..Default::default() - }, - - // Map integration - association: AssociationConfig::default(), - merger: MergerConfig::default(), - - // Corner detection - corner: CornerConfig::default(), - - // Loop closure - loop_closure: LoopClosureConfig::default(), - - // Query settings - occupancy: OccupancyConfig::default(), - frontier: FrontierConfig::default(), - - // Minimum confidence to integrate scan - min_integration_confidence: 0.3, - - // Maximum map size (lines) - max_map_lines: 10000, -}; -``` - -## Observe Result - -```rust -pub struct ObserveResult { - pub pose: Pose2D, // Localized pose - pub confidence: f32, // Match confidence (0.0-1.0) - pub features_extracted: usize, // Lines extracted from scan - pub features_added: usize, // New lines added to map - pub features_merged: usize, // Lines merged with existing - pub icp_iterations: usize, // ICP iterations performed - pub icp_converged: bool, // Did ICP converge? - pub loop_closure_detected: bool, // Was loop closure detected? -} -``` - -## LineStore - -Internal storage with synchronized spatial index: - -```rust -pub struct LineStore { - lines: Vec, - spatial_index: SpatialIndex, - observation_counts: Vec, -} - -impl LineStore { - fn push(&mut self, line: Line2D); - fn get(&self, idx: usize) -> Option<&Line2D>; - fn query_near(&self, point: Point2D, radius: f32) -> Vec; - fn rebuild_index(&mut self); -} -``` - -## SLAM Pipeline - -``` - ┌─────────────────┐ - │ Lidar Scan │ - └────────┬────────┘ - │ - ▼ - ┌─────────────────┐ - │ Extraction │ → Lines + Corners - └────────┬────────┘ - │ - ┌────────────────────┼────────────────────┐ - │ │ │ - ▼ ▼ ▼ - ┌────────────────┐ ┌────────────────┐ ┌────────────────┐ - │ ICP Match │ │ Loop Closure │ │ │ - │ (if enough │ │ Detection │ │ │ - │ features) │ │ │ │ │ - └───────┬────────┘ └───────┬────────┘ │ │ - │ │ │ │ - ▼ ▼ │ │ - ┌────────────────┐ ┌────────────────┐ │ │ - │ Confidence? │ │ Loop Found? │ │ │ - │ >= 0.3 │ │ │ │ │ - └───────┬────────┘ └───────┬────────┘ │ │ - │ │ │ │ - └──────────┬─────────┘ │ │ - │ │ │ - ▼ │ │ - ┌────────────────┐ │ │ - │ Integration │◄─────────────┘ │ - │ (Associate + │ │ - │ Merge + Add) │ │ - └───────┬────────┘ │ - │ │ - ▼ │ - ┌────────────────┐ │ - │ VectorMap │───────────────────────────────┘ - │ (Lines + │ - │ Corners + │ → Queries (raycast, occupancy) - │ Index) │ → Frontiers (exploration) - └────────────────┘ -``` - -## Thread Safety - -VectorMap implements `Send + Sync`, allowing it to be shared across threads with proper synchronization (e.g., `Arc>`). diff --git a/vastu-map/src/vector_map/config.rs b/vastu-map/src/vector_map/config.rs deleted file mode 100644 index 128ad54..0000000 --- a/vastu-map/src/vector_map/config.rs +++ /dev/null @@ -1,490 +0,0 @@ -//! Configuration for VectorMap. - -use serde::{Deserialize, Serialize}; - -use crate::extraction::{CornerConfig, RansacLineConfig, SplitMergeConfig}; -use crate::integration::{AssociationConfig, MergerConfig}; -use crate::loop_closure::LoopClosureConfig; -use crate::matching::IcpConfig; -use crate::query::cbvg::CBVGConfig; -use crate::query::{FrontierConfig, OccupancyConfig}; - -// ============================================================================ -// Configuration Validation -// ============================================================================ - -/// Errors that can occur during configuration validation. -#[derive(Debug, Clone, PartialEq, Serialize, Deserialize)] -pub enum ConfigError { - /// A threshold value is invalid (negative or zero when positive required). - InvalidThreshold { - /// Name of the configuration field. - field: &'static str, - /// The invalid value. - value: f32, - /// Description of the constraint violated. - constraint: &'static str, - }, - /// An iteration count is invalid (zero or too high). - InvalidIterations { - /// Name of the configuration field. - field: &'static str, - /// The invalid value. - value: usize, - /// Description of the constraint violated. - constraint: &'static str, - }, - /// A probability value is out of range [0, 1]. - InvalidProbability { - /// Name of the configuration field. - field: &'static str, - /// The invalid value. - value: f32, - }, - /// Configuration values are inconsistent with each other. - InconsistentConfig { - /// Description of the inconsistency. - message: String, - }, -} - -impl std::fmt::Display for ConfigError { - fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result { - match self { - ConfigError::InvalidThreshold { - field, - value, - constraint, - } => { - write!(f, "invalid threshold {}: {} ({})", field, value, constraint) - } - ConfigError::InvalidIterations { - field, - value, - constraint, - } => { - write!( - f, - "invalid iteration count {}: {} ({})", - field, value, constraint - ) - } - ConfigError::InvalidProbability { field, value } => { - write!( - f, - "invalid probability {}: {} (must be in [0, 1])", - field, value - ) - } - ConfigError::InconsistentConfig { message } => { - write!(f, "inconsistent configuration: {}", message) - } - } - } -} - -impl std::error::Error for ConfigError {} - -/// Configuration for VectorMap. -#[derive(Clone, Debug, Serialize, Deserialize)] -pub struct VectorMapConfig { - /// Configuration for line extraction (split-merge). - pub extraction: SplitMergeConfig, - - /// Configuration for RANSAC line extraction. - /// Used when `use_hybrid_extraction` is true. - pub ransac_extraction: RansacLineConfig, - - /// Whether to use hybrid RANSAC + split-merge extraction. - /// When true, RANSAC extracts dominant lines first, then split-merge - /// processes remaining points. This is more robust to outliers. - /// Default: true - pub use_hybrid_extraction: bool, - - /// Configuration for corner detection. - pub corner: CornerConfig, - - /// Configuration for scan matching (ICP). - pub matching: IcpConfig, - - /// Configuration for line association. - pub association: AssociationConfig, - - /// Configuration for line merging. - pub merger: MergerConfig, - - /// Configuration for frontier detection. - pub frontier: FrontierConfig, - - /// Configuration for occupancy queries. - pub occupancy: OccupancyConfig, - - /// Configuration for clearance-based visibility graph (CBVG). - #[serde(default)] - pub cbvg: CBVGConfig, - - /// Configuration for loop closure detection. - pub loop_closure: LoopClosureConfig, - - /// Minimum match confidence to use scan matching result. - /// Below this, odometry is used instead. - /// Default: 0.3 - pub min_match_confidence: f32, - - /// Whether to update the map with new observations. - /// Set to false for localization-only mode. - /// Default: true - pub mapping_enabled: bool, - - /// Whether loop closure detection is enabled. - /// Default: true - pub loop_closure_enabled: bool, -} - -impl Default for VectorMapConfig { - fn default() -> Self { - Self { - extraction: SplitMergeConfig::default(), - ransac_extraction: RansacLineConfig::default(), - use_hybrid_extraction: true, - corner: CornerConfig::default(), - matching: IcpConfig::default(), - association: AssociationConfig::default(), - merger: MergerConfig::default(), - frontier: FrontierConfig::default(), - occupancy: OccupancyConfig::default(), - cbvg: CBVGConfig::default(), - loop_closure: LoopClosureConfig::default(), - min_match_confidence: 0.3, - mapping_enabled: true, - loop_closure_enabled: true, - } - } -} - -impl VectorMapConfig { - /// Create a new configuration with default values. - pub fn new() -> Self { - Self::default() - } - - // ===== Preset Configurations ===== - - /// Preset for fast but less accurate operation. - /// - /// Use this when CPU is limited or real-time is critical: - /// - Fewer ICP iterations (15 vs 30) - /// - Larger convergence threshold (5e-4 vs 1e-4) - /// - Coarse search disabled - /// - Loop closure disabled - /// - /// # Example - /// ``` - /// use vastu_map::VectorMapConfig; - /// let config = VectorMapConfig::fast(); - /// assert!(!config.loop_closure_enabled); - /// ``` - pub fn fast() -> Self { - let mut config = Self::default(); - config.matching.max_iterations = 15; - config.matching.convergence_threshold = 5e-4; - config.matching.use_coarse_search = false; - config.loop_closure_enabled = false; - config - } - - /// Preset for high accuracy at the cost of speed. - /// - /// Use this when accuracy is more important than real-time: - /// - More ICP iterations (50 vs 30) - /// - Tighter convergence (5e-5 vs 1e-4) - /// - Stricter line association thresholds - /// - Coarse search enabled - /// - /// # Example - /// ``` - /// use vastu_map::VectorMapConfig; - /// let config = VectorMapConfig::accurate(); - /// assert_eq!(config.matching.max_iterations, 50); - /// ``` - pub fn accurate() -> Self { - let mut config = Self::default(); - config.matching.max_iterations = 50; - config.matching.convergence_threshold = 5e-5; - config.matching.use_coarse_search = true; - config.matching.coarse_search_confidence_threshold = 0.6; - config.association.max_perpendicular_distance = 0.10; // Stricter association - config.association.max_angle_difference = 0.15; - config - } - - /// Preset for localization-only mode (no map updates). - /// - /// Use this when operating with a pre-built map: - /// - Mapping disabled - /// - Loop closure disabled - /// - Lower min correspondences (map may be sparse) - /// - /// # Example - /// ``` - /// use vastu_map::VectorMapConfig; - /// let config = VectorMapConfig::localization_only(); - /// assert!(!config.mapping_enabled); - /// ``` - pub fn localization_only() -> Self { - Self { - mapping_enabled: false, - loop_closure_enabled: false, - matching: crate::matching::IcpConfig { - min_correspondences: 5, - ..crate::matching::IcpConfig::default() - }, - ..Self::default() - } - } - - // ===== Common Nested Config Shortcuts ===== - - /// Set the maximum ICP iterations. - /// - /// Shortcut for modifying `config.matching.max_iterations`. - pub fn with_max_icp_iterations(mut self, iterations: usize) -> Self { - self.matching.max_iterations = iterations; - self - } - - /// Set the correspondence distance threshold. - /// - /// Shortcut for modifying `config.matching.max_correspondence_distance`. - pub fn with_correspondence_distance(mut self, meters: f32) -> Self { - self.matching.max_correspondence_distance = meters; - self - } - - /// Set the line association distance threshold. - /// - /// Shortcut for modifying `config.association.max_perpendicular_distance`. - pub fn with_association_distance(mut self, meters: f32) -> Self { - self.association.max_perpendicular_distance = meters; - self - } - - // ===== Builder Methods ===== - - /// Builder-style setter for minimum match confidence. - pub fn with_min_match_confidence(mut self, confidence: f32) -> Self { - self.min_match_confidence = confidence; - self - } - - /// Builder-style setter for mapping enabled. - pub fn with_mapping_enabled(mut self, enabled: bool) -> Self { - self.mapping_enabled = enabled; - self - } - - /// Builder-style setter for loop closure enabled. - pub fn with_loop_closure_enabled(mut self, enabled: bool) -> Self { - self.loop_closure_enabled = enabled; - self - } - - /// Builder-style setter for hybrid RANSAC + split-merge extraction. - /// - /// When enabled, line extraction uses RANSAC to find dominant lines first, - /// then split-merge to process remaining points. This is more robust to - /// outliers and cluttered environments. - pub fn with_hybrid_extraction(mut self, enabled: bool) -> Self { - self.use_hybrid_extraction = enabled; - self - } - - /// Builder-style setter for multi-resolution ICP. - /// - /// When enabled, scan matching uses coarse-to-fine matching with - /// subsampled point clouds for faster convergence from poor initial guesses. - pub fn with_multi_resolution_icp(mut self, enabled: bool) -> Self { - self.matching.multi_resolution.enabled = enabled; - self - } - - /// Builder-style setter for robot radius. - /// - /// Sets the robot radius used for collision checking in path planning - /// and frontier detection. - pub fn with_robot_radius(mut self, radius: f32) -> Self { - self.frontier.robot_radius = radius; - // Update CBVG narrow clearance to match (robot_radius + small margin) - self.cbvg.min_clearance_narrow = radius + 0.02; - self - } - - /// Builder-style setter for CBVG configuration. - pub fn with_cbvg(mut self, config: CBVGConfig) -> Self { - self.cbvg = config; - self - } - - // ===== Configuration Validation ===== - - /// Validate the configuration parameters. - /// - /// Checks that all parameters are within valid ranges and consistent with each other. - /// Call this after building a configuration to catch errors early. - /// - /// # Returns - /// `Ok(())` if configuration is valid, `Err(ConfigError)` otherwise. - /// - /// # Example - /// ```rust - /// use vastu_map::VectorMapConfig; - /// - /// let config = VectorMapConfig::default(); - /// assert!(config.validate().is_ok()); - /// - /// // Invalid config with zero iterations would fail - /// let mut bad_config = VectorMapConfig::default(); - /// bad_config.matching.max_iterations = 0; - /// assert!(bad_config.validate().is_err()); - /// ``` - pub fn validate(&self) -> Result<(), ConfigError> { - // === ICP Configuration === - - if self.matching.max_iterations == 0 { - return Err(ConfigError::InvalidIterations { - field: "matching.max_iterations", - value: 0, - constraint: "must be at least 1", - }); - } - - if self.matching.max_correspondence_distance <= 0.0 { - return Err(ConfigError::InvalidThreshold { - field: "matching.max_correspondence_distance", - value: self.matching.max_correspondence_distance, - constraint: "must be positive", - }); - } - - if self.matching.convergence_threshold <= 0.0 { - return Err(ConfigError::InvalidThreshold { - field: "matching.convergence_threshold", - value: self.matching.convergence_threshold, - constraint: "must be positive", - }); - } - - if self.matching.min_correspondences == 0 { - return Err(ConfigError::InvalidIterations { - field: "matching.min_correspondences", - value: 0, - constraint: "must be at least 1", - }); - } - - // === RANSAC Configuration === - - if self.ransac_extraction.iterations == 0 { - return Err(ConfigError::InvalidIterations { - field: "ransac_extraction.iterations", - value: 0, - constraint: "must be at least 1", - }); - } - - if self.ransac_extraction.inlier_threshold <= 0.0 { - return Err(ConfigError::InvalidThreshold { - field: "ransac_extraction.inlier_threshold", - value: self.ransac_extraction.inlier_threshold, - constraint: "must be positive", - }); - } - - // === Association Configuration === - - if self.association.max_perpendicular_distance <= 0.0 { - return Err(ConfigError::InvalidThreshold { - field: "association.max_perpendicular_distance", - value: self.association.max_perpendicular_distance, - constraint: "must be positive", - }); - } - - if self.association.max_angle_difference <= 0.0 - || self.association.max_angle_difference > std::f32::consts::PI - { - return Err(ConfigError::InvalidThreshold { - field: "association.max_angle_difference", - value: self.association.max_angle_difference, - constraint: "must be in (0, π]", - }); - } - - // === Confidence Threshold === - - if self.min_match_confidence < 0.0 || self.min_match_confidence > 1.0 { - return Err(ConfigError::InvalidProbability { - field: "min_match_confidence", - value: self.min_match_confidence, - }); - } - - // === Split-Merge Configuration === - - if self.extraction.min_length <= 0.0 { - return Err(ConfigError::InvalidThreshold { - field: "extraction.min_length", - value: self.extraction.min_length, - constraint: "must be positive", - }); - } - - if self.extraction.split_threshold <= 0.0 { - return Err(ConfigError::InvalidThreshold { - field: "extraction.split_threshold", - value: self.extraction.split_threshold, - constraint: "must be positive", - }); - } - - // === Corner Detection === - - if self.corner.min_angle <= 0.0 || self.corner.min_angle >= std::f32::consts::PI { - return Err(ConfigError::InvalidThreshold { - field: "corner.min_angle", - value: self.corner.min_angle, - constraint: "must be in (0, π)", - }); - } - - if self.corner.max_angle <= self.corner.min_angle - || self.corner.max_angle > std::f32::consts::PI - { - return Err(ConfigError::InconsistentConfig { - message: format!( - "corner.max_angle ({}) must be > corner.min_angle ({}) and <= π", - self.corner.max_angle, self.corner.min_angle - ), - }); - } - - Ok(()) - } - - /// Recompute derived fields after deserialization. - /// - /// Call this after loading from YAML to ensure computed fields are correct. - pub fn finalize(&mut self) { - self.matching.finalize(); - self.loop_closure.icp_config.finalize(); - } - - /// Validate and return self for builder pattern. - /// - /// # Panics - /// Panics if validation fails. Use `validate()` for error handling. - pub fn validated(self) -> Self { - self.validate().expect("VectorMapConfig validation failed"); - self - } -} diff --git a/vastu-map/src/vector_map/line_store.rs b/vastu-map/src/vector_map/line_store.rs deleted file mode 100644 index 96ace52..0000000 --- a/vastu-map/src/vector_map/line_store.rs +++ /dev/null @@ -1,291 +0,0 @@ -//! LineStore: Encapsulates parallel line data structures. -//! -//! This module provides a wrapper that keeps multiple representations -//! of line features in sync: -//! - `Vec` for iteration and direct access -//! - `LineCollection` (SoA format) for SIMD operations -//! - `SpatialIndex` (R-tree) for spatial queries -//! - `Bounds` for bounding box queries - -use crate::core::{Bounds, Point2D}; -use crate::features::{Line2D, LineCollection}; -use crate::integration::SpatialIndex; - -/// Encapsulates line storage with synchronized auxiliary structures. -/// -/// All modifications go through this struct to ensure consistency -/// between the primary line storage and derived data structures. -pub struct LineStore { - /// Primary line storage. - lines: Vec, - /// SoA format for SIMD batch operations. - collection: LineCollection, - /// R-tree spatial index for fast queries. - index: SpatialIndex, - /// Bounding box of all lines. - bounds: Option, -} - -impl LineStore { - /// Create a new empty LineStore. - pub fn new() -> Self { - Self { - lines: Vec::new(), - collection: LineCollection::new(), - index: SpatialIndex::empty(), - bounds: None, - } - } - - /// Create a LineStore with pre-allocated capacity. - pub fn with_capacity(capacity: usize) -> Self { - Self { - lines: Vec::with_capacity(capacity), - collection: LineCollection::with_capacity(capacity), - index: SpatialIndex::empty(), - bounds: None, - } - } - - /// Check if the store is empty. - #[inline] - pub fn is_empty(&self) -> bool { - self.lines.is_empty() - } - - /// Get the number of lines. - #[inline] - pub fn len(&self) -> usize { - self.lines.len() - } - - /// Get all lines. - #[inline] - pub fn lines(&self) -> &[Line2D] { - &self.lines - } - - /// Get the LineCollection (SoA format). - #[inline] - pub fn collection(&self) -> &LineCollection { - &self.collection - } - - /// Get the spatial index. - #[inline] - pub fn index(&self) -> &SpatialIndex { - &self.index - } - - /// Get the current bounds. - #[inline] - pub fn bounds(&self) -> Option<&Bounds> { - self.bounds.as_ref() - } - - /// Add a single line to the store. - /// - /// Updates all auxiliary structures incrementally. - pub fn add(&mut self, line: Line2D) { - let idx = self.lines.len(); - self.lines.push(line); - self.collection.push_line(&line); - self.update_bounds(&line); - self.index.insert(line, idx); - } - - /// Add multiple lines to the store. - /// - /// Uses incremental updates for small additions, full rebuild for large ones. - pub fn add_many(&mut self, lines: &[Line2D]) { - if lines.is_empty() { - return; - } - - // Threshold for incremental vs full rebuild - const INCREMENTAL_THRESHOLD: usize = 20; - - let start_idx = self.lines.len(); - - for line in lines { - self.lines.push(*line); - self.collection.push_line(line); - self.update_bounds(line); - } - - if lines.len() <= INCREMENTAL_THRESHOLD && self.lines.len() > lines.len() * 2 { - // Incremental insert for small additions to large maps - for (i, line) in lines.iter().enumerate() { - self.index.insert(*line, start_idx + i); - } - } else { - // Full rebuild for large additions or small maps - self.rebuild_index(); - } - } - - /// Update a line at a specific index. - /// - /// Rebuilds auxiliary structures after modification. - pub fn update(&mut self, idx: usize, new_line: Line2D) { - if idx < self.lines.len() { - self.lines[idx] = new_line; - self.rebuild_all(); - } - } - - /// Get mutable access to the lines vector. - /// - /// After modifying lines directly, call `rebuild_all()` to sync structures. - pub fn lines_mut(&mut self) -> &mut Vec { - &mut self.lines - } - - /// Rebuild all auxiliary structures from the lines vector. - /// - /// Call this after making direct modifications to `lines_mut()`. - pub fn rebuild_all(&mut self) { - self.rebuild_collection(); - self.rebuild_index(); - self.recompute_bounds(); - } - - /// Clear all lines and auxiliary structures. - pub fn clear(&mut self) { - self.lines.clear(); - self.collection.clear(); - self.index.clear(); - self.bounds = None; - } - - /// Update bounds to include a line. - fn update_bounds(&mut self, line: &Line2D) { - match &mut self.bounds { - Some(b) => { - b.expand_to_include(line.start); - b.expand_to_include(line.end); - } - None => { - let mut bounds = Bounds::from_point(line.start); - bounds.expand_to_include(line.end); - self.bounds = Some(bounds); - } - } - } - - /// Recompute bounds from all lines. - fn recompute_bounds(&mut self) { - if self.lines.is_empty() { - self.bounds = None; - return; - } - - let mut bounds = Bounds::from_point(self.lines[0].start); - for line in &self.lines { - bounds.expand_to_include(line.start); - bounds.expand_to_include(line.end); - } - self.bounds = Some(bounds); - } - - /// Rebuild the spatial index from lines. - fn rebuild_index(&mut self) { - self.index.rebuild(&self.lines); - } - - /// Rebuild the LineCollection from lines. - fn rebuild_collection(&mut self) { - self.collection = LineCollection::from_lines(&self.lines); - } - - /// Query lines within distance of a point. - pub fn query_near(&self, point: Point2D, distance: f32) -> Vec<(usize, f32)> { - self.index.lines_within_distance(point, distance) - } -} - -impl Default for LineStore { - fn default() -> Self { - Self::new() - } -} - -#[cfg(test)] -mod tests { - use super::*; - - #[test] - fn test_new_store_is_empty() { - let store = LineStore::new(); - assert!(store.is_empty()); - assert_eq!(store.len(), 0); - assert!(store.bounds().is_none()); - } - - #[test] - fn test_add_single_line() { - let mut store = LineStore::new(); - let line = Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(5.0, 0.0)); - - store.add(line); - - assert_eq!(store.len(), 1); - assert!(!store.is_empty()); - assert!(store.bounds().is_some()); - assert_eq!(store.lines()[0], line); - } - - #[test] - fn test_add_many_lines() { - let mut store = LineStore::new(); - let lines = vec![ - Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(5.0, 0.0)), - Line2D::new(Point2D::new(5.0, 0.0), Point2D::new(5.0, 5.0)), - Line2D::new(Point2D::new(5.0, 5.0), Point2D::new(0.0, 5.0)), - ]; - - store.add_many(&lines); - - assert_eq!(store.len(), 3); - assert_eq!(store.collection().len(), 3); - } - - #[test] - fn test_bounds_grow() { - let mut store = LineStore::new(); - - store.add(Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(1.0, 0.0))); - let bounds1 = store.bounds().unwrap().clone(); - - store.add(Line2D::new(Point2D::new(1.0, 0.0), Point2D::new(5.0, 5.0))); - let bounds2 = store.bounds().unwrap(); - - assert!(bounds2.width() > bounds1.width()); - assert!(bounds2.height() > bounds1.height()); - } - - #[test] - fn test_clear() { - let mut store = LineStore::new(); - store.add(Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(5.0, 0.0))); - - store.clear(); - - assert!(store.is_empty()); - assert!(store.bounds().is_none()); - } - - #[test] - fn test_rebuild_after_mutation() { - let mut store = LineStore::new(); - store.add(Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(1.0, 0.0))); - - // Modify directly - store.lines_mut()[0] = Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(10.0, 0.0)); - store.rebuild_all(); - - // Bounds should reflect the new line - let bounds = store.bounds().unwrap(); - assert!(bounds.max.x >= 10.0); - } -} diff --git a/vastu-map/src/vector_map/map.rs b/vastu-map/src/vector_map/map.rs deleted file mode 100644 index 8257ae3..0000000 --- a/vastu-map/src/vector_map/map.rs +++ /dev/null @@ -1,1394 +0,0 @@ -//! VectorMap: The main SLAM map implementation. - -use std::time::Instant; - -use crate::config::ExplorationConfig; -use crate::core::{Bounds, Point2D, PointCloud2D, Pose2D}; -use crate::extraction::{detect_corners, extract_lines, extract_lines_hybrid}; -use crate::features::{Corner2D, FeatureSet, Line2D}; -use crate::integration::{ - PointAssociationConfig, RefitConfig, RefitResult, RefitStats, ScanStore, ScanStoreConfig, - associate_points_to_lines, batch_merge, create_new_line, find_associations, - find_unmatched_scan_lines, refit_line, -}; -use crate::loop_closure::{LoopClosure, LoopClosureDetector}; -use crate::matching::PointToLineIcp; -use crate::query::cbvg::{ClearanceVisibilityGraph, VisibilityRegion}; -use crate::query::{is_straight_path_clear, query_occupancy, raycast}; -use crate::{Frontier, Map, ObserveResult, Occupancy, Path, TimingBreakdown}; - -use super::config::VectorMapConfig; -use super::line_store::LineStore; - -// ============================================================================ -// Scratch Space for Zero-Allocation Observe Pipeline -// ============================================================================ - -/// Pre-allocated scratch space for the observe pipeline. -/// -/// Eliminates per-observation allocations by reusing buffers across calls. -/// This is critical for real-time SLAM at high observation rates (5-110 Hz). -/// -/// # Performance Impact -/// -/// Without scratch space, each `observe()` call allocates: -/// - `robot_frame_points`: O(n) points for ICP (done up to 3x per observation!) -/// - `lines_robot`: O(m) lines from extraction -/// - `lines_world`: O(m) lines after transform -/// - `scan_lines`: O(m) lines for integration -/// -/// With scratch space, these allocations happen once and are reused. -#[derive(Clone, Debug, Default)] -pub struct ObserveScratchSpace { - /// Buffer for robot-frame scan points. - /// Reused for ICP matching and loop closure. - robot_frame_points: Vec, - - /// Buffer for extracted lines in robot frame. - lines_robot: Vec, - - /// Buffer for lines transformed to world frame. - lines_world: Vec, -} - -impl ObserveScratchSpace { - /// Create new scratch space with capacity hints. - /// - /// # Arguments - /// * `max_points` - Expected maximum scan points (e.g., 360 for typical lidar) - /// * `max_lines` - Expected maximum lines per scan (e.g., 20-50) - pub fn with_capacity(max_points: usize, max_lines: usize) -> Self { - Self { - robot_frame_points: Vec::with_capacity(max_points), - lines_robot: Vec::with_capacity(max_lines), - lines_world: Vec::with_capacity(max_lines), - } - } - - /// Create with default capacity (360 points, 50 lines). - pub fn default_capacity() -> Self { - Self::with_capacity(360, 50) - } - - /// Fill robot_frame_points from a PointCloud2D. - #[inline] - pub fn fill_robot_frame_points(&mut self, scan: &PointCloud2D) { - self.robot_frame_points.clear(); - self.robot_frame_points.reserve(scan.len()); - self.robot_frame_points.extend(scan.iter()); - } - - /// Get robot frame points slice. - #[inline] - pub fn robot_frame_points(&self) -> &[Point2D] { - &self.robot_frame_points - } - - /// Fill lines_robot from extracted lines. - #[inline] - pub fn fill_lines_robot(&mut self, lines: &[Line2D]) { - self.lines_robot.clear(); - self.lines_robot.extend_from_slice(lines); - } - - /// Transform robot-frame lines to world frame and store in lines_world. - #[inline] - pub fn transform_lines_to_world(&mut self, pose: &Pose2D) { - self.lines_world.clear(); - self.lines_world.reserve(self.lines_robot.len()); - for line in &self.lines_robot { - self.lines_world.push(line.transform(pose)); - } - } - - /// Get world-frame lines slice. - #[inline] - pub fn lines_world(&self) -> &[Line2D] { - &self.lines_world - } -} - -/// VectorMap: Feature-based 2D SLAM map. -/// -/// Stores the environment as a collection of line and corner features. -/// Provides localization via scan matching and mapping via feature integration. -/// -/// # Example -/// -/// ```rust,ignore -/// use vastu_map::{VectorMap, VectorMapConfig, Map}; -/// use vastu_map::core::{Pose2D, PointCloud2D, Point2D}; -/// -/// let config = VectorMapConfig::default(); -/// let mut map = VectorMap::new(config); -/// -/// // Process scans -/// let scan = PointCloud2D::from_points(&[ -/// Point2D::new(1.0, 0.0), -/// Point2D::new(1.0, 0.1), -/// Point2D::new(1.0, 0.2), -/// ]); -/// let odometry = Pose2D::identity(); -/// -/// let result = map.observe(&scan, odometry); -/// println!("Pose: ({:.2}, {:.2}, {:.2})", result.pose.x, result.pose.y, result.pose.theta); -/// ``` -pub struct VectorMap { - /// Configuration. - config: VectorMapConfig, - - /// Line storage with synchronized auxiliary structures. - line_store: LineStore, - - /// Corner features. - corners: Vec, - - /// Current robot pose estimate. - current_pose: Pose2D, - - /// Number of observations processed. - observation_count: usize, - - /// Loop closure detector. - loop_detector: LoopClosureDetector, - - /// Detected loop closures. - loop_closures: Vec, - - /// Persistent ICP matcher (preserves state across scans for adaptive coarse search). - icp_matcher: PointToLineIcp, - - /// Pre-allocated scratch space for observe pipeline (zero-allocation). - observe_scratch: ObserveScratchSpace, - - /// Scan storage for exploration mode. - /// When enabled, stores raw scan data for point-cloud-based line re-fitting. - scan_store: Option, - - /// Exploration configuration (None if exploration mode disabled). - exploration_config: Option, - - /// Clearance-based visibility graph for path planning. - /// Automatically updated when lines are added to the map. - cbvg: ClearanceVisibilityGraph, - - /// Whether the CBVG needs rebuilding. - cbvg_dirty: bool, - - /// Visibility region tracking lidar-scanned areas. - /// Used to ensure CBVG nodes are only placed in observed areas. - visibility: VisibilityRegion, -} - -impl VectorMap { - /// Create a new empty VectorMap. - pub fn new(config: VectorMapConfig) -> Self { - let loop_detector = LoopClosureDetector::new(config.loop_closure.clone()); - let icp_matcher = PointToLineIcp::with_config(config.matching.clone()); - let cbvg = ClearanceVisibilityGraph::new(config.cbvg.clone()); - // Use CBVG coverage_node_interval as visibility resolution for consistency - let visibility = VisibilityRegion::new(config.cbvg.coverage_node_interval * 0.5); - Self { - config, - line_store: LineStore::new(), - corners: Vec::new(), - current_pose: Pose2D::identity(), - observation_count: 0, - loop_detector, - loop_closures: Vec::new(), - icp_matcher, - observe_scratch: ObserveScratchSpace::default_capacity(), - scan_store: None, - exploration_config: None, - cbvg, - cbvg_dirty: true, - visibility, - } - } - - /// Get the current configuration. - pub fn config(&self) -> &VectorMapConfig { - &self.config - } - - /// Get all lines in the map. - pub fn lines(&self) -> &[Line2D] { - self.line_store.lines() - } - - /// Get all corners in the map. - pub fn corners(&self) -> &[Corner2D] { - &self.corners - } - - /// Get the current robot pose. - pub fn current_pose(&self) -> Pose2D { - self.current_pose - } - - /// Get the number of observations processed. - pub fn observation_count(&self) -> usize { - self.observation_count - } - - /// Get the feature set (lines and corners). - pub fn features(&self) -> FeatureSet { - FeatureSet::from_features(self.line_store.lines(), &self.corners) - } - - /// Get detected loop closures. - /// - /// These can be used by an external pose graph optimizer to correct drift. - pub fn get_loop_closures(&self) -> &[LoopClosure] { - &self.loop_closures - } - - /// Get the number of detected loop closures. - pub fn loop_closure_count(&self) -> usize { - self.loop_closures.len() - } - - /// Get the loop closure detector (for advanced access). - pub fn loop_detector(&self) -> &LoopClosureDetector { - &self.loop_detector - } - - /// Clear all loop closures. - pub fn clear_loop_closures(&mut self) { - self.loop_closures.clear(); - } - - // ───────────────────────────────────────────────────────────────────────── - // Visibility and Path Planning - // ───────────────────────────────────────────────────────────────────────── - - /// Get the visibility region (for visualization/debugging). - pub fn visibility(&self) -> &VisibilityRegion { - &self.visibility - } - - /// Build and return a CBVG for visualization. - /// - /// Returns a clone of the internal CBVG which is incrementally updated - /// with each lidar scan via `add_nodes_from_scan`. - pub fn build_cbvg(&self) -> ClearanceVisibilityGraph { - self.cbvg.clone() - } - - /// Find the nearest CBVG node to a target point. - /// - /// This is useful for exploration to find reachable navigation targets - /// near frontiers. CBVG nodes are guaranteed to be in visible, reachable - /// areas with proper wall clearance. - /// - /// # Arguments - /// * `target` - The point to find the nearest node to - /// * `max_distance` - Maximum distance to search (nodes farther are ignored) - /// - /// # Returns - /// The position of the nearest CBVG node, or None if no node is within range. - pub fn nearest_cbvg_node(&self, target: Point2D, max_distance: f32) -> Option { - let cbvg = self.build_cbvg(); - cbvg.nearest_node(target, max_distance) - } - - // ───────────────────────────────────────────────────────────────────────── - // Exploration Mode - // ───────────────────────────────────────────────────────────────────────── - - /// Enable exploration mode with the given configuration. - /// - /// In exploration mode, VectorMap stores raw scan data and periodically - /// re-fits lines from accumulated point clouds, eliminating first-scan bias. - /// - /// # Arguments - /// * `config` - Exploration configuration - /// - /// # Example - /// ```rust,ignore - /// use vastu_map::config::ExplorationConfig; - /// - /// let config = ExplorationConfig::default() - /// .with_refit_interval(10) - /// .with_gap_threshold(0.3); - /// - /// map.enable_exploration_mode(config); - /// ``` - pub fn enable_exploration_mode(&mut self, config: ExplorationConfig) { - let scan_config = ScanStoreConfig::default(); - self.scan_store = Some(ScanStore::new(scan_config)); - self.exploration_config = Some(config); - } - - /// Enable exploration mode with custom scan store configuration. - /// - /// Allows configuring max scans, distance/rotation filtering, etc. - pub fn enable_exploration_mode_with_scan_config( - &mut self, - exploration_config: ExplorationConfig, - scan_config: ScanStoreConfig, - ) { - self.scan_store = Some(ScanStore::new(scan_config)); - self.exploration_config = Some(exploration_config); - } - - /// Disable exploration mode and return the accumulated scan store. - /// - /// Returns the ScanStore containing all accumulated scans, or None - /// if exploration mode was not enabled. - pub fn disable_exploration_mode(&mut self) -> Option { - self.exploration_config = None; - self.scan_store.take() - } - - /// Check if exploration mode is enabled. - pub fn is_exploration_mode(&self) -> bool { - self.exploration_config.is_some() - } - - /// Get the exploration configuration (if exploration mode is enabled). - pub fn exploration_config(&self) -> Option<&ExplorationConfig> { - self.exploration_config.as_ref() - } - - /// Get the scan store (if exploration mode is enabled). - pub fn scan_store(&self) -> Option<&ScanStore> { - self.scan_store.as_ref() - } - - /// Get mutable access to the scan store (if exploration mode is enabled). - pub fn scan_store_mut(&mut self) -> Option<&mut ScanStore> { - self.scan_store.as_mut() - } - - /// Add a line to the map. - pub fn add_line(&mut self, line: Line2D) { - self.line_store.add(line); - } - - /// Add multiple lines to the map. - pub fn add_lines(&mut self, lines: &[Line2D]) { - self.line_store.add_many(lines); - } - - /// Set the current pose directly. - pub fn set_pose(&mut self, pose: Pose2D) { - self.current_pose = pose; - } - - /// Process a scan observation. - /// - /// This is the main SLAM function. It: - /// 1. Extracts features from the scan - /// 2. Matches against the map (localization) - /// 3. Updates the map with new features (mapping) - /// 4. Checks for loop closure (if enabled) - fn process_scan(&mut self, scan: &PointCloud2D, odometry: Pose2D) -> ObserveResult { - let total_start = Instant::now(); - let mut timing = TimingBreakdown::new(); - - self.observation_count += 1; - - // If map is empty, use odometry and add all features - if self.line_store.is_empty() { - return self.initialize_from_scan(scan, odometry); - } - - // Apply odometry to get predicted pose - let predicted_pose = self.current_pose.compose(odometry); - - // Get robot-frame scan points for weighted ICP matching (reuse scratch buffer) - self.observe_scratch.fill_robot_frame_points(scan); - - // ─── ICP Matching ─── - let icp_start = Instant::now(); - // Use cached LineCollection from LineStore (avoids rebuilding SoA structure) - let match_result = self.icp_matcher.match_scan_robot_frame_collection( - self.observe_scratch.robot_frame_points(), - self.line_store.collection(), - predicted_pose, - ); - timing.icp_matching_us = icp_start.elapsed().as_micros() as u64; - - // Update ICP matcher's confidence state for adaptive coarse search triggering - self.icp_matcher - .update_last_confidence(match_result.confidence); - - // Determine final pose and capture ICP stats - let icp_valid = if match_result.converged - && match_result.confidence >= self.config.min_match_confidence - { - // Check for degenerate geometry - if match_result.condition_number > 100.0 { - log::debug!( - "Rejecting ICP match: degenerate geometry (cond={})", - match_result.condition_number - ); - false - } else { - // Check if ICP pose is reasonable (not too far from prediction) - let dx = match_result.pose.x - predicted_pose.x; - let dy = match_result.pose.y - predicted_pose.y; - let dist = (dx * dx + dy * dy).sqrt(); - let dtheta = - crate::core::math::angle_diff(match_result.pose.theta, predicted_pose.theta) - .abs(); - - if dist > 0.2 || dtheta > 0.175 { - log::debug!( - "Rejecting ICP match: large deviation (dist={:.3}m, dtheta={:.1}°)", - dist, - dtheta.to_degrees() - ); - false - } else { - true - } - } - } else { - false - }; - - let (final_pose, confidence, icp_iterations, icp_converged) = if icp_valid { - ( - match_result.pose, - match_result.confidence, - match_result.iterations, - match_result.converged, - ) - } else { - ( - predicted_pose, - 0.0, - match_result.iterations, - match_result.converged, - ) - }; - - self.current_pose = final_pose; - - // ─── Visibility Update ─── - // Update visibility region with scan points in world frame - // robot_frame_points are already in scratch buffer - let world_scan_points: Vec = self - .observe_scratch - .robot_frame_points() - .iter() - .map(|&p| final_pose.transform_point(p)) - .collect(); - self.visibility - .update_from_scan(final_pose, &world_scan_points); - // Incrementally add nodes from this scan - self.cbvg - .add_nodes_from_scan(final_pose, &world_scan_points, self.line_store.lines()); - // Clean up any existing nodes that are now too close to scan points - self.cbvg - .cleanup_nodes_near_scan_points(&world_scan_points, self.line_store.lines()); - // Clean up any edges that pass too close to scan points - self.cbvg.cleanup_edges_near_scan_points(&world_scan_points); - self.cbvg_dirty = false; - - // ─── Scan Storage (Exploration Mode) ─── - if let Some(ref mut store) = self.scan_store { - store.add_scan( - odometry, final_pose, scan, - 0, // features_extracted - will be updated after extraction - confidence, - ); - } - - // ─── Feature Extraction ─── - // (robot_frame_points already filled in scratch above) - let features = self.extract_features_timed(&final_pose, &mut timing); - - // ─── Feature Integration ─── - let (features_added, features_merged) = if self.config.mapping_enabled { - self.integrate_features_timed(&mut timing) - } else { - (0, 0) - }; - - // ─── Periodic Line Re-fitting (Exploration Mode) ─── - if let Some(ref config) = self.exploration_config - && config.refit_interval > 0 - && self.observation_count.is_multiple_of(config.refit_interval) - { - log::debug!( - "Triggering line re-fit at observation {}", - self.observation_count - ); - // Trigger line re-fitting from accumulated scans - let _ = self.refit_lines_from_scans(); - } - - // ─── Loop Closure ─── - let loop_closure_detected = if self.config.loop_closure_enabled { - let lc_start = Instant::now(); - // Reuse robot_frame_points from scratch (already filled for ICP) - let feature_lines = features.lines(); - let feature_corners = features.corners(); - - let detected = if let Some(loop_closure) = self.loop_detector.process( - final_pose, - &feature_lines, - &feature_corners, - self.observe_scratch.robot_frame_points(), - ) { - self.loop_closures.push(loop_closure); - true - } else { - false - }; - timing.loop_closure_us = lc_start.elapsed().as_micros() as u64; - detected - } else { - false - }; - - timing.total_us = total_start.elapsed().as_micros() as u64; - - ObserveResult { - pose: final_pose, - confidence, - features_extracted: features.lines().len(), - features_added, - features_merged, - icp_iterations, - icp_converged, - loop_closure_detected, - timing, - } - } - - /// Initialize map from first scan. - fn initialize_from_scan(&mut self, scan: &PointCloud2D, odometry: Pose2D) -> ObserveResult { - let total_start = Instant::now(); - let mut timing = TimingBreakdown::new(); - - self.current_pose = odometry; - - // Fill scratch with robot frame points (used by feature extraction and loop closure) - self.observe_scratch.fill_robot_frame_points(scan); - - // ─── Visibility Update ─── - // Update visibility region with scan points in world frame - let world_scan_points: Vec = self - .observe_scratch - .robot_frame_points() - .iter() - .map(|&p| odometry.transform_point(p)) - .collect(); - self.visibility - .update_from_scan(odometry, &world_scan_points); - // Incrementally add nodes from this scan - self.cbvg - .add_nodes_from_scan(odometry, &world_scan_points, self.line_store.lines()); - // Clean up any existing nodes that are now too close to scan points - self.cbvg - .cleanup_nodes_near_scan_points(&world_scan_points, self.line_store.lines()); - // Clean up any edges that pass too close to scan points - self.cbvg.cleanup_edges_near_scan_points(&world_scan_points); - self.cbvg_dirty = false; - - // ─── Scan Storage (Exploration Mode) ─── - if let Some(ref mut store) = self.scan_store { - store.add_scan( - odometry, odometry, // First scan: odometry = estimated pose - scan, 0, // features_extracted - unknown at this point - 0.0, // confidence - no ICP for first scan - ); - } - - // Extract features from robot-frame scan, transform to world using odometry - let features = self.extract_features_timed(&odometry, &mut timing); - - // Add all features to map - let num_lines = features.lines().len(); - self.add_lines(&features.lines()); - - for corner in features.corners() { - self.corners.push(corner); - } - - // Initialize first keyframe if loop closure is enabled - if self.config.loop_closure_enabled { - let lc_start = Instant::now(); - let feature_lines = features.lines(); - let feature_corners = features.corners(); - self.loop_detector.force_keyframe( - odometry, - &feature_lines, - &feature_corners, - self.observe_scratch.robot_frame_points(), - ); - timing.loop_closure_us = lc_start.elapsed().as_micros() as u64; - } - - timing.total_us = total_start.elapsed().as_micros() as u64; - - ObserveResult { - pose: odometry, - confidence: 0.0, - features_extracted: num_lines, - features_added: num_lines, - features_merged: 0, - icp_iterations: 0, - icp_converged: false, - loop_closure_detected: false, - timing, - } - } - - /// Extract features from a scan with timing. - /// - /// NOTE: Caller must fill observe_scratch.robot_frame_points before calling this. - fn extract_features_timed( - &mut self, - pose: &Pose2D, - timing: &mut TimingBreakdown, - ) -> FeatureSet { - // Use robot_frame_points from scratch (already filled by caller) - let points = self.observe_scratch.robot_frame_points(); - - // ─── Line Extraction ─── - let line_start = Instant::now(); - let lines_robot = if self.config.use_hybrid_extraction { - // Hybrid: RANSAC for dominant lines, then split-merge for remaining - extract_lines_hybrid( - points, - &self.config.ransac_extraction, - &self.config.extraction, - ) - } else { - // Default: split-merge (preserves angular ordering) - extract_lines(points, &self.config.extraction) - }; - - // Transform lines to world frame using scratch buffer - self.observe_scratch.fill_lines_robot(&lines_robot); - self.observe_scratch.transform_lines_to_world(pose); - timing.line_extraction_us = line_start.elapsed().as_micros() as u64; - - // ─── Corner Detection ─── - let corner_start = Instant::now(); - let corners = detect_corners(self.observe_scratch.lines_world(), &self.config.corner); - timing.corner_detection_us = corner_start.elapsed().as_micros() as u64; - - FeatureSet::from_features(self.observe_scratch.lines_world(), &corners) - } - - /// Integrate new features into the map with timing. - /// - /// NOTE: Uses lines_world from observe_scratch (filled by extract_features_timed). - fn integrate_features_timed(&mut self, timing: &mut TimingBreakdown) -> (usize, usize) { - // Use lines from scratch (already in world frame from extract_features_timed) - let scan_lines = self.observe_scratch.lines_world(); - - if scan_lines.is_empty() { - return (0, 0); - } - - // ─── Association ─── - let assoc_start = Instant::now(); - let associations = find_associations( - scan_lines, - self.line_store.lines(), - Some(self.line_store.index()), - &self.config.association, - ); - timing.association_us = assoc_start.elapsed().as_micros() as u64; - - // ─── Merging ─── - let merge_start = Instant::now(); - let merged = batch_merge( - scan_lines, - self.line_store.lines_mut(), - &associations, - &self.config.merger, - ); - let features_merged = merged.len(); - timing.merging_us = merge_start.elapsed().as_micros() as u64; - - // ─── New Features ─── - let new_start = Instant::now(); - let unmatched = find_unmatched_scan_lines( - scan_lines, - self.line_store.lines(), - Some(self.line_store.index()), - &self.config.association, - ); - - let mut features_added = 0; - for idx in unmatched { - let new_line = create_new_line(&scan_lines[idx]); - self.line_store.lines_mut().push(new_line); - features_added += 1; - } - - // Rebuild auxiliary structures - self.line_store.rebuild_all(); - - // Update corners - self.update_corners(); - timing.new_features_us = new_start.elapsed().as_micros() as u64; - - (features_added, features_merged) - } - - /// Update corners based on current lines. - fn update_corners(&mut self) { - self.corners = detect_corners(self.line_store.lines(), &self.config.corner); - } - - /// Optimize map by merging coplanar line segments. - /// - /// Call this periodically (e.g., after N observations) to clean up - /// redundant line segments that represent the same wall. - /// - /// # Arguments - /// * `config` - Configuration for coplanar merging thresholds - /// - /// # Returns - /// Number of lines merged (removed from the map) - /// - /// # Example - /// ```rust,ignore - /// use vastu_map::integration::CoplanarMergeConfig; - /// - /// // After exploration... - /// let config = CoplanarMergeConfig::default() - /// .with_max_angle_diff(0.0175) // 1 degree - /// .with_max_perpendicular_dist(0.02); // 2cm - /// let merged = map.optimize_lines(&config); - /// println!("Merged {} redundant lines", merged); - /// ``` - pub fn optimize_lines(&mut self, config: &crate::integration::CoplanarMergeConfig) -> usize { - let merged = - crate::integration::optimize_coplanar_lines(self.line_store.lines_mut(), config); - - if merged > 0 { - // Rebuild auxiliary structures - self.line_store.rebuild_all(); - // Update corners based on new line set - self.update_corners(); - } - - merged - } - - // ───────────────────────────────────────────────────────────────────────── - // Line Re-fitting from Accumulated Scans - // ───────────────────────────────────────────────────────────────────────── - - /// Re-fit all map lines from accumulated scan points. - /// - /// This method uses points from the ScanStore to re-fit line geometry, - /// eliminating first-scan bias. Call this periodically during exploration. - /// - /// Requires exploration mode to be enabled. - /// - /// # Returns - /// Statistics about the re-fitting operation, or None if exploration mode - /// is not enabled. - /// - /// # Example - /// ```rust,ignore - /// if map.is_exploration_mode() && map.observation_count() % 10 == 0 { - /// if let Some(stats) = map.refit_lines_from_scans() { - /// println!("Refitted lines: {}/{} success rate", - /// stats.lines_refitted_single + stats.lines_split, - /// stats.total_processed()); - /// } - /// } - /// ``` - pub fn refit_lines_from_scans(&mut self) -> Option { - // Get exploration config and scan store - let exploration_config = self.exploration_config.as_ref()?; - let scan_store = self.scan_store.as_ref()?; - - if self.line_store.is_empty() || scan_store.is_empty() { - return Some(RefitStats::default()); - } - - // Build association config from exploration config - let assoc_config = PointAssociationConfig::default() - .with_max_distance(exploration_config.max_point_distance); - - // Associate all stored points with map lines - let associations = associate_points_to_lines( - scan_store, - self.line_store.lines(), - self.line_store.index(), - &assoc_config, - ); - - log::debug!( - "Refit: {} scans, {} lines, {} points associated ({:.1}% rate)", - scan_store.len(), - self.line_store.lines().len(), - associations.total_associated, - associations.association_rate() * 100.0 - ); - - // Build robot pose map for weighted fitting - let robot_poses: std::collections::HashMap = scan_store - .iter() - .map(|scan| (scan.scan_id, scan.estimated_pose)) - .collect(); - - // Build refit config from exploration config - let refit_config = RefitConfig::default() - .with_min_points(exploration_config.min_points_per_line) - .with_gap_threshold(exploration_config.gap_threshold) - .with_noise_model(exploration_config.noise_model); - - // Re-fit each line - let mut stats = RefitStats::default(); - let mut new_lines: Vec = Vec::new(); - - for (line_idx, associated_points) in associations.line_points.iter().enumerate() { - let original = &self.line_store.lines()[line_idx]; - - let result = refit_line(associated_points, original, &robot_poses, &refit_config); - - match result { - RefitResult::Single(line) => { - new_lines.push(line); - stats.lines_refitted_single += 1; - } - RefitResult::Split(lines) => { - stats.total_segments_created += lines.len(); - new_lines.extend(lines); - stats.lines_split += 1; - } - RefitResult::Insufficient => { - // Keep original line - new_lines.push(*original); - stats.lines_insufficient += 1; - } - } - } - - // Replace all lines and rebuild - *self.line_store.lines_mut() = new_lines; - self.line_store.rebuild_all(); - self.update_corners(); - - log::debug!( - "Refit complete: {} single, {} split, {} insufficient, {} new segments", - stats.lines_refitted_single, - stats.lines_split, - stats.lines_insufficient, - stats.total_segments_created - ); - - Some(stats) - } -} - -impl Map for VectorMap { - fn observe(&mut self, scan: &PointCloud2D, odometry: Pose2D) -> ObserveResult { - self.process_scan(scan, odometry) - } - - fn raycast(&self, from: Point2D, direction: Point2D, max_range: f32) -> f32 { - raycast(from, direction, max_range, self.line_store.lines()) - } - - fn query(&self, point: Point2D) -> Occupancy { - query_occupancy( - point, - self.line_store.lines(), - self.line_store.bounds(), - &self.config.occupancy, - ) - } - - fn frontiers(&self) -> Vec { - // Use CBVG-based frontier detection - // A node is a frontier if it has a large uncovered angular gap - // (no neighbors or walls in that direction) - let (frontiers, _indices) = self.cbvg.detect_frontiers( - self.line_store.lines(), - self.config.frontier.min_frontier_angle, - self.config.frontier.max_wall_distance, - ); - frontiers - } - - fn get_path(&self, from: Point2D, to: Point2D) -> Option { - // Use the internal CBVG which is incrementally updated with each scan - self.cbvg.find_path(from, to, self.line_store.lines()) - } - - fn get_path_excluding_nodes( - &self, - from: Point2D, - to: Point2D, - excluded_goal_nodes: &[usize], - ) -> Option<(Path, usize)> { - // Use CBVG with node exclusions for alternative approach angles - self.cbvg - .find_path_excluding_nodes(from, to, self.line_store.lines(), excluded_goal_nodes) - } - - fn nearest_cbvg_node(&self, target: Point2D, max_distance: f32) -> Option { - self.cbvg.nearest_node(target, max_distance) - } - - fn is_path_clear(&self, from: Point2D, to: Point2D) -> bool { - is_straight_path_clear( - from, - to, - self.line_store.lines(), - self.config.frontier.robot_radius, - ) - } - - fn bounds(&self) -> Bounds { - self.line_store - .bounds() - .cloned() - .unwrap_or_else(Bounds::empty) - } - - fn clear(&mut self) { - self.line_store.clear(); - self.corners.clear(); - self.current_pose = Pose2D::identity(); - self.observation_count = 0; - self.loop_detector.clear(); - self.loop_closures.clear(); - self.icp_matcher.update_last_confidence(1.0); - self.cbvg.clear(); - self.cbvg_dirty = true; - // Reset visibility to empty (create new instance with same resolution) - self.visibility = VisibilityRegion::new(self.config.cbvg.coverage_node_interval * 0.5); - } -} - -impl Default for VectorMap { - fn default() -> Self { - Self::new(VectorMapConfig::default()) - } -} - -#[cfg(test)] -mod tests { - use super::*; - - fn make_simple_scan() -> PointCloud2D { - let mut xs = Vec::new(); - let mut ys = Vec::new(); - - for i in -10..=10 { - xs.push(2.0); - ys.push(i as f32 * 0.1); - } - - PointCloud2D { xs, ys } - } - - fn make_room_scan() -> PointCloud2D { - let mut xs = Vec::new(); - let mut ys = Vec::new(); - - for i in -10..=10 { - let y = i as f32 * 0.2; - xs.push(-2.0); - ys.push(y); - xs.push(2.0); - ys.push(y); - } - for i in -10..=10 { - let x = i as f32 * 0.2; - xs.push(x); - ys.push(-2.0); - xs.push(x); - ys.push(2.0); - } - - PointCloud2D { xs, ys } - } - - #[test] - fn test_new_map() { - let config = VectorMapConfig::default(); - let map = VectorMap::new(config); - - assert!(map.lines().is_empty()); - assert!(map.corners.is_empty()); - assert_eq!(map.observation_count, 0); - } - - #[test] - fn test_add_line() { - let mut map = VectorMap::default(); - let line = Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(5.0, 0.0)); - - map.add_line(line); - - assert_eq!(map.lines().len(), 1); - assert!(map.line_store.bounds().is_some()); - } - - #[test] - fn test_first_observation() { - let mut map = VectorMap::default(); - let scan = make_simple_scan(); - - let result = map.observe(&scan, Pose2D::identity()); - - assert_eq!(result.pose, Pose2D::identity()); - assert!(map.lines().len() > 0 || result.features_extracted > 0); - assert_eq!(map.observation_count, 1); - } - - #[test] - fn test_raycast_through_map() { - let mut map = VectorMap::default(); - map.add_line(Line2D::new(Point2D::new(5.0, -5.0), Point2D::new(5.0, 5.0))); - - let dist = map.raycast(Point2D::zero(), Point2D::new(1.0, 0.0), 100.0); - - assert!((dist - 5.0).abs() < 0.1); - } - - #[test] - fn test_query_occupancy() { - let mut map = VectorMap::default(); - map.add_line(Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(10.0, 0.0))); - - let on_line = map.query(Point2D::new(5.0, 0.0)); - assert_eq!(on_line, Occupancy::Occupied); - } - - #[test] - fn test_bounds() { - let mut map = VectorMap::default(); - - let empty_bounds = map.bounds(); - assert!(empty_bounds.is_empty()); - - map.add_line(Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(10.0, 5.0))); - - let bounds = map.bounds(); - assert!(!bounds.is_empty()); - assert!(bounds.contains(Point2D::new(5.0, 2.0))); - } - - #[test] - fn test_clear() { - let mut map = VectorMap::default(); - map.add_line(Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(5.0, 0.0))); - - map.clear(); - - assert!(map.lines().is_empty()); - assert!(map.corners.is_empty()); - assert!(map.line_store.bounds().is_none()); - assert_eq!(map.current_pose, Pose2D::identity()); - } - - #[test] - fn test_frontiers_empty_map() { - let map = VectorMap::default(); - let frontiers = map.frontiers(); - - assert!(frontiers.is_empty()); - } - - /// Create a point cloud for a partial room (3 walls, top missing). - /// Simulates a lidar scan from origin seeing walls at x=-2, x=2, y=-2. - fn make_partial_room_scan() -> PointCloud2D { - let mut xs = Vec::new(); - let mut ys = Vec::new(); - - // Left wall (x = -2) - for i in -10..=10 { - let y = i as f32 * 0.2; - xs.push(-2.0); - ys.push(y); - } - // Right wall (x = 2) - for i in -10..=10 { - let y = i as f32 * 0.2; - xs.push(2.0); - ys.push(y); - } - // Bottom wall (y = -2) - for i in -10..=10 { - let x = i as f32 * 0.2; - xs.push(x); - ys.push(-2.0); - } - // Top wall is MISSING - this is where frontier should be detected - - PointCloud2D { xs, ys } - } - - #[test] - fn test_frontiers_partial_room() { - let mut map = VectorMap::default(); - - // Use observe() to properly build lines AND CBVG nodes - let scan = make_partial_room_scan(); - map.observe(&scan, Pose2D::identity()); - - // With 3 walls visible and top open, we should have frontiers - let frontiers = map.frontiers(); - - // CBVG nodes should exist after observation - assert!( - !map.cbvg.nodes().is_empty(), - "CBVG should have nodes after observation" - ); - - // Frontiers should be detected in the open direction (toward +y) - assert!( - !frontiers.is_empty(), - "Partial room with open top should have frontiers" - ); - } - - #[test] - fn test_localization_only_mode() { - let config = VectorMapConfig::default().with_mapping_enabled(false); - let mut map = VectorMap::new(config); - - map.add_line(Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(10.0, 0.0))); - let initial_count = map.lines().len(); - - let scan = make_simple_scan(); - map.observe(&scan, Pose2D::identity()); - - assert_eq!(map.lines().len(), initial_count); - } - - #[test] - fn test_features() { - let mut map = VectorMap::default(); - map.add_lines(&[ - Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(5.0, 0.0)), - Line2D::new(Point2D::new(5.0, 0.0), Point2D::new(5.0, 5.0)), - ]); - - let features = map.features(); - - assert_eq!(features.lines().len(), 2); - } - - #[test] - fn test_multi_scan_mapping_grows_map() { - let mut map = VectorMap::default(); - - let mut scan1 = PointCloud2D { - xs: Vec::new(), - ys: Vec::new(), - }; - for i in 0..20 { - scan1.xs.push(-2.0); - scan1.ys.push(-2.0 + i as f32 * 0.2); - } - for i in 0..20 { - scan1.xs.push(2.0); - scan1.ys.push(-2.0 + i as f32 * 0.2); - } - - let _ = map.observe(&scan1, Pose2D::identity()); - assert_eq!(map.observation_count(), 1); - - let odometry = Pose2D::new(0.3, 0.2, 0.05); - let _ = map.observe(&scan1, odometry); - - assert_eq!(map.observation_count(), 2); - } - - #[test] - fn test_slam_cycle_extract_match_integrate() { - let mut map = VectorMap::default(); - - map.add_lines(&[ - Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(5.0, 0.0)), - Line2D::new(Point2D::new(5.0, 0.0), Point2D::new(5.0, 5.0)), - Line2D::new(Point2D::new(5.0, 5.0), Point2D::new(0.0, 5.0)), - Line2D::new(Point2D::new(0.0, 5.0), Point2D::new(0.0, 0.0)), - ]); - - let initial_lines = map.lines().len(); - - let mut scan = PointCloud2D { - xs: Vec::new(), - ys: Vec::new(), - }; - for i in 0..10 { - scan.xs.push(0.5 + i as f32 * 0.4); - scan.ys.push(0.05); - } - for i in 0..10 { - scan.xs.push(4.95); - scan.ys.push(0.5 + i as f32 * 0.4); - } - - map.set_pose(Pose2D::new(2.5, 2.5, 0.0)); - let _ = map.observe(&scan, Pose2D::identity()); - - assert!( - map.lines().len() <= initial_lines + 5, - "Map should not grow excessively: had {}, now {}", - initial_lines, - map.lines().len() - ); - } - - #[test] - fn test_localization_maintains_pose() { - let config = VectorMapConfig::default().with_min_match_confidence(0.1); - let mut map = VectorMap::new(config); - - map.add_lines(&[ - Line2D::new(Point2D::new(-2.0, -2.0), Point2D::new(2.0, -2.0)), - Line2D::new(Point2D::new(2.0, -2.0), Point2D::new(2.0, 2.0)), - Line2D::new(Point2D::new(2.0, 2.0), Point2D::new(-2.0, 2.0)), - Line2D::new(Point2D::new(-2.0, 2.0), Point2D::new(-2.0, -2.0)), - ]); - - map.set_pose(Pose2D::identity()); - - let scan = make_room_scan(); - let mut total_movement = 0.0; - - for _ in 0..5 { - let odom = Pose2D::new(0.1, 0.05, 0.02); - map.observe(&scan, odom); - total_movement += (0.1f32.powi(2) + 0.05f32.powi(2)).sqrt(); - } - - let pose = map.current_pose(); - let pose_dist = (pose.x.powi(2) + pose.y.powi(2)).sqrt(); - - assert!( - pose_dist < total_movement * 2.0, - "Pose drift too large: {} vs max {}", - pose_dist, - total_movement * 2.0 - ); - } - - #[test] - fn test_map_bounds_grow_with_exploration() { - let mut map = VectorMap::default(); - - map.add_line(Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(1.0, 0.0))); - let initial_bounds = map.bounds(); - - map.add_line(Line2D::new(Point2D::new(1.0, 0.0), Point2D::new(5.0, 0.0))); - map.add_line(Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(0.0, 3.0))); - - let expanded_bounds = map.bounds(); - - assert!( - expanded_bounds.width() > initial_bounds.width(), - "Bounds width should grow" - ); - assert!( - expanded_bounds.height() > initial_bounds.height(), - "Bounds height should grow" - ); - } - - #[test] - fn test_feature_count_stability() { - let mut map = VectorMap::default(); - let scan = make_room_scan(); - - map.observe(&scan, Pose2D::identity()); - let count_after_1 = map.lines().len(); - - for _ in 0..5 { - let small_move = Pose2D::new(0.01, 0.01, 0.005); - map.observe(&scan, small_move); - } - - let count_after_6 = map.lines().len(); - - assert!( - count_after_6 <= count_after_1 * 3, - "Feature count should stabilize: started with {}, ended with {}", - count_after_1, - count_after_6 - ); - } - - #[test] - fn test_config_propagation() { - let config = VectorMapConfig::default() - .with_min_match_confidence(0.5) - .with_mapping_enabled(false); - - let map = VectorMap::new(config); - - assert_eq!(map.config().min_match_confidence, 0.5); - assert!(!map.config().mapping_enabled); - } - - #[test] - fn test_set_pose() { - let mut map = VectorMap::default(); - - let target_pose = Pose2D::new(1.5, 2.5, 0.7); - map.set_pose(target_pose); - - assert_eq!(map.current_pose().x, target_pose.x); - assert_eq!(map.current_pose().y, target_pose.y); - assert_eq!(map.current_pose().theta, target_pose.theta); - } - - #[test] - fn test_observation_count_increments() { - let mut map = VectorMap::default(); - let scan = make_simple_scan(); - - assert_eq!(map.observation_count(), 0); - - map.observe(&scan, Pose2D::identity()); - assert_eq!(map.observation_count(), 1); - - map.observe(&scan, Pose2D::identity()); - assert_eq!(map.observation_count(), 2); - - map.observe(&scan, Pose2D::identity()); - assert_eq!(map.observation_count(), 3); - } - - #[test] - fn test_add_lines_batch() { - let mut map = VectorMap::default(); - - let lines = vec![ - Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(1.0, 0.0)), - Line2D::new(Point2D::new(1.0, 0.0), Point2D::new(1.0, 1.0)), - Line2D::new(Point2D::new(1.0, 1.0), Point2D::new(0.0, 1.0)), - ]; - - map.add_lines(&lines); - - assert_eq!(map.lines().len(), 3); - assert!(map.bounds().contains(Point2D::new(0.5, 0.5))); - } - - #[test] - fn test_empty_scan_handling() { - let mut map = VectorMap::default(); - map.add_line(Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(5.0, 0.0))); - - let empty_scan = PointCloud2D { - xs: vec![], - ys: vec![], - }; - let result = map.observe(&empty_scan, Pose2D::identity()); - - assert_eq!(result.features_extracted, 0); - assert_eq!(map.observation_count(), 1); - } -} diff --git a/vastu-map/src/vector_map/mod.rs b/vastu-map/src/vector_map/mod.rs deleted file mode 100644 index d124b08..0000000 --- a/vastu-map/src/vector_map/mod.rs +++ /dev/null @@ -1,39 +0,0 @@ -//! VectorMap: The main SLAM map implementation. -//! -//! VectorMap uses line and corner features as the primary map representation, -//! providing efficient storage and queries for indoor environments. -//! -//! # Components -//! -//! - [`VectorMap`]: The main SLAM map struct implementing the [`Map`](crate::Map) trait -//! - [`VectorMapConfig`]: Configuration for all SLAM components -//! - [`LineStore`]: Internal line storage with synchronized auxiliary structures -//! -//! # Example -//! -//! ```rust,ignore -//! use vastu_map::{VectorMap, VectorMapConfig, Map}; -//! use vastu_map::core::{Pose2D, PointCloud2D, Point2D}; -//! -//! let config = VectorMapConfig::default(); -//! let mut map = VectorMap::new(config); -//! -//! // Process scans -//! let scan = PointCloud2D::from_points(&[ -//! Point2D::new(1.0, 0.0), -//! Point2D::new(1.0, 0.1), -//! ]); -//! let odometry = Pose2D::identity(); -//! let result = map.observe(&scan, odometry); -//! ``` - -mod config; -mod line_store; -mod map; -mod yaml; - -// Re-export public API -pub use config::{ConfigError, VectorMapConfig}; -pub use line_store::LineStore; -pub use map::VectorMap; -pub use yaml::YamlConfigError; diff --git a/vastu-map/src/vector_map/yaml.rs b/vastu-map/src/vector_map/yaml.rs deleted file mode 100644 index e60c6ac..0000000 --- a/vastu-map/src/vector_map/yaml.rs +++ /dev/null @@ -1,461 +0,0 @@ -//! YAML configuration file loading for VectorMap. -//! -//! Load algorithm configuration from YAML files for reproducible experiments -//! and easy parameter tuning. -//! -//! # Example -//! -//! ```rust,ignore -//! use vastu_map::VectorMapConfig; -//! -//! // Load from file -//! let config = VectorMapConfig::from_yaml_file("config.yaml")?; -//! -//! // Validate before use -//! config.validate()?; -//! -//! let map = VectorMap::new(config); -//! ``` - -use std::io; -use std::path::Path; - -use crate::vector_map::config::{ConfigError, VectorMapConfig}; - -/// Errors that can occur when loading YAML configuration. -#[derive(Debug)] -pub enum YamlConfigError { - /// IO error reading the file. - Io(io::Error), - /// YAML parsing error. - Parse(serde_yaml::Error), - /// Configuration validation failed. - Validation(ConfigError), -} - -impl std::fmt::Display for YamlConfigError { - fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result { - match self { - YamlConfigError::Io(e) => write!(f, "IO error: {}", e), - YamlConfigError::Parse(e) => write!(f, "YAML parse error: {}", e), - YamlConfigError::Validation(e) => write!(f, "Config validation error: {}", e), - } - } -} - -impl std::error::Error for YamlConfigError { - fn source(&self) -> Option<&(dyn std::error::Error + 'static)> { - match self { - YamlConfigError::Io(e) => Some(e), - YamlConfigError::Parse(e) => Some(e), - YamlConfigError::Validation(e) => Some(e), - } - } -} - -impl From for YamlConfigError { - fn from(err: io::Error) -> Self { - YamlConfigError::Io(err) - } -} - -impl From for YamlConfigError { - fn from(err: serde_yaml::Error) -> Self { - YamlConfigError::Parse(err) - } -} - -impl From for YamlConfigError { - fn from(err: ConfigError) -> Self { - YamlConfigError::Validation(err) - } -} - -impl VectorMapConfig { - /// Load configuration from a YAML file. - /// - /// Parses the file and validates the configuration. - /// - /// # Arguments - /// * `path` - Path to the YAML configuration file - /// - /// # Errors - /// Returns `YamlConfigError` if the file cannot be read, parsed, or validated. - /// - /// # Example - /// ```rust,ignore - /// let config = VectorMapConfig::from_yaml_file("config.yaml")?; - /// let map = VectorMap::new(config); - /// ``` - pub fn from_yaml_file>(path: P) -> Result { - let contents = std::fs::read_to_string(path)?; - Self::from_yaml_str(&contents) - } - - /// Load configuration from a YAML string. - /// - /// Parses the string and validates the configuration. - /// - /// # Example - /// ```rust,ignore - /// let yaml = r#" - /// extraction: - /// split_threshold: 0.05 - /// "#; - /// let config = VectorMapConfig::from_yaml_str(yaml)?; - /// ``` - pub fn from_yaml_str(yaml: &str) -> Result { - let mut config: VectorMapConfig = serde_yaml::from_str(yaml)?; - - // Finalize derived fields (like convergence_threshold_sq) - config.finalize(); - - // Validate configuration - config.validate()?; - - Ok(config) - } - - /// Load configuration from a YAML file without validation. - /// - /// Use this if you want to manually validate or modify the config after loading. - /// - /// # Example - /// ```rust,ignore - /// let mut config = VectorMapConfig::from_yaml_file_unchecked("config.yaml")?; - /// config.matching.max_iterations = 50; // Modify - /// config.validate()?; // Manually validate - /// ``` - pub fn from_yaml_file_unchecked>(path: P) -> Result { - let contents = std::fs::read_to_string(path)?; - Self::from_yaml_str_unchecked(&contents) - } - - /// Load configuration from a YAML string without validation. - pub fn from_yaml_str_unchecked(yaml: &str) -> Result { - let mut config: VectorMapConfig = serde_yaml::from_str(yaml)?; - config.finalize(); - Ok(config) - } - - /// Serialize configuration to a YAML string. - /// - /// Useful for saving configurations or generating templates. - /// - /// # Example - /// ```rust,ignore - /// let config = VectorMapConfig::default(); - /// let yaml = config.to_yaml_string()?; - /// println!("{}", yaml); - /// ``` - pub fn to_yaml_string(&self) -> Result { - serde_yaml::to_string(self) - } - - /// Save configuration to a YAML file. - /// - /// # Example - /// ```rust,ignore - /// let config = VectorMapConfig::default(); - /// config.to_yaml_file("reference.yaml")?; - /// ``` - pub fn to_yaml_file>(&self, path: P) -> Result<(), YamlConfigError> { - let yaml = self.to_yaml_string()?; - std::fs::write(path, yaml)?; - Ok(()) - } -} - -#[cfg(test)] -mod tests { - use super::*; - - #[test] - fn test_roundtrip_default_config() { - let original = VectorMapConfig::default(); - let yaml = original.to_yaml_string().expect("Failed to serialize"); - let loaded = VectorMapConfig::from_yaml_str(&yaml).expect("Failed to deserialize"); - - // Key fields should match - assert_eq!( - original.matching.max_iterations, - loaded.matching.max_iterations - ); - assert_eq!(original.use_hybrid_extraction, loaded.use_hybrid_extraction); - assert_eq!(original.mapping_enabled, loaded.mapping_enabled); - } - - #[test] - fn test_validation_called_on_load() { - // Create invalid YAML with zero iterations - let yaml = r#" -extraction: - split_threshold: 0.05 - min_points: 5 - min_length: 0.1 - max_point_gap: 0.3 - merge_threshold: 0.05 - adaptive_range_scale: 0.03 -ransac_extraction: - iterations: 0 - inlier_threshold: 0.02 - min_inliers: 5 - min_line_length: 0.1 - max_point_gap: 0.1 - max_lines: 0 - seed: 0 -use_hybrid_extraction: true -corner: - min_angle: 0.524 - max_angle: 2.618 - max_endpoint_distance: 0.05 - curvature_window_size: 3 - curvature_threshold: 0.785 - merge_distance: 0.1 - nms_radius: 0.15 -matching: - max_iterations: 30 - convergence_threshold: 0.0001 - max_correspondence_distance: 0.5 - robust_cost: - type: Huber - delta: 0.03 - use_irls: true - min_correspondences: 10 - min_confidence: 0.3 - use_ransac_init: true - use_batch_search: true - noise_model: - sigma_base: 0.01 - sigma_range: 0.001 - use_coarse_search: true - coarse_search_confidence_threshold: 0.5 - multi_resolution: - enabled: true - levels: 3 - subsample_factor: 2 - iterations_per_level: [3, 5, 10] -association: - max_perpendicular_distance: 0.15 - max_angle_difference: 0.2 - min_overlap_ratio: 0.3 - max_endpoint_gap: 0.3 - distance_weight: 10.0 - angle_weight: 5.0 -merger: - map_weight: 0.8 - extend_endpoints: true - max_extension: 0.5 - min_extension: 0.05 -frontier: - connection_distance: 0.3 - min_distance_from_robot: 0.5 - safety_margin: 0.4 - gap_probe_distance: 2.0 - robot_radius: 0.15 - max_viewpoint_iterations: 5 - viewpoint_step: 0.1 - min_gap_width: 0.4 - max_gap_width: 10.0 - min_frontier_angle: 1.75 - max_wall_distance: 0.5 -occupancy: - obstacle_distance: 0.1 - known_distance: 2.0 -loop_closure: - min_travel_distance: 5.0 - max_descriptor_distance: 0.5 - min_verification_confidence: 0.6 - keyframe_interval: 2.0 - descriptor_radius: 2.0 - max_candidates_to_verify: 3 - min_lines_for_keyframe: 3 - min_corners_for_keyframe: 2 - icp_config: - max_iterations: 30 - convergence_threshold: 0.0001 - max_correspondence_distance: 0.5 - robust_cost: - type: Huber - delta: 0.03 - use_irls: true - min_correspondences: 10 - min_confidence: 0.3 - use_ransac_init: true - use_batch_search: true - noise_model: - sigma_base: 0.01 - sigma_range: 0.001 - use_coarse_search: true - coarse_search_confidence_threshold: 0.5 - multi_resolution: - enabled: true - levels: 3 - subsample_factor: 2 - iterations_per_level: [3, 5, 10] -min_match_confidence: 0.3 -mapping_enabled: true -loop_closure_enabled: true -"#; - - let result = VectorMapConfig::from_yaml_str(yaml); - assert!(result.is_err()); - assert!(matches!( - result.unwrap_err(), - YamlConfigError::Validation(_) - )); - } - - #[test] - fn test_unchecked_load_skips_validation() { - // Same invalid YAML but using unchecked method - let yaml = r#" -extraction: - split_threshold: 0.05 - min_points: 5 - min_length: 0.1 - max_point_gap: 0.3 - merge_threshold: 0.05 - adaptive_range_scale: 0.03 -ransac_extraction: - iterations: 0 - inlier_threshold: 0.02 - min_inliers: 5 - min_line_length: 0.1 - max_point_gap: 0.1 - max_lines: 0 - seed: 0 -use_hybrid_extraction: true -corner: - min_angle: 0.524 - max_angle: 2.618 - max_endpoint_distance: 0.05 - curvature_window_size: 3 - curvature_threshold: 0.785 - merge_distance: 0.1 - nms_radius: 0.15 -matching: - max_iterations: 30 - convergence_threshold: 0.0001 - max_correspondence_distance: 0.5 - robust_cost: - type: Huber - delta: 0.03 - use_irls: true - min_correspondences: 10 - min_confidence: 0.3 - use_ransac_init: true - use_batch_search: true - noise_model: - sigma_base: 0.01 - sigma_range: 0.001 - use_coarse_search: true - coarse_search_confidence_threshold: 0.5 - multi_resolution: - enabled: true - levels: 3 - subsample_factor: 2 - iterations_per_level: [3, 5, 10] -association: - max_perpendicular_distance: 0.15 - max_angle_difference: 0.2 - min_overlap_ratio: 0.3 - max_endpoint_gap: 0.3 - distance_weight: 10.0 - angle_weight: 5.0 -merger: - map_weight: 0.8 - extend_endpoints: true - max_extension: 0.5 - min_extension: 0.05 -frontier: - connection_distance: 0.3 - min_distance_from_robot: 0.5 - safety_margin: 0.4 - gap_probe_distance: 2.0 - robot_radius: 0.15 - max_viewpoint_iterations: 5 - viewpoint_step: 0.1 - min_gap_width: 0.4 - max_gap_width: 10.0 - min_frontier_angle: 1.75 - max_wall_distance: 0.5 -occupancy: - obstacle_distance: 0.1 - known_distance: 2.0 -loop_closure: - min_travel_distance: 5.0 - max_descriptor_distance: 0.5 - min_verification_confidence: 0.6 - keyframe_interval: 2.0 - descriptor_radius: 2.0 - max_candidates_to_verify: 3 - min_lines_for_keyframe: 3 - min_corners_for_keyframe: 2 - icp_config: - max_iterations: 30 - convergence_threshold: 0.0001 - max_correspondence_distance: 0.5 - robust_cost: - type: Huber - delta: 0.03 - use_irls: true - min_correspondences: 10 - min_confidence: 0.3 - use_ransac_init: true - use_batch_search: true - noise_model: - sigma_base: 0.01 - sigma_range: 0.001 - use_coarse_search: true - coarse_search_confidence_threshold: 0.5 - multi_resolution: - enabled: true - levels: 3 - subsample_factor: 2 - iterations_per_level: [3, 5, 10] -min_match_confidence: 0.3 -mapping_enabled: true -loop_closure_enabled: true -"#; - - // Should succeed with unchecked method - let result = VectorMapConfig::from_yaml_str_unchecked(yaml); - assert!(result.is_ok()); - - // But validation should fail - let config = result.unwrap(); - assert!(config.validate().is_err()); - } - - #[test] - fn test_empty_yaml_fails() { - let result = VectorMapConfig::from_yaml_str(""); - assert!(result.is_err()); - } - - #[test] - fn test_io_error_conversion() { - let io_err = io::Error::new(io::ErrorKind::NotFound, "file not found"); - let yaml_err: YamlConfigError = io_err.into(); - assert!(matches!(yaml_err, YamlConfigError::Io(_))); - } - - #[test] - fn test_load_default_yaml_file() { - // Load the reference config file from configs/default.yaml - let config_path = std::path::Path::new(env!("CARGO_MANIFEST_DIR")) - .join("configs") - .join("default.yaml"); - - let config = VectorMapConfig::from_yaml_file(&config_path) - .expect("Failed to load configs/default.yaml"); - - // Verify key values match expected defaults - assert_eq!(config.matching.max_iterations, 30); - assert!(config.use_hybrid_extraction); - assert!(config.mapping_enabled); - assert!(config.loop_closure_enabled); - assert!(config.validate().is_ok()); - } -} diff --git a/vastu-map/tools/.gitignore b/vastu-map/tools/.gitignore deleted file mode 100644 index f9544ad..0000000 --- a/vastu-map/tools/.gitignore +++ /dev/null @@ -1,2 +0,0 @@ -dataset/ -maps/ \ No newline at end of file diff --git a/vastu-map/tools/floorplan_converter.py b/vastu-map/tools/floorplan_converter.py deleted file mode 100644 index 6035841..0000000 --- a/vastu-map/tools/floorplan_converter.py +++ /dev/null @@ -1,503 +0,0 @@ -#!/usr/bin/env python3 -""" -Floor Plan to PGM Converter - -Converts floor plan images (PNG, JPG, SVG, PDF) to ROS-compatible PGM+YAML maps -for use with vastu-map SLAM testing. - -Usage: - python floorplan_converter.py input.png --width 10.0 --output apartment.pgm - python floorplan_converter.py input.jpg --resolution 0.05 --output test_map.pgm - python floorplan_converter.py input.png --width 15.0 --add-furniture --output furnished.pgm - -Dependencies: - pip install opencv-python numpy pillow pyyaml - -Author: VacuumTiger Project -""" - -import argparse -import os -import sys -from pathlib import Path -from typing import Tuple, Optional, List -import yaml - -try: - import cv2 - import numpy as np - from PIL import Image -except ImportError as e: - print(f"Missing dependency: {e}") - print("Install with: pip install opencv-python numpy pillow pyyaml") - sys.exit(1) - - -class FloorPlanConverter: - """Convert floor plan images to ROS-compatible PGM+YAML format.""" - - def __init__( - self, - resolution: float = 0.05, - wall_thickness: int = 2, - occupied_thresh: float = 0.65, - free_thresh: float = 0.196, - invert_colors: bool = False, - ): - """ - Initialize converter. - - Args: - resolution: Meters per pixel (default 0.05m = 5cm) - wall_thickness: Minimum wall thickness in pixels after processing - occupied_thresh: Threshold for occupied space (0-1, normalized) - free_thresh: Threshold for free space (0-1, normalized) - invert_colors: If True, treat dark areas as free space - """ - self.resolution = resolution - self.wall_thickness = wall_thickness - self.occupied_thresh = occupied_thresh - self.free_thresh = free_thresh - self.invert_colors = invert_colors - - def load_image(self, path: str) -> np.ndarray: - """Load image from various formats.""" - path = Path(path) - - if not path.exists(): - raise FileNotFoundError(f"Image not found: {path}") - - # Handle different formats - suffix = path.suffix.lower() - - if suffix in ['.png', '.jpg', '.jpeg', '.bmp', '.tiff', '.tif']: - img = cv2.imread(str(path), cv2.IMREAD_GRAYSCALE) - if img is None: - raise ValueError(f"Failed to load image: {path}") - return img - - elif suffix == '.svg': - # SVG requires cairosvg or similar - try: - import cairosvg - from io import BytesIO - png_data = cairosvg.svg2png(url=str(path)) - pil_img = Image.open(BytesIO(png_data)).convert('L') - return np.array(pil_img) - except ImportError: - raise ImportError("SVG support requires: pip install cairosvg") - - elif suffix == '.pdf': - # PDF requires pdf2image - try: - from pdf2image import convert_from_path - images = convert_from_path(str(path), dpi=300) - if images: - return np.array(images[0].convert('L')) - raise ValueError("No pages in PDF") - except ImportError: - raise ImportError("PDF support requires: pip install pdf2image") - - else: - raise ValueError(f"Unsupported format: {suffix}") - - def preprocess(self, img: np.ndarray) -> np.ndarray: - """Apply preprocessing to enhance wall detection.""" - # Apply Gaussian blur to reduce noise - blurred = cv2.GaussianBlur(img, (5, 5), 0) - - # Apply adaptive thresholding for better wall detection - # This works well with varying lighting in floor plans - binary = cv2.adaptiveThreshold( - blurred, - 255, - cv2.ADAPTIVE_THRESH_GAUSSIAN_C, - cv2.THRESH_BINARY, - blockSize=11, - C=2 - ) - - return binary - - def detect_walls_otsu(self, img: np.ndarray) -> np.ndarray: - """Detect walls using Otsu's thresholding.""" - blurred = cv2.GaussianBlur(img, (5, 5), 0) - _, binary = cv2.threshold(blurred, 0, 255, cv2.THRESH_BINARY + cv2.THRESH_OTSU) - return binary - - def detect_walls_adaptive(self, img: np.ndarray) -> np.ndarray: - """Detect walls using adaptive thresholding.""" - blurred = cv2.GaussianBlur(img, (3, 3), 0) - binary = cv2.adaptiveThreshold( - blurred, 255, - cv2.ADAPTIVE_THRESH_GAUSSIAN_C, - cv2.THRESH_BINARY, - blockSize=15, - C=3 - ) - return binary - - def detect_walls_canny(self, img: np.ndarray) -> np.ndarray: - """Detect walls using Canny edge detection + morphology.""" - # Edge detection - edges = cv2.Canny(img, 50, 150) - - # Dilate to connect nearby edges - kernel = np.ones((3, 3), np.uint8) - dilated = cv2.dilate(edges, kernel, iterations=2) - - # Fill holes - filled = cv2.morphologyEx(dilated, cv2.MORPH_CLOSE, kernel, iterations=3) - - # Invert so walls are black (0) - return 255 - filled - - def clean_walls(self, binary: np.ndarray) -> np.ndarray: - """Clean up detected walls with morphological operations.""" - kernel = np.ones((self.wall_thickness, self.wall_thickness), np.uint8) - - # Remove small noise - opened = cv2.morphologyEx(binary, cv2.MORPH_OPEN, kernel) - - # Fill small gaps in walls - closed = cv2.morphologyEx(opened, cv2.MORPH_CLOSE, kernel) - - return closed - - def scale_to_resolution( - self, - img: np.ndarray, - target_width_m: Optional[float] = None, - target_height_m: Optional[float] = None, - ) -> np.ndarray: - """ - Scale image to match desired physical dimensions. - - Args: - img: Input image - target_width_m: Target width in meters (None to use height ratio) - target_height_m: Target height in meters (None to use width ratio) - - Returns: - Scaled image where each pixel = self.resolution meters - """ - h, w = img.shape[:2] - - if target_width_m is not None: - target_width_px = int(target_width_m / self.resolution) - scale = target_width_px / w - target_height_px = int(h * scale) - elif target_height_m is not None: - target_height_px = int(target_height_m / self.resolution) - scale = target_height_px / h - target_width_px = int(w * scale) - else: - # Keep original size, assume source resolution matches - return img - - return cv2.resize(img, (target_width_px, target_height_px), interpolation=cv2.INTER_NEAREST) - - def add_border(self, img: np.ndarray, border_px: int = 4) -> np.ndarray: - """Add solid wall border around the map.""" - # 0 = occupied (wall), 255 = free - bordered = cv2.copyMakeBorder( - img, - border_px, border_px, border_px, border_px, - cv2.BORDER_CONSTANT, - value=0 - ) - return bordered - - def add_furniture( - self, - img: np.ndarray, - furniture_density: float = 0.1, - seed: Optional[int] = None - ) -> np.ndarray: - """ - Add random furniture obstacles to the map. - - Args: - img: Input map (255=free, 0=occupied) - furniture_density: Fraction of free space to fill with furniture - seed: Random seed for reproducibility - """ - if seed is not None: - np.random.seed(seed) - - result = img.copy() - h, w = result.shape - - # Find free space pixels - free_mask = result == 255 - free_pixels = np.sum(free_mask) - - # Common furniture pieces (width, height in pixels at 5cm resolution) - furniture_templates = [ - ('table', (16, 20)), # 80cm x 100cm dining table - ('sofa', (40, 16)), # 200cm x 80cm sofa - ('bed', (36, 40)), # 180cm x 200cm bed - ('desk', (24, 12)), # 120cm x 60cm desk - ('chair', (8, 8)), # 40cm x 40cm chair - ('wardrobe', (24, 12)), # 120cm x 60cm wardrobe - ('nightstand', (8, 8)), # 40cm x 40cm nightstand - ('bookshelf', (16, 6)), # 80cm x 30cm bookshelf - ] - - target_furniture_pixels = int(free_pixels * furniture_density) - placed_pixels = 0 - attempts = 0 - max_attempts = 1000 - - while placed_pixels < target_furniture_pixels and attempts < max_attempts: - attempts += 1 - - # Pick random furniture - name, (fw, fh) = furniture_templates[np.random.randint(len(furniture_templates))] - - # Random rotation (0 or 90 degrees) - if np.random.random() > 0.5: - fw, fh = fh, fw - - # Random position - x = np.random.randint(0, max(1, w - fw)) - y = np.random.randint(0, max(1, h - fh)) - - # Check if area is free (with margin) - margin = 2 - x1, y1 = max(0, x - margin), max(0, y - margin) - x2, y2 = min(w, x + fw + margin), min(h, y + fh + margin) - - region = result[y1:y2, x1:x2] - if np.all(region == 255): - # Place furniture (walls = 0) - result[y:y+fh, x:x+fw] = 0 - placed_pixels += fw * fh - - return result - - def convert( - self, - input_path: str, - width_m: Optional[float] = None, - height_m: Optional[float] = None, - detection_method: str = 'adaptive', - add_border: bool = True, - add_furniture: bool = False, - furniture_density: float = 0.05, - furniture_seed: Optional[int] = None, - ) -> Tuple[np.ndarray, dict]: - """ - Convert floor plan image to occupancy map. - - Args: - input_path: Path to input image - width_m: Physical width in meters - height_m: Physical height in meters - detection_method: 'adaptive', 'otsu', or 'canny' - add_border: Add wall border around map - add_furniture: Add random furniture obstacles - furniture_density: Density of furniture (0-1) - furniture_seed: Random seed for furniture placement - - Returns: - Tuple of (pgm_array, yaml_metadata) - """ - # Load image - img = self.load_image(input_path) - - # Invert if needed (some floor plans have white walls) - if self.invert_colors: - img = 255 - img - - # Detect walls - if detection_method == 'adaptive': - binary = self.detect_walls_adaptive(img) - elif detection_method == 'otsu': - binary = self.detect_walls_otsu(img) - elif detection_method == 'canny': - binary = self.detect_walls_canny(img) - else: - raise ValueError(f"Unknown detection method: {detection_method}") - - # Clean up - cleaned = self.clean_walls(binary) - - # Scale to physical dimensions - scaled = self.scale_to_resolution(cleaned, width_m, height_m) - - # Add border - if add_border: - scaled = self.add_border(scaled, border_px=2) - - # Add furniture - if add_furniture: - scaled = self.add_furniture(scaled, furniture_density, furniture_seed) - - # Calculate physical dimensions - h, w = scaled.shape - physical_width = w * self.resolution - physical_height = h * self.resolution - - # Create YAML metadata - metadata = { - 'resolution': self.resolution, - 'origin': [0.0, 0.0, 0.0], - 'negate': 0, - 'occupied_thresh': self.occupied_thresh, - 'free_thresh': self.free_thresh, - 'physical_size': { - 'width_m': physical_width, - 'height_m': physical_height, - }, - } - - return scaled, metadata - - def save_pgm(self, img: np.ndarray, output_path: str, comment: str = ""): - """Save image as ASCII PGM (P2 format).""" - h, w = img.shape - - with open(output_path, 'w') as f: - f.write("P2\n") - if comment: - # Split comment into multiple lines if needed - for line in comment.split('\n'): - f.write(f"# {line}\n") - f.write(f"{w} {h}\n") - f.write("255\n") - - for row in img: - f.write(' '.join(map(str, row)) + '\n') - - def save_yaml(self, metadata: dict, output_path: str, pgm_filename: str): - """Save YAML metadata file.""" - # Create ROS-compatible YAML - yaml_data = { - 'image': pgm_filename, - 'resolution': metadata['resolution'], - 'origin': metadata['origin'], - 'negate': metadata['negate'], - 'occupied_thresh': metadata['occupied_thresh'], - 'free_thresh': metadata['free_thresh'], - } - - with open(output_path, 'w') as f: - # Add comment header - f.write(f"# Map: {pgm_filename}\n") - f.write(f"# Physical size: {metadata['physical_size']['width_m']:.1f}m x {metadata['physical_size']['height_m']:.1f}m\n") - yaml.dump(yaml_data, f, default_flow_style=False, sort_keys=False) - - -def main(): - parser = argparse.ArgumentParser( - description='Convert floor plan images to ROS-compatible PGM+YAML maps', - formatter_class=argparse.RawDescriptionHelpFormatter, - epilog=""" -Examples: - # Convert with specific physical width (height calculated proportionally) - python floorplan_converter.py apartment.png --width 12.0 --output apartment.pgm - - # Convert with both dimensions specified - python floorplan_converter.py house.jpg --width 15.0 --height 10.0 --output house.pgm - - # Add furniture obstacles for exploration testing - python floorplan_converter.py layout.png --width 8.0 --add-furniture --output furnished.pgm - - # Use Canny edge detection for blueprints with thin lines - python floorplan_converter.py blueprint.png --width 20.0 --method canny --output blueprint.pgm - - # Invert colors (for floor plans with white walls) - python floorplan_converter.py inverted.png --width 10.0 --invert --output map.pgm - """ - ) - - parser.add_argument('input', help='Input floor plan image (PNG, JPG, SVG, PDF)') - parser.add_argument('--output', '-o', required=True, help='Output PGM file path') - parser.add_argument('--width', '-w', type=float, help='Physical width in meters') - parser.add_argument('--height', '-H', type=float, help='Physical height in meters') - parser.add_argument('--resolution', '-r', type=float, default=0.05, - help='Meters per pixel (default: 0.05)') - parser.add_argument('--method', '-m', choices=['adaptive', 'otsu', 'canny'], - default='adaptive', help='Wall detection method (default: adaptive)') - parser.add_argument('--invert', '-i', action='store_true', - help='Invert colors (treat dark as free space)') - parser.add_argument('--no-border', action='store_true', - help='Do not add wall border around map') - parser.add_argument('--add-furniture', '-f', action='store_true', - help='Add random furniture obstacles') - parser.add_argument('--furniture-density', type=float, default=0.05, - help='Furniture density 0-1 (default: 0.05)') - parser.add_argument('--furniture-seed', type=int, - help='Random seed for furniture placement') - parser.add_argument('--wall-thickness', type=int, default=2, - help='Minimum wall thickness in pixels (default: 2)') - parser.add_argument('--preview', '-p', action='store_true', - help='Show preview window (requires display)') - - args = parser.parse_args() - - # Validate input - if args.width is None and args.height is None: - parser.error("At least one of --width or --height must be specified") - - # Create converter - converter = FloorPlanConverter( - resolution=args.resolution, - wall_thickness=args.wall_thickness, - invert_colors=args.invert, - ) - - print(f"Converting: {args.input}") - print(f"Resolution: {args.resolution}m/pixel") - - # Convert - try: - pgm, metadata = converter.convert( - args.input, - width_m=args.width, - height_m=args.height, - detection_method=args.method, - add_border=not args.no_border, - add_furniture=args.add_furniture, - furniture_density=args.furniture_density, - furniture_seed=args.furniture_seed, - ) - except Exception as e: - print(f"Error: {e}") - sys.exit(1) - - # Generate output paths - output_pgm = Path(args.output) - output_yaml = output_pgm.with_suffix('.yaml') - - # Create comment for PGM - comment = f"Converted from: {Path(args.input).name}\n" - comment += f"Size: {metadata['physical_size']['width_m']:.1f}m x {metadata['physical_size']['height_m']:.1f}m" - if args.add_furniture: - comment += f"\nFurniture density: {args.furniture_density}" - - # Save files - converter.save_pgm(pgm, str(output_pgm), comment) - converter.save_yaml(metadata, str(output_yaml), output_pgm.name) - - h, w = pgm.shape - print(f"Output: {output_pgm}") - print(f" {output_yaml}") - print(f"Size: {w}x{h} pixels ({metadata['physical_size']['width_m']:.1f}m x {metadata['physical_size']['height_m']:.1f}m)") - - # Preview if requested - if args.preview: - try: - cv2.imshow('Converted Map', pgm) - print("Press any key to close preview...") - cv2.waitKey(0) - cv2.destroyAllWindows() - except Exception as e: - print(f"Preview not available: {e}") - - print("Done!") - - -if __name__ == '__main__': - main() diff --git a/vastu-map/tools/houseexpo_converter.py b/vastu-map/tools/houseexpo_converter.py deleted file mode 100644 index 3c6c161..0000000 --- a/vastu-map/tools/houseexpo_converter.py +++ /dev/null @@ -1,390 +0,0 @@ -#!/usr/bin/env python3 -""" -HouseExpo Dataset Converter - -Converts HouseExpo floor plan images to ROS-compatible PGM+YAML maps. - -HouseExpo format: -- Binary grayscale images (0=wall, 255=free) -- Resolution: 1 pixel = 1 cm -- ~35,000 floor plans from real Chinese apartments - -This converter: -- Scales images from 1cm/px to 5cm/px (vastu-map standard) -- Adds wall borders for closed environments -- Optionally adds furniture obstacles -- Supports batch conversion with filtering - -Usage: - # Convert single image - python houseexpo_converter.py dataset/0004d52d1aeeb8ae6de39d6bd993e992.png -o maps/ - - # Batch convert with size filtering - python houseexpo_converter.py dataset/ -o maps/ --min-size 5 --max-size 15 --count 50 - - # Convert with furniture - python houseexpo_converter.py dataset/ -o maps/ --add-furniture --count 20 - -Author: VacuumTiger Project -""" - -import argparse -import os -import sys -from pathlib import Path -from typing import List, Tuple, Optional -import random -import yaml - -try: - import cv2 - import numpy as np - from PIL import Image -except ImportError as e: - print(f"Missing dependency: {e}") - print("Install with: pip install opencv-python numpy pillow pyyaml") - sys.exit(1) - - -# HouseExpo resolution: 1 pixel = 1 cm = 0.01 m -HOUSEEXPO_RESOLUTION = 0.01 # meters per pixel - -# Target resolution for vastu-map -TARGET_RESOLUTION = 0.05 # meters per pixel (5 cm) - -# Scale factor: 1cm -> 5cm means divide by 5 -SCALE_FACTOR = int(TARGET_RESOLUTION / HOUSEEXPO_RESOLUTION) - - -def load_houseexpo_image(path: str) -> Tuple[np.ndarray, float, float]: - """ - Load HouseExpo image and return with physical dimensions. - - Returns: - Tuple of (image_array, width_meters, height_meters) - """ - img = Image.open(path).convert('L') - arr = np.array(img) - - # Calculate physical size (1px = 1cm) - width_m = img.size[0] * HOUSEEXPO_RESOLUTION - height_m = img.size[1] * HOUSEEXPO_RESOLUTION - - return arr, width_m, height_m - - -def scale_to_target_resolution(img: np.ndarray) -> np.ndarray: - """Scale image from 1cm/px to 5cm/px resolution.""" - h, w = img.shape - new_w = w // SCALE_FACTOR - new_h = h // SCALE_FACTOR - - # Use INTER_AREA for downscaling (best for reducing aliasing) - scaled = cv2.resize(img, (new_w, new_h), interpolation=cv2.INTER_AREA) - - # Re-binarize after scaling (threshold at 128) - _, binary = cv2.threshold(scaled, 128, 255, cv2.THRESH_BINARY) - - return binary - - -def add_border(img: np.ndarray, thickness: int = 2) -> np.ndarray: - """Add wall border around the map.""" - bordered = cv2.copyMakeBorder( - img, - thickness, thickness, thickness, thickness, - cv2.BORDER_CONSTANT, - value=0 - ) - return bordered - - -def clean_map(img: np.ndarray) -> np.ndarray: - """Clean up the map with morphological operations.""" - kernel = np.ones((2, 2), np.uint8) - - # Close small gaps in walls - closed = cv2.morphologyEx(img, cv2.MORPH_CLOSE, kernel) - - # Remove small noise - opened = cv2.morphologyEx(closed, cv2.MORPH_OPEN, kernel) - - return opened - - -def add_furniture( - img: np.ndarray, - density: float = 0.05, - seed: Optional[int] = None -) -> np.ndarray: - """Add random furniture obstacles.""" - if seed is not None: - np.random.seed(seed) - - result = img.copy() - h, w = result.shape - - # Furniture templates (width, height in pixels at 5cm resolution) - furniture = [ - (16, 20), # 80cm x 100cm table - (40, 16), # 200cm x 80cm sofa - (36, 40), # 180cm x 200cm bed - (24, 12), # 120cm x 60cm desk - (8, 8), # 40cm x 40cm chair - ] - - free_pixels = np.sum(result == 255) - target_furniture = int(free_pixels * density) - placed = 0 - attempts = 0 - - while placed < target_furniture and attempts < 500: - attempts += 1 - - fw, fh = furniture[np.random.randint(len(furniture))] - if np.random.random() > 0.5: - fw, fh = fh, fw - - x = np.random.randint(4, max(5, w - fw - 4)) - y = np.random.randint(4, max(5, h - fh - 4)) - - region = result[y:y+fh, x:x+fw] - if region.size > 0 and np.all(region == 255): - result[y:y+fh, x:x+fw] = 0 - placed += fw * fh - - return result - - -def convert_houseexpo( - input_path: str, - add_furniture_flag: bool = False, - furniture_density: float = 0.05, - furniture_seed: Optional[int] = None, -) -> Tuple[np.ndarray, dict]: - """ - Convert a HouseExpo image to vastu-map format. - - Returns: - Tuple of (pgm_array, metadata_dict) - """ - # Load original image - img, width_m, height_m = load_houseexpo_image(input_path) - - # Scale to 5cm resolution - scaled = scale_to_target_resolution(img) - - # Clean up - cleaned = clean_map(scaled) - - # Add border - bordered = add_border(cleaned, thickness=2) - - # Add furniture if requested - if add_furniture_flag: - bordered = add_furniture(bordered, furniture_density, furniture_seed) - - # Calculate final dimensions - h, w = bordered.shape - final_width_m = w * TARGET_RESOLUTION - final_height_m = h * TARGET_RESOLUTION - - metadata = { - 'resolution': TARGET_RESOLUTION, - 'origin': [0.0, 0.0, 0.0], - 'negate': 0, - 'occupied_thresh': 0.65, - 'free_thresh': 0.196, - 'source': 'HouseExpo', - 'original_size_m': { - 'width': width_m, - 'height': height_m, - }, - 'physical_size': { - 'width_m': final_width_m, - 'height_m': final_height_m, - }, - } - - return bordered, metadata - - -def save_pgm(img: np.ndarray, output_path: str, comment: str = ""): - """Save image as ASCII PGM (P2 format).""" - h, w = img.shape - with open(output_path, 'w') as f: - f.write("P2\n") - if comment: - for line in comment.split('\n'): - f.write(f"# {line}\n") - f.write(f"{w} {h}\n") - f.write("255\n") - for row in img: - f.write(' '.join(map(str, row)) + '\n') - - -def save_yaml(metadata: dict, output_path: str, pgm_filename: str): - """Save YAML metadata file.""" - yaml_data = { - 'image': pgm_filename, - 'resolution': metadata['resolution'], - 'origin': metadata['origin'], - 'negate': metadata['negate'], - 'occupied_thresh': metadata['occupied_thresh'], - 'free_thresh': metadata['free_thresh'], - } - with open(output_path, 'w') as f: - f.write(f"# HouseExpo floor plan: {pgm_filename}\n") - f.write(f"# Original size: {metadata['original_size_m']['width']:.1f}m x {metadata['original_size_m']['height']:.1f}m\n") - f.write(f"# Scaled size: {metadata['physical_size']['width_m']:.1f}m x {metadata['physical_size']['height_m']:.1f}m\n") - yaml.dump(yaml_data, f, default_flow_style=False, sort_keys=False) - - -def get_image_size(path: str) -> Tuple[float, float]: - """Get physical size of HouseExpo image in meters.""" - img = Image.open(path) - return img.size[0] * HOUSEEXPO_RESOLUTION, img.size[1] * HOUSEEXPO_RESOLUTION - - -def filter_by_size( - files: List[str], - input_dir: str, - min_size: float, - max_size: float, -) -> List[str]: - """Filter images by physical size (in meters).""" - filtered = [] - for f in files: - try: - w, h = get_image_size(os.path.join(input_dir, f)) - # Check if both dimensions are within range - if min_size <= w <= max_size and min_size <= h <= max_size: - filtered.append(f) - except Exception: - continue - return filtered - - -def main(): - parser = argparse.ArgumentParser( - description='Convert HouseExpo floor plans to vastu-map PGM format', - formatter_class=argparse.RawDescriptionHelpFormatter, - epilog=""" -HouseExpo Dataset Info: -- ~35,000 real floor plans from Chinese apartments -- Resolution: 1 pixel = 1 cm -- Binary format: 0=wall (black), 255=free (white) - -Examples: - # Convert a single floor plan - python houseexpo_converter.py dataset/0004d52d1aeeb8ae6de39d6bd993e992.png -o maps/ - - # Batch convert 50 medium-sized apartments (5-15m) - python houseexpo_converter.py dataset/ -o maps/ --min-size 5 --max-size 15 --count 50 - - # Convert with furniture for exploration testing - python houseexpo_converter.py dataset/ -o maps/ --add-furniture --count 20 - - # Convert all apartments between 8-12m - python houseexpo_converter.py dataset/ -o maps/ --min-size 8 --max-size 12 - """ - ) - - parser.add_argument('input', help='Input PNG file or directory of HouseExpo images') - parser.add_argument('--output', '-o', required=True, help='Output directory') - parser.add_argument('--min-size', type=float, default=0, - help='Minimum dimension in meters (filter)') - parser.add_argument('--max-size', type=float, default=100, - help='Maximum dimension in meters (filter)') - parser.add_argument('--count', '-n', type=int, default=0, - help='Maximum number of maps to convert (0=all)') - parser.add_argument('--add-furniture', '-f', action='store_true', - help='Add furniture obstacles') - parser.add_argument('--furniture-density', type=float, default=0.05, - help='Furniture density 0-1 (default: 0.05)') - parser.add_argument('--seed', '-s', type=int, - help='Random seed for reproducibility') - parser.add_argument('--prefix', '-p', default='houseexpo', - help='Output filename prefix (default: houseexpo)') - parser.add_argument('--shuffle', action='store_true', - help='Shuffle file order before selecting') - - args = parser.parse_args() - - # Create output directory - os.makedirs(args.output, exist_ok=True) - - input_path = Path(args.input) - - if input_path.is_file(): - # Single file conversion - files = [input_path.name] - input_dir = str(input_path.parent) - else: - # Directory batch conversion - input_dir = str(input_path) - files = [f for f in os.listdir(input_dir) if f.endswith('.png')] - files.sort() - - # Filter by size - if args.min_size > 0 or args.max_size < 100: - print(f"Filtering by size: {args.min_size}m - {args.max_size}m...") - files = filter_by_size(files, input_dir, args.min_size, args.max_size) - print(f"Found {len(files)} matching files") - - # Shuffle if requested - if args.shuffle: - random.seed(args.seed) - random.shuffle(files) - - # Limit count - if args.count > 0: - files = files[:args.count] - - if not files: - print("No files to convert") - return - - print(f"Converting {len(files)} HouseExpo floor plan(s)...") - - for i, filename in enumerate(files): - input_file = os.path.join(input_dir, filename) - - try: - # Convert - pgm, metadata = convert_houseexpo( - input_file, - add_furniture_flag=args.add_furniture, - furniture_density=args.furniture_density, - furniture_seed=args.seed + i if args.seed else None, - ) - - # Generate output names - base_name = Path(filename).stem - if len(files) > 1: - output_name = f"{args.prefix}_{i:04d}" - else: - output_name = f"{args.prefix}_{base_name[:8]}" - - pgm_path = os.path.join(args.output, f"{output_name}.pgm") - yaml_path = os.path.join(args.output, f"{output_name}.yaml") - - # Save - comment = f"HouseExpo: {filename}\n" - comment += f"Size: {metadata['physical_size']['width_m']:.1f}m x {metadata['physical_size']['height_m']:.1f}m" - - save_pgm(pgm, pgm_path, comment) - save_yaml(metadata, yaml_path, f"{output_name}.pgm") - - h, w = pgm.shape - print(f"[{i+1}/{len(files)}] {filename} -> {output_name}.pgm " - f"({w}x{h}px, {metadata['physical_size']['width_m']:.1f}x{metadata['physical_size']['height_m']:.1f}m)") - - except Exception as e: - print(f"[{i+1}/{len(files)}] Error converting {filename}: {e}") - - print(f"\nDone! Converted {len(files)} maps to {args.output}/") - - -if __name__ == '__main__': - main() diff --git a/vastu-map/tools/requirements.txt b/vastu-map/tools/requirements.txt deleted file mode 100644 index b242eda..0000000 --- a/vastu-map/tools/requirements.txt +++ /dev/null @@ -1,11 +0,0 @@ -# Floor plan converter and apartment generator dependencies -opencv-python>=4.5.0 -numpy>=1.20.0 -pillow>=8.0.0 -pyyaml>=5.4.0 - -# Optional: for SVG support -# cairosvg>=2.5.0 - -# Optional: for PDF support -# pdf2image>=1.14.0