Migrate to std::simd and add SIMD-optimized SoA ICP
- Replace custom Float4 wrapper with std::simd::f32x4 (portable_simd) - Add CorrespondenceSoA with inlined line data for contiguous SIMD loads - Add optimize_pose_simd() and find_correspondences_soa() functions - Simplify cargo LLVM flags (explicit SIMD vs auto-vectorization) - Add rust-toolchain.toml for nightly channel requirement The SoA layout inlines line normals and endpoints at correspondence creation time, enabling ~4x faster Gauss-Newton optimization by avoiding indexed access in the hot loop.
This commit is contained in:
codetiger
@@ -5,27 +5,19 @@
|
||||
# - Cortex-A8, A9, A15 (ARMv7-A with NEON)
|
||||
# - Cortex-A53, A55, A72 (ARMv8-A, backward compatible)
|
||||
#
|
||||
# Performance Optimizations:
|
||||
# - target-cpu=cortex-a7: Tune scheduling for Cortex-A7
|
||||
# - +neon: Enable NEON SIMD (128-bit vectors)
|
||||
# - +vfpv4: Enable VFPv4 floating point with FMA
|
||||
# - opt-level=3: Maximum optimization
|
||||
# - llvm-args: Enable fast-math and loop vectorization
|
||||
# SIMD: Uses std::simd (portable_simd) with explicit f32x4 operations.
|
||||
# The +neon feature enables ARM NEON codegen for std::simd types.
|
||||
|
||||
[target.armv7-unknown-linux-musleabihf]
|
||||
linker = "arm-linux-gnueabihf-gcc"
|
||||
rustflags = [
|
||||
# CPU targeting
|
||||
# CPU targeting - enables NEON for std::simd
|
||||
"-C", "target-cpu=cortex-a7",
|
||||
"-C", "target-feature=+neon,+vfpv4",
|
||||
|
||||
# Optimization flags
|
||||
# Optimization
|
||||
"-C", "opt-level=3",
|
||||
|
||||
# LLVM optimization passes for better auto-vectorization
|
||||
"-C", "llvm-args=-enable-unsafe-fp-math",
|
||||
"-C", "llvm-args=-vectorize-loops",
|
||||
"-C", "llvm-args=-vectorize-slp",
|
||||
|
||||
# Linker
|
||||
"-C", "link-arg=-lgcc",
|
||||
|
||||
+4
-4
@@ -6,8 +6,8 @@ A high-performance feature-based 2D SLAM library with SIMD-friendly data layouts
|
||||
|
||||
- **VectorMap Representation**: Uses line segments and corners instead of occupancy grids for memory-efficient mapping
|
||||
- **Full SLAM Pipeline**: Localization and mapping integrated into a clean API
|
||||
- **SIMD Optimized**: Data layouts designed for LLVM auto-vectorization (works on ARM, x86, etc.)
|
||||
- **SoA Data Layout**: Struct-of-Arrays for cache-friendly SIMD operations
|
||||
- **SIMD Optimized**: Explicit `std::simd` operations with `f32x4` for portable SIMD (ARM NEON, x86 SSE/AVX)
|
||||
- **SoA Data Layout**: Struct-of-Arrays for cache-friendly contiguous SIMD loads
|
||||
|
||||
## Quick Start
|
||||
|
||||
@@ -90,7 +90,7 @@ println!("Pose: ({:.2}, {:.2}), confidence: {:.2}",
|
||||
| [`query`](src/query/) | Map queries (raycast, occupancy, frontier detection) | [README](src/query/README.md) |
|
||||
| [`vector_map`](src/vector_map/) | Main VectorMap SLAM implementation | [README](src/vector_map/README.md) |
|
||||
| [`loop_closure`](src/loop_closure/) | Keyframe-based loop closure detection | [README](src/loop_closure/README.md) |
|
||||
| [`simd`](src/simd/) | SIMD-friendly types for auto-vectorization | [README](src/simd/README.md) |
|
||||
| [`simd`](src/simd/) | SIMD documentation (uses `std::simd` / `portable_simd`) | — |
|
||||
|
||||
## Coordinate Frame
|
||||
|
||||
@@ -102,7 +102,7 @@ All coordinates follow the ROS REP-103 convention:
|
||||
|
||||
## SIMD and Portability
|
||||
|
||||
The library uses LLVM auto-vectorization patterns that work across different architectures. No platform-specific intrinsics are used—users can configure compiler flags appropriate for their target platform.
|
||||
The library uses Rust's `std::simd` (`portable_simd` feature) with explicit `f32x4` operations for portable SIMD. This maps to ARM NEON on ARM targets and SSE/AVX on x86 targets. No platform-specific intrinsics are used—the compiler handles the architecture-specific codegen based on target features.
|
||||
|
||||
## Building
|
||||
|
||||
|
||||
@@ -0,0 +1,2 @@
|
||||
[toolchain]
|
||||
channel = "nightly"
|
||||
@@ -89,4 +89,4 @@ AoS (typical): SoA (this implementation):
|
||||
[Point{x,y}]
|
||||
```
|
||||
|
||||
This enables 4-wide SIMD operations via auto-vectorization.
|
||||
This enables 4-wide SIMD operations via `std::simd::f32x4`.
|
||||
|
||||
+37
-68
@@ -6,9 +6,10 @@
|
||||
//!
|
||||
//! The SoA layout is optimized for SIMD auto-vectorization.
|
||||
|
||||
use std::simd::{f32x4, num::SimdFloat};
|
||||
|
||||
use super::point::Point2D;
|
||||
use super::pose::Pose2D;
|
||||
use crate::simd::Float4;
|
||||
|
||||
/// Raw lidar scan in polar coordinates.
|
||||
///
|
||||
@@ -189,10 +190,10 @@ impl PointCloud2D {
|
||||
|
||||
/// Transform the point cloud by a pose.
|
||||
///
|
||||
/// This is SIMD-optimized: processes 4 points per iteration using Float4.
|
||||
/// This is SIMD-optimized: processes 4 points per iteration using f32x4.
|
||||
///
|
||||
/// # Performance
|
||||
/// Uses fused multiply-add (FMA) operations for efficient SIMD execution.
|
||||
/// Uses explicit SIMD operations for efficient vectorization.
|
||||
pub fn transform(&self, pose: &Pose2D) -> Self {
|
||||
let n = self.len();
|
||||
let mut result = Self::with_capacity(n);
|
||||
@@ -200,41 +201,29 @@ impl PointCloud2D {
|
||||
result.ys.resize(n, 0.0);
|
||||
|
||||
let (sin, cos) = pose.theta.sin_cos();
|
||||
let cos4 = Float4::splat(cos);
|
||||
let sin4 = Float4::splat(sin);
|
||||
let tx4 = Float4::splat(pose.x);
|
||||
let ty4 = Float4::splat(pose.y);
|
||||
let cos4 = f32x4::splat(cos);
|
||||
let sin4 = f32x4::splat(sin);
|
||||
let tx4 = f32x4::splat(pose.x);
|
||||
let ty4 = f32x4::splat(pose.y);
|
||||
|
||||
// Process 4 points at a time (SIMD-friendly)
|
||||
let chunks = n / 4;
|
||||
for i in 0..chunks {
|
||||
let base = i * 4;
|
||||
|
||||
// Load 4 points
|
||||
let xs = Float4::new([
|
||||
self.xs[base],
|
||||
self.xs[base + 1],
|
||||
self.xs[base + 2],
|
||||
self.xs[base + 3],
|
||||
]);
|
||||
let ys = Float4::new([
|
||||
self.ys[base],
|
||||
self.ys[base + 1],
|
||||
self.ys[base + 2],
|
||||
self.ys[base + 3],
|
||||
]);
|
||||
// Load 4 points - contiguous SIMD loads
|
||||
let xs = f32x4::from_slice(&self.xs[base..]);
|
||||
let ys = f32x4::from_slice(&self.ys[base..]);
|
||||
|
||||
// Rotation + translation using FMA:
|
||||
// Rotation + translation:
|
||||
// x' = x*cos - y*sin + tx
|
||||
// y' = x*sin + y*cos + ty
|
||||
let new_xs = ys.neg_mul_add(sin4, xs.mul_add(cos4, tx4));
|
||||
let new_ys = xs.mul_add(sin4, ys.mul_add(cos4, ty4));
|
||||
let new_xs = xs * cos4 - ys * sin4 + tx4;
|
||||
let new_ys = xs * sin4 + ys * cos4 + ty4;
|
||||
|
||||
// Store results
|
||||
let new_xs_arr = new_xs.to_array();
|
||||
let new_ys_arr = new_ys.to_array();
|
||||
result.xs[base..base + 4].copy_from_slice(&new_xs_arr);
|
||||
result.ys[base..base + 4].copy_from_slice(&new_ys_arr);
|
||||
result.xs[base..base + 4].copy_from_slice(&new_xs.to_array());
|
||||
result.ys[base..base + 4].copy_from_slice(&new_ys.to_array());
|
||||
}
|
||||
|
||||
// Handle remainder (scalar)
|
||||
@@ -251,7 +240,7 @@ impl PointCloud2D {
|
||||
/// Compute bounding box of the point cloud.
|
||||
///
|
||||
/// Returns (min, max) corners, or None if cloud is empty.
|
||||
/// SIMD-optimized using Float4 min/max operations.
|
||||
/// SIMD-optimized using f32x4 min/max operations.
|
||||
pub fn bounds(&self) -> Option<(Point2D, Point2D)> {
|
||||
if self.is_empty() {
|
||||
return None;
|
||||
@@ -268,38 +257,28 @@ impl PointCloud2D {
|
||||
// SIMD processing of remaining points
|
||||
let chunks = (n - 1) / 4;
|
||||
if chunks > 0 {
|
||||
let mut min_x4 = Float4::splat(min_x);
|
||||
let mut max_x4 = Float4::splat(max_x);
|
||||
let mut min_y4 = Float4::splat(min_y);
|
||||
let mut max_y4 = Float4::splat(max_y);
|
||||
let mut min_x4 = f32x4::splat(min_x);
|
||||
let mut max_x4 = f32x4::splat(max_x);
|
||||
let mut min_y4 = f32x4::splat(min_y);
|
||||
let mut max_y4 = f32x4::splat(max_y);
|
||||
|
||||
for i in 0..chunks {
|
||||
let base = 1 + i * 4;
|
||||
|
||||
let xs = Float4::new([
|
||||
self.xs[base],
|
||||
self.xs[base + 1],
|
||||
self.xs[base + 2],
|
||||
self.xs[base + 3],
|
||||
]);
|
||||
let ys = Float4::new([
|
||||
self.ys[base],
|
||||
self.ys[base + 1],
|
||||
self.ys[base + 2],
|
||||
self.ys[base + 3],
|
||||
]);
|
||||
let xs = f32x4::from_slice(&self.xs[base..]);
|
||||
let ys = f32x4::from_slice(&self.ys[base..]);
|
||||
|
||||
min_x4 = min_x4.min(xs);
|
||||
max_x4 = max_x4.max(xs);
|
||||
min_y4 = min_y4.min(ys);
|
||||
max_y4 = max_y4.max(ys);
|
||||
min_x4 = min_x4.simd_min(xs);
|
||||
max_x4 = max_x4.simd_max(xs);
|
||||
min_y4 = min_y4.simd_min(ys);
|
||||
max_y4 = max_y4.simd_max(ys);
|
||||
}
|
||||
|
||||
// Horizontal reduction
|
||||
min_x = min_x4.horizontal_min();
|
||||
max_x = max_x4.horizontal_max();
|
||||
min_y = min_y4.horizontal_min();
|
||||
max_y = max_y4.horizontal_max();
|
||||
min_x = min_x4.reduce_min();
|
||||
max_x = max_x4.reduce_max();
|
||||
min_y = min_y4.reduce_min();
|
||||
max_y = max_y4.reduce_max();
|
||||
}
|
||||
|
||||
// Handle remainder (scalar)
|
||||
@@ -329,30 +308,20 @@ impl PointCloud2D {
|
||||
// SIMD accumulation
|
||||
let chunks = n / 4;
|
||||
if chunks > 0 {
|
||||
let mut sum_x4 = Float4::zero();
|
||||
let mut sum_y4 = Float4::zero();
|
||||
let mut sum_x4 = f32x4::splat(0.0);
|
||||
let mut sum_y4 = f32x4::splat(0.0);
|
||||
|
||||
for i in 0..chunks {
|
||||
let base = i * 4;
|
||||
let xs = Float4::new([
|
||||
self.xs[base],
|
||||
self.xs[base + 1],
|
||||
self.xs[base + 2],
|
||||
self.xs[base + 3],
|
||||
]);
|
||||
let ys = Float4::new([
|
||||
self.ys[base],
|
||||
self.ys[base + 1],
|
||||
self.ys[base + 2],
|
||||
self.ys[base + 3],
|
||||
]);
|
||||
let xs = f32x4::from_slice(&self.xs[base..]);
|
||||
let ys = f32x4::from_slice(&self.ys[base..]);
|
||||
|
||||
sum_x4 += xs;
|
||||
sum_y4 += ys;
|
||||
}
|
||||
|
||||
sum_x = sum_x4.sum();
|
||||
sum_y = sum_y4.sum();
|
||||
sum_x = sum_x4.reduce_sum();
|
||||
sum_y = sum_y4.reduce_sum();
|
||||
}
|
||||
|
||||
// Handle remainder
|
||||
|
||||
@@ -10,10 +10,16 @@
|
||||
//! Pre-computed cached values avoid redundant calculations in hot paths:
|
||||
//! - `normal_xs`, `normal_ys`: Unit normal vectors (avoids sqrt per distance calc)
|
||||
//! - `inv_lengths`: Reciprocal of line lengths (avoids division in inner loops)
|
||||
//!
|
||||
//! # SIMD Implementation
|
||||
//!
|
||||
//! Uses `std::simd::f32x4` for explicit 4-wide SIMD operations via the
|
||||
//! `portable_simd` feature.
|
||||
|
||||
use std::simd::{f32x4, num::SimdFloat};
|
||||
|
||||
use super::line::Line2D;
|
||||
use crate::core::{Point2D, Pose2D};
|
||||
use crate::simd::Float4;
|
||||
|
||||
/// Collection of line segments with SoA layout for SIMD operations.
|
||||
///
|
||||
@@ -229,47 +235,22 @@ impl LineCollection {
|
||||
let n = self.len();
|
||||
let mut distances = vec![0.0; n];
|
||||
|
||||
let px = Float4::splat(point.x);
|
||||
let py = Float4::splat(point.y);
|
||||
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
|
||||
let sx = Float4::new([
|
||||
self.start_xs[base],
|
||||
self.start_xs[base + 1],
|
||||
self.start_xs[base + 2],
|
||||
self.start_xs[base + 3],
|
||||
]);
|
||||
let sy = Float4::new([
|
||||
self.start_ys[base],
|
||||
self.start_ys[base + 1],
|
||||
self.start_ys[base + 2],
|
||||
self.start_ys[base + 3],
|
||||
]);
|
||||
let ex = Float4::new([
|
||||
self.end_xs[base],
|
||||
self.end_xs[base + 1],
|
||||
self.end_xs[base + 2],
|
||||
self.end_xs[base + 3],
|
||||
]);
|
||||
let ey = Float4::new([
|
||||
self.end_ys[base],
|
||||
self.end_ys[base + 1],
|
||||
self.end_ys[base + 2],
|
||||
self.end_ys[base + 3],
|
||||
]);
|
||||
// Load line data - contiguous SIMD loads
|
||||
let sx = f32x4::from_slice(&self.start_xs[base..]);
|
||||
let sy = f32x4::from_slice(&self.start_ys[base..]);
|
||||
let ex = f32x4::from_slice(&self.end_xs[base..]);
|
||||
let ey = f32x4::from_slice(&self.end_ys[base..]);
|
||||
|
||||
// Load pre-computed inverse lengths
|
||||
let inv_len = Float4::new([
|
||||
self.inv_lengths[base],
|
||||
self.inv_lengths[base + 1],
|
||||
self.inv_lengths[base + 2],
|
||||
self.inv_lengths[base + 3],
|
||||
]);
|
||||
let inv_len = f32x4::from_slice(&self.inv_lengths[base..]);
|
||||
|
||||
// Compute direction vectors
|
||||
let dx = ex - sx;
|
||||
@@ -284,14 +265,12 @@ impl LineCollection {
|
||||
let cross = to_px * dy - to_py * dx;
|
||||
|
||||
// Distance = |cross| * inv_length (no sqrt needed!)
|
||||
// Using SIMD abs() for better vectorization
|
||||
let abs_cross = cross.abs();
|
||||
let dist_4 = abs_cross * inv_len;
|
||||
distances[base..base + 4].copy_from_slice(&dist_4.to_array());
|
||||
}
|
||||
|
||||
// Handle remainder (scalar) using pre-computed values
|
||||
// Using index loop to access multiple parallel arrays at same position
|
||||
#[allow(clippy::needless_range_loop)]
|
||||
for i in (chunks * 4)..n {
|
||||
let sx = self.start_xs[i];
|
||||
@@ -327,39 +306,19 @@ impl LineCollection {
|
||||
let n = self.len();
|
||||
let mut projections = vec![0.0; n];
|
||||
|
||||
let px = Float4::splat(point.x);
|
||||
let py = Float4::splat(point.y);
|
||||
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
|
||||
let sx = Float4::new([
|
||||
self.start_xs[base],
|
||||
self.start_xs[base + 1],
|
||||
self.start_xs[base + 2],
|
||||
self.start_xs[base + 3],
|
||||
]);
|
||||
let sy = Float4::new([
|
||||
self.start_ys[base],
|
||||
self.start_ys[base + 1],
|
||||
self.start_ys[base + 2],
|
||||
self.start_ys[base + 3],
|
||||
]);
|
||||
let ex = Float4::new([
|
||||
self.end_xs[base],
|
||||
self.end_xs[base + 1],
|
||||
self.end_xs[base + 2],
|
||||
self.end_xs[base + 3],
|
||||
]);
|
||||
let ey = Float4::new([
|
||||
self.end_ys[base],
|
||||
self.end_ys[base + 1],
|
||||
self.end_ys[base + 2],
|
||||
self.end_ys[base + 3],
|
||||
]);
|
||||
// Load line data - contiguous SIMD loads
|
||||
let sx = f32x4::from_slice(&self.start_xs[base..]);
|
||||
let sy = f32x4::from_slice(&self.start_ys[base..]);
|
||||
let ex = f32x4::from_slice(&self.end_xs[base..]);
|
||||
let ey = f32x4::from_slice(&self.end_ys[base..]);
|
||||
|
||||
// Compute direction vectors
|
||||
let dx = ex - sx;
|
||||
@@ -437,47 +396,22 @@ impl LineCollection {
|
||||
n
|
||||
);
|
||||
|
||||
let px = Float4::splat(point.x);
|
||||
let py = Float4::splat(point.y);
|
||||
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
|
||||
let sx = Float4::new([
|
||||
self.start_xs[base],
|
||||
self.start_xs[base + 1],
|
||||
self.start_xs[base + 2],
|
||||
self.start_xs[base + 3],
|
||||
]);
|
||||
let sy = Float4::new([
|
||||
self.start_ys[base],
|
||||
self.start_ys[base + 1],
|
||||
self.start_ys[base + 2],
|
||||
self.start_ys[base + 3],
|
||||
]);
|
||||
let ex = Float4::new([
|
||||
self.end_xs[base],
|
||||
self.end_xs[base + 1],
|
||||
self.end_xs[base + 2],
|
||||
self.end_xs[base + 3],
|
||||
]);
|
||||
let ey = Float4::new([
|
||||
self.end_ys[base],
|
||||
self.end_ys[base + 1],
|
||||
self.end_ys[base + 2],
|
||||
self.end_ys[base + 3],
|
||||
]);
|
||||
// Load line data - contiguous SIMD loads
|
||||
let sx = f32x4::from_slice(&self.start_xs[base..]);
|
||||
let sy = f32x4::from_slice(&self.start_ys[base..]);
|
||||
let ex = f32x4::from_slice(&self.end_xs[base..]);
|
||||
let ey = f32x4::from_slice(&self.end_ys[base..]);
|
||||
|
||||
// Load pre-computed inverse lengths
|
||||
let inv_len = Float4::new([
|
||||
self.inv_lengths[base],
|
||||
self.inv_lengths[base + 1],
|
||||
self.inv_lengths[base + 2],
|
||||
self.inv_lengths[base + 3],
|
||||
]);
|
||||
let inv_len = f32x4::from_slice(&self.inv_lengths[base..]);
|
||||
|
||||
// Compute direction vectors
|
||||
let dx = ex - sx;
|
||||
@@ -492,14 +426,12 @@ impl LineCollection {
|
||||
let cross = to_px * dy - to_py * dx;
|
||||
|
||||
// Distance = |cross| * inv_length (no sqrt needed!)
|
||||
// Using SIMD abs() for better vectorization
|
||||
let abs_cross = cross.abs();
|
||||
let dist_4 = abs_cross * inv_len;
|
||||
buffer[base..base + 4].copy_from_slice(&dist_4.to_array());
|
||||
}
|
||||
|
||||
// Handle remainder (scalar) using pre-computed values
|
||||
// Using index loop to access multiple parallel arrays at same position
|
||||
#[allow(clippy::needless_range_loop)]
|
||||
for i in (chunks * 4)..n {
|
||||
let sx = self.start_xs[i];
|
||||
@@ -569,73 +501,45 @@ impl LineCollection {
|
||||
result.inv_lengths = self.inv_lengths.clone();
|
||||
|
||||
let (sin, cos) = pose.theta.sin_cos();
|
||||
let cos4 = Float4::splat(cos);
|
||||
let sin4 = Float4::splat(sin);
|
||||
let tx4 = Float4::splat(pose.x);
|
||||
let ty4 = Float4::splat(pose.y);
|
||||
let cos4 = f32x4::splat(cos);
|
||||
let sin4 = f32x4::splat(sin);
|
||||
let tx4 = f32x4::splat(pose.x);
|
||||
let ty4 = f32x4::splat(pose.y);
|
||||
|
||||
// Process 4 start points at a time
|
||||
let chunks = n / 4;
|
||||
for i in 0..chunks {
|
||||
let base = i * 4;
|
||||
|
||||
// Transform start points
|
||||
let sx = Float4::new([
|
||||
self.start_xs[base],
|
||||
self.start_xs[base + 1],
|
||||
self.start_xs[base + 2],
|
||||
self.start_xs[base + 3],
|
||||
]);
|
||||
let sy = Float4::new([
|
||||
self.start_ys[base],
|
||||
self.start_ys[base + 1],
|
||||
self.start_ys[base + 2],
|
||||
self.start_ys[base + 3],
|
||||
]);
|
||||
// Transform start points - contiguous SIMD loads
|
||||
let sx = f32x4::from_slice(&self.start_xs[base..]);
|
||||
let sy = f32x4::from_slice(&self.start_ys[base..]);
|
||||
|
||||
let new_sx = sy.neg_mul_add(sin4, sx.mul_add(cos4, tx4));
|
||||
let new_sy = sx.mul_add(sin4, sy.mul_add(cos4, ty4));
|
||||
// new_sx = sx*cos - sy*sin + tx
|
||||
// new_sy = sx*sin + sy*cos + ty
|
||||
let new_sx = sx * cos4 - sy * sin4 + tx4;
|
||||
let new_sy = sx * sin4 + sy * cos4 + ty4;
|
||||
|
||||
result.start_xs[base..base + 4].copy_from_slice(&new_sx.to_array());
|
||||
result.start_ys[base..base + 4].copy_from_slice(&new_sy.to_array());
|
||||
|
||||
// Transform end points
|
||||
let ex = Float4::new([
|
||||
self.end_xs[base],
|
||||
self.end_xs[base + 1],
|
||||
self.end_xs[base + 2],
|
||||
self.end_xs[base + 3],
|
||||
]);
|
||||
let ey = Float4::new([
|
||||
self.end_ys[base],
|
||||
self.end_ys[base + 1],
|
||||
self.end_ys[base + 2],
|
||||
self.end_ys[base + 3],
|
||||
]);
|
||||
let ex = f32x4::from_slice(&self.end_xs[base..]);
|
||||
let ey = f32x4::from_slice(&self.end_ys[base..]);
|
||||
|
||||
let new_ex = ey.neg_mul_add(sin4, ex.mul_add(cos4, tx4));
|
||||
let new_ey = ex.mul_add(sin4, ey.mul_add(cos4, ty4));
|
||||
let new_ex = ex * cos4 - ey * sin4 + tx4;
|
||||
let new_ey = ex * sin4 + ey * cos4 + ty4;
|
||||
|
||||
result.end_xs[base..base + 4].copy_from_slice(&new_ex.to_array());
|
||||
result.end_ys[base..base + 4].copy_from_slice(&new_ey.to_array());
|
||||
|
||||
// Rotate normals (no translation needed for unit vectors)
|
||||
let nx = Float4::new([
|
||||
self.normal_xs[base],
|
||||
self.normal_xs[base + 1],
|
||||
self.normal_xs[base + 2],
|
||||
self.normal_xs[base + 3],
|
||||
]);
|
||||
let ny = Float4::new([
|
||||
self.normal_ys[base],
|
||||
self.normal_ys[base + 1],
|
||||
self.normal_ys[base + 2],
|
||||
self.normal_ys[base + 3],
|
||||
]);
|
||||
let nx = f32x4::from_slice(&self.normal_xs[base..]);
|
||||
let ny = f32x4::from_slice(&self.normal_ys[base..]);
|
||||
|
||||
// Rotate normal: n' = R * n (no translation)
|
||||
let new_nx = ny.neg_mul_add(sin4, nx * cos4);
|
||||
let new_ny = nx.mul_add(sin4, ny * cos4);
|
||||
let new_nx = nx * cos4 - ny * sin4;
|
||||
let new_ny = nx * sin4 + ny * cos4;
|
||||
|
||||
result.normal_xs[base..base + 4].copy_from_slice(&new_nx.to_array());
|
||||
result.normal_ys[base..base + 4].copy_from_slice(&new_ny.to_array());
|
||||
|
||||
@@ -1,3 +1,5 @@
|
||||
#![feature(portable_simd)]
|
||||
|
||||
//! # Vastu-Map: Feature-Based 2D SLAM Library
|
||||
//!
|
||||
//! A high-performance SLAM (Simultaneous Localization and Mapping) library
|
||||
|
||||
@@ -6,10 +6,10 @@ Scan-to-map matching algorithms for robot localization.
|
||||
|
||||
| File | Description |
|
||||
|------|-------------|
|
||||
| `correspondence.rs` | Point-to-line correspondence types and match results |
|
||||
| `correspondence.rs` | Point-to-line correspondence types (AoS and SoA layouts) |
|
||||
| `nearest_neighbor.rs` | Find correspondences by distance |
|
||||
| `ransac.rs` | Robust pose estimation with outlier rejection |
|
||||
| `gauss_newton.rs` | Nonlinear least squares optimization |
|
||||
| `gauss_newton.rs` | Nonlinear least squares optimization (scalar and SIMD) |
|
||||
| `icp/` | Point-to-Line ICP implementation |
|
||||
| `scratch.rs` | Pre-allocated buffers for zero-allocation ICP |
|
||||
| `traits.rs` | `ScanMatcher` trait for extensibility |
|
||||
@@ -83,7 +83,11 @@ let config = GaussNewtonConfig {
|
||||
convergence_threshold: 1e-6,
|
||||
};
|
||||
|
||||
// Standard optimization with CorrespondenceSet
|
||||
let result = optimize_pose_fast(&correspondences, initial_pose, &config);
|
||||
|
||||
// SIMD-optimized with CorrespondenceSoA (f32x4 operations)
|
||||
let result = optimize_pose_simd(&correspondences_soa, initial_pose, &config);
|
||||
```
|
||||
|
||||
## Correspondence Finding
|
||||
@@ -105,6 +109,11 @@ let correspondences = find_correspondences_weighted(&points, &lines, max_distanc
|
||||
let correspondences = find_correspondences_with_angle(
|
||||
&points, &lines, max_distance, max_angle_diff
|
||||
);
|
||||
|
||||
// SoA layout for SIMD Gauss-Newton (inlines line data)
|
||||
let correspondences_soa = find_correspondences_soa(
|
||||
&points, &ranges, &line_collection, &noise_model, &config
|
||||
);
|
||||
```
|
||||
|
||||
## Extensibility
|
||||
|
||||
@@ -2,8 +2,18 @@
|
||||
//!
|
||||
//! A correspondence associates a point from the current scan with
|
||||
//! a line in the map.
|
||||
//!
|
||||
//! # Data Layouts
|
||||
//!
|
||||
//! Two layouts are provided:
|
||||
//! - [`CorrespondenceSet`]: Array-of-Structs (AoS) layout for general use
|
||||
//! - [`CorrespondenceSoA`]: Struct-of-Arrays (SoA) layout optimized for SIMD Gauss-Newton
|
||||
//!
|
||||
//! The SoA layout inlines line data (normals, endpoints) at correspondence creation time,
|
||||
//! enabling contiguous SIMD loads in the hot loop instead of random indexed access.
|
||||
|
||||
use crate::core::Point2D;
|
||||
use crate::features::LineCollection;
|
||||
|
||||
/// A correspondence between a scan point and a map line.
|
||||
#[derive(Clone, Copy, Debug)]
|
||||
@@ -260,6 +270,331 @@ impl CorrespondenceSet {
|
||||
}
|
||||
}
|
||||
|
||||
// ─────────────────────────────────────────────────────────────────────────────
|
||||
// CorrespondenceSoA: SIMD-Optimized Struct-of-Arrays Layout
|
||||
// ─────────────────────────────────────────────────────────────────────────────
|
||||
|
||||
/// SIMD-optimized correspondence collection with Struct-of-Arrays layout.
|
||||
///
|
||||
/// Unlike [`CorrespondenceSet`], this structure inlines line data (normals,
|
||||
/// endpoints, inverse lengths) at correspondence creation time. This enables
|
||||
/// contiguous SIMD loads in the Gauss-Newton hot loop instead of random
|
||||
/// indexed access to line arrays.
|
||||
///
|
||||
/// # Memory Layout
|
||||
///
|
||||
/// **HOT data** (used in Gauss-Newton inner loop):
|
||||
/// - Point positions (point_xs, point_ys)
|
||||
/// - Line normals (normal_xs, normal_ys) - INLINED, not indexed
|
||||
/// - Line endpoints (line_start_xs, line_start_ys, line_end_xs, line_end_ys)
|
||||
/// - Inverse lengths (line_inv_lengths) - for residual computation
|
||||
/// - Weights
|
||||
///
|
||||
/// **COLD data** (metadata, not used in optimization):
|
||||
/// - Point indices, line indices, distances
|
||||
///
|
||||
/// # SIMD Usage
|
||||
///
|
||||
/// Arrays are padded to SIMD width (4) with zero-weight entries via `finalize()`.
|
||||
/// Use `simd_chunks()` to get the number of 4-element chunks for iteration.
|
||||
///
|
||||
/// # Example
|
||||
///
|
||||
/// ```rust,ignore
|
||||
/// use vastu_map::matching::CorrespondenceSoA;
|
||||
///
|
||||
/// let mut soa = CorrespondenceSoA::new();
|
||||
/// // ... push correspondences ...
|
||||
/// soa.finalize(); // Pad to SIMD width
|
||||
///
|
||||
/// for i in 0..soa.simd_chunks() {
|
||||
/// let base = i * 4;
|
||||
/// let px = f32x4::from_slice(&soa.point_xs[base..]);
|
||||
/// let nx = f32x4::from_slice(&soa.normal_xs[base..]);
|
||||
/// // ... SIMD operations ...
|
||||
/// }
|
||||
/// ```
|
||||
#[derive(Clone, Debug, Default)]
|
||||
pub struct CorrespondenceSoA {
|
||||
// === HOT: Contiguous arrays for SIMD (used in Gauss-Newton) ===
|
||||
/// X coordinates of scan points (world frame).
|
||||
pub point_xs: Vec<f32>,
|
||||
/// Y coordinates of scan points (world frame).
|
||||
pub point_ys: Vec<f32>,
|
||||
/// X components of line unit normals (INLINED from LineCollection).
|
||||
pub normal_xs: Vec<f32>,
|
||||
/// Y components of line unit normals (INLINED from LineCollection).
|
||||
pub normal_ys: Vec<f32>,
|
||||
/// X coordinates of line start points.
|
||||
pub line_start_xs: Vec<f32>,
|
||||
/// Y coordinates of line start points.
|
||||
pub line_start_ys: Vec<f32>,
|
||||
/// X coordinates of line end points.
|
||||
pub line_end_xs: Vec<f32>,
|
||||
/// Y coordinates of line end points.
|
||||
pub line_end_ys: Vec<f32>,
|
||||
/// Reciprocal of line lengths (1.0 / length).
|
||||
pub line_inv_lengths: Vec<f32>,
|
||||
/// Correspondence weights (1/σ² from noise model).
|
||||
pub weights: Vec<f32>,
|
||||
|
||||
// === COLD: Metadata (not used in optimization) ===
|
||||
/// Indices of points in original scan.
|
||||
pub point_indices: Vec<usize>,
|
||||
/// Indices of lines in map.
|
||||
pub line_indices: Vec<usize>,
|
||||
/// Perpendicular distances from points to lines.
|
||||
pub distances: Vec<f32>,
|
||||
|
||||
/// Number of real correspondences (before padding).
|
||||
len: usize,
|
||||
}
|
||||
|
||||
/// SIMD lane width for f32x4.
|
||||
const SIMD_WIDTH: usize = 4;
|
||||
|
||||
impl CorrespondenceSoA {
|
||||
/// Create a new empty SoA correspondence collection.
|
||||
pub fn new() -> Self {
|
||||
Self::default()
|
||||
}
|
||||
|
||||
/// Create with capacity hint.
|
||||
pub fn with_capacity(capacity: usize) -> Self {
|
||||
Self {
|
||||
point_xs: Vec::with_capacity(capacity),
|
||||
point_ys: Vec::with_capacity(capacity),
|
||||
normal_xs: Vec::with_capacity(capacity),
|
||||
normal_ys: Vec::with_capacity(capacity),
|
||||
line_start_xs: Vec::with_capacity(capacity),
|
||||
line_start_ys: Vec::with_capacity(capacity),
|
||||
line_end_xs: Vec::with_capacity(capacity),
|
||||
line_end_ys: Vec::with_capacity(capacity),
|
||||
line_inv_lengths: Vec::with_capacity(capacity),
|
||||
weights: Vec::with_capacity(capacity),
|
||||
point_indices: Vec::with_capacity(capacity),
|
||||
line_indices: Vec::with_capacity(capacity),
|
||||
distances: Vec::with_capacity(capacity),
|
||||
len: 0,
|
||||
}
|
||||
}
|
||||
|
||||
/// Add a correspondence, inlining line data from the LineCollection.
|
||||
///
|
||||
/// This copies the line's normal, endpoints, and inverse length into the
|
||||
/// SoA arrays, eliminating indexed access in the Gauss-Newton hot loop.
|
||||
#[inline]
|
||||
pub fn push(
|
||||
&mut self,
|
||||
point_idx: usize,
|
||||
line_idx: usize,
|
||||
point: Point2D,
|
||||
distance: f32,
|
||||
weight: f32,
|
||||
lines: &LineCollection,
|
||||
) {
|
||||
self.point_xs.push(point.x);
|
||||
self.point_ys.push(point.y);
|
||||
|
||||
// Inline line data (no indexed access needed in hot loop)
|
||||
let (normal_xs, normal_ys) = lines.normals();
|
||||
self.normal_xs.push(normal_xs[line_idx]);
|
||||
self.normal_ys.push(normal_ys[line_idx]);
|
||||
self.line_start_xs.push(lines.start_xs[line_idx]);
|
||||
self.line_start_ys.push(lines.start_ys[line_idx]);
|
||||
self.line_end_xs.push(lines.end_xs[line_idx]);
|
||||
self.line_end_ys.push(lines.end_ys[line_idx]);
|
||||
self.line_inv_lengths.push(lines.inv_lengths()[line_idx]);
|
||||
self.weights.push(weight);
|
||||
|
||||
// Metadata
|
||||
self.point_indices.push(point_idx);
|
||||
self.line_indices.push(line_idx);
|
||||
self.distances.push(distance);
|
||||
|
||||
self.len += 1;
|
||||
}
|
||||
|
||||
/// Add a correspondence with explicit line data (for when LineCollection isn't available).
|
||||
#[inline]
|
||||
pub fn push_with_line_data(
|
||||
&mut self,
|
||||
point_idx: usize,
|
||||
line_idx: usize,
|
||||
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,
|
||||
) {
|
||||
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.weights.push(weight);
|
||||
self.point_indices.push(point_idx);
|
||||
self.line_indices.push(line_idx);
|
||||
self.distances.push(distance);
|
||||
self.len += 1;
|
||||
}
|
||||
|
||||
/// Number of real correspondences (before SIMD padding).
|
||||
#[inline]
|
||||
pub fn len(&self) -> usize {
|
||||
self.len
|
||||
}
|
||||
|
||||
/// Check if empty.
|
||||
#[inline]
|
||||
pub fn is_empty(&self) -> bool {
|
||||
self.len == 0
|
||||
}
|
||||
|
||||
/// Pad arrays to SIMD width (4) with zero-weight entries.
|
||||
///
|
||||
/// Call this after all correspondences have been added and before
|
||||
/// SIMD iteration. Zero-weight entries don't affect the optimization.
|
||||
pub fn finalize(&mut self) {
|
||||
let remainder = self.len % SIMD_WIDTH;
|
||||
if remainder == 0 {
|
||||
return;
|
||||
}
|
||||
|
||||
let padding = SIMD_WIDTH - remainder;
|
||||
for _ in 0..padding {
|
||||
self.point_xs.push(0.0);
|
||||
self.point_ys.push(0.0);
|
||||
self.normal_xs.push(0.0);
|
||||
self.normal_ys.push(1.0); // Arbitrary valid normal
|
||||
self.line_start_xs.push(0.0);
|
||||
self.line_start_ys.push(0.0);
|
||||
self.line_end_xs.push(0.0);
|
||||
self.line_end_ys.push(0.0);
|
||||
self.line_inv_lengths.push(0.0);
|
||||
self.weights.push(0.0); // Zero weight = no contribution
|
||||
self.point_indices.push(0);
|
||||
self.line_indices.push(0);
|
||||
self.distances.push(0.0);
|
||||
}
|
||||
}
|
||||
|
||||
/// Number of SIMD chunks (4-element groups) for iteration.
|
||||
///
|
||||
/// Call after `finalize()` for correct padding.
|
||||
#[inline]
|
||||
pub fn simd_chunks(&self) -> usize {
|
||||
self.point_xs.len().div_ceil(SIMD_WIDTH)
|
||||
}
|
||||
|
||||
/// Clear all correspondences (keeps capacity).
|
||||
pub fn clear(&mut self) {
|
||||
self.point_xs.clear();
|
||||
self.point_ys.clear();
|
||||
self.normal_xs.clear();
|
||||
self.normal_ys.clear();
|
||||
self.line_start_xs.clear();
|
||||
self.line_start_ys.clear();
|
||||
self.line_end_xs.clear();
|
||||
self.line_end_ys.clear();
|
||||
self.line_inv_lengths.clear();
|
||||
self.weights.clear();
|
||||
self.point_indices.clear();
|
||||
self.line_indices.clear();
|
||||
self.distances.clear();
|
||||
self.len = 0;
|
||||
}
|
||||
|
||||
/// Get the mean correspondence distance.
|
||||
pub fn mean_distance(&self) -> f32 {
|
||||
if self.len == 0 {
|
||||
return 0.0;
|
||||
}
|
||||
let sum: f32 = self.distances[..self.len].iter().sum();
|
||||
sum / self.len as f32
|
||||
}
|
||||
|
||||
/// Get the RMS (root mean square) correspondence distance.
|
||||
pub fn rms_distance(&self) -> f32 {
|
||||
if self.len == 0 {
|
||||
return 0.0;
|
||||
}
|
||||
let sum_sq: f32 = self.distances[..self.len].iter().map(|d| d * d).sum();
|
||||
(sum_sq / self.len as f32).sqrt()
|
||||
}
|
||||
|
||||
/// Get the weighted RMS correspondence distance.
|
||||
pub fn weighted_rms_distance(&self) -> f32 {
|
||||
if self.len == 0 {
|
||||
return 0.0;
|
||||
}
|
||||
let mut sum_weighted_sq = 0.0f32;
|
||||
let mut sum_weights = 0.0f32;
|
||||
for i in 0..self.len {
|
||||
let w = self.weights[i];
|
||||
let d = self.distances[i];
|
||||
sum_weighted_sq += w * d * d;
|
||||
sum_weights += w;
|
||||
}
|
||||
if sum_weights > 0.0 {
|
||||
(sum_weighted_sq / sum_weights).sqrt()
|
||||
} else {
|
||||
self.rms_distance()
|
||||
}
|
||||
}
|
||||
|
||||
/// Get the total weight of all correspondences.
|
||||
pub fn total_weight(&self) -> f32 {
|
||||
self.weights[..self.len].iter().sum()
|
||||
}
|
||||
|
||||
/// Get the maximum correspondence distance.
|
||||
pub fn max_distance(&self) -> f32 {
|
||||
self.distances[..self.len]
|
||||
.iter()
|
||||
.copied()
|
||||
.fold(0.0, f32::max)
|
||||
}
|
||||
|
||||
/// Compute inlier ratio (correspondences within threshold).
|
||||
pub fn inlier_ratio(&self, threshold: f32) -> f32 {
|
||||
if self.len == 0 {
|
||||
return 0.0;
|
||||
}
|
||||
let inliers = self.distances[..self.len]
|
||||
.iter()
|
||||
.filter(|&&d| d <= threshold)
|
||||
.count();
|
||||
inliers as f32 / self.len as f32
|
||||
}
|
||||
|
||||
/// Convert to CorrespondenceSet (for compatibility with existing code).
|
||||
pub fn to_correspondence_set(&self) -> CorrespondenceSet {
|
||||
let mut set = CorrespondenceSet::with_capacity(self.len);
|
||||
for i in 0..self.len {
|
||||
set.push(Correspondence::with_weight(
|
||||
self.point_indices[i],
|
||||
self.line_indices[i],
|
||||
Point2D::new(self.point_xs[i], self.point_ys[i]),
|
||||
self.distances[i],
|
||||
0.0, // projection_t not stored in SoA
|
||||
self.weights[i],
|
||||
0.0, // range not stored in SoA
|
||||
));
|
||||
}
|
||||
set
|
||||
}
|
||||
}
|
||||
|
||||
/// A correspondence between a scan point and a map corner.
|
||||
///
|
||||
/// Used for point-to-point constraints in ICP, which provide
|
||||
|
||||
@@ -17,10 +17,12 @@
|
||||
//! 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;
|
||||
use super::correspondence::{CorrespondenceSet, CorrespondenceSoA};
|
||||
|
||||
/// Configuration for Levenberg-Marquardt solver.
|
||||
///
|
||||
@@ -817,6 +819,298 @@ 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::*;
|
||||
|
||||
@@ -71,20 +71,23 @@ pub mod traits;
|
||||
// Re-export main types
|
||||
pub use correspondence::{
|
||||
CornerCorrespondence, CornerCorrespondenceSet, Correspondence, CorrespondenceSet,
|
||||
IDENTITY_COVARIANCE, MatchResult, PoseCovariance,
|
||||
CorrespondenceSoA, IDENTITY_COVARIANCE, 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, NearestNeighborConfig, find_corner_correspondences,
|
||||
find_corner_correspondences_weighted, find_correspondences, find_correspondences_batch,
|
||||
find_correspondences_spatial, find_correspondences_weighted, find_correspondences_with_angle,
|
||||
find_correspondences_soa, find_correspondences_soa_unweighted, find_correspondences_spatial,
|
||||
find_correspondences_weighted, find_correspondences_with_angle,
|
||||
};
|
||||
// Re-export from icp submodule
|
||||
pub use icp::{
|
||||
CoarseSearchConfig, IcpConfig, MultiResolutionConfig, PointToLineIcp, match_scan,
|
||||
match_scan_multiresolution, match_scan_with_config,
|
||||
};
|
||||
pub use ransac::{RansacConfig, RansacResult, estimate_pose_ransac};
|
||||
pub use ransac::{RansacConfig, RansacResult, RansacWorkspace, estimate_pose_ransac};
|
||||
pub use robust::RobustCostFunction;
|
||||
pub use scratch::IcpScratchSpace;
|
||||
// Re-export traits
|
||||
|
||||
@@ -24,6 +24,7 @@ use crate::integration::SpatialIndex;
|
||||
|
||||
use super::correspondence::{
|
||||
CornerCorrespondence, CornerCorrespondenceSet, Correspondence, CorrespondenceSet,
|
||||
CorrespondenceSoA,
|
||||
};
|
||||
|
||||
/// Configuration for nearest neighbor matching.
|
||||
@@ -341,6 +342,156 @@ pub fn find_correspondences_weighted(
|
||||
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,
|
||||
|
||||
@@ -98,6 +98,80 @@ pub struct RansacResult {
|
||||
pub iterations: usize,
|
||||
}
|
||||
|
||||
/// Pre-allocated workspace for RANSAC operations.
|
||||
///
|
||||
/// RANSAC allocates ~140KB per call for inlier tracking. This workspace
|
||||
/// pre-allocates buffers that can be reused across multiple RANSAC calls.
|
||||
///
|
||||
/// # Memory Savings
|
||||
///
|
||||
/// Without workspace, each RANSAC call allocates:
|
||||
/// - Remaining indices buffer: ~3KB (for 400 correspondences)
|
||||
/// - Inlier mask: ~400 bytes
|
||||
/// - Best inliers buffer: ~10KB
|
||||
/// - Per-iteration inlier set: ~10KB × iterations
|
||||
///
|
||||
/// With workspace, these are allocated once and reused.
|
||||
///
|
||||
/// # Example
|
||||
///
|
||||
/// ```rust,ignore
|
||||
/// use vastu_map::matching::{RansacWorkspace, estimate_pose_ransac_with_workspace};
|
||||
///
|
||||
/// let mut workspace = RansacWorkspace::new(400);
|
||||
///
|
||||
/// for correspondences in scan_correspondences {
|
||||
/// let result = estimate_pose_ransac_with_workspace(
|
||||
/// &correspondences, &lines, &config, &mut workspace
|
||||
/// );
|
||||
/// }
|
||||
/// ```
|
||||
#[derive(Clone, Debug)]
|
||||
pub struct RansacWorkspace {
|
||||
/// Buffer for tracking which correspondence indices remain.
|
||||
pub remaining_indices: Vec<usize>,
|
||||
/// Bitmask for inlier detection.
|
||||
pub inlier_mask: Vec<bool>,
|
||||
/// Buffer for storing best inlier indices.
|
||||
pub best_inliers: Vec<usize>,
|
||||
/// Reusable inlier correspondence set.
|
||||
pub inliers: CorrespondenceSet,
|
||||
}
|
||||
|
||||
impl RansacWorkspace {
|
||||
/// Create a new workspace with capacity hint.
|
||||
///
|
||||
/// # Arguments
|
||||
/// * `max_correspondences` - Expected maximum number of correspondences
|
||||
pub fn new(max_correspondences: usize) -> Self {
|
||||
Self {
|
||||
remaining_indices: Vec::with_capacity(max_correspondences),
|
||||
inlier_mask: Vec::with_capacity(max_correspondences),
|
||||
best_inliers: Vec::with_capacity(max_correspondences),
|
||||
inliers: CorrespondenceSet::with_capacity(max_correspondences),
|
||||
}
|
||||
}
|
||||
|
||||
/// Create with default capacity (400 correspondences).
|
||||
pub fn default_capacity() -> Self {
|
||||
Self::new(400)
|
||||
}
|
||||
|
||||
/// Clear all buffers for reuse (keeps capacity).
|
||||
pub fn clear(&mut self) {
|
||||
self.remaining_indices.clear();
|
||||
self.inlier_mask.clear();
|
||||
self.best_inliers.clear();
|
||||
self.inliers.clear();
|
||||
}
|
||||
}
|
||||
|
||||
impl Default for RansacWorkspace {
|
||||
fn default() -> Self {
|
||||
Self::default_capacity()
|
||||
}
|
||||
}
|
||||
|
||||
/// Estimate pose transformation using RANSAC.
|
||||
///
|
||||
/// Samples random subsets of correspondences, computes pose hypotheses,
|
||||
|
||||
@@ -13,10 +13,11 @@
|
||||
//!
|
||||
//! With scratch space, these allocations happen once and are reused.
|
||||
|
||||
use crate::config::LidarNoiseModel;
|
||||
use crate::core::Point2D;
|
||||
use crate::features::LineCollection;
|
||||
|
||||
use super::correspondence::{Correspondence, CorrespondenceSet};
|
||||
use super::correspondence::{Correspondence, CorrespondenceSet, CorrespondenceSoA};
|
||||
use super::nearest_neighbor::NearestNeighborConfig;
|
||||
|
||||
/// Pre-allocated scratch space for ICP operations.
|
||||
@@ -48,8 +49,14 @@ pub struct IcpScratchSpace {
|
||||
/// Buffer for projection parameters (per point).
|
||||
pub(crate) projections: Vec<f32>,
|
||||
|
||||
/// Reusable correspondence set.
|
||||
/// Reusable correspondence set (AoS layout).
|
||||
pub(crate) correspondences: CorrespondenceSet,
|
||||
|
||||
/// Reusable correspondence set (SoA layout for SIMD).
|
||||
pub(crate) correspondences_soa: CorrespondenceSoA,
|
||||
|
||||
/// Buffer for ranges (distances from sensor).
|
||||
pub(crate) ranges: Vec<f32>,
|
||||
}
|
||||
|
||||
impl IcpScratchSpace {
|
||||
@@ -65,6 +72,8 @@ impl IcpScratchSpace {
|
||||
distances: Vec::with_capacity(max_lines),
|
||||
projections: Vec::with_capacity(max_lines),
|
||||
correspondences: CorrespondenceSet::with_capacity(max_points),
|
||||
correspondences_soa: CorrespondenceSoA::with_capacity(max_points),
|
||||
ranges: Vec::with_capacity(max_points),
|
||||
}
|
||||
}
|
||||
|
||||
@@ -81,6 +90,8 @@ impl IcpScratchSpace {
|
||||
self.distances.clear();
|
||||
self.projections.clear();
|
||||
self.correspondences.clear();
|
||||
self.correspondences_soa.clear();
|
||||
self.ranges.clear();
|
||||
}
|
||||
|
||||
/// Transform points by pose into internal buffer.
|
||||
@@ -227,6 +238,133 @@ impl IcpScratchSpace {
|
||||
pub fn num_correspondences(&self) -> usize {
|
||||
self.correspondences.len()
|
||||
}
|
||||
|
||||
// =========================================================================
|
||||
// SIMD-optimized SoA correspondence finding
|
||||
// =========================================================================
|
||||
|
||||
/// Store ranges for later use in SoA correspondence finding.
|
||||
#[inline]
|
||||
pub fn store_ranges(&mut self, points: &[Point2D]) {
|
||||
self.ranges.clear();
|
||||
self.ranges.reserve(points.len());
|
||||
for p in points {
|
||||
self.ranges.push((p.x * p.x + p.y * p.y).sqrt());
|
||||
}
|
||||
}
|
||||
|
||||
/// Find correspondences using pre-allocated buffers, producing SoA layout.
|
||||
///
|
||||
/// This is the SIMD-optimized hot path for ICP iterations.
|
||||
/// The result is stored in `self.correspondences_soa` and is ready for
|
||||
/// use with `optimize_pose_simd()`.
|
||||
///
|
||||
/// # Arguments
|
||||
/// * `lines` - Map lines in SoA format
|
||||
/// * `noise_model` - Lidar noise model for weight computation
|
||||
/// * `config` - Matching configuration
|
||||
pub fn find_correspondences_soa(
|
||||
&mut self,
|
||||
lines: &LineCollection,
|
||||
noise_model: &LidarNoiseModel,
|
||||
config: &NearestNeighborConfig,
|
||||
) {
|
||||
self.correspondences_soa.clear();
|
||||
|
||||
if self.transformed_xs.is_empty() || lines.is_empty() {
|
||||
return;
|
||||
}
|
||||
|
||||
let min_t = -config.max_projection_extension;
|
||||
let max_t = 1.0 + config.max_projection_extension;
|
||||
let n_lines = lines.len();
|
||||
let n_points = self.transformed_xs.len();
|
||||
|
||||
// Ensure buffers are large enough
|
||||
self.distances.resize(n_lines, 0.0);
|
||||
self.projections.resize(n_lines, 0.0);
|
||||
|
||||
for point_idx in 0..n_points {
|
||||
let point = Point2D::new(
|
||||
self.transformed_xs[point_idx],
|
||||
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 weight for this point based on its range
|
||||
let range = if point_idx < self.ranges.len() {
|
||||
self.ranges[point_idx]
|
||||
} else {
|
||||
1.0 // Default weight factor
|
||||
};
|
||||
let weight = noise_model.weight(range);
|
||||
|
||||
let mut best_distance = config.max_distance;
|
||||
let mut best_line_idx: Option<usize> = None;
|
||||
|
||||
for line_idx in 0..n_lines {
|
||||
let t = self.projections[line_idx];
|
||||
let distance = self.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 {
|
||||
self.correspondences_soa
|
||||
.push(point_idx, line_idx, point, distance, weight, lines);
|
||||
}
|
||||
}
|
||||
|
||||
if config.unique_per_point
|
||||
&& let Some(line_idx) = best_line_idx
|
||||
{
|
||||
self.correspondences_soa.push(
|
||||
point_idx,
|
||||
line_idx,
|
||||
point,
|
||||
best_distance,
|
||||
weight,
|
||||
lines,
|
||||
);
|
||||
}
|
||||
}
|
||||
|
||||
// Finalize with SIMD padding
|
||||
self.correspondences_soa.finalize();
|
||||
}
|
||||
|
||||
/// Get a reference to the current SoA correspondences.
|
||||
#[inline]
|
||||
pub fn correspondences_soa(&self) -> &CorrespondenceSoA {
|
||||
&self.correspondences_soa
|
||||
}
|
||||
|
||||
/// Get the number of SoA correspondences found.
|
||||
#[inline]
|
||||
pub fn num_correspondences_soa(&self) -> usize {
|
||||
self.correspondences_soa.len()
|
||||
}
|
||||
|
||||
/// Take the SoA correspondences out of the scratch space.
|
||||
#[inline]
|
||||
pub fn take_correspondences_soa(&mut self) -> CorrespondenceSoA {
|
||||
std::mem::take(&mut self.correspondences_soa)
|
||||
}
|
||||
}
|
||||
|
||||
impl Default for IcpScratchSpace {
|
||||
|
||||
@@ -1,93 +0,0 @@
|
||||
# SIMD Module
|
||||
|
||||
SIMD-friendly types designed for LLVM auto-vectorization.
|
||||
|
||||
## Components
|
||||
|
||||
| File | Description |
|
||||
|------|-------------|
|
||||
| `f32x4.rs` | `Float4` - 4-wide float vector for SIMD operations |
|
||||
|
||||
## Design Philosophy
|
||||
|
||||
We use LLVM auto-vectorization rather than platform-specific intrinsics:
|
||||
|
||||
- **Portability**: Works on ARM NEON, x86 SSE/AVX, and other architectures
|
||||
- **Simplicity**: No unsafe intrinsics or target-specific code
|
||||
- **Maintainability**: Standard Rust that's easy to debug
|
||||
|
||||
Users can configure appropriate compiler flags for their target platform.
|
||||
|
||||
## Float4
|
||||
|
||||
4-wide float vector aligned for SIMD:
|
||||
|
||||
```rust
|
||||
use vastu_map::simd::Float4;
|
||||
|
||||
// Create vectors
|
||||
let a = Float4::new([1.0, 2.0, 3.0, 4.0]);
|
||||
let b = Float4::splat(2.0); // [2.0, 2.0, 2.0, 2.0]
|
||||
|
||||
// Arithmetic
|
||||
let sum = a + b; // Element-wise add
|
||||
let diff = a - b; // Element-wise subtract
|
||||
let prod = a * b; // Element-wise multiply
|
||||
|
||||
// Fused multiply-add
|
||||
let result = a.mul_add(b, c); // a * b + c
|
||||
let result = a.neg_mul_add(b, c); // c - a * b
|
||||
|
||||
// Reduction
|
||||
let sum = a.sum(); // Sum all elements
|
||||
let min = a.min_element(); // Minimum element
|
||||
let max = a.max_element(); // Maximum element
|
||||
|
||||
// Element access
|
||||
let x = a[0];
|
||||
let arr: [f32; 4] = a.into();
|
||||
```
|
||||
|
||||
## Example: Point Transform
|
||||
|
||||
```rust
|
||||
// Transform 4 points at once
|
||||
let xs = Float4::new([1.0, 2.0, 3.0, 4.0]);
|
||||
let ys = Float4::new([0.0, 0.0, 0.0, 0.0]);
|
||||
let cos_theta = Float4::splat(0.866); // cos(30deg)
|
||||
let sin_theta = Float4::splat(0.5); // sin(30deg)
|
||||
let tx = Float4::splat(1.0);
|
||||
let ty = Float4::splat(2.0);
|
||||
|
||||
// Rotation: x' = x*cos - y*sin + tx
|
||||
// y' = x*sin + y*cos + ty
|
||||
let new_xs = ys.neg_mul_add(sin_theta, xs * cos_theta) + tx;
|
||||
let new_ys = xs.mul_add(sin_theta, ys * cos_theta) + ty;
|
||||
```
|
||||
|
||||
## Data Layout Patterns
|
||||
|
||||
For best SIMD utilization, use Struct-of-Arrays (SoA):
|
||||
|
||||
```
|
||||
AoS (poor SIMD): SoA (good SIMD):
|
||||
[Point{x,y}] xs: [x0, x1, x2, x3, ...]
|
||||
[Point{x,y}] → ys: [y0, y1, y2, y3, ...]
|
||||
[Point{x,y}]
|
||||
[Point{x,y}]
|
||||
```
|
||||
|
||||
SoA allows processing 4 coordinates per SIMD instruction:
|
||||
|
||||
```rust
|
||||
// Process 4 points at once
|
||||
for i in (0..xs.len()).step_by(4) {
|
||||
let x = Float4::new([xs[i], xs[i+1], xs[i+2], xs[i+3]]);
|
||||
let y = Float4::new([ys[i], ys[i+1], ys[i+2], ys[i+3]]);
|
||||
// Transform all 4 points simultaneously
|
||||
}
|
||||
```
|
||||
|
||||
## Alignment
|
||||
|
||||
`Float4` is 16-byte aligned (128-bit), matching common SIMD register sizes. This ensures optimal memory access patterns and enables aligned loads/stores.
|
||||
@@ -1,470 +0,0 @@
|
||||
//! 4-wide f32 SIMD vector type.
|
||||
//!
|
||||
//! This implementation is designed for LLVM auto-vectorization.
|
||||
//! The struct uses 16-byte alignment to match common 128-bit SIMD registers.
|
||||
//!
|
||||
//! # Design
|
||||
//!
|
||||
//! Uses standard Rust operations that LLVM can auto-vectorize for the target
|
||||
//! platform. No platform-specific intrinsics are used—users can configure
|
||||
//! appropriate compiler flags for their target architecture.
|
||||
|
||||
use std::ops::{Add, AddAssign, Mul, Sub};
|
||||
|
||||
/// A 4-wide f32 vector optimized for SIMD auto-vectorization.
|
||||
///
|
||||
/// # Memory Layout
|
||||
///
|
||||
/// Uses `#[repr(C, align(16))]` to ensure:
|
||||
/// - C-compatible memory layout for predictable access patterns
|
||||
/// - 16-byte alignment matching common 128-bit SIMD registers
|
||||
///
|
||||
/// # Auto-vectorization
|
||||
///
|
||||
/// All methods use `#[inline(always)]` to encourage LLVM to inline
|
||||
/// and recognize vectorizable patterns for the target architecture.
|
||||
#[derive(Clone, Copy, Debug, PartialEq)]
|
||||
#[repr(C, align(16))]
|
||||
pub struct Float4(pub [f32; 4]);
|
||||
|
||||
impl Float4 {
|
||||
/// Creates a new `Float4` from an array of 4 f32 values.
|
||||
///
|
||||
/// # Example
|
||||
/// ```
|
||||
/// use vastu_map::simd::Float4;
|
||||
/// let v = Float4::new([1.0, 2.0, 3.0, 4.0]);
|
||||
/// ```
|
||||
#[inline(always)]
|
||||
pub const fn new(arr: [f32; 4]) -> Self {
|
||||
Self(arr)
|
||||
}
|
||||
|
||||
/// Creates a `Float4` with all elements set to the same value.
|
||||
///
|
||||
/// # Example
|
||||
/// ```
|
||||
/// use vastu_map::simd::Float4;
|
||||
/// let v = Float4::splat(5.0);
|
||||
/// assert_eq!(v.to_array(), [5.0, 5.0, 5.0, 5.0]);
|
||||
/// ```
|
||||
#[inline(always)]
|
||||
pub const fn splat(v: f32) -> Self {
|
||||
Self([v, v, v, v])
|
||||
}
|
||||
|
||||
/// Creates a `Float4` with all elements set to zero.
|
||||
#[inline(always)]
|
||||
pub const fn zero() -> Self {
|
||||
Self([0.0, 0.0, 0.0, 0.0])
|
||||
}
|
||||
|
||||
/// Extracts the elements as an array.
|
||||
///
|
||||
/// # Example
|
||||
/// ```
|
||||
/// use vastu_map::simd::Float4;
|
||||
/// let v = Float4::new([1.0, 2.0, 3.0, 4.0]);
|
||||
/// let arr = v.to_array();
|
||||
/// assert_eq!(arr, [1.0, 2.0, 3.0, 4.0]);
|
||||
/// ```
|
||||
#[inline(always)]
|
||||
pub const fn to_array(self) -> [f32; 4] {
|
||||
self.0
|
||||
}
|
||||
|
||||
/// Element-wise minimum of two vectors.
|
||||
///
|
||||
/// Returns a new vector where each element is the minimum of the
|
||||
/// corresponding elements in `self` and `other`.
|
||||
///
|
||||
/// # Example
|
||||
/// ```
|
||||
/// use vastu_map::simd::Float4;
|
||||
/// let a = Float4::new([1.0, 5.0, 3.0, 8.0]);
|
||||
/// let b = Float4::new([2.0, 3.0, 4.0, 1.0]);
|
||||
/// let min = a.min(b);
|
||||
/// assert_eq!(min.to_array(), [1.0, 3.0, 3.0, 1.0]);
|
||||
/// ```
|
||||
#[inline(always)]
|
||||
pub fn min(self, other: Self) -> Self {
|
||||
Self([
|
||||
self.0[0].min(other.0[0]),
|
||||
self.0[1].min(other.0[1]),
|
||||
self.0[2].min(other.0[2]),
|
||||
self.0[3].min(other.0[3]),
|
||||
])
|
||||
}
|
||||
|
||||
/// Element-wise maximum of two vectors.
|
||||
///
|
||||
/// Returns a new vector where each element is the maximum of the
|
||||
/// corresponding elements in `self` and `other`.
|
||||
///
|
||||
/// # Example
|
||||
/// ```
|
||||
/// use vastu_map::simd::Float4;
|
||||
/// let a = Float4::new([1.0, 5.0, 3.0, 8.0]);
|
||||
/// let b = Float4::new([2.0, 3.0, 4.0, 1.0]);
|
||||
/// let max = a.max(b);
|
||||
/// assert_eq!(max.to_array(), [2.0, 5.0, 4.0, 8.0]);
|
||||
/// ```
|
||||
#[inline(always)]
|
||||
pub fn max(self, other: Self) -> Self {
|
||||
Self([
|
||||
self.0[0].max(other.0[0]),
|
||||
self.0[1].max(other.0[1]),
|
||||
self.0[2].max(other.0[2]),
|
||||
self.0[3].max(other.0[3]),
|
||||
])
|
||||
}
|
||||
|
||||
/// Fused multiply-add: `(self * a) + b`.
|
||||
///
|
||||
/// Computes `self * a + b` with better precision and performance than
|
||||
/// separate multiply and add operations.
|
||||
///
|
||||
/// # Example
|
||||
/// ```
|
||||
/// use vastu_map::simd::Float4;
|
||||
/// let a = Float4::new([1.0, 2.0, 3.0, 4.0]);
|
||||
/// let b = Float4::new([2.0, 2.0, 2.0, 2.0]);
|
||||
/// let c = Float4::new([10.0, 10.0, 10.0, 10.0]);
|
||||
/// let result = a.mul_add(b, c);
|
||||
/// // (1*2+10, 2*2+10, 3*2+10, 4*2+10) = (12, 14, 16, 18)
|
||||
/// assert_eq!(result.to_array(), [12.0, 14.0, 16.0, 18.0]);
|
||||
/// ```
|
||||
#[inline(always)]
|
||||
pub fn mul_add(self, a: Self, b: Self) -> Self {
|
||||
Self([
|
||||
self.0[0].mul_add(a.0[0], b.0[0]),
|
||||
self.0[1].mul_add(a.0[1], b.0[1]),
|
||||
self.0[2].mul_add(a.0[2], b.0[2]),
|
||||
self.0[3].mul_add(a.0[3], b.0[3]),
|
||||
])
|
||||
}
|
||||
|
||||
/// Fused negative multiply-add: `-(self * a) + b` = `b - (self * a)`.
|
||||
///
|
||||
/// Useful for computing expressions like `tx - xs * sin_theta`.
|
||||
///
|
||||
/// # Example
|
||||
/// ```
|
||||
/// use vastu_map::simd::Float4;
|
||||
/// let a = Float4::new([1.0, 2.0, 3.0, 4.0]);
|
||||
/// let b = Float4::new([2.0, 2.0, 2.0, 2.0]);
|
||||
/// let c = Float4::new([10.0, 10.0, 10.0, 10.0]);
|
||||
/// let result = a.neg_mul_add(b, c);
|
||||
/// // (10-1*2, 10-2*2, 10-3*2, 10-4*2) = (8, 6, 4, 2)
|
||||
/// assert_eq!(result.to_array(), [8.0, 6.0, 4.0, 2.0]);
|
||||
/// ```
|
||||
#[inline(always)]
|
||||
pub fn neg_mul_add(self, a: Self, b: Self) -> Self {
|
||||
Self([
|
||||
(-self.0[0]).mul_add(a.0[0], b.0[0]),
|
||||
(-self.0[1]).mul_add(a.0[1], b.0[1]),
|
||||
(-self.0[2]).mul_add(a.0[2], b.0[2]),
|
||||
(-self.0[3]).mul_add(a.0[3], b.0[3]),
|
||||
])
|
||||
}
|
||||
|
||||
/// Horizontal sum of all elements.
|
||||
///
|
||||
/// Returns the sum of all 4 elements in the vector.
|
||||
#[inline(always)]
|
||||
pub fn sum(self) -> f32 {
|
||||
self.0[0] + self.0[1] + self.0[2] + self.0[3]
|
||||
}
|
||||
|
||||
/// Horizontal minimum of all elements.
|
||||
///
|
||||
/// Returns the minimum of all 4 elements in the vector.
|
||||
#[inline(always)]
|
||||
pub fn horizontal_min(self) -> f32 {
|
||||
self.0[0].min(self.0[1]).min(self.0[2]).min(self.0[3])
|
||||
}
|
||||
|
||||
/// Horizontal maximum of all elements.
|
||||
///
|
||||
/// Returns the maximum of all 4 elements in the vector.
|
||||
#[inline(always)]
|
||||
pub fn horizontal_max(self) -> f32 {
|
||||
self.0[0].max(self.0[1]).max(self.0[2]).max(self.0[3])
|
||||
}
|
||||
|
||||
/// Element-wise absolute value.
|
||||
///
|
||||
/// Returns a new vector where each element is the absolute value of
|
||||
/// the corresponding element in `self`.
|
||||
///
|
||||
/// # Example
|
||||
/// ```
|
||||
/// use vastu_map::simd::Float4;
|
||||
/// let v = Float4::new([-1.0, 2.0, -3.0, 4.0]);
|
||||
/// let abs = v.abs();
|
||||
/// assert_eq!(abs.to_array(), [1.0, 2.0, 3.0, 4.0]);
|
||||
/// ```
|
||||
#[inline(always)]
|
||||
pub fn abs(self) -> Self {
|
||||
Self([
|
||||
self.0[0].abs(),
|
||||
self.0[1].abs(),
|
||||
self.0[2].abs(),
|
||||
self.0[3].abs(),
|
||||
])
|
||||
}
|
||||
|
||||
/// Element-wise reciprocal (1.0 / x).
|
||||
///
|
||||
/// Returns a new vector where each element is the reciprocal of
|
||||
/// the corresponding element in `self`.
|
||||
///
|
||||
/// # Example
|
||||
/// ```
|
||||
/// use vastu_map::simd::Float4;
|
||||
/// let v = Float4::new([1.0, 2.0, 4.0, 8.0]);
|
||||
/// let recip = v.recip();
|
||||
/// assert_eq!(recip.to_array(), [1.0, 0.5, 0.25, 0.125]);
|
||||
/// ```
|
||||
#[inline(always)]
|
||||
pub fn recip(self) -> Self {
|
||||
Self([
|
||||
1.0 / self.0[0],
|
||||
1.0 / self.0[1],
|
||||
1.0 / self.0[2],
|
||||
1.0 / self.0[3],
|
||||
])
|
||||
}
|
||||
}
|
||||
|
||||
impl Add for Float4 {
|
||||
type Output = Self;
|
||||
|
||||
/// Element-wise addition.
|
||||
#[inline(always)]
|
||||
fn add(self, rhs: Self) -> Self {
|
||||
Self([
|
||||
self.0[0] + rhs.0[0],
|
||||
self.0[1] + rhs.0[1],
|
||||
self.0[2] + rhs.0[2],
|
||||
self.0[3] + rhs.0[3],
|
||||
])
|
||||
}
|
||||
}
|
||||
|
||||
impl Sub for Float4 {
|
||||
type Output = Self;
|
||||
|
||||
/// Element-wise subtraction.
|
||||
#[inline(always)]
|
||||
fn sub(self, rhs: Self) -> Self {
|
||||
Self([
|
||||
self.0[0] - rhs.0[0],
|
||||
self.0[1] - rhs.0[1],
|
||||
self.0[2] - rhs.0[2],
|
||||
self.0[3] - rhs.0[3],
|
||||
])
|
||||
}
|
||||
}
|
||||
|
||||
impl Mul for Float4 {
|
||||
type Output = Self;
|
||||
|
||||
/// Element-wise multiplication.
|
||||
#[inline(always)]
|
||||
fn mul(self, rhs: Self) -> Self {
|
||||
Self([
|
||||
self.0[0] * rhs.0[0],
|
||||
self.0[1] * rhs.0[1],
|
||||
self.0[2] * rhs.0[2],
|
||||
self.0[3] * rhs.0[3],
|
||||
])
|
||||
}
|
||||
}
|
||||
|
||||
impl AddAssign for Float4 {
|
||||
/// In-place element-wise addition.
|
||||
#[inline(always)]
|
||||
fn add_assign(&mut self, rhs: Self) {
|
||||
self.0[0] += rhs.0[0];
|
||||
self.0[1] += rhs.0[1];
|
||||
self.0[2] += rhs.0[2];
|
||||
self.0[3] += rhs.0[3];
|
||||
}
|
||||
}
|
||||
|
||||
impl Default for Float4 {
|
||||
fn default() -> Self {
|
||||
Self::zero()
|
||||
}
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
mod tests {
|
||||
use super::*;
|
||||
|
||||
#[test]
|
||||
fn test_new_and_to_array() {
|
||||
let v = Float4::new([1.0, 2.0, 3.0, 4.0]);
|
||||
assert_eq!(v.to_array(), [1.0, 2.0, 3.0, 4.0]);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_splat() {
|
||||
let v = Float4::splat(5.0);
|
||||
assert_eq!(v.to_array(), [5.0, 5.0, 5.0, 5.0]);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_zero() {
|
||||
let v = Float4::zero();
|
||||
assert_eq!(v.to_array(), [0.0, 0.0, 0.0, 0.0]);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_add() {
|
||||
let a = Float4::new([1.0, 2.0, 3.0, 4.0]);
|
||||
let b = Float4::new([5.0, 6.0, 7.0, 8.0]);
|
||||
let sum = a + b;
|
||||
assert_eq!(sum.to_array(), [6.0, 8.0, 10.0, 12.0]);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_sub() {
|
||||
let a = Float4::new([5.0, 6.0, 7.0, 8.0]);
|
||||
let b = Float4::new([1.0, 2.0, 3.0, 4.0]);
|
||||
let diff = a - b;
|
||||
assert_eq!(diff.to_array(), [4.0, 4.0, 4.0, 4.0]);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_mul() {
|
||||
let a = Float4::new([1.0, 2.0, 3.0, 4.0]);
|
||||
let b = Float4::new([2.0, 3.0, 4.0, 5.0]);
|
||||
let prod = a * b;
|
||||
assert_eq!(prod.to_array(), [2.0, 6.0, 12.0, 20.0]);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_add_assign() {
|
||||
let mut a = Float4::new([1.0, 2.0, 3.0, 4.0]);
|
||||
let b = Float4::new([5.0, 6.0, 7.0, 8.0]);
|
||||
a += b;
|
||||
assert_eq!(a.to_array(), [6.0, 8.0, 10.0, 12.0]);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_min() {
|
||||
let a = Float4::new([1.0, 5.0, 3.0, 8.0]);
|
||||
let b = Float4::new([2.0, 3.0, 4.0, 1.0]);
|
||||
let min = a.min(b);
|
||||
assert_eq!(min.to_array(), [1.0, 3.0, 3.0, 1.0]);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_max() {
|
||||
let a = Float4::new([1.0, 5.0, 3.0, 8.0]);
|
||||
let b = Float4::new([2.0, 3.0, 4.0, 1.0]);
|
||||
let max = a.max(b);
|
||||
assert_eq!(max.to_array(), [2.0, 5.0, 4.0, 8.0]);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_mul_add() {
|
||||
let a = Float4::new([1.0, 2.0, 3.0, 4.0]);
|
||||
let b = Float4::new([2.0, 2.0, 2.0, 2.0]);
|
||||
let c = Float4::new([10.0, 10.0, 10.0, 10.0]);
|
||||
let result = a.mul_add(b, c);
|
||||
assert_eq!(result.to_array(), [12.0, 14.0, 16.0, 18.0]);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_neg_mul_add() {
|
||||
let a = Float4::new([1.0, 2.0, 3.0, 4.0]);
|
||||
let b = Float4::new([2.0, 2.0, 2.0, 2.0]);
|
||||
let c = Float4::new([10.0, 10.0, 10.0, 10.0]);
|
||||
let result = a.neg_mul_add(b, c);
|
||||
assert_eq!(result.to_array(), [8.0, 6.0, 4.0, 2.0]);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_sum() {
|
||||
let v = Float4::new([1.0, 2.0, 3.0, 4.0]);
|
||||
assert_eq!(v.sum(), 10.0);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_horizontal_min() {
|
||||
let v = Float4::new([3.0, 1.0, 4.0, 2.0]);
|
||||
assert_eq!(v.horizontal_min(), 1.0);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_horizontal_max() {
|
||||
let v = Float4::new([3.0, 1.0, 4.0, 2.0]);
|
||||
assert_eq!(v.horizontal_max(), 4.0);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_alignment() {
|
||||
let v = Float4::new([1.0, 2.0, 3.0, 4.0]);
|
||||
let ptr = &v as *const Float4 as usize;
|
||||
assert_eq!(ptr % 16, 0, "Float4 should be 16-byte aligned");
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_splat_special_values() {
|
||||
let inf = Float4::splat(f32::INFINITY);
|
||||
assert!(inf.to_array().iter().all(|&x| x == f32::INFINITY));
|
||||
|
||||
let neg_inf = Float4::splat(f32::NEG_INFINITY);
|
||||
assert!(neg_inf.to_array().iter().all(|&x| x == f32::NEG_INFINITY));
|
||||
|
||||
let zero = Float4::splat(0.0);
|
||||
assert_eq!(zero.to_array(), [0.0, 0.0, 0.0, 0.0]);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_fma_rotation_transform() {
|
||||
// Test FMA for 2D rotation: x' = x*cos - y*sin, y' = x*sin + y*cos
|
||||
let xs = Float4::new([1.0, 2.0, 3.0, 4.0]);
|
||||
let ys = Float4::new([0.0, 0.0, 0.0, 0.0]);
|
||||
let cos_t = Float4::splat(0.0); // cos(90°) = 0
|
||||
let sin_t = Float4::splat(1.0); // sin(90°) = 1
|
||||
|
||||
// x' = x*cos - y*sin
|
||||
let new_xs = ys.neg_mul_add(sin_t, xs * cos_t);
|
||||
// y' = x*sin + y*cos
|
||||
let new_ys = xs.mul_add(sin_t, ys * cos_t);
|
||||
|
||||
assert_eq!(new_xs.to_array(), [0.0, 0.0, 0.0, 0.0]);
|
||||
assert_eq!(new_ys.to_array(), [1.0, 2.0, 3.0, 4.0]);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_abs() {
|
||||
let v = Float4::new([-1.0, 2.0, -3.0, 4.0]);
|
||||
let abs = v.abs();
|
||||
assert_eq!(abs.to_array(), [1.0, 2.0, 3.0, 4.0]);
|
||||
|
||||
// Test with all negative
|
||||
let all_neg = Float4::new([-1.0, -2.0, -3.0, -4.0]);
|
||||
assert_eq!(all_neg.abs().to_array(), [1.0, 2.0, 3.0, 4.0]);
|
||||
|
||||
// Test with zeros
|
||||
let with_zeros = Float4::new([0.0, -0.0, 0.0, -0.0]);
|
||||
let abs_zeros = with_zeros.abs().to_array();
|
||||
assert!(abs_zeros.iter().all(|&x| x == 0.0));
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_recip() {
|
||||
let v = Float4::new([1.0, 2.0, 4.0, 8.0]);
|
||||
let recip = v.recip();
|
||||
assert_eq!(recip.to_array(), [1.0, 0.5, 0.25, 0.125]);
|
||||
|
||||
// Test with negative values
|
||||
let neg = Float4::new([-1.0, -2.0, -4.0, -8.0]);
|
||||
assert_eq!(neg.recip().to_array(), [-1.0, -0.5, -0.25, -0.125]);
|
||||
}
|
||||
}
|
||||
+34
-31
@@ -1,39 +1,42 @@
|
||||
//! SIMD types for auto-vectorization.
|
||||
//! SIMD utilities and documentation.
|
||||
//!
|
||||
//! This module provides SIMD-friendly types that encourage LLVM to
|
||||
//! emit vectorized instructions across different architectures.
|
||||
//! This crate uses Rust's `std::simd` (portable_simd) for SIMD operations.
|
||||
//! No custom wrapper types are needed - use `std::simd` types directly.
|
||||
//!
|
||||
//! # Design Philosophy
|
||||
//! # Usage
|
||||
//!
|
||||
//! We rely on LLVM's auto-vectorization rather than platform-specific intrinsics.
|
||||
//! The [`Float4`] type uses:
|
||||
//! - 16-byte alignment matching common 128-bit SIMD registers
|
||||
//! - `#[inline(always)]` to expose vectorizable patterns
|
||||
//! - Simple element-wise operations that map to SIMD instructions
|
||||
//! ```rust,ignore
|
||||
//! use std::simd::{f32x4, num::SimdFloat};
|
||||
//!
|
||||
//! This approach provides:
|
||||
//! - **Portability**: Works on ARM NEON, x86 SSE/AVX, and other architectures
|
||||
//! - **Simplicity**: No unsafe intrinsics or target-specific code
|
||||
//! - **Maintainability**: Standard Rust that's easy to debug
|
||||
//! // Contiguous load from slice
|
||||
//! let xs = f32x4::from_slice(&data[base..]);
|
||||
//!
|
||||
//! Users can configure appropriate compiler flags for their target platform.
|
||||
//! // Broadcast scalar to all lanes
|
||||
//! let scale = f32x4::splat(2.0);
|
||||
//!
|
||||
//! # Usage Example
|
||||
//! // Element-wise operations
|
||||
//! let result = xs * scale;
|
||||
//!
|
||||
//! ```rust
|
||||
//! use vastu_map::simd::Float4;
|
||||
//!
|
||||
//! // Transform 4 points at once
|
||||
//! let xs = Float4::new([1.0, 2.0, 3.0, 4.0]);
|
||||
//! let ys = Float4::new([0.0, 0.0, 0.0, 0.0]);
|
||||
//! let cos_theta = Float4::splat(0.866); // cos(30°)
|
||||
//! let sin_theta = Float4::splat(0.5); // sin(30°)
|
||||
//!
|
||||
//! // Rotation: x' = x*cos - y*sin, y' = x*sin + y*cos
|
||||
//! let new_xs = ys.neg_mul_add(sin_theta, xs * cos_theta);
|
||||
//! let new_ys = xs.mul_add(sin_theta, ys * cos_theta);
|
||||
//! // Horizontal reduction
|
||||
//! let sum = result.reduce_sum();
|
||||
//! ```
|
||||
|
||||
mod f32x4;
|
||||
|
||||
pub use f32x4::Float4;
|
||||
//!
|
||||
//! # 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
|
||||
|
||||
Reference in New Issue
Block a user