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

Optimize scan matching with inlined scoring loop

Browse Source 
- Inline score_pose loop in CorrelativeMatcher for better cache locality
- Pre-compute Gaussian scoring constants and sensor offsets outside loop
- Move BENCHMARK_LOG.md to benchmarks/ directory

Performance improvements:
- Scan matching: ~4% faster (360pts: 1793.7 -> 1724.9 µs)
- Grid update: ~10% faster (360pts: 209.88 -> 187.74 µs)
- Combined throughput: 499 -> 523 scans/sec at 360 points
This commit is contained in:
codetiger
2026-01-01 10:19:01 +05:30
parent 5e1bf5c5e5
commit 33c663e95f
5 changed files with 200 additions and 30 deletions
Download Patch File Download Diff File Expand all files Collapse all files
vastu-slam/BENCHMARK_LOG.md → vastu-slam/benchmarks/BENCHMARK_LOG.md
View File
vastu-slam/benchmarks/baseline.json
+69
View File
@@ -1,6 +1,75 @@
{
"version": "1.0",
"entries": [
{
"commit": "5e1bf5c5e55295fd0ded132fafd90ec30efd1281",
"date": "2026-01-01T12:30:00+05:30",
"message": "Optimize scan matching with inlined scoring loop",
"platform": {
"os": "Darwin 25.2.0",
"arch": "arm64",
"rust": "nightly"
},
"performance": {
"scan_matching": {
"180_pts_us": 924.08,
"360_pts_us": 1724.9,
"720_pts_us": 5783.8
},
"grid_update": {
"180_pts_us": 94.35,
"360_pts_us": 187.74,
"720_pts_us": 371.95
},
"evaluation": {
"coverage_stats_us": 125.11,
"ate_100_us": 1.58,
"rpe_100_us": 3.10,
"relations_100_us": 59.37,
"ground_truth_100_us": 20.03
}
},
"throughput": {
"scan_matching_360_per_sec": 580,
"grid_update_360_per_sec": 5326,
"combined_360_per_sec": 523,
"realtime_factor_5hz": 104
},
"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
},
"targets": {
"scan_matching_360_us": 3000,
"grid_update_360_us": 400,
"ate_trans_m": 0.01,
"ate_rot_deg": 0.5,
"loop_closure_precision": 0.9,
"loop_closure_recall": 0.8
},
"notes": [
"Inlined score_pose loop in CorrelativeMatcher for better cache locality",
"Scan matching improved ~4% (360pts: 1793.7 -> 1724.9 µs)",
"Grid update improved ~10% (360pts: 209.88 -> 187.74 µs)",
"Pre-computed Gaussian scoring constants and sensor offsets",
"All accuracy/quality thresholds met"
]
},
{
"commit": "1321540e164d6955c07759ce8a200a7313bcb57a",
"date": "2025-12-31T20:44:09+05:30",
vastu-slam/src/grid/lidar_update.rs
+6 -8
View File
@@ -40,7 +40,8 @@ pub fn update_from_lidar(
let mut result = ObserveResult::default();
// Track newly added wall cells for distance field update
let mut new_wall_coords: Vec<GridCoord> = Vec::new();
// Pre-allocate with typical capacity to avoid reallocations
let mut new_wall_coords: Vec<GridCoord> = Vec::with_capacity(32);
// Calculate sensor position in world frame
let sensor_pos = robot_pose.transform_point(sensor_config.lidar_offset);
@@ -88,18 +89,15 @@ pub fn update_from_lidar(
// Trace ray using Bresenham line algorithm
// Apply log-odds updates: miss for ray cells, hit for endpoint
let bresenham = BresenhamLine::new(sensor_coord, endpoint_coord);
// Use peekable to detect the last element without collecting
let mut iter = bresenham.peekable();
while let Some(coord) = iter.next() {
// Compare against endpoint directly instead of using peekable()
for coord in BresenhamLine::new(sensor_coord, endpoint_coord) {
if !storage.is_valid_coord(coord) {
continue;
}
let is_last = iter.peek().is_none();
let is_endpoint = coord == endpoint_coord;
if !is_last {
if !is_endpoint {
// Not the endpoint - apply "miss" observation (evidence of free space)
// This uses Bayesian log-odds update: L_new = L_old + L_MISS
if storage.apply_miss(coord) {
vastu-slam/src/grid/storage/core.rs
+6 -2
View File
@@ -43,6 +43,8 @@ pub struct GridStorage {
pub(super) width: usize,
pub(super) height: usize,
pub(super) resolution: f32,
/// Pre-computed 1.0 / resolution for faster world-to-grid conversion.
pub(super) inv_resolution: f32,
pub(super) origin: WorldPoint,
pub(super) max_distance: f32,
pub(super) log_odds_config: LogOddsConfig,
@@ -104,6 +106,7 @@ impl GridStorage {
width,
height,
resolution,
inv_resolution: 1.0 / resolution,
origin,
max_distance: Self::DEFAULT_MAX_DISTANCE,
log_odds_config,
@@ -184,10 +187,11 @@ impl GridStorage {
// === Coordinate Conversion ===
/// Convert world coordinates to grid coordinates.
/// Uses pre-computed inverse resolution for faster computation.
#[inline]
pub fn world_to_grid(&self, point: WorldPoint) -> GridCoord {
let x = ((point.x - self.origin.x) / self.resolution).floor() as i32;
let y = ((point.y - self.origin.y) / self.resolution).floor() as i32;
let x = ((point.x - self.origin.x) * self.inv_resolution).floor() as i32;
let y = ((point.y - self.origin.y) * self.inv_resolution).floor() as i32;
GridCoord::new(x, y)
}
vastu-slam/src/matching/matcher/core.rs
+119 -20
View File
@@ -23,6 +23,27 @@ struct SearchParams {
angular_step: f32,
}
/// Pre-computed parameters for pose scoring.
/// Groups trig values, sensor offset, and Gaussian scoring constants
/// to avoid redundant computations in tight loops.
#[derive(Clone, Copy)]
struct ScoringParams {
/// Pre-computed sin(theta)
sin_theta: f32,
/// Pre-computed cos(theta)
cos_theta: f32,
/// Sensor X offset
offset_x: f32,
/// Sensor Y offset
offset_y: f32,
/// Whether to use Gaussian scoring
use_gaussian: bool,
/// Pre-computed 1.0 / (2.0 * sigma^2)
inv_two_sigma_sq: f32,
/// Hit threshold (sigma)
hit_threshold: f32,
}
/// Correlative scan-to-map matcher.
///
/// Uses a brute-force search with optional multi-resolution refinement
@@ -421,11 +442,21 @@ impl CorrelativeMatcher {
let mut best_score = f32::NEG_INFINITY;
let mut best_hits = 0usize;
// Generate search poses
// 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 Gaussian scoring constants (avoid recomputing per pose)
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;
// Pre-compute sensor offset
let (offset_x, offset_y) = self.config.sensor_offset;
// Original loop order: x → y → theta (innermost)
for dx in -x_steps..=x_steps {
let x = center_pose.x + dx as f32 * linear_step;
@@ -436,7 +467,57 @@ impl CorrelativeMatcher {
let theta = center_pose.theta + dtheta as f32 * angular_step;
let candidate = Pose2D::new(x, y, theta);
let (score, hits) = self.score_pose(points, candidate, storage);
// Hardware sin/cos is faster than cache lookup on Apple Silicon
let sin_theta = theta.sin();
let cos_theta = theta.cos();
// Calculate sensor position in world frame
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 {
// Transform point from sensor frame to world frame
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);
// Check if point is within map bounds
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 for distance from ORIGINAL prior pose
// This prefers poses closer to odometry when scores are similar
@@ -476,28 +557,46 @@ impl CorrelativeMatcher {
pose: Pose2D,
storage: &GridStorage,
) -> (f32, usize) {
let cos_theta = pose.theta.cos();
let sin_theta = pose.theta.sin();
// Calculate sensor position in world frame (applying sensor offset)
let (offset_x, offset_y) = self.config.sensor_offset;
let sensor_x = pose.x + offset_x * cos_theta - offset_y * sin_theta;
let sensor_y = pose.y + offset_x * sin_theta + offset_y * cos_theta;
let sigma = self.config.gaussian_sigma;
let params = ScoringParams {
sin_theta: pose.theta.sin(),
cos_theta: pose.theta.cos(),
offset_x,
offset_y,
use_gaussian: self.config.use_gaussian_scoring,
inv_two_sigma_sq: 1.0 / (2.0 * sigma * sigma),
hit_threshold: sigma,
};
self.score_pose_with_trig(points, pose, storage, &params)
}
/// Internal scoring method with pre-computed trigonometric and Gaussian values.
///
/// This avoids redundant sin/cos and constant computations in tight loops.
#[inline]
fn score_pose_with_trig(
&self,
points: &[(f32, f32)],
pose: Pose2D,
storage: &GridStorage,
params: &ScoringParams,
) -> (f32, usize) {
// Calculate sensor position in world frame (applying sensor offset)
let sensor_x =
pose.x + params.offset_x * params.cos_theta - params.offset_y * params.sin_theta;
let sensor_y =
pose.y + params.offset_x * params.sin_theta + params.offset_y * params.cos_theta;
let mut hits = 0usize;
let mut score = 0.0f32;
// Precompute Gaussian constants
let use_gaussian = self.config.use_gaussian_scoring;
let sigma = self.config.gaussian_sigma;
let two_sigma_sq = 2.0 * sigma * sigma;
// Hit threshold: within 1 sigma of wall
let hit_threshold = sigma;
for &(px, py) in points {
// Transform point from sensor frame to world frame
let world_x = sensor_x + px * cos_theta - py * sin_theta;
let world_y = sensor_y + px * sin_theta + py * cos_theta;
let world_x = sensor_x + px * params.cos_theta - py * params.sin_theta;
let world_y = sensor_y + px * params.sin_theta + py * params.cos_theta;
let world_point = WorldPoint::new(world_x, world_y);
let grid_coord = storage.world_to_grid(world_point);
@@ -507,7 +606,7 @@ impl CorrelativeMatcher {
continue;
}
if use_gaussian {
if params.use_gaussian {
// Gaussian scoring based on distance field
let distance = storage.get_distance(grid_coord);
@@ -516,10 +615,10 @@ impl CorrelativeMatcher {
// Distance 0 → score 1.0
// Distance sigma → score ~0.6
// Distance 2*sigma → score ~0.14
let point_score = (-distance * distance / two_sigma_sq).exp();
let point_score = (-distance * distance * params.inv_two_sigma_sq).exp();
score += point_score;
if distance <= hit_threshold {
if distance <= params.hit_threshold {
hits += 1;
}
} else {