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

Add zero-allocation scratch space for RANSAC, ICP, and frontier detection

Browse Source 
Eliminate per-frame allocations in real-time SLAM pipeline by introducing
reusable scratch buffers:

- RansacScratchSpace: Pre-allocated buffers for RANSAC line extraction
  with extract_lines_ransac_with_scratch and hybrid variant
- IcpScratchSpace: Extended with subsampling buffers for multi-resolution
  matching; PointToLineIcp now holds internal scratch
- FrontierDetector: Persistent spatial hash grid and output buffers
- ObserveScratchSpace: Buffers for VectorMap observe pipeline
- CorrespondenceSet: In-place retain_by_distance/retain_within_segment

ScanMatcher trait methods changed from &self to &mut self to support
internal scratch space usage.
This commit is contained in:
codetiger
2025-12-21 12:19:08 +05:30
parent 06b0b06fe8
commit dfbb33bb4a
9 changed files with 1408 additions and 109 deletions
Download Patch File Download Diff File Expand all files Collapse all files
vastu-map/src/extraction/mod.rs
+54 -1
View File
@@ -56,7 +56,9 @@ pub use line_fitting::{
fitting_error, fitting_error_weighted, max_distance_point,
};
// Re-export from ransac_lines module
pub use ransac_lines::{RansacLineConfig, extract_lines_ransac};
pub use ransac_lines::{
RansacLineConfig, RansacScratchSpace, extract_lines_ransac, extract_lines_ransac_with_scratch,
};
// Re-export from split_merge submodule
pub use split_merge::{
SplitMergeConfig, adaptive_split_threshold, extract_lines, extract_lines_from_sensor,
@@ -119,3 +121,54 @@ pub fn extract_lines_hybrid(
lines
}
/// Extract lines using hybrid RANSAC + split-merge with pre-allocated scratch space.
///
/// This is the zero-allocation version for real-time applications.
/// Reuse the scratch space across multiple calls to eliminate allocations.
///
/// # Arguments
/// * `points` - Input point cloud
/// * `ransac_config` - Configuration for RANSAC extraction
/// * `split_merge_config` - Configuration for split-merge extraction
/// * `scratch` - Pre-allocated RANSAC scratch space
/// * `output_lines` - Buffer to store output lines (cleared and filled)
///
/// # Example
/// ```rust,ignore
/// let mut scratch = RansacScratchSpace::with_capacity(400);
/// let mut lines = Vec::new();
///
/// for scan in scans {
/// extract_lines_hybrid_with_scratch(&scan, &ransac_cfg, &sm_cfg, &mut scratch, &mut lines);
/// // lines contains extracted features, scratch is reused
/// }
/// ```
pub fn extract_lines_hybrid_with_scratch(
points: &[Point2D],
ransac_config: &RansacLineConfig,
split_merge_config: &SplitMergeConfig,
scratch: &mut RansacScratchSpace,
output_lines: &mut Vec<Line2D>,
) {
output_lines.clear();
if points.is_empty() {
return;
}
// Step 1: RANSAC for dominant lines (using scratch space)
let (ransac_lines, remaining_indices) =
extract_lines_ransac_with_scratch(points, ransac_config, scratch);
output_lines.extend_from_slice(ransac_lines);
// Step 2: Split-merge on remaining points
// Note: We still need to collect remaining points for split-merge,
// but at least the expensive RANSAC iterations are allocation-free
if remaining_indices.len() >= 3 {
let remaining_points: Vec<Point2D> = remaining_indices.iter().map(|&i| points[i]).collect();
let additional_lines = extract_lines(&remaining_points, split_merge_config);
output_lines.extend(additional_lines);
}
}
vastu-map/src/extraction/ransac_lines.rs
+546 -4
View File
@@ -7,13 +7,16 @@
//! # Usage
//!
//! ```rust,ignore
//! use vastu_map::extraction::{extract_lines_ransac, RansacLineConfig};
//! use vastu_map::extraction::{extract_lines_ransac, RansacLineConfig, RansacScratchSpace};
//!
//! let config = RansacLineConfig::default();
//! let (lines, remaining) = extract_lines_ransac(&points, &config);
//!
//! // `lines` contains extracted lines
//! // `remaining` contains indices of points not matched to any line
//! // Zero-allocation version with scratch space
//! let mut scratch = RansacScratchSpace::with_capacity(400);
//! let (lines, remaining) = extract_lines_ransac_with_scratch(&points, &config, &mut scratch);
//!
//! // Reuse scratch for next scan - no allocations!
//! let (lines2, remaining2) = extract_lines_ransac_with_scratch(&points2, &config, &mut scratch);
//! ```
use crate::core::Point2D;
@@ -21,6 +24,129 @@ use crate::features::Line2D;
use super::line_fitting::fit_line;
// ============================================================================
// Scratch Space for Zero-Allocation RANSAC
// ============================================================================
/// Pre-allocated scratch space for RANSAC operations.
///
/// Reuse this struct across multiple `extract_lines_ransac_with_scratch` calls
/// to eliminate per-iteration allocations. This is critical for real-time SLAM
/// where RANSAC may run 100+ iterations per scan.
///
/// # Performance Impact
///
/// Without scratch space, each RANSAC call allocates:
/// - `remaining_mask`: O(n) bools
/// - `remaining`: O(n) indices, rebuilt every outer iteration
/// - `current_inliers`: O(n) indices, rebuilt every RANSAC iteration (100x)
/// - `best_inliers`: O(n) indices
/// - `inlier_points`: O(n) Point2Ds
///
/// With scratch space, these allocations happen once and are reused.
///
/// # Example
/// ```rust,ignore
/// use vastu_map::extraction::{RansacScratchSpace, extract_lines_ransac_with_scratch};
///
/// // Create once, sized for typical lidar scans
/// let mut scratch = RansacScratchSpace::with_capacity(400);
///
/// for scan in scans {
/// let (lines, remaining) = extract_lines_ransac_with_scratch(&scan, &config, &mut scratch);
/// // scratch is reused, no allocations!
/// }
/// ```
#[derive(Clone, Debug)]
pub struct RansacScratchSpace {
/// Bit mask for remaining (unconsumed) points.
/// Avoids rebuilding Vec<usize> each iteration.
remaining_mask: Vec<bool>,
/// Buffer for current iteration's inlier indices.
/// Reused across all RANSAC iterations.
current_inliers: Vec<usize>,
/// Buffer for best inliers found so far.
best_inliers: Vec<usize>,
/// Buffer for inlier points (for line fitting and gap splitting).
inlier_points: Vec<Point2D>,
/// Buffer for inlier indices corresponding to inlier_points.
inlier_indices: Vec<usize>,
/// Buffer for output lines.
output_lines: Vec<Line2D>,
/// Buffer for remaining indices output.
remaining_output: Vec<usize>,
/// Buffer for segment points in split_at_gaps.
segment_points: Vec<Point2D>,
/// Buffer for segment indices in split_at_gaps.
segment_indices: Vec<usize>,
/// Buffer for projection sorting in split_at_gaps.
projection_buffer: Vec<(usize, f32)>,
}
impl RansacScratchSpace {
/// Create new scratch space with capacity hints.
///
/// # Arguments
/// * `max_points` - Expected maximum scan points (e.g., 360-400 for typical lidar)
pub fn with_capacity(max_points: usize) -> Self {
Self {
remaining_mask: Vec::with_capacity(max_points),
current_inliers: Vec::with_capacity(max_points),
best_inliers: Vec::with_capacity(max_points),
inlier_points: Vec::with_capacity(max_points),
inlier_indices: Vec::with_capacity(max_points),
output_lines: Vec::with_capacity(32), // Typical max lines per scan
remaining_output: Vec::with_capacity(max_points),
segment_points: Vec::with_capacity(max_points),
segment_indices: Vec::with_capacity(max_points),
projection_buffer: Vec::with_capacity(max_points),
}
}
/// Create with default capacity (400 points).
pub fn default_capacity() -> Self {
Self::with_capacity(400)
}
/// Reset scratch space for a new extraction run.
/// Clears all buffers but retains allocated capacity.
#[inline]
pub fn reset(&mut self, num_points: usize) {
self.remaining_mask.clear();
self.remaining_mask.resize(num_points, true);
self.current_inliers.clear();
self.best_inliers.clear();
self.inlier_points.clear();
self.inlier_indices.clear();
self.output_lines.clear();
self.remaining_output.clear();
self.segment_points.clear();
self.segment_indices.clear();
self.projection_buffer.clear();
}
/// Count remaining active points using the mask.
#[inline]
fn count_remaining(&self) -> usize {
self.remaining_mask.iter().filter(|&&b| b).count()
}
}
impl Default for RansacScratchSpace {
fn default() -> Self {
Self::default_capacity()
}
}
/// Configuration for RANSAC line extraction.
#[derive(Clone, Debug)]
pub struct RansacLineConfig {
@@ -361,6 +487,264 @@ pub fn extract_lines_ransac(
(lines, remaining)
}
/// Extract lines using RANSAC with pre-allocated scratch space.
///
/// This is the zero-allocation version for real-time applications.
/// Reuse the scratch space across multiple calls to eliminate allocations.
///
/// Returns extracted lines and indices of points not matched to any line.
/// The returned vectors are borrowed from scratch space and are valid until
/// the next call to this function.
///
/// # Arguments
/// * `points` - Input point cloud
/// * `config` - RANSAC configuration
/// * `scratch` - Pre-allocated scratch space (reused across calls)
///
/// # Returns
/// Tuple of (&[Line2D], &[usize]) - references into scratch space
///
/// # Example
/// ```rust,ignore
/// let mut scratch = RansacScratchSpace::with_capacity(400);
/// let (lines, remaining) = extract_lines_ransac_with_scratch(&points, &config, &mut scratch);
/// // Process lines and remaining before next call
/// ```
pub fn extract_lines_ransac_with_scratch<'a>(
points: &[Point2D],
config: &RansacLineConfig,
scratch: &'a mut RansacScratchSpace,
) -> (&'a [Line2D], &'a [usize]) {
// Reset scratch for this extraction
scratch.reset(points.len());
if points.len() < config.min_inliers {
// Return remaining as all indices
scratch.remaining_output.extend(0..points.len());
return (&scratch.output_lines, &scratch.remaining_output);
}
let mut rng = SimpleRng::new(config.seed);
loop {
// Check max lines limit
if config.max_lines > 0 && scratch.output_lines.len() >= config.max_lines {
break;
}
// Count remaining points without allocating
let remaining_count = scratch.count_remaining();
if remaining_count < config.min_inliers {
break;
}
// Run RANSAC to find best line
scratch.best_inliers.clear();
for _ in 0..config.iterations {
// Sample random indices from remaining points
// We need to pick the nth remaining point, not the nth point
let sample_pos1 = rng.gen_range(remaining_count);
let sample_pos2 = rng.gen_range(remaining_count);
if sample_pos1 == sample_pos2 {
continue;
}
// Convert sample positions to actual indices
let idx1 = nth_remaining(&scratch.remaining_mask, sample_pos1);
let idx2 = nth_remaining(&scratch.remaining_mask, sample_pos2);
let p1 = points[idx1];
let p2 = points[idx2];
// Check minimum distance between sample points
let sample_dist = p1.distance(p2);
if sample_dist < config.min_line_length * 0.3 {
continue;
}
// Create candidate line
let line = Line2D::new(p1, p2);
// Count inliers into scratch buffer (reused each iteration)
scratch.current_inliers.clear();
for (i, &is_remaining) in scratch.remaining_mask.iter().enumerate() {
if is_remaining && line.distance_to_point(points[i]) < config.inlier_threshold {
scratch.current_inliers.push(i);
}
}
// Update best if this is better
if scratch.current_inliers.len() > scratch.best_inliers.len() {
scratch.best_inliers.clear();
scratch
.best_inliers
.extend_from_slice(&scratch.current_inliers);
}
}
// Check if we found enough inliers
if scratch.best_inliers.len() < config.min_inliers {
break;
}
// Get inlier points into scratch buffer
scratch.inlier_points.clear();
scratch.inlier_indices.clear();
for &i in &scratch.best_inliers {
scratch.inlier_points.push(points[i]);
scratch.inlier_indices.push(i);
}
// Split at gaps and process segments
let added_any = split_at_gaps_with_scratch(config, scratch);
// If we couldn't add any valid lines from this iteration, stop
if !added_any {
break;
}
}
// Collect remaining point indices into output buffer
scratch.remaining_output.clear();
for (i, &is_remaining) in scratch.remaining_mask.iter().enumerate() {
if is_remaining {
scratch.remaining_output.push(i);
}
}
(&scratch.output_lines, &scratch.remaining_output)
}
/// Find the nth remaining (active) index in the mask.
#[inline]
fn nth_remaining(mask: &[bool], mut n: usize) -> usize {
for (i, &active) in mask.iter().enumerate() {
if active {
if n == 0 {
return i;
}
n -= 1;
}
}
// Fallback (shouldn't happen if n < count_remaining)
0
}
/// Split inliers at gaps using scratch buffers, add valid lines to output.
/// Returns true if any lines were added.
fn split_at_gaps_with_scratch(config: &RansacLineConfig, scratch: &mut RansacScratchSpace) -> bool {
let num_inliers = scratch.inlier_points.len();
if num_inliers < 2 {
// Single point segment - try to fit
if num_inliers >= config.min_inliers
&& let Some(line) = fit_line(&scratch.inlier_points)
&& line.length() >= config.min_line_length
{
scratch.output_lines.push(line);
for i in 0..scratch.inlier_indices.len() {
let idx = scratch.inlier_indices[i];
scratch.remaining_mask[idx] = false;
}
return true;
}
return false;
}
// Compute principal direction using PCA-like approach
let n = num_inliers as f32;
let mean_x: f32 = scratch.inlier_points.iter().map(|p| p.x).sum::<f32>() / n;
let mean_y: f32 = scratch.inlier_points.iter().map(|p| p.y).sum::<f32>() / n;
// Compute covariance matrix elements
let mut cxx = 0.0f32;
let mut cxy = 0.0f32;
let mut cyy = 0.0f32;
for p in &scratch.inlier_points {
let dx = p.x - mean_x;
let dy = p.y - mean_y;
cxx += dx * dx;
cxy += dx * dy;
cyy += dy * dy;
}
// Principal direction from dominant eigenvector
let theta = 0.5 * (2.0 * cxy).atan2(cxx - cyy);
let dir_x = theta.cos();
let dir_y = theta.sin();
// Project points onto principal direction and sort
scratch.projection_buffer.clear();
for (i, p) in scratch.inlier_points.iter().enumerate() {
let proj = (p.x - mean_x) * dir_x + (p.y - mean_y) * dir_y;
scratch.projection_buffer.push((i, proj));
}
scratch
.projection_buffer
.sort_by(|a, b| a.1.partial_cmp(&b.1).unwrap_or(std::cmp::Ordering::Equal));
// Process segments separated by gaps
// We need to iterate through projection_buffer and build segments,
// then mark consumed indices. To avoid borrow conflicts, we'll
// collect segment boundaries first, then process them.
let mut added_any = false;
scratch.segment_points.clear();
scratch.segment_indices.clear();
let projection_len = scratch.projection_buffer.len();
let mut segment_start = 0usize;
for i in 0..projection_len {
let (orig_idx, _proj) = scratch.projection_buffer[i];
// Check if there's a gap to the next point
let is_gap = if i + 1 < projection_len {
let next_orig_idx = scratch.projection_buffer[i + 1].0;
let p1 = scratch.inlier_points[orig_idx];
let p2 = scratch.inlier_points[next_orig_idx];
p1.distance(p2) > config.max_point_gap
} else {
true // End of points
};
if is_gap {
// Process segment from segment_start to i (inclusive)
let segment_len = i - segment_start + 1;
if segment_len >= config.min_inliers {
// Collect segment points
scratch.segment_points.clear();
scratch.segment_indices.clear();
for j in segment_start..=i {
let idx = scratch.projection_buffer[j].0;
scratch.segment_points.push(scratch.inlier_points[idx]);
scratch.segment_indices.push(scratch.inlier_indices[idx]);
}
// Try to fit line
if let Some(refined_line) = fit_line(&scratch.segment_points)
&& refined_line.length() >= config.min_line_length
{
scratch.output_lines.push(refined_line);
added_any = true;
// Mark segment inliers as consumed
for j in 0..scratch.segment_indices.len() {
let idx = scratch.segment_indices[j];
scratch.remaining_mask[idx] = false;
}
}
}
segment_start = i + 1;
}
}
added_any
}
#[cfg(test)]
mod tests {
use super::*;
@@ -595,4 +979,162 @@ mod tests {
assert!(line.length() > 0.15, "Line too short: {}", line.length());
}
}
// =========================================================================
// Tests for scratch space version
// =========================================================================
#[test]
fn test_scratch_space_creation() {
let scratch = RansacScratchSpace::with_capacity(400);
assert!(scratch.remaining_mask.capacity() >= 400);
assert!(scratch.current_inliers.capacity() >= 400);
}
#[test]
fn test_scratch_extract_single_line() {
// Same test as non-scratch version
let points: Vec<Point2D> = (0..20)
.map(|i| Point2D::new(i as f32 * 0.05, 0.0))
.collect();
let config = RansacLineConfig::default()
.with_seed(42)
.with_min_inliers(5);
let mut scratch = RansacScratchSpace::with_capacity(400);
let (lines, remaining) = extract_lines_ransac_with_scratch(&points, &config, &mut scratch);
assert_eq!(lines.len(), 1);
assert!(remaining.len() < 5);
let angle = lines[0].angle().abs();
assert!(angle < 0.1 || (std::f32::consts::PI - angle).abs() < 0.1);
}
#[test]
fn test_scratch_extract_two_lines() {
let mut points: Vec<Point2D> = (0..15)
.map(|i| Point2D::new(i as f32 * 0.05, 0.0))
.collect();
for i in 0..15 {
points.push(Point2D::new(0.0, i as f32 * 0.05));
}
let config = RansacLineConfig::default()
.with_seed(42)
.with_min_inliers(5)
.with_max_lines(2);
let mut scratch = RansacScratchSpace::with_capacity(400);
let (lines, _remaining) = extract_lines_ransac_with_scratch(&points, &config, &mut scratch);
assert_eq!(lines.len(), 2);
}
#[test]
fn test_scratch_reuse() {
// Test that scratch space can be reused across multiple calls
let mut scratch = RansacScratchSpace::with_capacity(400);
// First extraction
let points1: Vec<Point2D> = (0..20)
.map(|i| Point2D::new(i as f32 * 0.05, 0.0))
.collect();
let config = RansacLineConfig::default()
.with_seed(42)
.with_min_inliers(5);
let (lines1, _) = extract_lines_ransac_with_scratch(&points1, &config, &mut scratch);
assert_eq!(lines1.len(), 1);
// Second extraction with different points - reusing same scratch
let points2: Vec<Point2D> = (0..20)
.map(|i| Point2D::new(0.0, i as f32 * 0.05))
.collect();
let (lines2, _) = extract_lines_ransac_with_scratch(&points2, &config, &mut scratch);
assert_eq!(lines2.len(), 1);
// Verify the second extraction is independent
let angle2 = lines2[0].angle().abs();
let expected_vertical = std::f32::consts::FRAC_PI_2;
assert!((angle2 - expected_vertical).abs() < 0.1);
}
#[test]
fn test_scratch_equivalence_to_original() {
// Verify scratch version produces same results as original
let points: Vec<Point2D> = (0..25)
.map(|i| Point2D::new(i as f32 * 0.04, 0.0))
.collect();
let config = RansacLineConfig::default()
.with_seed(123)
.with_min_inliers(5)
.with_iterations(50);
// Original version
let (lines_orig, remaining_orig) = extract_lines_ransac(&points, &config);
// Scratch version
let mut scratch = RansacScratchSpace::with_capacity(100);
let (lines_scratch, remaining_scratch) =
extract_lines_ransac_with_scratch(&points, &config, &mut scratch);
// Same number of lines
assert_eq!(lines_orig.len(), lines_scratch.len());
// Same number of remaining points
assert_eq!(remaining_orig.len(), remaining_scratch.len());
}
#[test]
fn test_scratch_gap_splitting() {
// Collinear points with a gap
let mut points = Vec::new();
for i in 0..10 {
points.push(Point2D::new(i as f32 * 0.03, 0.0));
}
for i in 0..10 {
points.push(Point2D::new(0.52 + i as f32 * 0.03, 0.0));
}
let config = RansacLineConfig::default()
.with_seed(42)
.with_min_inliers(5)
.with_max_point_gap(0.10);
let mut scratch = RansacScratchSpace::with_capacity(100);
let (lines, _) = extract_lines_ransac_with_scratch(&points, &config, &mut scratch);
assert_eq!(lines.len(), 2);
for line in lines {
assert!(line.length() < 0.35);
assert!(line.length() > 0.15);
}
}
#[test]
fn test_scratch_empty_input() {
let points: Vec<Point2D> = vec![];
let config = RansacLineConfig::default();
let mut scratch = RansacScratchSpace::with_capacity(100);
let (lines, remaining) = extract_lines_ransac_with_scratch(&points, &config, &mut scratch);
assert!(lines.is_empty());
assert!(remaining.is_empty());
}
#[test]
fn test_scratch_too_few_points() {
let points = vec![Point2D::new(0.0, 0.0), Point2D::new(1.0, 0.0)];
let config = RansacLineConfig::default().with_min_inliers(5);
let mut scratch = RansacScratchSpace::with_capacity(100);
let (lines, remaining) = extract_lines_ransac_with_scratch(&points, &config, &mut scratch);
assert!(lines.is_empty());
assert_eq!(remaining.len(), 2);
}
}
vastu-map/src/matching/correspondence.rs
+15
View File
@@ -141,6 +141,15 @@ impl CorrespondenceSet {
}
}
/// Filter correspondences by maximum distance in-place (zero-allocation).
///
/// This is more efficient than `filter_by_distance` when you don't need
/// to preserve the original set.
#[inline]
pub fn retain_by_distance(&mut self, max_distance: f32) {
self.correspondences.retain(|c| c.distance <= max_distance);
}
/// Filter correspondences to only those within line segments.
pub fn filter_within_segment(&self) -> Self {
Self {
@@ -153,6 +162,12 @@ impl CorrespondenceSet {
}
}
/// Filter correspondences to only those within line segments in-place (zero-allocation).
#[inline]
pub fn retain_within_segment(&mut self) {
self.correspondences.retain(|c| c.is_within_segment());
}
/// Get the mean correspondence distance.
pub fn mean_distance(&self) -> f32 {
if self.is_empty() {
vastu-map/src/matching/icp/matcher.rs
+93 -60
View File
@@ -7,9 +7,7 @@ use super::super::correspondence::{
CorrespondenceSet, IDENTITY_COVARIANCE, MatchResult, PoseCovariance,
};
use super::super::gauss_newton::{GaussNewtonConfig, optimize_pose, optimize_pose_fast};
use super::super::nearest_neighbor::{
NearestNeighborConfig, find_correspondences, find_correspondences_batch,
};
use super::super::nearest_neighbor::NearestNeighborConfig;
use super::super::ransac::RansacConfig;
use super::super::ransac::estimate_pose_ransac;
use super::super::scratch::IcpScratchSpace;
@@ -18,6 +16,7 @@ use super::config::{CoarseSearchConfig, IcpConfig};
/// Point-to-Line ICP matcher.
///
/// Holds configuration and provides scan matching functionality.
/// Includes internal scratch space for zero-allocation matching.
pub struct PointToLineIcp {
/// ICP configuration.
config: IcpConfig,
@@ -29,6 +28,8 @@ pub struct PointToLineIcp {
coarse_config: CoarseSearchConfig,
/// Last match confidence for adaptive coarse search.
last_confidence: f32,
/// Pre-allocated scratch space for zero-allocation matching.
scratch: IcpScratchSpace,
}
impl PointToLineIcp {
@@ -53,6 +54,7 @@ impl PointToLineIcp {
gn_config,
coarse_config: CoarseSearchConfig::default(),
last_confidence: 1.0, // Start with high confidence (no coarse search first time)
scratch: IcpScratchSpace::default_capacity(), // 360 points, 200 lines
}
}
@@ -87,14 +89,13 @@ impl PointToLineIcp {
let t_steps = ((2.0 * cfg.translation_range / cfg.translation_step) as usize).max(1);
let r_steps = ((2.0 * cfg.rotation_range / cfg.rotation_step) as usize).max(1);
let mut best_pose = initial_pose;
let mut best_score = f32::MAX;
// Subsample points for fast evaluation
let subsampled_points: Vec<&Point2D> =
points.iter().step_by(cfg.subsample_rate.max(1)).collect();
// Search grid
let mut best_pose = initial_pose;
let mut best_score = f32::MAX;
for tx_i in 0..=t_steps {
let tx = initial_pose.x - cfg.translation_range + (tx_i as f32 * cfg.translation_step);
@@ -107,8 +108,6 @@ impl PointToLineIcp {
initial_pose.theta - cfg.rotation_range + (r_i as f32 * cfg.rotation_step);
let test_pose = Pose2D::new(tx, ty, theta);
// Compute score (sum of squared distances)
let score = self.score_pose(&subsampled_points, lines, &test_pose);
if score < best_score {
@@ -156,7 +155,7 @@ impl PointToLineIcp {
/// # Returns
/// Match result with optimized pose, covariance, and confidence.
pub fn match_scan(
&self,
&mut self,
points: &[Point2D],
lines: &[Line2D],
initial_pose: Pose2D,
@@ -184,8 +183,7 @@ impl PointToLineIcp {
// Optional RANSAC initialization (after coarse search if used)
if self.config.use_ransac_init {
let initial_corrs =
self.find_correspondences_for_pose(points, lines, Some(&line_collection), &pose);
let initial_corrs = self.find_correspondences_for_pose(points, &line_collection, &pose);
if initial_corrs.len() >= self.config.min_correspondences {
let ransac_config = RansacConfig::default()
@@ -207,7 +205,7 @@ impl PointToLineIcp {
// 1. Find correspondences at current pose
let correspondences =
self.find_correspondences_for_pose(points, lines, Some(&line_collection), &pose);
self.find_correspondences_for_pose(points, &line_collection, &pose);
if correspondences.len() < self.config.min_correspondences {
// Not enough correspondences - return with low confidence
@@ -278,7 +276,7 @@ impl PointToLineIcp {
/// Match scan against line collection (optimized for large maps).
pub fn match_scan_batch(
&self,
&mut self,
points: &[Point2D],
lines: &LineCollection,
initial_pose: Pose2D,
@@ -301,7 +299,7 @@ impl PointToLineIcp {
/// # Returns
/// Match result with optimized pose, covariance, and confidence.
pub fn match_scan_robot_frame(
&self,
&mut self,
robot_frame_points: &[Point2D],
lines: &[Line2D],
initial_pose: Pose2D,
@@ -323,7 +321,56 @@ impl PointToLineIcp {
// Convert lines to collection for batch operations
let line_collection = LineCollection::from_lines(lines);
self.match_scan_robot_frame_impl(robot_frame_points, lines, &line_collection, initial_pose)
}
/// Match scan with range-based weighted correspondences using cached LineCollection.
///
/// This is the preferred method when the LineCollection is already cached
/// (e.g., in LineStore), avoiding re-computation of the SoA structure.
///
/// # Arguments
/// * `robot_frame_points` - Scan points in robot frame (for weight computation)
/// * `line_collection` - Cached LineCollection from LineStore
/// * `initial_pose` - Initial pose estimate in world frame (from odometry)
///
/// # Returns
/// Match result with optimized pose, covariance, and confidence.
pub fn match_scan_robot_frame_collection(
&mut self,
robot_frame_points: &[Point2D],
line_collection: &LineCollection,
initial_pose: Pose2D,
) -> MatchResult {
if robot_frame_points.is_empty() || line_collection.is_empty() {
return MatchResult::new(initial_pose, CorrespondenceSet::new(), 0, false);
}
// Convert to lines for RANSAC/GN (they require line endpoints)
let lines = line_collection.to_lines();
// Use multi-resolution ICP if enabled
if self.config.multi_resolution.enabled {
return match_scan_multiresolution(
robot_frame_points,
&lines,
initial_pose,
&self.config.multi_resolution,
&self.config,
);
}
self.match_scan_robot_frame_impl(robot_frame_points, &lines, line_collection, initial_pose)
}
/// Internal implementation for robot-frame matching.
fn match_scan_robot_frame_impl(
&mut self,
robot_frame_points: &[Point2D],
lines: &[Line2D],
line_collection: &LineCollection,
initial_pose: Pose2D,
) -> MatchResult {
let mut pose = initial_pose;
let mut prev_pose = pose;
let mut final_correspondences = CorrespondenceSet::new();
@@ -338,18 +385,14 @@ impl PointToLineIcp {
// Optional coarse search when confidence is low
// Note: coarse_search expects robot-frame points and transforms them internally
if self.should_use_coarse_search() {
pose = self.coarse_search(robot_frame_points, &line_collection, initial_pose);
pose = self.coarse_search(robot_frame_points, line_collection, initial_pose);
used_coarse_search = true;
}
// Optional RANSAC initialization
if self.config.use_ransac_init {
let mut initial_corrs = self.find_correspondences_for_pose(
robot_frame_points,
lines,
Some(&line_collection),
&pose,
);
let mut initial_corrs =
self.find_correspondences_for_pose(robot_frame_points, line_collection, &pose);
// Apply weights based on robot-frame ranges
initial_corrs.apply_weights(robot_frame_points, &self.config.noise_model);
@@ -373,12 +416,8 @@ impl PointToLineIcp {
// 1. Find correspondences at current pose
// Pass robot-frame points - find_correspondences_for_pose transforms them internally
let mut correspondences = self.find_correspondences_for_pose(
robot_frame_points,
lines,
Some(&line_collection),
&pose,
);
let mut correspondences =
self.find_correspondences_for_pose(robot_frame_points, line_collection, &pose);
// Apply weights based on robot-frame ranges
correspondences.apply_weights(robot_frame_points, &self.config.noise_model);
@@ -467,7 +506,7 @@ impl PointToLineIcp {
/// # Example
/// ```rust,ignore
/// let mut scratch = IcpScratchSpace::default_capacity();
/// let icp = PointToLineIcp::new();
/// let mut icp = PointToLineIcp::new();
///
/// // Reuse scratch across multiple calls
/// for (scan, odom_pose) in scans.iter().zip(odometry.iter()) {
@@ -581,32 +620,26 @@ impl PointToLineIcp {
)
}
/// Find correspondences for points at a given pose.
/// Find correspondences for points at a given pose using scratch space.
///
/// Uses internal scratch space for zero-allocation transformation.
fn find_correspondences_for_pose(
&self,
&mut self,
points: &[Point2D],
lines: &[Line2D],
line_collection: Option<&LineCollection>,
line_collection: &LineCollection,
pose: &Pose2D,
) -> CorrespondenceSet {
// Transform points by pose
// Transform points by pose into scratch space (zero-allocation)
let (sin, cos) = pose.theta.sin_cos();
let transformed: Vec<Point2D> = points
.iter()
.map(|p| {
Point2D::new(
p.x * cos - p.y * sin + pose.x,
p.x * sin + p.y * cos + pose.y,
)
})
.collect();
self.scratch
.transform_points(points, sin, cos, pose.x, pose.y);
// Find correspondences
if let Some(collection) = line_collection {
find_correspondences_batch(&transformed, collection, &self.nn_config)
} else {
find_correspondences(&transformed, lines, &self.nn_config)
}
// Find correspondences using scratch (zero-allocation)
self.scratch
.find_correspondences(line_collection, &self.nn_config);
// Return a clone of the correspondences (scratch needs to be reusable)
self.scratch.correspondences().clone()
}
/// Apply IRLS robust weights to correspondences.
@@ -650,7 +683,7 @@ impl Default for PointToLineIcp {
/// # Returns
/// Match result with optimized pose.
pub fn match_scan(points: &[Point2D], lines: &[Line2D], initial_pose: Pose2D) -> MatchResult {
let icp = PointToLineIcp::new();
let mut icp = PointToLineIcp::new();
icp.match_scan(points, lines, initial_pose)
}
@@ -661,7 +694,7 @@ pub fn match_scan_with_config(
initial_pose: Pose2D,
config: IcpConfig,
) -> MatchResult {
let icp = PointToLineIcp::with_config(config);
let mut icp = PointToLineIcp::with_config(config);
icp.match_scan(points, lines, initial_pose)
}
@@ -732,7 +765,7 @@ pub fn match_scan_multiresolution(
level_config.max_iterations = level_iterations;
// Run ICP at this level
let icp = PointToLineIcp::with_config(level_config);
let mut icp = PointToLineIcp::with_config(level_config);
let result = icp.match_scan(&subsampled, lines, pose);
// Update pose for next level
@@ -796,7 +829,7 @@ mod tests {
let lines = make_room_lines();
let points = make_scan_points_at_origin();
let icp = PointToLineIcp::new();
let mut icp = PointToLineIcp::new();
let result = icp.match_scan(&points, &lines, Pose2D::identity());
assert!(result.converged);
@@ -830,7 +863,7 @@ mod tests {
.with_max_iterations(50)
.with_max_correspondence_distance(0.5);
let icp = PointToLineIcp::with_config(config);
let mut icp = PointToLineIcp::with_config(config);
let result = icp.match_scan(&points, &lines, Pose2D::identity());
// Should find correspondences
@@ -849,7 +882,7 @@ mod tests {
let points = vec![Point2D::new(0.0, -2.0)]; // Only one point
let config = IcpConfig::default().with_min_correspondences(5);
let icp = PointToLineIcp::with_config(config);
let mut icp = PointToLineIcp::with_config(config);
let result = icp.match_scan(&points, &lines, Pose2D::identity());
assert!(!result.converged);
@@ -860,7 +893,7 @@ mod tests {
let lines: Vec<Line2D> = vec![];
let points = make_scan_points_at_origin();
let icp = PointToLineIcp::new();
let mut icp = PointToLineIcp::new();
let result = icp.match_scan(&points, &lines, Pose2D::identity());
assert!(!result.converged);
@@ -882,7 +915,7 @@ mod tests {
.with_robust_cost(RobustCostFunction::Tukey { c: 0.1 })
.with_irls(true);
let icp = PointToLineIcp::with_config(config);
let mut icp = PointToLineIcp::with_config(config);
let result = icp.match_scan(&points, &lines, Pose2D::identity());
// Should still converge despite outliers (they get zero weight)
@@ -902,7 +935,7 @@ mod tests {
.with_robust_cost(RobustCostFunction::Huber { delta: 0.03 })
.with_irls(true);
let icp = PointToLineIcp::with_config(config);
let mut icp = PointToLineIcp::with_config(config);
let result = icp.match_scan(&points, &lines, Pose2D::identity());
assert!(result.converged);
@@ -946,7 +979,7 @@ mod tests {
.collect();
let config = IcpConfig::default().with_min_correspondences(3);
let icp = PointToLineIcp::with_config(config);
let mut icp = PointToLineIcp::with_config(config);
let result = icp.match_scan(&points, &lines, Pose2D::identity());
// Should handle gracefully (may or may not converge, but shouldn't crash)
@@ -977,7 +1010,7 @@ mod tests {
.with_convergence_threshold(1e-10) // Very strict to prevent early convergence
.with_min_correspondences(3);
let icp = PointToLineIcp::with_config(config);
let mut icp = PointToLineIcp::with_config(config);
let result = icp.match_scan(&points, &lines, Pose2D::identity());
// Should hit max iterations (may not converge)
vastu-map/src/matching/scratch.rs
+190
View File
@@ -57,6 +57,16 @@ pub struct IcpScratchSpace {
/// Buffer for ranges (distances from sensor).
pub(crate) ranges: Vec<f32>,
// =========================================================================
// Multi-resolution / Coarse search buffers
// =========================================================================
/// Buffer for subsampled point X coordinates.
/// Used by multi-resolution ICP and coarse search to avoid allocations.
pub(crate) subsampled_xs: Vec<f32>,
/// Buffer for subsampled point Y coordinates.
pub(crate) subsampled_ys: Vec<f32>,
}
impl IcpScratchSpace {
@@ -74,6 +84,8 @@ impl IcpScratchSpace {
correspondences: CorrespondenceSet::with_capacity(max_points),
correspondences_soa: CorrespondenceSoA::with_capacity(max_points),
ranges: Vec::with_capacity(max_points),
subsampled_xs: Vec::with_capacity(max_points),
subsampled_ys: Vec::with_capacity(max_points),
}
}
@@ -92,6 +104,80 @@ impl IcpScratchSpace {
self.correspondences.clear();
self.correspondences_soa.clear();
self.ranges.clear();
self.subsampled_xs.clear();
self.subsampled_ys.clear();
}
// =========================================================================
// Subsampling methods for multi-resolution ICP
// =========================================================================
/// Subsample points into internal buffer using step_by.
///
/// This is the zero-allocation version of subsampling for multi-resolution ICP.
/// After calling this, use `subsampled_point()` or `num_subsampled()` to access.
///
/// # Arguments
/// * `points` - Source points to subsample
/// * `step` - Step size (1 = no subsampling, 2 = every other point, etc.)
#[inline]
pub fn subsample_points(&mut self, points: &[Point2D], step: usize) {
self.subsampled_xs.clear();
self.subsampled_ys.clear();
let step = step.max(1);
let expected_len = points.len().div_ceil(step);
self.subsampled_xs.reserve(expected_len);
self.subsampled_ys.reserve(expected_len);
for p in points.iter().step_by(step) {
self.subsampled_xs.push(p.x);
self.subsampled_ys.push(p.y);
}
}
/// Subsample from SoA arrays into internal buffer.
///
/// Useful when points are already in SoA format (e.g., from PointCloud2D).
#[inline]
pub fn subsample_from_soa(&mut self, xs: &[f32], ys: &[f32], step: usize) {
self.subsampled_xs.clear();
self.subsampled_ys.clear();
let step = step.max(1);
let len = xs.len().min(ys.len());
let expected_len = len.div_ceil(step);
self.subsampled_xs.reserve(expected_len);
self.subsampled_ys.reserve(expected_len);
for i in (0..len).step_by(step) {
self.subsampled_xs.push(xs[i]);
self.subsampled_ys.push(ys[i]);
}
}
/// Get a subsampled point by index.
#[inline]
pub fn subsampled_point(&self, index: usize) -> Point2D {
Point2D::new(self.subsampled_xs[index], self.subsampled_ys[index])
}
/// Get number of subsampled points.
#[inline]
pub fn num_subsampled(&self) -> usize {
self.subsampled_xs.len()
}
/// Get slice of subsampled X coordinates.
#[inline]
pub fn subsampled_xs(&self) -> &[f32] {
&self.subsampled_xs
}
/// Get slice of subsampled Y coordinates.
#[inline]
pub fn subsampled_ys(&self) -> &[f32] {
&self.subsampled_ys
}
/// Transform points by pose into internal buffer.
@@ -525,4 +611,108 @@ mod tests {
scratch.find_correspondences(&lines, &config);
assert_eq!(scratch.num_correspondences(), 0);
}
// =========================================================================
// Subsample tests
// =========================================================================
#[test]
fn test_subsample_no_subsampling() {
let mut scratch = IcpScratchSpace::new(100, 10);
let points: Vec<Point2D> = (0..10).map(|i| Point2D::new(i as f32, 0.0)).collect();
scratch.subsample_points(&points, 1);
assert_eq!(scratch.num_subsampled(), 10);
for i in 0..10 {
let p = scratch.subsampled_point(i);
assert!((p.x - i as f32).abs() < 1e-6);
assert!((p.y - 0.0).abs() < 1e-6);
}
}
#[test]
fn test_subsample_every_other() {
let mut scratch = IcpScratchSpace::new(100, 10);
let points: Vec<Point2D> = (0..10).map(|i| Point2D::new(i as f32, 0.0)).collect();
scratch.subsample_points(&points, 2);
assert_eq!(scratch.num_subsampled(), 5);
// Should have 0, 2, 4, 6, 8
assert!((scratch.subsampled_point(0).x - 0.0).abs() < 1e-6);
assert!((scratch.subsampled_point(1).x - 2.0).abs() < 1e-6);
assert!((scratch.subsampled_point(2).x - 4.0).abs() < 1e-6);
assert!((scratch.subsampled_point(3).x - 6.0).abs() < 1e-6);
assert!((scratch.subsampled_point(4).x - 8.0).abs() < 1e-6);
}
#[test]
fn test_subsample_step_3() {
let mut scratch = IcpScratchSpace::new(100, 10);
let points: Vec<Point2D> = (0..12).map(|i| Point2D::new(i as f32, 0.0)).collect();
scratch.subsample_points(&points, 3);
assert_eq!(scratch.num_subsampled(), 4);
// Should have 0, 3, 6, 9
assert!((scratch.subsampled_point(0).x - 0.0).abs() < 1e-6);
assert!((scratch.subsampled_point(1).x - 3.0).abs() < 1e-6);
assert!((scratch.subsampled_point(2).x - 6.0).abs() < 1e-6);
assert!((scratch.subsampled_point(3).x - 9.0).abs() < 1e-6);
}
#[test]
fn test_subsample_soa() {
let mut scratch = IcpScratchSpace::new(100, 10);
let xs: Vec<f32> = (0..10).map(|i| i as f32).collect();
let ys: Vec<f32> = (0..10).map(|i| (i as f32) * 2.0).collect();
scratch.subsample_from_soa(&xs, &ys, 2);
assert_eq!(scratch.num_subsampled(), 5);
assert!((scratch.subsampled_xs()[0] - 0.0).abs() < 1e-6);
assert!((scratch.subsampled_xs()[1] - 2.0).abs() < 1e-6);
assert!((scratch.subsampled_ys()[0] - 0.0).abs() < 1e-6);
assert!((scratch.subsampled_ys()[1] - 4.0).abs() < 1e-6);
}
#[test]
fn test_subsample_reuse() {
let mut scratch = IcpScratchSpace::new(100, 10);
// First subsample
let points1: Vec<Point2D> = (0..10).map(|i| Point2D::new(i as f32, 0.0)).collect();
scratch.subsample_points(&points1, 2);
assert_eq!(scratch.num_subsampled(), 5);
// Second subsample (different data, reusing scratch)
let points2: Vec<Point2D> = (0..6).map(|i| Point2D::new(i as f32 * 10.0, 0.0)).collect();
scratch.subsample_points(&points2, 3);
assert_eq!(scratch.num_subsampled(), 2);
// Should have 0.0, 30.0
assert!((scratch.subsampled_point(0).x - 0.0).abs() < 1e-6);
assert!((scratch.subsampled_point(1).x - 30.0).abs() < 1e-6);
}
#[test]
fn test_subsample_empty() {
let mut scratch = IcpScratchSpace::new(100, 10);
let points: Vec<Point2D> = vec![];
scratch.subsample_points(&points, 2);
assert_eq!(scratch.num_subsampled(), 0);
}
#[test]
fn test_subsample_step_zero_treated_as_one() {
let mut scratch = IcpScratchSpace::new(100, 10);
let points: Vec<Point2D> = (0..5).map(|i| Point2D::new(i as f32, 0.0)).collect();
// Step of 0 should be treated as 1 (no subsampling)
scratch.subsample_points(&points, 0);
assert_eq!(scratch.num_subsampled(), 5);
}
}
vastu-map/src/matching/traits.rs
+15 -11
View File
@@ -20,7 +20,7 @@ use super::correspondence::MatchResult;
/// use vastu_map::core::{Point2D, Pose2D};
/// use vastu_map::features::Line2D;
///
/// let matcher = PointToLineIcp::with_config(IcpConfig::default());
/// let mut matcher = PointToLineIcp::with_config(IcpConfig::default());
/// let points = vec![Point2D::new(1.0, 0.1)];
/// let lines = vec![Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(5.0, 0.0))];
///
@@ -36,14 +36,18 @@ pub trait ScanMatcher: Send + Sync {
///
/// # Returns
/// Match result with optimized pose, covariance, and confidence.
fn match_scan(&self, points: &[Point2D], lines: &[Line2D], initial_pose: Pose2D)
-> MatchResult;
fn match_scan(
&mut self,
points: &[Point2D],
lines: &[Line2D],
initial_pose: Pose2D,
) -> MatchResult;
/// Match scan against a LineCollection (SoA format for SIMD).
///
/// Default implementation converts to Line2D slice and calls `match_scan`.
fn match_scan_collection(
&self,
&mut self,
points: &[Point2D],
lines: &LineCollection,
initial_pose: Pose2D,
@@ -59,7 +63,7 @@ pub trait ScanMatcher: Send + Sync {
impl ScanMatcher for super::PointToLineIcp {
fn match_scan(
&self,
&mut self,
points: &[Point2D],
lines: &[Line2D],
initial_pose: Pose2D,
@@ -69,7 +73,7 @@ impl ScanMatcher for super::PointToLineIcp {
}
fn match_scan_collection(
&self,
&mut self,
points: &[Point2D],
lines: &LineCollection,
initial_pose: Pose2D,
@@ -109,7 +113,7 @@ mod tests {
#[test]
fn test_scan_matcher_trait() {
let matcher = super::super::PointToLineIcp::with_config(IcpConfig::default());
let mut matcher = super::super::PointToLineIcp::with_config(IcpConfig::default());
let lines = make_room_lines();
let points = make_scan_points();
@@ -121,9 +125,9 @@ mod tests {
#[test]
fn test_trait_object() {
// Verify trait can be used as a trait object
let matcher: Box<dyn ScanMatcher> = Box::new(super::super::PointToLineIcp::with_config(
IcpConfig::default(),
));
let mut matcher: Box<dyn ScanMatcher> = Box::new(
super::super::PointToLineIcp::with_config(IcpConfig::default()),
);
let lines = make_room_lines();
let points = make_scan_points();
@@ -134,7 +138,7 @@ mod tests {
#[test]
fn test_collection_method() {
let matcher = super::super::PointToLineIcp::with_config(IcpConfig::default());
let mut matcher = super::super::PointToLineIcp::with_config(IcpConfig::default());
let lines = make_room_lines();
let collection = LineCollection::from_lines(&lines);
let points = make_scan_points();
vastu-map/src/query/frontier.rs
+365
View File
@@ -3,6 +3,24 @@
//! Frontiers are boundaries between known and unknown space.
//! In a VectorMap, frontiers are typically unconnected line endpoints
//! that indicate areas that haven't been fully explored.
//!
//! # Zero-Allocation Detection
//!
//! For repeated frontier detection (e.g., during SLAM), use `FrontierDetector`
//! which maintains persistent buffers to avoid per-call allocations:
//!
//! ```rust,ignore
//! use vastu_map::query::frontier::{FrontierDetector, FrontierConfig};
//!
//! let mut detector = FrontierDetector::new();
//! let config = FrontierConfig::default();
//!
//! // First detection - allocates buffers
//! let frontiers = detector.detect(&lines, &config);
//!
//! // Subsequent calls reuse buffers (zero allocation)
//! let frontiers = detector.detect(&new_lines, &config);
//! ```
use std::collections::HashMap;
@@ -14,6 +32,230 @@ use crate::features::{Line2D, LineCollection};
/// Maps (cell_x, cell_y) -> Vec<(point, line_index, is_start_endpoint)>
type EndpointGrid = HashMap<(i32, i32), Vec<(Point2D, usize, bool)>>;
/// Persistent frontier detector with reusable buffers.
///
/// Avoids per-call allocations by maintaining:
/// - Spatial hash grid with pre-allocated cell vectors
/// - Output frontier buffer
/// - Ranking distance buffer
///
/// # Example
///
/// ```rust,ignore
/// use vastu_map::query::frontier::{FrontierDetector, FrontierConfig};
///
/// let mut detector = FrontierDetector::new();
/// let config = FrontierConfig::default();
///
/// // Reuses internal buffers across calls
/// let frontiers = detector.detect(&lines, &config);
/// let ranked = detector.rank(robot_position, &lines);
/// ```
#[derive(Clone, Debug, Default)]
pub struct FrontierDetector {
/// Reusable spatial hash grid.
/// Cell vectors retain capacity across calls.
endpoint_grid: EndpointGrid,
/// Reusable output buffer for detected frontiers.
frontiers: Vec<Frontier>,
/// Reusable buffer for ranking frontiers by value.
ranked_frontiers: Vec<(Frontier, f32)>,
/// Reusable distance buffer for openness computation.
distance_buffer: Vec<f32>,
/// Cells that were used in the last detection (for efficient clearing).
used_cells: Vec<(i32, i32)>,
}
impl FrontierDetector {
/// Create a new frontier detector with default capacity.
pub fn new() -> Self {
Self::with_capacity(100, 50)
}
/// Create a frontier detector with capacity hints.
///
/// # Arguments
/// * `expected_lines` - Expected number of lines per detection
/// * `expected_frontiers` - Expected number of frontiers per detection
pub fn with_capacity(expected_lines: usize, expected_frontiers: usize) -> Self {
Self {
endpoint_grid: HashMap::with_capacity(expected_lines * 2),
frontiers: Vec::with_capacity(expected_frontiers),
ranked_frontiers: Vec::with_capacity(expected_frontiers),
distance_buffer: Vec::with_capacity(expected_lines),
used_cells: Vec::with_capacity(expected_lines * 2),
}
}
/// Clear grid by clearing each used cell's vector (retains capacity).
#[inline]
fn clear_grid(&mut self) {
for cell in &self.used_cells {
if let Some(vec) = self.endpoint_grid.get_mut(cell) {
vec.clear();
}
}
self.used_cells.clear();
}
/// Insert an endpoint into the grid.
#[inline]
fn insert_endpoint(&mut self, point: Point2D, line_idx: usize, is_start: bool, cell_size: f32) {
let cell = point_to_cell(point, cell_size);
// Track this cell for efficient clearing
if let std::collections::hash_map::Entry::Vacant(e) = self.endpoint_grid.entry(cell) {
e.insert(Vec::with_capacity(4));
self.used_cells.push(cell);
} else if self.endpoint_grid.get(&cell).is_none_or(|v| v.is_empty()) {
// Cell exists but was cleared - track it again
self.used_cells.push(cell);
}
self.endpoint_grid
.get_mut(&cell)
.unwrap()
.push((point, line_idx, is_start));
}
/// Detect frontiers from map lines.
///
/// Returns a slice of detected frontiers. The slice is valid until
/// the next call to `detect` or `detect_from_robot`.
///
/// # Arguments
/// * `lines` - Map lines
/// * `config` - Frontier detection configuration
pub fn detect(&mut self, lines: &[Line2D], config: &FrontierConfig) -> &[Frontier] {
self.frontiers.clear();
if lines.is_empty() {
return &self.frontiers;
}
let cell_size = config.connection_distance;
// Clear and rebuild grid
self.clear_grid();
for (idx, line) in lines.iter().enumerate() {
for (point, is_start) in [(line.start, true), (line.end, false)] {
self.insert_endpoint(point, idx, is_start, cell_size);
}
}
// Find unconnected endpoints
for (idx, line) in lines.iter().enumerate() {
for (point, is_start) in [(line.start, true), (line.end, false)] {
let cell = point_to_cell(point, cell_size);
if !is_connected_in_grid(
&self.endpoint_grid,
cell,
point,
idx,
config.connection_distance,
) {
self.frontiers.push(Frontier {
point,
line_idx: idx,
is_start,
});
}
}
}
&self.frontiers
}
/// Detect frontiers filtered by distance from robot.
///
/// Returns a slice of detected frontiers. The slice is valid until
/// the next call to `detect` or `detect_from_robot`.
pub fn detect_from_robot(
&mut self,
lines: &[Line2D],
robot_position: Point2D,
config: &FrontierConfig,
) -> &[Frontier] {
// First detect all frontiers
self.detect(lines, config);
// Filter in place by distance
self.frontiers
.retain(|f| f.point.distance(robot_position) >= config.min_distance_from_robot);
&self.frontiers
}
/// Rank the currently detected frontiers by exploration value.
///
/// Must be called after `detect` or `detect_from_robot`.
/// Returns a slice of (frontier, value) pairs sorted by value (best first).
pub fn rank(&mut self, robot_position: Point2D, lines: &[Line2D]) -> &[(Frontier, f32)] {
self.ranked_frontiers.clear();
if lines.is_empty() {
for f in &self.frontiers {
self.ranked_frontiers.push((f.clone(), 0.0));
}
return &self.ranked_frontiers;
}
// Create LineCollection for SIMD-optimized distance computation
let line_collection = LineCollection::from_lines(lines);
for f in &self.frontiers {
let distance = f.point.distance(robot_position);
let openness =
compute_openness_simd(f, lines, &line_collection, &mut self.distance_buffer);
let distance_factor = 1.0 / (1.0 + distance * 0.1);
let value = distance_factor * 0.3 + openness * 0.7;
self.ranked_frontiers.push((f.clone(), value));
}
// Sort by value (descending)
self.ranked_frontiers
.sort_by(|a, b| b.1.partial_cmp(&a.1).unwrap_or(std::cmp::Ordering::Equal));
&self.ranked_frontiers
}
/// Get the best frontier for exploration.
///
/// Convenience method that detects, filters, and ranks frontiers.
pub fn get_best(
&mut self,
lines: &[Line2D],
robot_position: Point2D,
config: &FrontierConfig,
) -> Option<Frontier> {
self.detect_from_robot(lines, robot_position, config);
if self.frontiers.is_empty() {
return None;
}
self.rank(robot_position, lines);
self.ranked_frontiers.first().map(|(f, _)| f.clone())
}
/// Get the currently detected frontiers (from last detect call).
pub fn frontiers(&self) -> &[Frontier] {
&self.frontiers
}
/// Get the currently ranked frontiers (from last rank call).
pub fn ranked_frontiers(&self) -> &[(Frontier, f32)] {
&self.ranked_frontiers
}
}
/// Configuration for frontier detection.
#[derive(Clone, Debug)]
pub struct FrontierConfig {
@@ -602,4 +844,127 @@ mod tests {
// End of single line should have high openness
assert!(openness > 0.0);
}
// ─────────────────────────────────────────────────────────────────────────
// FrontierDetector tests
// ─────────────────────────────────────────────────────────────────────────
#[test]
fn test_detector_new() {
let detector = FrontierDetector::new();
assert!(detector.frontiers().is_empty());
assert!(detector.ranked_frontiers().is_empty());
}
#[test]
fn test_detector_detect_single_line() {
let mut detector = FrontierDetector::new();
let lines = vec![Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(5.0, 0.0))];
let config = FrontierConfig::default();
let frontiers = detector.detect(&lines, &config);
// Both endpoints are unconnected
assert_eq!(frontiers.len(), 2);
}
#[test]
fn test_detector_detect_complete_room() {
let mut detector = FrontierDetector::new();
let lines = make_complete_room();
let config = FrontierConfig::default();
let frontiers = detector.detect(&lines, &config);
// Complete room - all corners connected
assert_eq!(frontiers.len(), 0);
}
#[test]
fn test_detector_detect_from_robot() {
let mut detector = FrontierDetector::new();
let lines = vec![Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(5.0, 0.0))];
let robot = Point2D::new(0.0, 0.0);
let config = FrontierConfig::default().with_min_distance_from_robot(1.0);
let frontiers = detector.detect_from_robot(&lines, robot, &config);
// Start endpoint at robot filtered, end endpoint included
assert_eq!(frontiers.len(), 1);
assert!(!frontiers[0].is_start);
}
#[test]
fn test_detector_rank() {
let mut detector = FrontierDetector::new();
let lines = vec![
Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(5.0, 0.0)),
Line2D::new(Point2D::new(0.0, 10.0), Point2D::new(5.0, 10.0)),
];
let robot = Point2D::new(0.0, 0.0);
let config = FrontierConfig::default();
detector.detect(&lines, &config);
let ranked = detector.rank(robot, &lines);
assert!(!ranked.is_empty());
// Values should be sorted descending
for i in 1..ranked.len() {
assert!(ranked[i - 1].1 >= ranked[i].1);
}
}
#[test]
fn test_detector_get_best() {
let mut detector = FrontierDetector::new();
let lines = make_partial_room();
let robot = Point2D::zero();
let config = FrontierConfig::default();
let best = detector.get_best(&lines, robot, &config);
assert!(best.is_some());
}
#[test]
fn test_detector_reuse_buffers() {
let mut detector = FrontierDetector::new();
let config = FrontierConfig::default();
// First detection
let lines1 = vec![Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(5.0, 0.0))];
let frontiers1 = detector.detect(&lines1, &config);
assert_eq!(frontiers1.len(), 2);
// Second detection with different lines - should reuse buffers
let lines2 = make_complete_room();
let frontiers2 = detector.detect(&lines2, &config);
assert_eq!(frontiers2.len(), 0);
// Third detection - back to single line
let frontiers3 = detector.detect(&lines1, &config);
assert_eq!(frontiers3.len(), 2);
}
#[test]
fn test_detector_equivalence_to_standalone() {
let mut detector = FrontierDetector::new();
let lines = make_partial_room();
let config = FrontierConfig::default();
// Compare detector results to standalone function results
let detector_frontiers = detector.detect(&lines, &config);
let standalone_frontiers = detect_frontiers(&lines, &config);
assert_eq!(detector_frontiers.len(), standalone_frontiers.len());
// Check same frontiers detected (may be in different order)
for f in standalone_frontiers.iter() {
assert!(
detector_frontiers
.iter()
.any(|df| { df.line_idx == f.line_idx && df.is_start == f.is_start })
);
}
}
}
vastu-map/src/query/mod.rs
+1 -1
View File
@@ -29,7 +29,7 @@ pub mod raycast;
// Re-export main types and functions
pub use frontier::{
FrontierConfig, cluster_centroid, cluster_frontiers, detect_frontiers,
FrontierConfig, FrontierDetector, cluster_centroid, cluster_frontiers, detect_frontiers,
detect_frontiers_from_robot, get_best_frontier, rank_frontiers,
};
pub use occupancy::{
vastu-map/src/vector_map/map.rs
+129 -32
View File
@@ -16,6 +16,94 @@ use crate::{Frontier, Map, ObserveResult, Occupancy, Path, TimingBreakdown};
use super::config::VectorMapConfig;
use super::line_store::LineStore;
// ============================================================================
// Scratch Space for Zero-Allocation Observe Pipeline
// ============================================================================
/// Pre-allocated scratch space for the observe pipeline.
///
/// Eliminates per-observation allocations by reusing buffers across calls.
/// This is critical for real-time SLAM at high observation rates (5-110 Hz).
///
/// # Performance Impact
///
/// Without scratch space, each `observe()` call allocates:
/// - `robot_frame_points`: O(n) points for ICP (done up to 3x per observation!)
/// - `lines_robot`: O(m) lines from extraction
/// - `lines_world`: O(m) lines after transform
/// - `scan_lines`: O(m) lines for integration
///
/// With scratch space, these allocations happen once and are reused.
#[derive(Clone, Debug, Default)]
pub struct ObserveScratchSpace {
/// Buffer for robot-frame scan points.
/// Reused for ICP matching and loop closure.
robot_frame_points: Vec<Point2D>,
/// Buffer for extracted lines in robot frame.
lines_robot: Vec<Line2D>,
/// Buffer for lines transformed to world frame.
lines_world: Vec<Line2D>,
}
impl ObserveScratchSpace {
/// Create new scratch space with capacity hints.
///
/// # Arguments
/// * `max_points` - Expected maximum scan points (e.g., 360 for typical lidar)
/// * `max_lines` - Expected maximum lines per scan (e.g., 20-50)
pub fn with_capacity(max_points: usize, max_lines: usize) -> Self {
Self {
robot_frame_points: Vec::with_capacity(max_points),
lines_robot: Vec::with_capacity(max_lines),
lines_world: Vec::with_capacity(max_lines),
}
}
/// Create with default capacity (360 points, 50 lines).
pub fn default_capacity() -> Self {
Self::with_capacity(360, 50)
}
/// Fill robot_frame_points from a PointCloud2D.
#[inline]
pub fn fill_robot_frame_points(&mut self, scan: &PointCloud2D) {
self.robot_frame_points.clear();
self.robot_frame_points.reserve(scan.len());
self.robot_frame_points.extend(scan.iter());
}
/// Get robot frame points slice.
#[inline]
pub fn robot_frame_points(&self) -> &[Point2D] {
&self.robot_frame_points
}
/// Fill lines_robot from extracted lines.
#[inline]
pub fn fill_lines_robot(&mut self, lines: &[Line2D]) {
self.lines_robot.clear();
self.lines_robot.extend_from_slice(lines);
}
/// Transform robot-frame lines to world frame and store in lines_world.
#[inline]
pub fn transform_lines_to_world(&mut self, pose: &Pose2D) {
self.lines_world.clear();
self.lines_world.reserve(self.lines_robot.len());
for line in &self.lines_robot {
self.lines_world.push(line.transform(pose));
}
}
/// Get world-frame lines slice.
#[inline]
pub fn lines_world(&self) -> &[Line2D] {
&self.lines_world
}
}
/// VectorMap: Feature-based 2D SLAM map.
///
/// Stores the environment as a collection of line and corner features.
@@ -65,6 +153,9 @@ pub struct VectorMap {
/// Persistent ICP matcher (preserves state across scans for adaptive coarse search).
icp_matcher: PointToLineIcp,
/// Pre-allocated scratch space for observe pipeline (zero-allocation).
observe_scratch: ObserveScratchSpace,
}
impl VectorMap {
@@ -81,6 +172,7 @@ impl VectorMap {
loop_detector,
loop_closures: Vec::new(),
icp_matcher,
observe_scratch: ObserveScratchSpace::default_capacity(),
}
}
@@ -172,14 +264,15 @@ impl VectorMap {
// Apply odometry to get predicted pose
let predicted_pose = self.current_pose.compose(odometry);
// Get robot-frame scan points for weighted ICP matching
let robot_frame_points: Vec<Point2D> = scan.iter().collect();
// Get robot-frame scan points for weighted ICP matching (reuse scratch buffer)
self.observe_scratch.fill_robot_frame_points(scan);
// ─── ICP Matching ───
let icp_start = Instant::now();
let match_result = self.icp_matcher.match_scan_robot_frame(
&robot_frame_points,
self.line_store.lines(),
// Use cached LineCollection from LineStore (avoids rebuilding SoA structure)
let match_result = self.icp_matcher.match_scan_robot_frame_collection(
self.observe_scratch.robot_frame_points(),
self.line_store.collection(),
predicted_pose,
);
timing.icp_matching_us = icp_start.elapsed().as_micros() as u64;
@@ -242,11 +335,12 @@ impl VectorMap {
self.current_pose = final_pose;
// ─── Feature Extraction ───
let features = self.extract_features_timed(scan, &final_pose, &mut timing);
// (robot_frame_points already filled in scratch above)
let features = self.extract_features_timed(&final_pose, &mut timing);
// ─── Feature Integration ───
let (features_added, features_merged) = if self.config.mapping_enabled {
self.integrate_features_timed(&features, &mut timing)
self.integrate_features_timed(&mut timing)
} else {
(0, 0)
};
@@ -254,7 +348,7 @@ impl VectorMap {
// ─── Loop Closure ───
let loop_closure_detected = if self.config.loop_closure_enabled {
let lc_start = Instant::now();
let robot_points: Vec<Point2D> = scan.iter().collect();
// Reuse robot_frame_points from scratch (already filled for ICP)
let feature_lines = features.lines();
let feature_corners = features.corners();
@@ -262,7 +356,7 @@ impl VectorMap {
final_pose,
&feature_lines,
&feature_corners,
&robot_points,
self.observe_scratch.robot_frame_points(),
) {
self.loop_closures.push(loop_closure);
true
@@ -297,13 +391,15 @@ impl VectorMap {
self.current_pose = odometry;
// Fill scratch with robot frame points (used by feature extraction and loop closure)
self.observe_scratch.fill_robot_frame_points(scan);
// Extract features from robot-frame scan, transform to world using odometry
let features = self.extract_features_timed(scan, &odometry, &mut timing);
let features = self.extract_features_timed(&odometry, &mut timing);
// Add all features to map
let num_lines = features.lines().len();
let feature_lines: Vec<Line2D> = features.lines().to_vec();
self.add_lines(&feature_lines);
self.add_lines(&features.lines());
for corner in features.corners() {
self.corners.push(corner);
@@ -312,14 +408,13 @@ impl VectorMap {
// Initialize first keyframe if loop closure is enabled
if self.config.loop_closure_enabled {
let lc_start = Instant::now();
let robot_points: Vec<Point2D> = scan.iter().collect();
let feature_lines = features.lines();
let feature_corners = features.corners();
self.loop_detector.force_keyframe(
odometry,
&feature_lines,
&feature_corners,
&robot_points,
self.observe_scratch.robot_frame_points(),
);
timing.loop_closure_us = lc_start.elapsed().as_micros() as u64;
}
@@ -340,47 +435,49 @@ impl VectorMap {
}
/// Extract features from a scan with timing.
///
/// NOTE: Caller must fill observe_scratch.robot_frame_points before calling this.
fn extract_features_timed(
&self,
scan_robot_frame: &PointCloud2D,
&mut self,
pose: &Pose2D,
timing: &mut TimingBreakdown,
) -> FeatureSet {
let points: Vec<Point2D> = scan_robot_frame.iter().collect();
// Use robot_frame_points from scratch (already filled by caller)
let points = self.observe_scratch.robot_frame_points();
// ─── Line Extraction ───
let line_start = Instant::now();
let lines_robot = if self.config.use_hybrid_extraction {
// Hybrid: RANSAC for dominant lines, then split-merge for remaining
extract_lines_hybrid(
&points,
points,
&self.config.ransac_extraction,
&self.config.extraction,
)
} else {
// Default: split-merge (preserves angular ordering)
extract_lines(&points, &self.config.extraction)
extract_lines(points, &self.config.extraction)
};
// Transform lines to world frame
let lines_world: Vec<Line2D> = lines_robot.iter().map(|l| l.transform(pose)).collect();
// Transform lines to world frame using scratch buffer
self.observe_scratch.fill_lines_robot(&lines_robot);
self.observe_scratch.transform_lines_to_world(pose);
timing.line_extraction_us = line_start.elapsed().as_micros() as u64;
// ─── Corner Detection ───
let corner_start = Instant::now();
let corners = detect_corners(&lines_world, &self.config.corner);
let corners = detect_corners(self.observe_scratch.lines_world(), &self.config.corner);
timing.corner_detection_us = corner_start.elapsed().as_micros() as u64;
FeatureSet::from_features(&lines_world, &corners)
FeatureSet::from_features(self.observe_scratch.lines_world(), &corners)
}
/// Integrate new features into the map with timing.
fn integrate_features_timed(
&mut self,
features: &FeatureSet,
timing: &mut TimingBreakdown,
) -> (usize, usize) {
let scan_lines: Vec<Line2D> = features.lines().to_vec();
///
/// NOTE: Uses lines_world from observe_scratch (filled by extract_features_timed).
fn integrate_features_timed(&mut self, timing: &mut TimingBreakdown) -> (usize, usize) {
// Use lines from scratch (already in world frame from extract_features_timed)
let scan_lines = self.observe_scratch.lines_world();
if scan_lines.is_empty() {
return (0, 0);
@@ -389,7 +486,7 @@ impl VectorMap {
// ─── Association ───
let assoc_start = Instant::now();
let associations = find_associations(
&scan_lines,
scan_lines,
self.line_store.lines(),
Some(self.line_store.index()),
&self.config.association,
@@ -399,7 +496,7 @@ impl VectorMap {
// ─── Merging ───
let merge_start = Instant::now();
let merged = batch_merge(
&scan_lines,
scan_lines,
self.line_store.lines_mut(),
&associations,
&self.config.merger,
@@ -410,7 +507,7 @@ impl VectorMap {
// ─── New Features ───
let new_start = Instant::now();
let unmatched = find_unmatched_scan_lines(
&scan_lines,
scan_lines,
self.line_store.lines(),
Some(self.line_store.index()),
&self.config.association,