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.
This commit is contained in:
codetiger
+1
-1
@@ -57,4 +57,4 @@ analysis/
|
||||
|
||||
*.log
|
||||
|
||||
results/
|
||||
output/
|
||||
|
||||
+7
-12
@@ -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
|
||||
|
||||
@@ -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
|
||||
@@ -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
|
||||
@@ -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}
|
||||
@@ -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
|
||||
@@ -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}
|
||||
@@ -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}
|
||||
@@ -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}
|
||||
@@ -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}
|
||||
@@ -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}
|
||||
@@ -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}
|
||||
@@ -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}
|
||||
@@ -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}
|
||||
@@ -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}
|
||||
@@ -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<RwLock<PoseGraph>>,
|
||||
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<PoseCorrections>) {
|
||||
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<Pose2D> {
|
||||
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.
|
||||
@@ -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)
|
||||
}
|
||||
@@ -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<ObservationRecord>,
|
||||
}
|
||||
|
||||
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<Visualizer>,
|
||||
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<vastu_map::core::PointCloud2D>,
|
||||
/// 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<Self, Box<dyn std::error::Error>> {
|
||||
// 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<ExplorationConfig>) -> 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
|
||||
}
|
||||
}
|
||||
@@ -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
|
||||
}
|
||||
@@ -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};
|
||||
@@ -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::<Vec<_>>()
|
||||
.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");
|
||||
}
|
||||
}
|
||||
@@ -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<String> = 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<ClearanceVisibilityGraph> = None;
|
||||
let mut last_planned_path: Option<PlannedPath> = 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<Point2D> = None;
|
||||
let mut last_cycle_print = 0;
|
||||
let mut pending_collision: Option<CollisionEvent> = 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();
|
||||
}
|
||||
}
|
||||
@@ -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<String>,
|
||||
|
||||
/// 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<String> {
|
||||
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::<f32>() * (max_x - min_x);
|
||||
let y = min_y + rng.random::<f32>() * (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::<f32>() * 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<f32> = scan_data.iter().map(|(_, d, _)| *d).collect();
|
||||
// Include lidar angle_offset so map observation matches ray-cast direction
|
||||
let angles: Vec<f32> = 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
|
||||
}
|
||||
@@ -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<String>,
|
||||
|
||||
/// 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<LidarScan>,
|
||||
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<LidarScan> {
|
||||
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<f32> = scan_data.iter().map(|(_, d, _)| *d).collect();
|
||||
let angles: Vec<f32> = 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<String> {
|
||||
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::<f32>() * (max_x - min_x);
|
||||
let y = min_y + rng.random::<f32>() * (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::<f32>() * 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,
|
||||
)
|
||||
}
|
||||
@@ -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<ScanMetrics>,
|
||||
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<ScanMetrics> = 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<f32> = scan_data.iter().map(|(_, d, _)| *d).collect();
|
||||
let angles: Vec<f32> = 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<f32> = scan_data.iter().map(|(_, d, _)| *d).collect();
|
||||
let angles: Vec<f32> = 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<Pose2D> = results.scans.iter().map(|s| s.gt_pose).collect();
|
||||
let est_poses: Vec<Pose2D> = results.scans.iter().map(|s| s.corrected_pose).collect();
|
||||
let odom_poses: Vec<Pose2D> = 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::<f32>() / 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::<f32>() / 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<f32> = Vec::new();
|
||||
let mut corr_errors: Vec<f32> = 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::<f32>() / odom_errors.len() as f32;
|
||||
let corr_avg = corr_errors.iter().sum::<f32>() / 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
|
||||
}
|
||||
@@ -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
|
||||
@@ -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
|
||||
@@ -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
|
||||
+626
-281
@@ -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<Self, ConfigLoadError> {
|
||||
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<Self, ConfigLoadError> {
|
||||
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<Self, ConfigLoadError> {
|
||||
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<f32> {
|
||||
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<f32> = 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);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -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`.
|
||||
@@ -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));
|
||||
}
|
||||
}
|
||||
@@ -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);
|
||||
}
|
||||
}
|
||||
@@ -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<Point2D> {
|
||||
// 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());
|
||||
}
|
||||
}
|
||||
+16
-78
@@ -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<f32>` and `ys: Vec<f32>` 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};
|
||||
|
||||
+162
-312
@@ -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<f32> for Point2D {
|
||||
impl Mul<f32> for WorldPoint {
|
||||
type Output = Self;
|
||||
|
||||
#[inline]
|
||||
fn mul(self, rhs: f32) -> Self {
|
||||
Self {
|
||||
x: self.x * rhs,
|
||||
y: self.y * rhs,
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
impl Mul<Point2D> for f32 {
|
||||
type Output = Point2D;
|
||||
|
||||
#[inline]
|
||||
fn mul(self, rhs: Point2D) -> Point2D {
|
||||
Point2D {
|
||||
x: self * rhs.x,
|
||||
y: self * rhs.y,
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
impl Div<f32> 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);
|
||||
}
|
||||
}
|
||||
|
||||
+146
-257
@@ -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);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -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<f32>,
|
||||
/// Y coordinates in meters (left direction).
|
||||
pub ys: Vec<f32>,
|
||||
}
|
||||
|
||||
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<Point2D> {
|
||||
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<Point2D> {
|
||||
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<Point2D>` (AoS layout).
|
||||
pub fn to_points(&self) -> Vec<Point2D> {
|
||||
self.xs
|
||||
.iter()
|
||||
.zip(&self.ys)
|
||||
.map(|(&x, &y)| Point2D::new(x, y))
|
||||
.collect()
|
||||
}
|
||||
|
||||
/// Iterate over points.
|
||||
pub fn iter(&self) -> impl Iterator<Item = Point2D> + '_ {
|
||||
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);
|
||||
}
|
||||
}
|
||||
@@ -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<f32>,
|
||||
/// Angle for each range in radians (0 = forward, CCW positive)
|
||||
pub angles: Vec<f32>,
|
||||
/// 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<f32>, angles: Vec<f32>, 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<Item = (f32, f32)> + '_ {
|
||||
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<WorldPoint> {
|
||||
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<WorldPoint> {
|
||||
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<WorldPoint> {
|
||||
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<WorldPoint> {
|
||||
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<f32> {
|
||||
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<WorldPoint> {
|
||||
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<WorldPoint> {
|
||||
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<LidarScan>,
|
||||
/// 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);
|
||||
}
|
||||
}
|
||||
@@ -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<f32>,
|
||||
/// Y coordinates
|
||||
pub ys: Vec<f32>,
|
||||
}
|
||||
|
||||
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<i32>,
|
||||
/// Y coordinates (row indices)
|
||||
pub ys: Vec<i32>,
|
||||
}
|
||||
|
||||
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)
|
||||
}
|
||||
}
|
||||
@@ -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<Self> {
|
||||
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);
|
||||
}
|
||||
}
|
||||
@@ -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()
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
+811
-1209
File diff suppressed because it is too large
Load Diff
@@ -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<Point2D>,
|
||||
|
||||
/// 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<f32> {
|
||||
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::<u8>() + 16);
|
||||
// VecDeque + Point2D entries
|
||||
let recent_size = self.recent_positions.len() * std::mem::size_of::<Point2D>();
|
||||
|
||||
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);
|
||||
}
|
||||
}
|
||||
@@ -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};
|
||||
|
||||
@@ -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<Point2D> = 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));
|
||||
}
|
||||
}
|
||||
@@ -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<ExplorationRegion>,
|
||||
|
||||
/// 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<ExplorationRegion>,
|
||||
robot_pos: Point2D,
|
||||
history: &ExplorationHistory,
|
||||
) -> Vec<ExplorationRegion> {
|
||||
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<ExplorationRegion> = 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());
|
||||
}
|
||||
}
|
||||
@@ -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<Frontier>,
|
||||
|
||||
/// 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<Frontier>) -> 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<usize>,
|
||||
|
||||
/// Reusable union-find rank array.
|
||||
rank: Vec<usize>,
|
||||
|
||||
/// Next region ID to assign.
|
||||
next_id: u32,
|
||||
|
||||
/// Spatial hash grid for efficient neighbor lookup.
|
||||
spatial_grid: HashMap<(i32, i32), Vec<usize>>,
|
||||
}
|
||||
|
||||
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<ExplorationRegion> {
|
||||
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<Frontier> =
|
||||
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<Vec<usize>> {
|
||||
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<usize, Vec<usize>> = 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);
|
||||
}
|
||||
}
|
||||
@@ -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<WorldPoint>,
|
||||
/// 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<WorldPoint>),
|
||||
|
||||
/// 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,
|
||||
}
|
||||
@@ -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);
|
||||
}
|
||||
}
|
||||
@@ -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<Pose2D>,
|
||||
/// Start time.
|
||||
start_time: Option<Instant>,
|
||||
/// 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<S: SensorSource>(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<S: SensorSource>(&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);
|
||||
}
|
||||
}
|
||||
@@ -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};
|
||||
@@ -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<LidarScan> {
|
||||
/// // 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<LidarScan>;
|
||||
|
||||
/// 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<u8> {
|
||||
None
|
||||
}
|
||||
}
|
||||
@@ -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<Pose2D>,
|
||||
/// Last submap origin pose (for computing delta between submaps).
|
||||
last_submap_origin: Option<Pose2D>,
|
||||
/// Scan counter for timestamps.
|
||||
scan_count: u64,
|
||||
/// Last submap count (to detect new submap creation).
|
||||
last_submap_count: usize,
|
||||
/// Start time.
|
||||
start_time: Option<Instant>,
|
||||
/// 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<S: SensorSource>(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<S: SensorSource>(&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);
|
||||
}
|
||||
}
|
||||
@@ -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<Line2D>;
|
||||
fn extract_with_pose(&self, points: &[Point2D], sensor_pose: Pose2D) -> Vec<Line2D>;
|
||||
}
|
||||
```
|
||||
|
||||
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()
|
||||
```
|
||||
@@ -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<Corner2D> {
|
||||
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<Corner2D> {
|
||||
// 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<Point2D> {
|
||||
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<Corner2D>, 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<Corner2D> {
|
||||
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<CurvatureCandidate> {
|
||||
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<Corner2D> {
|
||||
// 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::<Vec<_>>()
|
||||
);
|
||||
}
|
||||
|
||||
#[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);
|
||||
}
|
||||
}
|
||||
File diff suppressed because it is too large
Load Diff
@@ -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<Line2D> {
|
||||
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<Point2D> = 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<Line2D>,
|
||||
) {
|
||||
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<Point2D> = remaining_indices.iter().map(|&i| points[i]).collect();
|
||||
|
||||
let additional_lines = extract_lines(&remaining_points, split_merge_config);
|
||||
output_lines.extend(additional_lines);
|
||||
}
|
||||
}
|
||||
File diff suppressed because it is too large
Load Diff
@@ -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<Line2D> {
|
||||
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<Line2D> {
|
||||
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<Line2D> {
|
||||
// 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<usize>,
|
||||
) {
|
||||
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<Line2D> {
|
||||
// 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<usize>,
|
||||
) {
|
||||
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<Point2D> = 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);
|
||||
}
|
||||
}
|
||||
@@ -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
|
||||
}
|
||||
}
|
||||
@@ -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::<f32>() / 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<Vec<Point2D>> {
|
||||
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<f32> = 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<Point2D> = vec![];
|
||||
let threshold = adaptive_split_threshold(&points, 0.05, sensor_pos, 0.03);
|
||||
assert_eq!(threshold, 0.05); // Should return base threshold
|
||||
}
|
||||
}
|
||||
@@ -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;
|
||||
@@ -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<Line2D>;
|
||||
|
||||
/// 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<Line2D> {
|
||||
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<Line2D> {
|
||||
super::extract_lines(points, &self.config)
|
||||
}
|
||||
|
||||
fn extract_from_sensor(&self, points: &[Point2D], sensor_pos: Point2D) -> Vec<Line2D> {
|
||||
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<Point2D> = (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<Point2D> = (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<dyn LineExtractor> = Box::new(SplitMergeExtractor::default());
|
||||
|
||||
let points: Vec<Point2D> = (0..20).map(|i| Point2D::new(i as f32 * 0.1, 0.0)).collect();
|
||||
|
||||
let lines = extractor.extract(&points);
|
||||
assert_eq!(lines.len(), 1);
|
||||
}
|
||||
}
|
||||
@@ -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.
|
||||
@@ -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<f32>,
|
||||
/// Positions of all corners (SoA: y coordinates).
|
||||
pub ys: Vec<f32>,
|
||||
/// First line index for each corner.
|
||||
pub line1_indices: Vec<usize>,
|
||||
/// Second line index for each corner.
|
||||
pub line2_indices: Vec<usize>,
|
||||
/// Angles at each corner.
|
||||
pub angles: Vec<f32>,
|
||||
/// Observation counts.
|
||||
pub observation_counts: Vec<u32>,
|
||||
}
|
||||
|
||||
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<Corner2D> {
|
||||
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<Corner2D>`.
|
||||
pub fn to_corners(&self) -> Vec<Corner2D> {
|
||||
(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<Item = Corner2D> + '_ {
|
||||
(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));
|
||||
}
|
||||
}
|
||||
@@ -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<CornerDescriptor>,
|
||||
|
||||
/// 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::<f32>() / 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<Line2D> = 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);
|
||||
}
|
||||
}
|
||||
@@ -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<Line2D> {
|
||||
self.lines.get(index)
|
||||
}
|
||||
|
||||
/// Get all lines as a vector.
|
||||
pub fn lines(&self) -> Vec<Line2D> {
|
||||
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<Item = Line2D> + '_ {
|
||||
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<Corner2D> {
|
||||
self.corners.get(index)
|
||||
}
|
||||
|
||||
/// Get all corners as a vector.
|
||||
pub fn corners(&self) -> Vec<Corner2D> {
|
||||
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<Item = Corner2D> + '_ {
|
||||
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);
|
||||
}
|
||||
}
|
||||
@@ -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<f32> {
|
||||
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<Point2D> {
|
||||
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);
|
||||
}
|
||||
}
|
||||
@@ -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<f32>,
|
||||
/// Y coordinates of line start points.
|
||||
pub start_ys: Vec<f32>,
|
||||
/// X coordinates of line end points.
|
||||
pub end_xs: Vec<f32>,
|
||||
/// Y coordinates of line end points.
|
||||
pub end_ys: Vec<f32>,
|
||||
/// Observation counts for each line.
|
||||
pub observation_counts: Vec<u32>,
|
||||
/// Point counts for each line (scan points that formed the line).
|
||||
pub point_counts: Vec<u32>,
|
||||
|
||||
// === Pre-computed cached values for performance ===
|
||||
/// X components of unit normal vectors.
|
||||
normal_xs: Vec<f32>,
|
||||
/// Y components of unit normal vectors.
|
||||
normal_ys: Vec<f32>,
|
||||
/// Reciprocal of line lengths (1.0 / length).
|
||||
/// Used to avoid division in distance calculations.
|
||||
inv_lengths: Vec<f32>,
|
||||
}
|
||||
|
||||
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<Point2D> {
|
||||
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<f32> {
|
||||
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<Line2D> {
|
||||
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<Line2D>`.
|
||||
pub fn to_lines(&self) -> Vec<Line2D> {
|
||||
(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<f32> {
|
||||
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<f32> {
|
||||
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<f32>,
|
||||
) -> 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<Item = Line2D> + '_ {
|
||||
(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);
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -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,
|
||||
};
|
||||
@@ -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<Item = CellCoord> + '_ {
|
||||
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<CellCoord, Vec<usize>>,
|
||||
/// 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<CellCoord, Vec<usize>> = 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<usize> {
|
||||
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<usize>) {
|
||||
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);
|
||||
}
|
||||
}
|
||||
@@ -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<f32> = 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
|
||||
}
|
||||
}
|
||||
@@ -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);
|
||||
}
|
||||
}
|
||||
@@ -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<WorldPoint>,
|
||||
|
||||
/// 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<Self, ConfigError> {
|
||||
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<Self, ConfigError> {
|
||||
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<String, ConfigError> {
|
||||
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);
|
||||
}
|
||||
}
|
||||
@@ -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<GridCoord> = 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
|
||||
);
|
||||
}
|
||||
}
|
||||
@@ -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};
|
||||
@@ -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<Self::Item> {
|
||||
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<GridCoord>,
|
||||
/// 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<GridCoord>,
|
||||
}
|
||||
|
||||
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<Self::Item> {
|
||||
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<GridCoord> {
|
||||
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<GridCoord> {
|
||||
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));
|
||||
}
|
||||
}
|
||||
File diff suppressed because it is too large
Load Diff
@@ -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
|
||||
@@ -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<Association> {
|
||||
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<usize> = 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<Association> = 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<Association> {
|
||||
// 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<Association> {
|
||||
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<usize, Association> =
|
||||
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<usize> {
|
||||
let associations = find_associations(scan_lines, map_lines, index, config);
|
||||
let matched: std::collections::HashSet<usize> =
|
||||
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);
|
||||
}
|
||||
}
|
||||
File diff suppressed because it is too large
Load Diff
@@ -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<Line2D> = 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};
|
||||
@@ -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<Vec<AssociatedPoint>>,
|
||||
/// 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<AssociatedPoint>> = 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<AssociatedPoint>> = 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<usize> {
|
||||
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<Point2D> {
|
||||
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<f32> = points.iter().map(|(x, _)| *x).collect();
|
||||
let ys: Vec<f32> = 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<Line2D> = 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<u32> = 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);
|
||||
}
|
||||
}
|
||||
@@ -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<Line2D>),
|
||||
/// Not enough points to re-fit (keeps original geometry).
|
||||
Insufficient,
|
||||
}
|
||||
|
||||
impl RefitResult {
|
||||
/// Get all lines from the result.
|
||||
pub fn lines(&self) -> Vec<Line2D> {
|
||||
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<u32, Pose2D>,
|
||||
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<usize> {
|
||||
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<u32, Pose2D>,
|
||||
observation_count: u32,
|
||||
config: &RefitConfig,
|
||||
) -> Vec<Line2D> {
|
||||
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<u32, Pose2D>,
|
||||
observation_count: u32,
|
||||
noise_model: &LidarNoiseModel,
|
||||
) -> Option<Line2D> {
|
||||
if points.len() < 2 {
|
||||
return None;
|
||||
}
|
||||
|
||||
// Extract Point2D and compute weights
|
||||
let mut point_vec: Vec<Point2D> = Vec::with_capacity(points.len());
|
||||
let mut weights: Vec<f32> = 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<u32, Pose2D> = 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<AssociatedPoint> {
|
||||
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<AssociatedPoint> {
|
||||
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<AssociatedPoint> = 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<AssociatedPoint> = 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<AssociatedPoint> = 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<AssociatedPoint> = (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<AssociatedPoint> = 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);
|
||||
}
|
||||
}
|
||||
@@ -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<Point2D>,
|
||||
|
||||
/// 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<Point2D>,
|
||||
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<Point2D> {
|
||||
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<Point2D>) {
|
||||
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>() + self.points.len() * std::mem::size_of::<Point2D>()
|
||||
}
|
||||
}
|
||||
|
||||
/// 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<usize>,
|
||||
|
||||
/// 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<StoredScan>,
|
||||
|
||||
/// Next scan ID to assign.
|
||||
next_scan_id: u32,
|
||||
|
||||
/// Configuration.
|
||||
config: ScanStoreConfig,
|
||||
|
||||
/// Last stored pose (for distance/rotation filtering).
|
||||
last_stored_pose: Option<Pose2D>,
|
||||
}
|
||||
|
||||
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<u32> {
|
||||
// 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<u32> {
|
||||
// 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<Item = &StoredScan> {
|
||||
self.scans.iter()
|
||||
}
|
||||
|
||||
/// Iterate over scans in reverse order (newest first).
|
||||
pub fn iter_rev(&self) -> impl Iterator<Item = &StoredScan> {
|
||||
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<Item = &StoredScan> {
|
||||
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<Pose2D> {
|
||||
self.scans.iter().map(|s| s.estimated_pose).collect()
|
||||
}
|
||||
|
||||
/// Get the trajectory as a list of points.
|
||||
pub fn trajectory(&self) -> Vec<Point2D> {
|
||||
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<Point2D> {
|
||||
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<u32, Pose2D>,
|
||||
) -> 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::<Point2D>();
|
||||
let scan_overhead = self.scans.len() * std::mem::size_of::<StoredScan>();
|
||||
let vec_overhead = self.scans.capacity() * std::mem::size_of::<StoredScan>();
|
||||
|
||||
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<Point2D> {
|
||||
(0..n).map(|i| Point2D::new(i as f32 * 0.1, 0.0)).collect()
|
||||
}
|
||||
|
||||
fn make_point_cloud(n: usize) -> PointCloud2D {
|
||||
let xs: Vec<f32> = (0..n).map(|i| i as f32 * 0.1).collect();
|
||||
let ys: Vec<f32> = 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
|
||||
}
|
||||
}
|
||||
@@ -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<IndexedLine>,
|
||||
}
|
||||
|
||||
impl SpatialIndex {
|
||||
/// Create a new spatial index from lines.
|
||||
pub fn new(lines: &[Line2D]) -> Self {
|
||||
let indexed: Vec<IndexedLine> = 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<IndexedLine> = self.tree.iter().cloned().collect();
|
||||
let new: Vec<IndexedLine> = 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<usize> {
|
||||
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<usize> {
|
||||
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<IndexedLine> = 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<Line2D> {
|
||||
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());
|
||||
}
|
||||
}
|
||||
@@ -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};
|
||||
@@ -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<W: Write>(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);
|
||||
}
|
||||
}
|
||||
@@ -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<StartPose>,
|
||||
|
||||
/// Seed configuration for initial map building
|
||||
#[serde(default)]
|
||||
pub seed: SeedConfig,
|
||||
|
||||
/// Sequence of movement commands
|
||||
pub commands: Vec<Command>,
|
||||
|
||||
/// 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<String>,
|
||||
|
||||
/// 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<f32>,
|
||||
|
||||
/// Time interval for pose markers (seconds)
|
||||
#[serde(default)]
|
||||
pub marker_interval_s: Option<f32>,
|
||||
|
||||
/// 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<Self, ScenarioError> {
|
||||
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<Self, ScenarioError> {
|
||||
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");
|
||||
}
|
||||
}
|
||||
@@ -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<Pose2D>,
|
||||
/// Line color
|
||||
pub color: String,
|
||||
/// Marker positions (indices into poses)
|
||||
pub marker_indices: Vec<usize>,
|
||||
}
|
||||
|
||||
/// SVG visualization builder
|
||||
pub struct SvgVisualizer {
|
||||
config: SvgConfig,
|
||||
/// Grid storage for map
|
||||
storage: GridStorage,
|
||||
/// Ground truth trajectory
|
||||
gt_trajectory: Option<Trajectory>,
|
||||
/// Estimated/corrected trajectory
|
||||
est_trajectory: Option<Trajectory>,
|
||||
/// Odometry trajectory
|
||||
odom_trajectory: Option<Trajectory>,
|
||||
/// Title to display
|
||||
title: Option<String>,
|
||||
}
|
||||
|
||||
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<String>) -> Self {
|
||||
self.title = Some(title.into());
|
||||
self
|
||||
}
|
||||
|
||||
/// Add ground truth trajectory
|
||||
pub fn with_ground_truth(mut self, poses: Vec<Pose2D>, marker_indices: Vec<usize>) -> 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<Pose2D>, marker_indices: Vec<usize>) -> 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<Pose2D>, marker_indices: Vec<usize>) -> 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#"<?xml version="1.0" encoding="UTF-8"?>"#).unwrap();
|
||||
writeln!(
|
||||
&mut svg,
|
||||
r#"<svg xmlns="http://www.w3.org/2000/svg" width="{:.0}" height="{:.0}" viewBox="0 0 {:.0} {:.0}">"#,
|
||||
width, height, width, height
|
||||
).unwrap();
|
||||
|
||||
// Background
|
||||
writeln!(
|
||||
&mut svg,
|
||||
r##" <rect width="100%" height="100%" fill="#F8F8F8"/>"##
|
||||
)
|
||||
.unwrap();
|
||||
|
||||
// Title
|
||||
if let Some(ref title) = self.title {
|
||||
writeln!(
|
||||
&mut svg,
|
||||
r##" <text x="{:.0}" y="22" font-family="sans-serif" font-size="16" font-weight="bold" text-anchor="middle" fill="#333">{}</text>"##,
|
||||
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#" <g transform="translate({:.0}, {:.0})">"#,
|
||||
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, " </g>").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, "</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#" <g id="grid">"#).unwrap();
|
||||
|
||||
// Draw background for unknown area
|
||||
writeln!(
|
||||
svg,
|
||||
r#" <rect width="{:.0}" height="{:.0}" fill="{}"/>"#,
|
||||
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#" <rect x="{:.1}" y="{:.1}" width="{:.1}" height="{:.1}" fill="{}"/>"#,
|
||||
px_x, px_y, cell_size, cell_size, color
|
||||
)
|
||||
.unwrap();
|
||||
}
|
||||
|
||||
writeln!(svg, " </g>").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#" <g id="trajectory-{}">"#, 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="{}" fill="none" stroke="{}" stroke-width="{}" stroke-linecap="round" stroke-linejoin="round" opacity="0.8"/>"#,
|
||||
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#" <circle cx="{:.1}" cy="{:.1}" r="{:.1}" fill="{}" stroke="white" stroke-width="1"/>"#,
|
||||
px, py, self.config.marker_radius, traj.color
|
||||
).unwrap();
|
||||
|
||||
// Direction line
|
||||
writeln!(
|
||||
svg,
|
||||
r#" <line x1="{:.1}" y1="{:.1}" x2="{:.1}" y2="{:.1}" stroke="white" stroke-width="2"/>"#,
|
||||
px, py, px + dx, py + dy
|
||||
).unwrap();
|
||||
writeln!(
|
||||
svg,
|
||||
r#" <line x1="{:.1}" y1="{:.1}" x2="{:.1}" y2="{:.1}" stroke="{}" stroke-width="1.5"/>"#,
|
||||
px, py, px + dx, py + dy, traj.color
|
||||
).unwrap();
|
||||
}
|
||||
}
|
||||
|
||||
writeln!(svg, " </g>").unwrap();
|
||||
}
|
||||
|
||||
/// Render legend
|
||||
fn render_legend(&self, svg: &mut String, svg_width: f32, y_offset: f32) {
|
||||
writeln!(
|
||||
svg,
|
||||
r#" <g id="legend" font-family="sans-serif" font-size="12" transform="translate(0, {:.0})">"#,
|
||||
y_offset
|
||||
).unwrap();
|
||||
|
||||
// Legend background
|
||||
writeln!(
|
||||
svg,
|
||||
r##" <rect x="10" y="0" width="{:.0}" height="85" fill="white" stroke="#CCC" stroke-width="1" rx="4"/>"##,
|
||||
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#" <line x1="20" y1="{:.0}" x2="50" y2="{:.0}" stroke="{}" stroke-width="3"/>"#,
|
||||
entry_y, entry_y, color
|
||||
).unwrap();
|
||||
|
||||
// Label
|
||||
writeln!(
|
||||
svg,
|
||||
r##" <text x="60" y="{:.0}" fill="#333">{}</text>"##,
|
||||
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#" <rect x="{:.0}" y="10" width="15" height="15" fill="{}"/>"#,
|
||||
map_legend_x, self.config.colors.wall
|
||||
)
|
||||
.unwrap();
|
||||
writeln!(
|
||||
svg,
|
||||
r##" <text x="{:.0}" y="22" fill="#333">Walls/Obstacles</text>"##,
|
||||
map_legend_x + 20.0
|
||||
)
|
||||
.unwrap();
|
||||
|
||||
writeln!(
|
||||
svg,
|
||||
r##" <rect x="{:.0}" y="30" width="15" height="15" fill="{}" stroke="#CCC"/>"##,
|
||||
map_legend_x, self.config.colors.floor
|
||||
)
|
||||
.unwrap();
|
||||
writeln!(
|
||||
svg,
|
||||
r##" <text x="{:.0}" y="42" fill="#333">Floor</text>"##,
|
||||
map_legend_x + 20.0
|
||||
)
|
||||
.unwrap();
|
||||
|
||||
writeln!(
|
||||
svg,
|
||||
r#" <rect x="{:.0}" y="50" width="15" height="15" fill="{}"/>"#,
|
||||
map_legend_x, self.config.colors.unknown
|
||||
)
|
||||
.unwrap();
|
||||
writeln!(
|
||||
svg,
|
||||
r##" <text x="{:.0}" y="62" fill="#333">Unknown</text>"##,
|
||||
map_legend_x + 20.0
|
||||
)
|
||||
.unwrap();
|
||||
|
||||
writeln!(svg, " </g>").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<usize> {
|
||||
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<usize> {
|
||||
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<usize> = (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("<svg"));
|
||||
assert!(svg.contains("</svg>"));
|
||||
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"));
|
||||
}
|
||||
}
|
||||
@@ -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<W: Write>(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<GridStorage, IoError> {
|
||||
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<R: Read>(reader: &mut R) -> Result<GridStorage, IoError> {
|
||||
// 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);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
+400
-398
@@ -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<Point2D>,
|
||||
/// 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<CellCounts> 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<M: Map>(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<Frontier>;
|
||||
// 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<Path>;
|
||||
(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<Point2D>;
|
||||
/// 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);
|
||||
}
|
||||
}
|
||||
|
||||
Some files were not shown because too many files have changed in this diff Show More
Reference in New Issue
Block a user