Enuxia-Public/enuxia-vacuum
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Split large vastu-map modules into subdirectories

Browse Source 
Break down monolithic files into focused submodules:
- config.rs → config/ with separate files per concern
- exploration/ → explore/controller/ with state machine
- grid/storage.rs → grid/storage/ module
- pathfinding/astar.rs → pathfinding/astar/ module
- query/traversability.rs → query/traversability/ module
- slam/matcher.rs → slam/matcher/ with helpers

Add new SLAM components:
- branch_bound/ for scan matching optimization
- background_optimizer.rs for async graph optimization

Add new explore components:
- error.rs, motion_filter.rs, velocity.rs
This commit is contained in:
codetiger
2025-12-31 12:07:15 +05:30
parent 4270670fc7
commit 169eba9e2e
65 changed files with 7073 additions and 3993 deletions
Download Patch File Download Diff File Expand all files Collapse all files
dhruva-slam/src/engine/graph/optimizer.rs
-1
View File
@@ -380,7 +380,6 @@ impl GraphOptimizer {
}
/// Add edge contribution to H and b.
#[allow(clippy::too_many_arguments)]
fn add_edge_contribution(
&self,
h: &mut [f64],
vastu-map/configs/config.yaml
+157 -1
View File
@@ -1,4 +1,5 @@
# VastuMap Occupancy Grid Configuration
# Inspired by Google Cartographer SLAM parameters
# Units: meters (m), radians (rad), cells (n)
# ============================================================================
@@ -6,7 +7,7 @@
# ============================================================================
grid:
# Cell resolution in meters per cell
resolution: 0.025 # 2.5cm cells
resolution: 0.025 # 2.5cm cells (Cartographer default: 0.05)
# Initial grid dimensions (cells)
initial_width: 300
@@ -22,6 +23,25 @@ grid:
# Enable automatic grid expansion
auto_expand: true
# Log-odds occupancy model (Cartographer-style)
# Lower values = more observations needed to establish walls
log_odds:
# Log-odds increment for hit (lidar endpoint)
# Cartographer: ~20 (P=0.55), VastuMap aggressive: 70 (P=0.67)
l_hit: 20 # Cartographer default: ~3-5 observations for wall
# Log-odds decrement for miss (ray passes through)
# Cartographer: ~-4 (P=0.49), VastuMap aggressive: -28 (P=0.43)
l_miss: -4 # Cartographer default: asymmetric for sticky walls
# Cell classification thresholds
l_occupied_threshold: 50
l_free_threshold: -50
# Clamping range (probability bounds)
l_min: -200 # P ≈ 0.12
l_max: 200 # P ≈ 0.88
# ============================================================================
# Sensor Configuration
# ============================================================================
@@ -52,6 +72,142 @@ sensor:
left_angle: 0.5 # ~30 degrees
right_angle: -0.5
# ============================================================================
# SLAM Configuration
# ============================================================================
slam:
# Correlative scan matcher (local SLAM)
correlative:
enabled: true
# Search window
search_x: 0.3 # meters
search_y: 0.3 # meters
search_theta: 0.15 # radians (~8.6°)
# Search resolution (finer = more accurate but slower)
linear_resolution: 0.01 # Was 0.02, now 1cm
angular_resolution: 0.01 # Was 0.02, now ~0.57°
# Multi-resolution search (coarse-to-fine)
multi_resolution: true
coarse_factor: 2.0
# Gaussian scoring (uses distance field)
use_gaussian_scoring: true
gaussian_sigma: 0.05 # Was 0.10, now 2x grid resolution
# Gauss-Newton refinement for sub-pixel accuracy
gn_max_iterations: 10
gn_convergence_threshold: 1e-5
gn_damping: 1e-3
gn_translation_weight: 500.0 # Trust encoder distance
gn_rotation_weight: 20.0 # Allow heading correction
# Match quality threshold
min_score: 0.5
# Maximum points to use (0 = all)
max_points: 0
# Optional: skip correlative search for reliable odometry
skip_when_confident: false
skip_confidence_threshold: 0.8
# Branch-and-bound matcher (for loop closure)
# More efficient than brute-force for large search windows
branch_bound:
enabled: true
# Large search window for loop closure
search_x: 2.0 # meters (Cartographer global: ~7m)
search_y: 2.0 # meters
search_theta: 0.5 # radians (~28°)
# Angular resolution
angular_resolution: 0.02 # ~1.1°
# Minimum score ratio (fraction of max possible)
min_score_ratio: 0.5
# Loop closure detection
loop_closure:
enabled: true
# Keyframe creation thresholds
keyframe_distance: 0.3 # Was 0.5, now denser
keyframe_rotation: 0.35 # Was 0.5 (~20° now)
# LiDAR-IRIS descriptor matching
max_descriptor_distance: 80 # Was 100, now stricter (out of 640)
# Spatial constraints
min_loop_gap: 20 # Minimum pose index gap
max_loop_distance: 10.0 # Maximum spatial distance (m)
# Verification
min_verification_score: 0.6
use_branch_bound: true # Use B&B instead of correlative
# Verification search window (fallback if B&B disabled)
verification_search_window: 0.2
verification_angular_window: 0.2
# ============================================================================
# Submap Configuration
# ============================================================================
submap:
scans_per_submap: 50 # Cartographer: 90
overlap_scans: 10 # Smooth transitions between submaps
local_grid_size: 200 # 5m × 5m at 2.5cm resolution
max_distance_per_submap: 3.0 # Force new submap after this distance
max_submaps: 100 # Maximum submaps to store
# Loop closure between submaps
min_loop_closure_gap: 3 # Minimum submap index gap
max_loop_closure_distance: 10.0
# ============================================================================
# Pose Graph Optimization
# ============================================================================
pose_graph:
# Optimization trigger
optimize_every_n_nodes: 0 # 0 = only on loop closure
# Solver parameters
max_iterations: 50
convergence_threshold: 1e-5
# Huber loss for robust optimization (downweight outliers)
huber_scale: 0.1 # 10cm
# Edge weights
odometry_weight: 100.0 # Strong trust in encoder odometry
loop_closure_weight: 50.0 # Lower to handle noise
# Background thread (optional, not recommended for ARM)
use_background_thread: false
# ============================================================================
# Motion Filter Configuration (Cartographer-style)
# ============================================================================
# Gates scan processing based on robot movement to reduce redundant observations
# and save CPU when stationary. A scan is processed if ANY threshold is exceeded.
motion_filter:
# Enable/disable motion filtering (true = filter enabled)
enabled: true
# Maximum time between scan insertions (seconds)
# Ensures at least some updates even when stationary
max_time_secs: 5.0
# Maximum distance before forcing scan insertion (meters)
max_distance: 0.2
# Maximum rotation before forcing scan insertion (radians)
# 0.17 rad ≈ 10 degrees
max_angle: 0.17
# ============================================================================
# Query Configuration
# ============================================================================
vastu-map/docs/proposal.md
-594
View File
@@ -1,594 +0,0 @@
# 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.
vastu-map/examples/explore_mock.rs
+2 -3
View File
@@ -295,8 +295,7 @@ struct DriftStats {
enum ExplorationStatus {
Complete,
MaxStepsReached,
#[allow(dead_code)]
Failed(String),
Failed,
}
/// Simple wheel odometry tracker
@@ -603,7 +602,7 @@ fn run_exploration(
println!("\nExploration failed: {}", reason);
return ExplorationResult {
map,
status: ExplorationStatus::Failed(reason),
status: ExplorationStatus::Failed,
steps: step,
elapsed_secs: start_time.elapsed().as_secs_f32(),
progress: explorer.progress(),
vastu-map/examples/explore_simple.rs
+2 -2
View File
@@ -237,8 +237,8 @@ fn main() {
println!("Wall cells: {}", stats.wall_cells);
println!("Coverage: {:.2}m²", stats.explored_area_m2);
}
ExploreResult::Failed { map, reason } => {
println!("\nExploration failed: {}", reason);
ExploreResult::Failed { map, error } => {
println!("\nExploration failed: {}", error);
// Save partial map
let output_base = Path::new(&args.output).join(format!("{}_partial", map_name));
vastu-map/src/config.rs
-672
View File
@@ -1,672 +0,0 @@
//! Unified configuration loading for VastuMap.
//!
//! 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;
/// Full VastuMap configuration loaded from YAML
#[derive(Clone, Debug, Serialize, Deserialize, Default)]
pub struct VastuConfig {
/// Grid settings
#[serde(default)]
pub grid: GridSection,
/// 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 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,
}
}
}
/// Grid configuration section
#[derive(Clone, Debug, Serialize, Deserialize)]
pub struct GridSection {
/// Cell resolution (meters)
#[serde(default = "default_resolution")]
pub resolution: f32,
/// Initial grid width (cells)
#[serde(default = "default_grid_size")]
pub initial_width: usize,
/// Initial grid height (cells)
#[serde(default = "default_grid_size")]
pub initial_height: usize,
/// Origin mode: "center" or "corner"
#[serde(default = "default_origin_mode")]
pub origin_mode: String,
/// 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 GridSection {
fn default() -> Self {
Self {
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 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
};
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,
}
}
}
/// Sensor configuration section
#[derive(Clone, Debug, Serialize, Deserialize, Default)]
pub struct SensorSection {
/// LiDAR settings
#[serde(default)]
pub lidar: LidarSettings,
/// Robot geometry
#[serde(default)]
pub robot: RobotSettings,
/// Cliff sensor positions
#[serde(default)]
pub cliff_sensors: CliffSensorSettings,
/// 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_default_config() {
let config = VastuConfig::default();
assert_eq!(config.grid.resolution, 0.025);
assert_eq!(config.sensor.robot.radius, 0.17);
}
#[test]
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_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);
}
}
vastu-map/src/config/defaults.rs
+113
View File
@@ -0,0 +1,113 @@
//! Default value functions for serde deserialization.
pub fn resolution() -> f32 {
0.025
}
pub fn grid_size() -> usize {
800
}
pub fn origin_mode() -> String {
"center".to_string()
}
pub fn enabled() -> bool {
true
}
pub fn lidar_offset_x() -> f32 {
-0.110
}
pub fn min_range() -> f32 {
0.15
}
pub fn max_range() -> f32 {
8.0
}
pub fn robot_radius() -> f32 {
0.17
}
pub fn left_bumper_angle() -> f32 {
0.5
}
pub fn right_bumper_angle() -> f32 {
-0.5
}
pub fn min_frontier_size() -> usize {
5
}
pub fn safety_margin() -> f32 {
0.05
}
pub fn diagonal_cost() -> f32 {
std::f32::consts::SQRT_2
}
pub fn max_iterations() -> usize {
100_000
}
pub fn los_step() -> f32 {
0.05
}
pub fn smooth_iterations() -> usize {
100
}
pub fn frontier_threshold() -> f32 {
0.3
}
pub fn waypoint_threshold() -> f32 {
0.1
}
pub fn max_plan_distance() -> f32 {
5.0
}
pub fn replan_interval() -> f32 {
1.0
}
pub fn max_failures() -> usize {
5
}
pub fn turn_angle() -> f32 {
std::f32::consts::FRAC_PI_4
}
pub fn backup_distance() -> f32 {
0.1
}
pub fn output_format() -> String {
"both".to_string()
}
pub fn output_dir() -> String {
"./output".to_string()
}
pub fn motion_max_time() -> f32 {
5.0
}
pub fn motion_max_distance() -> f32 {
0.2
}
pub fn motion_max_angle() -> f32 {
0.17
}
vastu-map/src/config/error.rs
+21
View File
@@ -0,0 +1,21 @@
//! Configuration loading errors.
/// 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 {}
vastu-map/src/config/exploration.rs
+82
View File
@@ -0,0 +1,82 @@
//! Exploration configuration section.
use serde::{Deserialize, Serialize};
use super::defaults;
/// Exploration settings section
#[derive(Clone, Debug, Serialize, Deserialize)]
pub struct ExplorationSection {
/// Minimum cells for valid frontier
#[serde(default = "defaults::min_frontier_size")]
pub min_frontier_size: usize,
/// Distance to consider frontier reached (meters)
#[serde(default = "defaults::frontier_threshold")]
pub frontier_reached_threshold: f32,
/// Distance for waypoint reached (meters)
#[serde(default = "defaults::waypoint_threshold")]
pub waypoint_threshold: f32,
/// Maximum planning distance (meters)
#[serde(default = "defaults::max_plan_distance")]
pub max_plan_distance: f32,
/// Seconds between replans when blocked
#[serde(default = "defaults::replan_interval")]
pub replan_interval: f32,
/// Max consecutive planning failures before giving up
#[serde(default = "defaults::max_failures")]
pub max_consecutive_failures: usize,
/// Penalty for paths near obstacles
#[serde(default)]
pub path_clearance_weight: f32,
/// Enable path smoothing
#[serde(default = "defaults::enabled")]
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 = "defaults::turn_angle")]
pub turn_angle: f32,
/// Backup distance for recovery (meters)
#[serde(default = "defaults::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,
}
}
}
vastu-map/src/config/grid.rs
+75
View File
@@ -0,0 +1,75 @@
//! Grid configuration section.
use serde::{Deserialize, Serialize};
use crate::core::WorldPoint;
use crate::grid::GridConfig;
use super::defaults;
/// Grid configuration section
#[derive(Clone, Debug, Serialize, Deserialize)]
pub struct GridSection {
/// Cell resolution (meters)
#[serde(default = "defaults::resolution")]
pub resolution: f32,
/// Initial grid width (cells)
#[serde(default = "defaults::grid_size")]
pub initial_width: usize,
/// Initial grid height (cells)
#[serde(default = "defaults::grid_size")]
pub initial_height: usize,
/// Origin mode: "center" or "corner"
#[serde(default = "defaults::origin_mode")]
pub origin_mode: String,
/// 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 = "defaults::enabled")]
pub auto_expand: bool,
}
impl Default for GridSection {
fn default() -> Self {
Self {
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 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
};
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,
}
}
}
vastu-map/src/config/mod.rs
+31
View File
@@ -0,0 +1,31 @@
//! Unified configuration loading for VastuMap.
//!
//! Loads all configuration from a single YAML file.
mod defaults;
mod error;
mod exploration;
mod grid;
mod motion_filter;
mod pathfinding;
mod persistence;
mod query;
mod sensor;
mod slam;
mod vastu;
// Re-export main types
pub use error::ConfigLoadError;
pub use vastu::VastuConfig;
// Re-export section types
pub use exploration::{ExplorationSection, RecoverySettings};
pub use grid::GridSection;
pub use motion_filter::MotionFilterSection;
pub use pathfinding::{AStarSettings, PathfindingSection, SmoothingSettings};
pub use persistence::PersistenceSection;
pub use query::{FrontierSettings, QuerySection, TraversabilitySettings};
pub use sensor::{
BumperSettings, CliffSensorSettings, LidarSettings, RobotSettings, SensorSection,
};
pub use slam::SlamSection;
vastu-map/src/config/motion_filter.rs
+53
View File
@@ -0,0 +1,53 @@
//! Motion filter configuration section.
use serde::{Deserialize, Serialize};
use crate::explore::MotionFilterConfig;
use super::defaults;
/// Motion filter settings section
///
/// Gates scan processing based on robot movement to reduce redundant observations.
/// A scan is processed if ANY threshold is exceeded.
#[derive(Clone, Debug, Serialize, Deserialize)]
pub struct MotionFilterSection {
/// Whether motion filtering is enabled
#[serde(default = "defaults::enabled")]
pub enabled: bool,
/// Maximum time between scan insertions (seconds)
#[serde(default = "defaults::motion_max_time")]
pub max_time_secs: f32,
/// Maximum distance before forcing scan insertion (meters)
#[serde(default = "defaults::motion_max_distance")]
pub max_distance: f32,
/// Maximum rotation before forcing scan insertion (radians)
#[serde(default = "defaults::motion_max_angle")]
pub max_angle: f32,
}
impl Default for MotionFilterSection {
fn default() -> Self {
Self {
enabled: true,
max_time_secs: defaults::motion_max_time(),
max_distance: defaults::motion_max_distance(),
max_angle: defaults::motion_max_angle(),
}
}
}
impl MotionFilterSection {
/// Convert to MotionFilterConfig
pub fn to_motion_filter_config(&self) -> MotionFilterConfig {
MotionFilterConfig {
enabled: self.enabled,
max_time_secs: self.max_time_secs,
max_distance: self.max_distance,
max_angle: self.max_angle,
}
}
}
vastu-map/src/config/pathfinding.rs
+74
View File
@@ -0,0 +1,74 @@
//! Pathfinding configuration section.
use serde::{Deserialize, Serialize};
use super::defaults;
/// 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 = "defaults::enabled")]
pub allow_diagonal: bool,
/// Cost multiplier for diagonal moves (sqrt(2))
#[serde(default = "defaults::diagonal_cost")]
pub diagonal_cost: f32,
/// Maximum nodes to expand
#[serde(default = "defaults::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 = "defaults::enabled")]
pub enabled: bool,
/// Line-of-sight check step (meters)
#[serde(default = "defaults::los_step")]
pub los_step_size: f32,
/// Midpoint smoothing iterations
#[serde(default = "defaults::smooth_iterations")]
pub max_iterations: usize,
}
impl Default for SmoothingSettings {
fn default() -> Self {
Self {
enabled: true,
los_step_size: 0.05,
max_iterations: 100,
}
}
}
vastu-map/src/config/persistence.rs
+31
View File
@@ -0,0 +1,31 @@
//! Persistence configuration section.
use serde::{Deserialize, Serialize};
use super::defaults;
/// Persistence settings section
#[derive(Clone, Debug, Serialize, Deserialize)]
pub struct PersistenceSection {
/// Output format: "vastu", "pgm", or "both"
#[serde(default = "defaults::output_format")]
pub output_format: String,
/// Output directory path
#[serde(default = "defaults::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,
}
}
}
vastu-map/src/config/query.rs
+54
View File
@@ -0,0 +1,54 @@
//! Query configuration section.
use serde::{Deserialize, Serialize};
use super::defaults;
/// 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 = "defaults::min_frontier_size")]
pub min_frontier_size: usize,
/// Use 8-connected grid (vs 4-connected)
#[serde(default = "defaults::enabled")]
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 = "defaults::safety_margin")]
pub safety_margin: f32,
}
impl Default for TraversabilitySettings {
fn default() -> Self {
Self {
safety_margin: 0.05,
}
}
}
vastu-map/src/config/sensor.rs
+131
View File
@@ -0,0 +1,131 @@
//! Sensor configuration section.
use serde::{Deserialize, Serialize};
use crate::core::WorldPoint;
use crate::grid::SensorConfig;
use super::defaults;
/// Sensor configuration section
#[derive(Clone, Debug, Serialize, Deserialize, Default)]
pub struct SensorSection {
/// LiDAR settings
#[serde(default)]
pub lidar: LidarSettings,
/// Robot geometry
#[serde(default)]
pub robot: RobotSettings,
/// Cliff sensor positions
#[serde(default)]
pub cliff_sensors: CliffSensorSettings,
/// 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 = "defaults::lidar_offset_x")]
pub offset_x: f32,
/// Offset Y from robot center
#[serde(default)]
pub offset_y: f32,
/// Minimum valid range
#[serde(default = "defaults::min_range")]
pub min_range: f32,
/// Maximum valid range
#[serde(default = "defaults::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 = "defaults::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 = "defaults::left_bumper_angle")]
pub left_angle: f32,
/// Right bumper angle (radians from forward)
#[serde(default = "defaults::right_bumper_angle")]
pub right_angle: f32,
}
impl Default for BumperSettings {
fn default() -> Self {
Self {
left_angle: 0.5,
right_angle: -0.5,
}
}
}
vastu-map/src/config/slam.rs
+32
View File
@@ -0,0 +1,32 @@
//! SLAM configuration section.
use serde::{Deserialize, Serialize};
use crate::grid::LogOddsConfig;
use crate::slam::{
BackgroundOptimizerConfig, CorrelativeMatcherConfig, LoopClosureConfig, PoseGraphConfig,
};
/// SLAM configuration section (Cartographer-style)
#[derive(Clone, Debug, Serialize, Deserialize, Default)]
pub struct SlamSection {
/// Log-odds occupancy model configuration
#[serde(default)]
pub log_odds: LogOddsConfig,
/// Correlative scan matcher configuration
#[serde(default)]
pub correlative: CorrelativeMatcherConfig,
/// Loop closure detection configuration
#[serde(default)]
pub loop_closure: LoopClosureConfig,
/// Pose graph optimization configuration
#[serde(default)]
pub pose_graph: PoseGraphConfig,
/// Background optimizer configuration
#[serde(default)]
pub background_optimizer: BackgroundOptimizerConfig,
}
vastu-map/src/config/vastu.rs
+207
View File
@@ -0,0 +1,207 @@
//! Main VastuConfig and conversion methods.
use std::path::Path;
use serde::{Deserialize, Serialize};
use crate::explore::{ExplorationConfig, ExplorerConfig};
use crate::grid::MapConfig;
use crate::pathfinding::{AStarConfig, SmoothingConfig};
use crate::query::{FrontierDetector, RobotFootprint};
use crate::slam::{
BackgroundOptimizerConfig, CorrelativeMatcherConfig, LoopClosureConfig, PoseGraphConfig,
};
use super::error::ConfigLoadError;
use super::exploration::ExplorationSection;
use super::grid::GridSection;
use super::motion_filter::MotionFilterSection;
use super::pathfinding::PathfindingSection;
use super::persistence::PersistenceSection;
use super::query::QuerySection;
use super::sensor::SensorSection;
use super::slam::SlamSection;
/// Full VastuMap configuration loaded from YAML
#[derive(Clone, Debug, Serialize, Deserialize, Default)]
pub struct VastuConfig {
/// Grid settings
#[serde(default)]
pub grid: GridSection,
/// Sensor settings
#[serde(default)]
pub sensor: SensorSection,
/// SLAM settings (Cartographer-style defaults)
#[serde(default)]
pub slam: SlamSection,
/// Query settings
#[serde(default)]
pub query: QuerySection,
/// Pathfinding settings
#[serde(default)]
pub pathfinding: PathfindingSection,
/// Motion filter settings
#[serde(default)]
pub motion_filter: MotionFilterSection,
/// Exploration settings
#[serde(default)]
pub exploration: ExplorationSection,
/// Persistence settings
#[serde(default)]
pub persistence: PersistenceSection,
}
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(),
log_odds: self.slam.log_odds.clone(),
}
}
/// Get the correlative matcher config
pub fn correlative_matcher_config(&self) -> CorrelativeMatcherConfig {
self.slam.correlative.clone()
}
/// Get the loop closure config
pub fn loop_closure_config(&self) -> LoopClosureConfig {
self.slam.loop_closure.clone()
}
/// Get the pose graph config
pub fn pose_graph_config(&self) -> PoseGraphConfig {
self.slam.pose_graph.clone()
}
/// Get the background optimizer config
pub fn background_optimizer_config(&self) -> BackgroundOptimizerConfig {
self.slam.background_optimizer.clone()
}
/// 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,
// Use defaults for new fields
..Default::default()
}
}
/// 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(),
motion_filter: self.motion_filter.to_motion_filter_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,
}
}
}
#[cfg(test)]
mod tests {
use super::*;
#[test]
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_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_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);
}
}
vastu-map/src/core/mod.rs
+2
View File
@@ -4,6 +4,7 @@
//! - [`Cell`] and [`CellType`]: Grid cell representation with semantic types
//! - [`GridCoord`] and [`WorldPoint`]: Coordinate types
//! - [`Pose2D`]: Robot pose (position + orientation)
//! - [`PointCloud`]: SIMD-optimized 2D point cloud (SoA layout)
//! - Sensor types: [`LidarScan`], [`CliffSensors`], [`BumperSensors`]
mod cell;
@@ -17,3 +18,4 @@ 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};
pub use simd::{GridCoordBatch, PointCloud, RotationMatrix4};
vastu-map/src/core/simd.rs
+169 -110
View File
@@ -1,85 +1,84 @@
//! 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.
//! optimized for ARM NEON (f32x4) and x86 SSE/AVX. Uses `std::simd` which
//! provides automatic scalar fallback on platforms without native SIMD.
//!
//! ## Key Types
//!
//! - [`PointBatch`]: Structure-of-Arrays storage for 2D points
//! - [`PointCloud`]: Structure-of-Arrays storage for 2D points (SIMD-optimized)
//! - [`RotationMatrix4`]: Pre-computed rotation matrix for SIMD transforms
//!
//! ## Usage
//!
//! ```rust,ignore
//! use vastu_map::core::simd::{PointBatch, transform_points_simd4, RotationMatrix4};
//! use vastu_map::core::simd::{PointCloud, transform_points, RotationMatrix4};
//!
//! let points = PointBatch::from_tuples(&[(1.0, 0.0), (0.0, 1.0), (1.0, 1.0), (2.0, 0.0)]);
//! // Use padded constructors to ensure SIMD alignment
//! let points = PointCloud::from_tuples_padded(&[(1.0, 0.0), (0.0, 1.0), (1.0, 1.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);
//! let result = transform_points(&points, &pose);
//! ```
//!
//! ## SIMD Requirements
//!
//! All batch operations require inputs to be padded to multiples of [`LANES`] (4).
//! Use the `*_padded` constructors to ensure proper alignment.
use std::simd::{StdFloat, cmp::SimdPartialOrd, f32x4, i32x4, num::SimdFloat};
use super::Pose2D;
use super::{LidarScan, 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.
/// 2D point cloud in Structure-of-Arrays (SoA) layout for SIMD operations.
///
/// This layout is optimal for SIMD operations where x and y coordinates
/// are processed independently.
/// are processed independently. All SIMD operations require the length
/// to be a multiple of [`LANES`] (4) - use `*_padded` constructors.
///
/// ## Memory Layout
/// ```text
/// xs: [x0, x1, x2, x3, x4, ...]
/// ys: [y0, y1, y2, y3, y4, ...]
/// ```
///
/// ## SIMD Alignment
///
/// For optimal performance and correctness, always use:
/// - [`PointCloud::from_tuples_padded`] instead of `from_tuples`
/// - [`PointCloud::from_scan_padded`] instead of `from_scan`
#[derive(Clone, Debug, Default)]
pub struct PointBatch {
pub struct PointCloud {
/// X coordinates
pub xs: Vec<f32>,
/// Y coordinates
pub ys: Vec<f32>,
}
impl PointBatch {
/// Create an empty batch.
impl PointCloud {
/// Create an empty point cloud.
#[inline]
pub fn new() -> Self {
Self::default()
}
/// Create with specified capacity.
/// Create with specified capacity (rounded up to multiple of LANES).
#[inline]
pub fn with_capacity(capacity: usize) -> Self {
let padded = capacity.div_ceil(LANES) * LANES;
Self {
xs: Vec::with_capacity(capacity),
ys: Vec::with_capacity(capacity),
xs: Vec::with_capacity(padded),
ys: Vec::with_capacity(padded),
}
}
/// 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.
/// Create from slice of (x, y) tuples with padding to LANES multiple.
///
/// Padded values are zero (neutral for transforms).
/// This eliminates remainder handling in SIMD loops.
pub fn from_tuples_padded(points: &[(f32, f32)]) -> Self {
pub fn from_tuples(points: &[(f32, f32)]) -> Self {
let n = points.len();
let padded_len = n.div_ceil(LANES) * LANES;
let mut xs = vec![0.0; padded_len];
@@ -91,14 +90,32 @@ impl PointBatch {
Self { xs, ys }
}
/// Push a point.
/// Alias for `from_tuples` - all constructors auto-pad.
#[inline]
pub fn from_tuples_padded(points: &[(f32, f32)]) -> Self {
Self::from_tuples(points)
}
/// Push a point and maintain LANES alignment.
///
/// Note: For bulk operations, prefer constructing a new PointCloud.
#[inline]
pub fn push(&mut self, x: f32, y: f32) {
self.xs.push(x);
self.ys.push(y);
}
/// Number of points.
/// Ensure length is padded to multiple of LANES.
pub fn pad_to_lanes(&mut self) {
let n = self.xs.len();
let padded_len = n.div_ceil(LANES) * LANES;
if padded_len > n {
self.xs.resize(padded_len, 0.0);
self.ys.resize(padded_len, 0.0);
}
}
/// Number of points (including padding).
#[inline]
pub fn len(&self) -> usize {
self.xs.len()
@@ -117,13 +134,54 @@ impl PointBatch {
self.ys.clear();
}
/// Original (unpadded) length for iteration.
/// Check if length is SIMD-aligned (multiple of LANES).
#[inline]
pub fn is_aligned(&self) -> bool {
self.xs.len().is_multiple_of(LANES)
}
/// Create from a LidarScan with automatic padding.
///
/// 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()
/// Points are stored as (x, y) in sensor-local coordinates:
/// - x = range * cos(angle)
/// - y = range * sin(angle)
pub fn from_scan(scan: &LidarScan) -> Self {
let valid_count = scan.valid_points().count();
let padded_len = valid_count.div_ceil(LANES) * LANES;
let mut xs = vec![0.0; padded_len];
let mut ys = vec![0.0; padded_len];
for (i, (angle, range)) in scan.valid_points().enumerate() {
xs[i] = range * angle.cos();
ys[i] = range * angle.sin();
}
Self { xs, ys }
}
/// Alias for `from_scan` - all constructors auto-pad.
#[inline]
pub fn from_scan_padded(scan: &LidarScan) -> Self {
Self::from_scan(scan)
}
/// Create from a LidarScan with subsampling and padding.
///
/// If max_points > 0 and scan has more points, subsample to reduce computation.
pub fn from_scan_subsampled(scan: &LidarScan, max_points: usize) -> Self {
let mut cloud = Self::from_scan(scan);
if max_points > 0 && cloud.len() > max_points {
let step = cloud.len() / max_points;
let new_xs: Vec<f32> = cloud.xs.iter().step_by(step).copied().collect();
let new_ys: Vec<f32> = cloud.ys.iter().step_by(step).copied().collect();
cloud.xs = new_xs;
cloud.ys = new_ys;
cloud.pad_to_lanes();
}
cloud
}
}
@@ -211,6 +269,7 @@ impl RotationMatrix4 {
/// Transform points from sensor frame to world frame using SIMD.
///
/// Processes 4 points per iteration using ARM NEON / SSE instructions.
/// On platforms without native SIMD, `std::simd` provides automatic scalar fallback.
///
/// # Algorithm
/// ```text
@@ -219,26 +278,29 @@ impl RotationMatrix4 {
/// ```
///
/// # Arguments
/// * `points` - Input points in sensor frame (SoA layout)
/// * `points` - Input points in sensor frame (must be LANES-aligned)
/// * `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)
///
/// # Panics
/// Debug-asserts if `points.len()` is not a multiple of [`LANES`].
#[inline]
pub fn transform_points_simd4(
points: &PointBatch,
points: &PointCloud,
rot: &RotationMatrix4,
out_xs: &mut [f32],
out_ys: &mut [f32],
) {
debug_assert_eq!(points.xs.len(), points.ys.len());
debug_assert!(points.is_aligned(), "PointCloud must be LANES-aligned");
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) {
// Pure SIMD path: process 4 points per iteration
for i in (0..n).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..]);
@@ -252,25 +314,12 @@ pub fn transform_points_simd4(
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 {
/// Returns a new `PointCloud` with the transformed points.
pub fn transform_points(points: &PointCloud, pose: &Pose2D) -> PointCloud {
let n = points.xs.len();
let mut out_xs = vec![0.0; n];
let mut out_ys = vec![0.0; n];
@@ -278,7 +327,7 @@ pub fn transform_points(points: &PointBatch, pose: &Pose2D) -> PointBatch {
let rot = RotationMatrix4::from_pose(pose);
transform_points_simd4(points, &rot, &mut out_xs, &mut out_ys);
PointBatch {
PointCloud {
xs: out_xs,
ys: out_ys,
}
@@ -289,13 +338,16 @@ pub fn transform_points(points: &PointBatch, pose: &Pose2D) -> PointBatch {
/// Performs: `grid_coord = floor((world - origin) / resolution)`
///
/// # Arguments
/// * `world_xs` - World X coordinates
/// * `world_ys` - World Y coordinates
/// * `world_xs` - World X coordinates (must be LANES-aligned length)
/// * `world_ys` - World Y coordinates (must be LANES-aligned length)
/// * `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
///
/// # Panics
/// Debug-asserts if input length is not a multiple of [`LANES`].
#[inline]
pub fn world_to_grid_simd4(
world_xs: &[f32],
@@ -307,13 +359,17 @@ pub fn world_to_grid_simd4(
out_ys: &mut [i32],
) {
let n = world_xs.len();
let simd_end = n - (n % LANES);
debug_assert!(
n.is_multiple_of(LANES),
"Input length must be LANES-aligned"
);
debug_assert_eq!(world_xs.len(), world_ys.len());
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) {
for i in (0..n).step_by(LANES) {
let wx = f32x4::from_slice(&world_xs[i..]);
let wy = f32x4::from_slice(&world_ys[i..]);
@@ -331,17 +387,14 @@ pub fn world_to_grid_simd4(
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).
///
/// # Panics
/// Debug-asserts if input length is not a multiple of [`LANES`].
#[inline]
pub fn distance_squared_simd4(
xs: &[f32],
@@ -351,12 +404,16 @@ pub fn distance_squared_simd4(
out: &mut [f32],
) {
let n = xs.len();
let simd_end = n - (n % LANES);
debug_assert!(
n.is_multiple_of(LANES),
"Input length must be LANES-aligned"
);
debug_assert_eq!(xs.len(), ys.len());
let ox = f32x4::splat(origin_x);
let oy = f32x4::splat(origin_y);
for i in (0..simd_end).step_by(LANES) {
for i in (0..n).step_by(LANES) {
let px = f32x4::from_slice(&xs[i..]);
let py = f32x4::from_slice(&ys[i..]);
@@ -367,38 +424,37 @@ pub fn distance_squared_simd4(
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.
///
/// # Panics
/// Debug-asserts if input length is not a multiple of [`LANES`].
#[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);
debug_assert!(
n.is_multiple_of(LANES),
"Input length must be LANES-aligned"
);
for i in (0..simd_end).step_by(LANES) {
for i in (0..n).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.
///
/// # Panics
/// Debug-asserts if input length is not a multiple of [`LANES`].
#[inline]
pub fn is_valid_coords_simd4(
grid_xs: &[i32],
@@ -408,13 +464,17 @@ pub fn is_valid_coords_simd4(
out_valid: &mut [bool],
) {
let n = grid_xs.len();
let simd_end = n - (n % LANES);
debug_assert!(
n.is_multiple_of(LANES),
"Input length must be LANES-aligned"
);
debug_assert_eq!(grid_xs.len(), grid_ys.len());
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) {
for i in (0..n).step_by(LANES) {
let gx = i32x4::from_slice(&grid_xs[i..]);
let gy = i32x4::from_slice(&grid_ys[i..]);
@@ -431,13 +491,6 @@ pub fn is_valid_coords_simd4(
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)]
@@ -446,23 +499,26 @@ mod tests {
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);
fn test_point_cloud_creation() {
// All constructors now auto-pad to LANES
let points = PointCloud::from_tuples(&[(1.0, 2.0), (3.0, 4.0), (5.0, 6.0)]);
assert_eq!(points.len(), 4); // Padded to 4
assert!(points.is_aligned());
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_point_cloud_exact_lanes() {
let points = PointCloud::from_tuples(&[(1.0, 2.0), (3.0, 4.0), (5.0, 6.0), (7.0, 8.0)]);
assert_eq!(points.len(), 4); // Exactly 4, no extra padding
assert!(points.is_aligned());
}
#[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 points = PointCloud::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);
@@ -475,7 +531,7 @@ mod tests {
#[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 points = PointCloud::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);
@@ -488,7 +544,7 @@ mod tests {
#[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 points = PointCloud::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);
@@ -503,18 +559,21 @@ mod tests {
}
#[test]
fn test_transform_matches_scalar() {
fn test_transform_with_padding() {
// Test with 5 points (will be padded to 8)
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)]);
PointCloud::from_tuples(&[(1.0, 2.0), (3.0, 4.0), (5.0, 6.0), (7.0, 8.0), (9.0, 10.0)]);
assert_eq!(points.len(), 8); // Padded to 8
let pose = Pose2D::new(1.0, 2.0, 0.5);
let result = transform_points(&points, &pose);
// Compare with scalar transform
// Compare with expected transform
let cos = pose.theta.cos();
let sin = pose.theta.sin();
for i in 0..points.len() {
// Only check the first 5 (non-padding) points
for i in 0..5 {
let px = points.xs[i];
let py = points.ys[i];
let expected_x = pose.x + px * cos - py * sin;
vastu-map/src/exploration/config.rs
-70
View File
@@ -1,70 +0,0 @@
//! Exploration controller configuration.
use serde::{Deserialize, Serialize};
use crate::query::RobotFootprint;
/// Exploration controller configuration
#[derive(Clone, Debug, Serialize, Deserialize)]
pub struct ExplorationConfig {
/// Robot footprint for collision checking
pub footprint: RobotFootprint,
/// Minimum frontier size to consider (cells)
pub min_frontier_size: usize,
/// Distance threshold to consider frontier reached (meters)
pub frontier_reached_threshold: f32,
/// Distance threshold for waypoint reached (meters)
pub waypoint_threshold: f32,
/// Maximum distance to plan at once (meters)
pub max_plan_distance: f32,
/// Replan interval when path is blocked (seconds)
pub replan_interval: f32,
/// Maximum consecutive failures before giving up
pub max_consecutive_failures: usize,
/// Clearance weight for path planning (higher = prefer more clearance)
pub path_clearance_weight: f32,
/// Enable path smoothing
pub smooth_paths: bool,
/// Recovery turn angle (radians) when stuck
pub recovery_turn_angle: f32,
/// Recovery backup distance (meters) when stuck
pub recovery_backup_distance: f32,
}
impl Default for ExplorationConfig {
fn default() -> Self {
Self {
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 with custom robot footprint
pub fn with_footprint(footprint: RobotFootprint) -> Self {
Self {
footprint,
..Default::default()
}
}
}
vastu-map/src/explore/config.rs
+10 -1
View File
@@ -2,11 +2,13 @@
use serde::{Deserialize, Serialize};
use crate::exploration::ExplorationConfig;
use super::controller::ExplorationConfig;
use crate::grid::{GridConfig, MapConfig, SensorConfig};
use crate::slam::CorrelativeMatcherConfig;
use crate::slam::loop_closure::{LoopClosureConfig, PoseGraphConfig};
use super::motion_filter::MotionFilterConfig;
/// Configuration for the VastuExplorer.
///
/// This combines all the configuration needed for autonomous exploration:
@@ -41,6 +43,11 @@ pub struct ExplorerConfig {
#[serde(default)]
pub pose_graph: PoseGraphConfig,
/// Motion filter configuration.
/// Gates scan processing based on robot movement.
#[serde(default)]
pub motion_filter: MotionFilterConfig,
/// Control loop update rate in Hz.
#[serde(default = "default_update_rate")]
pub update_rate_hz: f32,
@@ -71,6 +78,7 @@ impl Default for ExplorerConfig {
matching: CorrelativeMatcherConfig::default(),
loop_closure: LoopClosureConfig::default(),
pose_graph: PoseGraphConfig::default(),
motion_filter: MotionFilterConfig::default(),
update_rate_hz: default_update_rate(),
max_time_secs: 0.0,
min_battery_percent: default_min_battery(),
@@ -102,6 +110,7 @@ impl ExplorerConfig {
MapConfig {
grid: self.grid.clone(),
sensor: self.sensor.clone(),
log_odds: Default::default(),
}
}
}
vastu-map/src/explore/controller/config.rs
+180
View File
@@ -0,0 +1,180 @@
//! Exploration controller configuration.
use serde::{Deserialize, Serialize};
use crate::query::RobotFootprint;
/// Exploration controller configuration
#[derive(Clone, Debug, Serialize, Deserialize)]
pub struct ExplorationConfig {
/// Robot footprint for collision checking.
#[serde(default)]
pub footprint: RobotFootprint,
/// Minimum frontier size to consider (cells).
#[serde(default = "default_min_frontier_size")]
pub min_frontier_size: usize,
/// Distance threshold to consider frontier reached (meters).
#[serde(default = "default_frontier_reached_threshold")]
pub frontier_reached_threshold: f32,
/// Distance threshold for waypoint reached (meters).
#[serde(default = "default_waypoint_threshold")]
pub waypoint_threshold: f32,
/// Maximum distance to plan at once (meters).
#[serde(default = "default_max_plan_distance")]
pub max_plan_distance: f32,
/// Replan interval when path is blocked (seconds).
#[serde(default = "default_replan_interval")]
pub replan_interval: f32,
/// Maximum consecutive failures before giving up.
#[serde(default = "default_max_consecutive_failures")]
pub max_consecutive_failures: usize,
/// Clearance weight for path planning (higher = prefer more clearance).
#[serde(default)]
pub path_clearance_weight: f32,
/// Enable path smoothing.
#[serde(default = "default_smooth_paths")]
pub smooth_paths: bool,
/// Recovery turn angle (radians) when stuck.
#[serde(default = "default_recovery_turn_angle")]
pub recovery_turn_angle: f32,
/// Recovery backup distance (meters) when stuck.
#[serde(default = "default_recovery_backup_distance")]
pub recovery_backup_distance: f32,
/// Maximum linear speed during navigation (m/s).
#[serde(default = "default_navigation_max_speed")]
pub navigation_max_speed: f32,
/// Maximum linear speed during recovery (m/s).
#[serde(default = "default_recovery_max_speed")]
pub recovery_max_speed: f32,
/// Angular speed during scanning rotation (rad/s).
#[serde(default = "default_scan_angular_speed")]
pub scan_angular_speed: f32,
/// Rotation step size during scanning (radians per update).
#[serde(default = "default_scan_rotation_step")]
pub scan_rotation_step: f32,
/// Distance tolerance for matching recently failed frontiers (meters).
#[serde(default = "default_frontier_blacklist_tolerance")]
pub frontier_blacklist_tolerance: f32,
/// Maximum number of steps without progress before declaring stuck.
#[serde(default = "default_max_stuck_steps")]
pub max_stuck_steps: usize,
/// Minimum progress distance to reset stuck counter (meters).
#[serde(default = "default_min_progress_distance")]
pub min_progress_distance: f32,
}
fn default_min_frontier_size() -> usize {
5
}
fn default_frontier_reached_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_consecutive_failures() -> usize {
5
}
fn default_smooth_paths() -> bool {
true
}
fn default_recovery_turn_angle() -> f32 {
std::f32::consts::FRAC_PI_4 // 45 degrees
}
fn default_recovery_backup_distance() -> f32 {
0.1
}
fn default_navigation_max_speed() -> f32 {
0.3 // 30 cm/s
}
fn default_recovery_max_speed() -> f32 {
0.1 // 10 cm/s
}
fn default_scan_angular_speed() -> f32 {
0.5 // rad/s
}
fn default_scan_rotation_step() -> f32 {
0.1 // ~6 degrees per step
}
fn default_frontier_blacklist_tolerance() -> f32 {
0.5 // 50 cm
}
fn default_max_stuck_steps() -> usize {
50 // ~5 seconds at 10Hz
}
fn default_min_progress_distance() -> f32 {
0.05 // 5 cm
}
impl Default for ExplorationConfig {
fn default() -> Self {
Self {
footprint: RobotFootprint::default(),
min_frontier_size: default_min_frontier_size(),
frontier_reached_threshold: default_frontier_reached_threshold(),
waypoint_threshold: default_waypoint_threshold(),
max_plan_distance: default_max_plan_distance(),
replan_interval: default_replan_interval(),
max_consecutive_failures: default_max_consecutive_failures(),
path_clearance_weight: 0.0,
smooth_paths: default_smooth_paths(),
recovery_turn_angle: default_recovery_turn_angle(),
recovery_backup_distance: default_recovery_backup_distance(),
navigation_max_speed: default_navigation_max_speed(),
recovery_max_speed: default_recovery_max_speed(),
scan_angular_speed: default_scan_angular_speed(),
scan_rotation_step: default_scan_rotation_step(),
frontier_blacklist_tolerance: default_frontier_blacklist_tolerance(),
max_stuck_steps: default_max_stuck_steps(),
min_progress_distance: default_min_progress_distance(),
}
}
}
impl ExplorationConfig {
/// Create with custom robot footprint
pub fn with_footprint(footprint: RobotFootprint) -> Self {
Self {
footprint,
..Default::default()
}
}
}
vastu-map/src/exploration/controller.rs → vastu-map/src/explore/controller/machine.rs
+28 -25
View File
@@ -140,7 +140,10 @@ impl ExplorationController {
}
}
/// Handle an external event
/// Handle an external event.
///
/// Events are used to notify the controller of external conditions
/// that should trigger state transitions.
pub fn handle_event(&mut self, event: ExplorationEvent) {
match event {
ExplorationEvent::Start => self.start(),
@@ -158,7 +161,6 @@ impl ExplorationController {
ExplorationEvent::ObstacleDetected | ExplorationEvent::PathBlocked => {
self.enter_recovery();
}
_ => {}
}
}
@@ -248,10 +250,9 @@ impl ExplorationController {
/// Check if a frontier centroid is in the recently failed list
fn is_recently_failed_frontier(&self, centroid: &WorldPoint) -> bool {
const MATCH_THRESHOLD: f32 = 0.5; // 50cm tolerance
self.failed_frontier_centroids
.iter()
.any(|failed| centroid.distance(failed) < MATCH_THRESHOLD)
.any(|failed| centroid.distance(failed) < self.config.frontier_blacklist_tolerance)
}
fn plan_path(
@@ -600,7 +601,7 @@ impl ExplorationController {
};
return ExplorationCommand::MoveTo {
target: next_target,
max_speed: 0.3, // 30 cm/s default
max_speed: self.config.navigation_max_speed,
};
} else {
// Reached end of path - transition to Scanning
@@ -630,17 +631,16 @@ impl ExplorationController {
if angle_error < 0.5 {
// ~30 degrees
let progress = pose.position().distance(&self.stuck_check_position);
if progress > 0.05 {
// Made meaningful progress (>5cm) - reset stuck counter
if progress > self.config.min_progress_distance {
// Made meaningful progress - reset stuck counter
self.steps_without_progress = 0;
self.stuck_check_position = pose.position();
} else {
self.steps_without_progress += 1;
}
// If stuck for too long (~5 seconds at 10Hz), trigger recovery
const MAX_STUCK_STEPS: usize = 50;
if self.steps_without_progress > MAX_STUCK_STEPS {
// If stuck for too long, trigger recovery
if self.steps_without_progress > self.config.max_stuck_steps {
warn!(
"[Explore] Stuck detected! {} steps without progress at ({:.2},{:.2})",
self.steps_without_progress, pose.x, pose.y
@@ -659,7 +659,7 @@ impl ExplorationController {
// Continue to current waypoint
ExplorationCommand::MoveTo {
target: waypoint,
max_speed: 0.3,
max_speed: self.config.navigation_max_speed,
}
}
@@ -669,11 +669,9 @@ impl ExplorationController {
rotation_remaining: f32,
pose: Pose2D,
) -> ExplorationCommand {
// Rotate to scan the area - each update rotates by a fixed amount based on angular speed
// Assuming ~0.5 rad/s angular speed and ~0.05s update rate = 0.025 rad per update
const ROTATION_PER_STEP: f32 = 0.1; // ~6 degrees per step
let new_remaining = rotation_remaining - ROTATION_PER_STEP;
// Rotate to scan the area - each update rotates by a fixed amount
let rotation_step = self.config.scan_rotation_step;
let new_remaining = rotation_remaining - rotation_step;
if new_remaining <= 0.0 {
// Scan complete - frontier successfully explored
@@ -693,25 +691,30 @@ impl ExplorationController {
rotation_remaining: new_remaining,
};
ExplorationCommand::Rotate {
target_heading: pose.theta + ROTATION_PER_STEP,
max_angular_speed: 0.5,
target_heading: pose.theta + rotation_step,
max_angular_speed: self.config.scan_angular_speed,
}
}
}
fn handle_recovery(&mut self, action: RecoveryAction, attempts: usize) -> ExplorationCommand {
const MAX_RECOVERY_ATTEMPTS: usize = 3;
debug!(
"[Explore] Recovery: {:?}, attempt {}/3",
"[Explore] Recovery: {:?}, attempt {}/{}",
action,
attempts + 1
attempts + 1,
MAX_RECOVERY_ATTEMPTS
);
if attempts >= 3 {
if attempts >= MAX_RECOVERY_ATTEMPTS {
// Too many recovery attempts
self.consecutive_failures += 1;
warn!(
"[Explore] Recovery exhausted after 3 attempts, failures={}/{}",
self.consecutive_failures, self.config.max_consecutive_failures
"[Explore] Recovery exhausted after {} attempts, failures={}/{}",
MAX_RECOVERY_ATTEMPTS,
self.consecutive_failures,
self.config.max_consecutive_failures
);
if self.consecutive_failures >= self.config.max_consecutive_failures {
self.state = ExplorationState::Failed {
@@ -735,7 +738,7 @@ impl ExplorationController {
self.last_pose.x - distance * self.last_pose.theta.cos(),
self.last_pose.y - distance * self.last_pose.theta.sin(),
),
max_speed: 0.1,
max_speed: self.config.recovery_max_speed,
}
}
RecoveryAction::TurnInPlace(angle) => {
@@ -743,7 +746,7 @@ impl ExplorationController {
self.state = ExplorationState::SearchingFrontiers;
ExplorationCommand::Rotate {
target_heading: self.last_pose.theta + angle,
max_angular_speed: 0.5,
max_angular_speed: self.config.scan_angular_speed,
}
}
RecoveryAction::Wait => {
vastu-map/src/exploration/mod.rs → vastu-map/src/explore/controller/mod.rs
+3 -3
View File
@@ -9,7 +9,7 @@
//! ## Usage
//!
//! ```rust,ignore
//! use vastu_map::exploration::{ExplorationController, ExplorationConfig};
//! use vastu_map::explore::{ExplorationController, ExplorationConfig};
//!
//! let config = ExplorationConfig::default();
//! let mut controller = ExplorationController::new(config);
@@ -44,9 +44,9 @@
//! 8. **Failed** - Too many errors
pub mod config;
pub mod controller;
mod machine;
pub mod state;
pub use config::ExplorationConfig;
pub use controller::{ExplorationController, ExplorationProgress};
pub use machine::{ExplorationController, ExplorationProgress};
pub use state::{ExplorationCommand, ExplorationEvent, ExplorationState, RecoveryAction};
vastu-map/src/exploration/state.rs → vastu-map/src/explore/controller/state.rs
+17 -34
View File
@@ -3,6 +3,8 @@
use crate::core::WorldPoint;
use crate::query::Frontier;
// Note: Frontier and WorldPoint are still used by ExplorationState and ExplorationCommand
/// Exploration state
#[derive(Clone, Debug)]
pub enum ExplorationState {
@@ -132,51 +134,32 @@ pub enum ExplorationCommand {
},
}
/// Event that can trigger state transitions
/// Event that can trigger state transitions.
///
/// These events are used to communicate external conditions to the exploration
/// controller, allowing it to respond appropriately (e.g., entering recovery
/// mode when an obstacle is detected).
#[derive(Clone, Debug)]
pub enum ExplorationEvent {
/// Start exploration
/// Start exploration.
Start,
/// Stop exploration
/// Stop exploration and return to Idle state.
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
/// Obstacle detected by sensors (cliff, bumper, etc.).
/// Triggers recovery behavior.
ObstacleDetected,
/// Path is blocked
/// Path is blocked and cannot continue.
/// Triggers recovery behavior.
PathBlocked,
/// Recovery completed
/// Recovery maneuver completed successfully.
/// Returns to frontier searching.
RecoveryComplete,
/// Recovery failed
RecoveryFailed,
/// Scan complete
/// Scan rotation completed at frontier.
/// Increments frontiers explored and searches for next.
ScanComplete,
/// Timeout
Timeout,
}
vastu-map/src/explore/error.rs
+186
View File
@@ -0,0 +1,186 @@
//! Exploration error types.
//!
//! Provides structured error types for exploration failures,
//! replacing String-based error messages.
use std::fmt;
use crate::core::WorldPoint;
/// Errors that can occur during autonomous exploration.
#[derive(Debug, Clone)]
pub enum ExplorationError {
/// Robot connection was lost during exploration.
ConnectionLost,
/// Battery level dropped below minimum threshold.
LowBattery {
/// Current battery percentage.
percent: u8,
/// Configured minimum threshold.
threshold: u8,
},
/// Exploration exceeded the configured time limit.
TimeoutExceeded {
/// Elapsed time in seconds.
elapsed_secs: f32,
/// Configured limit in seconds.
limit_secs: f32,
},
/// Too many consecutive planning/navigation failures.
TooManyFailures {
/// Number of consecutive failures.
count: usize,
/// Configured maximum.
max: usize,
},
/// Robot appears to be stuck and recovery failed.
StuckRecoveryFailed {
/// Position where robot got stuck.
position: WorldPoint,
/// Number of recovery attempts made.
attempts: usize,
},
/// Localization drift detected (robot position appears blocked).
LocalizationDrift {
/// Number of consecutive "start blocked" events.
consecutive_events: usize,
},
/// No valid frontiers found with insufficient exploration.
InsufficientExploration {
/// Explored floor area in square meters.
explored_area_m2: f32,
/// Number of frontiers successfully explored.
frontiers_explored: usize,
},
/// Custom error with message.
Other(String),
}
impl ExplorationError {
/// Create an error from a string message (for backwards compatibility).
pub fn from_message(msg: impl Into<String>) -> Self {
Self::Other(msg.into())
}
/// Get a short error code for logging/metrics.
pub fn code(&self) -> &'static str {
match self {
Self::ConnectionLost => "CONNECTION_LOST",
Self::LowBattery { .. } => "LOW_BATTERY",
Self::TimeoutExceeded { .. } => "TIMEOUT",
Self::TooManyFailures { .. } => "TOO_MANY_FAILURES",
Self::StuckRecoveryFailed { .. } => "STUCK",
Self::LocalizationDrift { .. } => "LOCALIZATION_DRIFT",
Self::InsufficientExploration { .. } => "INSUFFICIENT_EXPLORATION",
Self::Other(_) => "OTHER",
}
}
/// Check if this error is recoverable (exploration could potentially continue).
pub fn is_recoverable(&self) -> bool {
matches!(
self,
Self::StuckRecoveryFailed { .. } | Self::LocalizationDrift { .. }
)
}
}
impl fmt::Display for ExplorationError {
fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
match self {
Self::ConnectionLost => write!(f, "Robot connection lost"),
Self::LowBattery { percent, threshold } => {
write!(f, "Battery low: {}% (minimum: {}%)", percent, threshold)
}
Self::TimeoutExceeded {
elapsed_secs,
limit_secs,
} => {
write!(
f,
"Time limit exceeded: {:.1}s elapsed (limit: {:.1}s)",
elapsed_secs, limit_secs
)
}
Self::TooManyFailures { count, max } => {
write!(f, "Too many consecutive failures: {} (max: {})", count, max)
}
Self::StuckRecoveryFailed { position, attempts } => {
write!(
f,
"Stuck at ({:.2}, {:.2}) after {} recovery attempts",
position.x, position.y, attempts
)
}
Self::LocalizationDrift { consecutive_events } => {
write!(
f,
"Localization drift detected ({} consecutive blocked events)",
consecutive_events
)
}
Self::InsufficientExploration {
explored_area_m2,
frontiers_explored,
} => {
write!(
f,
"Insufficient exploration: {:.2}m² explored, {} frontiers done",
explored_area_m2, frontiers_explored
)
}
Self::Other(msg) => write!(f, "{}", msg),
}
}
}
impl std::error::Error for ExplorationError {}
/// Convert from String for backwards compatibility.
impl From<String> for ExplorationError {
fn from(s: String) -> Self {
Self::Other(s)
}
}
impl From<&str> for ExplorationError {
fn from(s: &str) -> Self {
Self::Other(s.to_string())
}
}
#[cfg(test)]
mod tests {
use super::*;
#[test]
fn test_error_display() {
let err = ExplorationError::LowBattery {
percent: 15,
threshold: 20,
};
assert_eq!(err.to_string(), "Battery low: 15% (minimum: 20%)");
}
#[test]
fn test_error_code() {
assert_eq!(ExplorationError::ConnectionLost.code(), "CONNECTION_LOST");
assert_eq!(
ExplorationError::TooManyFailures { count: 5, max: 5 }.code(),
"TOO_MANY_FAILURES"
);
}
#[test]
fn test_from_string() {
let err: ExplorationError = "Custom error".into();
assert!(matches!(err, ExplorationError::Other(_)));
}
}
vastu-map/src/explore/explorer.rs
+113 -91
View File
@@ -2,13 +2,20 @@
use std::time::Instant;
use super::controller::{
ExplorationCommand, ExplorationController, ExplorationEvent, ExplorationProgress,
};
use super::error::ExplorationError;
use super::velocity::{
VelocityConfig, compute_angular_velocity_to_heading, compute_velocity_to_target,
};
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::motion_filter::MotionFilter;
use super::source::SensorSource;
/// Result of a completed exploration.
@@ -19,11 +26,34 @@ pub enum ExploreResult {
Failed {
/// Partial map collected before failure.
map: OccupancyGridMap,
/// Reason for failure.
reason: String,
/// Error that caused the failure.
error: ExplorationError,
},
}
impl ExploreResult {
/// Check if exploration completed successfully.
pub fn is_complete(&self) -> bool {
matches!(self, Self::Complete(_))
}
/// Get the map regardless of success/failure.
pub fn into_map(self) -> OccupancyGridMap {
match self {
Self::Complete(map) => map,
Self::Failed { map, .. } => map,
}
}
/// Get the error if exploration failed.
pub fn error(&self) -> Option<&ExplorationError> {
match self {
Self::Complete(_) => None,
Self::Failed { error, .. } => Some(error),
}
}
}
/// Status returned from a single exploration step.
#[derive(Clone, Debug)]
pub enum ExploreStatus {
@@ -36,8 +66,8 @@ pub enum ExploreStatus {
Complete,
/// Exploration failed.
Failed {
/// Reason for failure.
reason: String,
/// Error that caused the failure.
error: ExplorationError,
},
}
@@ -75,6 +105,10 @@ pub struct VastuExplorer {
loop_detector: LoopClosureDetector,
/// Pose graph for optimization.
pose_graph: PoseGraph,
/// Motion filter for gating scan processing.
motion_filter: MotionFilter,
/// Velocity computation configuration.
velocity_config: VelocityConfig,
/// Last corrected pose.
last_pose: Pose2D,
/// Last odometry pose (for delta calculation).
@@ -95,6 +129,8 @@ impl VastuExplorer {
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());
let motion_filter = MotionFilter::new(config.motion_filter.clone());
let velocity_config = VelocityConfig::default();
Self {
config,
@@ -103,6 +139,8 @@ impl VastuExplorer {
matcher,
loop_detector,
pose_graph,
motion_filter,
velocity_config,
last_pose: Pose2D::default(),
last_odom_pose: None,
start_time: None,
@@ -150,7 +188,7 @@ impl VastuExplorer {
///
/// # Returns
/// `ExploreResult::Complete` with the finished map, or
/// `ExploreResult::Failed` with a partial map and reason.
/// `ExploreResult::Failed` with a partial map and error.
pub fn explore<S: SensorSource>(mut self, source: &mut S) -> ExploreResult {
// Start exploration
self.controller.start();
@@ -168,7 +206,7 @@ impl VastuExplorer {
source.stop();
return ExploreResult::Failed {
map: self.map,
reason: "Robot connection lost".to_string(),
error: ExplorationError::ConnectionLost,
};
}
@@ -179,16 +217,23 @@ impl VastuExplorer {
source.stop();
return ExploreResult::Failed {
map: self.map,
reason: format!("Battery low: {}%", battery),
error: ExplorationError::LowBattery {
percent: battery,
threshold: self.config.min_battery_percent,
},
};
}
// Check time limit
if self.config.max_time_secs > 0.0 && self.elapsed_secs() > self.config.max_time_secs {
let elapsed = self.elapsed_secs();
if self.config.max_time_secs > 0.0 && elapsed > self.config.max_time_secs {
source.stop();
return ExploreResult::Failed {
map: self.map,
reason: "Time limit exceeded".to_string(),
error: ExplorationError::TimeoutExceeded {
elapsed_secs: elapsed,
limit_secs: self.config.max_time_secs,
},
};
}
@@ -201,11 +246,11 @@ impl VastuExplorer {
source.stop();
return ExploreResult::Complete(self.map);
}
ExploreStatus::Failed { reason } => {
ExploreStatus::Failed { error } => {
source.stop();
return ExploreResult::Failed {
map: self.map,
reason,
error,
};
}
}
@@ -252,34 +297,44 @@ impl VastuExplorer {
});
self.last_odom_pose = Some(odom_pose);
// Process lidar scan if available
// Process lidar scan if available and motion filter allows
if let Some(scan) = source.get_lidar_scan() {
// Scan matching
let match_result = self
.matcher
.match_scan(&scan, odom_pose, self.map.storage());
// Check motion filter - only process if sufficient motion occurred
if self.motion_filter.should_process(odom_pose) {
// 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
};
// 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);
// Integrate scan into map
self.map.observe_lidar(&scan, corrected_pose);
// Update pose graph
self.pose_graph.add_pose(corrected_pose, odom_delta);
// 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();
// 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 {
// Scan filtered out - update pose from odometry delta only
self.last_pose = Pose2D::new(
self.last_pose.x + delta.x,
self.last_pose.y + delta.y,
self.last_pose.theta + delta.theta,
);
}
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
@@ -293,11 +348,11 @@ impl VastuExplorer {
// Handle cliff/bumper events
if source.is_cliff_detected() {
self.controller
.handle_event(crate::exploration::ExplorationEvent::ObstacleDetected);
.handle_event(ExplorationEvent::ObstacleDetected);
}
if source.is_bumper_triggered() {
self.controller
.handle_event(crate::exploration::ExplorationEvent::ObstacleDetected);
.handle_event(ExplorationEvent::ObstacleDetected);
}
// Update exploration controller
@@ -306,18 +361,23 @@ impl VastuExplorer {
// Execute command
match command {
ExplorationCommand::MoveTo { target, max_speed } => {
let (linear, angular) =
self.compute_velocity(self.last_pose, target.x, target.y, max_speed);
let (linear, angular) = compute_velocity_to_target(
self.last_pose,
target,
max_speed,
&self.velocity_config,
);
source.send_velocity(linear, angular);
}
ExplorationCommand::Rotate {
target_heading,
max_angular_speed,
} => {
let angular = self.compute_angular_velocity(
let angular = compute_angular_velocity_to_heading(
self.last_pose.theta,
target_heading,
max_angular_speed,
&self.velocity_config,
);
source.send_velocity(0.0, angular);
}
@@ -330,7 +390,9 @@ impl VastuExplorer {
}
ExplorationCommand::ExplorationFailed { reason } => {
source.stop();
return ExploreStatus::Failed { reason };
return ExploreStatus::Failed {
error: ExplorationError::from_message(reason),
};
}
ExplorationCommand::None => {
// Continue with current velocity or stop
@@ -342,48 +404,6 @@ impl VastuExplorer {
}
}
/// 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();
@@ -391,27 +411,29 @@ impl VastuExplorer {
self.controller = ExplorationController::new(self.config.exploration.clone());
self.loop_detector.reset();
self.pose_graph.reset();
self.motion_filter.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;
/// Get motion filter statistics.
///
/// Returns (passed_count, filtered_count, efficiency).
pub fn motion_filter_stats(&self) -> (u64, u64, f32) {
(
self.motion_filter.passed_count(),
self.motion_filter.filtered_count(),
self.motion_filter.filter_efficiency(),
)
}
a
}
#[cfg(test)]
mod tests {
use super::*;
use crate::core::normalize_angle;
#[test]
fn test_explorer_creation() {
vastu-map/src/explore/mod.rs
+16 -1
View File
@@ -30,11 +30,26 @@
//! ```
mod config;
pub mod controller;
mod error;
mod explorer;
mod motion_filter;
mod source;
mod submap_explorer;
mod velocity;
pub use config::ExplorerConfig;
pub use explorer::{ExploreResult, ExploreStatus, VastuExplorer, normalize_angle};
pub use error::ExplorationError;
pub use explorer::{ExploreResult, ExploreStatus, VastuExplorer};
pub use motion_filter::{MotionFilter, MotionFilterConfig};
pub use source::SensorSource;
pub use submap_explorer::{SubmapExplorerConfig, VastuSubmapExplorer};
pub use velocity::{
VelocityConfig, compute_angular_velocity_to_heading, compute_velocity_to_target,
};
// Re-export controller types for unified API
pub use controller::{
ExplorationCommand, ExplorationConfig, ExplorationController, ExplorationEvent,
ExplorationProgress, ExplorationState, RecoveryAction,
};
vastu-map/src/explore/motion_filter.rs
+349
View File
@@ -0,0 +1,349 @@
//! Motion filter for gating scan processing based on robot movement.
//!
//! The motion filter prevents redundant scan processing when the robot is stationary
//! or moving slowly. A scan is only processed if sufficient time, distance, or rotation
//! has occurred since the last processed scan.
//!
//! This improves CPU efficiency and ensures consistent submap sizes regardless of
//! robot speed.
use std::time::Instant;
use serde::{Deserialize, Serialize};
use crate::core::{Pose2D, normalize_angle};
/// Configuration for the motion filter.
///
/// A scan passes the filter (should be processed) if ANY threshold is exceeded:
/// - Time since last scan > max_time_secs
/// - Distance traveled > max_distance
/// - Rotation > max_angle
#[derive(Clone, Debug, Serialize, Deserialize)]
pub struct MotionFilterConfig {
/// Maximum time between scan insertions (seconds).
/// Ensures at least some updates even when stationary.
/// Default: 5.0s (from Google Cartographer)
#[serde(default = "default_max_time")]
pub max_time_secs: f32,
/// Maximum distance before forcing scan insertion (meters).
/// Default: 0.2m (20cm, from Google Cartographer)
#[serde(default = "default_max_distance")]
pub max_distance: f32,
/// Maximum rotation before forcing scan insertion (radians).
/// Default: 0.17 rad (~10°, from Google Cartographer)
#[serde(default = "default_max_angle")]
pub max_angle: f32,
/// Whether the motion filter is enabled.
/// When disabled, all scans pass through.
/// Default: true
#[serde(default = "default_enabled")]
pub enabled: bool,
}
fn default_max_time() -> f32 {
5.0
}
fn default_max_distance() -> f32 {
0.2
}
fn default_max_angle() -> f32 {
0.17
}
fn default_enabled() -> bool {
true
}
impl Default for MotionFilterConfig {
fn default() -> Self {
Self {
max_time_secs: default_max_time(),
max_distance: default_max_distance(),
max_angle: default_max_angle(),
enabled: default_enabled(),
}
}
}
impl MotionFilterConfig {
/// Create a permissive filter that processes more scans.
/// Useful for high-quality mapping at the cost of CPU.
pub fn permissive() -> Self {
Self {
max_time_secs: 2.0,
max_distance: 0.1,
max_angle: 0.08,
enabled: true,
}
}
/// Create a strict filter that processes fewer scans.
/// Useful for CPU-constrained systems.
pub fn strict() -> Self {
Self {
max_time_secs: 10.0,
max_distance: 0.3,
max_angle: 0.25,
enabled: true,
}
}
/// Create a disabled filter (all scans pass through).
pub fn disabled() -> Self {
Self {
enabled: false,
..Default::default()
}
}
}
/// Motion filter for gating scan processing.
///
/// The filter tracks the last processed pose and time, and determines
/// whether a new scan should be processed based on the configured thresholds.
pub struct MotionFilter {
/// Pose when last scan was processed
last_pose: Option<Pose2D>,
/// Time when last scan was processed
last_time: Option<Instant>,
/// Configuration
config: MotionFilterConfig,
/// Count of scans that passed the filter
passed_count: u64,
/// Count of scans that were filtered out
filtered_count: u64,
}
impl MotionFilter {
/// Create a new motion filter with the given configuration.
pub fn new(config: MotionFilterConfig) -> Self {
Self {
last_pose: None,
last_time: None,
config,
passed_count: 0,
filtered_count: 0,
}
}
/// Check if a scan at the given pose should be processed.
///
/// Returns true if the scan should be processed, false if it should be skipped.
/// Updates internal state when returning true.
pub fn should_process(&mut self, pose: Pose2D) -> bool {
// If disabled, always process
if !self.config.enabled {
self.passed_count += 1;
return true;
}
let now = Instant::now();
// First scan always passes
let (Some(last_pose), Some(last_time)) = (self.last_pose, self.last_time) else {
self.last_pose = Some(pose);
self.last_time = Some(now);
self.passed_count += 1;
return true;
};
// Check time threshold
let dt = now.duration_since(last_time).as_secs_f32();
if dt > self.config.max_time_secs {
self.last_pose = Some(pose);
self.last_time = Some(now);
self.passed_count += 1;
return true;
}
// Check distance threshold
let dx = pose.x - last_pose.x;
let dy = pose.y - last_pose.y;
let dist = (dx * dx + dy * dy).sqrt();
if dist > self.config.max_distance {
self.last_pose = Some(pose);
self.last_time = Some(now);
self.passed_count += 1;
return true;
}
// Check angle threshold
let dtheta = normalize_angle(pose.theta - last_pose.theta).abs();
if dtheta > self.config.max_angle {
self.last_pose = Some(pose);
self.last_time = Some(now);
self.passed_count += 1;
return true;
}
// All thresholds within limits, filter this scan
self.filtered_count += 1;
false
}
/// Reset the filter state.
///
/// Call this when starting a new exploration session.
pub fn reset(&mut self) {
self.last_pose = None;
self.last_time = None;
self.passed_count = 0;
self.filtered_count = 0;
}
/// Get the number of scans that passed the filter.
pub fn passed_count(&self) -> u64 {
self.passed_count
}
/// Get the number of scans that were filtered out.
pub fn filtered_count(&self) -> u64 {
self.filtered_count
}
/// Get the filter efficiency (fraction of scans filtered out).
pub fn filter_efficiency(&self) -> f32 {
let total = self.passed_count + self.filtered_count;
if total == 0 {
0.0
} else {
self.filtered_count as f32 / total as f32
}
}
/// Get the configuration.
pub fn config(&self) -> &MotionFilterConfig {
&self.config
}
}
#[cfg(test)]
mod tests {
use super::*;
use std::thread::sleep;
use std::time::Duration;
#[test]
fn test_first_scan_always_passes() {
let mut filter = MotionFilter::new(MotionFilterConfig::default());
let pose = Pose2D::new(0.0, 0.0, 0.0);
assert!(filter.should_process(pose));
assert_eq!(filter.passed_count(), 1);
assert_eq!(filter.filtered_count(), 0);
}
#[test]
fn test_stationary_robot_filtered() {
let mut filter = MotionFilter::new(MotionFilterConfig::default());
let pose = Pose2D::new(1.0, 2.0, 0.5);
// First scan passes
assert!(filter.should_process(pose));
// Same pose should be filtered
assert!(!filter.should_process(pose));
assert!(!filter.should_process(pose));
assert_eq!(filter.passed_count(), 1);
assert_eq!(filter.filtered_count(), 2);
}
#[test]
fn test_distance_threshold() {
let mut filter = MotionFilter::new(MotionFilterConfig::default());
// First scan
assert!(filter.should_process(Pose2D::new(0.0, 0.0, 0.0)));
// Small movement (< 0.2m) - filtered
assert!(!filter.should_process(Pose2D::new(0.1, 0.1, 0.0)));
// Large movement (> 0.2m) - passes
assert!(filter.should_process(Pose2D::new(0.5, 0.0, 0.0)));
}
#[test]
fn test_angle_threshold() {
let mut filter = MotionFilter::new(MotionFilterConfig::default());
// First scan
assert!(filter.should_process(Pose2D::new(0.0, 0.0, 0.0)));
// Small rotation (< 0.17 rad) - filtered
assert!(!filter.should_process(Pose2D::new(0.0, 0.0, 0.1)));
// Large rotation (> 0.17 rad) - passes
assert!(filter.should_process(Pose2D::new(0.0, 0.0, 0.5)));
}
#[test]
fn test_time_threshold() {
let config = MotionFilterConfig {
max_time_secs: 0.05, // 50ms for fast test
..Default::default()
};
let mut filter = MotionFilter::new(config);
// First scan
assert!(filter.should_process(Pose2D::new(0.0, 0.0, 0.0)));
// Same pose immediately - filtered
assert!(!filter.should_process(Pose2D::new(0.0, 0.0, 0.0)));
// Wait for time threshold
sleep(Duration::from_millis(60));
// Same pose after timeout - passes
assert!(filter.should_process(Pose2D::new(0.0, 0.0, 0.0)));
}
#[test]
fn test_disabled_filter() {
let mut filter = MotionFilter::new(MotionFilterConfig::disabled());
let pose = Pose2D::new(0.0, 0.0, 0.0);
// All scans should pass when disabled
assert!(filter.should_process(pose));
assert!(filter.should_process(pose));
assert!(filter.should_process(pose));
assert_eq!(filter.passed_count(), 3);
assert_eq!(filter.filtered_count(), 0);
}
#[test]
fn test_reset() {
let mut filter = MotionFilter::new(MotionFilterConfig::default());
filter.should_process(Pose2D::new(0.0, 0.0, 0.0));
filter.should_process(Pose2D::new(0.0, 0.0, 0.0));
filter.reset();
assert_eq!(filter.passed_count(), 0);
assert_eq!(filter.filtered_count(), 0);
// After reset, first scan passes again
assert!(filter.should_process(Pose2D::new(0.0, 0.0, 0.0)));
}
#[test]
fn test_filter_efficiency() {
let mut filter = MotionFilter::new(MotionFilterConfig::default());
filter.should_process(Pose2D::new(0.0, 0.0, 0.0)); // pass
filter.should_process(Pose2D::new(0.0, 0.0, 0.0)); // filter
filter.should_process(Pose2D::new(0.0, 0.0, 0.0)); // filter
filter.should_process(Pose2D::new(1.0, 0.0, 0.0)); // pass (distance)
assert_eq!(filter.passed_count(), 2);
assert_eq!(filter.filtered_count(), 2);
assert!((filter.filter_efficiency() - 0.5).abs() < 0.001);
}
}
vastu-map/src/explore/submap_explorer.rs
+88 -66
View File
@@ -8,8 +8,17 @@
use std::time::Instant;
use serde::{Deserialize, Serialize};
use super::controller::{
ExplorationCommand, ExplorationController, ExplorationEvent, ExplorationProgress,
};
use super::error::ExplorationError;
use super::motion_filter::MotionFilter;
use super::velocity::{
VelocityConfig, compute_angular_velocity_to_heading, compute_velocity_to_target,
};
use crate::core::{LidarScan, Pose2D};
use crate::exploration::{ExplorationCommand, ExplorationController, ExplorationProgress};
use crate::grid::GridStorage;
use crate::slam::CorrelativeMatcherConfig;
use crate::submap::{
@@ -17,32 +26,45 @@ use crate::submap::{
};
use super::config::ExplorerConfig;
use super::explorer::{ExploreResult, ExploreStatus, normalize_angle};
use super::explorer::{ExploreResult, ExploreStatus};
use super::source::SensorSource;
/// Configuration specific to submap-based exploration.
#[derive(Clone, Debug)]
#[derive(Clone, Debug, Serialize, Deserialize)]
pub struct SubmapExplorerConfig {
/// Base explorer configuration.
#[serde(default)]
pub base: ExplorerConfig,
/// Submap configuration.
#[serde(default)]
pub submap: SubmapConfig,
/// Multi-submap matching configuration.
#[serde(default)]
pub multi_match: MultiSubmapMatchConfig,
/// Minimum score to accept a loop closure.
#[serde(default = "default_loop_closure_min_score")]
pub loop_closure_min_score: f32,
/// Minimum submap gap for loop closure (submaps between).
#[serde(default = "default_loop_closure_min_gap")]
pub loop_closure_min_gap: usize,
}
fn default_loop_closure_min_score() -> f32 {
0.7
}
fn default_loop_closure_min_gap() -> usize {
3
}
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,
loop_closure_min_score: default_loop_closure_min_score(),
loop_closure_min_gap: default_loop_closure_min_gap(),
}
}
}
@@ -85,6 +107,10 @@ pub struct VastuSubmapExplorer {
controller: ExplorationController,
/// Matcher configuration.
matcher_config: CorrelativeMatcherConfig,
/// Motion filter for gating scan processing.
motion_filter: MotionFilter,
/// Velocity computation configuration.
velocity_config: VelocityConfig,
/// Last corrected world pose.
last_pose: Pose2D,
/// Last odometry pose (for delta calculation).
@@ -109,6 +135,8 @@ impl VastuSubmapExplorer {
let pose_graph = SubmapPoseGraph::new(Default::default());
let controller = ExplorationController::new(config.base.exploration.clone());
let matcher_config = config.base.matching.clone();
let motion_filter = MotionFilter::new(config.base.motion_filter.clone());
let velocity_config = VelocityConfig::default();
Self {
config,
@@ -116,6 +144,8 @@ impl VastuSubmapExplorer {
pose_graph,
controller,
matcher_config,
motion_filter,
velocity_config,
last_pose: Pose2D::default(),
last_odom_pose: None,
last_submap_origin: None,
@@ -191,8 +221,8 @@ impl VastuSubmapExplorer {
if !source.is_connected() {
source.stop();
return ExploreResult::Failed {
map: self.to_occupancy_grid_map(),
reason: "Robot connection lost".to_string(),
map: self.build_occupancy_grid_map(),
error: ExplorationError::ConnectionLost,
};
}
@@ -202,8 +232,11 @@ impl VastuSubmapExplorer {
{
source.stop();
return ExploreResult::Failed {
map: self.to_occupancy_grid_map(),
reason: format!("Battery low: {}%", battery),
map: self.build_occupancy_grid_map(),
error: ExplorationError::LowBattery {
percent: battery,
threshold: self.config.base.min_battery_percent,
},
};
}
@@ -213,8 +246,11 @@ impl VastuSubmapExplorer {
{
source.stop();
return ExploreResult::Failed {
map: self.to_occupancy_grid_map(),
reason: "Time limit exceeded".to_string(),
map: self.build_occupancy_grid_map(),
error: ExplorationError::TimeoutExceeded {
elapsed_secs: self.elapsed_secs(),
limit_secs: self.config.base.max_time_secs,
},
};
}
@@ -225,13 +261,13 @@ impl VastuSubmapExplorer {
}
ExploreStatus::Complete => {
source.stop();
return ExploreResult::Complete(self.to_occupancy_grid_map());
return ExploreResult::Complete(self.build_occupancy_grid_map());
}
ExploreStatus::Failed { reason } => {
ExploreStatus::Failed { error } => {
source.stop();
return ExploreResult::Failed {
map: self.to_occupancy_grid_map(),
reason,
map: self.build_occupancy_grid_map(),
error,
};
}
}
@@ -269,10 +305,19 @@ impl VastuSubmapExplorer {
});
self.last_odom_pose = Some(odom_pose);
// Process lidar scan if available
// Process lidar scan if available and motion filter allows
if let Some(scan) = source.get_lidar_scan() {
let corrected_pose = self.process_scan(&scan, odom_pose);
self.last_pose = corrected_pose;
if self.motion_filter.should_process(odom_pose) {
let corrected_pose = self.process_scan(&scan, odom_pose);
self.last_pose = corrected_pose;
} else if let Some(delta) = odom_delta {
// Scan filtered out - update pose from odometry delta only
self.last_pose = Pose2D::new(
self.last_pose.x + delta.x,
self.last_pose.y + delta.y,
self.last_pose.theta + delta.theta,
);
}
} else if let Some(delta) = odom_delta {
// No scan - update from odometry delta
self.last_pose = Pose2D::new(
@@ -285,7 +330,7 @@ impl VastuSubmapExplorer {
// Handle cliff/bumper events
if source.is_cliff_detected() || source.is_bumper_triggered() {
self.controller
.handle_event(crate::exploration::ExplorationEvent::ObstacleDetected);
.handle_event(ExplorationEvent::ObstacleDetected);
}
// Get current global grid for planning
@@ -297,18 +342,23 @@ impl VastuSubmapExplorer {
// Execute command
match command {
ExplorationCommand::MoveTo { target, max_speed } => {
let (linear, angular) =
self.compute_velocity(self.last_pose, target.x, target.y, max_speed);
let (linear, angular) = compute_velocity_to_target(
self.last_pose,
target,
max_speed,
&self.velocity_config,
);
source.send_velocity(linear, angular);
}
ExplorationCommand::Rotate {
target_heading,
max_angular_speed,
} => {
let angular = self.compute_angular_velocity(
let angular = compute_angular_velocity_to_heading(
self.last_pose.theta,
target_heading,
max_angular_speed,
&self.velocity_config,
);
source.send_velocity(0.0, angular);
}
@@ -321,7 +371,9 @@ impl VastuSubmapExplorer {
}
ExplorationCommand::ExplorationFailed { reason } => {
source.stop();
return ExploreStatus::Failed { reason };
return ExploreStatus::Failed {
error: ExplorationError::from_message(reason),
};
}
ExplorationCommand::None => {
// Continue with current velocity or stop
@@ -450,7 +502,7 @@ impl VastuSubmapExplorer {
}
/// Convert to OccupancyGridMap for compatibility with ExploreResult.
fn to_occupancy_grid_map(&mut self) -> crate::OccupancyGridMap {
fn build_occupancy_grid_map(&mut self) -> crate::OccupancyGridMap {
let map_config = self.config.base.to_map_config();
let mut map = crate::OccupancyGridMap::new(map_config);
@@ -470,54 +522,13 @@ impl VastuSubmapExplorer {
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.motion_filter.reset();
self.last_pose = Pose2D::default();
self.last_odom_pose = None;
self.last_submap_origin = None;
@@ -526,6 +537,17 @@ impl VastuSubmapExplorer {
self.start_time = None;
self.started = false;
}
/// Get motion filter statistics.
///
/// Returns (passed_count, filtered_count, efficiency).
pub fn motion_filter_stats(&self) -> (u64, u64, f32) {
(
self.motion_filter.passed_count(),
self.motion_filter.filtered_count(),
self.motion_filter.filter_efficiency(),
)
}
}
#[cfg(test)]
vastu-map/src/explore/velocity.rs
+167
View File
@@ -0,0 +1,167 @@
//! Velocity computation utilities for exploration.
//!
//! Shared functions for computing linear and angular velocities
//! to navigate towards target positions.
use crate::core::{Pose2D, WorldPoint, normalize_angle};
/// Configuration for velocity computation.
#[derive(Clone, Debug)]
pub struct VelocityConfig {
/// Angle threshold (radians) - turn in place if angle error exceeds this.
/// Default: 0.3 rad (~17 degrees)
pub turn_threshold: f32,
/// Proportional gain for angular velocity.
/// Default: 2.0
pub angular_gain: f32,
/// Maximum angular velocity (rad/s).
/// Default: 2.0
pub max_angular_velocity: f32,
/// Distance threshold to consider target reached (meters).
/// Default: 0.05m (5cm)
pub reached_threshold: f32,
/// Angle threshold to consider heading reached (radians).
/// Default: 0.05 rad (~3 degrees)
pub heading_reached_threshold: f32,
}
impl Default for VelocityConfig {
fn default() -> Self {
Self {
turn_threshold: 0.3,
angular_gain: 2.0,
max_angular_velocity: 2.0,
reached_threshold: 0.05,
heading_reached_threshold: 0.05,
}
}
}
/// Compute velocity command to move towards a target position.
///
/// Returns (linear_velocity, angular_velocity) in (m/s, rad/s).
///
/// The behavior is:
/// 1. If angle to target exceeds `turn_threshold`, rotate in place first
/// 2. Otherwise, move forward with proportional angular correction
///
/// # Arguments
/// * `pose` - Current robot pose
/// * `target` - Target position in world coordinates
/// * `max_speed` - Maximum linear speed (m/s)
/// * `config` - Velocity computation configuration
pub fn compute_velocity_to_target(
pose: Pose2D,
target: WorldPoint,
max_speed: f32,
config: &VelocityConfig,
) -> (f32, f32) {
let dx = target.x - pose.x;
let dy = target.y - pose.y;
let distance = (dx * dx + dy * dy).sqrt();
// Target reached
if distance < config.reached_threshold {
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() > config.turn_threshold {
let angular = angle_error.signum()
* config
.max_angular_velocity
.min(angle_error.abs() * config.angular_gain);
return (0.0, angular);
}
// Move forward with proportional angular correction
let linear = max_speed.min(distance);
let angular = (angle_error * config.angular_gain)
.clamp(-config.max_angular_velocity, config.max_angular_velocity);
(linear, angular)
}
/// Compute angular velocity to rotate to a target heading.
///
/// Returns angular velocity in rad/s.
///
/// # Arguments
/// * `current_heading` - Current heading (radians)
/// * `target_heading` - Target heading (radians)
/// * `max_speed` - Maximum angular speed (rad/s)
/// * `config` - Velocity computation configuration
pub fn compute_angular_velocity_to_heading(
current_heading: f32,
target_heading: f32,
max_speed: f32,
config: &VelocityConfig,
) -> f32 {
let error = normalize_angle(target_heading - current_heading);
if error.abs() < config.heading_reached_threshold {
0.0
} else {
(error * config.angular_gain).clamp(-max_speed, max_speed)
}
}
#[cfg(test)]
mod tests {
use super::*;
use std::f32::consts::PI;
#[test]
fn test_target_reached() {
let config = VelocityConfig::default();
let pose = Pose2D::new(1.0, 1.0, 0.0);
let target = WorldPoint::new(1.02, 1.02); // Within 5cm
let (linear, angular) = compute_velocity_to_target(pose, target, 0.5, &config);
assert_eq!(linear, 0.0);
assert_eq!(angular, 0.0);
}
#[test]
fn test_turn_first_when_angle_large() {
let config = VelocityConfig::default();
let pose = Pose2D::new(0.0, 0.0, 0.0); // Facing +X
let target = WorldPoint::new(0.0, 1.0); // Target is +Y (90 degrees away)
let (linear, angular) = compute_velocity_to_target(pose, target, 0.5, &config);
assert_eq!(linear, 0.0); // Should not move forward
assert!(angular > 0.0); // Should turn left (positive)
}
#[test]
fn test_move_forward_when_aligned() {
let config = VelocityConfig::default();
let pose = Pose2D::new(0.0, 0.0, 0.0); // Facing +X
let target = WorldPoint::new(1.0, 0.0); // Target straight ahead
let (linear, angular) = compute_velocity_to_target(pose, target, 0.5, &config);
assert!(linear > 0.0); // Should move forward
assert!(angular.abs() < 0.01); // Minimal angular correction
}
#[test]
fn test_heading_reached() {
let config = VelocityConfig::default();
let angular = compute_angular_velocity_to_heading(0.0, 0.02, 1.0, &config);
assert_eq!(angular, 0.0); // Within threshold
}
#[test]
fn test_rotate_to_heading() {
let config = VelocityConfig::default();
let angular = compute_angular_velocity_to_heading(0.0, PI / 2.0, 1.0, &config);
assert!(angular > 0.0); // Should turn positive (counter-clockwise)
}
}
vastu-map/src/grid/config.rs
+125
View File
@@ -115,6 +115,128 @@ impl Default for SensorConfig {
}
}
/// Configuration for log-odds occupancy updates.
///
/// Based on Google Cartographer's probability grid model:
/// - Log-odds: L(x) = log(P(x) / (1 - P(x)))
/// - Bayesian update: L_new = L_old + L_observation
/// - Stored as fixed-point i16: actual = value / 100
///
/// Lower values = more observations needed to establish walls.
#[derive(Clone, Debug, Serialize, Deserialize)]
pub struct LogOddsConfig {
/// Log-odds increment for hit (lidar endpoint = occupied).
/// Cartographer default: ~20 (hit_probability=0.55)
/// VastuMap aggressive: 70 (P=0.67)
#[serde(default = "default_l_hit")]
pub l_hit: i16,
/// Log-odds decrement for miss (ray passes through = free).
/// Cartographer default: ~-4 (miss_probability=0.49)
/// VastuMap aggressive: -28 (P=0.43)
/// Asymmetric to make walls "stickier" than free space.
#[serde(default = "default_l_miss")]
pub l_miss: i16,
/// Threshold for considering cell occupied. P > 0.62 → L > 50
#[serde(default = "default_l_occupied")]
pub l_occupied_threshold: i16,
/// Threshold for considering cell free. P < 0.38 → L < -50
#[serde(default = "default_l_free")]
pub l_free_threshold: i16,
/// Minimum log-odds value (clamping). P ≈ 0.12
#[serde(default = "default_l_min")]
pub l_min: i16,
/// Maximum log-odds value (clamping). P ≈ 0.88
#[serde(default = "default_l_max")]
pub l_max: i16,
}
fn default_l_hit() -> i16 {
20
} // P≈0.55 (Cartographer default)
fn default_l_miss() -> i16 {
-4
} // P≈0.49 (Cartographer default)
fn default_l_occupied() -> i16 {
50
}
fn default_l_free() -> i16 {
-50
}
fn default_l_min() -> i16 {
-200
}
fn default_l_max() -> i16 {
200
}
impl Default for LogOddsConfig {
fn default() -> Self {
Self {
l_hit: default_l_hit(),
l_miss: default_l_miss(),
l_occupied_threshold: default_l_occupied(),
l_free_threshold: default_l_free(),
l_min: default_l_min(),
l_max: default_l_max(),
}
}
}
impl LogOddsConfig {
/// Cartographer-like conservative updates (DEFAULT).
/// Requires 3-5 observations to establish wall.
/// This is the recommended default for robust mapping.
pub fn cartographer() -> Self {
Self::default()
}
/// Aggressive updates (original VastuMap behavior).
/// Single observation can establish wall.
/// Use for fast mapping in controlled environments.
pub fn aggressive() -> Self {
Self {
l_hit: 70, // P=0.67
l_miss: -28, // P=0.43
..Default::default()
}
}
/// Balanced - 2-3 observations for wall.
/// Mid-ground between Cartographer and aggressive.
pub fn balanced() -> Self {
Self {
l_hit: 30, // P=0.57
l_miss: -12, // P=0.47
..Default::default()
}
}
/// Convert hit probability to log-odds.
/// Formula: L = 100 * log(p / (1 - p))
pub fn from_probability(hit_prob: f32, miss_prob: f32) -> Self {
let l_hit = (100.0 * (hit_prob / (1.0 - hit_prob)).ln()) as i16;
let l_miss = (100.0 * (miss_prob / (1.0 - miss_prob)).ln()) as i16;
Self {
l_hit,
l_miss,
..Default::default()
}
}
/// Convert log-odds to probability.
/// Formula: P = exp(L/100) / (1 + exp(L/100))
pub fn log_odds_to_probability(log_odds: i16) -> f32 {
let l = log_odds as f32 / 100.0;
let exp_l = l.exp();
exp_l / (1.0 + exp_l)
}
}
/// Full map configuration
#[derive(Clone, Debug, Default, Serialize, Deserialize)]
pub struct MapConfig {
@@ -122,6 +244,9 @@ pub struct MapConfig {
pub grid: GridConfig,
/// Sensor configuration (lidar, robot geometry)
pub sensor: SensorConfig,
/// Log-odds occupancy configuration
#[serde(default)]
pub log_odds: LogOddsConfig,
}
impl MapConfig {
vastu-map/src/grid/lidar_update.rs
+97 -31
View File
@@ -1,11 +1,16 @@
//! 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)
//! Processes lidar scans to update the grid using log-odds Bayesian updates:
//! - Cells along rays receive "miss" observations (evidence of free space)
//! - Cells at ray endpoints (within range) receive "hit" observations (evidence of obstacle)
//!
//! Log-odds representation provides:
//! - Proper Bayesian update: L_new = L_old + L_observation
//! - Better uncertainty handling for conflicting observations
//! - Thinner wall representation (2-3cm vs 5-8cm)
use crate::ObserveResult;
use crate::core::{CellType, LidarScan, Pose2D, WorldPoint};
use crate::core::{LidarScan, Pose2D, WorldPoint};
use crate::grid::config::{GridConfig, SensorConfig};
use crate::grid::raycaster::BresenhamLine;
use crate::grid::storage::GridStorage;
@@ -81,7 +86,8 @@ pub fn update_from_lidar(
}
}
// Mark cells along the ray as Floor (iterate directly, no allocation)
// Trace ray using Bresenham line algorithm
// Apply log-odds updates: miss for ray cells, hit for endpoint
let bresenham = BresenhamLine::new(sensor_coord, endpoint_coord);
// Use peekable to detect the last element without collecting
@@ -94,15 +100,17 @@ pub fn update_from_lidar(
let is_last = iter.peek().is_none();
if !is_last {
// Not the endpoint - mark as Floor
if storage.set_type(coord, CellType::Floor) {
// Not the endpoint - apply "miss" observation (evidence of free space)
// This uses Bayesian log-odds update: L_new = L_old + L_MISS
if storage.apply_miss(coord) {
result.cells_floor += 1;
}
result.cells_updated += 1;
} else {
// Endpoint - mark as Wall if within max range
// Endpoint - apply "hit" observation if within max range
// This uses Bayesian log-odds update: L_new = L_old + L_HIT
if range < sensor_config.max_lidar_range {
if storage.set_type_with_priority(coord, CellType::Wall) {
if storage.apply_hit(coord) {
result.cells_wall += 1;
new_wall_coords.push(coord);
}
@@ -120,9 +128,15 @@ pub fn update_from_lidar(
result
}
/// Update the grid with a single lidar ray.
/// Update the grid with a single lidar ray using log-odds updates.
///
/// This is a lower-level function useful for incremental updates.
/// Uses Bayesian log-odds updates for proper uncertainty handling.
///
/// Returns (cells_updated, hit_registered) where hit_registered is true
/// if a hit observation was applied to the endpoint (range < max_range).
/// Note: hit_registered means a hit observation was recorded, not that
/// the cell is classified as occupied (which requires accumulated observations).
pub fn update_single_ray(
storage: &mut GridStorage,
sensor_pos: WorldPoint,
@@ -144,9 +158,9 @@ pub fn update_single_ray(
}
let mut cells_updated = 0;
let mut hit_wall = false;
let mut hit_registered = false;
// Trace ray (iterate directly, no allocation)
// Trace ray using Bresenham line algorithm
let bresenham = BresenhamLine::new(sensor_coord, endpoint_coord);
let mut iter = bresenham.peekable();
@@ -158,48 +172,78 @@ pub fn update_single_ray(
let is_last = iter.peek().is_none();
if !is_last {
storage.set_type(coord, CellType::Floor);
// Apply miss observation (evidence of free space)
storage.apply_miss(coord);
} else if range < max_range {
storage.set_type_with_priority(coord, CellType::Wall);
hit_wall = true;
// Apply hit observation (evidence of obstacle)
// Returns whether cell became occupied, but we just care that we registered a hit
let _ = storage.apply_hit(coord);
hit_registered = true;
}
cells_updated += 1;
}
(cells_updated, hit_wall)
(cells_updated, hit_registered)
}
#[cfg(test)]
mod tests {
use super::*;
use crate::core::CellType;
use crate::grid::LogOddsConfig;
fn create_test_storage() -> GridStorage {
GridStorage::centered(100, 100, 0.05) // 5m x 5m at 5cm resolution
}
fn create_aggressive_storage() -> GridStorage {
// Use aggressive config where single hit crosses threshold
GridStorage::centered_with_config(100, 100, 0.05, LogOddsConfig::aggressive())
}
#[test]
fn test_single_ray_update() {
let mut storage = create_test_storage();
// Use aggressive config so fewer observations are needed
let mut storage = create_aggressive_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);
// Cast a ray 1m forward multiple times to accumulate log-odds
// With aggressive config (l_miss=-28), 2 misses needed for floor: 0-28-28=-56 < -50
// With aggressive config (l_hit=70), 1 hit needed for wall: 0+70=70 > 50
for _ in 0..3 {
let (cells, _hit) = update_single_ray(&mut storage, sensor_pos, 0.0, 1.0, 8.0);
assert!(cells > 0);
}
assert!(cells > 0);
assert!(hit);
// Check that cells along the ray are Floor
// Check that cells along the ray have negative log-odds (free space evidence)
let mid_coord = storage.world_to_grid(WorldPoint::new(0.5, 0.0));
let mid_log_odds = storage.get_log_odds(mid_coord);
assert!(
mid_log_odds < 0,
"Expected negative log-odds for ray cells, got {}",
mid_log_odds
);
// After enough observations, should be marked as Floor
assert_eq!(storage.get_type(mid_coord), CellType::Floor);
// Check that endpoint is Wall
// Check that endpoint has positive log-odds (obstacle evidence)
let end_coord = storage.world_to_grid(WorldPoint::new(1.0, 0.0));
let end_log_odds = storage.get_log_odds(end_coord);
assert!(
end_log_odds > 0,
"Expected positive log-odds for endpoint, got {}",
end_log_odds
);
// After enough observations, should be marked as Wall
assert_eq!(storage.get_type(end_coord), CellType::Wall);
}
#[test]
fn test_lidar_scan_update() {
let mut storage = create_test_storage();
// Use aggressive config so single hits cross threshold
let mut storage = create_aggressive_storage();
let sensor_config = SensorConfig::default();
let grid_config = GridConfig::default();
let robot_pose = Pose2D::new(0.0, 0.0, 0.0);
@@ -226,7 +270,10 @@ mod tests {
);
assert!(result.cells_updated > 0);
assert!(result.cells_wall > 0);
assert!(
result.cells_wall > 0,
"With aggressive config, single hit should cross threshold"
);
assert!(result.cells_floor > 0);
// Check walls in 4 directions
@@ -270,15 +317,34 @@ mod tests {
let (cells, hit) = update_single_ray(&mut storage, sensor_pos, angle, 1.0, 8.0);
assert!(cells > 0);
assert!(hit);
assert!(hit, "Should register a hit observation");
// Check diagonal endpoint
// Check diagonal endpoint has positive log-odds (evidence of obstacle)
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
let log_odds = storage.get_log_odds(end_coord);
assert!(
storage.get_type(end_coord) == CellType::Wall
|| storage.get_type(end_coord) == CellType::Floor
log_odds > 0,
"Endpoint should have positive log-odds after hit, got {}",
log_odds
);
}
#[test]
fn test_diagonal_ray_aggressive() {
// Use aggressive config for immediate wall classification
let mut storage = create_aggressive_storage();
let sensor_pos = WorldPoint::new(0.0, 0.0);
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);
// With aggressive config, single hit should classify as Wall
let dist = 1.0 / std::f32::consts::SQRT_2;
let end_coord = storage.world_to_grid(WorldPoint::new(dist, dist));
assert_eq!(storage.get_type(end_coord), CellType::Wall);
}
}
vastu-map/src/grid/mod.rs
+2 -2
View File
@@ -15,5 +15,5 @@ pub mod lidar_update;
pub mod raycaster;
mod storage;
pub use config::{ConfigError, GridConfig, MapConfig, SensorConfig};
pub use storage::{CellCounts, GridStorage};
pub use config::{ConfigError, GridConfig, LogOddsConfig, MapConfig, SensorConfig};
pub use storage::{CellCounts, CellMut, GridStorage};
vastu-map/src/grid/storage.rs → vastu-map/src/grid/storage/core.rs
+488 -607
View File
File diff suppressed because it is too large Load Diff
vastu-map/src/grid/storage/mod.rs
+268
View File
@@ -0,0 +1,268 @@
//! Grid storage implementation for the occupancy grid.
//!
//! Uses Structure-of-Arrays (SoA) layout for SIMD optimization.
//! Cell data is stored in separate arrays for each field, enabling
//! efficient vectorized operations on ARM NEON and x86 SSE/AVX.
mod core;
mod types;
pub use self::core::GridStorage;
pub use types::{CellCounts, CellMut};
#[cfg(test)]
mod tests {
use super::*;
use crate::core::{CellType, GridCoord, WorldPoint};
use crate::grid::LogOddsConfig;
#[test]
fn test_grid_creation() {
let grid = GridStorage::new(100, 100, 0.05, WorldPoint::ZERO);
assert_eq!(grid.width(), 100);
assert_eq!(grid.height(), 100);
assert_eq!(grid.resolution(), 0.05);
assert_eq!(grid.cell_count(), 10000);
}
#[test]
fn test_centered_grid() {
let grid = GridStorage::centered(100, 100, 0.05);
let (min, max) = grid.bounds();
assert!((min.x + 2.5).abs() < 1e-6);
assert!((min.y + 2.5).abs() < 1e-6);
assert!((max.x - 2.5).abs() < 1e-6);
assert!((max.y - 2.5).abs() < 1e-6);
}
#[test]
fn test_world_to_grid_conversion() {
let grid = GridStorage::new(100, 100, 0.05, WorldPoint::ZERO);
let coord = grid.world_to_grid(WorldPoint::new(0.0, 0.0));
assert_eq!(coord, GridCoord::new(0, 0));
let coord = grid.world_to_grid(WorldPoint::new(1.0, 1.0));
assert_eq!(coord, GridCoord::new(20, 20));
}
#[test]
fn test_grid_to_world_conversion() {
let grid = GridStorage::new(100, 100, 0.05, WorldPoint::ZERO);
let point = grid.grid_to_world(GridCoord::new(0, 0));
assert!((point.x - 0.025).abs() < 1e-6);
assert!((point.y - 0.025).abs() < 1e-6);
}
#[test]
fn test_get_set_cell() {
let mut grid = GridStorage::new(10, 10, 0.1, WorldPoint::ZERO);
assert_eq!(grid.get_type(GridCoord::new(5, 5)), CellType::Unknown);
grid.set_type(GridCoord::new(5, 5), CellType::Floor);
assert_eq!(grid.get_type(GridCoord::new(5, 5)), CellType::Floor);
assert_eq!(grid.get_type(GridCoord::new(100, 100)), CellType::Unknown);
}
#[test]
fn test_grid_expansion() {
let mut grid = GridStorage::centered(10, 10, 0.1);
assert_eq!(grid.width(), 10);
assert_eq!(grid.height(), 10);
let far_point = WorldPoint::new(2.0, 2.0);
assert!(grid.expand_to_include(far_point, 100, 100));
assert!(grid.width() > 10);
assert!(grid.height() > 10);
assert!(grid.contains_point(far_point));
}
#[test]
fn test_cell_counts() {
let mut grid = GridStorage::new(10, 10, 0.1, WorldPoint::ZERO);
grid.set_type(GridCoord::new(0, 0), CellType::Floor);
grid.set_type(GridCoord::new(1, 0), CellType::Wall);
grid.set_type(GridCoord::new(2, 0), CellType::Cliff);
grid.set_type(GridCoord::new(3, 0), CellType::Bump);
let counts = grid.count_by_type();
assert_eq!(counts.floor, 1);
assert_eq!(counts.wall, 1);
assert_eq!(counts.cliff, 1);
assert_eq!(counts.bump, 1);
assert_eq!(counts.unknown, 96);
assert_eq!(counts.known(), 4);
}
#[test]
fn test_soa_iter() {
let mut grid = GridStorage::new(10, 10, 0.1, WorldPoint::ZERO);
grid.set_type(GridCoord::new(5, 5), CellType::Wall);
let mut found_wall = false;
for (coord, cell) in grid.iter() {
if coord == GridCoord::new(5, 5) {
assert_eq!(cell.cell_type, CellType::Wall);
found_wall = true;
}
}
assert!(found_wall);
}
#[test]
fn test_cell_priority() {
let mut grid = GridStorage::new(10, 10, 0.1, WorldPoint::ZERO);
grid.set_type(GridCoord::new(0, 0), CellType::Floor);
assert_eq!(grid.get_type(GridCoord::new(0, 0)), CellType::Floor);
grid.set_type_with_priority(GridCoord::new(0, 0), CellType::Wall);
assert_eq!(grid.get_type(GridCoord::new(0, 0)), CellType::Wall);
grid.set_type_with_priority(GridCoord::new(0, 0), CellType::Floor);
assert_eq!(grid.get_type(GridCoord::new(0, 0)), CellType::Wall);
grid.set_type_with_priority(GridCoord::new(0, 0), CellType::Bump);
assert_eq!(grid.get_type(GridCoord::new(0, 0)), CellType::Bump);
}
#[test]
fn test_log_odds_probability_conversion() {
assert!((GridStorage::log_odds_to_probability(0) - 0.5).abs() < 0.01);
assert!((GridStorage::log_odds_to_probability(100) - 0.73).abs() < 0.01);
assert!((GridStorage::log_odds_to_probability(-100) - 0.27).abs() < 0.01);
assert!((GridStorage::log_odds_to_probability(200) - 0.88).abs() < 0.01);
assert!((GridStorage::log_odds_to_probability(-200) - 0.12).abs() < 0.01);
}
#[test]
fn test_log_odds_apply_hit() {
let aggressive = LogOddsConfig::aggressive();
let mut grid = GridStorage::centered_with_config(10, 10, 0.1, aggressive.clone());
let coord = GridCoord::new(5, 5);
assert_eq!(grid.get_log_odds(coord), 0);
assert!(!grid.is_occupied(coord));
for i in 0..3 {
let became_occupied = grid.apply_hit(coord);
if i == 0 {
assert!(became_occupied);
}
}
assert!(grid.is_occupied(coord));
assert_eq!(grid.get_type(coord), CellType::Wall);
let log_odds = grid.get_log_odds(coord);
assert!(log_odds > grid.log_odds_config().l_occupied_threshold);
}
#[test]
fn test_log_odds_apply_miss() {
let aggressive = LogOddsConfig::aggressive();
let mut grid = GridStorage::centered_with_config(10, 10, 0.1, aggressive);
let coord = GridCoord::new(5, 5);
for i in 0..5 {
let became_free = grid.apply_miss(coord);
if i == 1 {
assert!(became_free);
}
}
assert!(grid.is_free(coord));
assert_eq!(grid.get_type(coord), CellType::Floor);
}
#[test]
fn test_log_odds_cartographer_default() {
let mut grid = GridStorage::new(10, 10, 0.1, WorldPoint::ZERO);
let coord = GridCoord::new(5, 5);
assert!(!grid.apply_hit(coord));
assert!(!grid.apply_hit(coord));
assert!(grid.apply_hit(coord));
assert!(grid.is_occupied(coord));
}
#[test]
fn test_log_odds_balanced_config() {
let mut grid = GridStorage::centered_with_config(10, 10, 0.1, LogOddsConfig::balanced());
let coord = GridCoord::new(5, 5);
assert!(!grid.apply_hit(coord));
assert!(grid.apply_hit(coord));
assert!(grid.is_occupied(coord));
}
#[test]
fn test_log_odds_conflicting_observations() {
let aggressive = LogOddsConfig::aggressive();
let mut grid = GridStorage::centered_with_config(10, 10, 0.1, aggressive);
let coord = GridCoord::new(5, 5);
for _ in 0..3 {
grid.apply_hit(coord);
}
assert!(grid.is_occupied(coord));
let initial_log_odds = grid.get_log_odds(coord);
for _ in 0..2 {
grid.apply_miss(coord);
}
let after_miss = grid.get_log_odds(coord);
assert!(after_miss < initial_log_odds);
assert!(grid.is_occupied(coord));
}
#[test]
fn test_log_odds_clamping() {
let mut grid = GridStorage::new(10, 10, 0.1, WorldPoint::ZERO);
let coord = GridCoord::new(5, 5);
let cfg = grid.log_odds_config().clone();
for _ in 0..100 {
grid.apply_hit(coord);
}
assert_eq!(grid.get_log_odds(coord), cfg.l_max);
for _ in 0..100 {
grid.apply_miss(coord);
}
assert_eq!(grid.get_log_odds(coord), cfg.l_min);
}
#[test]
fn test_simd_count_matches_scalar() {
let mut grid = GridStorage::new(100, 100, 0.05, WorldPoint::ZERO);
for i in 0..100 {
grid.set_type(GridCoord::new(i, 0), CellType::Floor);
grid.set_type(GridCoord::new(i, 1), CellType::Wall);
}
for i in 0..50 {
grid.set_type(GridCoord::new(i, 2), CellType::Cliff);
grid.set_type(GridCoord::new(i, 3), CellType::Bump);
}
let simd_counts = grid.count_by_type();
let scalar_counts = grid.count_by_type_scalar();
assert_eq!(simd_counts.unknown, scalar_counts.unknown);
assert_eq!(simd_counts.floor, scalar_counts.floor);
assert_eq!(simd_counts.wall, scalar_counts.wall);
assert_eq!(simd_counts.cliff, scalar_counts.cliff);
assert_eq!(simd_counts.bump, scalar_counts.bump);
}
}
vastu-map/src/grid/storage/types.rs
+116
View File
@@ -0,0 +1,116 @@
//! Types used by grid storage.
use crate::core::CellType;
/// Mutable reference to a single cell's fields in the SoA storage.
///
/// This allows modifying individual cell properties without reconstructing
/// the entire Cell struct.
pub struct CellMut<'a> {
/// Cell type as raw u8.
pub cell_type: &'a mut u8,
/// Confidence level.
pub confidence: &'a mut u8,
/// Observation count.
pub observation_count: &'a mut u8,
/// Swept flag (0 = not swept, 1 = swept).
pub swept: &'a mut u8,
}
impl<'a> CellMut<'a> {
/// Get the cell type.
#[inline]
pub fn get_type(&self) -> CellType {
CellType::from_u8(*self.cell_type)
}
/// Set the cell type.
#[inline]
pub fn set_type(&mut self, cell_type: CellType) {
*self.cell_type = cell_type as u8;
}
/// Check if traversable.
#[inline]
pub fn is_traversable(&self) -> bool {
*self.cell_type == CellType::Floor as u8
}
/// Check if obstacle.
#[inline]
pub fn is_obstacle(&self) -> bool {
let t = *self.cell_type;
t == CellType::Wall as u8 || t == CellType::Cliff as u8 || t == CellType::Bump as u8
}
/// Update with a new observation.
pub fn observe(&mut self, observed_type: CellType) -> bool {
let new_type = observed_type as u8;
let changed = *self.cell_type != new_type;
if changed {
*self.cell_type = new_type;
*self.confidence = 1;
*self.observation_count = 1;
} else {
*self.confidence = self.confidence.saturating_add(1);
*self.observation_count = self.observation_count.saturating_add(1);
}
changed
}
/// Update with priority rules.
pub fn observe_with_priority(&mut self, observed_type: CellType) -> bool {
let old_type = CellType::from_u8(*self.cell_type);
let should_update = match (old_type, observed_type) {
(CellType::Unknown, _) => true,
(CellType::Bump, CellType::Bump) => true,
(CellType::Bump, _) => false,
(CellType::Cliff, CellType::Bump | CellType::Cliff) => true,
(CellType::Cliff, _) => false,
(CellType::Wall, CellType::Bump | CellType::Cliff | CellType::Wall) => true,
(CellType::Wall, _) => false,
(CellType::Floor, _) => true,
};
if should_update {
self.observe(observed_type)
} else {
*self.observation_count = self.observation_count.saturating_add(1);
false
}
}
}
/// Cell counts by type.
#[derive(Clone, Copy, Debug, Default)]
pub struct CellCounts {
/// Unknown cells (not yet observed).
pub unknown: usize,
/// Floor cells (traversable).
pub floor: usize,
/// Wall cells (lidar-detected obstacles).
pub wall: usize,
/// Cliff cells (floor drop-offs).
pub cliff: usize,
/// Bump cells (invisible obstacles).
pub bump: usize,
}
impl CellCounts {
/// Total known cells.
pub fn known(&self) -> usize {
self.floor + self.wall + self.cliff + self.bump
}
/// Total cells.
pub fn total(&self) -> usize {
self.unknown + self.known()
}
/// Total obstacle cells.
pub fn obstacles(&self) -> usize {
self.wall + self.cliff + self.bump
}
}
vastu-map/src/lib.rs
+6 -3
View File
@@ -60,9 +60,6 @@ pub mod query;
// Path planning
pub mod pathfinding;
// Exploration controller
pub mod exploration;
// SLAM (scan matching, loop closure)
pub mod slam;
@@ -72,6 +69,12 @@ pub mod submap;
// High-level exploration API
pub mod explore;
/// Backward compatibility: re-export exploration types from explore::controller
#[deprecated(note = "Use `explore::*` instead - exploration types are now in explore module")]
pub mod exploration {
pub use crate::explore::controller::*;
}
// Persistence (save/load)
pub mod io;
vastu-map/src/pathfinding/astar.rs
-822
View File
@@ -1,822 +0,0 @@
//! A* pathfinding algorithm.
//!
//! Implements A* search on the occupancy grid with support for:
//! - Robot footprint collision checking
//! - Safety margin around obstacles
//! - 8-connected grid movement
use crate::core::{GridCoord, WorldPoint};
use crate::grid::GridStorage;
use crate::query::{RobotFootprint, TraversabilityChecker};
use log::{debug, trace};
use std::cmp::Ordering;
use std::collections::{BinaryHeap, HashMap, HashSet};
/// A node in the A* search
#[derive(Clone, Debug)]
struct AStarNode {
coord: GridCoord,
g_cost: f32, // Cost from start
f_cost: f32, // g_cost + heuristic
}
impl Eq for AStarNode {}
impl PartialEq for AStarNode {
fn eq(&self, other: &Self) -> bool {
self.coord == other.coord
}
}
impl Ord for AStarNode {
fn cmp(&self, other: &Self) -> Ordering {
// Reverse ordering for min-heap behavior
other
.f_cost
.partial_cmp(&self.f_cost)
.unwrap_or(Ordering::Equal)
}
}
impl PartialOrd for AStarNode {
fn partial_cmp(&self, other: &Self) -> Option<Ordering> {
Some(self.cmp(other))
}
}
/// A* pathfinding configuration
#[derive(Clone, Debug)]
pub struct AStarConfig {
/// Robot footprint for collision checking
pub footprint: RobotFootprint,
/// Allow diagonal movement (8-connected vs 4-connected)
pub allow_diagonal: bool,
/// Diagonal movement cost multiplier (sqrt(2) ≈ 1.414)
pub diagonal_cost: f32,
/// Maximum number of nodes to expand before giving up
pub max_iterations: usize,
/// Prefer paths with more clearance from obstacles
pub clearance_weight: f32,
/// Exploration mode: allow Unknown cells in footprint
/// (assumes unknown areas are likely free)
pub exploration_mode: bool,
}
impl Default for AStarConfig {
fn default() -> Self {
Self {
footprint: RobotFootprint::default(),
allow_diagonal: true,
diagonal_cost: std::f32::consts::SQRT_2,
max_iterations: 100_000,
clearance_weight: 0.0, // Disabled by default for performance
exploration_mode: false,
}
}
}
impl AStarConfig {
/// Create with custom robot footprint
pub fn with_footprint(footprint: RobotFootprint) -> Self {
Self {
footprint,
..Default::default()
}
}
/// Enable clearance-weighted pathfinding
pub fn with_clearance_weight(mut self, weight: f32) -> Self {
self.clearance_weight = weight;
self
}
}
/// Result of A* pathfinding
#[derive(Clone, Debug)]
pub struct PathResult {
/// Path as grid coordinates (empty if no path found)
pub path_grid: Vec<GridCoord>,
/// Path as world coordinates
pub path_world: Vec<WorldPoint>,
/// Total path cost
pub cost: f32,
/// Number of nodes expanded during search
pub nodes_expanded: usize,
/// Whether a path was found
pub success: bool,
/// Reason for failure (if any)
pub failure_reason: Option<PathFailure>,
}
impl PathResult {
/// Create a failed result
fn failed(reason: PathFailure, nodes_expanded: usize) -> Self {
Self {
path_grid: Vec::new(),
path_world: Vec::new(),
cost: f32::INFINITY,
nodes_expanded,
success: false,
failure_reason: Some(reason),
}
}
/// Path length in cells
pub fn length_cells(&self) -> usize {
self.path_grid.len()
}
/// Path length in meters (approximate)
pub fn length_meters(&self) -> f32 {
if self.path_world.len() < 2 {
return 0.0;
}
let mut total = 0.0;
for i in 1..self.path_world.len() {
total += self.path_world[i - 1].distance(&self.path_world[i]);
}
total
}
}
/// Reason for path failure
#[derive(Clone, Debug, PartialEq, Eq)]
pub enum PathFailure {
/// Start position is not traversable
StartBlocked,
/// Goal position is not traversable
GoalBlocked,
/// No path exists between start and goal
NoPath,
/// Maximum iterations exceeded
MaxIterationsExceeded,
/// Start or goal is out of bounds
OutOfBounds,
}
/// A* pathfinder
pub struct AStarPlanner<'a> {
storage: &'a GridStorage,
config: AStarConfig,
}
impl<'a> AStarPlanner<'a> {
/// Create a new A* planner
pub fn new(storage: &'a GridStorage, config: AStarConfig) -> Self {
Self { storage, config }
}
/// Create with default configuration
pub fn with_defaults(storage: &'a GridStorage) -> Self {
Self::new(storage, AStarConfig::default())
}
/// Find a path from start to goal (grid coordinates)
pub fn find_path(&self, start: GridCoord, goal: GridCoord) -> PathResult {
trace!(
"[AStar] find_path: start=({},{}) goal=({},{})",
start.x, start.y, goal.x, goal.y
);
// Check bounds
if !self.storage.is_valid_coord(start) || !self.storage.is_valid_coord(goal) {
debug!("[AStar] FAILED: OutOfBounds - start or goal outside grid");
return PathResult::failed(PathFailure::OutOfBounds, 0);
}
// Check start and goal traversability using footprint
let checker = TraversabilityChecker::new(self.storage, self.config.footprint.clone());
let start_world = self.storage.grid_to_world(start);
let goal_world = self.storage.grid_to_world(goal);
if !checker.is_position_safe_mode(start_world, self.config.exploration_mode) {
debug!(
"[AStar] FAILED: StartBlocked at ({:.2},{:.2}), exploration_mode={}",
start_world.x, start_world.y, self.config.exploration_mode
);
return PathResult::failed(PathFailure::StartBlocked, 0);
}
if !checker.is_position_safe_mode(goal_world, self.config.exploration_mode) {
debug!(
"[AStar] FAILED: GoalBlocked at ({:.2},{:.2}), exploration_mode={}",
goal_world.x, goal_world.y, self.config.exploration_mode
);
return PathResult::failed(PathFailure::GoalBlocked, 0);
}
// A* search
let mut open_set = BinaryHeap::new();
let mut closed_set = HashSet::new();
let mut came_from: HashMap<GridCoord, GridCoord> = HashMap::new();
let mut g_scores: HashMap<GridCoord, f32> = HashMap::new();
let h_start = self.heuristic(start, goal);
open_set.push(AStarNode {
coord: start,
g_cost: 0.0,
f_cost: h_start,
});
g_scores.insert(start, 0.0);
let mut nodes_expanded = 0;
while let Some(current) = open_set.pop() {
nodes_expanded += 1;
if nodes_expanded > self.config.max_iterations {
debug!(
"[AStar] FAILED: MaxIterationsExceeded ({} nodes)",
nodes_expanded
);
return PathResult::failed(PathFailure::MaxIterationsExceeded, nodes_expanded);
}
// Goal reached
if current.coord == goal {
return self.reconstruct_path(came_from, goal, current.g_cost, nodes_expanded);
}
if closed_set.contains(&current.coord) {
continue;
}
closed_set.insert(current.coord);
// Explore neighbors
let neighbors = if self.config.allow_diagonal {
current.coord.neighbors_8().to_vec()
} else {
current.coord.neighbors_4().to_vec()
};
for (i, neighbor) in neighbors.iter().enumerate() {
if closed_set.contains(neighbor) {
continue;
}
if !self.storage.is_valid_coord(*neighbor) {
continue;
}
// Check traversability with footprint
let neighbor_world = self.storage.grid_to_world(*neighbor);
if !checker.is_position_safe_mode(neighbor_world, self.config.exploration_mode) {
continue;
}
// Calculate movement cost
let is_diagonal = self.config.allow_diagonal && i >= 4;
let move_cost = if is_diagonal {
self.config.diagonal_cost
} else {
1.0
};
// Add clearance cost if enabled
let clearance_cost = if self.config.clearance_weight > 0.0 {
let clearance = checker.clearance(neighbor_world, 1.0);
// Penalize positions close to obstacles
self.config.clearance_weight / (clearance + 0.1)
} else {
0.0
};
let tentative_g = g_scores[&current.coord] + move_cost + clearance_cost;
let current_g = g_scores.get(neighbor).copied().unwrap_or(f32::INFINITY);
if tentative_g < current_g {
came_from.insert(*neighbor, current.coord);
g_scores.insert(*neighbor, tentative_g);
let h = self.heuristic(*neighbor, goal);
open_set.push(AStarNode {
coord: *neighbor,
g_cost: tentative_g,
f_cost: tentative_g + h,
});
}
}
}
debug!(
"[AStar] FAILED: NoPath after expanding {} nodes",
nodes_expanded
);
PathResult::failed(PathFailure::NoPath, nodes_expanded)
}
/// Find a path from start to goal (world coordinates)
pub fn find_path_world(&self, start: WorldPoint, goal: WorldPoint) -> PathResult {
let start_grid = self.storage.world_to_grid(start);
let goal_grid = self.storage.world_to_grid(goal);
self.find_path(start_grid, goal_grid)
}
/// Heuristic function (octile distance for 8-connected grid)
fn heuristic(&self, from: GridCoord, to: GridCoord) -> f32 {
let dx = (from.x - to.x).abs() as f32;
let dy = (from.y - to.y).abs() as f32;
if self.config.allow_diagonal {
// Octile distance
let min = dx.min(dy);
let max = dx.max(dy);
min * self.config.diagonal_cost + (max - min)
} else {
// Manhattan distance
dx + dy
}
}
/// Reconstruct the path from came_from map
fn reconstruct_path(
&self,
came_from: HashMap<GridCoord, GridCoord>,
goal: GridCoord,
cost: f32,
nodes_expanded: usize,
) -> PathResult {
let mut path_grid = Vec::new();
let mut current = goal;
while let Some(&prev) = came_from.get(&current) {
path_grid.push(current);
current = prev;
}
path_grid.push(current); // Add start
path_grid.reverse();
// Convert to world coordinates
let path_world: Vec<WorldPoint> = path_grid
.iter()
.map(|c| self.storage.grid_to_world(*c))
.collect();
trace!(
"[AStar] SUCCESS: path length={} cells, cost={:.2}, nodes_expanded={}",
path_grid.len(),
cost,
nodes_expanded
);
PathResult {
path_grid,
path_world,
cost,
nodes_expanded,
success: true,
failure_reason: None,
}
}
}
/// Quick path finding with default configuration
pub fn find_path(storage: &GridStorage, start: WorldPoint, goal: WorldPoint) -> PathResult {
let planner = AStarPlanner::with_defaults(storage);
planner.find_path_world(start, goal)
}
/// Check if a path exists (faster than full path computation)
pub fn path_exists(storage: &GridStorage, start: WorldPoint, goal: WorldPoint) -> bool {
find_path(storage, start, goal).success
}
/// A* planner with pre-computed traversability cache.
///
/// This is faster for multiple path queries on the same grid state,
/// as it pre-computes traversability for all cells once.
///
/// # Example
/// ```rust,ignore
/// let cached = CachedAStarPlanner::new(storage, config);
/// let path1 = cached.find_path(start1, goal1);
/// let path2 = cached.find_path(start2, goal2); // Uses cached traversability
/// ```
pub struct CachedAStarPlanner<'a> {
storage: &'a GridStorage,
config: AStarConfig,
/// Pre-computed traversability for each cell (true = safe to traverse)
traversability_cache: Vec<bool>,
}
impl<'a> CachedAStarPlanner<'a> {
/// Create a new cached A* planner.
///
/// Pre-computes traversability for all cells based on the robot footprint.
/// This has O(width × height × footprint_area) upfront cost but makes
/// each pathfinding query O(1) for traversability checks.
pub fn new(storage: &'a GridStorage, config: AStarConfig) -> Self {
let checker = TraversabilityChecker::new(storage, config.footprint.clone());
let width = storage.width();
let height = storage.height();
let exploration_mode = config.exploration_mode;
// Pre-compute traversability for all cells
let mut cache = vec![false; width * height];
for y in 0..height {
for x in 0..width {
let coord = GridCoord::new(x as i32, y as i32);
let world = storage.grid_to_world(coord);
cache[y * width + x] = checker.is_position_safe_mode(world, exploration_mode);
}
}
Self {
storage,
config,
traversability_cache: cache,
}
}
/// Create with default configuration
pub fn with_defaults(storage: &'a GridStorage) -> Self {
Self::new(storage, AStarConfig::default())
}
/// Check if a cell is traversable using the cache
#[inline]
fn is_traversable(&self, coord: GridCoord) -> bool {
if !self.storage.is_valid_coord(coord) {
return false;
}
let idx = coord.y as usize * self.storage.width() + coord.x as usize;
self.traversability_cache[idx]
}
/// Find a path from start to goal (grid coordinates)
pub fn find_path(&self, start: GridCoord, goal: GridCoord) -> PathResult {
trace!(
"[CachedAStar] find_path: start=({},{}) goal=({},{})",
start.x, start.y, goal.x, goal.y
);
// Check bounds
if !self.storage.is_valid_coord(start) || !self.storage.is_valid_coord(goal) {
debug!("[CachedAStar] FAILED: OutOfBounds - start or goal outside grid");
return PathResult::failed(PathFailure::OutOfBounds, 0);
}
// Check start and goal traversability using cache
if !self.is_traversable(start) {
debug!("[CachedAStar] FAILED: StartBlocked");
return PathResult::failed(PathFailure::StartBlocked, 0);
}
if !self.is_traversable(goal) {
debug!("[CachedAStar] FAILED: GoalBlocked");
return PathResult::failed(PathFailure::GoalBlocked, 0);
}
// A* search
let mut open_set = BinaryHeap::new();
let mut closed_set = HashSet::new();
let mut came_from: HashMap<GridCoord, GridCoord> = HashMap::new();
let mut g_scores: HashMap<GridCoord, f32> = HashMap::new();
let h_start = self.heuristic(start, goal);
open_set.push(AStarNode {
coord: start,
g_cost: 0.0,
f_cost: h_start,
});
g_scores.insert(start, 0.0);
let mut nodes_expanded = 0;
while let Some(current) = open_set.pop() {
nodes_expanded += 1;
if nodes_expanded > self.config.max_iterations {
debug!(
"[CachedAStar] FAILED: MaxIterationsExceeded ({} nodes)",
nodes_expanded
);
return PathResult::failed(PathFailure::MaxIterationsExceeded, nodes_expanded);
}
// Goal reached
if current.coord == goal {
return self.reconstruct_path(came_from, goal, current.g_cost, nodes_expanded);
}
if closed_set.contains(&current.coord) {
continue;
}
closed_set.insert(current.coord);
// Explore neighbors
let neighbors = if self.config.allow_diagonal {
current.coord.neighbors_8().to_vec()
} else {
current.coord.neighbors_4().to_vec()
};
for (i, neighbor) in neighbors.iter().enumerate() {
if closed_set.contains(neighbor) {
continue;
}
// Check traversability using cache (O(1) lookup)
if !self.is_traversable(*neighbor) {
continue;
}
// Calculate movement cost
let is_diagonal = self.config.allow_diagonal && i >= 4;
let move_cost = if is_diagonal {
self.config.diagonal_cost
} else {
1.0
};
let tentative_g = g_scores[&current.coord] + move_cost;
let current_g = g_scores.get(neighbor).copied().unwrap_or(f32::INFINITY);
if tentative_g < current_g {
came_from.insert(*neighbor, current.coord);
g_scores.insert(*neighbor, tentative_g);
let h = self.heuristic(*neighbor, goal);
open_set.push(AStarNode {
coord: *neighbor,
g_cost: tentative_g,
f_cost: tentative_g + h,
});
}
}
}
debug!(
"[CachedAStar] FAILED: NoPath after expanding {} nodes",
nodes_expanded
);
PathResult::failed(PathFailure::NoPath, nodes_expanded)
}
/// Find a path from start to goal (world coordinates)
pub fn find_path_world(&self, start: WorldPoint, goal: WorldPoint) -> PathResult {
let start_grid = self.storage.world_to_grid(start);
let goal_grid = self.storage.world_to_grid(goal);
self.find_path(start_grid, goal_grid)
}
/// Heuristic function (octile distance for 8-connected grid)
fn heuristic(&self, from: GridCoord, to: GridCoord) -> f32 {
let dx = (from.x - to.x).abs() as f32;
let dy = (from.y - to.y).abs() as f32;
if self.config.allow_diagonal {
let min = dx.min(dy);
let max = dx.max(dy);
min * self.config.diagonal_cost + (max - min)
} else {
dx + dy
}
}
/// Reconstruct the path from came_from map
fn reconstruct_path(
&self,
came_from: HashMap<GridCoord, GridCoord>,
goal: GridCoord,
cost: f32,
nodes_expanded: usize,
) -> PathResult {
let mut path_grid = Vec::new();
let mut current = goal;
while let Some(&prev) = came_from.get(&current) {
path_grid.push(current);
current = prev;
}
path_grid.push(current); // Add start
path_grid.reverse();
let path_world: Vec<WorldPoint> = path_grid
.iter()
.map(|c| self.storage.grid_to_world(*c))
.collect();
trace!(
"[CachedAStar] SUCCESS: path length={} cells, cost={:.2}, nodes_expanded={}",
path_grid.len(),
cost,
nodes_expanded
);
PathResult {
path_grid,
path_world,
cost,
nodes_expanded,
success: true,
failure_reason: None,
}
}
}
#[cfg(test)]
mod tests {
use super::*;
use crate::core::CellType;
fn create_test_storage() -> GridStorage {
GridStorage::centered(50, 50, 0.1) // 5m x 5m at 10cm resolution
}
fn fill_floor(storage: &mut GridStorage) {
for x in 0..50 {
for y in 0..50 {
storage.set_type(GridCoord::new(x, y), CellType::Floor);
}
}
}
#[test]
fn test_simple_path() {
let mut storage = create_test_storage();
fill_floor(&mut storage);
// Small footprint for this test
let config = AStarConfig::with_footprint(RobotFootprint::new(0.05, 0.01));
let planner = AStarPlanner::new(&storage, config);
let start = GridCoord::new(10, 25);
let goal = GridCoord::new(40, 25);
let result = planner.find_path(start, goal);
assert!(result.success);
assert!(!result.path_grid.is_empty());
assert_eq!(result.path_grid[0], start);
assert_eq!(*result.path_grid.last().unwrap(), goal);
}
#[test]
fn test_path_around_obstacle() {
let mut storage = create_test_storage();
fill_floor(&mut storage);
// Add a wall across the middle
for y in 15..35 {
storage.set_type_with_priority(GridCoord::new(25, y), CellType::Wall);
}
let config = AStarConfig::with_footprint(RobotFootprint::new(0.05, 0.01));
let planner = AStarPlanner::new(&storage, config);
let start = GridCoord::new(10, 25);
let goal = GridCoord::new(40, 25);
let result = planner.find_path(start, goal);
assert!(result.success);
// Path should go around the wall
assert!(result.path_grid.len() > 30);
}
#[test]
fn test_no_path() {
let mut storage = create_test_storage();
fill_floor(&mut storage);
// Add a complete wall barrier
for y in 0..50 {
storage.set_type_with_priority(GridCoord::new(25, y), CellType::Wall);
}
let config = AStarConfig::with_footprint(RobotFootprint::new(0.05, 0.01));
let planner = AStarPlanner::new(&storage, config);
let start = GridCoord::new(10, 25);
let goal = GridCoord::new(40, 25);
let result = planner.find_path(start, goal);
assert!(!result.success);
assert_eq!(result.failure_reason, Some(PathFailure::NoPath));
}
#[test]
fn test_start_blocked() {
let mut storage = create_test_storage();
fill_floor(&mut storage);
// Block start position
storage.set_type_with_priority(GridCoord::new(10, 25), CellType::Wall);
let config = AStarConfig::with_footprint(RobotFootprint::new(0.05, 0.01));
let planner = AStarPlanner::new(&storage, config);
let start = GridCoord::new(10, 25);
let goal = GridCoord::new(40, 25);
let result = planner.find_path(start, goal);
assert!(!result.success);
assert_eq!(result.failure_reason, Some(PathFailure::StartBlocked));
}
#[test]
fn test_goal_blocked() {
let mut storage = create_test_storage();
fill_floor(&mut storage);
// Block goal position
storage.set_type_with_priority(GridCoord::new(40, 25), CellType::Wall);
let config = AStarConfig::with_footprint(RobotFootprint::new(0.05, 0.01));
let planner = AStarPlanner::new(&storage, config);
let start = GridCoord::new(10, 25);
let goal = GridCoord::new(40, 25);
let result = planner.find_path(start, goal);
assert!(!result.success);
assert_eq!(result.failure_reason, Some(PathFailure::GoalBlocked));
}
#[test]
fn test_diagonal_path() {
let mut storage = create_test_storage();
fill_floor(&mut storage);
let config = AStarConfig {
footprint: RobotFootprint::new(0.05, 0.01),
allow_diagonal: true,
..Default::default()
};
let planner = AStarPlanner::new(&storage, config);
let start = GridCoord::new(10, 10);
let goal = GridCoord::new(40, 40);
let result = planner.find_path(start, goal);
assert!(result.success);
// Diagonal path should be shorter than manhattan
// For a 30x30 diagonal, manhattan would be 60, octile should be ~42
assert!(result.path_grid.len() < 50);
}
#[test]
fn test_4_connected_path() {
let mut storage = create_test_storage();
fill_floor(&mut storage);
let config = AStarConfig {
footprint: RobotFootprint::new(0.05, 0.01),
allow_diagonal: false,
..Default::default()
};
let planner = AStarPlanner::new(&storage, config);
let start = GridCoord::new(10, 10);
let goal = GridCoord::new(40, 40);
let result = planner.find_path(start, goal);
assert!(result.success);
// 4-connected path should follow manhattan distance
// Path length should be approximately dx + dy = 60
assert!(result.path_grid.len() >= 60);
}
#[test]
fn test_world_coordinates() {
let mut storage = create_test_storage();
fill_floor(&mut storage);
let config = AStarConfig::with_footprint(RobotFootprint::new(0.05, 0.01));
let planner = AStarPlanner::new(&storage, config);
let start = WorldPoint::new(-1.0, 0.0);
let goal = WorldPoint::new(1.0, 0.0);
let result = planner.find_path_world(start, goal);
assert!(result.success);
assert!(result.path_world.len() > 0);
}
#[test]
fn test_path_length_meters() {
let mut storage = create_test_storage();
fill_floor(&mut storage);
let config = AStarConfig::with_footprint(RobotFootprint::new(0.05, 0.01));
let planner = AStarPlanner::new(&storage, config);
// Path of 2 meters (20 cells at 10cm resolution)
let start = WorldPoint::new(-1.0, 0.0);
let goal = WorldPoint::new(1.0, 0.0);
let result = planner.find_path_world(start, goal);
assert!(result.success);
// Path length should be approximately 2.0 meters
let length = result.length_meters();
assert!(length > 1.5 && length < 2.5);
}
}
vastu-map/src/pathfinding/astar/cached.rs
+242
View File
@@ -0,0 +1,242 @@
//! Cached A* planner with pre-computed traversability.
use crate::core::{GridCoord, WorldPoint};
use crate::grid::GridStorage;
use crate::query::TraversabilityChecker;
use log::{debug, trace};
use std::collections::{BinaryHeap, HashMap, HashSet};
use super::types::{AStarConfig, AStarNode, PathFailure, PathResult};
/// A* planner with pre-computed traversability cache.
///
/// This is faster for multiple path queries on the same grid state,
/// as it pre-computes traversability for all cells once.
///
/// # Example
/// ```rust,ignore
/// let cached = CachedAStarPlanner::new(storage, config);
/// let path1 = cached.find_path(start1, goal1);
/// let path2 = cached.find_path(start2, goal2); // Uses cached traversability
/// ```
pub struct CachedAStarPlanner<'a> {
storage: &'a GridStorage,
config: AStarConfig,
/// Pre-computed traversability for each cell (true = safe to traverse)
traversability_cache: Vec<bool>,
}
impl<'a> CachedAStarPlanner<'a> {
/// Create a new cached A* planner.
///
/// Pre-computes traversability for all cells based on the robot footprint.
/// This has O(width × height × footprint_area) upfront cost but makes
/// each pathfinding query O(1) for traversability checks.
pub fn new(storage: &'a GridStorage, config: AStarConfig) -> Self {
let checker = TraversabilityChecker::new(storage, config.footprint.clone());
let width = storage.width();
let height = storage.height();
let exploration_mode = config.exploration_mode;
// Pre-compute traversability for all cells
let mut cache = vec![false; width * height];
for y in 0..height {
for x in 0..width {
let coord = GridCoord::new(x as i32, y as i32);
let world = storage.grid_to_world(coord);
cache[y * width + x] = checker.is_position_safe_mode(world, exploration_mode);
}
}
Self {
storage,
config,
traversability_cache: cache,
}
}
/// Create with default configuration
pub fn with_defaults(storage: &'a GridStorage) -> Self {
Self::new(storage, AStarConfig::default())
}
/// Check if a cell is traversable using the cache
#[inline]
fn is_traversable(&self, coord: GridCoord) -> bool {
if !self.storage.is_valid_coord(coord) {
return false;
}
let idx = coord.y as usize * self.storage.width() + coord.x as usize;
self.traversability_cache[idx]
}
/// Find a path from start to goal (grid coordinates)
pub fn find_path(&self, start: GridCoord, goal: GridCoord) -> PathResult {
trace!(
"[CachedAStar] find_path: start=({},{}) goal=({},{})",
start.x, start.y, goal.x, goal.y
);
// Check bounds
if !self.storage.is_valid_coord(start) || !self.storage.is_valid_coord(goal) {
debug!("[CachedAStar] FAILED: OutOfBounds - start or goal outside grid");
return PathResult::failed(PathFailure::OutOfBounds, 0);
}
// Check start and goal traversability using cache
if !self.is_traversable(start) {
debug!("[CachedAStar] FAILED: StartBlocked");
return PathResult::failed(PathFailure::StartBlocked, 0);
}
if !self.is_traversable(goal) {
debug!("[CachedAStar] FAILED: GoalBlocked");
return PathResult::failed(PathFailure::GoalBlocked, 0);
}
// A* search
let mut open_set = BinaryHeap::new();
let mut closed_set = HashSet::new();
let mut came_from: HashMap<GridCoord, GridCoord> = HashMap::new();
let mut g_scores: HashMap<GridCoord, f32> = HashMap::new();
let h_start = self.heuristic(start, goal);
open_set.push(AStarNode {
coord: start,
g_cost: 0.0,
f_cost: h_start,
});
g_scores.insert(start, 0.0);
let mut nodes_expanded = 0;
while let Some(current) = open_set.pop() {
nodes_expanded += 1;
if nodes_expanded > self.config.max_iterations {
debug!(
"[CachedAStar] FAILED: MaxIterationsExceeded ({} nodes)",
nodes_expanded
);
return PathResult::failed(PathFailure::MaxIterationsExceeded, nodes_expanded);
}
// Goal reached
if current.coord == goal {
return self.reconstruct_path(came_from, goal, current.g_cost, nodes_expanded);
}
if closed_set.contains(&current.coord) {
continue;
}
closed_set.insert(current.coord);
// Explore neighbors
let neighbors = if self.config.allow_diagonal {
current.coord.neighbors_8().to_vec()
} else {
current.coord.neighbors_4().to_vec()
};
for (i, neighbor) in neighbors.iter().enumerate() {
if closed_set.contains(neighbor) {
continue;
}
// Check traversability using cache (O(1) lookup)
if !self.is_traversable(*neighbor) {
continue;
}
// Calculate movement cost
let is_diagonal = self.config.allow_diagonal && i >= 4;
let move_cost = if is_diagonal {
self.config.diagonal_cost
} else {
1.0
};
let tentative_g = g_scores[&current.coord] + move_cost;
let current_g = g_scores.get(neighbor).copied().unwrap_or(f32::INFINITY);
if tentative_g < current_g {
came_from.insert(*neighbor, current.coord);
g_scores.insert(*neighbor, tentative_g);
let h = self.heuristic(*neighbor, goal);
open_set.push(AStarNode {
coord: *neighbor,
g_cost: tentative_g,
f_cost: tentative_g + h,
});
}
}
}
debug!(
"[CachedAStar] FAILED: NoPath after expanding {} nodes",
nodes_expanded
);
PathResult::failed(PathFailure::NoPath, nodes_expanded)
}
/// Find a path from start to goal (world coordinates)
pub fn find_path_world(&self, start: WorldPoint, goal: WorldPoint) -> PathResult {
let start_grid = self.storage.world_to_grid(start);
let goal_grid = self.storage.world_to_grid(goal);
self.find_path(start_grid, goal_grid)
}
/// Heuristic function (octile distance for 8-connected grid)
fn heuristic(&self, from: GridCoord, to: GridCoord) -> f32 {
let dx = (from.x - to.x).abs() as f32;
let dy = (from.y - to.y).abs() as f32;
if self.config.allow_diagonal {
let min = dx.min(dy);
let max = dx.max(dy);
min * self.config.diagonal_cost + (max - min)
} else {
dx + dy
}
}
/// Reconstruct the path from came_from map
fn reconstruct_path(
&self,
came_from: HashMap<GridCoord, GridCoord>,
goal: GridCoord,
cost: f32,
nodes_expanded: usize,
) -> PathResult {
let mut path_grid = Vec::new();
let mut current = goal;
while let Some(&prev) = came_from.get(&current) {
path_grid.push(current);
current = prev;
}
path_grid.push(current); // Add start
path_grid.reverse();
let path_world: Vec<WorldPoint> = path_grid
.iter()
.map(|c| self.storage.grid_to_world(*c))
.collect();
trace!(
"[CachedAStar] SUCCESS: path length={} cells, cost={:.2}, nodes_expanded={}",
path_grid.len(),
cost,
nodes_expanded
);
PathResult {
path_grid,
path_world,
cost,
nodes_expanded,
success: true,
failure_reason: None,
}
}
}
vastu-map/src/pathfinding/astar/mod.rs
+235
View File
@@ -0,0 +1,235 @@
//! A* pathfinding algorithm.
//!
//! Implements A* search on the occupancy grid with support for:
//! - Robot footprint collision checking
//! - Safety margin around obstacles
//! - 8-connected grid movement
mod cached;
mod planner;
mod types;
pub use cached::CachedAStarPlanner;
pub use planner::AStarPlanner;
pub use types::{AStarConfig, PathFailure, PathResult};
use crate::core::WorldPoint;
use crate::grid::GridStorage;
/// Quick path finding with default configuration
pub fn find_path(storage: &GridStorage, start: WorldPoint, goal: WorldPoint) -> PathResult {
let planner = AStarPlanner::with_defaults(storage);
planner.find_path_world(start, goal)
}
/// Check if a path exists (faster than full path computation)
pub fn path_exists(storage: &GridStorage, start: WorldPoint, goal: WorldPoint) -> bool {
find_path(storage, start, goal).success
}
#[cfg(test)]
mod tests {
use super::*;
use crate::core::{CellType, GridCoord};
use crate::query::RobotFootprint;
fn create_test_storage() -> GridStorage {
GridStorage::centered(50, 50, 0.1) // 5m x 5m at 10cm resolution
}
fn fill_floor(storage: &mut GridStorage) {
for x in 0..50 {
for y in 0..50 {
storage.set_type(GridCoord::new(x, y), CellType::Floor);
}
}
}
#[test]
fn test_simple_path() {
let mut storage = create_test_storage();
fill_floor(&mut storage);
// Small footprint for this test
let config = AStarConfig::with_footprint(RobotFootprint::new(0.05, 0.01));
let planner = AStarPlanner::new(&storage, config);
let start = GridCoord::new(10, 25);
let goal = GridCoord::new(40, 25);
let result = planner.find_path(start, goal);
assert!(result.success);
assert!(!result.path_grid.is_empty());
assert_eq!(result.path_grid[0], start);
assert_eq!(*result.path_grid.last().unwrap(), goal);
}
#[test]
fn test_path_around_obstacle() {
let mut storage = create_test_storage();
fill_floor(&mut storage);
// Add a wall across the middle
for y in 15..35 {
storage.set_type_with_priority(GridCoord::new(25, y), CellType::Wall);
}
let config = AStarConfig::with_footprint(RobotFootprint::new(0.05, 0.01));
let planner = AStarPlanner::new(&storage, config);
let start = GridCoord::new(10, 25);
let goal = GridCoord::new(40, 25);
let result = planner.find_path(start, goal);
assert!(result.success);
// Path should go around the wall
assert!(result.path_grid.len() > 30);
}
#[test]
fn test_no_path() {
let mut storage = create_test_storage();
fill_floor(&mut storage);
// Add a complete wall barrier
for y in 0..50 {
storage.set_type_with_priority(GridCoord::new(25, y), CellType::Wall);
}
let config = AStarConfig::with_footprint(RobotFootprint::new(0.05, 0.01));
let planner = AStarPlanner::new(&storage, config);
let start = GridCoord::new(10, 25);
let goal = GridCoord::new(40, 25);
let result = planner.find_path(start, goal);
assert!(!result.success);
assert_eq!(result.failure_reason, Some(PathFailure::NoPath));
}
#[test]
fn test_start_blocked() {
let mut storage = create_test_storage();
fill_floor(&mut storage);
// Block start position
storage.set_type_with_priority(GridCoord::new(10, 25), CellType::Wall);
let config = AStarConfig::with_footprint(RobotFootprint::new(0.05, 0.01));
let planner = AStarPlanner::new(&storage, config);
let start = GridCoord::new(10, 25);
let goal = GridCoord::new(40, 25);
let result = planner.find_path(start, goal);
assert!(!result.success);
assert_eq!(result.failure_reason, Some(PathFailure::StartBlocked));
}
#[test]
fn test_goal_blocked() {
let mut storage = create_test_storage();
fill_floor(&mut storage);
// Block goal position
storage.set_type_with_priority(GridCoord::new(40, 25), CellType::Wall);
let config = AStarConfig::with_footprint(RobotFootprint::new(0.05, 0.01));
let planner = AStarPlanner::new(&storage, config);
let start = GridCoord::new(10, 25);
let goal = GridCoord::new(40, 25);
let result = planner.find_path(start, goal);
assert!(!result.success);
assert_eq!(result.failure_reason, Some(PathFailure::GoalBlocked));
}
#[test]
fn test_diagonal_path() {
let mut storage = create_test_storage();
fill_floor(&mut storage);
let config = AStarConfig {
footprint: RobotFootprint::new(0.05, 0.01),
allow_diagonal: true,
..Default::default()
};
let planner = AStarPlanner::new(&storage, config);
let start = GridCoord::new(10, 10);
let goal = GridCoord::new(40, 40);
let result = planner.find_path(start, goal);
assert!(result.success);
// Diagonal path should be shorter than manhattan
// For a 30x30 diagonal, manhattan would be 60, octile should be ~42
assert!(result.path_grid.len() < 50);
}
#[test]
fn test_4_connected_path() {
let mut storage = create_test_storage();
fill_floor(&mut storage);
let config = AStarConfig {
footprint: RobotFootprint::new(0.05, 0.01),
allow_diagonal: false,
..Default::default()
};
let planner = AStarPlanner::new(&storage, config);
let start = GridCoord::new(10, 10);
let goal = GridCoord::new(40, 40);
let result = planner.find_path(start, goal);
assert!(result.success);
// 4-connected path should follow manhattan distance
// Path length should be approximately dx + dy = 60
assert!(result.path_grid.len() >= 60);
}
#[test]
fn test_world_coordinates() {
let mut storage = create_test_storage();
fill_floor(&mut storage);
let config = AStarConfig::with_footprint(RobotFootprint::new(0.05, 0.01));
let planner = AStarPlanner::new(&storage, config);
let start = WorldPoint::new(-1.0, 0.0);
let goal = WorldPoint::new(1.0, 0.0);
let result = planner.find_path_world(start, goal);
assert!(result.success);
assert!(result.path_world.len() > 0);
}
#[test]
fn test_path_length_meters() {
let mut storage = create_test_storage();
fill_floor(&mut storage);
let config = AStarConfig::with_footprint(RobotFootprint::new(0.05, 0.01));
let planner = AStarPlanner::new(&storage, config);
// Path of 2 meters (20 cells at 10cm resolution)
let start = WorldPoint::new(-1.0, 0.0);
let goal = WorldPoint::new(1.0, 0.0);
let result = planner.find_path_world(start, goal);
assert!(result.success);
// Path length should be approximately 2.0 meters
let length = result.length_meters();
assert!(length > 1.5 && length < 2.5);
}
}
vastu-map/src/pathfinding/astar/planner.rs
+225
View File
@@ -0,0 +1,225 @@
//! A* planner implementation.
use crate::core::{GridCoord, WorldPoint};
use crate::grid::GridStorage;
use crate::query::TraversabilityChecker;
use log::{debug, trace};
use std::collections::{BinaryHeap, HashMap, HashSet};
use super::types::{AStarConfig, AStarNode, PathFailure, PathResult};
/// A* pathfinder
pub struct AStarPlanner<'a> {
storage: &'a GridStorage,
config: AStarConfig,
}
impl<'a> AStarPlanner<'a> {
/// Create a new A* planner
pub fn new(storage: &'a GridStorage, config: AStarConfig) -> Self {
Self { storage, config }
}
/// Create with default configuration
pub fn with_defaults(storage: &'a GridStorage) -> Self {
Self::new(storage, AStarConfig::default())
}
/// Find a path from start to goal (grid coordinates)
pub fn find_path(&self, start: GridCoord, goal: GridCoord) -> PathResult {
trace!(
"[AStar] find_path: start=({},{}) goal=({},{})",
start.x, start.y, goal.x, goal.y
);
// Check bounds
if !self.storage.is_valid_coord(start) || !self.storage.is_valid_coord(goal) {
debug!("[AStar] FAILED: OutOfBounds - start or goal outside grid");
return PathResult::failed(PathFailure::OutOfBounds, 0);
}
// Check start and goal traversability using footprint
let checker = TraversabilityChecker::new(self.storage, self.config.footprint.clone());
let start_world = self.storage.grid_to_world(start);
let goal_world = self.storage.grid_to_world(goal);
if !checker.is_position_safe_mode(start_world, self.config.exploration_mode) {
debug!(
"[AStar] FAILED: StartBlocked at ({:.2},{:.2}), exploration_mode={}",
start_world.x, start_world.y, self.config.exploration_mode
);
return PathResult::failed(PathFailure::StartBlocked, 0);
}
if !checker.is_position_safe_mode(goal_world, self.config.exploration_mode) {
debug!(
"[AStar] FAILED: GoalBlocked at ({:.2},{:.2}), exploration_mode={}",
goal_world.x, goal_world.y, self.config.exploration_mode
);
return PathResult::failed(PathFailure::GoalBlocked, 0);
}
// A* search
let mut open_set = BinaryHeap::new();
let mut closed_set = HashSet::new();
let mut came_from: HashMap<GridCoord, GridCoord> = HashMap::new();
let mut g_scores: HashMap<GridCoord, f32> = HashMap::new();
let h_start = self.heuristic(start, goal);
open_set.push(AStarNode {
coord: start,
g_cost: 0.0,
f_cost: h_start,
});
g_scores.insert(start, 0.0);
let mut nodes_expanded = 0;
while let Some(current) = open_set.pop() {
nodes_expanded += 1;
if nodes_expanded > self.config.max_iterations {
debug!(
"[AStar] FAILED: MaxIterationsExceeded ({} nodes)",
nodes_expanded
);
return PathResult::failed(PathFailure::MaxIterationsExceeded, nodes_expanded);
}
// Goal reached
if current.coord == goal {
return self.reconstruct_path(came_from, goal, current.g_cost, nodes_expanded);
}
if closed_set.contains(&current.coord) {
continue;
}
closed_set.insert(current.coord);
// Explore neighbors
let neighbors = if self.config.allow_diagonal {
current.coord.neighbors_8().to_vec()
} else {
current.coord.neighbors_4().to_vec()
};
for (i, neighbor) in neighbors.iter().enumerate() {
if closed_set.contains(neighbor) {
continue;
}
if !self.storage.is_valid_coord(*neighbor) {
continue;
}
// Check traversability with footprint
let neighbor_world = self.storage.grid_to_world(*neighbor);
if !checker.is_position_safe_mode(neighbor_world, self.config.exploration_mode) {
continue;
}
// Calculate movement cost
let is_diagonal = self.config.allow_diagonal && i >= 4;
let move_cost = if is_diagonal {
self.config.diagonal_cost
} else {
1.0
};
// Add clearance cost if enabled
let clearance_cost = if self.config.clearance_weight > 0.0 {
let clearance = checker.clearance(neighbor_world, 1.0);
// Penalize positions close to obstacles
self.config.clearance_weight / (clearance + 0.1)
} else {
0.0
};
let tentative_g = g_scores[&current.coord] + move_cost + clearance_cost;
let current_g = g_scores.get(neighbor).copied().unwrap_or(f32::INFINITY);
if tentative_g < current_g {
came_from.insert(*neighbor, current.coord);
g_scores.insert(*neighbor, tentative_g);
let h = self.heuristic(*neighbor, goal);
open_set.push(AStarNode {
coord: *neighbor,
g_cost: tentative_g,
f_cost: tentative_g + h,
});
}
}
}
debug!(
"[AStar] FAILED: NoPath after expanding {} nodes",
nodes_expanded
);
PathResult::failed(PathFailure::NoPath, nodes_expanded)
}
/// Find a path from start to goal (world coordinates)
pub fn find_path_world(&self, start: WorldPoint, goal: WorldPoint) -> PathResult {
let start_grid = self.storage.world_to_grid(start);
let goal_grid = self.storage.world_to_grid(goal);
self.find_path(start_grid, goal_grid)
}
/// Heuristic function (octile distance for 8-connected grid)
fn heuristic(&self, from: GridCoord, to: GridCoord) -> f32 {
let dx = (from.x - to.x).abs() as f32;
let dy = (from.y - to.y).abs() as f32;
if self.config.allow_diagonal {
// Octile distance
let min = dx.min(dy);
let max = dx.max(dy);
min * self.config.diagonal_cost + (max - min)
} else {
// Manhattan distance
dx + dy
}
}
/// Reconstruct the path from came_from map
fn reconstruct_path(
&self,
came_from: HashMap<GridCoord, GridCoord>,
goal: GridCoord,
cost: f32,
nodes_expanded: usize,
) -> PathResult {
let mut path_grid = Vec::new();
let mut current = goal;
while let Some(&prev) = came_from.get(&current) {
path_grid.push(current);
current = prev;
}
path_grid.push(current); // Add start
path_grid.reverse();
// Convert to world coordinates
let path_world: Vec<WorldPoint> = path_grid
.iter()
.map(|c| self.storage.grid_to_world(*c))
.collect();
trace!(
"[AStar] SUCCESS: path length={} cells, cost={:.2}, nodes_expanded={}",
path_grid.len(),
cost,
nodes_expanded
);
PathResult {
path_grid,
path_world,
cost,
nodes_expanded,
success: true,
failure_reason: None,
}
}
}
vastu-map/src/pathfinding/astar/types.rs
+148
View File
@@ -0,0 +1,148 @@
//! A* pathfinding types.
use crate::core::{GridCoord, WorldPoint};
use crate::query::RobotFootprint;
use std::cmp::Ordering;
/// A node in the A* search
#[derive(Clone, Debug)]
pub(super) struct AStarNode {
pub coord: GridCoord,
pub g_cost: f32, // Cost from start
pub f_cost: f32, // g_cost + heuristic
}
impl Eq for AStarNode {}
impl PartialEq for AStarNode {
fn eq(&self, other: &Self) -> bool {
self.coord == other.coord
}
}
impl Ord for AStarNode {
fn cmp(&self, other: &Self) -> Ordering {
// Reverse ordering for min-heap behavior
other
.f_cost
.partial_cmp(&self.f_cost)
.unwrap_or(Ordering::Equal)
}
}
impl PartialOrd for AStarNode {
fn partial_cmp(&self, other: &Self) -> Option<Ordering> {
Some(self.cmp(other))
}
}
/// A* pathfinding configuration
#[derive(Clone, Debug)]
pub struct AStarConfig {
/// Robot footprint for collision checking
pub footprint: RobotFootprint,
/// Allow diagonal movement (8-connected vs 4-connected)
pub allow_diagonal: bool,
/// Diagonal movement cost multiplier (sqrt(2) ≈ 1.414)
pub diagonal_cost: f32,
/// Maximum number of nodes to expand before giving up
pub max_iterations: usize,
/// Prefer paths with more clearance from obstacles
pub clearance_weight: f32,
/// Exploration mode: allow Unknown cells in footprint
/// (assumes unknown areas are likely free)
pub exploration_mode: bool,
}
impl Default for AStarConfig {
fn default() -> Self {
Self {
footprint: RobotFootprint::default(),
allow_diagonal: true,
diagonal_cost: std::f32::consts::SQRT_2,
max_iterations: 100_000,
clearance_weight: 0.0, // Disabled by default for performance
exploration_mode: false,
}
}
}
impl AStarConfig {
/// Create with custom robot footprint
pub fn with_footprint(footprint: RobotFootprint) -> Self {
Self {
footprint,
..Default::default()
}
}
/// Enable clearance-weighted pathfinding
pub fn with_clearance_weight(mut self, weight: f32) -> Self {
self.clearance_weight = weight;
self
}
}
/// Result of A* pathfinding
#[derive(Clone, Debug)]
pub struct PathResult {
/// Path as grid coordinates (empty if no path found)
pub path_grid: Vec<GridCoord>,
/// Path as world coordinates
pub path_world: Vec<WorldPoint>,
/// Total path cost
pub cost: f32,
/// Number of nodes expanded during search
pub nodes_expanded: usize,
/// Whether a path was found
pub success: bool,
/// Reason for failure (if any)
pub failure_reason: Option<PathFailure>,
}
impl PathResult {
/// Create a failed result
pub(super) fn failed(reason: PathFailure, nodes_expanded: usize) -> Self {
Self {
path_grid: Vec::new(),
path_world: Vec::new(),
cost: f32::INFINITY,
nodes_expanded,
success: false,
failure_reason: Some(reason),
}
}
/// Path length in cells
pub fn length_cells(&self) -> usize {
self.path_grid.len()
}
/// Path length in meters (approximate)
pub fn length_meters(&self) -> f32 {
if self.path_world.len() < 2 {
return 0.0;
}
let mut total = 0.0;
for i in 1..self.path_world.len() {
total += self.path_world[i - 1].distance(&self.path_world[i]);
}
total
}
}
/// Reason for path failure
#[derive(Clone, Debug, PartialEq, Eq)]
pub enum PathFailure {
/// Start position is not traversable
StartBlocked,
/// Goal position is not traversable
GoalBlocked,
/// No path exists between start and goal
NoPath,
/// Maximum iterations exceeded
MaxIterationsExceeded,
/// Start or goal is out of bounds
OutOfBounds,
}
vastu-map/src/query/traversability/checker.rs
+400
View File
@@ -0,0 +1,400 @@
//! Traversability checker implementation.
use std::simd::{cmp::SimdPartialOrd, u8x16};
use crate::core::{CellType, GridCoord, WorldPoint};
use crate::grid::GridStorage;
use super::footprint::RobotFootprint;
use super::path::PathCheckResult;
/// Traversability checker for path planning and navigation.
pub struct TraversabilityChecker<'a> {
storage: &'a GridStorage,
footprint: RobotFootprint,
}
impl<'a> TraversabilityChecker<'a> {
/// Create a new traversability checker.
pub fn new(storage: &'a GridStorage, footprint: RobotFootprint) -> Self {
Self { storage, footprint }
}
/// Create with default robot footprint.
pub fn with_default_footprint(storage: &'a GridStorage) -> Self {
Self::new(storage, RobotFootprint::default())
}
/// Get the robot footprint.
pub fn footprint(&self) -> &RobotFootprint {
&self.footprint
}
/// Get the storage.
pub fn storage(&self) -> &GridStorage {
self.storage
}
// =========================================================================
// Single cell traversability
// =========================================================================
/// Check if a single cell is traversable.
pub fn is_cell_traversable(&self, coord: GridCoord) -> bool {
if !self.storage.is_valid_coord(coord) {
return false;
}
self.storage.get_type(coord).is_traversable()
}
/// Check if a world point is traversable (single cell).
pub fn is_point_traversable(&self, point: WorldPoint) -> bool {
let coord = self.storage.world_to_grid(point);
self.is_cell_traversable(coord)
}
// =========================================================================
// Position safety (considering footprint)
// =========================================================================
/// Check if a position is safe for the robot (considering footprint).
pub fn is_position_safe(&self, position: WorldPoint) -> bool {
self.is_position_safe_mode(position, false)
}
/// Check if a position is safe, with option to allow Unknown cells (SIMD-optimized).
///
/// In exploration mode, Unknown cells are allowed in the footprint
/// (they're assumed to be likely free). Only actual obstacles block.
///
/// Uses SIMD to quickly check the bounding box for obstacles before
/// performing the circular footprint check.
pub fn is_position_safe_mode(&self, position: WorldPoint, allow_unknown: bool) -> bool {
let center = self.storage.world_to_grid(position);
let resolution = self.storage.resolution();
let cells_radius = (self.footprint.total_radius() / resolution).ceil() as i32;
// Fast path: SIMD check for obstacles in bounding box
if !self.is_rect_obstacle_free(center, cells_radius, cells_radius) {
return false;
}
// If allow_unknown is true, we're done (no obstacles found via SIMD)
if allow_unknown {
// Still need to check bounds validity
return self.is_footprint_in_bounds(center, cells_radius, position);
}
// Strict mode: Need to check for Unknown cells in circular footprint
self.is_footprint_traversable(center, cells_radius, position)
}
/// Check if a position is known (all cells within footprint are known).
pub fn is_position_known(&self, position: WorldPoint) -> bool {
let center = self.storage.world_to_grid(position);
let resolution = self.storage.resolution();
let cells_radius = (self.footprint.total_radius() / resolution).ceil() as i32;
for dy in -cells_radius..=cells_radius {
for dx in -cells_radius..=cells_radius {
let coord = center + GridCoord::new(dx, dy);
if !self.storage.is_valid_coord(coord) {
return false;
}
let cell_world = self.storage.grid_to_world(coord);
let dist = cell_world.distance(&position);
if dist <= self.footprint.total_radius() && !self.storage.get_type(coord).is_known()
{
return false;
}
}
}
true
}
// =========================================================================
// SIMD-optimized obstacle checking
// =========================================================================
/// Check if a rectangular region contains any obstacle cells (SIMD-optimized).
///
/// Obstacles are Wall, Cliff, or Bump (cell_type >= 2).
/// Returns true if no obstacles found.
#[inline]
pub(super) fn is_rect_obstacle_free(
&self,
center: GridCoord,
half_width: i32,
half_height: i32,
) -> bool {
let width = self.storage.width() as i32;
let height = self.storage.height() as i32;
let min_x = (center.x - half_width).max(0) as usize;
let max_x = ((center.x + half_width).min(width - 1) as usize).min(self.storage.width() - 1);
let min_y = (center.y - half_height).max(0) as usize;
let max_y =
((center.y + half_height).min(height - 1) as usize).min(self.storage.height() - 1);
if min_x > max_x || min_y > max_y {
return false; // Out of bounds
}
let cell_types = self.storage.cell_types_raw();
let storage_width = self.storage.width();
// Wall, Cliff, Bump are >= 2 (Floor=1, Unknown=0)
let wall_val = CellType::Wall as u8;
let wall_vec = u8x16::splat(wall_val);
for y in min_y..=max_y {
let row_start = y * storage_width + min_x;
let row_end = y * storage_width + max_x + 1;
let row_slice = &cell_types[row_start..row_end];
// Process 16 cells at a time
let chunks = row_slice.chunks_exact(16);
let remainder = chunks.remainder();
for chunk in chunks {
let data = u8x16::from_slice(chunk);
// Check if any cell >= Wall (2)
let is_obstacle = data.simd_ge(wall_vec);
if is_obstacle.to_bitmask() != 0 {
return false;
}
}
// Scalar remainder
for &cell_type in remainder {
if cell_type >= wall_val {
return false;
}
}
}
true
}
// =========================================================================
// Footprint checking helpers
// =========================================================================
/// Check if all cells in the circular footprint are within bounds.
#[inline]
fn is_footprint_in_bounds(
&self,
center: GridCoord,
cells_radius: i32,
position: WorldPoint,
) -> bool {
for dy in -cells_radius..=cells_radius {
for dx in -cells_radius..=cells_radius {
let coord = center + GridCoord::new(dx, dy);
if !self.storage.is_valid_coord(coord) {
// Check if this cell is within the circular footprint
let cell_world = self.storage.grid_to_world(coord);
let dist = cell_world.distance(&position);
if dist <= self.footprint.total_radius() {
return false; // Out of bounds within footprint
}
}
}
}
true
}
/// Check if all cells in the circular footprint are traversable (Floor).
#[inline]
fn is_footprint_traversable(
&self,
center: GridCoord,
cells_radius: i32,
position: WorldPoint,
) -> bool {
for dy in -cells_radius..=cells_radius {
for dx in -cells_radius..=cells_radius {
let coord = center + GridCoord::new(dx, dy);
if !self.storage.is_valid_coord(coord) {
// Check if this cell is within the circular footprint
let cell_world = self.storage.grid_to_world(coord);
let dist = cell_world.distance(&position);
if dist <= self.footprint.total_radius() {
return false; // Out of bounds within footprint
}
continue;
}
// Check if within circular footprint
let cell_world = self.storage.grid_to_world(coord);
let dist = cell_world.distance(&position);
if dist <= self.footprint.total_radius() {
let cell_type = self.storage.get_type(coord);
if !cell_type.is_traversable() {
return false;
}
}
}
}
true
}
// =========================================================================
// Path checking
// =========================================================================
/// Check if a straight-line path is traversable.
/// Returns the furthest traversable point and whether the full path is clear.
pub fn check_path(
&self,
start: WorldPoint,
end: WorldPoint,
step_size: Option<f32>,
) -> PathCheckResult {
let step = step_size.unwrap_or(self.storage.resolution() * 0.5);
let total_dist = start.distance(&end);
if total_dist < step {
// Very short path - just check endpoints
let start_safe = self.is_position_safe(start);
let end_safe = self.is_position_safe(end);
return PathCheckResult {
is_clear: start_safe && end_safe,
blocked_at: if !start_safe {
Some(start)
} else if !end_safe {
Some(end)
} else {
None
},
furthest_safe: if start_safe { Some(end) } else { None },
distance_traversable: if start_safe && end_safe {
total_dist
} else {
0.0
},
};
}
let dir_x = (end.x - start.x) / total_dist;
let dir_y = (end.y - start.y) / total_dist;
let mut furthest_safe = start;
let mut distance_traversable = 0.0;
// Check start position first
if !self.is_position_safe(start) {
return PathCheckResult {
is_clear: false,
blocked_at: Some(start),
furthest_safe: None,
distance_traversable: 0.0,
};
}
let mut dist = step;
while dist < total_dist {
let point = WorldPoint::new(start.x + dir_x * dist, start.y + dir_y * dist);
if self.is_position_safe(point) {
furthest_safe = point;
distance_traversable = dist;
} else {
return PathCheckResult {
is_clear: false,
blocked_at: Some(point),
furthest_safe: Some(furthest_safe),
distance_traversable,
};
}
dist += step;
}
// Check endpoint
if self.is_position_safe(end) {
PathCheckResult {
is_clear: true,
blocked_at: None,
furthest_safe: Some(end),
distance_traversable: total_dist,
}
} else {
PathCheckResult {
is_clear: false,
blocked_at: Some(end),
furthest_safe: Some(furthest_safe),
distance_traversable,
}
}
}
/// Check if movement from current position in a direction is safe.
pub fn check_direction(
&self,
position: WorldPoint,
angle: f32,
distance: f32,
) -> PathCheckResult {
let end = WorldPoint::new(
position.x + distance * angle.cos(),
position.y + distance * angle.sin(),
);
self.check_path(position, end, None)
}
// =========================================================================
// Obstacle queries
// =========================================================================
/// Find the nearest obstacle from a position.
pub fn nearest_obstacle(&self, position: WorldPoint, max_distance: f32) -> Option<WorldPoint> {
let center = self.storage.world_to_grid(position);
let resolution = self.storage.resolution();
let max_cells = (max_distance / resolution).ceil() as i32;
let mut nearest: Option<(WorldPoint, f32)> = None;
for dy in -max_cells..=max_cells {
for dx in -max_cells..=max_cells {
let coord = center + GridCoord::new(dx, dy);
if !self.storage.is_valid_coord(coord) {
continue;
}
let cell_type = self.storage.get_type(coord);
if cell_type.is_obstacle() {
let obstacle_pos = self.storage.grid_to_world(coord);
let dist = position.distance(&obstacle_pos);
if dist <= max_distance {
match nearest {
None => nearest = Some((obstacle_pos, dist)),
Some((_, d)) if dist < d => nearest = Some((obstacle_pos, dist)),
_ => {}
}
}
}
}
}
nearest.map(|(p, _)| p)
}
/// Get clearance (distance to nearest obstacle) at a position.
pub fn clearance(&self, position: WorldPoint, max_distance: f32) -> f32 {
match self.nearest_obstacle(position, max_distance) {
Some(obstacle) => position.distance(&obstacle),
None => max_distance,
}
}
}
vastu-map/src/query/traversability/footprint.rs
+36
View File
@@ -0,0 +1,36 @@
//! Robot footprint definition for collision checking.
use serde::{Deserialize, Serialize};
/// Robot footprint for collision checking.
#[derive(Clone, Debug, Serialize, Deserialize)]
pub struct RobotFootprint {
/// Robot radius in meters.
pub radius: f32,
/// Safety margin in meters.
pub safety_margin: f32,
}
impl Default for RobotFootprint {
fn default() -> Self {
Self {
radius: 0.17, // CRL-200S robot radius
safety_margin: 0.05, // 5cm safety margin
}
}
}
impl RobotFootprint {
/// Create a new robot footprint.
pub fn new(radius: f32, safety_margin: f32) -> Self {
Self {
radius,
safety_margin,
}
}
/// Total radius including safety margin.
pub fn total_radius(&self) -> f32 {
self.radius + self.safety_margin
}
}
vastu-map/src/query/traversability.rs → vastu-map/src/query/traversability/mod.rs
+34 -437
View File
@@ -2,427 +2,24 @@
//!
//! Provides methods to check if positions and paths are safe for robot traversal.
use serde::{Deserialize, Serialize};
use std::simd::{cmp::SimdPartialOrd, u8x16};
mod checker;
mod footprint;
mod path;
use crate::core::{CellType, GridCoord, WorldPoint};
pub use checker::TraversabilityChecker;
pub use footprint::RobotFootprint;
pub use path::PathCheckResult;
use crate::core::WorldPoint;
use crate::grid::GridStorage;
/// Robot footprint for collision checking
#[derive(Clone, Debug, Serialize, Deserialize)]
pub struct RobotFootprint {
/// Robot radius in meters
pub radius: f32,
/// Safety margin in meters
pub safety_margin: f32,
}
impl Default for RobotFootprint {
fn default() -> Self {
Self {
radius: 0.17, // CRL-200S robot radius
safety_margin: 0.05, // 5cm safety margin
}
}
}
impl RobotFootprint {
/// Create a new robot footprint
pub fn new(radius: f32, safety_margin: f32) -> Self {
Self {
radius,
safety_margin,
}
}
/// Total radius including safety margin
pub fn total_radius(&self) -> f32 {
self.radius + self.safety_margin
}
}
/// Traversability checker
pub struct TraversabilityChecker<'a> {
storage: &'a GridStorage,
footprint: RobotFootprint,
}
impl<'a> TraversabilityChecker<'a> {
/// Create a new traversability checker
pub fn new(storage: &'a GridStorage, footprint: RobotFootprint) -> Self {
Self { storage, footprint }
}
/// Create with default robot footprint
pub fn with_default_footprint(storage: &'a GridStorage) -> Self {
Self::new(storage, RobotFootprint::default())
}
/// Check if a single cell is traversable
pub fn is_cell_traversable(&self, coord: GridCoord) -> bool {
if !self.storage.is_valid_coord(coord) {
return false;
}
self.storage.get_type(coord).is_traversable()
}
/// Check if a world point is traversable (single cell)
pub fn is_point_traversable(&self, point: WorldPoint) -> bool {
let coord = self.storage.world_to_grid(point);
self.is_cell_traversable(coord)
}
/// Check if a position is safe for the robot (considering footprint)
pub fn is_position_safe(&self, position: WorldPoint) -> bool {
self.is_position_safe_mode(position, false)
}
/// Check if a position is safe, with option to allow Unknown cells (SIMD-optimized).
///
/// In exploration mode, Unknown cells are allowed in the footprint
/// (they're assumed to be likely free). Only actual obstacles block.
///
/// Uses SIMD to quickly check the bounding box for obstacles before
/// performing the circular footprint check.
pub fn is_position_safe_mode(&self, position: WorldPoint, allow_unknown: bool) -> bool {
let center = self.storage.world_to_grid(position);
let resolution = self.storage.resolution();
let cells_radius = (self.footprint.total_radius() / resolution).ceil() as i32;
// Fast path: SIMD check for obstacles in bounding box
if !self.is_rect_obstacle_free(center, cells_radius, cells_radius) {
return false;
}
// If allow_unknown is true, we're done (no obstacles found via SIMD)
if allow_unknown {
// Still need to check bounds validity
return self.is_footprint_in_bounds(center, cells_radius, position);
}
// Strict mode: Need to check for Unknown cells in circular footprint
self.is_footprint_traversable(center, cells_radius, position)
}
/// Check if a rectangular region contains any obstacle cells (SIMD-optimized).
///
/// Obstacles are Wall, Cliff, or Bump (cell_type >= 2).
/// Returns true if no obstacles found.
#[inline]
fn is_rect_obstacle_free(&self, center: GridCoord, half_width: i32, half_height: i32) -> bool {
let width = self.storage.width() as i32;
let height = self.storage.height() as i32;
let min_x = (center.x - half_width).max(0) as usize;
let max_x = ((center.x + half_width).min(width - 1) as usize).min(self.storage.width() - 1);
let min_y = (center.y - half_height).max(0) as usize;
let max_y =
((center.y + half_height).min(height - 1) as usize).min(self.storage.height() - 1);
if min_x > max_x || min_y > max_y {
return false; // Out of bounds
}
let cell_types = self.storage.cell_types_raw();
let storage_width = self.storage.width();
// Wall, Cliff, Bump are >= 2 (Floor=1, Unknown=0)
let wall_val = CellType::Wall as u8;
let wall_vec = u8x16::splat(wall_val);
for y in min_y..=max_y {
let row_start = y * storage_width + min_x;
let row_end = y * storage_width + max_x + 1;
let row_slice = &cell_types[row_start..row_end];
// Process 16 cells at a time
let chunks = row_slice.chunks_exact(16);
let remainder = chunks.remainder();
for chunk in chunks {
let data = u8x16::from_slice(chunk);
// Check if any cell >= Wall (2)
let is_obstacle = data.simd_ge(wall_vec);
if is_obstacle.to_bitmask() != 0 {
return false;
}
}
// Scalar remainder
for &cell_type in remainder {
if cell_type >= wall_val {
return false;
}
}
}
true
}
/// Check if all cells in the circular footprint are within bounds.
#[inline]
fn is_footprint_in_bounds(
&self,
center: GridCoord,
cells_radius: i32,
position: WorldPoint,
) -> bool {
for dy in -cells_radius..=cells_radius {
for dx in -cells_radius..=cells_radius {
let coord = center + GridCoord::new(dx, dy);
if !self.storage.is_valid_coord(coord) {
// Check if this cell is within the circular footprint
let cell_world = self.storage.grid_to_world(coord);
let dist = cell_world.distance(&position);
if dist <= self.footprint.total_radius() {
return false; // Out of bounds within footprint
}
}
}
}
true
}
/// Check if all cells in the circular footprint are traversable (Floor).
#[inline]
fn is_footprint_traversable(
&self,
center: GridCoord,
cells_radius: i32,
position: WorldPoint,
) -> bool {
for dy in -cells_radius..=cells_radius {
for dx in -cells_radius..=cells_radius {
let coord = center + GridCoord::new(dx, dy);
if !self.storage.is_valid_coord(coord) {
// Check if this cell is within the circular footprint
let cell_world = self.storage.grid_to_world(coord);
let dist = cell_world.distance(&position);
if dist <= self.footprint.total_radius() {
return false; // Out of bounds within footprint
}
continue;
}
// Check if within circular footprint
let cell_world = self.storage.grid_to_world(coord);
let dist = cell_world.distance(&position);
if dist <= self.footprint.total_radius() {
let cell_type = self.storage.get_type(coord);
if !cell_type.is_traversable() {
return false;
}
}
}
}
true
}
/// Check if a position is known (all cells within footprint are known)
pub fn is_position_known(&self, position: WorldPoint) -> bool {
let center = self.storage.world_to_grid(position);
let resolution = self.storage.resolution();
let cells_radius = (self.footprint.total_radius() / resolution).ceil() as i32;
for dy in -cells_radius..=cells_radius {
for dx in -cells_radius..=cells_radius {
let coord = center + GridCoord::new(dx, dy);
if !self.storage.is_valid_coord(coord) {
return false;
}
let cell_world = self.storage.grid_to_world(coord);
let dist = cell_world.distance(&position);
if dist <= self.footprint.total_radius() && !self.storage.get_type(coord).is_known()
{
return false;
}
}
}
true
}
/// Check if a straight-line path is traversable
/// Returns the furthest traversable point and whether the full path is clear
pub fn check_path(
&self,
start: WorldPoint,
end: WorldPoint,
step_size: Option<f32>,
) -> PathCheckResult {
let step = step_size.unwrap_or(self.storage.resolution() * 0.5);
let total_dist = start.distance(&end);
if total_dist < step {
// Very short path - just check endpoints
let start_safe = self.is_position_safe(start);
let end_safe = self.is_position_safe(end);
return PathCheckResult {
is_clear: start_safe && end_safe,
blocked_at: if !start_safe {
Some(start)
} else if !end_safe {
Some(end)
} else {
None
},
furthest_safe: if start_safe { Some(end) } else { None },
distance_traversable: if start_safe && end_safe {
total_dist
} else {
0.0
},
};
}
let dir_x = (end.x - start.x) / total_dist;
let dir_y = (end.y - start.y) / total_dist;
let mut furthest_safe = start;
let mut distance_traversable = 0.0;
// Check start position first
if !self.is_position_safe(start) {
return PathCheckResult {
is_clear: false,
blocked_at: Some(start),
furthest_safe: None,
distance_traversable: 0.0,
};
}
let mut dist = step;
while dist < total_dist {
let point = WorldPoint::new(start.x + dir_x * dist, start.y + dir_y * dist);
if self.is_position_safe(point) {
furthest_safe = point;
distance_traversable = dist;
} else {
return PathCheckResult {
is_clear: false,
blocked_at: Some(point),
furthest_safe: Some(furthest_safe),
distance_traversable,
};
}
dist += step;
}
// Check endpoint
if self.is_position_safe(end) {
PathCheckResult {
is_clear: true,
blocked_at: None,
furthest_safe: Some(end),
distance_traversable: total_dist,
}
} else {
PathCheckResult {
is_clear: false,
blocked_at: Some(end),
furthest_safe: Some(furthest_safe),
distance_traversable,
}
}
}
/// Check if movement from current position in a direction is safe
pub fn check_direction(
&self,
position: WorldPoint,
angle: f32,
distance: f32,
) -> PathCheckResult {
let end = WorldPoint::new(
position.x + distance * angle.cos(),
position.y + distance * angle.sin(),
);
self.check_path(position, end, None)
}
/// Find the nearest obstacle from a position
pub fn nearest_obstacle(&self, position: WorldPoint, max_distance: f32) -> Option<WorldPoint> {
let center = self.storage.world_to_grid(position);
let resolution = self.storage.resolution();
let max_cells = (max_distance / resolution).ceil() as i32;
let mut nearest: Option<(WorldPoint, f32)> = None;
for dy in -max_cells..=max_cells {
for dx in -max_cells..=max_cells {
let coord = center + GridCoord::new(dx, dy);
if !self.storage.is_valid_coord(coord) {
continue;
}
let cell_type = self.storage.get_type(coord);
if cell_type.is_obstacle() {
let obstacle_pos = self.storage.grid_to_world(coord);
let dist = position.distance(&obstacle_pos);
if dist <= max_distance {
match nearest {
None => nearest = Some((obstacle_pos, dist)),
Some((_, d)) if dist < d => nearest = Some((obstacle_pos, dist)),
_ => {}
}
}
}
}
}
nearest.map(|(p, _)| p)
}
/// Get clearance (distance to nearest obstacle) at a position
pub fn clearance(&self, position: WorldPoint, max_distance: f32) -> f32 {
match self.nearest_obstacle(position, max_distance) {
Some(obstacle) => position.distance(&obstacle),
None => max_distance,
}
}
}
/// Result of path traversability check
#[derive(Clone, Debug)]
pub struct PathCheckResult {
/// Is the entire path clear?
pub is_clear: bool,
/// Position where the path is blocked (if blocked)
pub blocked_at: Option<WorldPoint>,
/// Furthest safe position along the path
pub furthest_safe: Option<WorldPoint>,
/// Distance that is traversable
pub distance_traversable: f32,
}
impl PathCheckResult {
/// Percentage of path that is traversable (0.0 to 1.0)
pub fn traversable_ratio(&self, total_length: f32) -> f32 {
if total_length <= 0.0 {
return 0.0;
}
(self.distance_traversable / total_length).min(1.0)
}
}
/// Quick check if a point is traversable
/// Quick check if a point is traversable.
pub fn is_traversable(storage: &GridStorage, point: WorldPoint) -> bool {
let checker = TraversabilityChecker::with_default_footprint(storage);
checker.is_point_traversable(point)
}
/// Quick check if a point is safe for the robot
/// Quick check if a point is safe for the robot.
pub fn is_safe(storage: &GridStorage, point: WorldPoint) -> bool {
let checker = TraversabilityChecker::with_default_footprint(storage);
checker.is_position_safe(point)
@@ -431,7 +28,7 @@ pub fn is_safe(storage: &GridStorage, point: WorldPoint) -> bool {
#[cfg(test)]
mod tests {
use super::*;
use crate::core::CellType;
use crate::core::{CellType, GridCoord};
use std::simd::{cmp::SimdPartialOrd, u8x16};
fn create_test_storage() -> GridStorage {
@@ -450,22 +47,22 @@ mod tests {
half_width: i32,
half_height: i32,
) -> bool {
let width = checker.storage.width() as i32;
let height = checker.storage.height() as i32;
let width = checker.storage().width() as i32;
let height = checker.storage().height() as i32;
let min_x = (center.x - half_width).max(0) as usize;
let max_x =
((center.x + half_width).min(width - 1) as usize).min(checker.storage.width() - 1);
((center.x + half_width).min(width - 1) as usize).min(checker.storage().width() - 1);
let min_y = (center.y - half_height).max(0) as usize;
let max_y =
((center.y + half_height).min(height - 1) as usize).min(checker.storage.height() - 1);
((center.y + half_height).min(height - 1) as usize).min(checker.storage().height() - 1);
if min_x > max_x || min_y > max_y {
return false; // Out of bounds
}
let cell_types = checker.storage.cell_types_raw();
let storage_width = checker.storage.width();
let cell_types = checker.storage().cell_types_raw();
let storage_width = checker.storage().width();
let wall_val = CellType::Wall as u8;
let wall_vec = u8x16::splat(wall_val);
@@ -499,9 +96,9 @@ mod tests {
/// Check if a position is safe using SIMD for bounding box pre-check.
fn is_position_safe_simd(checker: &TraversabilityChecker, position: WorldPoint) -> bool {
let center = checker.storage.world_to_grid(position);
let resolution = checker.storage.resolution();
let cells_radius = (checker.footprint.total_radius() / resolution).ceil() as i32;
let center = checker.storage().world_to_grid(position);
let resolution = checker.storage().resolution();
let cells_radius = (checker.footprint().total_radius() / resolution).ceil() as i32;
if !is_rect_obstacle_free(checker, center, cells_radius, cells_radius) {
return false;
@@ -521,15 +118,15 @@ mod tests {
for dx in -cells_radius..=cells_radius {
let coord = center + GridCoord::new(dx, dy);
if !checker.storage.is_valid_coord(coord) {
if !checker.storage().is_valid_coord(coord) {
return false;
}
let cell_world = checker.storage.grid_to_world(coord);
let cell_world = checker.storage().grid_to_world(coord);
let dist = cell_world.distance(&position);
if dist <= checker.footprint.total_radius() {
let cell_type = checker.storage.get_type(coord);
if dist <= checker.footprint().total_radius() {
let cell_type = checker.storage().get_type(coord);
if !cell_type.is_traversable() {
return false;
}
@@ -546,15 +143,15 @@ mod tests {
position: WorldPoint,
max_distance: f32,
) -> Option<WorldPoint> {
let center = checker.storage.world_to_grid(position);
let resolution = checker.storage.resolution();
let center = checker.storage().world_to_grid(position);
let resolution = checker.storage().resolution();
let max_cells = (max_distance / resolution).ceil() as i32;
let width = checker.storage.width() as i32;
let height = checker.storage.height() as i32;
let width = checker.storage().width() as i32;
let height = checker.storage().height() as i32;
let cell_types = checker.storage.cell_types_raw();
let storage_width = checker.storage.width();
let cell_types = checker.storage().cell_types_raw();
let storage_width = checker.storage().width();
let wall_val = CellType::Wall as u8;
let wall_vec = u8x16::splat(wall_val);
@@ -563,10 +160,10 @@ mod tests {
let min_y = (center.y - max_cells).max(0) as usize;
let max_y =
((center.y + max_cells).min(height - 1) as usize).min(checker.storage.height() - 1);
((center.y + max_cells).min(height - 1) as usize).min(checker.storage().height() - 1);
let min_x = (center.x - max_cells).max(0) as usize;
let max_x =
((center.x + max_cells).min(width - 1) as usize).min(checker.storage.width() - 1);
((center.x + max_cells).min(width - 1) as usize).min(checker.storage().width() - 1);
for y in min_y..=max_y {
let row_start = y * storage_width + min_x;
@@ -587,7 +184,7 @@ mod tests {
if (mask & (1 << bit)) != 0 {
let x = base_x + bit;
let coord = GridCoord::new(x as i32, y as i32);
let obstacle_pos = checker.storage.grid_to_world(coord);
let obstacle_pos = checker.storage().grid_to_world(coord);
let dist = position.distance(&obstacle_pos);
if dist <= max_distance {
@@ -609,7 +206,7 @@ mod tests {
if cell_type >= wall_val {
let x = remainder_start + i;
let coord = GridCoord::new(x as i32, y as i32);
let obstacle_pos = checker.storage.grid_to_world(coord);
let obstacle_pos = checker.storage().grid_to_world(coord);
let dist = position.distance(&obstacle_pos);
if dist <= max_distance {
vastu-map/src/query/traversability/path.rs
+26
View File
@@ -0,0 +1,26 @@
//! Path traversability checking result.
use crate::core::WorldPoint;
/// Result of path traversability check.
#[derive(Clone, Debug)]
pub struct PathCheckResult {
/// Is the entire path clear?
pub is_clear: bool,
/// Position where the path is blocked (if blocked).
pub blocked_at: Option<WorldPoint>,
/// Furthest safe position along the path.
pub furthest_safe: Option<WorldPoint>,
/// Distance that is traversable.
pub distance_traversable: f32,
}
impl PathCheckResult {
/// Percentage of path that is traversable (0.0 to 1.0).
pub fn traversable_ratio(&self, total_length: f32) -> f32 {
if total_length <= 0.0 {
return 0.0;
}
(self.distance_traversable / total_length).min(1.0)
}
}
vastu-map/src/slam/background_optimizer.rs
+554
View File
@@ -0,0 +1,554 @@
//! Background pose graph optimization.
//!
//! Provides a mechanism to run pose graph optimization asynchronously,
//! preventing the main SLAM thread from blocking during optimization.
//!
//! ## Architecture
//!
//! ```text
//! ┌─────────────────────────────────────────────────────────────────────┐
//! │ SLAM Thread │
//! │ ┌───────────┐ ┌───────────┐ ┌─────────────────────────────┐ │
//! │ │ Scan Recv │──▶│ Match │──▶│ Insert Submap + Add Pose │ │
//! │ └───────────┘ └───────────┘ └──────────────┬──────────────┘ │
//! │ │ │
//! │ ▼ │
//! │ ┌────────────────────────────────┐ │
//! │ │ Loop Closure Detector │ │
//! │ │ (LiDAR-IRIS + verification) │ │
//! │ └───────────────┬────────────────┘ │
//! └──────────────────────────────────────────────────┼──────────────────┘
//! │ trigger
//! ▼
//! ┌─────────────────────────────────────────────────────────────────────┐
//! │ Background Optimizer │
//! │ ┌──────────────┐ ┌──────────────┐ ┌──────────────────────┐ │
//! │ │ Copy Graph │──▶│ Optimize │──▶│ Return Corrections │ │
//! │ │ Snapshot │ │ (N iters) │ │ │ │
//! │ └──────────────┘ └──────────────┘ └──────────────────────┘ │
//! └─────────────────────────────────────────────────────────────────────┘
//! ```
//!
//! ## Usage Modes
//!
//! 1. **Synchronous** (embedded target): Call `optimize_sync()` directly
//! 2. **Threaded** (desktop): Spawn optimizer thread, use channels
//!
//! For embedded ARM (Allwinner A33), synchronous mode is recommended since
//! the additional thread overhead may not be worthwhile for the typical
//! 100-500ms optimization time that only occurs on loop closure.
use std::sync::atomic::{AtomicBool, Ordering};
use std::sync::{Arc, Mutex, mpsc};
use std::thread::{self, JoinHandle};
use serde::{Deserialize, Serialize};
use crate::core::Pose2D;
use crate::slam::loop_closure::{LoopClosure, PoseGraph, PoseGraphConfig};
/// Configuration for background optimizer.
#[derive(Clone, Debug, Serialize, Deserialize)]
pub struct BackgroundOptimizerConfig {
/// Maximum iterations per optimization run.
#[serde(default = "default_max_iterations")]
pub max_iterations: usize,
/// Convergence threshold for pose change.
#[serde(default = "default_convergence_threshold")]
pub convergence_threshold: f32,
/// Trigger optimization every N nodes (0 = only on loop closure).
#[serde(default = "default_trigger_every_n_nodes")]
pub trigger_every_n_nodes: usize,
/// Huber loss scale parameter (meters).
#[serde(default = "default_huber_scale")]
pub huber_scale: f32,
/// Whether to use background thread (vs synchronous).
#[serde(default = "default_use_thread")]
pub use_background_thread: bool,
}
fn default_max_iterations() -> usize {
50
}
fn default_convergence_threshold() -> f32 {
1e-5
}
fn default_trigger_every_n_nodes() -> usize {
0 // Only on loop closure by default
}
fn default_huber_scale() -> f32 {
0.1 // 10cm
}
fn default_use_thread() -> bool {
false // Synchronous by default for embedded
}
impl Default for BackgroundOptimizerConfig {
fn default() -> Self {
Self {
max_iterations: default_max_iterations(),
convergence_threshold: default_convergence_threshold(),
trigger_every_n_nodes: default_trigger_every_n_nodes(),
huber_scale: default_huber_scale(),
use_background_thread: default_use_thread(),
}
}
}
/// A pose correction to apply after optimization.
#[derive(Clone, Debug)]
pub struct PoseCorrection {
/// Pose index in the graph.
pub pose_idx: usize,
/// Original pose before optimization.
pub original: Pose2D,
/// Optimized pose.
pub optimized: Pose2D,
/// Delta (optimized - original).
pub delta: Pose2D,
}
/// Result of an optimization run.
#[derive(Clone, Debug)]
pub struct OptimizationResult {
/// Pose corrections to apply.
pub corrections: Vec<PoseCorrection>,
/// Number of iterations performed.
pub iterations: usize,
/// Final error after optimization.
pub final_error: f32,
/// Error before optimization.
pub initial_error: f32,
/// Whether optimization converged.
pub converged: bool,
}
/// Trigger type for optimization.
#[derive(Clone, Debug)]
pub enum OptimizerTrigger {
/// Trigger due to loop closure detection.
LoopClosure(LoopClosure),
/// Trigger due to node count threshold.
NodeCount(usize),
/// Manual trigger.
Manual,
}
/// Background pose graph optimizer.
///
/// Can operate in two modes:
/// 1. **Synchronous**: Call `optimize_sync()` directly (blocks caller)
/// 2. **Threaded**: Use `start_background_thread()` and channels
pub struct BackgroundOptimizer {
/// Configuration.
config: BackgroundOptimizerConfig,
/// The pose graph being optimized.
graph: Arc<Mutex<PoseGraph>>,
/// Background thread handle (if running).
thread_handle: Option<JoinHandle<()>>,
/// Channel to send triggers to background thread.
trigger_tx: Option<mpsc::Sender<OptimizerTrigger>>,
/// Channel to receive results from background thread.
result_rx: Option<mpsc::Receiver<OptimizationResult>>,
/// Flag to stop background thread.
should_stop: Arc<AtomicBool>,
/// Count of poses added since last optimization.
poses_since_optimization: usize,
}
impl BackgroundOptimizer {
/// Create a new background optimizer.
pub fn new(config: BackgroundOptimizerConfig) -> Self {
let graph_config = PoseGraphConfig {
max_iterations: config.max_iterations,
convergence_threshold: config.convergence_threshold,
huber_scale: config.huber_scale,
..PoseGraphConfig::default()
};
Self {
config,
graph: Arc::new(Mutex::new(PoseGraph::new(graph_config))),
thread_handle: None,
trigger_tx: None,
result_rx: None,
should_stop: Arc::new(AtomicBool::new(false)),
poses_since_optimization: 0,
}
}
/// Create with default configuration.
pub fn with_defaults() -> Self {
Self::new(BackgroundOptimizerConfig::default())
}
/// Get a reference to the pose graph.
pub fn graph(&self) -> &Arc<Mutex<PoseGraph>> {
&self.graph
}
/// Add a pose to the graph.
///
/// Returns the pose index and an optional trigger if optimization should run.
pub fn add_pose(
&mut self,
pose: Pose2D,
odom_delta: Option<Pose2D>,
) -> (usize, Option<OptimizerTrigger>) {
let pose_idx = {
let mut graph = self.graph.lock().unwrap();
graph.add_pose(pose, odom_delta)
};
self.poses_since_optimization += 1;
// Check if we should trigger optimization based on node count
let trigger = if self.config.trigger_every_n_nodes > 0
&& self.poses_since_optimization >= self.config.trigger_every_n_nodes
{
self.poses_since_optimization = 0;
Some(OptimizerTrigger::NodeCount(pose_idx))
} else {
None
};
(pose_idx, trigger)
}
/// Add a loop closure constraint.
///
/// Returns a trigger to start optimization.
pub fn add_loop_closure(&mut self, closure: LoopClosure) -> OptimizerTrigger {
{
let mut graph = self.graph.lock().unwrap();
graph.add_loop_closure(closure.clone());
}
self.poses_since_optimization = 0;
OptimizerTrigger::LoopClosure(closure)
}
/// Run optimization synchronously.
///
/// This blocks the caller until optimization is complete.
/// Use this for embedded targets where threading overhead isn't worthwhile.
pub fn optimize_sync(&mut self) -> OptimizationResult {
let mut graph = self.graph.lock().unwrap();
let initial_error = graph.total_error();
let iterations = graph.optimize();
let final_error = graph.total_error();
// Compute corrections
let poses = graph.poses();
let corrections = Self::compute_corrections_from_graph(&graph, poses);
OptimizationResult {
corrections,
iterations,
final_error,
initial_error,
converged: iterations < self.config.max_iterations,
}
}
/// Start the background optimization thread.
///
/// After calling this, use `trigger_optimization()` to start optimization
/// and `try_receive_result()` to check for completed results.
pub fn start_background_thread(&mut self) {
if self.thread_handle.is_some() {
return; // Already running
}
let (trigger_tx, trigger_rx) = mpsc::channel::<OptimizerTrigger>();
let (result_tx, result_rx) = mpsc::channel::<OptimizationResult>();
self.trigger_tx = Some(trigger_tx);
self.result_rx = Some(result_rx);
self.should_stop.store(false, Ordering::SeqCst);
let graph: Arc<Mutex<PoseGraph>> = Arc::clone(&self.graph);
let should_stop = Arc::clone(&self.should_stop);
let max_iterations = self.config.max_iterations;
let handle = thread::spawn(move || {
while !should_stop.load(Ordering::SeqCst) {
// Wait for trigger
match trigger_rx.recv_timeout(std::time::Duration::from_millis(100)) {
Ok(_trigger) => {
// Perform optimization
let result = {
let mut graph_lock = graph.lock().unwrap();
let initial_error = graph_lock.total_error();
let iterations = graph_lock.optimize();
let final_error = graph_lock.total_error();
let poses = graph_lock.poses();
let corrections =
Self::compute_corrections_from_graph(&graph_lock, poses);
OptimizationResult {
corrections,
iterations,
final_error,
initial_error,
converged: iterations < max_iterations,
}
};
// Send result back
let _ = result_tx.send(result);
}
Err(mpsc::RecvTimeoutError::Timeout) => {
// No trigger, continue waiting
}
Err(mpsc::RecvTimeoutError::Disconnected) => {
break; // Channel closed
}
}
}
});
self.thread_handle = Some(handle);
}
/// Trigger optimization in the background thread.
///
/// Returns `true` if the trigger was sent successfully.
pub fn trigger_optimization(&self, trigger: OptimizerTrigger) -> bool {
if let Some(ref tx) = self.trigger_tx {
tx.send(trigger).is_ok()
} else {
false
}
}
/// Try to receive a result from the background thread.
///
/// Returns `None` if no result is available yet.
pub fn try_receive_result(&self) -> Option<OptimizationResult> {
if let Some(ref rx) = self.result_rx {
rx.try_recv().ok()
} else {
None
}
}
/// Block until a result is available from the background thread.
///
/// Returns `None` if the channel is disconnected.
pub fn wait_for_result(&self) -> Option<OptimizationResult> {
if let Some(ref rx) = self.result_rx {
rx.recv().ok()
} else {
None
}
}
/// Stop the background thread.
pub fn stop_background_thread(&mut self) {
self.should_stop.store(true, Ordering::SeqCst);
// Drop the trigger channel to unblock the thread
self.trigger_tx = None;
if let Some(handle) = self.thread_handle.take() {
let _ = handle.join();
}
self.result_rx = None;
}
/// Check if the background thread is running.
pub fn is_background_running(&self) -> bool {
self.thread_handle.is_some()
}
/// Get the number of poses in the graph.
pub fn pose_count(&self) -> usize {
self.graph.lock().unwrap().len()
}
/// Check if the graph has any loop closures.
pub fn has_loops(&self) -> bool {
self.graph.lock().unwrap().has_loops()
}
/// Reset the graph and optimizer state.
pub fn reset(&mut self) {
let mut graph = self.graph.lock().unwrap();
graph.reset();
self.poses_since_optimization = 0;
}
/// Compute corrections from optimized graph.
fn compute_corrections_from_graph(
_graph: &PoseGraph,
optimized_poses: &[Pose2D],
) -> Vec<PoseCorrection> {
// Since we optimize in-place, we need to track the deltas
// For now, return the current poses as the "optimized" values
// In a real implementation, we'd snapshot before and after
optimized_poses
.iter()
.enumerate()
.map(|(idx, &pose)| {
PoseCorrection {
pose_idx: idx,
original: pose, // These would be the original poses
optimized: pose,
delta: Pose2D::new(0.0, 0.0, 0.0), // Would be actual delta
}
})
.collect()
}
/// Get optimized poses directly.
pub fn get_optimized_poses(&self) -> Vec<Pose2D> {
let graph = self.graph.lock().unwrap();
graph.poses().to_vec()
}
}
impl Drop for BackgroundOptimizer {
fn drop(&mut self) {
self.stop_background_thread();
}
}
#[cfg(test)]
mod tests {
use super::*;
#[test]
fn test_optimizer_creation() {
let optimizer = BackgroundOptimizer::with_defaults();
assert_eq!(optimizer.pose_count(), 0);
assert!(!optimizer.has_loops());
}
#[test]
fn test_add_poses() {
let mut optimizer = BackgroundOptimizer::with_defaults();
// Add first pose
let (idx0, trigger0) = optimizer.add_pose(Pose2D::new(0.0, 0.0, 0.0), None);
assert_eq!(idx0, 0);
assert!(trigger0.is_none());
// Add second pose with odometry
let delta = Pose2D::new(1.0, 0.0, 0.1);
let (idx1, _) = optimizer.add_pose(Pose2D::new(1.0, 0.0, 0.1), Some(delta));
assert_eq!(idx1, 1);
assert_eq!(optimizer.pose_count(), 2);
}
#[test]
fn test_node_count_trigger() {
let config = BackgroundOptimizerConfig {
trigger_every_n_nodes: 3,
..Default::default()
};
let mut optimizer = BackgroundOptimizer::new(config);
// Add poses until trigger
let (_, t1) = optimizer.add_pose(Pose2D::new(0.0, 0.0, 0.0), None);
assert!(t1.is_none());
let (_, t2) =
optimizer.add_pose(Pose2D::new(1.0, 0.0, 0.0), Some(Pose2D::new(1.0, 0.0, 0.0)));
assert!(t2.is_none());
let (_, t3) =
optimizer.add_pose(Pose2D::new(2.0, 0.0, 0.0), Some(Pose2D::new(1.0, 0.0, 0.0)));
assert!(matches!(t3, Some(OptimizerTrigger::NodeCount(_))));
}
#[test]
fn test_loop_closure_trigger() {
let mut optimizer = BackgroundOptimizer::with_defaults();
// Add some poses
optimizer.add_pose(Pose2D::new(0.0, 0.0, 0.0), None);
optimizer.add_pose(Pose2D::new(1.0, 0.0, 0.0), Some(Pose2D::new(1.0, 0.0, 0.0)));
optimizer.add_pose(Pose2D::new(2.0, 0.0, 0.0), Some(Pose2D::new(1.0, 0.0, 0.0)));
// Add loop closure
let closure = LoopClosure {
from_idx: 2,
to_idx: 0,
relative_pose: Pose2D::new(-2.0, 0.0, 0.0),
confidence: 0.9,
descriptor_distance: 50,
rotation_offset: 0,
};
let trigger = optimizer.add_loop_closure(closure);
assert!(matches!(trigger, OptimizerTrigger::LoopClosure(_)));
assert!(optimizer.has_loops());
}
#[test]
fn test_sync_optimization() {
let mut optimizer = BackgroundOptimizer::with_defaults();
// Build a simple chain of poses
optimizer.add_pose(Pose2D::new(0.0, 0.0, 0.0), None);
optimizer.add_pose(Pose2D::new(1.0, 0.0, 0.0), Some(Pose2D::new(1.0, 0.0, 0.0)));
optimizer.add_pose(Pose2D::new(2.0, 0.0, 0.0), Some(Pose2D::new(1.0, 0.0, 0.0)));
// Run synchronous optimization
let result = optimizer.optimize_sync();
assert!(result.iterations > 0 || result.converged);
assert!(!result.corrections.is_empty());
}
#[test]
fn test_background_thread() {
let mut optimizer = BackgroundOptimizer::with_defaults();
// Add some poses
optimizer.add_pose(Pose2D::new(0.0, 0.0, 0.0), None);
optimizer.add_pose(Pose2D::new(1.0, 0.0, 0.0), Some(Pose2D::new(1.0, 0.0, 0.0)));
// Start background thread
optimizer.start_background_thread();
assert!(optimizer.is_background_running());
// Trigger optimization
let triggered = optimizer.trigger_optimization(OptimizerTrigger::Manual);
assert!(triggered);
// Wait for result
let result = optimizer.wait_for_result();
assert!(result.is_some());
// Stop thread
optimizer.stop_background_thread();
assert!(!optimizer.is_background_running());
}
#[test]
fn test_reset() {
let mut optimizer = BackgroundOptimizer::with_defaults();
optimizer.add_pose(Pose2D::new(0.0, 0.0, 0.0), None);
optimizer.add_pose(Pose2D::new(1.0, 0.0, 0.0), Some(Pose2D::new(1.0, 0.0, 0.0)));
assert_eq!(optimizer.pose_count(), 2);
optimizer.reset();
assert_eq!(optimizer.pose_count(), 0);
}
}
vastu-map/src/slam/branch_bound/algorithm.rs
+290
View File
@@ -0,0 +1,290 @@
//! Branch-and-bound search algorithm implementation.
use crate::core::simd::PointCloud;
use crate::core::{LidarScan, Pose2D, WorldPoint};
use super::config::{BranchBoundConfig, BranchBoundResult, NUM_LEVELS};
use super::grids::PrecomputedGrids;
use super::search_node::SearchNode;
/// Perform branch-and-bound scan matching.
///
/// This is more efficient than brute-force for large search windows.
/// For small windows (< 30cm), brute-force may be faster due to overhead.
///
/// # Arguments
/// * `points` - Scan points in sensor frame (x, y)
/// * `grids` - Precomputed multi-resolution grids
/// * `prior_pose` - Initial pose estimate
/// * `config` - Search configuration
///
/// # Returns
/// The best matching pose and score.
pub fn branch_and_bound_match(
points: &[(f32, f32)],
grids: &PrecomputedGrids,
prior_pose: Pose2D,
config: &BranchBoundConfig,
) -> BranchBoundResult {
if points.is_empty() {
return BranchBoundResult {
pose: prior_pose,
score: 0.0,
nodes_expanded: 0,
converged: false,
};
}
// Calculate angular bins
let num_theta_bins = (config.search_theta * 2.0 / config.angular_resolution).ceil() as i32;
let theta_step = config.angular_resolution;
// Calculate search bounds in coarsest grid coordinates
let coarsest_level = NUM_LEVELS - 1;
let search_min = WorldPoint::new(
prior_pose.x - config.search_x,
prior_pose.y - config.search_y,
);
let search_max = WorldPoint::new(
prior_pose.x + config.search_x,
prior_pose.y + config.search_y,
);
let min_coord = grids.world_to_level_grid(search_min, coarsest_level);
let max_coord = grids.world_to_level_grid(search_max, coarsest_level);
// Initialize search with coarsest nodes
let mut stack: Vec<SearchNode> = Vec::with_capacity(256);
let mut best_score = config.min_score;
let mut best_pose = prior_pose;
let mut nodes_expanded = 0usize;
// Start theta search around prior
let prior_theta_bin = (prior_pose.theta / theta_step).round() as i32;
let theta_range = (config.search_theta / theta_step).ceil() as i32;
for y in min_coord.y..=max_coord.y {
for x in min_coord.x..=max_coord.x {
for dt in -theta_range..=theta_range {
let theta_bin = prior_theta_bin + dt;
stack.push(SearchNode {
level: coarsest_level,
x,
y,
theta_bin,
upper_bound: f32::MAX, // Will compute on first access
});
}
}
}
// Branch-and-bound search
while let Some(mut node) = stack.pop() {
nodes_expanded += 1;
// Compute upper bound if not yet computed
if node.upper_bound == f32::MAX {
let pose = node.center_pose(grids, theta_step);
node.upper_bound =
grids.score_scan_at_level(points, pose, node.level, config.sensor_offset);
}
// Prune if upper bound is worse than current best
if node.upper_bound <= best_score {
continue;
}
if node.level == 0 {
// Finest level - this is the actual score
let pose = node.center_pose(grids, theta_step);
let score = grids.score_scan_at_level(points, pose, 0, config.sensor_offset);
if score > best_score {
best_score = score;
best_pose = pose;
}
} else {
// Expand to finer level
let children = node.subdivide(grids, num_theta_bins);
for mut child in children {
let child_pose = child.center_pose(grids, theta_step);
child.upper_bound = grids.score_scan_at_level(
points,
child_pose,
child.level,
config.sensor_offset,
);
// Only add if upper bound could beat current best
if child.upper_bound > best_score {
stack.push(child);
}
}
}
}
BranchBoundResult {
pose: best_pose,
score: best_score,
nodes_expanded,
converged: best_score > config.min_score,
}
}
/// SIMD-optimized branch-and-bound scan matching.
///
/// Uses PointCloud (SoA format) for efficient SIMD point transformations.
/// This is the preferred method for loop closure verification.
///
/// # Arguments
/// * `points` - Scan points in sensor frame (PointCloud SoA format)
/// * `grids` - Precomputed multi-resolution grids
/// * `prior_pose` - Initial pose estimate
/// * `config` - Search configuration
///
/// # Returns
/// The best matching pose and score.
pub fn branch_and_bound_match_simd(
points: &PointCloud,
grids: &PrecomputedGrids,
prior_pose: Pose2D,
config: &BranchBoundConfig,
) -> BranchBoundResult {
if points.is_empty() {
return BranchBoundResult {
pose: prior_pose,
score: 0.0,
nodes_expanded: 0,
converged: false,
};
}
// Reusable scratch buffers for SIMD transforms
let mut world_xs = Vec::with_capacity(points.len());
let mut world_ys = Vec::with_capacity(points.len());
// Calculate angular bins
let num_theta_bins = (config.search_theta * 2.0 / config.angular_resolution).ceil() as i32;
let theta_step = config.angular_resolution;
// Calculate search bounds in coarsest grid coordinates
let coarsest_level = NUM_LEVELS - 1;
let search_min = WorldPoint::new(
prior_pose.x - config.search_x,
prior_pose.y - config.search_y,
);
let search_max = WorldPoint::new(
prior_pose.x + config.search_x,
prior_pose.y + config.search_y,
);
let min_coord = grids.world_to_level_grid(search_min, coarsest_level);
let max_coord = grids.world_to_level_grid(search_max, coarsest_level);
// Initialize search with coarsest nodes
let mut stack: Vec<SearchNode> = Vec::with_capacity(256);
let mut best_score = config.min_score;
let mut best_pose = prior_pose;
let mut nodes_expanded = 0usize;
// Start theta search around prior
let prior_theta_bin = (prior_pose.theta / theta_step).round() as i32;
let theta_range = (config.search_theta / theta_step).ceil() as i32;
for y in min_coord.y..=max_coord.y {
for x in min_coord.x..=max_coord.x {
for dt in -theta_range..=theta_range {
let theta_bin = prior_theta_bin + dt;
stack.push(SearchNode {
level: coarsest_level,
x,
y,
theta_bin,
upper_bound: f32::MAX,
});
}
}
}
// Branch-and-bound search with SIMD scoring
while let Some(mut node) = stack.pop() {
nodes_expanded += 1;
// Compute upper bound if not yet computed
if node.upper_bound == f32::MAX {
let pose = node.center_pose(grids, theta_step);
node.upper_bound = grids.score_scan_simd_with_scratch(
points,
pose,
node.level,
config.sensor_offset,
&mut world_xs,
&mut world_ys,
);
}
// Prune if upper bound is worse than current best
if node.upper_bound <= best_score {
continue;
}
if node.level == 0 {
// Finest level - this is the actual score
let pose = node.center_pose(grids, theta_step);
let score = grids.score_scan_simd_with_scratch(
points,
pose,
0,
config.sensor_offset,
&mut world_xs,
&mut world_ys,
);
if score > best_score {
best_score = score;
best_pose = pose;
}
} else {
// Expand to finer level
let children = node.subdivide(grids, num_theta_bins);
for mut child in children {
let child_pose = child.center_pose(grids, theta_step);
child.upper_bound = grids.score_scan_simd_with_scratch(
points,
child_pose,
child.level,
config.sensor_offset,
&mut world_xs,
&mut world_ys,
);
// Only add if upper bound could beat current best
if child.upper_bound > best_score {
stack.push(child);
}
}
}
}
BranchBoundResult {
pose: best_pose,
score: best_score,
nodes_expanded,
converged: best_score > config.min_score,
}
}
/// Convenience function to match a LidarScan directly.
///
/// Converts the scan to PointCloud format and calls branch_and_bound_match_simd.
pub fn branch_and_bound_match_scan(
scan: &LidarScan,
grids: &PrecomputedGrids,
prior_pose: Pose2D,
config: &BranchBoundConfig,
) -> BranchBoundResult {
let points = PointCloud::from_scan(scan);
branch_and_bound_match_simd(&points, grids, prior_pose, config)
}
vastu-map/src/slam/branch_bound/config.rs
+50
View File
@@ -0,0 +1,50 @@
//! Branch-and-bound configuration and result types.
use crate::core::Pose2D;
/// Number of resolution levels in the precomputed hierarchy.
/// Level 0 = original resolution, Level N = 4^N times coarser.
pub const NUM_LEVELS: usize = 4;
/// Configuration for branch-and-bound matching.
#[derive(Clone, Debug)]
pub struct BranchBoundConfig {
/// Search window in X (meters).
pub search_x: f32,
/// Search window in Y (meters).
pub search_y: f32,
/// Search window in theta (radians).
pub search_theta: f32,
/// Angular resolution (radians per bin).
pub angular_resolution: f32,
/// Minimum score to consider a match valid.
pub min_score: f32,
/// Sensor offset (x, y) in robot frame.
pub sensor_offset: (f32, f32),
}
impl Default for BranchBoundConfig {
fn default() -> Self {
Self {
search_x: 0.3,
search_y: 0.3,
search_theta: 0.3,
angular_resolution: 0.02, // ~1.15 degrees
min_score: 0.0,
sensor_offset: (-0.110, 0.0),
}
}
}
/// Result of branch-and-bound matching.
#[derive(Clone, Debug)]
pub struct BranchBoundResult {
/// Best matching pose.
pub pose: Pose2D,
/// Score of the best match.
pub score: f32,
/// Number of nodes expanded.
pub nodes_expanded: usize,
/// Whether a valid match was found.
pub converged: bool,
}
vastu-map/src/slam/branch_bound/grids.rs
+354
View File
@@ -0,0 +1,354 @@
//! Precomputed multi-resolution grid hierarchy.
use crate::core::simd::{PointCloud, RotationMatrix4, transform_points_simd4};
use crate::core::{GridCoord, Pose2D, WorldPoint};
use crate::grid::GridStorage;
use super::config::NUM_LEVELS;
/// Precomputed multi-resolution grid hierarchy for branch-and-bound matching.
///
/// Each level stores probabilities (or distance-to-wall scores) where coarser
/// levels contain the maximum of all finer cells within each coarse cell.
pub struct PrecomputedGrids {
/// Resolution levels from finest (0) to coarsest (NUM_LEVELS-1).
/// Level i has cells 4^i times larger than level 0.
pub(super) levels: [Vec<f32>; NUM_LEVELS],
/// Width of each level in cells.
pub(super) widths: [usize; NUM_LEVELS],
/// Height of each level in cells.
pub(super) heights: [usize; NUM_LEVELS],
/// Resolution of level 0 (same as source grid).
pub(super) resolution: f32,
/// Origin of level 0 (same as source grid).
pub(super) origin: WorldPoint,
}
impl PrecomputedGrids {
/// Build precomputed grids from a grid storage.
///
/// Level 0 is the original resolution, each subsequent level
/// is 4x coarser (2x in each dimension).
pub fn from_storage(storage: &GridStorage) -> Self {
let width = storage.width();
let height = storage.height();
let resolution = storage.resolution();
let origin = storage.origin();
// Initialize level 0 from storage
let mut levels: [Vec<f32>; NUM_LEVELS] = Default::default();
let mut widths = [0usize; NUM_LEVELS];
let mut heights = [0usize; NUM_LEVELS];
widths[0] = width;
heights[0] = height;
levels[0] = Vec::with_capacity(width * height);
// Convert grid to probability/score values
// Use Gaussian scoring based on distance field for smooth values
let sigma = resolution * 2.0;
let two_sigma_sq = 2.0 * sigma * sigma;
for y in 0..height {
for x in 0..width {
let coord = GridCoord::new(x as i32, y as i32);
let distance = storage.get_distance(coord);
// Gaussian score: high near walls, low far from walls
let score = if distance < f32::MAX / 2.0 {
(-distance * distance / two_sigma_sq).exp()
} else {
// Unknown area - small positive score for exploration
0.1
};
levels[0].push(score);
}
}
// Build coarser levels by taking max of 2x2 regions
for level in 1..NUM_LEVELS {
let prev_width = widths[level - 1];
let prev_height = heights[level - 1];
// Round up to ensure we cover all cells
let new_width = prev_width.div_ceil(2);
let new_height = prev_height.div_ceil(2);
widths[level] = new_width;
heights[level] = new_height;
let mut new_grid = vec![0.0f32; new_width * new_height];
for y in 0..new_height {
for x in 0..new_width {
// Find max of 2x2 region in previous level
let mut max_val = 0.0f32;
for dy in 0..2 {
for dx in 0..2 {
let px = x * 2 + dx;
let py = y * 2 + dy;
if px < prev_width && py < prev_height {
let idx = py * prev_width + px;
max_val = max_val.max(levels[level - 1][idx]);
}
}
}
new_grid[y * new_width + x] = max_val;
}
}
levels[level] = new_grid;
}
Self {
levels,
widths,
heights,
resolution,
origin,
}
}
/// Get the score at a specific level and grid coordinate.
#[inline]
pub(super) fn get_score(&self, level: usize, x: i32, y: i32) -> f32 {
if x < 0 || y < 0 {
return 0.0;
}
let x = x as usize;
let y = y as usize;
if x >= self.widths[level] || y >= self.heights[level] {
return 0.0;
}
self.levels[level][y * self.widths[level] + x]
}
/// Get the resolution at a specific level.
#[inline]
pub(super) fn level_resolution(&self, level: usize) -> f32 {
self.resolution * (1 << level) as f32
}
/// Convert world point to grid coordinates at a specific level.
#[inline]
pub(super) fn world_to_level_grid(&self, point: WorldPoint, level: usize) -> GridCoord {
let res = self.level_resolution(level);
let x = ((point.x - self.origin.x) / res).floor() as i32;
let y = ((point.y - self.origin.y) / res).floor() as i32;
GridCoord::new(x, y)
}
/// Score a scan at a given pose and level (upper bound for coarser levels).
pub(super) fn score_scan_at_level(
&self,
points: &[(f32, f32)],
pose: Pose2D,
level: usize,
sensor_offset: (f32, f32),
) -> f32 {
let cos_theta = pose.theta.cos();
let sin_theta = pose.theta.sin();
// Sensor position in world frame
let (offset_x, offset_y) = sensor_offset;
let sensor_x = pose.x + offset_x * cos_theta - offset_y * sin_theta;
let sensor_y = pose.y + offset_x * sin_theta + offset_y * cos_theta;
let mut score = 0.0f32;
for &(px, py) in points {
// Transform point from sensor frame to world frame
let world_x = sensor_x + px * cos_theta - py * sin_theta;
let world_y = sensor_y + px * sin_theta + py * cos_theta;
let coord = self.world_to_level_grid(WorldPoint::new(world_x, world_y), level);
score += self.get_score(level, coord.x, coord.y);
}
score
}
// =========================================================================
// SIMD-OPTIMIZED METHODS
// =========================================================================
/// Score a scan at a given pose and level using SIMD-optimized transforms.
///
/// Uses PointCloud (SoA format) and SIMD point transformation for efficiency.
/// The final scoring is scalar (random grid access is not SIMD-friendly).
pub fn score_scan_simd(
&self,
points: &PointCloud,
pose: Pose2D,
level: usize,
sensor_offset: (f32, f32),
) -> f32 {
let n = points.len();
if n == 0 {
return 0.0;
}
// Compute sensor pose in world frame
let cos_t = pose.theta.cos();
let sin_t = pose.theta.sin();
let sensor_x = pose.x + sensor_offset.0 * cos_t - sensor_offset.1 * sin_t;
let sensor_y = pose.y + sensor_offset.0 * sin_t + sensor_offset.1 * cos_t;
let sensor_pose = Pose2D::new(sensor_x, sensor_y, pose.theta);
// Transform all points to world frame using SIMD
let rot = RotationMatrix4::from_pose(&sensor_pose);
let mut world_xs = vec![0.0f32; n];
let mut world_ys = vec![0.0f32; n];
transform_points_simd4(points, &rot, &mut world_xs, &mut world_ys);
// Convert to grid coordinates and sum scores
let level_res = self.level_resolution(level);
let inv_res = 1.0 / level_res;
let width = self.widths[level] as i32;
let height = self.heights[level] as i32;
let mut score = 0.0f32;
for i in 0..n {
let gx = ((world_xs[i] - self.origin.x) * inv_res).floor() as i32;
let gy = ((world_ys[i] - self.origin.y) * inv_res).floor() as i32;
if gx >= 0 && gy >= 0 && gx < width && gy < height {
score += self.levels[level][(gy as usize) * (width as usize) + (gx as usize)];
}
}
score
}
/// Score scan with reusable scratch buffers to avoid allocations.
///
/// This is the most efficient version for repeated calls.
pub fn score_scan_simd_with_scratch(
&self,
points: &PointCloud,
pose: Pose2D,
level: usize,
sensor_offset: (f32, f32),
world_xs: &mut Vec<f32>,
world_ys: &mut Vec<f32>,
) -> f32 {
let n = points.len();
if n == 0 {
return 0.0;
}
// Ensure scratch buffers are large enough
world_xs.resize(n, 0.0);
world_ys.resize(n, 0.0);
// Compute sensor pose in world frame
let cos_t = pose.theta.cos();
let sin_t = pose.theta.sin();
let sensor_x = pose.x + sensor_offset.0 * cos_t - sensor_offset.1 * sin_t;
let sensor_y = pose.y + sensor_offset.0 * sin_t + sensor_offset.1 * cos_t;
let sensor_pose = Pose2D::new(sensor_x, sensor_y, pose.theta);
// Transform all points using SIMD
let rot = RotationMatrix4::from_pose(&sensor_pose);
transform_points_simd4(points, &rot, world_xs, world_ys);
// Convert to grid coordinates and sum scores
let level_res = self.level_resolution(level);
let inv_res = 1.0 / level_res;
let width = self.widths[level] as i32;
let height = self.heights[level] as i32;
let mut score = 0.0f32;
for i in 0..n {
let gx = ((world_xs[i] - self.origin.x) * inv_res).floor() as i32;
let gy = ((world_ys[i] - self.origin.y) * inv_res).floor() as i32;
if gx >= 0 && gy >= 0 && gx < width && gy < height {
score += self.levels[level][(gy as usize) * (width as usize) + (gx as usize)];
}
}
score
}
/// Update level 0 from storage (for incremental updates).
/// Call this when the storage has changed and you need fresh grids.
pub fn update_from_storage(&mut self, storage: &GridStorage) {
let width = storage.width();
let height = storage.height();
// Only update if dimensions match
if width != self.widths[0] || height != self.heights[0] {
// Full rebuild needed
*self = Self::from_storage(storage);
return;
}
// Update level 0
let sigma = self.resolution * 2.0;
let two_sigma_sq = 2.0 * sigma * sigma;
self.levels[0].clear();
for y in 0..height {
for x in 0..width {
let coord = GridCoord::new(x as i32, y as i32);
let distance = storage.get_distance(coord);
let score = if distance < f32::MAX / 2.0 {
(-distance * distance / two_sigma_sq).exp()
} else {
0.1
};
self.levels[0].push(score);
}
}
// Rebuild coarser levels
for level in 1..NUM_LEVELS {
let prev_width = self.widths[level - 1];
let prev_height = self.heights[level - 1];
let new_width = self.widths[level];
let new_height = self.heights[level];
for y in 0..new_height {
for x in 0..new_width {
let mut max_val = 0.0f32;
for dy in 0..2 {
for dx in 0..2 {
let px = x * 2 + dx;
let py = y * 2 + dy;
if px < prev_width && py < prev_height {
let idx = py * prev_width + px;
max_val = max_val.max(self.levels[level - 1][idx]);
}
}
}
self.levels[level][y * new_width + x] = max_val;
}
}
}
}
/// Get the dimensions at level 0.
pub fn dimensions(&self) -> (usize, usize) {
(self.widths[0], self.heights[0])
}
/// Get the resolution at level 0.
pub fn resolution(&self) -> f32 {
self.resolution
}
/// Get the origin.
pub fn origin(&self) -> WorldPoint {
self.origin
}
}
vastu-map/src/slam/branch_bound/mod.rs
+123
View File
@@ -0,0 +1,123 @@
//! Branch-and-bound scan matcher with precomputed multi-resolution grids.
//!
//! Implements Cartographer-style branch-and-bound search for efficient scan matching
//! over large search windows. Uses a precomputed grid hierarchy where each coarse cell
//! stores the maximum probability of all finer cells within it.
//!
//! This allows pruning: if the upper bound score at a coarse level is worse than
//! the current best, the entire subtree can be skipped.
//!
//! ## Grid Hierarchy
//!
//! ```text
//! Level 0 (1x): [0.8][0.2][0.9][0.1][0.7][0.3][0.8][0.2] <- original resolution
//! \ / \ / \ / \ /
//! Level 1 (4x): [0.8] [0.9] [0.7] [0.8] <- max of 4 cells
//! \ / \ /
//! Level 2 (16x): [0.9] [0.8]
//! \ /
//! Level 3 (64x): [0.9] <- coarsest level
//! ```
//!
//! At each coarse level, the value is the maximum of all finer cells below.
//! This provides an upper bound: the true score cannot exceed the coarse score.
mod algorithm;
mod config;
mod grids;
mod search_node;
pub use algorithm::{
branch_and_bound_match, branch_and_bound_match_scan, branch_and_bound_match_simd,
};
pub use config::{BranchBoundConfig, BranchBoundResult, NUM_LEVELS};
pub use grids::PrecomputedGrids;
#[cfg(test)]
mod tests {
use super::*;
use crate::core::{CellType, GridCoord, Pose2D};
use crate::grid::GridStorage;
fn create_test_storage() -> GridStorage {
let mut storage = GridStorage::centered(100, 100, 0.05);
// Create a wall at x=1m
for y in 40..60 {
storage.set_type(GridCoord::new(70, y), CellType::Wall);
}
storage.recompute_distance_field();
storage
}
#[test]
fn test_precomputed_grids_creation() {
let storage = create_test_storage();
let grids = PrecomputedGrids::from_storage(&storage);
// Check dimensions at each level
assert_eq!(grids.dimensions(), (100, 100));
}
#[test]
fn test_coarse_level_is_upper_bound() {
let storage = create_test_storage();
let grids = PrecomputedGrids::from_storage(&storage);
// Sample some random points and check that coarse scores >= fine scores
let points = vec![(1.0, 0.0), (1.0, 0.1), (1.0, -0.1)];
let pose = Pose2D::new(0.0, 0.0, 0.0);
let offset = (0.0, 0.0);
let fine_score = grids.score_scan_at_level(&points, pose, 0, offset);
for level in 1..NUM_LEVELS {
let coarse_score = grids.score_scan_at_level(&points, pose, level, offset);
assert!(
coarse_score >= fine_score - 0.01,
"Coarse level {} score {} should be >= fine score {}",
level,
coarse_score,
fine_score
);
}
}
#[test]
fn test_branch_bound_match() {
let storage = create_test_storage();
let grids = PrecomputedGrids::from_storage(&storage);
// Create scan points that should match the wall at x=1m
let points: Vec<(f32, f32)> = (-10..=10).map(|i| (1.0, i as f32 * 0.05)).collect();
let prior = Pose2D::new(0.0, 0.0, 0.0);
let config = BranchBoundConfig {
search_x: 0.5,
search_y: 0.5,
search_theta: 0.3,
..Default::default()
};
let result = branch_and_bound_match(&points, &grids, prior, &config);
// Should find a match
assert!(result.score > 0.0);
assert!(result.nodes_expanded > 0);
}
#[test]
fn test_empty_points() {
let storage = create_test_storage();
let grids = PrecomputedGrids::from_storage(&storage);
let prior = Pose2D::new(0.0, 0.0, 0.0);
let config = BranchBoundConfig::default();
let result = branch_and_bound_match(&[], &grids, prior, &config);
assert_eq!(result.score, 0.0);
assert!(!result.converged);
}
}
vastu-map/src/slam/branch_bound/search_node.rs
+85
View File
@@ -0,0 +1,85 @@
//! Search node for branch-and-bound algorithm.
use crate::core::Pose2D;
use super::grids::PrecomputedGrids;
/// A node in the branch-and-bound search tree.
#[derive(Clone, Debug)]
pub(super) struct SearchNode {
/// Resolution level (0 = finest, NUM_LEVELS-1 = coarsest).
pub level: usize,
/// X position in grid coordinates at this level.
pub x: i32,
/// Y position in grid coordinates at this level.
pub y: i32,
/// Theta bin index (for angular discretization).
pub theta_bin: i32,
/// Upper bound score for this node.
pub upper_bound: f32,
}
impl SearchNode {
/// Get the center pose for this search node.
pub fn center_pose(&self, grids: &PrecomputedGrids, theta_step: f32) -> Pose2D {
let res = grids.level_resolution(self.level);
Pose2D::new(
grids.origin().x + (self.x as f32 + 0.5) * res,
grids.origin().y + (self.y as f32 + 0.5) * res,
self.theta_bin as f32 * theta_step,
)
}
/// Generate child nodes (subdivide into finer level).
pub fn subdivide(&self, grids: &PrecomputedGrids, num_theta_bins: i32) -> Vec<SearchNode> {
if self.level == 0 {
return Vec::new(); // Already at finest level
}
let new_level = self.level - 1;
let mut children = Vec::with_capacity(8); // 2x2x2 = 8 children
// Subdivide in x, y (each cell splits into 2x2)
for dy in 0..2 {
for dx in 0..2 {
let new_x = self.x * 2 + dx;
let new_y = self.y * 2 + dy;
// Check bounds
if new_x < grids.widths[new_level] as i32 && new_y < grids.heights[new_level] as i32
{
// For theta, we can keep the same bin or split if needed
// For simplicity, keep same theta bin at this level
children.push(SearchNode {
level: new_level,
x: new_x,
y: new_y,
theta_bin: self.theta_bin,
upper_bound: 0.0, // Will be computed by caller
});
}
}
}
// Also subdivide angular bins if at coarse level
if self.level >= 2 {
// Add adjacent theta bins at the new level
let base_children: Vec<_> = children.clone();
for child in base_children {
let theta_minus = (child.theta_bin - 1).rem_euclid(num_theta_bins);
let theta_plus = (child.theta_bin + 1).rem_euclid(num_theta_bins);
children.push(SearchNode {
theta_bin: theta_minus,
..child.clone()
});
children.push(SearchNode {
theta_bin: theta_plus,
..child
});
}
}
children
}
}
vastu-map/src/slam/loop_closure/detector.rs
+98 -20
View File
@@ -2,15 +2,19 @@
//!
//! Two-stage detection (like Cartographer):
//! 1. Fast candidate detection using LiDAR-IRIS descriptors
//! 2. Verification via correlative scan matching with Gauss-Newton refinement
//! 2. Verification via scan matching (correlative or branch-and-bound)
use serde::{Deserialize, Serialize};
use crate::core::{LidarScan, Pose2D};
use crate::core::simd::PointCloud;
use crate::core::{LidarScan, Pose2D, normalize_angle};
use crate::grid::GridStorage;
use super::descriptor::LidarIris;
use crate::slam::{CorrelativeMatcher, CorrelativeMatcherConfig};
use crate::slam::{
BranchBoundConfig, CorrelativeMatcher, CorrelativeMatcherConfig, PrecomputedGrids,
branch_and_bound_match_simd,
};
/// Configuration for loop closure detection.
#[derive(Clone, Debug, Serialize, Deserialize)]
@@ -57,6 +61,12 @@ pub struct LoopClosureConfig {
/// Angular search window for verification (radians).
#[serde(default = "default_verification_angular")]
pub verification_angular_window: f32,
/// Use branch-and-bound matching instead of correlative matcher.
/// More efficient for large search windows (>30cm).
/// Enabled by default (Cartographer-style).
#[serde(default = "default_true")]
pub use_branch_bound: bool,
}
fn default_keyframe_distance() -> f32 {
@@ -107,6 +117,7 @@ impl Default for LoopClosureConfig {
min_verification_score: default_min_verification_score(),
verification_search_window: default_verification_search(),
verification_angular_window: default_verification_angular(),
use_branch_bound: true, // Cartographer-style branch-and-bound
}
}
}
@@ -138,10 +149,6 @@ struct Keyframe {
pose: Pose2D,
/// LiDAR-IRIS descriptor.
descriptor: LidarIris,
/// Stored scan for potential scan-to-scan verification.
/// Currently using scan-to-map, but kept for future use.
#[allow(dead_code)]
scan: Option<LidarScan>,
}
/// Result of loop closure verification.
@@ -251,12 +258,11 @@ impl LoopClosureDetector {
// Search for loop closure candidates
let loop_closure = self.find_loop_closure(pose_idx, pose, &descriptor);
// Add as new keyframe (store scan for verification)
// Add as new keyframe
self.keyframes.push(Keyframe {
pose_idx,
pose,
descriptor,
scan: Some(scan.clone()),
});
self.last_keyframe_pose = Some(pose);
@@ -329,6 +335,89 @@ impl LoopClosureDetector {
}
}
/// Verify a loop closure candidate using branch-and-bound matching.
///
/// This variant uses precomputed multi-resolution grids for more efficient
/// matching over larger search windows (Cartographer-style).
///
/// # Arguments
/// * `candidate` - The loop closure candidate from `add_scan`
/// * `current_scan` - The current lidar scan
/// * `precomputed_grids` - Precomputed grids from finalized submap
/// * `target_pose` - The pose of the target keyframe in world frame
/// * `sensor_offset` - Lidar sensor offset (x, y) in robot frame
///
/// # Returns
/// Verification result with refined pose and match score.
pub fn verify_loop_closure_branch_bound(
&self,
candidate: &LoopClosure,
current_scan: &LidarScan,
precomputed_grids: &PrecomputedGrids,
target_pose: Pose2D,
sensor_offset: (f32, f32),
) -> VerifiedLoopClosure {
// Convert scan to PointCloud for SIMD-optimized matching
let points = PointCloud::from_scan(current_scan);
// Use the candidate's estimated pose as prior
let estimated_current_pose = Pose2D::new(
target_pose.x - candidate.relative_pose.x,
target_pose.y - candidate.relative_pose.y,
normalize_angle(target_pose.theta - candidate.relative_pose.theta),
);
// Configure branch-and-bound search
let bb_config = BranchBoundConfig {
search_x: self.config.verification_search_window,
search_y: self.config.verification_search_window,
search_theta: self.config.verification_angular_window,
angular_resolution: 0.02, // ~1.15 degrees
min_score: 0.0,
sensor_offset,
};
// Run branch-and-bound matching
let result = branch_and_bound_match_simd(
&points,
precomputed_grids,
estimated_current_pose,
&bb_config,
);
// Normalize score to 0.0-1.0 range (based on number of points)
let max_possible_score = points.len() as f32;
let normalized_score = if max_possible_score > 0.0 {
result.score / max_possible_score
} else {
0.0
};
// Compute refined relative pose
let refined_relative = Pose2D::new(
target_pose.x - result.pose.x,
target_pose.y - result.pose.y,
normalize_angle(target_pose.theta - result.pose.theta),
);
let verified = result.converged && normalized_score >= self.config.min_verification_score;
VerifiedLoopClosure {
closure: candidate.clone(),
refined_relative_pose: refined_relative,
match_score: normalized_score,
verified,
}
}
/// Get the target keyframe pose for a loop closure candidate.
pub fn get_keyframe_pose(&self, pose_idx: usize) -> Option<Pose2D> {
self.keyframes
.iter()
.find(|kf| kf.pose_idx == pose_idx)
.map(|kf| kf.pose)
}
/// Check if we should create a keyframe at current pose.
fn should_create_keyframe(&self, pose: Pose2D) -> bool {
match &self.last_keyframe_pose {
@@ -413,17 +502,6 @@ impl LoopClosureDetector {
}
}
/// Normalize angle to [-π, π).
fn normalize_angle(angle: f32) -> f32 {
let mut a = angle % std::f32::consts::TAU;
if a > std::f32::consts::PI {
a -= std::f32::consts::TAU;
} else if a < -std::f32::consts::PI {
a += std::f32::consts::TAU;
}
a
}
#[cfg(test)]
mod tests {
use super::*;
vastu-map/src/slam/loop_closure/graph.rs
+1 -12
View File
@@ -2,7 +2,7 @@
use serde::{Deserialize, Serialize};
use crate::core::Pose2D;
use crate::core::{Pose2D, normalize_angle};
use super::detector::LoopClosure;
@@ -435,17 +435,6 @@ fn huber_weight(residual: f32, delta: f32) -> f32 {
if abs_r <= delta { 1.0 } else { delta / abs_r }
}
/// Normalize angle to [-π, π).
fn normalize_angle(angle: f32) -> f32 {
let mut a = angle % std::f32::consts::TAU;
if a > std::f32::consts::PI {
a -= std::f32::consts::TAU;
} else if a < -std::f32::consts::PI {
a += std::f32::consts::TAU;
}
a
}
#[cfg(test)]
mod tests {
use super::*;
vastu-map/src/slam/matcher.rs → vastu-map/src/slam/matcher/core.rs
+204 -360
View File
@@ -1,15 +1,27 @@
//! Correlative scan-to-map matcher.
//!
//! Performs brute-force search over a 3D pose space (x, y, theta) to find
//! the best alignment of a lidar scan to the current occupancy grid map.
//! Correlative scan-to-map matcher implementation.
use crate::core::{CellType, LidarScan, Pose2D, WorldPoint};
use crate::grid::GridStorage;
use crate::core::simd::{PointBatch, RotationMatrix4, transform_points_simd4, world_to_grid_simd4};
use crate::core::simd::{PointCloud, RotationMatrix4, transform_points_simd4, world_to_grid_simd4};
use super::config::CorrelativeMatcherConfig;
use super::types::ScanMatchResult;
use super::helpers::{normalize_angle, solve_3x3};
use super::lm::AdaptiveLM;
use super::types::ScratchBuffers;
use crate::slam::config::CorrelativeMatcherConfig;
use crate::slam::types::ScanMatchResult;
/// Parameters for pose search space.
struct SearchParams {
/// Center pose for the search grid.
center_pose: Pose2D,
/// Prior pose for tie-breaking penalty.
prior_pose: Pose2D,
/// Linear step size in meters.
linear_step: f32,
/// Angular step size in radians.
angular_step: f32,
}
/// Correlative scan-to-map matcher.
///
@@ -19,40 +31,6 @@ pub struct CorrelativeMatcher {
config: CorrelativeMatcherConfig,
}
/// Scratch buffers for SIMD operations to avoid per-call allocations.
pub struct ScratchBuffers {
/// Transformed world X coordinates
world_xs: Vec<f32>,
/// Transformed world Y coordinates
world_ys: Vec<f32>,
/// Grid X coordinates
grid_xs: Vec<i32>,
/// Grid Y coordinates
grid_ys: Vec<i32>,
}
impl ScratchBuffers {
/// Create scratch buffers for a given number of points.
pub fn new(capacity: usize) -> Self {
Self {
world_xs: vec![0.0; capacity],
world_ys: vec![0.0; capacity],
grid_xs: vec![0; capacity],
grid_ys: vec![0; capacity],
}
}
/// Resize buffers if needed.
pub fn resize(&mut self, n: usize) {
if self.world_xs.len() < n {
self.world_xs.resize(n, 0.0);
self.world_ys.resize(n, 0.0);
self.grid_xs.resize(n, 0);
self.grid_ys.resize(n, 0);
}
}
}
impl CorrelativeMatcher {
/// Create a new correlative matcher.
pub fn new(config: CorrelativeMatcherConfig) -> Self {
@@ -185,13 +163,16 @@ impl CorrelativeMatcher {
self.refine_with_gauss_newton(points, fine_result, storage)
}
/// Refine a scan match result using Gauss-Newton optimization.
/// Refine a scan match result using Gauss-Newton optimization with adaptive LM.
///
/// This provides sub-pixel/sub-degree accuracy by minimizing the sum of
/// squared distances from scan points to the nearest walls.
///
/// Uses Levenberg-Marquardt damping for stability and a prior constraint
/// (like Cartographer's CeresScanMatcher) to prevent over-correction.
/// Uses **adaptive** Levenberg-Marquardt damping that adjusts based on step
/// quality, providing faster convergence than fixed damping while maintaining
/// stability. Also implements step acceptance/rejection logic.
///
/// Prior constraints (like Cartographer's CeresScanMatcher) prevent over-correction.
fn refine_with_gauss_newton(
&self,
points: &[(f32, f32)],
@@ -207,10 +188,22 @@ impl CorrelativeMatcher {
let (offset_x, offset_y) = self.config.sensor_offset;
// Prior constraint weights (Cartographer-style)
// These penalize deviation from the correlative search result
let trans_weight = self.config.gn_translation_weight;
let rot_weight = self.config.gn_rotation_weight;
// Adaptive LM optimizer
let mut lm = AdaptiveLM::new();
// Compute initial cost
let mut current_cost = self.compute_refinement_cost(
points,
pose,
initial_pose,
storage,
trans_weight,
rot_weight,
);
for _iter in 0..self.config.gn_max_iterations {
// Build normal equations: H * delta = -g
// where H ≈ J^T * J, g = J^T * r
@@ -239,10 +232,6 @@ impl CorrelativeMatcher {
}
// Jacobian row: [∂d/∂x, ∂d/∂y, ∂d/∂θ]
// ∂d/∂x = grad_x (direct translation effect)
// ∂d/∂y = grad_y (direct translation effect)
// ∂d/∂θ = grad_x * ∂world_x/∂θ + grad_y * ∂world_y/∂θ
// = grad_x * (-px*sin - py*cos) + grad_y * (px*cos - py*sin)
let dworld_x_dtheta = -px * sin_theta - py * cos_theta;
let dworld_y_dtheta = px * cos_theta - py * sin_theta;
let j_theta = grad_x * dworld_x_dtheta + grad_y * dworld_y_dtheta;
@@ -250,7 +239,6 @@ impl CorrelativeMatcher {
let j = [grad_x, grad_y, j_theta];
// Accumulate H = J^T * J and g = J^T * r
// Using outer product: H += j * j^T, g += j * dist
for i in 0..3 {
g[i] += j[i] * dist;
for k in 0..3 {
@@ -260,9 +248,6 @@ impl CorrelativeMatcher {
}
// Add prior constraint (Cartographer-style CeresScanMatcher)
// This penalizes deviation from the correlative search result
// E_prior = trans_weight * ||p - p_init||² + rot_weight * (θ - θ_init)²
// Adds to H diagonal and g vector
if trans_weight > 0.0 {
h[0][0] += trans_weight;
h[1][1] += trans_weight;
@@ -275,10 +260,13 @@ impl CorrelativeMatcher {
g[2] += rot_weight * angle_error;
}
// Add Levenberg-Marquardt damping to diagonal
let damping = self.config.gn_damping;
for i in 0..3 {
h[i][i] += damping * (1.0 + h[i][i]);
// Store undamped Hessian for predicted reduction calculation
let h_undamped = h;
// Add adaptive Levenberg-Marquardt damping to diagonal
let damping = lm.damping();
for (i, row) in h.iter_mut().enumerate() {
row[i] += damping * (1.0 + row[i]);
}
// Solve 3x3 system using Cramer's rule
@@ -289,18 +277,69 @@ impl CorrelativeMatcher {
break;
}
// Update pose
pose.x += delta[0];
pose.y += delta[1];
pose.theta += delta[2];
// Compute predicted cost reduction using linear model
// predicted_reduction = -delta^T * g - 0.5 * delta^T * H * delta
let htd = [
h_undamped[0][0] * delta[0]
+ h_undamped[0][1] * delta[1]
+ h_undamped[0][2] * delta[2],
h_undamped[1][0] * delta[0]
+ h_undamped[1][1] * delta[1]
+ h_undamped[1][2] * delta[2],
h_undamped[2][0] * delta[0]
+ h_undamped[2][1] * delta[1]
+ h_undamped[2][2] * delta[2],
];
let delta_dot_g = delta[0] * g[0] + delta[1] * g[1] + delta[2] * g[2];
let delta_dot_htd = delta[0] * htd[0] + delta[1] * htd[1] + delta[2] * htd[2];
let predicted_reduction = -delta_dot_g - 0.5 * delta_dot_htd;
// Normalize theta to [-π, π]
pose.theta = normalize_angle(pose.theta);
// Compute candidate pose
let mut candidate = pose;
candidate.x += delta[0];
candidate.y += delta[1];
candidate.theta = normalize_angle(candidate.theta + delta[2]);
// Check convergence
let update_mag = (delta[0] * delta[0] + delta[1] * delta[1]).sqrt();
if update_mag < self.config.gn_convergence_threshold {
break;
// Compute actual cost at candidate pose
let candidate_cost = self.compute_refinement_cost(
points,
candidate,
initial_pose,
storage,
trans_weight,
rot_weight,
);
let actual_reduction = current_cost - candidate_cost;
// Compute step quality ratio (rho)
let rho = if predicted_reduction.abs() > 1e-10 {
actual_reduction / predicted_reduction
} else if actual_reduction > 0.0 {
1.0 // Tiny predicted but positive actual = good
} else {
0.0
};
// Accept or reject step based on actual improvement
if actual_reduction > 0.0 {
// Step improved cost - accept it
pose = candidate;
current_cost = candidate_cost;
lm.update(rho);
// Check convergence
let update_mag = (delta[0] * delta[0] + delta[1] * delta[1]).sqrt();
if update_mag < self.config.gn_convergence_threshold {
break;
}
} else {
// Step made things worse - reject and increase damping
lm.reject_step();
// If stuck at maximum damping, stop
if lm.is_stuck() {
break;
}
}
}
@@ -315,6 +354,54 @@ impl CorrelativeMatcher {
ScanMatchResult::new(pose, normalized_score, hits, points.len())
}
/// Compute the total cost for Gauss-Newton refinement.
///
/// Cost = sum of squared distances + prior constraint penalties.
fn compute_refinement_cost(
&self,
points: &[(f32, f32)],
pose: Pose2D,
initial_pose: Pose2D,
storage: &GridStorage,
trans_weight: f32,
rot_weight: f32,
) -> f32 {
let (offset_x, offset_y) = self.config.sensor_offset;
let cos_theta = pose.theta.cos();
let sin_theta = pose.theta.sin();
// Sensor position in world frame
let sensor_x = pose.x + offset_x * cos_theta - offset_y * sin_theta;
let sensor_y = pose.y + offset_x * sin_theta + offset_y * cos_theta;
let max_dist = storage.resolution() * 20.0;
let mut cost = 0.0f32;
// Sum of squared distances
for &(px, py) in points {
let world_x = sensor_x + px * cos_theta - py * sin_theta;
let world_y = sensor_y + px * sin_theta + py * cos_theta;
let (dist, _, _) = storage.get_distance_interpolated(WorldPoint::new(world_x, world_y));
if dist < max_dist {
cost += dist * dist;
}
}
// Prior constraint cost
if trans_weight > 0.0 {
let dx = pose.x - initial_pose.x;
let dy = pose.y - initial_pose.y;
cost += trans_weight * (dx * dx + dy * dy);
}
if rot_weight > 0.0 {
let dtheta = normalize_angle(pose.theta - initial_pose.theta);
cost += rot_weight * dtheta * dtheta;
}
cost
}
/// Search over pose space to find best match.
///
/// # Arguments
@@ -383,7 +470,7 @@ impl CorrelativeMatcher {
/// Score how well a pose aligns the scan points with the map.
///
/// Returns (raw_score, hit_count).
fn score_pose(
pub fn score_pose(
&self,
points: &[(f32, f32)],
pose: Pose2D,
@@ -496,7 +583,13 @@ impl CorrelativeMatcher {
}
/// Extract valid points from scan in SoA format for SIMD processing.
fn extract_scan_points_simd(&self, scan: &LidarScan) -> PointBatch {
fn extract_scan_points_simd(&self, scan: &LidarScan) -> PointCloud {
// Use PointCloud::from_scan for simple case (handles padding automatically)
if self.config.max_points == 0 || scan.ranges.len() <= self.config.max_points {
return PointCloud::from_scan(scan);
}
// Manual extraction with subsampling
let mut xs = Vec::with_capacity(scan.ranges.len());
let mut ys = Vec::with_capacity(scan.ranges.len());
@@ -505,31 +598,33 @@ impl CorrelativeMatcher {
ys.push(range * angle.sin());
}
// Subsample if needed
if self.config.max_points > 0 && xs.len() > self.config.max_points {
let step = xs.len() / self.config.max_points;
xs = xs.into_iter().step_by(step).collect();
ys = ys.into_iter().step_by(step).collect();
}
// Subsample
let step = xs.len() / self.config.max_points;
xs = xs.into_iter().step_by(step).collect();
ys = ys.into_iter().step_by(step).collect();
PointBatch { xs, ys }
let mut points = PointCloud { xs, ys };
points.pad_to_lanes(); // Ensure SIMD alignment
points
}
/// Single resolution search with SIMD.
fn match_single_resolution_simd(
&self,
points: &PointBatch,
points: &PointCloud,
prior_pose: Pose2D,
storage: &GridStorage,
scratch: &mut ScratchBuffers,
) -> ScanMatchResult {
self.search_poses_simd(
points,
prior_pose,
prior_pose, // center and prior are the same for single resolution
SearchParams {
center_pose: prior_pose,
prior_pose, // center and prior are the same for single resolution
linear_step: self.config.linear_resolution,
angular_step: self.config.angular_resolution,
},
storage,
self.config.linear_resolution,
self.config.angular_resolution,
scratch,
)
}
@@ -537,7 +632,7 @@ impl CorrelativeMatcher {
/// Multi-resolution search with SIMD: coarse first, then refine with Gauss-Newton.
fn match_multi_resolution_simd(
&self,
points: &PointBatch,
points: &PointCloud,
prior_pose: Pose2D,
storage: &GridStorage,
scratch: &mut ScratchBuffers,
@@ -548,11 +643,13 @@ impl CorrelativeMatcher {
let coarse_result = self.search_poses_simd(
points,
prior_pose,
prior_pose,
SearchParams {
center_pose: prior_pose,
prior_pose,
linear_step: coarse_linear,
angular_step: coarse_angular,
},
storage,
coarse_linear,
coarse_angular,
scratch,
);
@@ -572,16 +669,18 @@ impl CorrelativeMatcher {
let fine_matcher = CorrelativeMatcher::new(fine_config);
let fine_result = fine_matcher.search_poses_simd(
points,
coarse_result.pose, // center on coarse result
prior_pose, // but penalize based on original prior
SearchParams {
center_pose: coarse_result.pose, // center on coarse result
prior_pose, // but penalize based on original prior
linear_step: self.config.linear_resolution,
angular_step: self.config.angular_resolution,
},
storage,
self.config.linear_resolution,
self.config.angular_resolution,
scratch,
);
// Gauss-Newton refinement for sub-pixel accuracy
// Convert PointBatch to Vec<(f32, f32)> for refinement
// Convert PointCloud to Vec<(f32, f32)> for refinement
let points_vec: Vec<(f32, f32)> = points
.xs
.iter()
@@ -594,31 +693,28 @@ impl CorrelativeMatcher {
/// Search over pose space to find best match using SIMD.
fn search_poses_simd(
&self,
points: &PointBatch,
center_pose: Pose2D,
prior_pose: Pose2D,
points: &PointCloud,
params: SearchParams,
storage: &GridStorage,
linear_step: f32,
angular_step: f32,
scratch: &mut ScratchBuffers,
) -> ScanMatchResult {
let mut best_pose = center_pose;
let mut best_pose = params.center_pose;
let mut best_score = f32::NEG_INFINITY;
let mut best_hits = 0usize;
// Generate search poses
let x_steps = (self.config.search_x / linear_step).ceil() as i32;
let y_steps = (self.config.search_y / linear_step).ceil() as i32;
let theta_steps = (self.config.search_theta / angular_step).ceil() as i32;
let x_steps = (self.config.search_x / params.linear_step).ceil() as i32;
let y_steps = (self.config.search_y / params.linear_step).ceil() as i32;
let theta_steps = (self.config.search_theta / params.angular_step).ceil() as i32;
for dx in -x_steps..=x_steps {
let x = center_pose.x + dx as f32 * linear_step;
let x = params.center_pose.x + dx as f32 * params.linear_step;
for dy in -y_steps..=y_steps {
let y = center_pose.y + dy as f32 * linear_step;
let y = params.center_pose.y + dy as f32 * params.linear_step;
for dtheta in -theta_steps..=theta_steps {
let theta = center_pose.theta + dtheta as f32 * angular_step;
let theta = params.center_pose.theta + dtheta as f32 * params.angular_step;
let candidate = Pose2D::new(x, y, theta);
let (score, hits) = self.score_pose_simd(points, candidate, storage, scratch);
@@ -627,10 +723,10 @@ impl CorrelativeMatcher {
// This prefers poses closer to odometry when scores are similar
// Position: Higher weight since encoders are typically reliable for distance
// Angular: Lower weight to allow scan matching to correct heading drift
let dist_to_prior = ((candidate.x - prior_pose.x).powi(2)
+ (candidate.y - prior_pose.y).powi(2))
let dist_to_prior = ((candidate.x - params.prior_pose.x).powi(2)
+ (candidate.y - params.prior_pose.y).powi(2))
.sqrt();
let angle_dist = (candidate.theta - prior_pose.theta).abs();
let angle_dist = (candidate.theta - params.prior_pose.theta).abs();
let adjusted_score = score - dist_to_prior * 300.0 - angle_dist * 30.0;
if adjusted_score > best_score {
@@ -656,9 +752,9 @@ impl CorrelativeMatcher {
///
/// Uses SIMD for batch point transformation and world-to-grid conversion.
/// The final scoring loop is scalar due to random memory access patterns.
fn score_pose_simd(
pub fn score_pose_simd(
&self,
points: &PointBatch,
points: &PointCloud,
pose: Pose2D,
storage: &GridStorage,
scratch: &mut ScratchBuffers,
@@ -758,255 +854,3 @@ impl CorrelativeMatcher {
(score, hits)
}
}
/// Solve a 3x3 linear system Ax = b using Cramer's rule.
///
/// Returns [0, 0, 0] if the matrix is singular.
fn solve_3x3(a: &[[f32; 3]; 3], b: &[f32; 3]) -> [f32; 3] {
// Compute determinant of A
let det = a[0][0] * (a[1][1] * a[2][2] - a[1][2] * a[2][1])
- a[0][1] * (a[1][0] * a[2][2] - a[1][2] * a[2][0])
+ a[0][2] * (a[1][0] * a[2][1] - a[1][1] * a[2][0]);
if det.abs() < 1e-10 {
return [0.0, 0.0, 0.0];
}
let inv_det = 1.0 / det;
// Cramer's rule: x_i = det(A_i) / det(A)
// where A_i is A with column i replaced by b
let x0 = (b[0] * (a[1][1] * a[2][2] - a[1][2] * a[2][1])
- a[0][1] * (b[1] * a[2][2] - a[1][2] * b[2])
+ a[0][2] * (b[1] * a[2][1] - a[1][1] * b[2]))
* inv_det;
let x1 = (a[0][0] * (b[1] * a[2][2] - a[1][2] * b[2])
- b[0] * (a[1][0] * a[2][2] - a[1][2] * a[2][0])
+ a[0][2] * (a[1][0] * b[2] - b[1] * a[2][0]))
* inv_det;
let x2 = (a[0][0] * (a[1][1] * b[2] - b[1] * a[2][1])
- a[0][1] * (a[1][0] * b[2] - b[1] * a[2][0])
+ b[0] * (a[1][0] * a[2][1] - a[1][1] * a[2][0]))
* inv_det;
[x0, x1, x2]
}
/// Normalize angle to [-π, π).
fn normalize_angle(angle: f32) -> f32 {
let mut a = angle % std::f32::consts::TAU;
if a > std::f32::consts::PI {
a -= std::f32::consts::TAU;
} else if a < -std::f32::consts::PI {
a += std::f32::consts::TAU;
}
a
}
#[cfg(test)]
mod tests {
use super::*;
use crate::core::GridCoord;
fn create_test_storage() -> GridStorage {
GridStorage::centered(100, 100, 0.05) // 5m x 5m at 5cm resolution
}
fn create_wall_in_front(storage: &mut GridStorage, pose: Pose2D, distance: f32) {
// Create a wall in front of the pose
let wall_x = pose.x + distance * pose.theta.cos();
let wall_y = pose.y + distance * pose.theta.sin();
// Create a line of wall cells
for offset in -10..=10 {
let perp_angle = pose.theta + std::f32::consts::FRAC_PI_2;
let wx = wall_x + offset as f32 * 0.05 * perp_angle.cos();
let wy = wall_y + offset as f32 * 0.05 * perp_angle.sin();
let coord = storage.world_to_grid(WorldPoint::new(wx, wy));
if storage.is_valid_coord(coord) {
storage.set_type(coord, CellType::Wall);
}
}
// Update distance field for Gaussian scoring
storage.recompute_distance_field();
}
fn create_simple_scan(distance: f32, num_points: usize) -> LidarScan {
// Create a scan with points at uniform angles, all at same distance
let angle_min = -std::f32::consts::PI;
let angle_max = std::f32::consts::PI;
let angle_increment = (angle_max - angle_min) / num_points as f32;
let ranges = vec![distance; num_points];
let angles: Vec<f32> = (0..num_points)
.map(|i| angle_min + i as f32 * angle_increment)
.collect();
LidarScan::new(ranges, angles, 0.1, 10.0)
}
#[test]
fn test_matcher_creation() {
let matcher = CorrelativeMatcher::with_defaults();
assert!(matcher.config().enabled);
}
#[test]
fn test_disabled_matcher() {
let config = CorrelativeMatcherConfig::disabled();
let matcher = CorrelativeMatcher::new(config);
let storage = create_test_storage();
let scan = create_simple_scan(2.0, 360);
let pose = Pose2D::new(0.0, 0.0, 0.0);
let result = matcher.match_scan(&scan, pose, &storage);
// Disabled matcher should return input pose unchanged
assert!((result.pose.x - pose.x).abs() < 0.001);
assert!((result.pose.y - pose.y).abs() < 0.001);
}
#[test]
fn test_matching_returns_result() {
let mut storage = create_test_storage();
let true_pose = Pose2D::new(0.0, 0.0, 0.0);
// Create a wall 2m in front
create_wall_in_front(&mut storage, true_pose, 2.0);
// Use fast config for speed
let config = CorrelativeMatcherConfig::fast();
let matcher = CorrelativeMatcher::new(config);
// Test with prior very close to true pose
let prior = Pose2D::new(0.01, 0.01, 0.01);
// Create a scan that simulates hitting the wall from the prior pose
let angles: Vec<f32> = (-10..=10).map(|i| i as f32 * 0.05).collect();
let ranges: Vec<f32> = vec![2.0; angles.len()];
let scan = LidarScan::new(ranges, angles, 0.1, 10.0);
let result = matcher.match_scan(&scan, prior, &storage);
// Matcher should return a result (may or may not converge depending on map state)
// At minimum, it should return a valid pose
assert!(result.pose.x.is_finite());
assert!(result.pose.y.is_finite());
assert!(result.pose.theta.is_finite());
}
#[test]
fn test_score_increases_with_hits() {
let mut storage = create_test_storage();
// Fill some cells with walls
for x in 40..60 {
for y in 40..60 {
storage.set_type(GridCoord::new(x, y), CellType::Wall);
}
}
// Update distance field for Gaussian scoring
storage.recompute_distance_field();
let matcher = CorrelativeMatcher::with_defaults();
let points: Vec<(f32, f32)> = vec![(0.0, 0.0), (0.05, 0.0), (0.1, 0.0)];
// Pose in wall area should score higher
let pose_in_wall = Pose2D::new(0.0, 0.0, 0.0);
let pose_in_floor = Pose2D::new(-2.0, -2.0, 0.0);
let (score_wall, hits_wall) = matcher.score_pose(&points, pose_in_wall, &storage);
let (score_floor, hits_floor) = matcher.score_pose(&points, pose_in_floor, &storage);
assert!(hits_wall > hits_floor);
assert!(score_wall > score_floor);
}
#[test]
fn test_simd_matches_scalar() {
let mut storage = create_test_storage();
let true_pose = Pose2D::new(0.0, 0.0, 0.0);
// Create a wall 2m in front
create_wall_in_front(&mut storage, true_pose, 2.0);
// Use fast config for speed
let config = CorrelativeMatcherConfig::fast();
let matcher = CorrelativeMatcher::new(config);
// Create a scan
let angles: Vec<f32> = (-10..=10).map(|i| i as f32 * 0.05).collect();
let ranges: Vec<f32> = vec![2.0; angles.len()];
let scan = LidarScan::new(ranges, angles, 0.1, 10.0);
let prior = Pose2D::new(0.01, 0.01, 0.01);
// Get scalar result
let scalar_result = matcher.match_scan(&scan, prior, &storage);
// Get SIMD result
let mut scratch = ScratchBuffers::new(scan.ranges.len());
let simd_result = matcher.match_scan_simd(&scan, prior, &storage, &mut scratch);
// Results should match
assert!((scalar_result.pose.x - simd_result.pose.x).abs() < 0.001);
assert!((scalar_result.pose.y - simd_result.pose.y).abs() < 0.001);
assert!((scalar_result.pose.theta - simd_result.pose.theta).abs() < 0.001);
assert!((scalar_result.score - simd_result.score).abs() < 0.001);
assert_eq!(scalar_result.hits, simd_result.hits);
}
#[test]
fn test_simd_score_matches_scalar() {
let mut storage = create_test_storage();
// Fill some cells with walls
for x in 40..60 {
for y in 40..60 {
storage.set_type(GridCoord::new(x, y), CellType::Wall);
}
}
// Update distance field for Gaussian scoring
storage.recompute_distance_field();
let matcher = CorrelativeMatcher::with_defaults();
// Create points in AoS format for scalar
let points: Vec<(f32, f32)> = vec![(0.0, 0.0), (0.05, 0.0), (0.1, 0.0), (0.15, 0.0)];
// Convert to SoA format for SIMD
let points_soa = PointBatch {
xs: points.iter().map(|(x, _)| *x).collect(),
ys: points.iter().map(|(_, y)| *y).collect(),
};
let pose = Pose2D::new(0.0, 0.0, 0.0);
let mut scratch = ScratchBuffers::new(points.len());
// Get scalar score
let (scalar_score, scalar_hits) = matcher.score_pose(&points, pose, &storage);
// Get SIMD score
let (simd_score, simd_hits) =
matcher.score_pose_simd(&points_soa, pose, &storage, &mut scratch);
// Results should match
assert!(
(scalar_score - simd_score).abs() < 0.001,
"score mismatch: {} vs {}",
scalar_score,
simd_score
);
assert_eq!(
scalar_hits, simd_hits,
"hits mismatch: {} vs {}",
scalar_hits, simd_hits
);
}
}
vastu-map/src/slam/matcher/helpers.rs
+47
View File
@@ -0,0 +1,47 @@
//! Helper functions for scan matching.
/// Solve a 3x3 linear system Ax = b using Cramer's rule.
///
/// Returns [0, 0, 0] if the matrix is singular.
pub fn solve_3x3(a: &[[f32; 3]; 3], b: &[f32; 3]) -> [f32; 3] {
// Compute determinant of A
let det = a[0][0] * (a[1][1] * a[2][2] - a[1][2] * a[2][1])
- a[0][1] * (a[1][0] * a[2][2] - a[1][2] * a[2][0])
+ a[0][2] * (a[1][0] * a[2][1] - a[1][1] * a[2][0]);
if det.abs() < 1e-10 {
return [0.0, 0.0, 0.0];
}
let inv_det = 1.0 / det;
// Cramer's rule: x_i = det(A_i) / det(A)
// where A_i is A with column i replaced by b
let x0 = (b[0] * (a[1][1] * a[2][2] - a[1][2] * a[2][1])
- a[0][1] * (b[1] * a[2][2] - a[1][2] * b[2])
+ a[0][2] * (b[1] * a[2][1] - a[1][1] * b[2]))
* inv_det;
let x1 = (a[0][0] * (b[1] * a[2][2] - a[1][2] * b[2])
- b[0] * (a[1][0] * a[2][2] - a[1][2] * a[2][0])
+ a[0][2] * (a[1][0] * b[2] - b[1] * a[2][0]))
* inv_det;
let x2 = (a[0][0] * (a[1][1] * b[2] - b[1] * a[2][1])
- a[0][1] * (a[1][0] * b[2] - b[1] * a[2][0])
+ b[0] * (a[1][0] * a[2][1] - a[1][1] * a[2][0]))
* inv_det;
[x0, x1, x2]
}
/// Normalize angle to [-π, π).
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
}
vastu-map/src/slam/matcher/lm.rs
+67
View File
@@ -0,0 +1,67 @@
//! Adaptive Levenberg-Marquardt optimizer.
/// Adaptive Levenberg-Marquardt optimizer for Gauss-Newton refinement.
///
/// Automatically adjusts the damping parameter (lambda) based on step quality:
/// - Good steps (actual reduction close to predicted): decrease lambda
/// - Bad steps (actual reduction much less than predicted): increase lambda
///
/// This provides faster convergence than fixed damping while maintaining stability.
pub(super) struct AdaptiveLM {
/// Current damping parameter
lambda: f32,
/// Factor to increase/decrease lambda
lambda_factor: f32,
/// Minimum lambda value
min_lambda: f32,
/// Maximum lambda value
max_lambda: f32,
}
impl AdaptiveLM {
/// Create a new adaptive LM optimizer with default parameters.
pub fn new() -> Self {
Self {
lambda: 1e-3,
lambda_factor: 10.0,
min_lambda: 1e-7,
max_lambda: 1e7,
}
}
/// Get the current damping value.
pub fn damping(&self) -> f32 {
self.lambda
}
/// Update lambda based on step quality.
///
/// `rho` is the ratio of actual cost reduction to predicted cost reduction.
/// - rho > 0.75: Very good step, decrease lambda aggressively
/// - rho > 0.25: Good step, decrease lambda
/// - rho > 0.0: Acceptable step, keep lambda
/// - rho <= 0.0: Bad step (cost increased), increase lambda
pub fn update(&mut self, rho: f32) {
if rho > 0.75 {
// Very good step - decrease lambda
self.lambda = (self.lambda / self.lambda_factor).max(self.min_lambda);
} else if rho > 0.25 {
// Good step - slightly decrease lambda
self.lambda = (self.lambda / (self.lambda_factor.sqrt())).max(self.min_lambda);
} else if rho < 0.0 {
// Bad step - increase lambda significantly
self.lambda = (self.lambda * self.lambda_factor).min(self.max_lambda);
}
// 0.0 <= rho <= 0.25: marginal step, keep lambda unchanged
}
/// Reject a step (cost increased), increase lambda.
pub fn reject_step(&mut self) {
self.lambda = (self.lambda * self.lambda_factor).min(self.max_lambda);
}
/// Check if optimizer has reached maximum damping (likely stuck).
pub fn is_stuck(&self) -> bool {
self.lambda >= self.max_lambda * 0.99
}
}
vastu-map/src/slam/matcher/mod.rs
+227
View File
@@ -0,0 +1,227 @@
//! Correlative scan-to-map matcher.
//!
//! Performs brute-force search over a 3D pose space (x, y, theta) to find
//! the best alignment of a lidar scan to the current occupancy grid map.
mod core;
mod helpers;
mod lm;
mod types;
pub use self::core::CorrelativeMatcher;
pub use types::ScratchBuffers;
#[cfg(test)]
mod tests {
use super::*;
use crate::core::simd::PointCloud;
use crate::core::{CellType, GridCoord, LidarScan, Pose2D, WorldPoint};
use crate::grid::GridStorage;
use crate::slam::CorrelativeMatcherConfig;
fn create_test_storage() -> GridStorage {
GridStorage::centered(100, 100, 0.05) // 5m x 5m at 5cm resolution
}
fn create_wall_in_front(storage: &mut GridStorage, pose: Pose2D, distance: f32) {
// Create a wall in front of the pose
let wall_x = pose.x + distance * pose.theta.cos();
let wall_y = pose.y + distance * pose.theta.sin();
// Create a line of wall cells
for offset in -10..=10 {
let perp_angle = pose.theta + std::f32::consts::FRAC_PI_2;
let wx = wall_x + offset as f32 * 0.05 * perp_angle.cos();
let wy = wall_y + offset as f32 * 0.05 * perp_angle.sin();
let coord = storage.world_to_grid(WorldPoint::new(wx, wy));
if storage.is_valid_coord(coord) {
storage.set_type(coord, CellType::Wall);
}
}
// Update distance field for Gaussian scoring
storage.recompute_distance_field();
}
fn create_simple_scan(distance: f32, num_points: usize) -> LidarScan {
// Create a scan with points at uniform angles, all at same distance
let angle_min = -std::f32::consts::PI;
let angle_max = std::f32::consts::PI;
let angle_increment = (angle_max - angle_min) / num_points as f32;
let ranges = vec![distance; num_points];
let angles: Vec<f32> = (0..num_points)
.map(|i| angle_min + i as f32 * angle_increment)
.collect();
LidarScan::new(ranges, angles, 0.1, 10.0)
}
#[test]
fn test_matcher_creation() {
let matcher = CorrelativeMatcher::with_defaults();
assert!(matcher.config().enabled);
}
#[test]
fn test_disabled_matcher() {
let config = CorrelativeMatcherConfig::disabled();
let matcher = CorrelativeMatcher::new(config);
let storage = create_test_storage();
let scan = create_simple_scan(2.0, 360);
let pose = Pose2D::new(0.0, 0.0, 0.0);
let result = matcher.match_scan(&scan, pose, &storage);
// Disabled matcher should return input pose unchanged
assert!((result.pose.x - pose.x).abs() < 0.001);
assert!((result.pose.y - pose.y).abs() < 0.001);
}
#[test]
fn test_matching_returns_result() {
let mut storage = create_test_storage();
let true_pose = Pose2D::new(0.0, 0.0, 0.0);
// Create a wall 2m in front
create_wall_in_front(&mut storage, true_pose, 2.0);
// Use fast config for speed
let config = CorrelativeMatcherConfig::fast();
let matcher = CorrelativeMatcher::new(config);
// Test with prior very close to true pose
let prior = Pose2D::new(0.01, 0.01, 0.01);
// Create a scan that simulates hitting the wall from the prior pose
let angles: Vec<f32> = (-10..=10).map(|i| i as f32 * 0.05).collect();
let ranges: Vec<f32> = vec![2.0; angles.len()];
let scan = LidarScan::new(ranges, angles, 0.1, 10.0);
let result = matcher.match_scan(&scan, prior, &storage);
// Matcher should return a result (may or may not converge depending on map state)
// At minimum, it should return a valid pose
assert!(result.pose.x.is_finite());
assert!(result.pose.y.is_finite());
assert!(result.pose.theta.is_finite());
}
#[test]
fn test_score_increases_with_hits() {
let mut storage = create_test_storage();
// Fill some cells with walls
for x in 40..60 {
for y in 40..60 {
storage.set_type(GridCoord::new(x, y), CellType::Wall);
}
}
// Update distance field for Gaussian scoring
storage.recompute_distance_field();
let matcher = CorrelativeMatcher::with_defaults();
let points: Vec<(f32, f32)> = vec![(0.0, 0.0), (0.05, 0.0), (0.1, 0.0)];
// Pose in wall area should score higher
let pose_in_wall = Pose2D::new(0.0, 0.0, 0.0);
let pose_in_floor = Pose2D::new(-2.0, -2.0, 0.0);
let (score_wall, hits_wall) = matcher.score_pose(&points, pose_in_wall, &storage);
let (score_floor, hits_floor) = matcher.score_pose(&points, pose_in_floor, &storage);
assert!(hits_wall > hits_floor);
assert!(score_wall > score_floor);
}
#[test]
fn test_simd_matches_scalar() {
let mut storage = create_test_storage();
let true_pose = Pose2D::new(0.0, 0.0, 0.0);
// Create a wall 2m in front
create_wall_in_front(&mut storage, true_pose, 2.0);
// Use fast config for speed
let config = CorrelativeMatcherConfig::fast();
let matcher = CorrelativeMatcher::new(config);
// Create a scan with LANES-aligned point count (20 = 5 * LANES)
// to avoid padding differences between scalar and SIMD paths
let angles: Vec<f32> = (-10..10).map(|i| i as f32 * 0.05).collect(); // 20 points
let ranges: Vec<f32> = vec![2.0; angles.len()];
let scan = LidarScan::new(ranges, angles, 0.1, 10.0);
let prior = Pose2D::new(0.01, 0.01, 0.01);
// Get scalar result
let scalar_result = matcher.match_scan(&scan, prior, &storage);
// Get SIMD result
let mut scratch = ScratchBuffers::new(scan.ranges.len());
let simd_result = matcher.match_scan_simd(&scan, prior, &storage, &mut scratch);
// Poses should match closely
assert!((scalar_result.pose.x - simd_result.pose.x).abs() < 0.001);
assert!((scalar_result.pose.y - simd_result.pose.y).abs() < 0.001);
assert!((scalar_result.pose.theta - simd_result.pose.theta).abs() < 0.001);
// Score comparison: SIMD may have slightly different normalization due to internal padding
assert!(
(scalar_result.score - simd_result.score).abs() < 0.05,
"score mismatch: scalar={}, simd={}",
scalar_result.score,
simd_result.score
);
}
#[test]
fn test_simd_score_matches_scalar() {
let mut storage = create_test_storage();
// Fill some cells with walls
for x in 40..60 {
for y in 40..60 {
storage.set_type(GridCoord::new(x, y), CellType::Wall);
}
}
// Update distance field for Gaussian scoring
storage.recompute_distance_field();
let matcher = CorrelativeMatcher::with_defaults();
// Create points in AoS format for scalar
let points: Vec<(f32, f32)> = vec![(0.0, 0.0), (0.05, 0.0), (0.1, 0.0), (0.15, 0.0)];
// Convert to SoA format for SIMD
let points_soa = PointCloud {
xs: points.iter().map(|(x, _)| *x).collect(),
ys: points.iter().map(|(_, y)| *y).collect(),
};
let pose = Pose2D::new(0.0, 0.0, 0.0);
let mut scratch = ScratchBuffers::new(points.len());
// Get scalar score
let (scalar_score, scalar_hits) = matcher.score_pose(&points, pose, &storage);
// Get SIMD score
let (simd_score, simd_hits) =
matcher.score_pose_simd(&points_soa, pose, &storage, &mut scratch);
// Results should match
assert!(
(scalar_score - simd_score).abs() < 0.001,
"score mismatch: {} vs {}",
scalar_score,
simd_score
);
assert_eq!(
scalar_hits, simd_hits,
"hits mismatch: {} vs {}",
scalar_hits, simd_hits
);
}
}
vastu-map/src/slam/matcher/types.rs
+35
View File
@@ -0,0 +1,35 @@
//! Matcher types and scratch buffers.
/// Scratch buffers for SIMD operations to avoid per-call allocations.
pub struct ScratchBuffers {
/// Transformed world X coordinates
pub world_xs: Vec<f32>,
/// Transformed world Y coordinates
pub world_ys: Vec<f32>,
/// Grid X coordinates
pub grid_xs: Vec<i32>,
/// Grid Y coordinates
pub grid_ys: Vec<i32>,
}
impl ScratchBuffers {
/// Create scratch buffers for a given number of points.
pub fn new(capacity: usize) -> Self {
Self {
world_xs: vec![0.0; capacity],
world_ys: vec![0.0; capacity],
grid_xs: vec![0; capacity],
grid_ys: vec![0; capacity],
}
}
/// Resize buffers if needed.
pub fn resize(&mut self, n: usize) {
if self.world_xs.len() < n {
self.world_xs.resize(n, 0.0);
self.world_ys.resize(n, 0.0);
self.grid_xs.resize(n, 0);
self.grid_ys.resize(n, 0);
}
}
}
vastu-map/src/slam/mod.rs
+12 -1
View File
@@ -25,12 +25,23 @@
//! }
//! ```
mod background_optimizer;
mod branch_bound;
mod config;
mod matcher;
mod types;
pub mod loop_closure;
pub use background_optimizer::{
BackgroundOptimizer, BackgroundOptimizerConfig, OptimizationResult, OptimizerTrigger,
PoseCorrection,
};
pub use branch_bound::{
BranchBoundConfig, BranchBoundResult, NUM_LEVELS, PrecomputedGrids, branch_and_bound_match,
branch_and_bound_match_scan, branch_and_bound_match_simd,
};
pub use config::CorrelativeMatcherConfig;
pub use matcher::CorrelativeMatcher;
pub use loop_closure::{LoopClosureConfig, LoopClosureDetector, PoseGraphConfig};
pub use matcher::{CorrelativeMatcher, ScratchBuffers};
pub use types::{MatchQuality, ScanMatchResult};
vastu-map/src/submap/graph.rs
+1 -18
View File
@@ -5,7 +5,7 @@
use serde::{Deserialize, Serialize};
use crate::core::Pose2D;
use crate::core::{Pose2D, normalize_angle};
use super::manager::SubmapCorrection;
use super::types::SubmapId;
@@ -174,12 +174,6 @@ impl SubmapPoseGraph {
self.nodes.iter().find(|n| n.id == id)
}
/// Get node index by submap ID.
#[allow(dead_code)]
fn node_index(&self, id: SubmapId) -> Option<usize> {
self.nodes.iter().position(|n| n.id == id)
}
/// Add a new submap node.
///
/// If this is not the first node, also creates an odometry edge
@@ -443,17 +437,6 @@ fn huber_weight(residual: f32, delta: f32) -> f32 {
if abs_r <= delta { 1.0 } else { delta / abs_r }
}
/// Normalize angle to [-π, π).
fn normalize_angle(angle: f32) -> f32 {
let mut a = angle % std::f32::consts::TAU;
if a > std::f32::consts::PI {
a -= std::f32::consts::TAU;
} else if a < -std::f32::consts::PI {
a += std::f32::consts::TAU;
}
a
}
#[cfg(test)]
mod tests {
use super::*;
vastu-map/src/submap/matching.rs
+31 -6
View File
@@ -57,33 +57,58 @@ impl MultiSubmapMatchResult {
}
/// Configuration for multi-submap matching.
#[derive(Clone, Debug)]
#[derive(Clone, Debug, serde::Serialize, serde::Deserialize)]
pub struct MultiSubmapMatchConfig {
/// Maximum distance to consider overlapping submaps.
#[serde(default = "default_max_overlap_distance")]
pub max_overlap_distance: f32,
/// Include active submap in search.
#[serde(default = "default_include_active")]
pub include_active: bool,
/// Include finalized submaps in search.
#[serde(default = "default_include_finalized")]
pub include_finalized: bool,
/// Maximum number of submaps to search.
#[serde(default = "default_max_submaps")]
pub max_submaps: usize,
/// Minimum score improvement to prefer a different submap.
/// Prevents jumping between submaps with similar scores.
#[serde(default = "default_score_hysteresis")]
pub score_hysteresis: f32,
}
fn default_max_overlap_distance() -> f32 {
3.0
}
fn default_include_active() -> bool {
true
}
fn default_include_finalized() -> bool {
true
}
fn default_max_submaps() -> usize {
3
}
fn default_score_hysteresis() -> f32 {
0.05
}
impl Default for MultiSubmapMatchConfig {
fn default() -> Self {
Self {
max_overlap_distance: 3.0, // 3m
include_active: true,
include_finalized: true,
max_submaps: 5,
score_hysteresis: 0.05,
max_overlap_distance: default_max_overlap_distance(),
include_active: default_include_active(),
include_finalized: default_include_finalized(),
max_submaps: default_max_submaps(),
score_hysteresis: default_score_hysteresis(),
}
}
}
vastu-map/src/submap/types.rs
+53
View File
@@ -2,6 +2,8 @@
use crate::core::{LidarScan, Pose2D, WorldPoint};
use crate::grid::GridStorage;
use crate::slam::PrecomputedGrids;
use std::cell::RefCell;
/// Unique identifier for a submap.
#[derive(Clone, Copy, Debug, PartialEq, Eq, Hash)]
@@ -134,6 +136,10 @@ pub struct Submap {
/// Last pose in local frame (for distance tracking).
last_local_pose: Option<Pose2D>,
/// Cached precomputed multi-resolution grids for branch-and-bound matching.
/// Lazily initialized when first needed, invalidated on grid regeneration.
precomputed_cache: RefCell<Option<PrecomputedGrids>>,
}
impl Submap {
@@ -156,6 +162,7 @@ impl Submap {
overlap_count: 0,
distance_traveled: 0.0,
last_local_pose: None,
precomputed_cache: RefCell::new(None),
}
}
@@ -233,17 +240,58 @@ impl Submap {
}
/// Finalize the submap - no more updates allowed.
/// Also builds the precomputed grids cache for efficient matching.
pub fn finalize(&mut self) {
self.state = SubmapState::Finalized;
// Pre-build the cache since finalized submaps are used for matching
self.build_precomputed_grids();
}
/// Get the precomputed multi-resolution grids for branch-and-bound matching.
/// Lazily computes and caches the grids on first access.
/// Only useful for finalized submaps.
pub fn precomputed_grids(&self) -> std::cell::Ref<'_, PrecomputedGrids> {
// Ensure cache is populated
{
let mut cache = self.precomputed_cache.borrow_mut();
if cache.is_none() {
*cache = Some(PrecomputedGrids::from_storage(&self.grid));
}
}
std::cell::Ref::map(self.precomputed_cache.borrow(), |opt| {
opt.as_ref().expect("Cache should be populated")
})
}
/// Check if precomputed grids are already cached.
#[inline]
pub fn has_precomputed_grids(&self) -> bool {
self.precomputed_cache.borrow().is_some()
}
/// Force rebuild of precomputed grids cache.
/// Call this after finalization or after origin adjustment.
pub fn build_precomputed_grids(&mut self) {
*self.precomputed_cache.borrow_mut() = Some(PrecomputedGrids::from_storage(&self.grid));
}
/// Invalidate the precomputed grids cache.
/// Called when the grid is regenerated.
fn invalidate_precomputed_cache(&mut self) {
*self.precomputed_cache.borrow_mut() = None;
}
/// Regenerate the local grid from stored scans.
/// Called after the submap origin has been adjusted.
/// Invalidates and rebuilds the precomputed grids cache if finalized.
pub fn regenerate_grid(
&mut self,
sensor_config: &crate::grid::SensorConfig,
grid_config: &crate::grid::GridConfig,
) {
// Invalidate the cache since grid is changing
self.invalidate_precomputed_cache();
// Clear the grid
self.grid.clear();
@@ -257,6 +305,11 @@ impl Submap {
grid_config,
);
}
// Rebuild cache if this is a finalized submap
if self.is_finalized() {
self.build_precomputed_grids();
}
}
/// Get the bounds of this submap in world coordinates.