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

Add SIMD fused distance/projection computation and cache optimizations

Browse Source 
- Add distances_and_projections_into() to LineCollection for single-pass
  computation with SIMD, improving cache locality over separate calls
- SIMD-optimize transform_points() in IcpScratchSpace (4 points per iteration)
- Add LineDataRaw struct to group line properties for cleaner correspondence API
- Simplify CorrespondenceSoA::push_inline() to take LineDataRaw instead of 7 params
- Remove simd module (std::simd documentation no longer needed as separate module)
This commit is contained in:
codetiger
2025-12-21 15:22:21 +05:30
parent dfbb33bb4a
commit 0b6086b704
6 changed files with 195 additions and 98 deletions
Download Patch File Download Diff File Expand all files Collapse all files
vastu-map/src/features/line_collection.rs
+103
View File
@@ -450,6 +450,109 @@ impl LineCollection {
}
}
/// Compute distances AND projections in a single pass (cache-friendly).
///
/// This fused method loads line data once and computes both values,
/// providing better cache utilization than calling `distances_to_point_into`
/// and `project_point` separately.
///
/// # Panics
/// Panics if `distances.len() < self.len()` or `projections.len() < self.len()`.
pub fn distances_and_projections_into(
&self,
point: Point2D,
distances: &mut [f32],
projections: &mut [f32],
) {
let n = self.len();
debug_assert!(
distances.len() >= n,
"Distance buffer too small: {} < {}",
distances.len(),
n
);
debug_assert!(
projections.len() >= n,
"Projection buffer too small: {} < {}",
projections.len(),
n
);
let px = f32x4::splat(point.x);
let py = f32x4::splat(point.y);
// Process 4 lines at a time
let chunks = n / 4;
for i in 0..chunks {
let base = i * 4;
// Load line data once - the key optimization
let sx = f32x4::from_slice(&self.start_xs[base..]);
let sy = f32x4::from_slice(&self.start_ys[base..]);
let ex = f32x4::from_slice(&self.end_xs[base..]);
let ey = f32x4::from_slice(&self.end_ys[base..]);
let inv_len = f32x4::from_slice(&self.inv_lengths[base..]);
// Compute direction vectors
let dx = ex - sx;
let dy = ey - sy;
// Compute vectors from start to point
let to_px = px - sx;
let to_py = py - sy;
// === Distance computation ===
// Cross product: to_px * dy - to_py * dx
let cross = to_px * dy - to_py * dx;
let dist_4 = cross.abs() * inv_len;
distances[base..base + 4].copy_from_slice(&dist_4.to_array());
// === Projection computation ===
// t = dot(point - start, end - start) / |end - start|²
let dot = to_px * dx + to_py * dy;
let len_sq = dx * dx + dy * dy;
// Handle degenerate lines: use 0.5 if len_sq < epsilon
let len_sq_arr = len_sq.to_array();
let dot_arr = dot.to_array();
for j in 0..4 {
projections[base + j] = if len_sq_arr[j] > f32::EPSILON {
dot_arr[j] / len_sq_arr[j]
} else {
0.5 // Degenerate line - midpoint
};
}
}
// Handle remainder (scalar)
#[allow(clippy::needless_range_loop)]
for i in (chunks * 4)..n {
let sx = self.start_xs[i];
let sy = self.start_ys[i];
let ex = self.end_xs[i];
let ey = self.end_ys[i];
let inv_len = self.inv_lengths[i];
let dx = ex - sx;
let dy = ey - sy;
let to_px = point.x - sx;
let to_py = point.y - sy;
// Distance
let cross = to_px * dy - to_py * dx;
distances[i] = cross.abs() * inv_len;
// Projection
let len_sq = dx * dx + dy * dy;
projections[i] = if len_sq > f32::EPSILON {
(to_px * dx + to_py * dy) / len_sq
} else {
0.5
};
}
}
/// Find the nearest line to a point using a reusable buffer.
///
/// Zero-allocation variant of `nearest_line()` for hot paths like ICP.
vastu-map/src/lib.rs
-1
View File
@@ -119,7 +119,6 @@ pub mod loop_closure;
pub mod matching;
pub mod motion_model;
pub mod query;
pub mod simd;
pub mod vector_map;
// Re-export main types at crate root
vastu-map/src/matching/correspondence.rs
+31 -14
View File
@@ -15,6 +15,29 @@
use crate::core::Point2D;
use crate::features::LineCollection;
/// Raw line data for correspondence creation when LineCollection isn't available.
///
/// This struct groups all line properties needed for a correspondence,
/// enabling the SoA structure to inline line data without requiring
/// a full LineCollection reference.
#[derive(Clone, Copy, Debug)]
pub struct LineDataRaw {
/// X component of line unit normal.
pub normal_x: f32,
/// Y component of line unit normal.
pub normal_y: f32,
/// X coordinate of line start point.
pub start_x: f32,
/// Y coordinate of line start point.
pub start_y: f32,
/// X coordinate of line end point.
pub end_x: f32,
/// Y coordinate of line end point.
pub end_y: f32,
/// Reciprocal of line length (1.0 / length).
pub inv_length: f32,
}
/// A correspondence between a scan point and a map line.
#[derive(Clone, Copy, Debug)]
pub struct Correspondence {
@@ -439,23 +462,17 @@ impl CorrespondenceSoA {
point: Point2D,
distance: f32,
weight: f32,
normal_x: f32,
normal_y: f32,
start_x: f32,
start_y: f32,
end_x: f32,
end_y: f32,
inv_length: f32,
line: LineDataRaw,
) {
self.point_xs.push(point.x);
self.point_ys.push(point.y);
self.normal_xs.push(normal_x);
self.normal_ys.push(normal_y);
self.line_start_xs.push(start_x);
self.line_start_ys.push(start_y);
self.line_end_xs.push(end_x);
self.line_end_ys.push(end_y);
self.line_inv_lengths.push(inv_length);
self.normal_xs.push(line.normal_x);
self.normal_ys.push(line.normal_y);
self.line_start_xs.push(line.start_x);
self.line_start_ys.push(line.start_y);
self.line_end_xs.push(line.end_x);
self.line_end_ys.push(line.end_y);
self.line_inv_lengths.push(line.inv_length);
self.weights.push(weight);
self.point_indices.push(point_idx);
self.line_indices.push(line_idx);
vastu-map/src/matching/mod.rs
+1 -1
View File
@@ -71,7 +71,7 @@ pub mod traits;
// Re-export main types
pub use correspondence::{
CornerCorrespondence, CornerCorrespondenceSet, Correspondence, CorrespondenceSet,
CorrespondenceSoA, IDENTITY_COVARIANCE, MatchResult, PoseCovariance,
CorrespondenceSoA, IDENTITY_COVARIANCE, LineDataRaw, MatchResult, PoseCovariance,
};
pub use gauss_newton::{
GaussNewtonConfig, GaussNewtonResult, optimize_pose_fast, optimize_pose_simd,
vastu-map/src/matching/scratch.rs
+60 -40
View File
@@ -13,6 +13,8 @@
//!
//! With scratch space, these allocations happen once and are reused.
use std::simd::f32x4;
use crate::config::LidarNoiseModel;
use crate::core::Point2D;
use crate::features::LineCollection;
@@ -182,21 +184,67 @@ impl IcpScratchSpace {
/// Transform points by pose into internal buffer.
///
/// SIMD-optimized: processes 4 points per iteration for better performance.
/// After calling this, `transformed_point()` can be used to access results.
#[inline]
pub fn transform_points(&mut self, points: &[Point2D], sin: f32, cos: f32, tx: f32, ty: f32) {
let n = points.len();
// Resize buffers (keeps capacity, avoids reallocation)
self.transformed_xs.clear();
self.transformed_ys.clear();
self.transformed_xs.resize(n, 0.0);
self.transformed_ys.resize(n, 0.0);
// Pre-extend to avoid reallocations during push
self.transformed_xs.reserve(points.len());
self.transformed_ys.reserve(points.len());
if n == 0 {
return;
}
for p in points {
let new_x = p.x * cos - p.y * sin + tx;
let new_y = p.x * sin + p.y * cos + ty;
self.transformed_xs.push(new_x);
self.transformed_ys.push(new_y);
// SIMD constants
let cos4 = f32x4::splat(cos);
let sin4 = f32x4::splat(sin);
let tx4 = f32x4::splat(tx);
let ty4 = f32x4::splat(ty);
// Process 4 points at a time
// Note: Point2D is stored as AoS, so we need to gather/scatter
let chunks = n / 4;
for i in 0..chunks {
let base = i * 4;
// Gather 4 points into separate x and y vectors
let px = f32x4::from_array([
points[base].x,
points[base + 1].x,
points[base + 2].x,
points[base + 3].x,
]);
let py = f32x4::from_array([
points[base].y,
points[base + 1].y,
points[base + 2].y,
points[base + 3].y,
]);
// Transform: new_x = x*cos - y*sin + tx
// new_y = x*sin + y*cos + ty
let new_x = px * cos4 - py * sin4 + tx4;
let new_y = px * sin4 + py * cos4 + ty4;
// Scatter to output buffers
self.transformed_xs[base..base + 4].copy_from_slice(&new_x.to_array());
self.transformed_ys[base..base + 4].copy_from_slice(&new_y.to_array());
}
// Handle remainder (scalar)
let remainder = chunks * 4;
for (p, (out_x, out_y)) in points[remainder..n].iter().zip(
self.transformed_xs[remainder..n]
.iter_mut()
.zip(&mut self.transformed_ys[remainder..n]),
) {
*out_x = p.x * cos - p.y * sin + tx;
*out_y = p.x * sin + p.y * cos + ty;
}
}
@@ -237,9 +285,8 @@ impl IcpScratchSpace {
self.transformed_ys[point_idx],
);
// Compute distances and projections into buffers
lines.distances_to_point_into(point, &mut self.distances);
self.compute_projections_into(point, lines);
// Compute distances and projections in a single pass (cache-friendly)
lines.distances_and_projections_into(point, &mut self.distances, &mut self.projections);
let mut best_distance = config.max_distance;
let mut best_corr: Option<Correspondence> = None;
@@ -278,32 +325,6 @@ impl IcpScratchSpace {
}
}
/// Compute projection parameters into the projections buffer.
///
/// Formula: t = dot(point - start, end - start) / |end - start|²
fn compute_projections_into(&mut self, point: Point2D, lines: &LineCollection) {
let n = lines.len();
for i in 0..n {
let sx = lines.start_xs[i];
let sy = lines.start_ys[i];
let ex = lines.end_xs[i];
let ey = lines.end_ys[i];
let dx = ex - sx;
let dy = ey - sy;
let len_sq = dx * dx + dy * dy;
if len_sq < f32::EPSILON {
self.projections[i] = 0.5; // Degenerate line
} else {
let to_px = point.x - sx;
let to_py = point.y - sy;
self.projections[i] = (to_px * dx + to_py * dy) / len_sq;
}
}
}
/// Take the correspondences out of the scratch space.
///
/// This moves the correspondence set, leaving an empty one in the scratch space.
@@ -376,9 +397,8 @@ impl IcpScratchSpace {
self.transformed_ys[point_idx],
);
// Compute distances and projections into buffers
lines.distances_to_point_into(point, &mut self.distances);
self.compute_projections_into(point, lines);
// Compute distances and projections in a single pass (cache-friendly)
lines.distances_and_projections_into(point, &mut self.distances, &mut self.projections);
// Compute weight for this point based on its range
let range = if point_idx < self.ranges.len() {
vastu-map/src/simd/mod.rs
-42
View File
@@ -1,42 +0,0 @@
//! SIMD utilities and documentation.
//!
//! This crate uses Rust's `std::simd` (portable_simd) for SIMD operations.
//! No custom wrapper types are needed - use `std::simd` types directly.
//!
//! # Usage
//!
//! ```rust,ignore
//! use std::simd::{f32x4, num::SimdFloat};
//!
//! // Contiguous load from slice
//! let xs = f32x4::from_slice(&data[base..]);
//!
//! // Broadcast scalar to all lanes
//! let scale = f32x4::splat(2.0);
//!
//! // Element-wise operations
//! let result = xs * scale;
//!
//! // Horizontal reduction
//! let sum = result.reduce_sum();
//! ```
//!
//! # Key Operations
//!
//! | Operation | Code |
//! |------------------------|-------------------------------|
//! | Load from array | `f32x4::from_array([a,b,c,d])` |
//! | Load from slice | `f32x4::from_slice(&slice[..])` |
//! | Broadcast scalar | `f32x4::splat(x)` |
//! | Horizontal sum | `.reduce_sum()` |
//! | Horizontal min/max | `.reduce_min()`, `.reduce_max()` |
//! | Element-wise min/max | `.simd_min(other)`, `.simd_max(other)` |
//! | Absolute value | `.abs()` |
//! | Store to array | `.to_array()` |
//!
//! # Performance Notes
//!
//! - Use contiguous `from_slice()` loads for best performance
//! - Avoid indexed/gather access in hot loops
//! - Pad arrays to SIMD width (4) for clean iteration
//! - Use `reduce_*` methods for horizontal operations