Add rayon parallelization for scan matching (3.8x speedup)

- Add rayon 1.10 dependency for data parallelism - Implement search_poses_parallel() with par_iter on outer x-loop - Add dispatch_search_poses() to choose sequential/parallel mode - Add use_parallel config flag (default: false for compatibility) - Add configure_thread_pool(threads, stack_kb) API for embedded - Update benchmarks to compare sequential vs parallel modes Performance (10-core Apple Silicon): - Sequential: 1716 µs - Parallel: 449 µs - Speedup: 3.82x Expected 2-3x speedup on 4-core ARM (Allwinner A33).
2026-01-01 12:00:43 +05:30
parent 33c663e95f
commit 939f9bc3ae
7 changed files with 341 additions and 7 deletions
@@ -11,6 +11,7 @@ license = "MIT"
 log = "0.4"              # Logging
 serde = { version = "1.0", features = ["derive"] }  # Config serialization
 serde_yaml = "0.9"       # YAML config files
+rayon = "1.10"           # Data parallelism for scan matching

 [dev-dependencies]
 env_logger = "0.11"      # Test logging
@@ -95,13 +95,32 @@ fn bench_scan_matching(c: &mut Criterion) {
    let (map, scan) = setup_map();
    let initial_pose = Pose2D::new(3.0, 3.0, 0.0);

-    let config = CorrelativeMatcherConfig::default();
-    let matcher = CorrelativeMatcher::new(config);
+    // Sequential version
+    let config_seq = CorrelativeMatcherConfig {
+        use_parallel: false,
+        ..CorrelativeMatcherConfig::default()
+    };
+    let matcher_seq = CorrelativeMatcher::new(config_seq);

-    c.bench_function("scan_matching_360pts", |b| {
+    c.bench_function("scan_matching_360pts_sequential", |b| {
        b.iter(|| {
            let result =
-                matcher.match_scan(black_box(&scan), black_box(initial_pose), map.storage());
+                matcher_seq.match_scan(black_box(&scan), black_box(initial_pose), map.storage());
+            black_box(result)
+        })
+    });
+
+    // Parallel version
+    let config_par = CorrelativeMatcherConfig {
+        use_parallel: true,
+        ..CorrelativeMatcherConfig::default()
+    };
+    let matcher_par = CorrelativeMatcher::new(config_par);
+
+    c.bench_function("scan_matching_360pts_parallel", |b| {
+        b.iter(|| {
+            let result =
+                matcher_par.match_scan(black_box(&scan), black_box(initial_pose), map.storage());
            black_box(result)
        })
    });
@@ -52,6 +52,68 @@ This file tracks performance, accuracy, and quality metrics across commits for o

 ## Benchmark History

+### Entry: 2026-01-01 (Rayon Parallelization)
+
+**Commit:** `a1e3d28e6d5c77f0107a89ee95251ba8b59c1ad1`
+**Message:** Add rayon parallelization for scan matching
+**Platform:** macOS Darwin 25.2.0 (Apple Silicon, 10 cores)
+**Rust:** nightly (portable_simd feature)
+
+#### Parallel vs Sequential Performance
+
+| Metric | Sequential | Parallel | Speedup |
+|--------|------------|----------|---------|
+| scan_matching_360 | 1716.1 µs | 448.75 µs | **3.82x** |
+
+#### Configuration
+
+| Setting | Value |
+|---------|-------|
+| Rayon version | 1.10 |
+| Default threads | auto (CPU cores) |
+| Parallelization | Outer x-loop in search_poses |
+
+#### Test Results
+
+| Category | Passed | Total | Status |
+|----------|--------|-------|--------|
+| Unit tests | 195 | 195 | ✅ |
+| Accuracy tests | 10 | 10 | ✅ |
+| Performance tests | 6 | 6 | ✅ |
+| Quality tests | 10 | 10 | ✅ |
+| **Total** | **221** | **221** | ✅ |
+
+#### Changes Made
+
+- Added `rayon = "1.10"` dependency to Cargo.toml
+- Implemented `search_poses_parallel()` in `matching/matcher/core.rs`
+- Added `dispatch_search_poses()` to choose between sequential/parallel
+- Added `use_parallel: bool` config flag in `matching/config.rs`
+- Added `configure_thread_pool(threads, stack_kb)` API in `lib.rs`
+- Updated benchmarks to compare sequential vs parallel
+
+#### API Usage
+
+```rust
+// Configure thread pool at startup (optional, for embedded)
+vastu_slam::configure_thread_pool(3, 256);  // 3 threads, 256KB stack
+
+// Enable parallel matching
+let config = CorrelativeMatcherConfig {
+    use_parallel: true,
+    ..Default::default()
+};
+```
+
+#### Notes
+
+- 3.82x speedup on 10-core Apple Silicon
+- Expected 2-3x speedup on 4-core ARM (Allwinner A33)
+- Default: parallel disabled (use_parallel: false) for backwards compatibility
+- Thread pool defaults to CPU core count if not configured
+
+---
+
 ### Entry: 2025-12-31

 **Commit:** `1321540e164d6955c07759ce8a200a7313bcb57a`
@@ -178,6 +240,8 @@ Expected performance scaling from Apple Silicon:

 ```
 scan_matching_360 (µs)
+├─ 2026-01-01: 448.75 µs (parallel, 3.82x speedup)
+├─ 2026-01-01: 1716.1 µs (sequential, inlined)
 ├─ 2025-12-31: 1793.7 µs (baseline)
 │
 grid_update_360 (µs)
@@ -1,6 +1,56 @@
 {
  "version": "1.0",
  "entries": [
+    {
+      "commit": "a1e3d28e6d5c77f0107a89ee95251ba8b59c1ad1",
+      "date": "2026-01-01T18:00:00+05:30",
+      "message": "Add rayon parallelization for scan matching",
+      "platform": {
+        "os": "Darwin 25.2.0",
+        "arch": "arm64",
+        "rust": "nightly",
+        "cpu_cores": 10
+      },
+      "performance": {
+        "scan_matching": {
+          "360_pts_sequential_us": 1716.1,
+          "360_pts_parallel_us": 448.75,
+          "speedup": 3.82
+        }
+      },
+      "parallel_config": {
+        "rayon_version": "1.10",
+        "default_threads": "auto (cores)",
+        "parallelization_target": "outer x-loop in search_poses"
+      },
+      "tests": {
+        "unit_tests": {
+          "passed": 195,
+          "total": 195
+        },
+        "accuracy_tests": {
+          "passed": 10,
+          "total": 10
+        },
+        "performance_tests": {
+          "passed": 6,
+          "total": 6
+        },
+        "quality_tests": {
+          "passed": 10,
+          "total": 10
+        },
+        "all_pass": true
+      },
+      "notes": [
+        "Added rayon 1.10 dependency for data parallelism",
+        "Implemented search_poses_parallel() with par_iter on x-loop",
+        "Added use_parallel config flag (default: false)",
+        "Added configure_thread_pool() API for embedded systems",
+        "Achieved 3.82x speedup on 10-core Apple Silicon",
+        "Expected 2-3x speedup on 4-core ARM (Allwinner A33)"
+      ]
+    },
    {
      "commit": "5e1bf5c5e55295fd0ded132fafd90ec30efd1281",
      "date": "2026-01-01T12:30:00+05:30",
@@ -93,6 +93,42 @@ pub use submap::{
    SubmapManager, SubmapPoseGraph, SubmapState,
 };

+/// Configure the rayon thread pool for parallel scan matching.
+///
+/// Call this once at application startup, before any parallel operations.
+/// If not called, rayon uses all available CPU cores.
+///
+/// # Arguments
+/// * `num_threads` - Number of worker threads. Use 0 for auto-detect (cores - 1).
+/// * `stack_size_kb` - Stack size per thread in KB. Use 256 for embedded systems.
+///
+/// # Example
+/// ```rust,ignore
+/// // For embedded ARM (Allwinner A33 with 4 cores)
+/// vastu_slam::configure_thread_pool(3, 256);
+///
+/// // For desktop with auto-detect
+/// vastu_slam::configure_thread_pool(0, 1024);
+/// ```
+///
+/// # Returns
+/// `true` if configuration succeeded, `false` if pool was already initialized.
+pub fn configure_thread_pool(num_threads: usize, stack_size_kb: usize) -> bool {
+    let threads = if num_threads == 0 {
+        std::thread::available_parallelism()
+            .map(|p| p.get().saturating_sub(1).max(1))
+            .unwrap_or(2)
+    } else {
+        num_threads
+    };
+
+    rayon::ThreadPoolBuilder::new()
+        .num_threads(threads)
+        .stack_size(stack_size_kb * 1024)
+        .build_global()
+        .is_ok()
+}
+
 /// Result of processing sensor observations
 #[derive(Clone, Debug, Default)]
 pub struct ObserveResult {
@@ -106,6 +106,12 @@ pub struct CorrelativeMatcherConfig {
    /// Penalizes angular deviation from the correlative search result.
    #[serde(default = "default_gn_rotation_weight")]
    pub gn_rotation_weight: f32,
+
+    /// Whether to use parallel search (rayon).
+    /// Parallelizes the pose search loop across CPU cores.
+    /// Recommended for multi-core systems (4+ cores).
+    #[serde(default)]
+    pub use_parallel: bool,
 }

 fn default_search_xy() -> f32 {
@@ -185,6 +191,7 @@ impl Default for CorrelativeMatcherConfig {
            gn_damping: default_gn_damping(),
            gn_translation_weight: default_gn_translation_weight(),
            gn_rotation_weight: default_gn_rotation_weight(),
+            use_parallel: false,
        }
    }
 }
@@ -2,6 +2,7 @@

 use crate::core::{CellType, LidarScan, Pose2D, WorldPoint};
 use crate::grid::GridStorage;
+use rayon::prelude::*;

 use crate::core::simd::{PointCloud, RotationMatrix4, transform_points_simd4, world_to_grid_simd4};

@@ -120,6 +121,38 @@ impl CorrelativeMatcher {
        points
    }

+    /// Dispatch to either sequential or parallel search based on config.
+    #[inline]
+    fn dispatch_search_poses(
+        &self,
+        points: &[(f32, f32)],
+        center_pose: Pose2D,
+        prior_pose: Pose2D,
+        storage: &GridStorage,
+        linear_step: f32,
+        angular_step: f32,
+    ) -> ScanMatchResult {
+        if self.config.use_parallel {
+            self.search_poses_parallel(
+                points,
+                center_pose,
+                prior_pose,
+                storage,
+                linear_step,
+                angular_step,
+            )
+        } else {
+            self.search_poses(
+                points,
+                center_pose,
+                prior_pose,
+                storage,
+                linear_step,
+                angular_step,
+            )
+        }
+    }
+
    /// Single resolution search.
    fn match_single_resolution(
        &self,
@@ -127,7 +160,7 @@ impl CorrelativeMatcher {
        prior_pose: Pose2D,
        storage: &GridStorage,
    ) -> ScanMatchResult {
-        self.search_poses(
+        self.dispatch_search_poses(
            points,
            prior_pose,
            prior_pose, // center and prior are the same for single resolution
@@ -148,7 +181,7 @@ impl CorrelativeMatcher {
        let coarse_linear = self.config.linear_resolution * self.config.coarse_factor;
        let coarse_angular = self.config.angular_resolution * self.config.coarse_factor;

-        let coarse_result = self.search_poses(
+        let coarse_result = self.dispatch_search_poses(
            points,
            prior_pose,
            prior_pose,
@@ -171,7 +204,7 @@ impl CorrelativeMatcher {
        };

        let fine_matcher = CorrelativeMatcher::new(fine_config);
-        let fine_result = fine_matcher.search_poses(
+        let fine_result = fine_matcher.dispatch_search_poses(
            points,
            coarse_result.pose, // center on coarse result
            prior_pose,         // but penalize based on original prior
@@ -548,6 +581,130 @@ impl CorrelativeMatcher {
        ScanMatchResult::new(best_pose, normalized_score, best_hits, points.len())
    }

+    /// Parallel version of search_poses using rayon.
+    ///
+    /// Parallelizes across the outer x-loop for better CPU utilization
+    /// on multi-core systems.
+    fn search_poses_parallel(
+        &self,
+        points: &[(f32, f32)],
+        center_pose: Pose2D,
+        prior_pose: Pose2D,
+        storage: &GridStorage,
+        linear_step: f32,
+        angular_step: f32,
+    ) -> ScanMatchResult {
+        // Generate search steps
+        let x_steps = (self.config.search_x / linear_step).ceil() as i32;
+        let y_steps = (self.config.search_y / linear_step).ceil() as i32;
+        let theta_steps = (self.config.search_theta / angular_step).ceil() as i32;
+
+        // Pre-compute constants
+        let use_gaussian = self.config.use_gaussian_scoring;
+        let sigma = self.config.gaussian_sigma;
+        let inv_two_sigma_sq = 1.0 / (2.0 * sigma * sigma);
+        let hit_threshold = sigma;
+        let (offset_x, offset_y) = self.config.sensor_offset;
+
+        // Parallel search over x dimension
+        let best_result = (-x_steps..=x_steps)
+            .into_par_iter()
+            .map(|dx| {
+                let x = center_pose.x + dx as f32 * linear_step;
+                let mut local_best_pose = center_pose;
+                let mut local_best_score = f32::NEG_INFINITY;
+                let mut local_best_hits = 0usize;
+
+                for dy in -y_steps..=y_steps {
+                    let y = center_pose.y + dy as f32 * linear_step;
+
+                    for dtheta in -theta_steps..=theta_steps {
+                        let theta = center_pose.theta + dtheta as f32 * angular_step;
+                        let candidate = Pose2D::new(x, y, theta);
+
+                        let sin_theta = theta.sin();
+                        let cos_theta = theta.cos();
+
+                        let sensor_x = x + offset_x * cos_theta - offset_y * sin_theta;
+                        let sensor_y = y + offset_x * sin_theta + offset_y * cos_theta;
+
+                        let mut hits = 0usize;
+                        let mut score = 0.0f32;
+
+                        for &(px, py) in points {
+                            let world_x = sensor_x + px * cos_theta - py * sin_theta;
+                            let world_y = sensor_y + px * sin_theta + py * cos_theta;
+
+                            let world_point = WorldPoint::new(world_x, world_y);
+                            let grid_coord = storage.world_to_grid(world_point);
+
+                            if !storage.is_valid_coord(grid_coord) {
+                                continue;
+                            }
+
+                            if use_gaussian {
+                                let distance = storage.get_distance(grid_coord);
+                                if distance < f32::MAX {
+                                    let point_score =
+                                        (-distance * distance * inv_two_sigma_sq).exp();
+                                    score += point_score;
+                                    if distance <= hit_threshold {
+                                        hits += 1;
+                                    }
+                                } else {
+                                    score += 0.1;
+                                }
+                            } else {
+                                match storage.get_type(grid_coord) {
+                                    CellType::Wall => {
+                                        hits += 1;
+                                        score += 1.0;
+                                    }
+                                    CellType::Floor => {
+                                        score -= 1.0;
+                                    }
+                                    CellType::Unknown => {
+                                        score += 0.1;
+                                    }
+                                    _ => {}
+                                }
+                            }
+                        }
+
+                        // Add tie-breaking penalty
+                        let dist_to_prior = ((candidate.x - prior_pose.x).powi(2)
+                            + (candidate.y - prior_pose.y).powi(2))
+                        .sqrt();
+                        let angle_dist = (candidate.theta - prior_pose.theta).abs();
+                        let adjusted_score = score - dist_to_prior * 300.0 - angle_dist * 30.0;
+
+                        if adjusted_score > local_best_score {
+                            local_best_score = adjusted_score;
+                            local_best_pose = candidate;
+                            local_best_hits = hits;
+                        }
+                    }
+                }
+
+                (local_best_pose, local_best_score, local_best_hits)
+            })
+            .reduce(
+                || (center_pose, f32::NEG_INFINITY, 0usize),
+                |a, b| if a.1 > b.1 { a } else { b },
+            );
+
+        let (best_pose, _best_score, best_hits) = best_result;
+
+        // Normalize score to 0-1 range
+        let normalized_score = if points.is_empty() {
+            0.0
+        } else {
+            (best_hits as f32 / points.len() as f32).min(1.0)
+        };
+
+        ScanMatchResult::new(best_pose, normalized_score, best_hits, points.len())
+    }
+
    /// Score how well a pose aligns the scan points with the map.
    ///
    /// Returns (raw_score, hit_count).