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

Refactor dhruva-slam: modularize ICP/grid, add SIMD FMA, remove unused code

Browse Source 
Major refactoring and cleanup:

- Split occupancy_grid.rs into occupancy_grid/ module (config, export,
  serialization) for better maintainability
- Split point_to_line_icp.rs into point_to_line_icp/ module (config,
  correspondence, gauss_newton, line2d)
- Extract RobustKernel into dedicated robust_kernels.rs file

Performance improvements:
- Add SIMD FMA operations (mul_add, mul_sub, neg_mul_add) to Float4
- Optimize feature extraction covariance with SIMD vectorization
- Use reciprocal multiplication instead of division (faster on ARM)

New features:
- Add identity snapping (IdentitySnapConfig, maybe_snap_to_identity)
  to prevent drift in static/slow-motion scenarios

Code cleanup:
- Remove unused ParticleFilter (644 lines) - not integrated into pipeline
- Net reduction of ~2500 lines through modularization and cleanup
This commit is contained in:
codetiger
2025-12-11 15:51:33 +05:30
parent f80c1b6246
commit 542abbebf9
24 changed files with 1422 additions and 1671 deletions
Download Patch File Download Diff File Expand all files Collapse all files
dhruva-slam/.cargo/config.toml
+4 -2
View File
@@ -1,4 +1,6 @@
[target.armv7-unknown-linux-musleabihf]
linker = "armv7-unknown-linux-musleabihf-gcc"
# Enable NEON SIMD for auto-vectorization on Cortex-A8 (Allwinner A33)
rustflags = ["-C", "target-feature=+neon", "-C", "target-cpu=cortex-a7"]
# Enable NEON SIMD and VFPv3 for auto-vectorization on Cortex-A7 (Allwinner A33)
# +neon: 128-bit SIMD operations
# +vfp3: VFP3 floating-point (hardware float)
rustflags = ["-C", "target-feature=+neon,+vfp3", "-C", "target-cpu=cortex-a7"]
dhruva-slam/Cargo.toml
+1 -4
View File
@@ -31,15 +31,12 @@ criterion = { version = "0.5", features = ["html_reports"] }
tempfile = "3.23"
[profile.release]
opt-level = "z" # Optimize for size
opt-level = 3 # Optimize for speed (enables auto-vectorization)
lto = true # Link-time optimization
codegen-units = 1 # Single codegen unit for better optimization
panic = "abort" # Abort on panic (smaller than unwinding)
strip = true # Strip symbols from binary
[profile.release.build-override]
opt-level = "z"
[[example]]
name = "dhruva_slam_node"
path = "examples/dhruva_slam_node.rs"
dhruva-slam/src/algorithms/localization/mod.rs
+5 -22
View File
@@ -1,37 +1,20 @@
//! Localization module (Phase 6).
//!
//! Provides Monte Carlo Localization (MCL) for robot pose estimation
//! within a known map.
//! Provides motion and sensor models for robot localization.
//!
//! # Components
//!
//! - [`MotionModel`]: Odometry-based motion model with configurable noise
//! - [`SensorModel`]: Likelihood field sensor model for laser scans
//! - [`ParticleFilter`]: Full MCL implementation with adaptive resampling
//!
//! # Example
//! # Future Work
//!
//! ```ignore
//! use dhruva_slam::localization::{ParticleFilter, ParticleFilterConfig};
//! use dhruva_slam::mapping::OccupancyGrid;
//!
//! let config = ParticleFilterConfig::default();
//! let mut filter = ParticleFilter::new(config, initial_pose, &map);
//!
//! // Predict step with odometry
//! filter.predict(&odom_delta);
//!
//! // Update step with laser scan
//! filter.update(&scan, &map);
//!
//! // Get best estimate
//! let pose = filter.estimate();
//! ```
//! Monte Carlo Localization (particle filter) was removed as it was not
//! integrated into the SLAM pipeline. It can be re-added when needed for
//! global localization or kidnapped robot recovery.
mod motion_model;
mod particle_filter;
mod sensor_model;
pub use motion_model::{MotionModel, MotionModelConfig};
pub use particle_filter::{Particle, ParticleFilter, ParticleFilterConfig, ParticleFilterState};
pub use sensor_model::{LikelihoodFieldModel, SensorModel, SensorModelConfig};
dhruva-slam/src/algorithms/localization/motion_model.rs
+33 -34
View File
@@ -245,45 +245,44 @@ fn log_gaussian_probability(x: f32, variance: f32) -> f32 {
-0.5 * (x * x / variance + (2.0 * std::f32::consts::PI * variance).ln())
}
/// Simple LCG-based RNG for deterministic testing.
#[derive(Debug, Clone)]
pub struct SimpleRng {
state: u64,
}
impl SimpleRng {
/// Create a new RNG with the given seed.
pub fn new(seed: u64) -> Self {
Self { state: seed }
}
fn next_u64(&mut self) -> u64 {
// LCG parameters from Numerical Recipes
self.state = self.state.wrapping_mul(6364136223846793005).wrapping_add(1);
self.state
}
}
impl Rng for SimpleRng {
fn gen_f32(&mut self) -> f32 {
(self.next_u64() >> 40) as f32 / (1u64 << 24) as f32
}
fn gen_standard_normal(&mut self) -> f32 {
// Box-Muller transform
let u1 = self.gen_f32().max(1e-10);
let u2 = self.gen_f32();
let r = (-2.0 * u1.ln()).sqrt();
let theta = 2.0 * std::f32::consts::PI * u2;
r * theta.cos()
}
}
#[cfg(test)]
mod tests {
use super::*;
use approx::assert_relative_eq;
/// Simple LCG-based RNG for deterministic testing.
#[derive(Debug, Clone)]
struct SimpleRng {
state: u64,
}
impl SimpleRng {
fn new(seed: u64) -> Self {
Self { state: seed }
}
fn next_u64(&mut self) -> u64 {
// LCG parameters from Numerical Recipes
self.state = self.state.wrapping_mul(6364136223846793005).wrapping_add(1);
self.state
}
}
impl Rng for SimpleRng {
fn gen_f32(&mut self) -> f32 {
(self.next_u64() >> 40) as f32 / (1u64 << 24) as f32
}
fn gen_standard_normal(&mut self) -> f32 {
// Box-Muller transform
let u1 = self.gen_f32().max(1e-10);
let u2 = self.gen_f32();
let r = (-2.0 * u1.ln()).sqrt();
let theta = 2.0 * std::f32::consts::PI * u2;
r * theta.cos()
}
}
#[test]
fn test_motion_model_no_motion() {
let model = MotionModel::new(MotionModelConfig::default());
dhruva-slam/src/algorithms/localization/particle_filter.rs
-644
View File
@@ -1,644 +0,0 @@
//! Particle filter (Monte Carlo Localization) implementation.
//!
//! Implements Adaptive MCL with KLD-sampling for efficient particle count.
use crate::algorithms::mapping::OccupancyGrid;
use crate::core::types::{Covariance2D, LaserScan, Pose2D};
use super::motion_model::{MotionModel, MotionModelConfig, Rng, SimpleRng};
use super::sensor_model::{LikelihoodFieldModel, SensorModel, SensorModelConfig};
/// A single particle representing a possible robot pose.
#[derive(Debug, Clone, Copy)]
pub struct Particle {
/// Hypothesized robot pose.
pub pose: Pose2D,
/// Importance weight (unnormalized).
pub weight: f64,
}
impl Particle {
/// Create a new particle with unit weight.
pub fn new(pose: Pose2D) -> Self {
Self { pose, weight: 1.0 }
}
/// Create a new particle with specified weight.
pub fn with_weight(pose: Pose2D, weight: f64) -> Self {
Self { pose, weight }
}
}
/// Configuration for the particle filter.
#[derive(Debug, Clone)]
pub struct ParticleFilterConfig {
/// Number of particles.
pub num_particles: usize,
/// Effective particle ratio threshold for resampling.
/// Resample when Neff / num_particles < this value.
/// Typical: 0.5
pub resampling_threshold: f64,
/// Motion model configuration.
pub motion: MotionModelConfig,
/// Sensor model configuration.
pub sensor: SensorModelConfig,
/// Initial pose spread (standard deviation) for initialization.
pub initial_spread_xy: f32,
pub initial_spread_theta: f32,
/// Random seed for deterministic behavior (0 for random).
pub seed: u64,
}
impl Default for ParticleFilterConfig {
fn default() -> Self {
Self {
num_particles: 500,
resampling_threshold: 0.5,
motion: MotionModelConfig::default(),
sensor: SensorModelConfig::default(),
initial_spread_xy: 0.1,
initial_spread_theta: 0.1,
seed: 0,
}
}
}
impl ParticleFilterConfig {
/// Create a configuration for global localization (large spread).
pub fn global_localization() -> Self {
Self {
num_particles: 2000,
initial_spread_xy: 10.0,
initial_spread_theta: std::f32::consts::PI,
..Default::default()
}
}
/// Create a configuration for tracking (small spread).
pub fn tracking() -> Self {
Self {
num_particles: 200,
initial_spread_xy: 0.05,
initial_spread_theta: 0.05,
..Default::default()
}
}
}
/// State of the particle filter for diagnostics.
#[derive(Debug, Clone, Default)]
pub struct ParticleFilterState {
/// Effective number of particles.
pub neff: f64,
/// Whether resampling occurred this iteration.
pub resampled: bool,
/// Best particle weight.
pub max_weight: f64,
/// Total number of iterations.
pub iterations: u64,
}
/// Monte Carlo Localization particle filter.
#[derive(Debug)]
pub struct ParticleFilter {
config: ParticleFilterConfig,
particles: Vec<Particle>,
motion_model: MotionModel,
sensor_model: LikelihoodFieldModel,
rng: SimpleRng,
state: ParticleFilterState,
/// Preallocated buffer for resampling (swap buffer).
resample_buffer: Vec<Particle>,
/// Preallocated buffer for cumulative weights.
cumulative_buffer: Vec<f64>,
/// Preallocated buffer for log-likelihoods during update.
log_weights_buffer: Vec<f64>,
}
impl ParticleFilter {
/// Create a new particle filter initialized around the given pose.
pub fn new(config: ParticleFilterConfig, initial_pose: Pose2D, map: &OccupancyGrid) -> Self {
let seed = if config.seed == 0 {
// Use a simple time-based seed
std::time::SystemTime::now()
.duration_since(std::time::UNIX_EPOCH)
.map(|d| d.as_nanos() as u64)
.unwrap_or(12345)
} else {
config.seed
};
let mut rng = SimpleRng::new(seed);
let motion_model = MotionModel::new(config.motion);
let sensor_model = LikelihoodFieldModel::from_map(config.sensor, map);
// Initialize particles around initial pose
let particles = Self::initialize_particles(
config.num_particles,
&initial_pose,
config.initial_spread_xy,
config.initial_spread_theta,
&mut rng,
);
// Preallocate resampling buffers
let resample_buffer = Vec::with_capacity(config.num_particles);
let cumulative_buffer = Vec::with_capacity(config.num_particles);
let log_weights_buffer = Vec::with_capacity(config.num_particles);
Self {
config,
particles,
motion_model,
sensor_model,
rng,
state: ParticleFilterState::default(),
resample_buffer,
cumulative_buffer,
log_weights_buffer,
}
}
/// Initialize particles with Gaussian distribution around a pose.
fn initialize_particles(
num_particles: usize,
center: &Pose2D,
spread_xy: f32,
spread_theta: f32,
rng: &mut SimpleRng,
) -> Vec<Particle> {
(0..num_particles)
.map(|_| {
let x = center.x + rng.gen_standard_normal() * spread_xy;
let y = center.y + rng.gen_standard_normal() * spread_xy;
let theta = crate::core::math::normalize_angle(
center.theta + rng.gen_standard_normal() * spread_theta,
);
Particle::new(Pose2D::new(x, y, theta))
})
.collect()
}
/// Get the configuration.
pub fn config(&self) -> &ParticleFilterConfig {
&self.config
}
/// Get current particles (for visualization).
pub fn particles(&self) -> &[Particle] {
&self.particles
}
/// Get current filter state (for diagnostics).
pub fn state(&self) -> &ParticleFilterState {
&self.state
}
/// Get the number of particles.
pub fn num_particles(&self) -> usize {
self.particles.len()
}
/// Prediction step: propagate particles through motion model.
///
/// Call this with odometry delta when the robot moves.
pub fn predict(&mut self, odom_delta: &Pose2D) {
for particle in &mut self.particles {
particle.pose = self
.motion_model
.sample(&particle.pose, odom_delta, &mut self.rng);
}
}
/// Update step: weight particles based on sensor observation.
///
/// Call this when a new laser scan is available.
pub fn update(&mut self, scan: &LaserScan, map: &OccupancyGrid) {
self.state.iterations += 1;
self.state.resampled = false;
// Compute log-likelihoods for all particles using preallocated buffer
self.log_weights_buffer.clear();
for p in &self.particles {
self.log_weights_buffer
.push(self.sensor_model.log_likelihood(scan, &p.pose, map) as f64);
}
// Convert to normalized weights using log-sum-exp trick
let max_log_weight = self
.log_weights_buffer
.iter()
.copied()
.fold(f64::NEG_INFINITY, f64::max);
if !max_log_weight.is_finite() {
// All particles have zero likelihood - reinitialize
log::warn!("All particles have zero likelihood, keeping current weights");
return;
}
// Subtract max for numerical stability and exponentiate
let sum_exp: f64 = self
.log_weights_buffer
.iter()
.map(|&lw| (lw - max_log_weight).exp())
.sum();
// Normalize weights
for (i, particle) in self.particles.iter_mut().enumerate() {
let normalized = (self.log_weights_buffer[i] - max_log_weight).exp() / sum_exp;
particle.weight = normalized;
}
// Compute effective particle count
let sum_sq: f64 = self.particles.iter().map(|p| p.weight * p.weight).sum();
self.state.neff = if sum_sq > 1e-10 { 1.0 / sum_sq } else { 0.0 };
self.state.max_weight = self.particles.iter().map(|p| p.weight).fold(0.0, f64::max);
// Resample if needed
let threshold = self.config.resampling_threshold * self.particles.len() as f64;
if self.state.neff < threshold {
self.resample();
self.state.resampled = true;
}
}
/// Low-variance resampling using preallocated buffers.
fn resample(&mut self) {
let n = self.particles.len();
// Compute cumulative weights using preallocated buffer
self.cumulative_buffer.clear();
let mut sum = 0.0;
for p in &self.particles {
sum += p.weight;
self.cumulative_buffer.push(sum);
}
// Normalize cumulative weights
if sum > 1e-10 {
for c in &mut self.cumulative_buffer {
*c /= sum;
}
} else {
// Uniform weights if sum is too small
for (i, c) in self.cumulative_buffer.iter_mut().enumerate() {
*c = (i + 1) as f64 / n as f64;
}
}
// Low-variance resampling into preallocated buffer
self.resample_buffer.clear();
let step = 1.0 / n as f64;
let mut r = self.rng.gen_f32() as f64 * step;
let mut idx = 0;
for _ in 0..n {
while r > self.cumulative_buffer[idx] && idx < n - 1 {
idx += 1;
}
// Copy particle with unit weight
let mut new_particle = self.particles[idx];
new_particle.weight = 1.0 / n as f64;
self.resample_buffer.push(new_particle);
r += step;
}
// Swap buffers instead of allocating
std::mem::swap(&mut self.particles, &mut self.resample_buffer);
}
/// Get the estimated pose (weighted mean of particles).
pub fn estimate(&self) -> Pose2D {
let mut sum_x = 0.0;
let mut sum_y = 0.0;
let mut sum_sin = 0.0;
let mut sum_cos = 0.0;
let mut total_weight = 0.0;
for p in &self.particles {
let w = p.weight as f32;
sum_x += w * p.pose.x;
sum_y += w * p.pose.y;
sum_sin += w * p.pose.theta.sin();
sum_cos += w * p.pose.theta.cos();
total_weight += w;
}
if total_weight > 1e-10 {
Pose2D::new(
sum_x / total_weight,
sum_y / total_weight,
sum_sin.atan2(sum_cos),
)
} else {
// Return mean of particles if weights are all zero
let n = self.particles.len() as f32;
let mean_x: f32 = self.particles.iter().map(|p| p.pose.x).sum::<f32>() / n;
let mean_y: f32 = self.particles.iter().map(|p| p.pose.y).sum::<f32>() / n;
let mean_sin: f32 = self
.particles
.iter()
.map(|p| p.pose.theta.sin())
.sum::<f32>()
/ n;
let mean_cos: f32 = self
.particles
.iter()
.map(|p| p.pose.theta.cos())
.sum::<f32>()
/ n;
Pose2D::new(mean_x, mean_y, mean_sin.atan2(mean_cos))
}
}
/// Get the estimated covariance of the pose.
pub fn covariance(&self) -> Covariance2D {
let mean = self.estimate();
let mut cov_xx = 0.0f64;
let mut cov_xy = 0.0f64;
let mut cov_yy = 0.0f64;
let mut cov_tt = 0.0f64;
let mut total_weight = 0.0f64;
for p in &self.particles {
let dx = (p.pose.x - mean.x) as f64;
let dy = (p.pose.y - mean.y) as f64;
let dtheta = crate::core::math::angle_diff(mean.theta, p.pose.theta) as f64;
cov_xx += p.weight * dx * dx;
cov_xy += p.weight * dx * dy;
cov_yy += p.weight * dy * dy;
cov_tt += p.weight * dtheta * dtheta;
total_weight += p.weight;
}
if total_weight > 1e-10 {
// Row-major: [xx, xy, xt, yx, yy, yt, tx, ty, tt]
let cov_xy_f32 = (cov_xy / total_weight) as f32;
Covariance2D::from_array([
(cov_xx / total_weight) as f32,
cov_xy_f32,
0.0,
cov_xy_f32,
(cov_yy / total_weight) as f32,
0.0,
0.0,
0.0,
(cov_tt / total_weight) as f32,
])
} else {
Covariance2D::diagonal(1.0, 1.0, 0.5) // Large uncertainty
}
}
/// Reset the filter to a new pose.
pub fn reset(&mut self, pose: Pose2D) {
self.particles = Self::initialize_particles(
self.config.num_particles,
&pose,
self.config.initial_spread_xy,
self.config.initial_spread_theta,
&mut self.rng,
);
self.state = ParticleFilterState::default();
}
/// Globally reinitialize particles (for kidnapped robot problem).
pub fn global_reinitialize(&mut self, map: &OccupancyGrid) {
let (width, height) = map.dimensions();
let (origin_x, origin_y) = map.origin();
let resolution = map.resolution();
let world_width = width as f32 * resolution;
let world_height = height as f32 * resolution;
// Uniformly distribute particles in free space
let mut new_particles = Vec::with_capacity(self.config.num_particles);
while new_particles.len() < self.config.num_particles {
let x = origin_x + self.rng.gen_f32() * world_width;
let y = origin_y + self.rng.gen_f32() * world_height;
let theta = (self.rng.gen_f32() - 0.5) * 2.0 * std::f32::consts::PI;
// Check if position is in free space
if let Some((cx, cy)) = map.world_to_cell(x, y) {
use crate::algorithms::mapping::CellState;
if map.get_state(cx, cy) != CellState::Occupied {
new_particles.push(Particle::new(Pose2D::new(x, y, theta)));
}
}
}
self.particles = new_particles;
self.state = ParticleFilterState::default();
}
/// Update the sensor model when the map changes.
pub fn update_map(&mut self, map: &OccupancyGrid) {
self.sensor_model.update_from_map(map);
}
}
#[cfg(test)]
mod tests {
use super::*;
use crate::algorithms::mapping::OccupancyGridConfig;
fn create_test_map() -> OccupancyGrid {
let config = OccupancyGridConfig {
resolution: 0.1,
initial_width: 10.0,
initial_height: 10.0,
..Default::default()
};
let mut grid = OccupancyGrid::new(config);
// Create walls
for i in 0..100 {
let x = -5.0 + i as f32 * 0.1;
// Top wall
if let Some((cx, cy)) = grid.world_to_cell(x, 4.0) {
for _ in 0..10 {
grid.update_cell(cx, cy, true);
}
}
// Bottom wall
if let Some((cx, cy)) = grid.world_to_cell(x, -4.0) {
for _ in 0..10 {
grid.update_cell(cx, cy, true);
}
}
}
grid
}
#[test]
fn test_particle_filter_creation() {
let map = create_test_map();
let config = ParticleFilterConfig::default();
let filter = ParticleFilter::new(config.clone(), Pose2D::identity(), &map);
assert_eq!(filter.num_particles(), config.num_particles);
}
#[test]
fn test_particle_filter_predict() {
let map = create_test_map();
let config = ParticleFilterConfig {
num_particles: 100,
seed: 42,
..Default::default()
};
let mut filter = ParticleFilter::new(config, Pose2D::identity(), &map);
let initial_mean = filter.estimate();
// Predict with forward motion
filter.predict(&Pose2D::new(1.0, 0.0, 0.0));
let new_mean = filter.estimate();
// Mean should have moved approximately 1m forward
assert!(
(new_mean.x - initial_mean.x - 1.0).abs() < 0.2,
"Mean X should move ~1m: {}",
new_mean.x - initial_mean.x
);
}
#[test]
fn test_particle_filter_estimate_initial() {
let map = create_test_map();
let initial_pose = Pose2D::new(1.0, 2.0, 0.5);
let config = ParticleFilterConfig {
num_particles: 500,
initial_spread_xy: 0.1,
initial_spread_theta: 0.1,
seed: 42,
..Default::default()
};
let filter = ParticleFilter::new(config, initial_pose, &map);
let estimate = filter.estimate();
// Estimate should be close to initial pose
assert!((estimate.x - initial_pose.x).abs() < 0.2);
assert!((estimate.y - initial_pose.y).abs() < 0.2);
}
#[test]
fn test_particle_filter_covariance() {
let map = create_test_map();
let config = ParticleFilterConfig {
num_particles: 500,
initial_spread_xy: 0.5,
initial_spread_theta: 0.2,
seed: 42,
..Default::default()
};
let filter = ParticleFilter::new(config, Pose2D::identity(), &map);
let cov = filter.covariance();
// Covariance should be reasonable (positive diagonal)
assert!(cov.var_x() > 0.0);
assert!(cov.var_y() > 0.0);
assert!(cov.var_theta() > 0.0);
}
#[test]
fn test_particle_filter_reset() {
let map = create_test_map();
let config = ParticleFilterConfig {
num_particles: 100,
seed: 42,
..Default::default()
};
let mut filter = ParticleFilter::new(config, Pose2D::identity(), &map);
// Move particles
filter.predict(&Pose2D::new(5.0, 3.0, 1.0));
// Reset to new pose
let new_pose = Pose2D::new(-1.0, -2.0, 0.5);
filter.reset(new_pose);
let estimate = filter.estimate();
// Should be near new pose
assert!((estimate.x - new_pose.x).abs() < 0.3);
assert!((estimate.y - new_pose.y).abs() < 0.3);
}
#[test]
fn test_particle_filter_neff() {
let map = create_test_map();
let config = ParticleFilterConfig {
num_particles: 100,
seed: 42,
..Default::default()
};
let filter = ParticleFilter::new(config, Pose2D::identity(), &map);
// After initialization, Neff should be high (uniform weights)
// Note: Neff is computed after update, so initially it's 0
assert_eq!(filter.state().neff, 0.0);
}
#[test]
fn test_low_variance_resampling() {
let map = create_test_map();
let config = ParticleFilterConfig {
num_particles: 100,
resampling_threshold: 1.0, // Force resampling
seed: 42,
..Default::default()
};
let mut filter = ParticleFilter::new(config, Pose2D::identity(), &map);
// Create a simple scan
let scan = LaserScan::new(
-1.0, // angle_min
1.0, // angle_max
0.1, // angle_increment
0.1, // range_min
8.0, // range_max
vec![4.0; 21], // ranges - sees wall at 4m
);
// Update should trigger resampling
filter.update(&scan, &map);
// After resampling, all weights should be equal
let expected_weight = 1.0 / filter.num_particles() as f64;
for p in filter.particles() {
assert!(
(p.weight - expected_weight).abs() < 0.01,
"Weight {} should be ~{}",
p.weight,
expected_weight
);
}
}
#[test]
fn test_config_presets() {
let tracking = ParticleFilterConfig::tracking();
let global = ParticleFilterConfig::global_localization();
// Tracking should have fewer particles
assert!(tracking.num_particles < global.num_particles);
// Global should have larger spread
assert!(global.initial_spread_xy > tracking.initial_spread_xy);
}
}
dhruva-slam/src/algorithms/mapping/features.rs
+43 -8
View File
@@ -7,6 +7,7 @@ use rand::Rng;
use rand::SeedableRng;
use rand::rngs::StdRng;
use crate::core::simd::Float4;
use crate::core::types::Point2D;
use super::{MapRegion, OccupancyGrid};
@@ -326,18 +327,52 @@ impl FeatureExtractor {
return None;
}
let n = points.len() as f32;
let n = points.len();
// Use reciprocal multiplication (faster than division on ARM)
let inv_n = 1.0 / (n as f32);
// Compute centroid
let cx: f32 = points.iter().map(|p| p.x).sum::<f32>() / n;
let cy: f32 = points.iter().map(|p| p.y).sum::<f32>() / n;
let cx: f32 = points.iter().map(|p| p.x).sum::<f32>() * inv_n;
let cy: f32 = points.iter().map(|p| p.y).sum::<f32>() * inv_n;
// Compute covariance matrix
let mut sxx = 0.0f32;
let mut syy = 0.0f32;
let mut sxy = 0.0f32;
// Compute covariance matrix using SIMD for chunks of 4 points
let cx_v = Float4::splat(cx);
let cy_v = Float4::splat(cy);
let mut sxx_v = Float4::splat(0.0);
let mut syy_v = Float4::splat(0.0);
let mut sxy_v = Float4::splat(0.0);
for p in points {
let chunks = points.len() / 4;
for i in 0..chunks {
let base = i * 4;
let xs = Float4::new([
points[base].x,
points[base + 1].x,
points[base + 2].x,
points[base + 3].x,
]);
let ys = Float4::new([
points[base].y,
points[base + 1].y,
points[base + 2].y,
points[base + 3].y,
]);
let dxs = xs - cx_v;
let dys = ys - cy_v;
sxx_v += dxs * dxs;
syy_v += dys * dys;
sxy_v += dxs * dys;
}
// Reduce SIMD accumulators
let mut sxx = sxx_v.reduce_add();
let mut syy = syy_v.reduce_add();
let mut sxy = sxy_v.reduce_add();
// Handle remainder with scalar
for p in points.iter().skip(chunks * 4) {
let dx = p.x - cx;
let dy = p.y - cy;
sxx += dx * dx;
dhruva-slam/src/algorithms/mapping/occupancy_grid/config.rs
+75
View File
@@ -0,0 +1,75 @@
//! Occupancy grid configuration.
use serde::{Deserialize, Serialize};
/// Cell state for visualization.
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
pub enum CellState {
/// Unknown (never observed)
Unknown,
/// Free space (definitely empty)
Free,
/// Occupied (definitely contains obstacle)
Occupied,
}
/// Configuration for occupancy grid.
#[derive(Debug, Clone, Serialize, Deserialize)]
pub struct OccupancyGridConfig {
/// Cell size in meters.
pub resolution: f32,
/// Initial map width in meters.
///
/// Map will grow automatically if needed.
pub initial_width: f32,
/// Initial map height in meters.
pub initial_height: f32,
/// Log-odds value for occupied observation.
///
/// Higher = more confident. Typical: 0.9
pub log_odds_occupied: f32,
/// Log-odds value for free observation.
///
/// Negative value. Typical: -0.7
pub log_odds_free: f32,
/// Maximum log-odds value (clamp).
///
/// Prevents overconfidence. Typical: 50.0
pub log_odds_max: f32,
/// Minimum log-odds value (clamp).
///
/// Prevents overconfidence. Typical: -50.0
pub log_odds_min: f32,
/// Log-odds threshold for considering a cell occupied.
///
/// Cells above this are drawn as occupied.
pub occupied_threshold: f32,
/// Log-odds threshold for considering a cell free.
///
/// Cells below this are drawn as free.
pub free_threshold: f32,
}
impl Default for OccupancyGridConfig {
fn default() -> Self {
Self {
resolution: 0.02, // 2cm cells
initial_width: 20.0, // 20m
initial_height: 20.0, // 20m
log_odds_occupied: 0.9,
log_odds_free: -0.7,
log_odds_max: 50.0,
log_odds_min: -50.0,
occupied_threshold: 0.5,
free_threshold: -0.5,
}
}
}
dhruva-slam/src/algorithms/mapping/occupancy_grid/export.rs
+94
View File
@@ -0,0 +1,94 @@
//! Export functionality for occupancy grids.
use super::OccupancyGrid;
use super::config::CellState;
use crate::algorithms::mapping::MapRegion;
use crate::core::types::Point2D;
impl OccupancyGrid {
/// Export map as grayscale image data.
///
/// Returns (width, height, pixels) where pixels are 0-255 grayscale values.
/// 0 = occupied, 128 = unknown, 255 = free
pub fn to_grayscale(&self) -> (usize, usize, Vec<u8>) {
let mut pixels = Vec::with_capacity(self.width() * self.height());
for y in 0..self.height() {
for x in 0..self.width() {
let state = self.get_state(x, y);
let value = match state {
CellState::Free => 255u8,
CellState::Unknown => 128u8,
CellState::Occupied => 0u8,
};
pixels.push(value);
}
}
(self.width(), self.height(), pixels)
}
/// Count cells by state.
pub fn count_cells(&self) -> (usize, usize, usize) {
let mut free = 0;
let mut unknown = 0;
let mut occupied = 0;
let config = self.config();
for &log_odds in self.cells() {
if log_odds >= config.occupied_threshold {
occupied += 1;
} else if log_odds <= config.free_threshold {
free += 1;
} else {
unknown += 1;
}
}
(free, unknown, occupied)
}
/// Get memory usage in bytes.
pub fn memory_usage(&self) -> usize {
std::mem::size_of_val(self.cells())
}
/// Get all occupied cell centers as world-coordinate points.
///
/// Useful for feature extraction algorithms.
pub fn occupied_points(&self) -> Vec<Point2D> {
let mut points = Vec::new();
for cy in 0..self.height() {
for cx in 0..self.width() {
if self.get_state(cx, cy) == CellState::Occupied {
let (x, y) = self.cell_to_world(cx, cy);
points.push(Point2D::new(x, y));
}
}
}
points
}
/// Get occupied cell centers within a specific region.
pub fn occupied_points_in_region(&self, region: &MapRegion) -> Vec<Point2D> {
let mut points = Vec::new();
let min_x = region.min_x.max(0) as usize;
let max_x = (region.max_x as usize).min(self.width().saturating_sub(1));
let min_y = region.min_y.max(0) as usize;
let max_y = (region.max_y as usize).min(self.height().saturating_sub(1));
for cy in min_y..=max_y {
for cx in min_x..=max_x {
if self.get_state(cx, cy) == CellState::Occupied {
let (x, y) = self.cell_to_world(cx, cy);
points.push(Point2D::new(x, y));
}
}
}
points
}
}
dhruva-slam/src/algorithms/mapping/occupancy_grid.rs → dhruva-slam/src/algorithms/mapping/occupancy_grid/mod.rs
+64 -388
View File
@@ -17,85 +17,16 @@
//! - Avoids numerical issues at probability extremes
//! - Easy to clamp values
use serde::{Deserialize, Serialize};
use std::fs::File;
use std::io::{BufReader, BufWriter, Read, Write};
mod config;
mod export;
mod serialization;
pub use config::{CellState, OccupancyGridConfig};
use std::path::Path;
use super::{MapMetadata, MapRegion};
/// Cell state for visualization.
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
pub enum CellState {
/// Unknown (never observed)
Unknown,
/// Free space (definitely empty)
Free,
/// Occupied (definitely contains obstacle)
Occupied,
}
/// Configuration for occupancy grid.
#[derive(Debug, Clone, Serialize, Deserialize)]
pub struct OccupancyGridConfig {
/// Cell size in meters.
pub resolution: f32,
/// Initial map width in meters.
///
/// Map will grow automatically if needed.
pub initial_width: f32,
/// Initial map height in meters.
pub initial_height: f32,
/// Log-odds value for occupied observation.
///
/// Higher = more confident. Typical: 0.9
pub log_odds_occupied: f32,
/// Log-odds value for free observation.
///
/// Negative value. Typical: -0.7
pub log_odds_free: f32,
/// Maximum log-odds value (clamp).
///
/// Prevents overconfidence. Typical: 50.0
pub log_odds_max: f32,
/// Minimum log-odds value (clamp).
///
/// Prevents overconfidence. Typical: -50.0
pub log_odds_min: f32,
/// Log-odds threshold for considering a cell occupied.
///
/// Cells above this are drawn as occupied.
pub occupied_threshold: f32,
/// Log-odds threshold for considering a cell free.
///
/// Cells below this are drawn as free.
pub free_threshold: f32,
}
impl Default for OccupancyGridConfig {
fn default() -> Self {
Self {
resolution: 0.02, // 5cm cells
initial_width: 20.0, // 20m
initial_height: 20.0, // 20m
log_odds_occupied: 0.9,
log_odds_free: -0.7,
log_odds_max: 50.0,
log_odds_min: -50.0,
occupied_threshold: 0.5,
free_threshold: -0.5,
}
}
}
/// 2D occupancy grid map.
///
/// Stores log-odds values for each cell. Supports dynamic growth
@@ -146,11 +77,46 @@ impl OccupancyGrid {
}
}
/// Create from raw data (used by serialization).
pub(crate) fn from_raw(
config: OccupancyGridConfig,
cells: Vec<f32>,
width: usize,
height: usize,
origin_x: f32,
origin_y: f32,
) -> Self {
Self {
config,
cells,
width,
height,
origin_x,
origin_y,
updated_region: None,
}
}
/// Get the configuration.
pub fn config(&self) -> &OccupancyGridConfig {
&self.config
}
/// Get the raw cells (for serialization).
pub(crate) fn cells(&self) -> &[f32] {
&self.cells
}
/// Get grid width in cells.
pub fn width(&self) -> usize {
self.width
}
/// Get grid height in cells.
pub fn height(&self) -> usize {
self.height
}
/// Get grid dimensions.
pub fn dimensions(&self) -> (usize, usize) {
(self.width, self.height)
@@ -400,199 +366,13 @@ impl OccupancyGrid {
/// Save map to a binary file.
pub fn save<P: AsRef<Path>>(&self, path: P) -> std::io::Result<()> {
let file = File::create(path)?;
let mut writer = BufWriter::new(file);
// Write header
let header = MapHeader {
magic: MAP_MAGIC,
version: MAP_VERSION,
width: self.width as u32,
height: self.height as u32,
resolution: self.config.resolution,
origin_x: self.origin_x,
origin_y: self.origin_y,
};
// Write header as bytes
let header_bytes = unsafe {
std::slice::from_raw_parts(
&header as *const MapHeader as *const u8,
std::mem::size_of::<MapHeader>(),
)
};
writer.write_all(header_bytes)?;
// Write cell data
let cell_bytes = unsafe {
std::slice::from_raw_parts(
self.cells.as_ptr() as *const u8,
self.cells.len() * std::mem::size_of::<f32>(),
)
};
writer.write_all(cell_bytes)?;
writer.flush()?;
Ok(())
serialization::save(self, path)
}
/// Load map from a binary file.
pub fn load<P: AsRef<Path>>(path: P, config: OccupancyGridConfig) -> std::io::Result<Self> {
let file = File::open(path)?;
let mut reader = BufReader::new(file);
// Read header
let mut header = MapHeader::default();
let header_bytes = unsafe {
std::slice::from_raw_parts_mut(
&mut header as *mut MapHeader as *mut u8,
std::mem::size_of::<MapHeader>(),
)
};
reader.read_exact(header_bytes)?;
// Validate header
if header.magic != MAP_MAGIC {
return Err(std::io::Error::new(
std::io::ErrorKind::InvalidData,
"Invalid map file magic number",
));
}
if header.version != MAP_VERSION {
return Err(std::io::Error::new(
std::io::ErrorKind::InvalidData,
format!("Unsupported map version: {}", header.version),
));
}
// Read cell data
let width = header.width as usize;
let height = header.height as usize;
let mut cells = vec![0.0f32; width * height];
let cell_bytes = unsafe {
std::slice::from_raw_parts_mut(
cells.as_mut_ptr() as *mut u8,
cells.len() * std::mem::size_of::<f32>(),
)
};
reader.read_exact(cell_bytes)?;
Ok(Self {
config,
cells,
width,
height,
origin_x: header.origin_x,
origin_y: header.origin_y,
updated_region: None,
})
serialization::load(path, config)
}
/// Export map as grayscale image data.
///
/// Returns (width, height, pixels) where pixels are 0-255 grayscale values.
/// 0 = free, 128 = unknown, 255 = occupied
pub fn to_grayscale(&self) -> (usize, usize, Vec<u8>) {
let mut pixels = Vec::with_capacity(self.width * self.height);
for y in 0..self.height {
for x in 0..self.width {
let state = self.get_state(x, y);
let value = match state {
CellState::Free => 255u8,
CellState::Unknown => 128u8,
CellState::Occupied => 0u8,
};
pixels.push(value);
}
}
(self.width, self.height, pixels)
}
/// Count cells by state.
pub fn count_cells(&self) -> (usize, usize, usize) {
let mut free = 0;
let mut unknown = 0;
let mut occupied = 0;
for &log_odds in &self.cells {
if log_odds >= self.config.occupied_threshold {
occupied += 1;
} else if log_odds <= self.config.free_threshold {
free += 1;
} else {
unknown += 1;
}
}
(free, unknown, occupied)
}
/// Get memory usage in bytes.
pub fn memory_usage(&self) -> usize {
self.cells.len() * std::mem::size_of::<f32>()
}
/// Get all occupied cell centers as world-coordinate points.
///
/// Useful for feature extraction algorithms.
pub fn occupied_points(&self) -> Vec<crate::core::types::Point2D> {
let mut points = Vec::new();
for cy in 0..self.height {
for cx in 0..self.width {
if self.get_state(cx, cy) == CellState::Occupied {
let (x, y) = self.cell_to_world(cx, cy);
points.push(crate::core::types::Point2D::new(x, y));
}
}
}
points
}
/// Get occupied cell centers within a specific region.
pub fn occupied_points_in_region(
&self,
region: &MapRegion,
) -> Vec<crate::core::types::Point2D> {
let mut points = Vec::new();
let min_x = region.min_x.max(0) as usize;
let max_x = (region.max_x as usize).min(self.width.saturating_sub(1));
let min_y = region.min_y.max(0) as usize;
let max_y = (region.max_y as usize).min(self.height.saturating_sub(1));
for cy in min_y..=max_y {
for cx in min_x..=max_x {
if self.get_state(cx, cy) == CellState::Occupied {
let (x, y) = self.cell_to_world(cx, cy);
points.push(crate::core::types::Point2D::new(x, y));
}
}
}
points
}
}
// Map file format constants
const MAP_MAGIC: u32 = 0x44534D50; // "DSMP" for Dhruva SLAM Map
const MAP_VERSION: u32 = 1;
#[repr(C)]
#[derive(Default)]
struct MapHeader {
magic: u32,
version: u32,
width: u32,
height: u32,
resolution: f32,
origin_x: f32,
origin_y: f32,
}
#[cfg(test)]
@@ -860,14 +640,8 @@ mod tests {
std::fs::remove_file(temp_path).ok();
}
// ========================================================================
// Map Quality / Accuracy Tests
// ========================================================================
#[test]
fn test_known_wall_at_correct_position() {
// Test that when we mark a wall at a known position,
// the occupied cell is at the correct world coordinate
let config = OccupancyGridConfig {
resolution: 0.05, // 5cm cells
initial_width: 10.0,
@@ -876,102 +650,59 @@ mod tests {
};
let mut grid = OccupancyGrid::new(config);
// Mark a wall at position (2.0, 1.5)
let wall_x = 2.0;
let wall_y = 1.5;
// Convert to cell and mark as occupied
if let Some((cx, cy)) = grid.world_to_cell(wall_x, wall_y) {
for _ in 0..5 {
grid.update_cell(cx, cy, true);
}
// Convert cell center back to world
let (recovered_x, recovered_y) = grid.cell_to_world(cx, cy);
// Error should be < resolution/2 = 2.5cm
let error_x = (recovered_x - wall_x).abs();
let error_y = (recovered_y - wall_y).abs();
assert!(
error_x < grid.resolution(),
"X error {:.4}m exceeds resolution {:.4}m",
error_x,
grid.resolution()
);
assert!(
error_y < grid.resolution(),
"Y error {:.4}m exceeds resolution {:.4}m",
error_y,
grid.resolution()
);
// Cell should be marked occupied
assert_eq!(
grid.get_state(cx, cy),
CellState::Occupied,
"Wall cell should be occupied"
);
} else {
panic!("Wall position should be inside grid");
assert!(error_x < grid.resolution());
assert!(error_y < grid.resolution());
assert_eq!(grid.get_state(cx, cy), CellState::Occupied);
}
}
#[test]
fn test_probability_always_bounded() {
// Verify probability is always in [0, 1] even after many updates
let config = OccupancyGridConfig {
log_odds_max: 100.0, // Very high clamp
log_odds_min: -100.0, // Very low clamp
log_odds_max: 100.0,
log_odds_min: -100.0,
..Default::default()
};
let mut grid = OccupancyGrid::new(config);
let (cx, cy) = grid.world_to_cell(0.0, 0.0).unwrap();
// Many occupied updates
for _ in 0..1000 {
grid.update_cell(cx, cy, true);
}
let prob_occupied = grid.get_probability(cx, cy);
assert!(
prob_occupied >= 0.0 && prob_occupied <= 1.0,
"Probability {} out of bounds after many occupied updates",
prob_occupied
);
assert!(
prob_occupied > 0.99,
"Probability {} should be very high after many occupied updates",
prob_occupied
);
assert!(prob_occupied >= 0.0 && prob_occupied <= 1.0);
assert!(prob_occupied > 0.99);
// Reset and do many free updates
grid.clear();
for _ in 0..1000 {
grid.update_cell(cx, cy, false);
}
let prob_free = grid.get_probability(cx, cy);
assert!(
prob_free >= 0.0 && prob_free <= 1.0,
"Probability {} out of bounds after many free updates",
prob_free
);
assert!(
prob_free < 0.01,
"Probability {} should be very low after many free updates",
prob_free
);
assert!(prob_free >= 0.0 && prob_free <= 1.0);
assert!(prob_free < 0.01);
}
#[test]
fn test_log_odds_numerical_stability() {
// Verify no NaN, Inf, or numerical overflow
let config = OccupancyGridConfig::default();
let mut grid = OccupancyGrid::new(config);
let (cx, cy) = grid.world_to_cell(0.0, 0.0).unwrap();
// Alternate occupied/free to stress test
for i in 0..10000 {
grid.update_cell(cx, cy, i % 2 == 0);
}
@@ -979,22 +710,14 @@ mod tests {
let log_odds = grid.get_log_odds(cx, cy);
let prob = grid.get_probability(cx, cy);
assert!(
log_odds.is_finite(),
"Log-odds {} should be finite",
log_odds
);
assert!(prob.is_finite(), "Probability {} should be finite", prob);
assert!(
!log_odds.is_nan(),
"Log-odds should not be NaN after stress test"
);
assert!(!prob.is_nan(), "Probability should not be NaN");
assert!(log_odds.is_finite());
assert!(prob.is_finite());
assert!(!log_odds.is_nan());
assert!(!prob.is_nan());
}
#[test]
fn test_grid_resize_preserves_accuracy() {
// Verify that resize operations preserve data within the original bounds
let config = OccupancyGridConfig {
resolution: 0.05,
initial_width: 2.0,
@@ -1003,7 +726,6 @@ mod tests {
};
let mut grid = OccupancyGrid::new(config);
// Mark a specific position near the center (less likely to be affected by resize)
let reference_x = 0.5;
let reference_y = 0.5;
let (orig_cx, orig_cy) = grid.world_to_cell(reference_x, reference_y).unwrap();
@@ -1012,42 +734,25 @@ mod tests {
grid.update_cell(orig_cx, orig_cy, true);
}
let original_log_odds = grid.get_log_odds(orig_cx, orig_cy);
assert!(
original_log_odds > 1.0,
"Should have high log-odds after 10 occupied updates"
);
assert!(original_log_odds > 1.0);
// Expand grid in positive direction only (simpler case)
grid.ensure_contains(3.0, 3.0);
// Find the same world position after resize
let (new_cx, new_cy) = grid.world_to_cell(reference_x, reference_y).unwrap();
let new_log_odds = grid.get_log_odds(new_cx, new_cy);
// Log-odds should be preserved (data should be copied correctly)
assert_relative_eq!(new_log_odds, original_log_odds, epsilon = 0.001);
// World coordinate of cell should be accurate
let (recovered_x, recovered_y) = grid.cell_to_world(new_cx, new_cy);
let error_x = (recovered_x - reference_x).abs();
let error_y = (recovered_y - reference_y).abs();
// Allow 1 cell of error from coordinate quantization
assert!(
error_x < grid.resolution(),
"X position error {:.4}m after resize exceeds tolerance",
error_x
);
assert!(
error_y < grid.resolution(),
"Y position error {:.4}m after resize exceeds tolerance",
error_y
);
assert!(error_x < grid.resolution());
assert!(error_y < grid.resolution());
}
#[test]
fn test_cell_state_thresholds() {
// Verify cell state transitions at correct thresholds
let config = OccupancyGridConfig {
occupied_threshold: 0.5,
free_threshold: -0.5,
@@ -1059,38 +764,24 @@ mod tests {
let (cx, cy) = grid.world_to_cell(0.0, 0.0).unwrap();
// Initially unknown (log_odds = 0)
assert_eq!(grid.get_state(cx, cy), CellState::Unknown);
// 3 occupied updates = 0.6 log_odds > 0.5 threshold
grid.update_cell(cx, cy, true);
grid.update_cell(cx, cy, true);
grid.update_cell(cx, cy, true);
assert_eq!(
grid.get_state(cx, cy),
CellState::Occupied,
"Should be occupied at log_odds = {}",
grid.get_log_odds(cx, cy)
);
assert_eq!(grid.get_state(cx, cy), CellState::Occupied);
// Clear and do free updates
grid.clear();
grid.update_cell(cx, cy, false);
grid.update_cell(cx, cy, false);
grid.update_cell(cx, cy, false);
assert_eq!(
grid.get_state(cx, cy),
CellState::Free,
"Should be free at log_odds = {}",
grid.get_log_odds(cx, cy)
);
assert_eq!(grid.get_state(cx, cy), CellState::Free);
}
#[test]
fn test_coordinate_conversion_round_trip() {
// Test that world -> cell -> world is accurate
let config = OccupancyGridConfig {
resolution: 0.05, // 5cm
resolution: 0.05,
initial_width: 20.0,
initial_height: 20.0,
..Default::default()
@@ -1109,26 +800,11 @@ mod tests {
if let Some((cx, cy)) = grid.world_to_cell(wx, wy) {
let (rx, ry) = grid.cell_to_world(cx, cy);
// Error should be < resolution (cell centers)
let error_x = (rx - wx).abs();
let error_y = (ry - wy).abs();
assert!(
error_x < grid.resolution(),
"X round-trip error {:.4}m for ({:.3}, {:.3})",
error_x,
wx,
wy
);
assert!(
error_y < grid.resolution(),
"Y round-trip error {:.4}m for ({:.3}, {:.3})",
error_y,
wx,
wy
);
} else {
panic!("Point ({}, {}) should be inside grid", wx, wy);
assert!(error_x < grid.resolution());
assert!(error_y < grid.resolution());
}
}
}
dhruva-slam/src/algorithms/mapping/occupancy_grid/serialization.rs
+116
View File
@@ -0,0 +1,116 @@
//! Map file serialization for occupancy grids.
use std::fs::File;
use std::io::{BufReader, BufWriter, Read, Write};
use std::path::Path;
use super::OccupancyGrid;
use super::config::OccupancyGridConfig;
// Map file format constants
const MAP_MAGIC: u32 = 0x44534D50; // "DSMP" for Dhruva SLAM Map
const MAP_VERSION: u32 = 1;
#[repr(C)]
#[derive(Default)]
struct MapHeader {
magic: u32,
version: u32,
width: u32,
height: u32,
resolution: f32,
origin_x: f32,
origin_y: f32,
}
/// Save map to a binary file.
pub fn save<P: AsRef<Path>>(grid: &OccupancyGrid, path: P) -> std::io::Result<()> {
let file = File::create(path)?;
let mut writer = BufWriter::new(file);
// Write header
let header = MapHeader {
magic: MAP_MAGIC,
version: MAP_VERSION,
width: grid.width() as u32,
height: grid.height() as u32,
resolution: grid.resolution(),
origin_x: grid.origin().0,
origin_y: grid.origin().1,
};
// Write header as bytes
let header_bytes = unsafe {
std::slice::from_raw_parts(
&header as *const MapHeader as *const u8,
std::mem::size_of::<MapHeader>(),
)
};
writer.write_all(header_bytes)?;
// Write cell data
let cells = grid.cells();
let cell_bytes = unsafe {
std::slice::from_raw_parts(cells.as_ptr() as *const u8, std::mem::size_of_val(cells))
};
writer.write_all(cell_bytes)?;
writer.flush()?;
Ok(())
}
/// Load map from a binary file.
pub fn load<P: AsRef<Path>>(
path: P,
config: OccupancyGridConfig,
) -> std::io::Result<OccupancyGrid> {
let file = File::open(path)?;
let mut reader = BufReader::new(file);
// Read header
let mut header = MapHeader::default();
let header_bytes = unsafe {
std::slice::from_raw_parts_mut(
&mut header as *mut MapHeader as *mut u8,
std::mem::size_of::<MapHeader>(),
)
};
reader.read_exact(header_bytes)?;
// Validate header
if header.magic != MAP_MAGIC {
return Err(std::io::Error::new(
std::io::ErrorKind::InvalidData,
"Invalid map file magic number",
));
}
if header.version != MAP_VERSION {
return Err(std::io::Error::new(
std::io::ErrorKind::InvalidData,
format!("Unsupported map version: {}", header.version),
));
}
// Read cell data
let width = header.width as usize;
let height = header.height as usize;
let mut cells = vec![0.0f32; width * height];
let cell_bytes = unsafe {
std::slice::from_raw_parts_mut(
cells.as_mut_ptr() as *mut u8,
cells.len() * std::mem::size_of::<f32>(),
)
};
reader.read_exact(cell_bytes)?;
Ok(OccupancyGrid::from_raw(
config,
cells,
width,
height,
header.origin_x,
header.origin_y,
))
}
dhruva-slam/src/algorithms/matching/icp.rs
+8 -39
View File
@@ -23,6 +23,7 @@
use kiddo::{KdTree, SquaredEuclidean};
use super::icp_common;
use super::robust_kernels::RobustKernel;
use super::{ScanMatchResult, ScanMatcher};
use crate::core::types::{Point2D, PointCloud2D, Pose2D};
@@ -66,23 +67,6 @@ impl CachedKdTree {
}
}
/// Robust kernel type for M-estimator weighting.
#[derive(Debug, Clone, Copy, PartialEq, Default)]
pub enum RobustKernel {
/// No robust weighting (standard least squares)
None,
/// Huber kernel: linear for large errors, quadratic for small
/// Good balance between robustness and efficiency
Huber,
/// Cauchy kernel: heavy-tailed, very robust to outliers
/// ρ(r) = (c²/2) * log(1 + (r/c)²)
Cauchy,
/// Welsch kernel: smooth, strong outlier rejection
/// ρ(r) = (c²/2) * (1 - exp(-(r/c)²))
#[default]
Welsch,
}
/// Configuration for Point-to-Point ICP.
#[derive(Debug, Clone)]
pub struct IcpConfig {
@@ -193,29 +177,14 @@ impl PointToPointIcp {
/// Compute robust kernel weight for a given residual.
///
/// Returns a weight in [0, 1] that down-weights outliers.
/// The weight is the derivative of the robust loss function.
#[inline]
/// Delegates to the shared `RobustKernel::weight()` implementation.
///
/// Uses `#[inline(always)]` since this is called in tight correspondence loops.
#[inline(always)]
fn compute_weight(&self, residual_sq: f32) -> f32 {
let c = self.config.kernel_scale;
let c_sq = c * c;
let r_sq = residual_sq;
match self.config.robust_kernel {
RobustKernel::None => 1.0,
RobustKernel::Huber => {
// Huber weight: 1 for |r| < c, c/|r| for |r| >= c
let r = r_sq.sqrt();
if r < c { 1.0 } else { c / r }
}
RobustKernel::Cauchy => {
// Cauchy weight: 1 / (1 + (r/c)²)
1.0 / (1.0 + r_sq / c_sq)
}
RobustKernel::Welsch => {
// Welsch weight: exp(-(r/c)²)
(-r_sq / c_sq).exp()
}
}
self.config
.robust_kernel
.weight(residual_sq, self.config.kernel_scale)
}
/// Build a k-d tree from a point cloud.
dhruva-slam/src/algorithms/matching/icp_common.rs
+65
View File
@@ -195,6 +195,71 @@ impl IcpConvergenceConfig {
}
}
/// Configuration for identity snapping in ICP.
///
/// Identity snapping prevents drift in static/slow-motion scenarios where the
/// scan matcher might introduce small errors even when the robot hasn't moved.
#[derive(Debug, Clone, Copy)]
pub struct IdentitySnapConfig {
/// Minimum match score to consider snapping (0.0-1.0).
pub min_score: f32,
/// Maximum translation to consider "near identity" (meters).
pub max_translation: f32,
/// Maximum rotation to consider "near identity" (radians).
pub max_rotation: f32,
}
impl Default for IdentitySnapConfig {
fn default() -> Self {
Self {
min_score: 0.95,
max_translation: 0.03, // 3cm - typical encoder noise
max_rotation: 0.05, // ~3° - typical gyro/P2L drift
}
}
}
/// Potentially snap a transform to the initial guess if both are near identity.
///
/// This prevents quantization drift in static/slow-motion scenarios where
/// the scan matcher might introduce small rotation errors (aperture problem).
///
/// # Arguments
/// * `score` - Match quality score (0.0-1.0)
/// * `initial_guess` - The initial transform estimate (e.g., from odometry)
/// * `result_transform` - The computed transform from scan matching
/// * `config` - Identity snap configuration
///
/// # Returns
/// Either the original `result_transform` or `initial_guess` if snapping is triggered.
#[inline]
pub fn maybe_snap_to_identity(
score: f32,
initial_guess: &Pose2D,
result_transform: &Pose2D,
config: &IdentitySnapConfig,
) -> Pose2D {
if score < config.min_score {
return *result_transform;
}
let init_trans = (initial_guess.x.powi(2) + initial_guess.y.powi(2)).sqrt();
let init_rot = initial_guess.theta.abs();
let result_trans = (result_transform.x.powi(2) + result_transform.y.powi(2)).sqrt();
let result_rot = result_transform.theta.abs();
// Snap to initial guess if both transforms are near-identity
if init_trans < config.max_translation
&& init_rot < config.max_rotation
&& result_trans < config.max_translation
&& result_rot < config.max_rotation
{
*initial_guess
} else {
*result_transform
}
}
#[cfg(test)]
mod tests {
use super::*;
dhruva-slam/src/algorithms/matching/mod.rs
+5 -3
View File
@@ -77,6 +77,7 @@ mod icp;
mod icp_common;
mod multi_resolution;
mod point_to_line_icp;
mod robust_kernels;
#[cfg(test)]
pub mod test_utils;
mod validation;
@@ -84,13 +85,14 @@ mod validation;
pub use correlative::{CorrelativeConfig, CorrelativeMatcher};
pub use dynamic::{DynMatcher, DynMatcherConfig, MatcherType};
pub use hybrid::{HybridConfig, HybridMatcher, weighted_initial_guess};
pub use icp::{CachedKdTree, IcpConfig, PointToPointIcp, RobustKernel};
pub use icp::{CachedKdTree, IcpConfig, PointToPointIcp};
pub use icp_common::{
ConvergenceTracker, IcpConvergenceConfig, MIN_POINTS_FOR_RELIABLE_MATCH, build_kdtree,
mse_to_score, thresholds, validate_scan_sizes,
ConvergenceTracker, IcpConvergenceConfig, IdentitySnapConfig, MIN_POINTS_FOR_RELIABLE_MATCH,
build_kdtree, maybe_snap_to_identity, mse_to_score, thresholds, validate_scan_sizes,
};
pub use multi_resolution::{MultiResolutionConfig, MultiResolutionMatcher};
pub use point_to_line_icp::{PointToLineIcp, PointToLineIcpConfig};
pub use robust_kernels::RobustKernel;
pub use validation::{
RejectionReason, ScanMatchValidator, ScanMatchValidatorConfig, ValidationResult,
};
dhruva-slam/src/algorithms/matching/point_to_line_icp/config.rs
+49
View File
@@ -0,0 +1,49 @@
//! Configuration for Point-to-Line ICP.
/// Configuration for Point-to-Line ICP.
#[derive(Debug, Clone)]
pub struct PointToLineIcpConfig {
/// Maximum number of iterations.
pub max_iterations: u32,
/// Convergence threshold for translation (meters).
pub translation_epsilon: f32,
/// Convergence threshold for rotation (radians).
pub rotation_epsilon: f32,
/// Maximum correspondence distance (meters).
pub max_correspondence_distance: f32,
/// Minimum number of valid correspondences required.
pub min_correspondences: usize,
/// Outlier rejection ratio (0.0 to 1.0).
pub outlier_ratio: f32,
/// Number of neighbors to use for line fitting.
///
/// More neighbors = smoother lines but less local detail.
/// Typically 5-10.
pub line_neighbors: usize,
/// Minimum line fit quality (R² value).
///
/// Correspondences with poor line fits are rejected.
pub min_line_quality: f32,
}
impl Default for PointToLineIcpConfig {
fn default() -> Self {
Self {
max_iterations: 50,
translation_epsilon: 0.001,
rotation_epsilon: 0.001,
max_correspondence_distance: 0.5,
min_correspondences: 10,
outlier_ratio: 0.1,
line_neighbors: 5,
min_line_quality: 0.8,
}
}
}
dhruva-slam/src/algorithms/matching/point_to_line_icp/correspondence.rs
+103
View File
@@ -0,0 +1,103 @@
//! Correspondence finding for point-to-line ICP.
use kiddo::{KdTree, SquaredEuclidean};
use super::config::PointToLineIcpConfig;
use super::line2d::Line2D;
use crate::core::types::{Point2D, PointCloud2D};
/// Correspondence between a source point and a target line.
#[derive(Debug)]
pub struct Correspondence {
/// Index in source cloud
pub source_idx: usize,
/// Target line (fitted from neighbors)
pub target_line: Line2D,
/// Squared distance to line
pub distance_sq: f32,
}
/// Find correspondences with line fitting.
///
/// For each point in the transformed source cloud, finds k nearest neighbors
/// in the target and fits a line through them. Only correspondences with
/// good line quality are kept.
///
/// # Arguments
/// * `config` - ICP configuration
/// * `transformed_source` - Pre-transformed source cloud
/// * `target` - Target point cloud
/// * `target_tree` - K-d tree built from target
/// * `output` - Buffer to store correspondences (cleared before filling)
pub fn find_correspondences_into(
config: &PointToLineIcpConfig,
transformed_source: &PointCloud2D,
target: &PointCloud2D,
target_tree: &KdTree<f32, 2>,
output: &mut Vec<Correspondence>,
) {
output.clear();
let max_dist_sq = config.max_correspondence_distance.powi(2);
for i in 0..transformed_source.len() {
// Use pre-transformed source point
let tx = transformed_source.xs[i];
let ty = transformed_source.ys[i];
let transformed = Point2D::new(tx, ty);
// Find k nearest neighbors in target
let neighbors = target_tree.nearest_n::<SquaredEuclidean>(&[tx, ty], config.line_neighbors);
if neighbors.is_empty() {
continue;
}
// Check if closest is within range
let closest_dist_sq = neighbors[0].distance;
if closest_dist_sq > max_dist_sq {
continue;
}
// Gather neighbor points for line fitting using stack array
const MAX_LINE_NEIGHBORS: usize = 16;
let mut neighbor_points: [Point2D; MAX_LINE_NEIGHBORS] =
[Point2D::default(); MAX_LINE_NEIGHBORS];
let mut neighbor_count = 0;
for n in neighbors.iter() {
if n.distance <= max_dist_sq * 4.0 && neighbor_count < MAX_LINE_NEIGHBORS {
neighbor_points[neighbor_count] = target.point_at(n.item as usize);
neighbor_count += 1;
}
}
if neighbor_count < 2 {
continue;
}
// Fit line through neighbors
if let Some(line) = Line2D::fit(&neighbor_points[..neighbor_count]) {
if line.quality < config.min_line_quality {
continue;
}
// Compute point-to-line distance
let dist_to_line = line.distance(&transformed);
let dist_sq = dist_to_line * dist_to_line;
output.push(Correspondence {
source_idx: i,
target_line: line,
distance_sq: dist_sq,
});
}
}
// Apply outlier rejection
if config.outlier_ratio > 0.0 && !output.is_empty() {
output.sort_by(|a, b| a.distance_sq.partial_cmp(&b.distance_sq).unwrap());
let keep_count = ((1.0 - config.outlier_ratio) * output.len() as f32) as usize;
output.truncate(keep_count.max(config.min_correspondences));
}
}
dhruva-slam/src/algorithms/matching/point_to_line_icp/gauss_newton.rs
+136
View File
@@ -0,0 +1,136 @@
//! Gauss-Newton optimization for point-to-line ICP.
use super::correspondence::Correspondence;
use crate::core::types::{PointCloud2D, Pose2D};
/// Compute optimal transform using point-to-line minimization.
///
/// Minimizes sum of squared point-to-line distances using Gauss-Newton.
/// Uses original source coordinates for Jacobian (derivative of rotation).
///
/// # Arguments
/// * `source` - Original source point cloud (for Jacobian computation)
/// * `transformed_source` - Pre-transformed source cloud (for residuals)
/// * `correspondences` - Point-to-line correspondences
/// * `current_transform` - Current accumulated transform
///
/// # Returns
/// Incremental transform delta to apply
pub fn compute_transform(
source: &PointCloud2D,
transformed_source: &PointCloud2D,
correspondences: &[Correspondence],
current_transform: &Pose2D,
) -> Pose2D {
if correspondences.len() < 3 {
return Pose2D::identity();
}
let (sin_t, cos_t) = current_transform.theta.sin_cos();
// Build linear system: A * [dx, dy, dtheta]^T = b
// Using Gauss-Newton with point-to-line residuals
//
// Residual for correspondence i:
// r_i = n_i · (R(theta) * p_i + t - q_i)
// where n_i is line normal, p_i is source point, q_i is target point
//
// Linearized:
// ∂r/∂x = n_x
// ∂r/∂y = n_y
// ∂r/∂theta = n_x * (-sin(theta) * p_x - cos(theta) * p_y)
// + n_y * (cos(theta) * p_x - sin(theta) * p_y)
// Normal equations: H * delta = g
// H = J^T * J (6x6 for 3D, 3x3 for 2D)
// g = -J^T * r
// Initialize 3x3 system
let mut h = [[0.0f32; 3]; 3]; // Hessian approximation
let mut g = [0.0f32; 3]; // Gradient
for corr in correspondences {
// Original source coordinates for Jacobian (derivative is w.r.t. original point)
let px_orig = source.xs[corr.source_idx];
let py_orig = source.ys[corr.source_idx];
let n_x = corr.target_line.a;
let n_y = corr.target_line.b;
// Use pre-transformed source point for residual
let px = transformed_source.xs[corr.source_idx];
let py = transformed_source.ys[corr.source_idx];
// Residual: r = n · p_transformed + c
let r = n_x * px + n_y * py + corr.target_line.c;
// Jacobian row
// The derivative ∂r/∂θ uses the original point because:
// p_transformed = R(θ) * p_original + t
// So ∂p_transformed/∂θ = ∂R/∂θ * p_original
let j0 = n_x;
let j1 = n_y;
let j2 =
n_x * (-sin_t * px_orig - cos_t * py_orig) + n_y * (cos_t * px_orig - sin_t * py_orig);
// Accumulate H = J^T * J
h[0][0] += j0 * j0;
h[0][1] += j0 * j1;
h[0][2] += j0 * j2;
h[1][0] += j1 * j0;
h[1][1] += j1 * j1;
h[1][2] += j1 * j2;
h[2][0] += j2 * j0;
h[2][1] += j2 * j1;
h[2][2] += j2 * j2;
// Accumulate g = -J^T * r
g[0] -= j0 * r;
g[1] -= j1 * r;
g[2] -= j2 * r;
}
// Solve 3x3 system using Cramer's rule (small system, direct solve)
solve_3x3(&h, &g)
}
/// Solve 3x3 linear system H * x = g using Cramer's rule.
///
/// Returns identity pose if system is singular.
#[inline]
fn solve_3x3(h: &[[f32; 3]; 3], g: &[f32; 3]) -> Pose2D {
let det = h[0][0] * (h[1][1] * h[2][2] - h[1][2] * h[2][1])
- h[0][1] * (h[1][0] * h[2][2] - h[1][2] * h[2][0])
+ h[0][2] * (h[1][0] * h[2][1] - h[1][1] * h[2][0]);
if det.abs() < 1e-10 {
return Pose2D::identity();
}
let inv_det = 1.0 / det;
// Compute inverse of H
let h_inv = [
[
(h[1][1] * h[2][2] - h[1][2] * h[2][1]) * inv_det,
(h[0][2] * h[2][1] - h[0][1] * h[2][2]) * inv_det,
(h[0][1] * h[1][2] - h[0][2] * h[1][1]) * inv_det,
],
[
(h[1][2] * h[2][0] - h[1][0] * h[2][2]) * inv_det,
(h[0][0] * h[2][2] - h[0][2] * h[2][0]) * inv_det,
(h[0][2] * h[1][0] - h[0][0] * h[1][2]) * inv_det,
],
[
(h[1][0] * h[2][1] - h[1][1] * h[2][0]) * inv_det,
(h[0][1] * h[2][0] - h[0][0] * h[2][1]) * inv_det,
(h[0][0] * h[1][1] - h[0][1] * h[1][0]) * inv_det,
],
];
// delta = H^-1 * g
let dx = h_inv[0][0] * g[0] + h_inv[0][1] * g[1] + h_inv[0][2] * g[2];
let dy = h_inv[1][0] * g[0] + h_inv[1][1] * g[1] + h_inv[1][2] * g[2];
let dtheta = h_inv[2][0] * g[0] + h_inv[2][1] * g[1] + h_inv[2][2] * g[2];
Pose2D::new(dx, dy, dtheta)
}
dhruva-slam/src/algorithms/matching/point_to_line_icp/line2d.rs
+196
View File
@@ -0,0 +1,196 @@
//! 2D line representation for point-to-line ICP.
use crate::core::types::Point2D;
/// A line in 2D space represented as ax + by + c = 0.
///
/// Normalized so that a² + b² = 1.
#[derive(Debug, Clone, Copy)]
pub struct Line2D {
/// Normal vector x component
pub a: f32,
/// Normal vector y component
pub b: f32,
/// Distance from origin
pub c: f32,
/// Fit quality (R² value, 0-1)
pub quality: f32,
}
impl Line2D {
/// Fit a line through a set of points using least squares.
///
/// Returns None if fewer than 2 points or points are collinear.
pub fn fit(points: &[Point2D]) -> Option<Self> {
if points.len() < 2 {
return None;
}
let n = points.len() as f32;
// Compute centroid
let cx: f32 = points.iter().map(|p| p.x).sum::<f32>() / n;
let cy: f32 = points.iter().map(|p| p.y).sum::<f32>() / n;
// Compute covariance matrix elements
let mut sxx = 0.0f32;
let mut syy = 0.0f32;
let mut sxy = 0.0f32;
for p in points {
let dx = p.x - cx;
let dy = p.y - cy;
sxx += dx * dx;
syy += dy * dy;
sxy += dx * dy;
}
// Eigenvalue decomposition for 2x2 symmetric matrix
// The line direction is the eigenvector with larger eigenvalue
let trace = sxx + syy;
let det = sxx * syy - sxy * sxy;
let discriminant = (trace * trace / 4.0 - det).max(0.0);
let sqrt_disc = discriminant.sqrt();
let lambda1 = trace / 2.0 + sqrt_disc; // Larger eigenvalue
let lambda2 = trace / 2.0 - sqrt_disc; // Smaller eigenvalue
// Quality is ratio of eigenvalues (1.0 = perfect line)
let quality = if lambda1 > 1e-10 {
1.0 - (lambda2 / lambda1)
} else {
0.0
};
// Normal vector is eigenvector of smaller eigenvalue
// For [sxx, sxy; sxy, syy], eigenvector of lambda2 is [sxy, lambda2 - sxx]
// or [-lambda2 + syy, sxy] (perpendicular to line direction)
let (a, b) = if sxy.abs() > 1e-10 {
(sxy, lambda2 - sxx)
} else if sxx > syy {
// Line is horizontal (y = const)
(0.0, 1.0)
} else {
// Line is vertical (x = const)
(1.0, 0.0)
};
// Normalize
let norm = (a * a + b * b).sqrt();
if norm < 1e-10 {
return None;
}
let a = a / norm;
let b = b / norm;
// c = -(ax + by) for a point on the line (use centroid)
let c = -(a * cx + b * cy);
Some(Self { a, b, c, quality })
}
/// Compute signed distance from a point to the line.
#[inline]
pub fn distance(&self, p: &Point2D) -> f32 {
self.a * p.x + self.b * p.y + self.c
}
/// Project a point onto the line (used in tests).
#[cfg(test)]
pub fn project(&self, p: &Point2D) -> Point2D {
let d = self.distance(p);
Point2D::new(p.x - self.a * d, p.y - self.b * d)
}
}
#[cfg(test)]
mod tests {
use super::*;
use approx::assert_relative_eq;
#[test]
fn test_line_fit_horizontal() {
let points = vec![
Point2D::new(0.0, 0.0),
Point2D::new(1.0, 0.0),
Point2D::new(2.0, 0.0),
Point2D::new(3.0, 0.0),
];
let line = Line2D::fit(&points).unwrap();
// Horizontal line: y = 0, or 0*x + 1*y + 0 = 0
assert!(
line.quality > 0.99,
"Quality should be high: {}",
line.quality
);
assert!(line.b.abs() > 0.99, "Should be near horizontal");
}
#[test]
fn test_line_fit_vertical() {
let points = vec![
Point2D::new(0.0, 0.0),
Point2D::new(0.0, 1.0),
Point2D::new(0.0, 2.0),
Point2D::new(0.0, 3.0),
];
let line = Line2D::fit(&points).unwrap();
assert!(line.quality > 0.99);
assert!(line.a.abs() > 0.99, "Should be near vertical");
}
#[test]
fn test_line_fit_diagonal() {
let points = vec![
Point2D::new(0.0, 0.0),
Point2D::new(1.0, 1.0),
Point2D::new(2.0, 2.0),
Point2D::new(3.0, 3.0),
];
let line = Line2D::fit(&points).unwrap();
assert!(line.quality > 0.99);
// For y = x, normal is [1, -1]/sqrt(2) or [-1, 1]/sqrt(2)
assert_relative_eq!(line.a.abs(), line.b.abs(), epsilon = 0.01);
}
#[test]
fn test_line_distance() {
let line = Line2D {
a: 0.0,
b: 1.0,
c: -1.0, // y = 1
quality: 1.0,
};
let p1 = Point2D::new(0.0, 1.0);
let p2 = Point2D::new(0.0, 2.0);
let p3 = Point2D::new(0.0, 0.0);
assert_relative_eq!(line.distance(&p1), 0.0, epsilon = 0.001);
assert_relative_eq!(line.distance(&p2), 1.0, epsilon = 0.001);
assert_relative_eq!(line.distance(&p3), -1.0, epsilon = 0.001);
}
#[test]
fn test_line_project() {
let line = Line2D {
a: 0.0,
b: 1.0,
c: -1.0, // y = 1
quality: 1.0,
};
let p = Point2D::new(5.0, 3.0);
let projected = line.project(&p);
assert_relative_eq!(projected.x, 5.0, epsilon = 0.001);
assert_relative_eq!(projected.y, 1.0, epsilon = 0.001);
}
}
dhruva-slam/src/algorithms/matching/point_to_line_icp.rs → dhruva-slam/src/algorithms/matching/point_to_line_icp/mod.rs
+46 -500
View File
@@ -21,172 +21,19 @@
//!
//! - Censi, A. "An ICP variant using a point-to-line metric"
mod config;
mod correspondence;
mod gauss_newton;
mod line2d;
pub use config::PointToLineIcpConfig;
use kiddo::{KdTree, SquaredEuclidean};
use super::icp_common;
use super::icp_common::{self, IdentitySnapConfig, maybe_snap_to_identity};
use super::{ScanMatchResult, ScanMatcher};
use crate::core::types::{Covariance2D, Point2D, PointCloud2D, Pose2D};
/// Configuration for Point-to-Line ICP.
#[derive(Debug, Clone)]
pub struct PointToLineIcpConfig {
/// Maximum number of iterations.
pub max_iterations: u32,
/// Convergence threshold for translation (meters).
pub translation_epsilon: f32,
/// Convergence threshold for rotation (radians).
pub rotation_epsilon: f32,
/// Maximum correspondence distance (meters).
pub max_correspondence_distance: f32,
/// Minimum number of valid correspondences required.
pub min_correspondences: usize,
/// Outlier rejection ratio (0.0 to 1.0).
pub outlier_ratio: f32,
/// Number of neighbors to use for line fitting.
///
/// More neighbors = smoother lines but less local detail.
/// Typically 5-10.
pub line_neighbors: usize,
/// Minimum line fit quality (R² value).
///
/// Correspondences with poor line fits are rejected.
pub min_line_quality: f32,
}
impl Default for PointToLineIcpConfig {
fn default() -> Self {
Self {
max_iterations: 50,
translation_epsilon: 0.001,
rotation_epsilon: 0.001,
max_correspondence_distance: 0.5,
min_correspondences: 10,
outlier_ratio: 0.1,
line_neighbors: 5,
min_line_quality: 0.8,
}
}
}
/// A line in 2D space represented as ax + by + c = 0.
///
/// Normalized so that a² + b² = 1.
#[derive(Debug, Clone, Copy)]
struct Line2D {
/// Normal vector x component
a: f32,
/// Normal vector y component
b: f32,
/// Distance from origin
c: f32,
/// Fit quality (R² value, 0-1)
quality: f32,
}
impl Line2D {
/// Fit a line through a set of points using least squares.
///
/// Returns None if fewer than 2 points or points are collinear.
fn fit(points: &[Point2D]) -> Option<Self> {
if points.len() < 2 {
return None;
}
let n = points.len() as f32;
// Compute centroid
let cx: f32 = points.iter().map(|p| p.x).sum::<f32>() / n;
let cy: f32 = points.iter().map(|p| p.y).sum::<f32>() / n;
// Compute covariance matrix elements
let mut sxx = 0.0f32;
let mut syy = 0.0f32;
let mut sxy = 0.0f32;
for p in points {
let dx = p.x - cx;
let dy = p.y - cy;
sxx += dx * dx;
syy += dy * dy;
sxy += dx * dy;
}
// Eigenvalue decomposition for 2x2 symmetric matrix
// The line direction is the eigenvector with larger eigenvalue
let trace = sxx + syy;
let det = sxx * syy - sxy * sxy;
let discriminant = (trace * trace / 4.0 - det).max(0.0);
let sqrt_disc = discriminant.sqrt();
let lambda1 = trace / 2.0 + sqrt_disc; // Larger eigenvalue
let lambda2 = trace / 2.0 - sqrt_disc; // Smaller eigenvalue
// Quality is ratio of eigenvalues (1.0 = perfect line)
let quality = if lambda1 > 1e-10 {
1.0 - (lambda2 / lambda1)
} else {
0.0
};
// Normal vector is eigenvector of smaller eigenvalue
// For [sxx, sxy; sxy, syy], eigenvector of lambda2 is [sxy, lambda2 - sxx]
// or [-lambda2 + syy, sxy] (perpendicular to line direction)
let (a, b) = if sxy.abs() > 1e-10 {
(sxy, lambda2 - sxx)
} else if sxx > syy {
// Line is horizontal (y = const)
(0.0, 1.0)
} else {
// Line is vertical (x = const)
(1.0, 0.0)
};
// Normalize
let norm = (a * a + b * b).sqrt();
if norm < 1e-10 {
return None;
}
let a = a / norm;
let b = b / norm;
// c = -(ax + by) for a point on the line (use centroid)
let c = -(a * cx + b * cy);
Some(Self { a, b, c, quality })
}
/// Compute signed distance from a point to the line.
#[inline]
fn distance(&self, p: &Point2D) -> f32 {
self.a * p.x + self.b * p.y + self.c
}
/// Project a point onto the line (used in tests).
#[cfg(test)]
fn project(&self, p: &Point2D) -> Point2D {
let d = self.distance(p);
Point2D::new(p.x - self.a * d, p.y - self.b * d)
}
}
/// Correspondence between a source point and a target line.
#[derive(Debug)]
struct Correspondence {
/// Index in source cloud
source_idx: usize,
/// Target line (fitted from neighbors)
target_line: Line2D,
/// Squared distance to line
distance_sq: f32,
}
use crate::core::types::{Covariance2D, PointCloud2D, Pose2D};
use correspondence::{Correspondence, find_correspondences_into};
/// Point-to-Line ICP scan matcher.
///
@@ -240,197 +87,6 @@ impl PointToLineIcp {
icp_common::build_kdtree(cloud)
}
/// Find correspondences with line fitting into preallocated buffer.
/// Assumes transform_source() was called before this.
fn find_correspondences_into(
&self,
target: &PointCloud2D,
target_tree: &KdTree<f32, 2>,
output: &mut Vec<Correspondence>,
) {
output.clear();
let max_dist_sq = self.config.max_correspondence_distance.powi(2);
for i in 0..self.transformed_source.len() {
// Use pre-transformed source point
let tx = self.transformed_source.xs[i];
let ty = self.transformed_source.ys[i];
let transformed = Point2D::new(tx, ty);
// Find k nearest neighbors in target
let neighbors =
target_tree.nearest_n::<SquaredEuclidean>(&[tx, ty], self.config.line_neighbors);
if neighbors.is_empty() {
continue;
}
// Check if closest is within range
let closest_dist_sq = neighbors[0].distance;
if closest_dist_sq > max_dist_sq {
continue;
}
// Gather neighbor points for line fitting using stack array
const MAX_LINE_NEIGHBORS: usize = 16;
let mut neighbor_points: [Point2D; MAX_LINE_NEIGHBORS] =
[Point2D::default(); MAX_LINE_NEIGHBORS];
let mut neighbor_count = 0;
for n in neighbors.iter() {
if n.distance <= max_dist_sq * 4.0 && neighbor_count < MAX_LINE_NEIGHBORS {
neighbor_points[neighbor_count] = target.point_at(n.item as usize);
neighbor_count += 1;
}
}
if neighbor_count < 2 {
continue;
}
// Fit line through neighbors
if let Some(line) = Line2D::fit(&neighbor_points[..neighbor_count]) {
if line.quality < self.config.min_line_quality {
continue;
}
// Compute point-to-line distance
let dist_to_line = line.distance(&transformed);
let dist_sq = dist_to_line * dist_to_line;
output.push(Correspondence {
source_idx: i,
target_line: line,
distance_sq: dist_sq,
});
}
}
// Apply outlier rejection
if self.config.outlier_ratio > 0.0 && !output.is_empty() {
output.sort_by(|a, b| a.distance_sq.partial_cmp(&b.distance_sq).unwrap());
let keep_count = ((1.0 - self.config.outlier_ratio) * output.len() as f32) as usize;
output.truncate(keep_count.max(self.config.min_correspondences));
}
}
/// Compute optimal transform using point-to-line minimization.
///
/// Minimizes sum of squared point-to-line distances.
/// Assumes transform_source() was called before this with current_transform.
/// Uses original source coordinates for Jacobian (derivative of rotation).
fn compute_transform(
&self,
source: &PointCloud2D,
correspondences: &[Correspondence],
current_transform: &Pose2D,
) -> Pose2D {
if correspondences.len() < 3 {
return Pose2D::identity();
}
let (sin_t, cos_t) = current_transform.theta.sin_cos();
// Build linear system: A * [dx, dy, dtheta]^T = b
// Using Gauss-Newton with point-to-line residuals
//
// Residual for correspondence i:
// r_i = n_i · (R(theta) * p_i + t - q_i)
// where n_i is line normal, p_i is source point, q_i is target point
//
// Linearized:
// ∂r/∂x = n_x
// ∂r/∂y = n_y
// ∂r/∂theta = n_x * (-sin(theta) * p_x - cos(theta) * p_y)
// + n_y * (cos(theta) * p_x - sin(theta) * p_y)
// Normal equations: H * delta = g
// H = J^T * J (6x6 for 3D, 3x3 for 2D)
// g = -J^T * r
// Initialize 3x3 system
let mut h = [[0.0f32; 3]; 3]; // Hessian approximation
let mut g = [0.0f32; 3]; // Gradient
for corr in correspondences {
// Original source coordinates for Jacobian (derivative is w.r.t. original point)
let px_orig = source.xs[corr.source_idx];
let py_orig = source.ys[corr.source_idx];
let n_x = corr.target_line.a;
let n_y = corr.target_line.b;
// Use pre-transformed source point for residual
let px = self.transformed_source.xs[corr.source_idx];
let py = self.transformed_source.ys[corr.source_idx];
// Residual: r = n · p_transformed + c
let r = n_x * px + n_y * py + corr.target_line.c;
// Jacobian row
// The derivative ∂r/∂θ uses the original point because:
// p_transformed = R(θ) * p_original + t
// So ∂p_transformed/∂θ = ∂R/∂θ * p_original
let j0 = n_x;
let j1 = n_y;
let j2 = n_x * (-sin_t * px_orig - cos_t * py_orig)
+ n_y * (cos_t * px_orig - sin_t * py_orig);
// Accumulate H = J^T * J
h[0][0] += j0 * j0;
h[0][1] += j0 * j1;
h[0][2] += j0 * j2;
h[1][0] += j1 * j0;
h[1][1] += j1 * j1;
h[1][2] += j1 * j2;
h[2][0] += j2 * j0;
h[2][1] += j2 * j1;
h[2][2] += j2 * j2;
// Accumulate g = -J^T * r
g[0] -= j0 * r;
g[1] -= j1 * r;
g[2] -= j2 * r;
}
// Solve 3x3 system using Cramer's rule (small system, direct solve)
let det = h[0][0] * (h[1][1] * h[2][2] - h[1][2] * h[2][1])
- h[0][1] * (h[1][0] * h[2][2] - h[1][2] * h[2][0])
+ h[0][2] * (h[1][0] * h[2][1] - h[1][1] * h[2][0]);
if det.abs() < 1e-10 {
return Pose2D::identity();
}
let inv_det = 1.0 / det;
// Compute inverse of H
let h_inv = [
[
(h[1][1] * h[2][2] - h[1][2] * h[2][1]) * inv_det,
(h[0][2] * h[2][1] - h[0][1] * h[2][2]) * inv_det,
(h[0][1] * h[1][2] - h[0][2] * h[1][1]) * inv_det,
],
[
(h[1][2] * h[2][0] - h[1][0] * h[2][2]) * inv_det,
(h[0][0] * h[2][2] - h[0][2] * h[2][0]) * inv_det,
(h[0][2] * h[1][0] - h[0][0] * h[1][2]) * inv_det,
],
[
(h[1][0] * h[2][1] - h[1][1] * h[2][0]) * inv_det,
(h[0][1] * h[2][0] - h[0][0] * h[2][1]) * inv_det,
(h[0][0] * h[1][1] - h[0][1] * h[1][0]) * inv_det,
],
];
// delta = H^-1 * g
let dx = h_inv[0][0] * g[0] + h_inv[0][1] * g[1] + h_inv[0][2] * g[2];
let dy = h_inv[1][0] * g[0] + h_inv[1][1] * g[1] + h_inv[1][2] * g[2];
let dtheta = h_inv[2][0] * g[0] + h_inv[2][1] * g[1] + h_inv[2][2] * g[2];
Pose2D::new(dx, dy, dtheta)
}
/// Compute mean squared error of correspondences (point-to-line distance).
///
/// Note: This is the point-to-line MSE, which is typically smaller than
@@ -510,7 +166,7 @@ impl PointToLineIcp {
target_tree: &KdTree<f32, 2>,
initial_guess: &Pose2D,
) -> ScanMatchResult {
// FIX #3: Sparse scan handling - accept with lower confidence
// Sparse scan handling - accept with lower confidence
// Real lidar scans typically have 100-360 points after preprocessing
// Threshold of 30 allows tests with synthetic clouds to pass
const MIN_POINTS_FOR_RELIABLE_MATCH: usize = 30;
@@ -539,7 +195,7 @@ impl PointToLineIcp {
let mut current_transform = *initial_guess;
let mut iterations = 0u32;
// FIX #1: Consecutive failure tracking - only terminate after multiple bad iterations
// Consecutive failure tracking - only terminate after multiple bad iterations
let mut consecutive_increases = 0u32;
const MAX_CONSECUTIVE_INCREASES: u32 = 3;
let mut last_mse = f32::MAX;
@@ -551,7 +207,13 @@ impl PointToLineIcp {
self.transform_source(source, &current_transform);
// Find correspondences with line fitting using pre-transformed cloud
self.find_correspondences_into(target, target_tree, &mut corr_buffer);
find_correspondences_into(
&self.config,
&self.transformed_source,
target,
target_tree,
&mut corr_buffer,
);
if corr_buffer.len() < self.config.min_correspondences {
// Return buffer before early exit
@@ -560,7 +222,12 @@ impl PointToLineIcp {
}
// Compute incremental transform (needs original source for Jacobian)
let delta = self.compute_transform(source, &corr_buffer, &current_transform);
let delta = gauss_newton::compute_transform(
source,
&self.transformed_source,
&corr_buffer,
&current_transform,
);
// Apply delta
current_transform = current_transform.compose(&delta);
@@ -579,65 +246,32 @@ impl PointToLineIcp {
let p2p_mse = self.compute_p2p_mse(target, target_tree);
let score = self.mse_to_score(p2p_mse);
// Identity snapping: If we have a high-quality match and both the initial guess
// and final transform are very close to identity, snap to the initial guess.
// This prevents quantization drift in static/slow-motion scenarios.
//
// We check translation and rotation separately with appropriate thresholds:
// - Translation: 3cm (typical encoder noise)
// - Rotation: 0.05 rad (~3°, typical gyro/P2L drift for static scans)
let final_transform = if score > 0.95 {
let init_trans = (initial_guess.x.powi(2) + initial_guess.y.powi(2)).sqrt();
let init_rot = initial_guess.theta.abs();
let curr_trans =
(current_transform.x.powi(2) + current_transform.y.powi(2)).sqrt();
let curr_rot = current_transform.theta.abs();
// If both initial guess and result are near-identity, snap to initial
// This prevents P2L aperture problem from causing rotation drift
if init_trans < 0.03 && init_rot < 0.05 && curr_trans < 0.03 && curr_rot < 0.05
{
*initial_guess
} else {
current_transform
}
} else {
current_transform
};
// Identity snapping: prevent drift in static/slow-motion scenarios
let snap_config = IdentitySnapConfig::default();
let final_transform =
maybe_snap_to_identity(score, initial_guess, &current_transform, &snap_config);
// Return buffer before early exit
self.correspondence_buffer = corr_buffer;
return ScanMatchResult::success(final_transform, score, iterations, p2p_mse);
}
// FIX #4: Transform-based convergence - if delta is tiny, accept
// Transform-based convergence - if delta is tiny, accept
if translation_change < 0.001 && rotation_change < 0.001 {
// Compute point-to-point MSE for fair comparison
let p2p_mse = self.compute_p2p_mse(target, target_tree);
let score = self.mse_to_score(p2p_mse);
// Identity snapping (same as above)
let final_transform = if score > 0.95 {
let init_trans = (initial_guess.x.powi(2) + initial_guess.y.powi(2)).sqrt();
let init_rot = initial_guess.theta.abs();
let curr_trans =
(current_transform.x.powi(2) + current_transform.y.powi(2)).sqrt();
let curr_rot = current_transform.theta.abs();
if init_trans < 0.03 && init_rot < 0.05 && curr_trans < 0.03 && curr_rot < 0.05
{
*initial_guess
} else {
current_transform
}
} else {
current_transform
};
// Identity snapping: prevent drift in static/slow-motion scenarios
let snap_config = IdentitySnapConfig::default();
let final_transform =
maybe_snap_to_identity(score, initial_guess, &current_transform, &snap_config);
self.correspondence_buffer = corr_buffer;
return ScanMatchResult::success(final_transform, score, iterations, p2p_mse);
}
// FIX #1: Check if MSE is diverging (consecutive check)
// Check if MSE is diverging (consecutive check)
// Only count as increase if worse than last iteration by significant margin
if mse > last_mse * 1.1 {
consecutive_increases += 1;
@@ -653,26 +287,22 @@ impl PointToLineIcp {
// Max iterations reached - compute final quality
self.transform_source(source, &current_transform);
self.find_correspondences_into(target, target_tree, &mut corr_buffer);
find_correspondences_into(
&self.config,
&self.transformed_source,
target,
target_tree,
&mut corr_buffer,
);
// Use point-to-point MSE for fair comparison with other algorithms
let p2p_mse = self.compute_p2p_mse(target, target_tree);
let score = self.mse_to_score(p2p_mse);
// Identity snapping for max-iteration case
let final_transform = if score > 0.95 {
let init_trans = (initial_guess.x.powi(2) + initial_guess.y.powi(2)).sqrt();
let init_rot = initial_guess.theta.abs();
let curr_trans = (current_transform.x.powi(2) + current_transform.y.powi(2)).sqrt();
let curr_rot = current_transform.theta.abs();
if init_trans < 0.03 && init_rot < 0.05 && curr_trans < 0.03 && curr_rot < 0.05 {
*initial_guess
} else {
current_transform
}
} else {
current_transform
};
let snap_config = IdentitySnapConfig::default();
let final_transform =
maybe_snap_to_identity(score, initial_guess, &current_transform, &snap_config);
// Return buffer for reuse
self.correspondence_buffer = corr_buffer;
@@ -714,6 +344,7 @@ impl ScanMatcher for PointToLineIcp {
#[cfg(test)]
mod tests {
use super::*;
use crate::core::types::Point2D;
use approx::assert_relative_eq;
/// Create an L-shaped point cloud (two perpendicular walls)
@@ -763,91 +394,6 @@ mod tests {
cloud
}
#[test]
fn test_line_fit_horizontal() {
let points = vec![
Point2D::new(0.0, 0.0),
Point2D::new(1.0, 0.0),
Point2D::new(2.0, 0.0),
Point2D::new(3.0, 0.0),
];
let line = Line2D::fit(&points).unwrap();
// Horizontal line: y = 0, or 0*x + 1*y + 0 = 0
assert!(
line.quality > 0.99,
"Quality should be high: {}",
line.quality
);
assert!(line.b.abs() > 0.99, "Should be near horizontal");
}
#[test]
fn test_line_fit_vertical() {
let points = vec![
Point2D::new(0.0, 0.0),
Point2D::new(0.0, 1.0),
Point2D::new(0.0, 2.0),
Point2D::new(0.0, 3.0),
];
let line = Line2D::fit(&points).unwrap();
assert!(line.quality > 0.99);
assert!(line.a.abs() > 0.99, "Should be near vertical");
}
#[test]
fn test_line_fit_diagonal() {
let points = vec![
Point2D::new(0.0, 0.0),
Point2D::new(1.0, 1.0),
Point2D::new(2.0, 2.0),
Point2D::new(3.0, 3.0),
];
let line = Line2D::fit(&points).unwrap();
assert!(line.quality > 0.99);
// For y = x, normal is [1, -1]/sqrt(2) or [-1, 1]/sqrt(2)
assert_relative_eq!(line.a.abs(), line.b.abs(), epsilon = 0.01);
}
#[test]
fn test_line_distance() {
let line = Line2D {
a: 0.0,
b: 1.0,
c: -1.0, // y = 1
quality: 1.0,
};
let p1 = Point2D::new(0.0, 1.0);
let p2 = Point2D::new(0.0, 2.0);
let p3 = Point2D::new(0.0, 0.0);
assert_relative_eq!(line.distance(&p1), 0.0, epsilon = 0.001);
assert_relative_eq!(line.distance(&p2), 1.0, epsilon = 0.001);
assert_relative_eq!(line.distance(&p3), -1.0, epsilon = 0.001);
}
#[test]
fn test_line_project() {
let line = Line2D {
a: 0.0,
b: 1.0,
c: -1.0, // y = 1
quality: 1.0,
};
let p = Point2D::new(5.0, 3.0);
let projected = line.project(&p);
assert_relative_eq!(projected.x, 5.0, epsilon = 0.001);
assert_relative_eq!(projected.y, 1.0, epsilon = 0.001);
}
#[test]
fn test_identity_transform() {
let cloud = create_l_shape(50, 2.0);
dhruva-slam/src/algorithms/matching/robust_kernels.rs
+208
View File
@@ -0,0 +1,208 @@
//! Robust kernel functions for M-estimator weighting in ICP.
//!
//! Robust kernels down-weight outliers during scan matching to improve
//! convergence in the presence of noise, occlusions, and dynamic objects.
//!
//! # Available Kernels
//!
//! - [`RobustKernel::None`]: Standard least squares (no robustness)
//! - [`RobustKernel::Huber`]: Linear for large errors, quadratic for small
//! - [`RobustKernel::Cauchy`]: Heavy-tailed, very robust to outliers
//! - [`RobustKernel::Welsch`]: Smooth, strong outlier rejection (default)
//!
//! # Usage
//!
//! ```ignore
//! let kernel = RobustKernel::Welsch;
//! let scale = 0.10; // 10cm
//! let weight = kernel.weight(residual_sq, scale);
//! ```
/// Robust kernel type for M-estimator weighting.
///
/// These kernels are used to down-weight outlier correspondences during
/// ICP optimization, improving robustness to noise and occlusions.
#[derive(Debug, Clone, Copy, PartialEq, Default)]
pub enum RobustKernel {
/// No robust weighting (standard least squares).
///
/// All correspondences are weighted equally. Fast but sensitive to outliers.
None,
/// Huber kernel: linear for large errors, quadratic for small.
///
/// Good balance between robustness and efficiency. The influence function
/// is bounded, making it robust to outliers while maintaining efficiency
/// for inliers.
///
/// Weight: `1` for `|r| < c`, `c/|r|` for `|r| >= c`
Huber,
/// Cauchy kernel: heavy-tailed, very robust to outliers.
///
/// Also known as the Lorentzian kernel. Provides stronger outlier
/// rejection than Huber but may be less efficient for inliers.
///
/// Loss: `ρ(r) = (c²/2) * log(1 + (r/c)²)`
/// Weight: `1 / (1 + (r/c)²)`
Cauchy,
/// Welsch kernel: smooth, strong outlier rejection.
///
/// Provides the strongest outlier rejection with a smooth influence
/// function that approaches zero for large residuals.
///
/// Loss: `ρ(r) = (c²/2) * (1 - exp(-(r/c)²))`
/// Weight: `exp(-(r/c)²)`
#[default]
Welsch,
}
impl RobustKernel {
/// Compute the weight for a given squared residual.
///
/// The weight is the derivative of the robust loss function, used in
/// iteratively reweighted least squares (IRLS) optimization.
///
/// # Arguments
/// * `residual_sq` - Squared residual (distance²)
/// * `scale` - Kernel scale parameter (in same units as residual)
///
/// # Returns
/// Weight in [0, 1] that down-weights outliers.
///
/// # Example
/// ```
/// use dhruva_slam::algorithms::matching::RobustKernel;
///
/// let kernel = RobustKernel::Welsch;
/// let scale = 0.10; // 10cm
///
/// // Small residual -> high weight
/// let w1 = kernel.weight(0.001, scale); // 1cm² residual
/// assert!(w1 > 0.9);
///
/// // Large residual -> low weight
/// let w2 = kernel.weight(0.25, scale); // 50cm² residual
/// assert!(w2 < 0.1);
/// ```
#[inline(always)]
pub fn weight(&self, residual_sq: f32, scale: f32) -> f32 {
let c_sq = scale * scale;
match self {
RobustKernel::None => 1.0,
RobustKernel::Huber => {
// Huber weight: 1 for |r| < c, c/|r| for |r| >= c
let r = residual_sq.sqrt();
if r < scale { 1.0 } else { scale / r }
}
RobustKernel::Cauchy => {
// Cauchy weight: 1 / (1 + (r/c)²)
1.0 / (1.0 + residual_sq / c_sq)
}
RobustKernel::Welsch => {
// Welsch weight: exp(-(r/c)²)
(-residual_sq / c_sq).exp()
}
}
}
/// Check if this kernel provides outlier robustness.
#[inline]
pub fn is_robust(&self) -> bool {
!matches!(self, RobustKernel::None)
}
}
#[cfg(test)]
mod tests {
use super::*;
#[test]
fn test_none_kernel_always_one() {
let kernel = RobustKernel::None;
assert_eq!(kernel.weight(0.0, 0.1), 1.0);
assert_eq!(kernel.weight(0.01, 0.1), 1.0);
assert_eq!(kernel.weight(1.0, 0.1), 1.0);
assert_eq!(kernel.weight(100.0, 0.1), 1.0);
}
#[test]
fn test_huber_kernel() {
let kernel = RobustKernel::Huber;
let scale = 0.1;
// Within threshold: weight = 1
assert_eq!(kernel.weight(0.0, scale), 1.0);
assert_eq!(kernel.weight(0.005 * 0.005, scale), 1.0); // 0.5cm < 10cm
// Beyond threshold: weight = c/|r|
let w = kernel.weight(0.04, scale); // 20cm residual
assert!((w - 0.5).abs() < 0.01); // 0.1 / 0.2 = 0.5
}
#[test]
fn test_cauchy_kernel() {
let kernel = RobustKernel::Cauchy;
let scale = 0.1;
// Zero residual: weight = 1
assert_eq!(kernel.weight(0.0, scale), 1.0);
// Residual = scale: weight = 0.5
let w = kernel.weight(0.01, scale); // r² = c²
assert!((w - 0.5).abs() < 0.01);
// Large residual: weight approaches 0
let w_large = kernel.weight(1.0, scale);
assert!(w_large < 0.01);
}
#[test]
fn test_welsch_kernel() {
let kernel = RobustKernel::Welsch;
let scale = 0.1;
// Zero residual: weight = 1
assert!((kernel.weight(0.0, scale) - 1.0).abs() < 1e-6);
// Residual = scale: weight = exp(-1) ≈ 0.368
let w = kernel.weight(0.01, scale);
assert!((w - (-1.0_f32).exp()).abs() < 0.01);
// Large residual: weight approaches 0
let w_large = kernel.weight(1.0, scale);
assert!(w_large < 0.001);
}
#[test]
fn test_default_is_welsch() {
assert_eq!(RobustKernel::default(), RobustKernel::Welsch);
}
#[test]
fn test_is_robust() {
assert!(!RobustKernel::None.is_robust());
assert!(RobustKernel::Huber.is_robust());
assert!(RobustKernel::Cauchy.is_robust());
assert!(RobustKernel::Welsch.is_robust());
}
#[test]
fn test_weights_decrease_with_residual() {
for kernel in [
RobustKernel::Huber,
RobustKernel::Cauchy,
RobustKernel::Welsch,
] {
let scale = 0.1;
let w1 = kernel.weight(0.001, scale);
let w2 = kernel.weight(0.01, scale);
let w3 = kernel.weight(0.1, scale);
assert!(w1 >= w2, "{:?}: w1={} should be >= w2={}", kernel, w1, w2);
assert!(w2 >= w3, "{:?}: w2={} should be >= w3={}", kernel, w2, w3);
}
}
}
dhruva-slam/src/core/simd/f32x4.rs
+122
View File
@@ -135,6 +135,81 @@ impl Float4 {
let sum23 = self.0[2] + self.0[3];
sum01 + sum23
}
/// Fused multiply-add: `(self * a) + b`.
///
/// Computes `self * a + b` with better precision and performance than
/// separate multiply and add operations. On ARM NEON, this maps to
/// `vfmaq_f32` (VFPv4) or equivalent.
///
/// # Example
/// ```
/// use dhruva_slam::core::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 multiply-subtract: `(self * a) - b`.
///
/// Computes `self * a - b` with better precision and performance than
/// separate multiply and subtract operations.
///
/// # Example
/// ```
/// use dhruva_slam::core::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([1.0, 1.0, 1.0, 1.0]);
/// let result = a.mul_sub(b, c);
/// // (1*2-1, 2*2-1, 3*2-1, 4*2-1) = (1, 3, 5, 7)
/// assert_eq!(result.to_array(), [1.0, 3.0, 5.0, 7.0]);
/// ```
#[inline(always)]
pub fn mul_sub(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 dhruva_slam::core::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]),
])
}
}
impl Add for Float4 {
@@ -289,4 +364,51 @@ mod tests {
let zero = Float4::splat(0.0);
assert_eq!(zero.to_array(), [0.0, 0.0, 0.0, 0.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);
// (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]);
}
#[test]
fn test_mul_sub() {
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([1.0, 1.0, 1.0, 1.0]);
let result = a.mul_sub(b, c);
// (1*2-1, 2*2-1, 3*2-1, 4*2-1) = (1, 3, 5, 7)
assert_eq!(result.to_array(), [1.0, 3.0, 5.0, 7.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);
// (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]);
}
#[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 = x*0 - y*1 = -y
let new_xs = xs.mul_sub(cos_t, ys * sin_t);
// y' = x*sin + y*cos = x*1 + y*0 = x
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]);
}
}
dhruva-slam/src/core/types/scan.rs
+30 -25
View File
@@ -493,7 +493,9 @@ impl PointCloud2D {
total_y += self.ys[i];
}
Some(Point2D::new(total_x / n as f32, total_y / n as f32))
// Use reciprocal multiplication (faster than division on ARM)
let inv_n = 1.0 / (n as f32);
Some(Point2D::new(total_x * inv_n, total_y * inv_n))
}
/// SIMD-accelerated transform of all points by a pose.
@@ -514,25 +516,26 @@ impl PointCloud2D {
let chunks = n / 4;
// Process 4 points at a time with SIMD
// Process 4 points at a time with SIMD using FMA
for i in 0..chunks {
let base = i * 4;
let xs = Float4::new(self.xs[base..base + 4].try_into().unwrap());
let ys = Float4::new(self.ys[base..base + 4].try_into().unwrap());
// x' = tx + x*cos - y*sin
// y' = ty + x*sin + y*cos
let new_xs = tx_v + xs * cos_v - ys * sin_v;
let new_ys = ty_v + xs * sin_v + ys * cos_v;
// x' = tx + x*cos - y*sin → x*cos + (tx - y*sin)
// y' = ty + x*sin + y*cos → x*sin + (ty + y*cos)
// Using FMA: mul_add(a,b) = self*a + b, neg_mul_add(a,b) = -self*a + b = b - self*a
let new_xs = xs.mul_add(cos_v, ys.neg_mul_add(sin_v, tx_v));
let new_ys = xs.mul_add(sin_v, ys.mul_add(cos_v, ty_v));
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 with scalar
// Handle remainder with scalar FMA
for i in (chunks * 4)..n {
result.xs[i] = pose.x + self.xs[i] * cos_t - self.ys[i] * sin_t;
result.ys[i] = pose.y + self.xs[i] * sin_t + self.ys[i] * cos_t;
result.xs[i] = self.xs[i].mul_add(cos_t, (-self.ys[i]).mul_add(sin_t, pose.x));
result.ys[i] = self.xs[i].mul_add(sin_t, self.ys[i].mul_add(cos_t, pose.y));
}
result.intensities = self.intensities.clone();
@@ -551,25 +554,27 @@ impl PointCloud2D {
let chunks = n / 4;
// Process 4 points at a time with SIMD
// Process 4 points at a time with SIMD using FMA
for i in 0..chunks {
let base = i * 4;
let xs = Float4::new(self.xs[base..base + 4].try_into().unwrap());
let ys = Float4::new(self.ys[base..base + 4].try_into().unwrap());
let new_xs = tx_v + xs * cos_v - ys * sin_v;
let new_ys = ty_v + xs * sin_v + ys * cos_v;
// x' = tx + x*cos - y*sin → x*cos + (tx - y*sin)
// y' = ty + x*sin + y*cos → x*sin + (ty + y*cos)
let new_xs = xs.mul_add(cos_v, ys.neg_mul_add(sin_v, tx_v));
let new_ys = xs.mul_add(sin_v, ys.mul_add(cos_v, ty_v));
self.xs[base..base + 4].copy_from_slice(&new_xs.to_array());
self.ys[base..base + 4].copy_from_slice(&new_ys.to_array());
}
// Handle remainder with scalar
// Handle remainder with scalar FMA
for i in (chunks * 4)..n {
let new_x = pose.x + self.xs[i] * cos_t - self.ys[i] * sin_t;
let new_y = pose.y + self.xs[i] * sin_t + self.ys[i] * cos_t;
self.xs[i] = new_x;
self.ys[i] = new_y;
let old_x = self.xs[i];
let old_y = self.ys[i];
self.xs[i] = old_x.mul_add(cos_t, (-old_y).mul_add(sin_t, pose.x));
self.ys[i] = old_x.mul_add(sin_t, old_y.mul_add(cos_t, pose.y));
}
}
@@ -589,7 +594,7 @@ impl PointCloud2D {
let chunks = n / 4;
// Process 4 points at a time with SIMD
// Process 4 points at a time with SIMD using FMA
for i in 0..chunks {
let base = i * 4;
let xs = Float4::new(self.xs[base..base + 4].try_into().unwrap());
@@ -598,21 +603,21 @@ impl PointCloud2D {
let dx = xs - px_v;
let dy = ys - py_v;
// x' = dx*cos + dy*sin
// y' = -dx*sin + dy*cos
let new_xs = dx * cos_v + dy * sin_v;
let new_ys = dy * cos_v - dx * sin_v;
// x' = dx*cos + dy*sin → dx.mul_add(cos, dy*sin)
// y' = dy*cos - dx*sin → dy.mul_add(cos, dx.neg_mul_add(sin, 0)) = dx.neg_mul_add(sin, dy*cos)
let new_xs = dx.mul_add(cos_v, dy * sin_v);
let new_ys = dx.neg_mul_add(sin_v, dy * cos_v);
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 with scalar
// Handle remainder with scalar FMA
for i in (chunks * 4)..n {
let dx = self.xs[i] - pose.x;
let dy = self.ys[i] - pose.y;
result.xs[i] = dx * cos_t + dy * sin_t;
result.ys[i] = -dx * sin_t + dy * cos_t;
result.xs[i] = dx.mul_add(cos_t, dy * sin_t);
result.ys[i] = (-dx).mul_add(sin_t, dy * cos_t);
}
result.intensities = self.intensities.clone();
dhruva-slam/src/engine/graph/loop_detector.rs
+10
View File
@@ -8,6 +8,16 @@
//! 1. **Distance-based trigger**: Only check for loops near previous poses
//! 2. **Scan context matching**: Use global descriptor for place recognition
//! 3. **Geometric verification**: Validate matches with scan matching
//!
//! # TODO: Future Integration
//!
//! This module detects loop closures but does not yet trigger pose graph
//! optimization. Future work needed:
//!
//! - Wire detected closures into `GraphOptimizer` to add edges
//! - Call optimization when sufficient closures are detected
//! - Update map after optimization converges
//! - Add loop closure visualization/diagnostics
use crate::algorithms::matching::{IcpConfig, PointToPointIcp, ScanMatcher};
use crate::core::types::{PointCloud2D, Pose2D};
dhruva-slam/src/engine/slam/online_slam.rs
+8
View File
@@ -133,6 +133,14 @@ pub struct OnlineSlamConfig {
pub lost_threshold: f32,
/// Whether to use scan-to-submap matching (vs scan-to-scan).
///
/// **WARNING**: Currently disabled by default due to a semantic mismatch:
/// `match_to_submap()` returns an absolute pose in the global frame, but
/// `apply_scan_match()` expects a relative transform (delta). This needs
/// to be fixed before enabling submap matching.
///
/// TODO: Fix `match_to_submap()` to return delta transform instead of
/// absolute pose, or modify `apply_scan_match()` to handle both cases.
pub use_submap_matching: bool,
/// Minimum points in scan to process.
dhruva-slam/src/lib.rs
+1 -2
View File
@@ -162,8 +162,7 @@ pub use algorithms::mapping::{
// Algorithms - Localization
pub use algorithms::localization::{
LikelihoodFieldModel, MotionModel, MotionModelConfig, Particle, ParticleFilter,
ParticleFilterConfig, ParticleFilterState, SensorModel, SensorModelConfig,
LikelihoodFieldModel, MotionModel, MotionModelConfig, SensorModel, SensorModelConfig,
};
// Engine - SLAM