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

Remove dead code from vastu-map

Browse Source 
Delete unused modules and functions that are no longer referenced:
- preprocessing.rs: Entire module (outlier removal, downsampling)
- nearest_neighbor.rs: SIMD/batch/weighted/spatial variants
- gauss_newton.rs: optimize_pose_simd (SIMD variant)
- line_fitting.rs: LineFitConfig, _checked variants
- raycast.rs: Indexed/batch raycast variants
- Line2D: perpendicular_at, extended, project_point_onto_line
- occupancy.rs: Batch/indexed query variants
- Various unused helpers in bounds, math, corner modules

Reduces codebase by ~2600 lines while preserving all active functionality.
This commit is contained in:
codetiger
2025-12-23 23:09:51 +05:30
parent a78735dbd0
commit 9b2a11f3fa
16 changed files with 17 additions and 2619 deletions
Download Patch File Download Diff File Expand all files Collapse all files
vastu-map/src/core/bounds.rs
-31
View File
@@ -75,15 +75,6 @@ impl Bounds {
}
}
/// Create bounds from center and half-size.
#[inline]
pub fn from_center_half_size(center: Point2D, half_size: Point2D) -> Self {
Self {
min: center - half_size,
max: center + half_size,
}
}
/// Width of the bounding box (x extent).
#[inline]
pub fn width(&self) -> f32 {
@@ -111,18 +102,6 @@ impl Bounds {
)
}
/// Half-size of the bounding box.
#[inline]
pub fn half_size(&self) -> Point2D {
Point2D::new(self.width() * 0.5, self.height() * 0.5)
}
/// Area of the bounding box.
#[inline]
pub fn area(&self) -> f32 {
self.width() * self.height()
}
/// Check if a point is inside the bounding box.
#[inline]
pub fn contains(&self, point: Point2D) -> bool {
@@ -237,14 +216,6 @@ mod tests {
assert_eq!(bounds.height(), 0.0);
}
#[test]
fn test_from_center_half_size() {
let bounds = Bounds::from_center_half_size(Point2D::new(5.0, 5.0), Point2D::new(2.0, 3.0));
assert_eq!(bounds.min, Point2D::new(3.0, 2.0));
assert_eq!(bounds.max, Point2D::new(7.0, 8.0));
}
#[test]
fn test_dimensions() {
let bounds = Bounds::new(Point2D::new(1.0, 2.0), Point2D::new(5.0, 8.0));
@@ -253,8 +224,6 @@ mod tests {
assert_eq!(bounds.height(), 6.0);
assert_eq!(bounds.size(), Point2D::new(4.0, 6.0));
assert_eq!(bounds.center(), Point2D::new(3.0, 5.0));
assert_eq!(bounds.half_size(), Point2D::new(2.0, 3.0));
assert_eq!(bounds.area(), 24.0);
}
#[test]
vastu-map/src/core/math.rs
-94
View File
@@ -74,36 +74,6 @@ pub fn angles_approx_equal(a: f32, b: f32, tolerance: f32) -> bool {
angle_diff(a, b).abs() <= tolerance
}
/// Convert degrees to radians.
#[inline]
pub fn deg_to_rad(deg: f32) -> f32 {
deg * PI / 180.0
}
/// Convert radians to degrees.
#[inline]
pub fn rad_to_deg(rad: f32) -> f32 {
rad * 180.0 / PI
}
/// Clamp a value to a range.
#[inline]
pub fn clamp(value: f32, min: f32, max: f32) -> f32 {
value.max(min).min(max)
}
/// Linear interpolation between two values.
#[inline]
pub fn lerp(a: f32, b: f32, t: f32) -> f32 {
a + (b - a) * t
}
/// Square of a value. Useful for avoiding `pow(x, 2)`.
#[inline]
pub fn sq(x: f32) -> f32 {
x * x
}
// ─────────────────────────────────────────────────────────────────────────────
// Geometry Utilities (shared by line fitting, RANSAC, etc.)
// ─────────────────────────────────────────────────────────────────────────────
@@ -198,41 +168,6 @@ pub fn compute_covariance(points: &[Point2D], centroid: Point2D) -> Covariance2D
Covariance2D { cxx, cyy, cxy }
}
/// Compute weighted covariance matrix elements.
///
/// Points with higher weights contribute more to the covariance.
/// Useful for range-weighted lidar point fitting.
///
/// # Arguments
/// * `points` - Point set
/// * `weights` - Weight for each point (must have same length as points)
/// * `centroid` - Pre-computed weighted centroid
///
/// # Panics
/// Panics if `points.len() != weights.len()`.
#[inline]
pub fn compute_covariance_weighted(
points: &[Point2D],
weights: &[f32],
centroid: Point2D,
) -> Covariance2D {
debug_assert_eq!(points.len(), weights.len());
let mut cxx: f32 = 0.0;
let mut cyy: f32 = 0.0;
let mut cxy: f32 = 0.0;
for (p, &w) in points.iter().zip(weights.iter()) {
let dx = p.x - centroid.x;
let dy = p.y - centroid.y;
cxx += w * dx * dx;
cyy += w * dy * dy;
cxy += w * dx * dy;
}
Covariance2D { cxx, cyy, cxy }
}
/// Compute the principal direction from a 2x2 covariance matrix.
///
/// Returns the eigenvector corresponding to the larger eigenvalue,
@@ -323,35 +258,6 @@ mod tests {
assert!(!angles_approx_equal(0.0, PI, 0.1));
}
#[test]
fn test_deg_rad_conversion() {
assert_relative_eq!(deg_to_rad(180.0), PI, epsilon = 1e-6);
assert_relative_eq!(deg_to_rad(90.0), PI / 2.0, epsilon = 1e-6);
assert_relative_eq!(rad_to_deg(PI), 180.0, epsilon = 1e-6);
assert_relative_eq!(rad_to_deg(PI / 2.0), 90.0, epsilon = 1e-6);
}
#[test]
fn test_clamp() {
assert_eq!(clamp(5.0, 0.0, 10.0), 5.0);
assert_eq!(clamp(-5.0, 0.0, 10.0), 0.0);
assert_eq!(clamp(15.0, 0.0, 10.0), 10.0);
}
#[test]
fn test_lerp() {
assert_eq!(lerp(0.0, 10.0, 0.0), 0.0);
assert_eq!(lerp(0.0, 10.0, 1.0), 10.0);
assert_eq!(lerp(0.0, 10.0, 0.5), 5.0);
}
#[test]
fn test_sq() {
assert_eq!(sq(2.0), 4.0);
assert_eq!(sq(-3.0), 9.0);
assert_eq!(sq(0.0), 0.0);
}
// ─────────────────────────────────────────────────────────────────────────
// Geometry Utility Tests
// ─────────────────────────────────────────────────────────────────────────
vastu-map/src/extraction/corner_detection.rs
+1 -26
View File
@@ -164,31 +164,6 @@ pub fn detect_corners(lines: &[Line2D], config: &CornerConfig) -> Vec<Corner2D>
corners
}
/// Detect corners from all pairs of lines (not just adjacent).
///
/// This is useful for finding corners in maps where lines may not be
/// in sequential order.
///
/// # Note
/// This has O(n²) complexity, so use sparingly for large line sets.
pub fn detect_all_corners(lines: &[Line2D], config: &CornerConfig) -> Vec<Corner2D> {
if lines.len() < 2 {
return Vec::new();
}
let mut corners = Vec::new();
for i in 0..lines.len() {
for j in i + 1..lines.len() {
if let Some(corner) = detect_corner_between(&lines[i], &lines[j], i, j, config) {
corners.push(corner);
}
}
}
corners
}
/// Detect corner between two specific lines.
fn detect_corner_between(
line1: &Line2D,
@@ -603,7 +578,7 @@ mod tests {
}
#[test]
fn test_detect_all_corners() {
fn test_detect_multiple_corners() {
// Three lines forming two corners
let lines = vec![
Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(1.0, 0.0)),
vastu-map/src/extraction/line_fitting.rs
-374
View File
@@ -18,87 +18,12 @@
//! let points = vec![/* lidar hits */];
//! let line = fit_line_from_sensor(&points, sensor_pos, None);
//! ```
//!
//! # Error Handling
//!
//! For explicit error handling, use the `_checked` variants that return `Result`:
//!
//! ```rust,ignore
//! use vastu_map::extraction::{fit_line_checked, FitError};
//!
//! match fit_line_checked(&points) {
//! Ok(line) => println!("Fitted line: {:?}", line),
//! Err(FitError::InsufficientPoints { got, required }) => {
//! println!("Need {} points, got {}", required, got);
//! }
//! Err(FitError::DegenerateGeometry) => {
//! println!("Points are collocated or form a circle");
//! }
//! }
//! ```
use crate::config::LidarNoiseModel;
use crate::core::Point2D;
use crate::core::math::{compute_centroid, compute_covariance, principal_direction};
use crate::features::Line2D;
// ============================================================================
// Configuration
// ============================================================================
/// Configuration for line fitting operations.
///
/// Controls how lines are fitted to point sets, including maximum line length
/// constraints that can improve robustness by creating smaller line segments
/// that are later merged by the integration layer.
///
/// # Example
/// ```
/// use vastu_map::extraction::LineFitConfig;
///
/// // Create config with 1m max line length
/// let config = LineFitConfig::new()
/// .with_max_line_length(1.0);
///
/// // Lines longer than 1m will be split into smaller segments
/// ```
#[derive(Debug, Clone, Default)]
pub struct LineFitConfig {
/// Maximum length for a single line segment.
///
/// If a fitted line exceeds this length, it will be split into multiple
/// smaller segments. This can improve robustness as shorter segments are
/// less prone to fitting errors from outliers.
///
/// Set to `None` (default) for no length limit.
pub max_line_length: Option<f32>,
}
impl LineFitConfig {
/// Create a new configuration with default settings.
pub fn new() -> Self {
Self::default()
}
/// Set the maximum line length.
///
/// Lines longer than this will be split into smaller segments.
/// Pass `None` to disable length limiting (default).
///
/// # Arguments
/// * `length` - Maximum length in meters, or None for no limit
pub fn with_max_line_length(mut self, length: f32) -> Self {
self.max_line_length = Some(length);
self
}
/// Set the maximum line length (optional variant).
pub fn with_max_line_length_opt(mut self, length: Option<f32>) -> Self {
self.max_line_length = length;
self
}
}
// ============================================================================
// Line Splitting
// ============================================================================
@@ -168,17 +93,6 @@ pub fn split_line_by_length(line: &Line2D, max_length: f32) -> Vec<Line2D> {
segments
}
/// Split multiple lines by maximum length.
///
/// Convenience function that applies `split_line_by_length` to all lines.
pub fn split_lines_by_length(lines: &[Line2D], max_length: f32) -> Vec<Line2D> {
let mut result = Vec::new();
for line in lines {
result.extend(split_line_by_length(line, max_length));
}
result
}
// ============================================================================
// Error Types
// ============================================================================
@@ -318,29 +232,6 @@ fn fit_line_impl(points: &[Point2D]) -> Result<Line2D, FitError> {
Ok(Line2D::with_point_count(start, end, points.len() as u32))
}
/// Fit a line to a set of points using Total Least Squares (with explicit errors).
///
/// This is the error-returning variant of [`fit_line`]. Use this when you need
/// to distinguish between different failure modes.
///
/// # Errors
///
/// Returns `Err(FitError::InsufficientPoints)` if fewer than 2 points provided.
/// Returns `Err(FitError::DegenerateGeometry)` if points are collocated or form a circle.
///
/// # Example
/// ```
/// use vastu_map::extraction::{fit_line_checked, FitError};
/// use vastu_map::core::Point2D;
///
/// let points = vec![Point2D::new(0.0, 0.0)]; // Only one point
/// let result = fit_line_checked(&points);
/// assert!(matches!(result, Err(FitError::InsufficientPoints { .. })));
/// ```
pub fn fit_line_checked(points: &[Point2D]) -> Result<Line2D, FitError> {
fit_line_impl(points)
}
/// Simple line fitting fallback (shared implementation).
fn fit_line_simple_impl(points: &[Point2D]) -> Result<Line2D, FitError> {
if points.len() < 2 {
@@ -526,119 +417,6 @@ pub fn fit_line_from_sensor(
fit_line_weighted(points, &weights)
}
// ============================================================================
// Config-based Line Fitting (with max length splitting)
// ============================================================================
/// Fit a line to points with configuration, potentially splitting into segments.
///
/// If `config.max_line_length` is set, lines longer than this threshold
/// will be split into smaller segments for improved robustness.
///
/// # Arguments
/// * `points` - Points to fit line to
/// * `config` - Configuration controlling fitting behavior
///
/// # Returns
/// Vector of fitted line segments. Returns empty if fitting fails.
///
/// # Example
/// ```
/// use vastu_map::extraction::{fit_line_with_config, LineFitConfig};
/// use vastu_map::core::Point2D;
///
/// let points: Vec<Point2D> = (0..50)
/// .map(|i| Point2D::new(i as f32 * 0.1, 0.0)) // 5m line
/// .collect();
///
/// let config = LineFitConfig::new().with_max_line_length(1.0);
/// let lines = fit_line_with_config(&points, &config);
///
/// // Line is split into ~5 segments of ~1m each
/// assert!(lines.len() >= 4);
/// ```
pub fn fit_line_with_config(points: &[Point2D], config: &LineFitConfig) -> Vec<Line2D> {
let Some(line) = fit_line(points) else {
return Vec::new();
};
match config.max_line_length {
Some(max_len) if max_len > 0.0 => split_line_by_length(&line, max_len),
_ => vec![line],
}
}
/// Fit a line to points with weighting and configuration.
///
/// Combines weighted fitting with max length splitting for robust line extraction.
///
/// # Arguments
/// * `points` - Points to fit line to
/// * `weights` - Weight for each point
/// * `config` - Configuration controlling fitting behavior
///
/// # Returns
/// Vector of fitted line segments. Returns empty if fitting fails.
pub fn fit_line_weighted_with_config(
points: &[Point2D],
weights: &[f32],
config: &LineFitConfig,
) -> Vec<Line2D> {
let Some(line) = fit_line_weighted(points, weights) else {
return Vec::new();
};
match config.max_line_length {
Some(max_len) if max_len > 0.0 => split_line_by_length(&line, max_len),
_ => vec![line],
}
}
/// Fit a line using sensor-based weighting with configuration.
///
/// Combines range-based weighting with max length splitting.
///
/// # Arguments
/// * `points` - Points to fit line to
/// * `sensor_pos` - Position of the sensor (typically robot position)
/// * `noise_model` - Lidar noise model parameters (uses default if None)
/// * `config` - Configuration controlling fitting behavior
///
/// # Returns
/// Vector of fitted line segments. Returns empty if fitting fails.
///
/// # Example
/// ```
/// use vastu_map::extraction::{fit_line_from_sensor_with_config, LineFitConfig};
/// use vastu_map::core::Point2D;
///
/// let sensor_pos = Point2D::new(0.0, 0.0);
/// let points: Vec<Point2D> = (1..30)
/// .map(|i| Point2D::new(i as f32 * 0.1, 1.0)) // ~3m wall
/// .collect();
///
/// let config = LineFitConfig::new().with_max_line_length(0.5);
/// let lines = fit_line_from_sensor_with_config(&points, sensor_pos, None, &config);
///
/// // Wall is split into ~6 segments of ~0.5m each
/// assert!(lines.len() >= 5);
/// ```
pub fn fit_line_from_sensor_with_config(
points: &[Point2D],
sensor_pos: Point2D,
noise_model: Option<&LidarNoiseModel>,
config: &LineFitConfig,
) -> Vec<Line2D> {
let Some(line) = fit_line_from_sensor(points, sensor_pos, noise_model) else {
return Vec::new();
};
match config.max_line_length {
Some(max_len) if max_len > 0.0 => split_line_by_length(&line, max_len),
_ => vec![line],
}
}
/// Compute weighted fitting error (RMS distance to line, weighted by measurement quality).
///
/// Returns the weighted root-mean-square of perpendicular distances.
@@ -703,29 +481,6 @@ pub fn max_distance_point(points: &[Point2D], line: &Line2D) -> Option<(usize, f
Some((max_idx, max_dist))
}
/// Fit a line segment to points with known start and end indices.
///
/// The endpoints are snapped to actual point positions for accurate
/// segment boundaries.
pub fn fit_line_segment(points: &[Point2D], start_idx: usize, end_idx: usize) -> Option<Line2D> {
if start_idx >= points.len() || end_idx >= points.len() || start_idx >= end_idx {
return None;
}
let segment_points = &points[start_idx..=end_idx];
let mut line = fit_line(segment_points)?;
// Snap endpoints to actual point projections
// This ensures the segment covers exactly the input points
let start_proj = line.project_point_onto_line(points[start_idx]);
let end_proj = line.project_point_onto_line(points[end_idx]);
line.start = start_proj;
line.end = end_proj;
Some(line)
}
#[cfg(test)]
mod tests {
use super::*;
@@ -863,23 +618,6 @@ mod tests {
assert_relative_eq!(dist, 3.0, epsilon = 1e-6);
}
#[test]
fn test_fit_line_segment() {
let points = vec![
Point2D::new(0.0, 0.0), // idx 0
Point2D::new(1.0, 0.0), // idx 1
Point2D::new(2.0, 0.0), // idx 2
Point2D::new(3.0, 0.0), // idx 3
Point2D::new(4.0, 0.0), // idx 4
];
let line = fit_line_segment(&points, 1, 3).unwrap();
// Should span from point 1 to point 3
assert_relative_eq!(line.start.x, 1.0, epsilon = 0.01);
assert_relative_eq!(line.end.x, 3.0, epsilon = 0.01);
}
// ========================================
// Weighted TLS Tests
// ========================================
@@ -1145,116 +883,4 @@ mod tests {
let segments_neg = split_line_by_length(&line, -1.0);
assert_eq!(segments_neg.len(), 1);
}
#[test]
fn test_split_lines_by_length() {
let lines = vec![
Line2D::with_point_count(Point2D::new(0.0, 0.0), Point2D::new(3.0, 0.0), 30),
Line2D::with_point_count(Point2D::new(0.0, 1.0), Point2D::new(0.5, 1.0), 5),
];
let segments = split_lines_by_length(&lines, 1.0);
// First line splits into 3, second line stays as 1
assert_eq!(segments.len(), 4);
}
// ========================================
// Config-based Fitting Tests
// ========================================
#[test]
fn test_line_fit_config_default() {
let config = LineFitConfig::default();
assert!(config.max_line_length.is_none());
}
#[test]
fn test_line_fit_config_builder() {
let config = LineFitConfig::new().with_max_line_length(1.5);
assert_eq!(config.max_line_length, Some(1.5));
}
#[test]
fn test_fit_line_with_config_no_split() {
let points: Vec<Point2D> = (0..10).map(|i| Point2D::new(i as f32 * 0.1, 0.0)).collect();
// Config with no max length
let config = LineFitConfig::new();
let lines = fit_line_with_config(&points, &config);
assert_eq!(lines.len(), 1);
}
#[test]
fn test_fit_line_with_config_with_split() {
// Create a 5m horizontal line
let points: Vec<Point2D> = (0..51).map(|i| Point2D::new(i as f32 * 0.1, 0.0)).collect();
// Config with 1m max length
let config = LineFitConfig::new().with_max_line_length(1.0);
let lines = fit_line_with_config(&points, &config);
// 5m line should split into 5 segments
assert_eq!(lines.len(), 5);
// Each segment should be approximately 1m
for line in &lines {
assert!(
line.length() <= 1.01,
"Line length {} > 1.01",
line.length()
);
}
}
#[test]
fn test_fit_line_with_config_empty_input() {
let points: Vec<Point2D> = vec![];
let config = LineFitConfig::new().with_max_line_length(1.0);
let lines = fit_line_with_config(&points, &config);
assert!(lines.is_empty());
}
#[test]
fn test_fit_line_from_sensor_with_config() {
let sensor_pos = Point2D::new(0.0, 0.0);
let points: Vec<Point2D> = (10..40)
.map(|i| Point2D::new(i as f32 * 0.1, 1.0)) // 3m wall at y=1
.collect();
let config = LineFitConfig::new().with_max_line_length(0.5);
let lines = fit_line_from_sensor_with_config(&points, sensor_pos, None, &config);
// 3m line should split into 6 segments of ~0.5m
assert!(
lines.len() >= 5,
"Expected >= 5 segments, got {}",
lines.len()
);
// All segments should be approximately horizontal
for line in &lines {
assert!(
line.angle().abs() < 0.1,
"Line angle {} is not horizontal",
line.angle()
);
}
}
#[test]
fn test_fit_line_weighted_with_config() {
let points: Vec<Point2D> = (0..31).map(|i| Point2D::new(i as f32 * 0.1, 0.0)).collect();
let weights: Vec<f32> = vec![1.0; 31];
let config = LineFitConfig::new().with_max_line_length(1.0);
let lines = fit_line_weighted_with_config(&points, &weights, &config);
// 3m line should split into 3 segments
assert_eq!(lines.len(), 3);
}
}
vastu-map/src/extraction/mod.rs
+4 -6
View File
@@ -48,14 +48,12 @@ pub mod split_merge;
pub mod traits;
pub use corner_detection::{
CornerConfig, deduplicate_corners, detect_all_corners, detect_corners,
detect_curvature_corners, detect_hybrid_corners,
CornerConfig, deduplicate_corners, detect_corners, detect_curvature_corners,
detect_hybrid_corners,
};
pub use line_fitting::{
FitError, LineFitConfig, compute_range_weights, fit_line, fit_line_checked,
fit_line_from_sensor, fit_line_from_sensor_with_config, fit_line_segment, fit_line_weighted,
fit_line_weighted_with_config, fit_line_with_config, fitting_error, fitting_error_weighted,
max_distance_point, split_line_by_length, split_lines_by_length,
FitError, compute_range_weights, fit_line, fit_line_from_sensor, fit_line_weighted,
fitting_error, fitting_error_weighted, max_distance_point, split_line_by_length,
};
// Re-export from ransac_lines module
pub use ransac_lines::{
vastu-map/src/features/corner.rs
-18
View File
@@ -79,16 +79,6 @@ impl Corner2D {
self.position.approx_eq(other.position, pos_epsilon)
&& (self.angle - other.angle).abs() <= angle_epsilon
}
/// Check if the corner angle is valid (between min and max thresholds).
///
/// # Arguments
/// * `min_angle` - Minimum valid angle in radians
/// * `max_angle` - Maximum valid angle in radians
#[inline]
pub fn is_valid_angle(&self, min_angle: f32, max_angle: f32) -> bool {
self.angle >= min_angle && self.angle <= max_angle
}
}
impl Default for Corner2D {
@@ -288,14 +278,6 @@ mod tests {
assert_eq!(c1.distance(&c2), 5.0);
}
#[test]
fn test_corner_is_valid_angle() {
let corner = Corner2D::new(Point2D::new(0.0, 0.0), 0, 1, FRAC_PI_2);
assert!(corner.is_valid_angle(FRAC_PI_4, std::f32::consts::PI - FRAC_PI_4));
assert!(!corner.is_valid_angle(FRAC_PI_2 + 0.1, std::f32::consts::PI));
}
#[test]
fn test_corner_collection_from_corners() {
let corners = vec![
vastu-map/src/features/feature_set.rs
+1 -33
View File
@@ -56,12 +56,6 @@ impl FeatureSet {
self.lines.push_line(&line);
}
/// Add a line from components.
#[inline]
pub fn add_line_from_points(&mut self, start: Point2D, end: Point2D) {
self.lines.push_full(start.x, start.y, end.x, end.y, 1, 0);
}
/// Number of lines.
#[inline]
pub fn line_count(&self) -> usize {
@@ -96,13 +90,6 @@ impl FeatureSet {
self.lines.iter()
}
/// Remove a line by index (swap-remove).
pub fn remove_line(&mut self, index: usize) {
self.lines.swap_remove(index);
// Note: This invalidates corner line indices!
// Caller should update or clear corners after removing lines.
}
// ─────────────────────────────────────────────────────────────────────────
// Corner operations
// ─────────────────────────────────────────────────────────────────────────
@@ -201,11 +188,6 @@ impl FeatureSet {
self.corners.clear();
}
/// Clear only corners (useful after modifying lines).
pub fn clear_corners(&mut self) {
self.corners.clear();
}
/// Find the nearest line to a point.
///
/// Returns (index, distance) or None if no lines.
@@ -214,20 +196,6 @@ impl FeatureSet {
self.lines.nearest_line(point)
}
/// Find the nearest corner to a point.
///
/// Returns (index, distance) or None if no corners.
#[inline]
pub fn nearest_corner(&self, point: Point2D) -> Option<(usize, f32)> {
self.corners.nearest_corner(point)
}
/// Compute distances from a point to all lines.
#[inline]
pub fn distances_to_lines(&self, point: Point2D) -> Vec<f32> {
self.lines.distances_to_point(point)
}
/// Merge another feature set into this one.
///
/// Note: This is a simple append. For intelligent merging with
@@ -261,7 +229,7 @@ mod tests {
fn test_add_line() {
let mut fs = FeatureSet::new();
fs.add_line(Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(1.0, 0.0)));
fs.add_line_from_points(Point2D::new(0.0, 1.0), Point2D::new(1.0, 1.0));
fs.add_line(Line2D::new(Point2D::new(0.0, 1.0), Point2D::new(1.0, 1.0)));
assert_eq!(fs.line_count(), 2);
assert!(!fs.is_empty());
vastu-map/src/features/line.rs
-113
View File
@@ -135,68 +135,6 @@ impl Line2D {
)
}
/// Get a perpendicular line at parameter t along this line.
///
/// Returns a unit-length line perpendicular to this line,
/// centered at `point_at(t)`.
///
/// # Arguments
/// * `t` - Parameter along line (0=start, 1=end, 0.5=midpoint)
/// * `length` - Length of the perpendicular line (centered on the point)
///
/// # Example
/// ```rust,ignore
/// let line = Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(4.0, 0.0));
/// let perp = line.perpendicular_at(0.5, 2.0);
/// // perp goes from (2, -1) to (2, 1) - perpendicular at midpoint
/// ```
#[inline]
pub fn perpendicular_at(&self, t: f32, length: f32) -> Line2D {
let center = self.point_at(t);
let normal = self.normal();
let half_len = length * 0.5;
Line2D::new(
Point2D::new(
center.x - normal.x * half_len,
center.y - normal.y * half_len,
),
Point2D::new(
center.x + normal.x * half_len,
center.y + normal.y * half_len,
),
)
}
/// Extend this line by a factor in both directions.
///
/// # Arguments
/// * `factor` - Extension factor (1.0 = no change, 2.0 = double length)
///
/// The line is extended symmetrically from its midpoint.
///
/// # Example
/// ```rust,ignore
/// let line = Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(2.0, 0.0));
/// let extended = line.extended(2.0);
/// // extended goes from (-1, 0) to (3, 0) - doubled in length
/// ```
#[inline]
pub fn extended(&self, factor: f32) -> Line2D {
let mid = self.midpoint();
let half_dir = Point2D::new(
(self.end.x - self.start.x) * 0.5 * factor,
(self.end.y - self.start.y) * 0.5 * factor,
);
Line2D::full(
Point2D::new(mid.x - half_dir.x, mid.y - half_dir.y),
Point2D::new(mid.x + half_dir.x, mid.y + half_dir.y),
self.observation_count,
self.point_count,
)
}
/// Angle of the line direction from X-axis (in radians, [-π, π]).
#[inline]
pub fn angle(&self) -> f32 {
@@ -263,13 +201,6 @@ impl Line2D {
to_point.dot(dir) / len_sq
}
/// Get the projected point on the infinite line.
#[inline]
pub fn project_point_onto_line(&self, point: Point2D) -> Point2D {
let t = self.project_point(point);
self.point_at(t)
}
/// Check if a projection parameter t falls within the segment [0, 1].
#[inline]
pub fn contains_projection(&self, t: f32) -> bool {
@@ -564,50 +495,6 @@ mod tests {
assert_eq!(line.point_at(0.5), Point2D::new(5.0, 0.0));
}
#[test]
fn test_perpendicular_at() {
// Horizontal line
let line = Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(4.0, 0.0));
// Perpendicular at midpoint with length 2
let perp = line.perpendicular_at(0.5, 2.0);
// Center should be at (2, 0)
assert_relative_eq!(perp.midpoint().x, 2.0, epsilon = 1e-6);
assert_relative_eq!(perp.midpoint().y, 0.0, epsilon = 1e-6);
// Should be vertical (length 2)
assert_relative_eq!(perp.length(), 2.0, epsilon = 1e-6);
// Endpoints should be at (2, -1) and (2, 1)
assert_relative_eq!(perp.start.x, 2.0, epsilon = 1e-6);
assert_relative_eq!(perp.end.x, 2.0, epsilon = 1e-6);
assert_relative_eq!((perp.start.y - perp.end.y).abs(), 2.0, epsilon = 1e-6);
}
#[test]
fn test_extended() {
let line = Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(2.0, 0.0));
// Double the length
let ext = line.extended(2.0);
// Midpoint should be same
assert_relative_eq!(ext.midpoint().x, 1.0, epsilon = 1e-6);
assert_relative_eq!(ext.midpoint().y, 0.0, epsilon = 1e-6);
// Length should be doubled
assert_relative_eq!(ext.length(), 4.0, epsilon = 1e-6);
// Endpoints should be at (-1, 0) and (3, 0)
assert_relative_eq!(ext.start.x, -1.0, epsilon = 1e-6);
assert_relative_eq!(ext.end.x, 3.0, epsilon = 1e-6);
// No change when factor is 1.0
let same = line.extended(1.0);
assert_relative_eq!(same.length(), 2.0, epsilon = 1e-6);
}
#[test]
fn test_distance_to_point_segment() {
let line = Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(10.0, 0.0));
vastu-map/src/lib.rs
-1
View File
@@ -120,7 +120,6 @@ pub mod loop_closure;
pub mod matching;
pub mod motion_model;
pub mod odometry;
pub mod preprocessing;
pub mod query;
pub mod vector_map;
vastu-map/src/matching/gauss_newton.rs
+1 -295
View File
@@ -17,12 +17,10 @@
//! Use `optimize_pose_fast` in hot paths for ~15-20% speedup by avoiding
//! redundant sqrt computations.
use std::simd::{f32x4, num::SimdFloat};
use crate::core::{Point2D, Pose2D};
use crate::features::{Line2D, LineCollection};
use super::correspondence::{CorrespondenceSet, CorrespondenceSoA};
use super::correspondence::CorrespondenceSet;
/// Configuration for Levenberg-Marquardt solver.
///
@@ -819,298 +817,6 @@ fn compute_residual_fast(
(sum_sq / correspondences.len() as f32).sqrt()
}
// ============================================================================
// SIMD-Optimized versions using CorrespondenceSoA
// ============================================================================
/// Optimize pose using SIMD with SoA layout for maximum performance.
///
/// This version uses `CorrespondenceSoA` with inlined line data, enabling
/// contiguous SIMD loads in the hot loop. This is the fastest variant for
/// production use.
///
/// # Performance
///
/// ~4x faster than `optimize_pose_fast` due to:
/// - No indexed access in hot loop (line data is inlined)
/// - Explicit f32x4 SIMD operations
/// - Cache-friendly contiguous memory access
///
/// # Example
///
/// ```rust,ignore
/// use vastu_map::matching::{optimize_pose_simd, GaussNewtonConfig, find_correspondences_soa};
///
/// let soa = find_correspondences_soa(&points, &ranges, &lines, &noise_model, &config);
/// let result = optimize_pose_simd(&soa, initial_pose, &GaussNewtonConfig::default());
/// ```
pub fn optimize_pose_simd(
correspondences: &CorrespondenceSoA,
initial_pose: Pose2D,
config: &GaussNewtonConfig,
) -> GaussNewtonResult {
if correspondences.is_empty() {
return GaussNewtonResult {
pose: initial_pose,
iterations: 0,
residual: 0.0,
converged: true,
covariance: IDENTITY_COVARIANCE,
condition_number: 1.0,
};
}
let mut pose = initial_pose;
let mut converged = false;
let mut iterations = 0;
let mut final_hessian = [[0.0f32; 3]; 3];
let mut final_sum_weighted_sq = 0.0f32;
// LM state
let use_lm = config.lm_initial_lambda > 0.0;
let mut lambda = config.lm_initial_lambda;
let mut prev_residual = if use_lm {
compute_residual_simd(correspondences, &pose)
} else {
f32::MAX
};
for iter in 0..config.max_iterations {
iterations = iter + 1;
// Build linear system using SIMD
let (h, g, sum_weighted_sq, _sum_weights) =
build_linear_system_simd(correspondences, &pose);
final_hessian = h;
final_sum_weighted_sq = sum_weighted_sq;
// Add LM damping to Hessian diagonal
let mut h_damped = h;
if use_lm {
h_damped[0][0] += lambda;
h_damped[1][1] += lambda;
h_damped[2][2] += lambda;
}
// Solve for Δx
let Some(delta) = solve_3x3(&h_damped, &g, config.regularization) else {
break;
};
// Compute trial pose
let trial_pose = Pose2D::new(
pose.x + delta[0],
pose.y + delta[1],
crate::core::math::normalize_angle(pose.theta + delta[2]),
);
if use_lm {
let trial_residual = compute_residual_simd(correspondences, &trial_pose);
if trial_residual < prev_residual {
// Good step
pose = trial_pose;
prev_residual = trial_residual;
lambda = (lambda / config.lm_lambda_factor).max(config.lm_min_lambda);
let delta_norm_sq = delta[0] * delta[0] + delta[1] * delta[1] + delta[2] * delta[2];
if delta_norm_sq < config.convergence_threshold_sq {
converged = true;
break;
}
} else {
// Bad step
lambda *= config.lm_lambda_factor;
if lambda > config.lm_max_lambda {
break;
}
continue;
}
} else {
pose = trial_pose;
let delta_norm_sq = delta[0] * delta[0] + delta[1] * delta[1] + delta[2] * delta[2];
if delta_norm_sq < config.convergence_threshold_sq {
converged = true;
break;
}
}
}
let final_residual = compute_residual_simd(correspondences, &pose);
let (covariance, condition_number) = compute_covariance_with_dof(
&final_hessian,
final_sum_weighted_sq,
correspondences.len(),
config.regularization,
);
GaussNewtonResult {
pose,
iterations,
residual: final_residual,
converged,
covariance,
condition_number,
}
}
/// Build linear system using SIMD with contiguous loads from CorrespondenceSoA.
///
/// This is the key optimization: instead of indexed access to line arrays,
/// all data is contiguous and can be loaded directly into SIMD registers.
#[inline]
fn build_linear_system_simd(
corr: &CorrespondenceSoA,
pose: &Pose2D,
) -> ([[f32; 3]; 3], [f32; 3], f32, f32) {
let (sin, cos) = pose.theta.sin_cos();
let neg_sin = -sin;
let neg_cos = -cos;
// SIMD constants
let cos4 = f32x4::splat(cos);
let sin4 = f32x4::splat(sin);
let neg_sin4 = f32x4::splat(neg_sin);
let neg_cos4 = f32x4::splat(neg_cos);
let pose_x4 = f32x4::splat(pose.x);
let pose_y4 = f32x4::splat(pose.y);
// Accumulators (scalar - reduce from SIMD at end of each chunk)
let mut h00 = 0.0f32;
let mut h01 = 0.0f32;
let mut h02 = 0.0f32;
let mut h11 = 0.0f32;
let mut h12 = 0.0f32;
let mut h22 = 0.0f32;
let mut g0 = 0.0f32;
let mut g1 = 0.0f32;
let mut g2 = 0.0f32;
let mut sum_weighted_sq = 0.0f32;
let mut sum_weights = 0.0f32;
let chunks = corr.simd_chunks();
for i in 0..chunks {
let base = i * 4;
// CONTIGUOUS LOADS - the key optimization!
let px = f32x4::from_slice(&corr.point_xs[base..]);
let py = f32x4::from_slice(&corr.point_ys[base..]);
let nx = f32x4::from_slice(&corr.normal_xs[base..]);
let ny = f32x4::from_slice(&corr.normal_ys[base..]);
let sx = f32x4::from_slice(&corr.line_start_xs[base..]);
let sy = f32x4::from_slice(&corr.line_start_ys[base..]);
let ex = f32x4::from_slice(&corr.line_end_xs[base..]);
let ey = f32x4::from_slice(&corr.line_end_ys[base..]);
let inv_len = f32x4::from_slice(&corr.line_inv_lengths[base..]);
let w = f32x4::from_slice(&corr.weights[base..]);
// Transform points: tx = px*cos - py*sin + pose.x
let tx = px * cos4 - py * sin4 + pose_x4;
let ty = px * sin4 + py * cos4 + pose_y4;
// Compute residual: cross(end-start, transformed-start) * inv_length
let dx = ex - sx;
let dy = ey - sy;
let to_px = tx - sx;
let to_py = ty - sy;
let cross = dx * to_py - dy * to_px;
let residual = cross * inv_len;
// Accumulate weighted squared error
sum_weighted_sq += (w * residual * residual).reduce_sum();
sum_weights += w.reduce_sum();
// Jacobian components
// j0 = nx, j1 = ny
// j2 = nx * (px * -sin + py * -cos) + ny * (px * cos + py * -sin)
let j2 = nx * (px * neg_sin4 + py * neg_cos4) + ny * (px * cos4 + py * neg_sin4);
// Weighted Jacobian terms
let wj0 = w * nx;
let wj1 = w * ny;
let wj2 = w * j2;
// Accumulate Hessian (H = J^T W J)
h00 += (wj0 * nx).reduce_sum();
h01 += (wj0 * ny).reduce_sum();
h02 += (wj0 * j2).reduce_sum();
h11 += (wj1 * ny).reduce_sum();
h12 += (wj1 * j2).reduce_sum();
h22 += (wj2 * j2).reduce_sum();
// Accumulate gradient (g = J^T W r)
let wr = w * residual;
g0 += (nx * wr).reduce_sum();
g1 += (ny * wr).reduce_sum();
g2 += (j2 * wr).reduce_sum();
}
// Build symmetric Hessian
let h = [[h00, h01, h02], [h01, h11, h12], [h02, h12, h22]];
let g = [g0, g1, g2];
(h, g, sum_weighted_sq, sum_weights)
}
/// Compute RMS residual using SIMD.
#[inline]
fn compute_residual_simd(corr: &CorrespondenceSoA, pose: &Pose2D) -> f32 {
if corr.is_empty() {
return 0.0;
}
let (sin, cos) = pose.theta.sin_cos();
let cos4 = f32x4::splat(cos);
let sin4 = f32x4::splat(sin);
let pose_x4 = f32x4::splat(pose.x);
let pose_y4 = f32x4::splat(pose.y);
let mut sum_sq = 0.0f32;
let chunks = corr.simd_chunks();
for i in 0..chunks {
let base = i * 4;
let px = f32x4::from_slice(&corr.point_xs[base..]);
let py = f32x4::from_slice(&corr.point_ys[base..]);
let sx = f32x4::from_slice(&corr.line_start_xs[base..]);
let sy = f32x4::from_slice(&corr.line_start_ys[base..]);
let ex = f32x4::from_slice(&corr.line_end_xs[base..]);
let ey = f32x4::from_slice(&corr.line_end_ys[base..]);
let inv_len = f32x4::from_slice(&corr.line_inv_lengths[base..]);
let w = f32x4::from_slice(&corr.weights[base..]);
// Transform points
let tx = px * cos4 - py * sin4 + pose_x4;
let ty = px * sin4 + py * cos4 + pose_y4;
// Compute distance
let dx = ex - sx;
let dy = ey - sy;
let to_px = tx - sx;
let to_py = ty - sy;
let cross = dx * to_py - dy * to_px;
let dist = cross.abs() * inv_len;
// Only count real correspondences (w > 0), but since padding has w=0,
// multiplying by w then dividing by sum(w) handles it correctly.
// For unweighted RMS, we need to track the actual count.
// Since we pad with zero weights, we use weighted sum approach.
sum_sq += (w * dist * dist).reduce_sum();
}
// Use actual correspondence count (not padded)
let total_weight: f32 = corr.weights[..corr.len()].iter().sum();
if total_weight > 0.0 {
(sum_sq / total_weight).sqrt()
} else {
0.0
}
}
#[cfg(test)]
mod tests {
use super::*;
vastu-map/src/matching/mod.rs
+2 -7
View File
@@ -73,14 +73,9 @@ pub use correspondence::{
CornerCorrespondence, CornerCorrespondenceSet, Correspondence, CorrespondenceSet,
CorrespondenceSoA, IDENTITY_COVARIANCE, LineDataRaw, MatchResult, PoseCovariance,
};
pub use gauss_newton::{
GaussNewtonConfig, GaussNewtonResult, optimize_pose_fast, optimize_pose_simd,
};
pub use gauss_newton::{GaussNewtonConfig, GaussNewtonResult, optimize_pose_fast};
pub use nearest_neighbor::{
CornerMatchConfig, CorrespondenceCandidate, NearestNeighborConfig, find_corner_correspondences,
find_corner_correspondences_weighted, find_correspondences, find_correspondences_batch,
find_correspondences_soa, find_correspondences_soa_unweighted, find_correspondences_spatial,
find_correspondences_weighted, find_correspondences_with_angle,
CorrespondenceCandidate, NearestNeighborConfig, find_correspondences,
validate_correspondence_candidate,
};
// Re-export from icp submodule
vastu-map/src/matching/nearest_neighbor.rs
+5 -887
View File
@@ -3,29 +3,14 @@
//! Finds correspondences between scan points and map lines using
//! distance-based matching with geometric constraints.
//!
//! # Performance
//!
//! Three variants are provided:
//! - `find_correspondences`: Basic O(n×m) brute force
//! - `find_correspondences_batch`: SIMD-optimized O(n×m) using LineCollection
//! - `find_correspondences_spatial`: O(n×log(m)) using R-tree spatial index
//!
//! Use `find_correspondences_spatial` for large maps (>50 lines).
//!
//! # Weighted Correspondences
//!
//! For uncertainty-aware matching, use `find_correspondences_weighted` which
//! computes weights based on range using a LidarNoiseModel.
//! Note: For ICP hot paths, use `IcpScratchSpace::find_correspondences()`
//! which is zero-allocation. This module's `find_correspondences()` function
//! is provided for testing and simple use cases.
use crate::config::LidarNoiseModel;
use crate::core::Point2D;
use crate::features::{Corner2D, Line2D, LineCollection};
use crate::integration::SpatialIndex;
use crate::features::Line2D;
use super::correspondence::{
CornerCorrespondence, CornerCorrespondenceSet, Correspondence, CorrespondenceSet,
CorrespondenceSoA,
};
use super::correspondence::{Correspondence, CorrespondenceSet};
// ============================================================================
// Correspondence Validation Helper
@@ -259,621 +244,6 @@ pub fn find_correspondences(
result
}
/// Find correspondences using SoA LineCollection (SIMD-optimized).
///
/// This version uses batch distance computation for better performance
/// on large line sets.
///
/// # Arguments
/// * `points` - Scan points in world frame
/// * `lines` - Map lines in SoA format
/// * `config` - Matching configuration
///
/// # Returns
/// Set of valid correspondences.
pub fn find_correspondences_batch(
points: &[Point2D],
lines: &LineCollection,
config: &NearestNeighborConfig,
) -> CorrespondenceSet {
if points.is_empty() || lines.is_empty() {
return CorrespondenceSet::new();
}
let mut result = CorrespondenceSet::with_capacity(points.len());
let min_t = -config.max_projection_extension;
let max_t = 1.0 + config.max_projection_extension;
let n_lines = lines.len();
for (point_idx, &point) in points.iter().enumerate() {
// Compute distances to all lines at once (SIMD-friendly)
let distances = lines.distances_to_point(point);
let projections = lines.project_point(point);
let mut best_distance = config.max_distance;
let mut best_corr: Option<Correspondence> = None;
for line_idx in 0..n_lines {
let t = projections[line_idx];
let distance = distances[line_idx];
// Check projection bounds
if t < min_t || t > max_t {
continue;
}
// Check distance threshold
if distance > config.max_distance {
continue;
}
if config.unique_per_point {
if distance < best_distance {
best_distance = distance;
best_corr = Some(Correspondence::new(point_idx, line_idx, point, distance, t));
}
} else {
result.push(Correspondence::new(point_idx, line_idx, point, distance, t));
}
}
if config.unique_per_point
&& let Some(corr) = best_corr
{
result.push(corr);
}
}
result
}
/// Find correspondences with range-based weights using LineCollection.
///
/// Computes weights for each correspondence based on the range from sensor
/// to the point, using the provided noise model. Closer points are weighted
/// higher as they have less measurement uncertainty.
///
/// # Arguments
/// * `points` - Scan points in world frame
/// * `ranges` - Range (distance from sensor) for each point
/// * `lines` - Map lines in SoA format
/// * `noise_model` - Lidar noise model for weight computation
/// * `config` - Matching configuration
///
/// # Returns
/// Set of weighted correspondences.
pub fn find_correspondences_weighted(
points: &[Point2D],
ranges: &[f32],
lines: &LineCollection,
noise_model: &LidarNoiseModel,
config: &NearestNeighborConfig,
) -> CorrespondenceSet {
if points.is_empty() || lines.is_empty() {
return CorrespondenceSet::new();
}
debug_assert_eq!(
points.len(),
ranges.len(),
"points and ranges must have same length"
);
let mut result = CorrespondenceSet::with_capacity(points.len());
let min_t = -config.max_projection_extension;
let max_t = 1.0 + config.max_projection_extension;
let n_lines = lines.len();
for (point_idx, (&point, &range)) in points.iter().zip(ranges.iter()).enumerate() {
// Compute distances to all lines at once (SIMD-friendly)
let distances = lines.distances_to_point(point);
let projections = lines.project_point(point);
// Compute weight for this point based on its range
let weight = noise_model.weight(range);
let mut best_distance = config.max_distance;
let mut best_corr: Option<Correspondence> = None;
for line_idx in 0..n_lines {
let t = projections[line_idx];
let distance = distances[line_idx];
// Check projection bounds
if t < min_t || t > max_t {
continue;
}
// Check distance threshold
if distance > config.max_distance {
continue;
}
if config.unique_per_point {
if distance < best_distance {
best_distance = distance;
best_corr = Some(Correspondence::with_weight(
point_idx, line_idx, point, distance, t, weight, range,
));
}
} else {
result.push(Correspondence::with_weight(
point_idx, line_idx, point, distance, t, weight, range,
));
}
}
if config.unique_per_point
&& let Some(corr) = best_corr
{
result.push(corr);
}
}
result
}
/// Find correspondences using LineCollection, producing SIMD-optimized SoA layout.
///
/// This function produces `CorrespondenceSoA` directly, inlining line data at
/// correspondence creation time. Use this when the correspondences will be
/// used in the SIMD-optimized Gauss-Newton solver.
///
/// # Arguments
/// * `points` - Scan points in world frame
/// * `ranges` - Range (distance from sensor) for each point
/// * `lines` - Map lines in SoA format
/// * `noise_model` - Lidar noise model for weight computation
/// * `config` - Matching configuration
///
/// # Returns
/// SIMD-optimized SoA correspondence collection with finalized padding.
pub fn find_correspondences_soa(
points: &[Point2D],
ranges: &[f32],
lines: &LineCollection,
noise_model: &LidarNoiseModel,
config: &NearestNeighborConfig,
) -> CorrespondenceSoA {
if points.is_empty() || lines.is_empty() {
return CorrespondenceSoA::new();
}
debug_assert_eq!(
points.len(),
ranges.len(),
"points and ranges must have same length"
);
let mut result = CorrespondenceSoA::with_capacity(points.len());
let min_t = -config.max_projection_extension;
let max_t = 1.0 + config.max_projection_extension;
let n_lines = lines.len();
for (point_idx, (&point, &range)) in points.iter().zip(ranges.iter()).enumerate() {
// Compute distances to all lines at once (SIMD-friendly)
let distances = lines.distances_to_point(point);
let projections = lines.project_point(point);
// Compute weight for this point based on its range
let weight = noise_model.weight(range);
let mut best_distance = config.max_distance;
let mut best_line_idx: Option<usize> = None;
let mut best_t = 0.0f32;
for line_idx in 0..n_lines {
let t = projections[line_idx];
let distance = distances[line_idx];
// Check projection bounds
if t < min_t || t > max_t {
continue;
}
// Check distance threshold
if distance > config.max_distance {
continue;
}
if config.unique_per_point {
if distance < best_distance {
best_distance = distance;
best_line_idx = Some(line_idx);
best_t = t;
}
} else {
// Add all valid matches directly
result.push(point_idx, line_idx, point, distance, weight, lines);
}
}
if config.unique_per_point
&& let Some(line_idx) = best_line_idx
{
let _ = best_t; // Unused but computed for consistency
result.push(point_idx, line_idx, point, best_distance, weight, lines);
}
}
// Finalize with SIMD padding
result.finalize();
result
}
/// Find correspondences using LineCollection, producing SoA layout without weights.
///
/// Similar to `find_correspondences_soa` but with default weights of 1.0.
/// Useful when noise model is not available.
pub fn find_correspondences_soa_unweighted(
points: &[Point2D],
lines: &LineCollection,
config: &NearestNeighborConfig,
) -> CorrespondenceSoA {
if points.is_empty() || lines.is_empty() {
return CorrespondenceSoA::new();
}
let mut result = CorrespondenceSoA::with_capacity(points.len());
let min_t = -config.max_projection_extension;
let max_t = 1.0 + config.max_projection_extension;
let n_lines = lines.len();
for (point_idx, &point) in points.iter().enumerate() {
// Compute distances to all lines at once (SIMD-friendly)
let distances = lines.distances_to_point(point);
let projections = lines.project_point(point);
let mut best_distance = config.max_distance;
let mut best_line_idx: Option<usize> = None;
for line_idx in 0..n_lines {
let t = projections[line_idx];
let distance = distances[line_idx];
// Check projection bounds
if t < min_t || t > max_t {
continue;
}
// Check distance threshold
if distance > config.max_distance {
continue;
}
if config.unique_per_point {
if distance < best_distance {
best_distance = distance;
best_line_idx = Some(line_idx);
}
} else {
result.push(point_idx, line_idx, point, distance, 1.0, lines);
}
}
if config.unique_per_point
&& let Some(line_idx) = best_line_idx
{
result.push(point_idx, line_idx, point, best_distance, 1.0, lines);
}
}
// Finalize with SIMD padding
result.finalize();
result
}
/// Find correspondences using R-tree spatial index (O(n log m) complexity).
///
/// This version uses a spatial index for fast nearest-neighbor queries,
/// making it much faster than brute-force for large maps (>50 lines).
///
/// # Arguments
/// * `points` - Scan points in world frame
/// * `lines` - Map lines to match against
/// * `index` - Pre-built spatial index for the lines
/// * `config` - Matching configuration
///
/// # Performance
///
/// - For small maps (<50 lines): Use `find_correspondences_batch` instead
/// - For large maps (>50 lines): This version is 5-10x faster
///
/// # Example
/// ```rust,ignore
/// use vastu_map::matching::{find_correspondences_spatial, NearestNeighborConfig};
/// use vastu_map::integration::SpatialIndex;
///
/// let lines = vec![/* map lines */];
/// let index = SpatialIndex::new(&lines);
///
/// for scan in scans {
/// // Reuse index across scans
/// let corrs = find_correspondences_spatial(&scan, &lines, &index, &config);
/// }
/// ```
pub fn find_correspondences_spatial(
points: &[Point2D],
lines: &[Line2D],
index: &SpatialIndex,
config: &NearestNeighborConfig,
) -> CorrespondenceSet {
if points.is_empty() || lines.is_empty() {
return CorrespondenceSet::new();
}
let mut result = CorrespondenceSet::with_capacity(points.len());
for (point_idx, &point) in points.iter().enumerate() {
// Use spatial index to find candidate lines within distance threshold
// This is O(log n) instead of O(n)
let candidates = index.lines_within_distance(point, config.max_distance);
if candidates.is_empty() {
continue;
}
let mut best_distance = config.max_distance;
let mut best_corr: Option<Correspondence> = None;
for (line_idx, distance) in candidates {
let line = &lines[line_idx];
// Check projection bounds only (distance already validated by spatial index)
let t = line.project_point(point);
if !line.contains_projection_extended(t, config.max_projection_extension) {
continue;
}
if config.unique_per_point {
if distance < best_distance {
best_distance = distance;
best_corr = Some(Correspondence::new(point_idx, line_idx, point, distance, t));
}
} else {
result.push(Correspondence::new(point_idx, line_idx, point, distance, t));
}
}
if config.unique_per_point
&& let Some(corr) = best_corr
{
result.push(corr);
}
}
result
}
/// Find correspondences with angle constraint.
///
/// Adds an additional constraint that the point's bearing from the sensor
/// should be approximately perpendicular to the line normal.
///
/// # Arguments
/// * `points` - Scan points in world frame
/// * `lines` - Map lines to match against
/// * `sensor_position` - Position of the sensor in world frame
/// * `max_angle_deviation` - Maximum deviation from perpendicular (radians)
/// * `config` - Matching configuration
pub fn find_correspondences_with_angle(
points: &[Point2D],
lines: &[Line2D],
sensor_position: Point2D,
max_angle_deviation: f32,
config: &NearestNeighborConfig,
) -> CorrespondenceSet {
if points.is_empty() || lines.is_empty() {
return CorrespondenceSet::new();
}
let mut result = CorrespondenceSet::with_capacity(points.len());
let cos_threshold = max_angle_deviation.cos();
for (point_idx, &point) in points.iter().enumerate() {
// Compute bearing from sensor to point
let bearing = (point - sensor_position).normalized();
let mut best_distance = config.max_distance;
let mut best_corr: Option<Correspondence> = None;
for (line_idx, line) in lines.iter().enumerate() {
// Check angle constraint first (cheap)
let line_normal = line.normal();
let dot = bearing.dot(line_normal).abs();
// bearing should be roughly parallel to normal (hitting line head-on)
if dot < cos_threshold {
continue;
}
// Use validation helper for projection and distance constraints
let Some(candidate) = validate_correspondence_candidate(
point,
line,
config.max_distance,
config.max_projection_extension,
) else {
continue;
};
if config.unique_per_point {
if candidate.distance < best_distance {
best_distance = candidate.distance;
best_corr = Some(Correspondence::new(
point_idx,
line_idx,
point,
candidate.distance,
candidate.t,
));
}
} else {
result.push(Correspondence::new(
point_idx,
line_idx,
point,
candidate.distance,
candidate.t,
));
}
}
if config.unique_per_point
&& let Some(corr) = best_corr
{
result.push(corr);
}
}
result
}
/// Configuration for corner correspondence matching.
#[derive(Clone, Debug)]
pub struct CornerMatchConfig {
/// Maximum distance from point to corner for a valid correspondence (meters).
/// Default: 0.15m (15cm)
pub max_distance: f32,
/// Whether to only keep the best correspondence per point.
/// Default: true
pub unique_per_point: bool,
}
impl Default for CornerMatchConfig {
fn default() -> Self {
Self {
max_distance: 0.15,
unique_per_point: true,
}
}
}
impl CornerMatchConfig {
/// Create a new configuration with default values.
pub fn new() -> Self {
Self::default()
}
/// Builder-style setter for maximum distance.
pub fn with_max_distance(mut self, max_distance: f32) -> Self {
self.max_distance = max_distance;
self
}
/// Builder-style setter for unique per point.
pub fn with_unique_per_point(mut self, unique: bool) -> Self {
self.unique_per_point = unique;
self
}
}
/// Find correspondences between scan points and map corners.
///
/// Uses simple nearest-neighbor matching to associate scan points with
/// map corners within a distance threshold.
///
/// # Arguments
/// * `points` - Scan points in world frame (after initial pose transform)
/// * `corners` - Map corners to match against
/// * `config` - Matching configuration
///
/// # Returns
/// Set of corner correspondences, where each point is matched to its
/// nearest corner within the distance threshold.
///
/// # Example
/// ```rust,ignore
/// use vastu_map::matching::{find_corner_correspondences, CornerMatchConfig};
///
/// let corners = vec![/* map corners */];
/// let points = vec![/* scan points in world frame */];
/// let config = CornerMatchConfig::default();
///
/// let corrs = find_corner_correspondences(&points, &corners, &config);
/// println!("Found {} corner matches", corrs.len());
/// ```
pub fn find_corner_correspondences(
points: &[Point2D],
corners: &[Corner2D],
config: &CornerMatchConfig,
) -> CornerCorrespondenceSet {
if points.is_empty() || corners.is_empty() {
return CornerCorrespondenceSet::new();
}
let mut result = CornerCorrespondenceSet::with_capacity(corners.len().min(points.len()));
let max_dist_sq = config.max_distance * config.max_distance;
for (point_idx, &point) in points.iter().enumerate() {
let mut best_distance_sq = max_dist_sq;
let mut best_corner_idx: Option<usize> = None;
for (corner_idx, corner) in corners.iter().enumerate() {
let dx = point.x - corner.position.x;
let dy = point.y - corner.position.y;
let distance_sq = dx * dx + dy * dy;
if distance_sq < best_distance_sq {
best_distance_sq = distance_sq;
best_corner_idx = Some(corner_idx);
// Early exit if we find a very close match
if distance_sq < 0.001 {
break;
}
}
}
if let Some(corner_idx) = best_corner_idx {
let distance = best_distance_sq.sqrt();
let corner = &corners[corner_idx];
result.push(CornerCorrespondence::new(
point_idx,
corner_idx,
point,
corner.position,
distance,
));
}
}
result
}
/// Find corner correspondences with weighting based on range.
///
/// Similar to `find_corner_correspondences`, but applies weights based on
/// the range from the sensor to each point using a lidar noise model.
///
/// # Arguments
/// * `points` - Scan points in world frame
/// * `robot_frame_points` - Original scan points in robot frame (for range computation)
/// * `corners` - Map corners to match against
/// * `config` - Matching configuration
/// * `noise_model` - Lidar noise model for weight computation
pub fn find_corner_correspondences_weighted(
points: &[Point2D],
robot_frame_points: &[Point2D],
corners: &[Corner2D],
config: &CornerMatchConfig,
noise_model: &LidarNoiseModel,
) -> CornerCorrespondenceSet {
let mut result = find_corner_correspondences(points, corners, config);
// Apply weights based on range
for corr in result.iter_mut() {
if corr.point_idx < robot_frame_points.len() {
let p = robot_frame_points[corr.point_idx];
let range = (p.x * p.x + p.y * p.y).sqrt();
corr.weight = noise_model.weight(range);
}
}
result
}
#[cfg(test)]
mod tests {
use super::*;
@@ -1038,23 +408,6 @@ mod tests {
assert!(corrs2.is_empty());
}
#[test]
fn test_find_correspondences_batch() {
let mut collection = LineCollection::new();
collection.push_line(&make_horizontal_line());
collection.push_line(&make_vertical_line());
let points = vec![
Point2D::new(1.0, 0.1), // Near horizontal
Point2D::new(0.1, 1.0), // Near vertical
];
let config = NearestNeighborConfig::default();
let corrs = find_correspondences_batch(&points, &collection, &config);
assert_eq!(corrs.len(), 2);
}
#[test]
fn test_projection_accuracy() {
let line = make_horizontal_line(); // From (0,0) to (2,0)
@@ -1133,110 +486,6 @@ mod tests {
);
}
#[test]
fn test_batch_vs_nonbatch_consistency() {
// Results from batch and non-batch should be identical
let lines_vec = vec![
Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(5.0, 0.0)),
Line2D::new(Point2D::new(5.0, 0.0), Point2D::new(5.0, 5.0)),
Line2D::new(Point2D::new(5.0, 5.0), Point2D::new(0.0, 5.0)),
Line2D::new(Point2D::new(0.0, 5.0), Point2D::new(0.0, 0.0)),
];
let mut collection = LineCollection::new();
for line in &lines_vec {
collection.push_line(line);
}
let points = vec![
Point2D::new(2.5, 0.1),
Point2D::new(4.9, 2.5),
Point2D::new(2.5, 4.9),
Point2D::new(0.1, 2.5),
];
let config = NearestNeighborConfig::default().with_unique_per_point(true);
let corrs_regular = find_correspondences(&points, &lines_vec, &config);
let corrs_batch = find_correspondences_batch(&points, &collection, &config);
// Same number of correspondences
assert_eq!(corrs_regular.len(), corrs_batch.len());
// Same line indices matched
for i in 0..corrs_regular.len() {
assert_eq!(
corrs_regular.correspondences[i].line_idx, corrs_batch.correspondences[i].line_idx,
"Line index mismatch at position {}",
i
);
assert!(
(corrs_regular.correspondences[i].distance
- corrs_batch.correspondences[i].distance)
.abs()
< 0.001,
"Distance mismatch at position {}",
i
);
}
}
#[test]
fn test_find_correspondences_with_angle_basic() {
// Test angle-constrained matching
let lines = vec![
Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(5.0, 0.0)), // Horizontal
Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(0.0, 5.0)), // Vertical
];
// Sensor at center, point on horizontal line
let sensor_pos = Point2D::new(2.5, 2.5);
let points = vec![Point2D::new(2.5, 0.1)]; // Point below sensor, near horizontal line
// Bearing from sensor to point is straight down (perpendicular to horizontal line normal)
let config = NearestNeighborConfig::default();
let max_angle_deviation = 0.5; // ~29 degrees
let corrs = find_correspondences_with_angle(
&points,
&lines,
sensor_pos,
max_angle_deviation,
&config,
);
// Should match horizontal line (bearing is perpendicular to line, parallel to normal)
assert_eq!(corrs.len(), 1);
assert_eq!(corrs.correspondences[0].line_idx, 0);
}
#[test]
fn test_find_correspondences_with_angle_rejection() {
// Test that angle constraint rejects bad matches
let lines = vec![
Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(5.0, 0.0)), // Horizontal
];
// Sensor to the left, point to the right of sensor
let sensor_pos = Point2D::new(0.0, 0.5);
let points = vec![Point2D::new(2.5, 0.5)]; // Point to the right, bearing is horizontal
// Bearing is parallel to line (perpendicular to normal) - should be rejected
let config = NearestNeighborConfig::default();
let max_angle_deviation = 0.3; // ~17 degrees - strict
let corrs = find_correspondences_with_angle(
&points,
&lines,
sensor_pos,
max_angle_deviation,
&config,
);
// Should reject because bearing is parallel to line, not hitting it head-on
assert!(corrs.is_empty(), "Should reject grazing angle match");
}
#[test]
fn test_config_default_values() {
let config = NearestNeighborConfig::default();
@@ -1269,48 +518,6 @@ mod tests {
}
}
#[test]
fn test_batch_empty_inputs() {
let config = NearestNeighborConfig::default();
// Empty points
let collection = LineCollection::new();
let corrs1 = find_correspondences_batch(&[], &collection, &config);
assert!(corrs1.is_empty());
// Empty lines
let mut collection2 = LineCollection::new();
collection2.push_line(&make_horizontal_line());
let corrs2 = find_correspondences_batch(&[], &collection2, &config);
assert!(corrs2.is_empty());
}
#[test]
fn test_angle_with_empty_inputs() {
let config = NearestNeighborConfig::default();
let sensor_pos = Point2D::new(0.0, 0.0);
// Empty points
let corrs1 = find_correspondences_with_angle(
&[],
&[make_horizontal_line()],
sensor_pos,
0.5,
&config,
);
assert!(corrs1.is_empty());
// Empty lines
let corrs2 = find_correspondences_with_angle(
&[Point2D::new(1.0, 0.1)],
&[],
sensor_pos,
0.5,
&config,
);
assert!(corrs2.is_empty());
}
#[test]
fn test_non_unique_mode_with_many_lines() {
// Test non-unique mode returns all valid matches
@@ -1332,93 +539,4 @@ mod tests {
// Should match all three lines (distances: 0.1, 0.0, 0.1)
assert_eq!(corrs.len(), 3, "Should match all 3 parallel lines");
}
// === Corner correspondence tests ===
fn make_corner(x: f32, y: f32) -> Corner2D {
Corner2D::new(Point2D::new(x, y), 0, 1, std::f32::consts::FRAC_PI_2)
}
#[test]
fn test_find_corner_correspondences_basic() {
let corners = vec![
make_corner(0.0, 0.0),
make_corner(5.0, 0.0),
make_corner(5.0, 5.0),
make_corner(0.0, 5.0),
];
// Point near corner 0
let points = vec![Point2D::new(0.05, 0.02)];
let config = CornerMatchConfig::default();
let corrs = find_corner_correspondences(&points, &corners, &config);
assert_eq!(corrs.len(), 1);
assert_eq!(corrs.correspondences[0].corner_idx, 0);
// Distance should be sqrt(0.05^2 + 0.02^2) ≈ 0.054
assert!(corrs.correspondences[0].distance < 0.06);
}
#[test]
fn test_find_corner_correspondences_multiple_points() {
let corners = vec![
make_corner(0.0, 0.0),
make_corner(5.0, 0.0),
make_corner(5.0, 5.0),
make_corner(0.0, 5.0),
];
// Points near each corner
let points = vec![
Point2D::new(0.05, 0.02), // Near corner 0
Point2D::new(4.98, 0.01), // Near corner 1
Point2D::new(5.02, 4.97), // Near corner 2
Point2D::new(0.01, 5.03), // Near corner 3
];
let config = CornerMatchConfig::default();
let corrs = find_corner_correspondences(&points, &corners, &config);
assert_eq!(corrs.len(), 4);
// Check that each point matched the expected corner
assert_eq!(corrs.correspondences[0].corner_idx, 0);
assert_eq!(corrs.correspondences[1].corner_idx, 1);
assert_eq!(corrs.correspondences[2].corner_idx, 2);
assert_eq!(corrs.correspondences[3].corner_idx, 3);
}
#[test]
fn test_find_corner_correspondences_no_match() {
let corners = vec![make_corner(0.0, 0.0)];
let points = vec![Point2D::new(1.0, 1.0)]; // Too far from corner
let config = CornerMatchConfig::default().with_max_distance(0.1);
let corrs = find_corner_correspondences(&points, &corners, &config);
assert!(corrs.is_empty());
}
#[test]
fn test_find_corner_correspondences_empty_inputs() {
let config = CornerMatchConfig::default();
// Empty points
let corrs1 = find_corner_correspondences(&[], &[make_corner(0.0, 0.0)], &config);
assert!(corrs1.is_empty());
// Empty corners
let corrs2 = find_corner_correspondences(&[Point2D::new(0.0, 0.0)], &[], &config);
assert!(corrs2.is_empty());
}
#[test]
fn test_corner_match_config_builder() {
let config = CornerMatchConfig::new()
.with_max_distance(0.2)
.with_unique_per_point(false);
assert_eq!(config.max_distance, 0.2);
assert!(!config.unique_per_point);
}
}
vastu-map/src/preprocessing.rs
-445
View File
@@ -1,445 +0,0 @@
//! Scan preprocessing for point cloud filtering.
//!
//! Provides outlier removal and downsampling operations for lidar scans.
//!
//! # Example
//!
//! ```rust
//! use vastu_map::preprocessing::{PreprocessingConfig, preprocess, remove_outliers, downsample};
//! use vastu_map::core::PointCloud2D;
//!
//! // Create a point cloud with some outliers
//! let mut cloud = PointCloud2D::new();
//! cloud.push(0.0, 0.0);
//! cloud.push(0.1, 0.0);
//! cloud.push(0.2, 0.0);
//! cloud.push(10.0, 10.0); // Outlier
//!
//! // Remove outliers
//! let config = PreprocessingConfig::default();
//! let clean = remove_outliers(&cloud, &config);
//! assert!(clean.len() < cloud.len()); // Outlier removed
//! ```
use crate::core::PointCloud2D;
use std::collections::HashMap;
/// Configuration for scan preprocessing.
#[derive(Clone, Debug)]
pub struct PreprocessingConfig {
/// Radius for neighbor search in outlier removal (meters).
/// Default: 0.1m
pub outlier_radius: f32,
/// Minimum number of neighbors required to keep a point.
/// Points with fewer neighbors are removed as outliers.
/// Default: 2
pub min_neighbors: usize,
/// Voxel size for downsampling (meters).
/// Set to 0.0 to disable downsampling.
/// Default: 0.0 (disabled)
pub downsample_resolution: f32,
}
impl Default for PreprocessingConfig {
fn default() -> Self {
Self {
outlier_radius: 0.1,
min_neighbors: 2,
downsample_resolution: 0.0,
}
}
}
impl PreprocessingConfig {
/// Create a new configuration with default values.
pub fn new() -> Self {
Self::default()
}
/// Builder-style setter for outlier radius.
pub fn with_outlier_radius(mut self, radius: f32) -> Self {
self.outlier_radius = radius;
self
}
/// Builder-style setter for minimum neighbors.
pub fn with_min_neighbors(mut self, min: usize) -> Self {
self.min_neighbors = min;
self
}
/// Builder-style setter for downsample resolution.
pub fn with_downsample_resolution(mut self, resolution: f32) -> Self {
self.downsample_resolution = resolution;
self
}
}
/// Remove isolated outlier points using radius-based filtering.
///
/// Keeps points that have at least `config.min_neighbors` other points
/// within `config.outlier_radius` distance.
///
/// # Arguments
///
/// * `cloud` - Input point cloud
/// * `config` - Preprocessing configuration
///
/// # Returns
///
/// New point cloud with outliers removed.
///
/// # Complexity
///
/// O(n²) where n is the number of points. Sufficient for typical lidar scans (~360 points).
pub fn remove_outliers(cloud: &PointCloud2D, config: &PreprocessingConfig) -> PointCloud2D {
let mut result = PointCloud2D::with_capacity(cloud.len());
remove_outliers_into(cloud, config, &mut result);
result
}
/// Zero-allocation variant of outlier removal.
///
/// Writes results to a pre-allocated output buffer.
pub fn remove_outliers_into(
cloud: &PointCloud2D,
config: &PreprocessingConfig,
out: &mut PointCloud2D,
) {
out.clear();
if cloud.is_empty() {
return;
}
let radius_sq = config.outlier_radius * config.outlier_radius;
let n = cloud.len();
// For each point, count neighbors within radius
for i in 0..n {
let xi = cloud.xs[i];
let yi = cloud.ys[i];
let mut neighbor_count = 0;
for j in 0..n {
if i == j {
continue;
}
let dx = cloud.xs[j] - xi;
let dy = cloud.ys[j] - yi;
let dist_sq = dx * dx + dy * dy;
if dist_sq <= radius_sq {
neighbor_count += 1;
if neighbor_count >= config.min_neighbors {
break; // Early exit - we have enough neighbors
}
}
}
if neighbor_count >= config.min_neighbors {
out.push(xi, yi);
}
}
}
/// Reduce point density using voxel grid downsampling.
///
/// Divides space into grid cells of the given resolution and keeps
/// the first point encountered in each cell.
///
/// # Arguments
///
/// * `cloud` - Input point cloud
/// * `resolution` - Voxel grid cell size in meters
///
/// # Returns
///
/// Downsampled point cloud.
///
/// # Complexity
///
/// O(n) where n is the number of points.
pub fn downsample(cloud: &PointCloud2D, resolution: f32) -> PointCloud2D {
let mut result = PointCloud2D::with_capacity(cloud.len());
downsample_into(cloud, resolution, &mut result);
result
}
/// Zero-allocation variant of downsampling.
///
/// Writes results to a pre-allocated output buffer.
pub fn downsample_into(cloud: &PointCloud2D, resolution: f32, out: &mut PointCloud2D) {
out.clear();
if cloud.is_empty() || resolution <= 0.0 {
// If resolution is invalid, copy all points
if resolution <= 0.0 {
out.xs.extend_from_slice(&cloud.xs);
out.ys.extend_from_slice(&cloud.ys);
}
return;
}
let inv_resolution = 1.0 / resolution;
// Hash map to track which cells have been occupied
// Key: (grid_x, grid_y) as i32 tuple converted to single i64
let mut occupied: HashMap<(i32, i32), bool> = HashMap::with_capacity(cloud.len());
for i in 0..cloud.len() {
let x = cloud.xs[i];
let y = cloud.ys[i];
// Compute grid cell indices
let gx = (x * inv_resolution).floor() as i32;
let gy = (y * inv_resolution).floor() as i32;
// Keep first point in each cell
if occupied.insert((gx, gy), true).is_none() {
out.push(x, y);
}
}
}
/// Apply full preprocessing pipeline.
///
/// Applies outlier removal followed by downsampling (if resolution > 0).
///
/// # Arguments
///
/// * `cloud` - Input point cloud
/// * `config` - Preprocessing configuration
///
/// # Returns
///
/// Preprocessed point cloud.
pub fn preprocess(cloud: &PointCloud2D, config: &PreprocessingConfig) -> PointCloud2D {
let mut result = PointCloud2D::with_capacity(cloud.len());
preprocess_into(cloud, config, &mut result);
result
}
/// Zero-allocation variant of full preprocessing.
///
/// Uses an internal temporary buffer for intermediate results.
pub fn preprocess_into(cloud: &PointCloud2D, config: &PreprocessingConfig, out: &mut PointCloud2D) {
// First, remove outliers
let mut temp = PointCloud2D::with_capacity(cloud.len());
remove_outliers_into(cloud, config, &mut temp);
// Then, downsample if enabled
if config.downsample_resolution > 0.0 {
downsample_into(&temp, config.downsample_resolution, out);
} else {
// Just copy the outlier-filtered result
out.clear();
out.xs.extend_from_slice(&temp.xs);
out.ys.extend_from_slice(&temp.ys);
}
}
#[cfg(test)]
mod tests {
use super::*;
fn make_line_cloud() -> PointCloud2D {
let mut cloud = PointCloud2D::new();
// Line of points at y=0, x from 0 to 1 in 0.05 increments
for i in 0..21 {
cloud.push(i as f32 * 0.05, 0.0);
}
cloud
}
fn make_cloud_with_outlier() -> PointCloud2D {
let mut cloud = make_line_cloud();
// Add an outlier far from the line
cloud.push(10.0, 10.0);
cloud
}
#[test]
fn test_config_default() {
let config = PreprocessingConfig::default();
assert_eq!(config.outlier_radius, 0.1);
assert_eq!(config.min_neighbors, 2);
assert_eq!(config.downsample_resolution, 0.0);
}
#[test]
fn test_config_builder() {
let config = PreprocessingConfig::new()
.with_outlier_radius(0.2)
.with_min_neighbors(3)
.with_downsample_resolution(0.05);
assert_eq!(config.outlier_radius, 0.2);
assert_eq!(config.min_neighbors, 3);
assert_eq!(config.downsample_resolution, 0.05);
}
#[test]
fn test_remove_outliers_empty() {
let cloud = PointCloud2D::new();
let config = PreprocessingConfig::default();
let result = remove_outliers(&cloud, &config);
assert!(result.is_empty());
}
#[test]
fn test_remove_outliers_keeps_dense_points() {
let cloud = make_line_cloud();
let config = PreprocessingConfig::default();
let result = remove_outliers(&cloud, &config);
// Most points should be kept (they have neighbors)
// Edge points might be removed
assert!(result.len() >= cloud.len() - 4);
}
#[test]
fn test_remove_outliers_removes_isolated() {
let cloud = make_cloud_with_outlier();
let config = PreprocessingConfig::default();
let result = remove_outliers(&cloud, &config);
// The outlier at (10, 10) should be removed
assert!(result.len() < cloud.len());
// Check that the outlier is not in the result
for i in 0..result.len() {
assert!(result.xs[i] < 5.0, "Outlier should be removed");
}
}
#[test]
fn test_remove_outliers_into() {
let cloud = make_cloud_with_outlier();
let config = PreprocessingConfig::default();
let mut result = PointCloud2D::new();
remove_outliers_into(&cloud, &config, &mut result);
assert!(result.len() < cloud.len());
}
#[test]
fn test_downsample_empty() {
let cloud = PointCloud2D::new();
let result = downsample(&cloud, 0.1);
assert!(result.is_empty());
}
#[test]
fn test_downsample_zero_resolution() {
let cloud = make_line_cloud();
let result = downsample(&cloud, 0.0);
// Should copy all points when resolution is 0
assert_eq!(result.len(), cloud.len());
}
#[test]
fn test_downsample_reduces_density() {
let cloud = make_line_cloud(); // 21 points from 0 to 1 at 0.05 spacing
// Downsample with 0.1 resolution - should reduce by ~half
let result = downsample(&cloud, 0.1);
assert!(result.len() < cloud.len());
assert!(result.len() >= 10); // At least 10 cells from x=0 to x=1
}
#[test]
fn test_downsample_into() {
let cloud = make_line_cloud();
let mut result = PointCloud2D::new();
downsample_into(&cloud, 0.1, &mut result);
assert!(result.len() < cloud.len());
}
#[test]
fn test_downsample_preserves_first_point() {
let mut cloud = PointCloud2D::new();
cloud.push(0.0, 0.0);
cloud.push(0.01, 0.01); // Same cell at 0.1 resolution
cloud.push(0.02, 0.02); // Same cell
let result = downsample(&cloud, 0.1);
// Should keep only the first point
assert_eq!(result.len(), 1);
assert_eq!(result.xs[0], 0.0);
assert_eq!(result.ys[0], 0.0);
}
#[test]
fn test_preprocess_outlier_only() {
let cloud = make_cloud_with_outlier();
let config = PreprocessingConfig::default(); // downsample disabled
let result = preprocess(&cloud, &config);
// Should remove outlier
assert!(result.len() < cloud.len());
}
#[test]
fn test_preprocess_full_pipeline() {
let cloud = make_cloud_with_outlier();
let config = PreprocessingConfig::new()
.with_outlier_radius(0.1)
.with_min_neighbors(2)
.with_downsample_resolution(0.1);
let result = preprocess(&cloud, &config);
// Should remove outlier AND downsample
assert!(result.len() < cloud.len() - 5);
}
#[test]
fn test_preprocess_into() {
let cloud = make_cloud_with_outlier();
let config = PreprocessingConfig::new().with_downsample_resolution(0.1);
let mut result = PointCloud2D::new();
preprocess_into(&cloud, &config, &mut result);
assert!(result.len() < cloud.len());
}
#[test]
fn test_negative_coordinates() {
let mut cloud = PointCloud2D::new();
cloud.push(-1.0, -1.0);
cloud.push(-0.95, -0.95);
cloud.push(-0.9, -0.9);
cloud.push(1.0, 1.0);
cloud.push(1.05, 1.05);
cloud.push(1.1, 1.1);
let config = PreprocessingConfig::new()
.with_outlier_radius(0.2)
.with_min_neighbors(2);
let result = remove_outliers(&cloud, &config);
// Should keep points in both clusters
assert_eq!(result.len(), 6);
}
}
vastu-map/src/query/mod.rs
+2 -8
View File
@@ -42,15 +42,9 @@ pub use frontier::{
FrontierConfig, FrontierDetector, cluster_centroid, cluster_frontiers, detect_frontiers,
get_best_frontier, rank_frontiers,
};
pub use occupancy::{
OccupancyConfig, is_path_clear, is_region_clear, query_occupancy, query_occupancy_batch,
query_occupancy_indexed,
};
pub use occupancy::{OccupancyConfig, is_path_clear, is_region_clear, query_occupancy};
pub use path_planning::is_straight_path_clear;
pub use raycast::{
RaycastResult, raycast, raycast_360, raycast_batch, raycast_detailed, raycast_indexed,
raycast_sweep,
};
pub use raycast::{RaycastResult, raycast, raycast_detailed};
// Re-export CBVG types
pub use cbvg::{CBVGConfig, CBVGNode, ClearanceVisibilityGraph, NodeType};
vastu-map/src/query/occupancy.rs
-72
View File
@@ -8,7 +8,6 @@ use serde::{Deserialize, Serialize};
use crate::Occupancy;
use crate::core::{Bounds, Point2D};
use crate::features::Line2D;
use crate::integration::SpatialIndex;
/// Configuration for occupancy queries.
#[derive(Clone, Debug, Serialize, Deserialize)]
@@ -107,58 +106,6 @@ pub fn query_occupancy(
Occupancy::Unknown
}
/// Query occupancy using spatial index for efficiency.
pub fn query_occupancy_indexed(
point: Point2D,
lines: &[Line2D],
index: &SpatialIndex,
bounds: Option<&Bounds>,
config: &OccupancyConfig,
) -> Occupancy {
// First check if point is outside map bounds
if let Some(b) = bounds
&& !b.contains(point)
{
return Occupancy::Unknown;
}
if lines.is_empty() {
return Occupancy::Unknown;
}
// Use spatial index to find nearest line
if let Some((_idx, dist)) = index.nearest_line(point) {
// Point is very close to a line - occupied
if dist < config.obstacle_distance {
return Occupancy::Occupied;
}
// Check if any lines are within known distance
let nearby = index.lines_within_distance(point, config.known_distance);
if !nearby.is_empty() {
return Occupancy::Free;
}
}
// No nearby lines - unknown
Occupancy::Unknown
}
/// Query occupancy for multiple points.
///
/// Returns occupancy for each point.
pub fn query_occupancy_batch(
points: &[Point2D],
lines: &[Line2D],
bounds: Option<&Bounds>,
config: &OccupancyConfig,
) -> Vec<Occupancy> {
points
.iter()
.map(|&p| query_occupancy(p, lines, bounds, config))
.collect()
}
/// Check if a path is collision-free.
///
/// # Arguments
@@ -307,25 +254,6 @@ mod tests {
assert_eq!(result, Occupancy::Unknown);
}
#[test]
fn test_query_batch() {
let lines = make_room();
// Center is 5m from walls, so need larger known_distance
let config = OccupancyConfig::default().with_known_distance(6.0);
let points = vec![
Point2D::zero(), // Free (center)
Point2D::new(0.0, -5.0), // Occupied (on wall)
Point2D::new(2.0, 0.0), // Free (inside room, near walls)
];
let results = query_occupancy_batch(&points, &lines, None, &config);
assert_eq!(results[0], Occupancy::Free);
assert_eq!(results[1], Occupancy::Occupied);
assert_eq!(results[2], Occupancy::Free);
}
#[test]
fn test_is_path_clear_straight() {
let lines = make_room();
vastu-map/src/query/raycast.rs
+1 -209
View File
@@ -4,8 +4,7 @@
//! first obstacle (line segment) in the map.
use crate::core::Point2D;
use crate::features::{Line2D, LineCollection};
use crate::integration::SpatialIndex;
use crate::features::Line2D;
/// Result of a raycast operation.
#[derive(Clone, Debug)]
@@ -98,129 +97,11 @@ pub fn raycast_detailed(
}
}
/// Cast a ray using spatial index for efficiency.
///
/// Uses the spatial index to only test lines in the ray's path.
/// This is O(log n + k) where k is the number of candidate lines,
/// compared to O(n) for the brute-force version.
pub fn raycast_indexed(
origin: Point2D,
direction: Point2D,
max_range: f32,
lines: &[Line2D],
index: &SpatialIndex,
) -> RaycastResult {
use crate::core::Bounds;
if lines.is_empty() {
return RaycastResult::miss(max_range);
}
let dir = direction.normalized();
let end_point = origin + dir * max_range;
// Compute the bounding box of the ray
let min_x = origin.x.min(end_point.x);
let max_x = origin.x.max(end_point.x);
let min_y = origin.y.min(end_point.y);
let max_y = origin.y.max(end_point.y);
// Query the spatial index for candidate lines
let bounds = Bounds::new(Point2D::new(min_x, min_y), Point2D::new(max_x, max_y));
let candidates = index.lines_in_bounds(&bounds);
if candidates.is_empty() {
return RaycastResult::miss(max_range);
}
// Test only candidate lines
let mut closest_dist = max_range;
let mut closest_line: Option<usize> = None;
let mut closest_point: Option<Point2D> = None;
for &idx in &candidates {
if let Some(line) = lines.get(idx)
&& let Some(t) = line.ray_intersection(origin, dir)
&& t > 0.0
&& t < closest_dist
{
closest_dist = t;
closest_line = Some(idx);
closest_point = Some(origin + dir * t);
}
}
match closest_line {
Some(idx) => RaycastResult::hit(closest_dist, idx, closest_point.unwrap()),
None => RaycastResult::miss(max_range),
}
}
/// Cast multiple rays (e.g., for simulating a lidar).
///
/// # Arguments
/// * `origin` - Ray origin
/// * `start_angle` - Starting angle (radians)
/// * `end_angle` - Ending angle (radians)
/// * `num_rays` - Number of rays to cast
/// * `max_range` - Maximum range
/// * `lines` - Map lines
///
/// # Returns
/// Vector of distances for each ray.
pub fn raycast_sweep(
origin: Point2D,
start_angle: f32,
end_angle: f32,
num_rays: usize,
max_range: f32,
lines: &[Line2D],
) -> Vec<f32> {
if num_rays == 0 {
return Vec::new();
}
let angle_step = if num_rays > 1 {
(end_angle - start_angle) / (num_rays - 1) as f32
} else {
0.0
};
(0..num_rays)
.map(|i| {
let angle = start_angle + i as f32 * angle_step;
let dir = Point2D::from_polar(1.0, angle);
raycast(origin, dir, max_range, lines)
})
.collect()
}
/// Cast rays in a full 360° sweep.
pub fn raycast_360(origin: Point2D, num_rays: usize, max_range: f32, lines: &[Line2D]) -> Vec<f32> {
use std::f32::consts::PI;
raycast_sweep(origin, -PI, PI, num_rays, max_range, lines)
}
/// Batch raycast using LineCollection (optimized for SIMD).
///
/// Note: Currently falls back to per-line iteration.
/// Future optimization could use SIMD for parallel intersection tests.
pub fn raycast_batch(
origin: Point2D,
direction: Point2D,
max_range: f32,
lines: &LineCollection,
) -> f32 {
let lines_vec = lines.to_lines();
raycast(origin, direction, max_range, &lines_vec)
}
#[cfg(test)]
mod tests {
use super::*;
use crate::core::Point2D;
use approx::assert_relative_eq;
use std::f32::consts::PI;
fn make_room() -> Vec<Line2D> {
vec![
@@ -295,39 +176,6 @@ mod tests {
assert_eq!(dist, 10.0);
}
#[test]
fn test_raycast_sweep() {
let lines = make_room();
let origin = Point2D::zero();
// Cast 4 rays: N, E, S, W
let distances = raycast_sweep(origin, -PI, PI, 4, 100.0, &lines);
// Note: with 4 rays from -π to π:
// Ray 0: angle = -π (West) → should hit left wall at distance 5
// Ray 1: angle = -π/3 ≈ -60° (roughly SW)
// etc.
assert_eq!(distances.len(), 4);
// All should be around 5m (hitting walls)
for dist in &distances {
assert!(*dist <= 10.0); // Should hit some wall
}
}
#[test]
fn test_raycast_360() {
let lines = make_room();
let origin = Point2D::zero();
let distances = raycast_360(origin, 8, 100.0, &lines);
assert_eq!(distances.len(), 8);
// All rays from center should hit a wall
for dist in &distances {
assert!(*dist < 100.0);
}
}
#[test]
fn test_raycast_from_edge() {
let lines = make_room();
@@ -367,60 +215,4 @@ mod tests {
// Line is behind, should not hit
assert_eq!(dist, 100.0);
}
#[test]
fn test_raycast_indexed_hit() {
let lines = make_room();
let index = SpatialIndex::new(&lines);
let origin = Point2D::zero();
let direction = Point2D::new(1.0, 0.0); // East
let result = raycast_indexed(origin, direction, 100.0, &lines, &index);
// Should hit right wall at x=5
assert!(result.hit);
assert_relative_eq!(result.distance, 5.0, epsilon = 0.01);
assert_eq!(result.line_idx, Some(1)); // Right wall
}
#[test]
fn test_raycast_indexed_miss() {
let lines = vec![Line2D::new(
Point2D::new(10.0, 0.0),
Point2D::new(10.0, 10.0),
)];
let index = SpatialIndex::new(&lines);
let origin = Point2D::zero();
let direction = Point2D::new(-1.0, 0.0); // West (away from line)
let result = raycast_indexed(origin, direction, 5.0, &lines, &index);
// Should miss (line is behind)
assert!(!result.hit);
assert_eq!(result.distance, 5.0);
}
#[test]
fn test_raycast_indexed_matches_brute_force() {
let lines = make_room();
let index = SpatialIndex::new(&lines);
// Test several directions
let origin = Point2D::zero();
let directions = [
Point2D::new(1.0, 0.0), // East
Point2D::new(-1.0, 0.0), // West
Point2D::new(0.0, 1.0), // North
Point2D::new(0.0, -1.0), // South
Point2D::new(1.0, 1.0).normalized(), // NE
];
for dir in &directions {
let brute = raycast_detailed(origin, *dir, 100.0, &lines);
let indexed = raycast_indexed(origin, *dir, 100.0, &lines, &index);
assert_eq!(brute.hit, indexed.hit);
assert_relative_eq!(brute.distance, indexed.distance, epsilon = 0.01);
}
}
}