Add robust ICP with M-estimators, multi-resolution matching, and RANSAC line extraction
Major improvements to scan matching accuracy and convergence: - Add Levenberg-Marquardt damping to Gauss-Newton solver for robust convergence from poor initial guesses (replaces pure Gauss-Newton) - Add IRLS (Iteratively Reweighted Least Squares) with M-estimators: Huber (default), Cauchy, Tukey, Geman-McClure for soft outlier rejection - Add multi-resolution ICP (coarse-to-fine) for better convergence - Add corner correspondences for point-to-point constraints - Add DOF-corrected covariance computation for accurate uncertainty Line extraction improvements: - Add RANSAC-based line extraction for robustness to outliers - Add hybrid extraction mode (RANSAC + split-merge) New modules: - src/matching/robust.rs: M-estimator cost functions - src/motion_model.rs: Constant-velocity motion model for pose prediction - src/extraction/ransac_lines.rs: RANSAC line extraction Test harness: - Add fast mode (~22x speedup) for integration tests - Simplify observation records
This commit is contained in:
codetiger
@@ -7,6 +7,8 @@
|
||||
//!
|
||||
//! - **Line Fitting**: Total Least Squares (orthogonal regression), with weighted variant
|
||||
//! - **Line Extraction**: Split-and-Merge algorithm for ordered point sequences
|
||||
//! - **RANSAC Lines**: Random Sample Consensus for robust line extraction
|
||||
//! - **Hybrid Extraction**: RANSAC for dominant lines, then split-merge for remaining
|
||||
//! - **Corner Detection**: Find corners at line intersections
|
||||
//!
|
||||
//! # Weighted Line Fitting
|
||||
@@ -22,9 +24,26 @@
|
||||
//! let points = vec![/* lidar hits */];
|
||||
//! let line = fit_line_from_sensor(&points, sensor_pos, None);
|
||||
//! ```
|
||||
//!
|
||||
//! # Hybrid Extraction
|
||||
//!
|
||||
//! The hybrid approach combines RANSAC's robustness with split-merge's
|
||||
//! ability to handle structured point sequences:
|
||||
//!
|
||||
//! ```rust,ignore
|
||||
//! use vastu_map::extraction::{extract_lines_hybrid, RansacLineConfig, SplitMergeConfig};
|
||||
//!
|
||||
//! let ransac_config = RansacLineConfig::default();
|
||||
//! let split_merge_config = SplitMergeConfig::default();
|
||||
//! let lines = extract_lines_hybrid(&points, &ransac_config, &split_merge_config);
|
||||
//! ```
|
||||
|
||||
use crate::core::Point2D;
|
||||
use crate::features::Line2D;
|
||||
|
||||
pub mod corner_detection;
|
||||
pub mod line_fitting;
|
||||
pub mod ransac_lines;
|
||||
pub mod split_merge;
|
||||
pub mod traits;
|
||||
|
||||
@@ -36,9 +55,67 @@ pub use line_fitting::{
|
||||
compute_range_weights, fit_line, fit_line_from_sensor, fit_line_segment, fit_line_weighted,
|
||||
fitting_error, fitting_error_weighted, max_distance_point,
|
||||
};
|
||||
// Re-export from ransac_lines module
|
||||
pub use ransac_lines::{RansacLineConfig, extract_lines_ransac};
|
||||
// Re-export from split_merge submodule
|
||||
pub use split_merge::{
|
||||
SplitMergeConfig, adaptive_split_threshold, extract_lines, extract_lines_from_sensor,
|
||||
};
|
||||
// Re-export traits
|
||||
pub use traits::{LineExtractor, SplitMergeExtractor};
|
||||
|
||||
/// Extract lines using hybrid RANSAC + split-merge approach.
|
||||
///
|
||||
/// This method combines RANSAC's robustness to outliers with split-merge's
|
||||
/// ability to handle structured point sequences:
|
||||
///
|
||||
/// 1. **RANSAC first**: Extracts dominant lines that may span across gaps
|
||||
/// 2. **Split-merge second**: Processes remaining points for additional structure
|
||||
///
|
||||
/// This is useful when:
|
||||
/// - Point cloud has significant outliers (RANSAC handles these)
|
||||
/// - Some features are weak/fragmented (split-merge catches these)
|
||||
/// - You want the best of both algorithms
|
||||
///
|
||||
/// # Arguments
|
||||
/// * `points` - Input point cloud
|
||||
/// * `ransac_config` - Configuration for RANSAC extraction
|
||||
/// * `split_merge_config` - Configuration for split-merge extraction
|
||||
///
|
||||
/// # Returns
|
||||
/// Combined list of extracted lines from both algorithms.
|
||||
///
|
||||
/// # Example
|
||||
/// ```rust,ignore
|
||||
/// use vastu_map::extraction::{extract_lines_hybrid, RansacLineConfig, SplitMergeConfig};
|
||||
///
|
||||
/// let ransac_config = RansacLineConfig::default()
|
||||
/// .with_max_lines(5) // Limit RANSAC to 5 dominant lines
|
||||
/// .with_inlier_threshold(0.02);
|
||||
///
|
||||
/// let split_merge_config = SplitMergeConfig::default();
|
||||
///
|
||||
/// let all_lines = extract_lines_hybrid(&points, &ransac_config, &split_merge_config);
|
||||
/// ```
|
||||
pub fn extract_lines_hybrid(
|
||||
points: &[Point2D],
|
||||
ransac_config: &RansacLineConfig,
|
||||
split_merge_config: &SplitMergeConfig,
|
||||
) -> Vec<Line2D> {
|
||||
if points.is_empty() {
|
||||
return Vec::new();
|
||||
}
|
||||
|
||||
// Step 1: RANSAC for dominant lines
|
||||
let (mut lines, remaining_indices) = extract_lines_ransac(points, ransac_config);
|
||||
|
||||
// Step 2: Split-merge on remaining points
|
||||
if remaining_indices.len() >= 3 {
|
||||
let remaining_points: Vec<Point2D> = remaining_indices.iter().map(|&i| points[i]).collect();
|
||||
|
||||
let additional_lines = extract_lines(&remaining_points, split_merge_config);
|
||||
lines.extend(additional_lines);
|
||||
}
|
||||
|
||||
lines
|
||||
}
|
||||
|
||||
@@ -0,0 +1,598 @@
|
||||
//! RANSAC-based line extraction.
|
||||
//!
|
||||
//! This module provides RANSAC (Random Sample Consensus) for extracting
|
||||
//! dominant lines from point clouds. RANSAC is robust to outliers and
|
||||
//! can find strong lines even in cluttered environments.
|
||||
//!
|
||||
//! # Usage
|
||||
//!
|
||||
//! ```rust,ignore
|
||||
//! use vastu_map::extraction::{extract_lines_ransac, RansacLineConfig};
|
||||
//!
|
||||
//! let config = RansacLineConfig::default();
|
||||
//! let (lines, remaining) = extract_lines_ransac(&points, &config);
|
||||
//!
|
||||
//! // `lines` contains extracted lines
|
||||
//! // `remaining` contains indices of points not matched to any line
|
||||
//! ```
|
||||
|
||||
use crate::core::Point2D;
|
||||
use crate::features::Line2D;
|
||||
|
||||
use super::line_fitting::fit_line;
|
||||
|
||||
/// Configuration for RANSAC line extraction.
|
||||
#[derive(Clone, Debug)]
|
||||
pub struct RansacLineConfig {
|
||||
/// Number of RANSAC iterations per line.
|
||||
/// More iterations = better chance of finding optimal line.
|
||||
/// Default: 100
|
||||
pub iterations: usize,
|
||||
|
||||
/// Inlier distance threshold (meters).
|
||||
/// Points within this distance of a line are considered inliers.
|
||||
/// Default: 0.02m (2cm)
|
||||
pub inlier_threshold: f32,
|
||||
|
||||
/// Minimum inliers for a valid line.
|
||||
/// Lines with fewer inliers are rejected.
|
||||
/// Default: 5
|
||||
pub min_inliers: usize,
|
||||
|
||||
/// Minimum line length (meters).
|
||||
/// Lines shorter than this are rejected.
|
||||
/// Default: 0.1m (10cm)
|
||||
pub min_line_length: f32,
|
||||
|
||||
/// Maximum gap between consecutive inliers along a line (meters).
|
||||
/// If inliers have a gap larger than this, the line is split.
|
||||
/// This prevents connecting collinear walls separated by doorways.
|
||||
/// Default: 0.10m (10cm)
|
||||
pub max_point_gap: f32,
|
||||
|
||||
/// Maximum lines to extract (0 = unlimited).
|
||||
/// Stops after extracting this many lines.
|
||||
/// Default: 0 (unlimited)
|
||||
pub max_lines: usize,
|
||||
|
||||
/// Random seed for reproducibility.
|
||||
/// 0 = use entropy-based seed (non-deterministic).
|
||||
/// Default: 0
|
||||
pub seed: u64,
|
||||
}
|
||||
|
||||
impl Default for RansacLineConfig {
|
||||
fn default() -> Self {
|
||||
Self {
|
||||
iterations: 100,
|
||||
inlier_threshold: 0.02,
|
||||
min_inliers: 5,
|
||||
min_line_length: 0.1,
|
||||
max_point_gap: 0.10,
|
||||
max_lines: 0,
|
||||
seed: 0,
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
impl RansacLineConfig {
|
||||
/// Create a new config with default values.
|
||||
pub fn new() -> Self {
|
||||
Self::default()
|
||||
}
|
||||
|
||||
/// Builder-style setter for iterations.
|
||||
pub fn with_iterations(mut self, iterations: usize) -> Self {
|
||||
self.iterations = iterations;
|
||||
self
|
||||
}
|
||||
|
||||
/// Builder-style setter for inlier threshold.
|
||||
pub fn with_inlier_threshold(mut self, threshold: f32) -> Self {
|
||||
self.inlier_threshold = threshold;
|
||||
self
|
||||
}
|
||||
|
||||
/// Builder-style setter for minimum inliers.
|
||||
pub fn with_min_inliers(mut self, min_inliers: usize) -> Self {
|
||||
self.min_inliers = min_inliers;
|
||||
self
|
||||
}
|
||||
|
||||
/// Builder-style setter for minimum line length.
|
||||
pub fn with_min_line_length(mut self, length: f32) -> Self {
|
||||
self.min_line_length = length;
|
||||
self
|
||||
}
|
||||
|
||||
/// Builder-style setter for maximum lines.
|
||||
pub fn with_max_lines(mut self, max_lines: usize) -> Self {
|
||||
self.max_lines = max_lines;
|
||||
self
|
||||
}
|
||||
|
||||
/// Builder-style setter for maximum point gap.
|
||||
pub fn with_max_point_gap(mut self, gap: f32) -> Self {
|
||||
self.max_point_gap = gap;
|
||||
self
|
||||
}
|
||||
|
||||
/// Builder-style setter for random seed.
|
||||
pub fn with_seed(mut self, seed: u64) -> Self {
|
||||
self.seed = seed;
|
||||
self
|
||||
}
|
||||
}
|
||||
|
||||
/// Simple LCG random number generator for deterministic behavior.
|
||||
struct SimpleRng {
|
||||
state: u64,
|
||||
}
|
||||
|
||||
impl SimpleRng {
|
||||
fn new(seed: u64) -> Self {
|
||||
// Use a non-zero seed
|
||||
Self {
|
||||
state: if 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 {
|
||||
seed
|
||||
},
|
||||
}
|
||||
}
|
||||
|
||||
fn next(&mut self) -> u64 {
|
||||
// LCG parameters from Numerical Recipes
|
||||
self.state = self.state.wrapping_mul(6364136223846793005).wrapping_add(1);
|
||||
self.state
|
||||
}
|
||||
|
||||
fn gen_range(&mut self, max: usize) -> usize {
|
||||
if max == 0 {
|
||||
return 0;
|
||||
}
|
||||
(self.next() % (max as u64)) as usize
|
||||
}
|
||||
}
|
||||
|
||||
/// Split inliers at gaps to prevent connecting separate wall segments.
|
||||
///
|
||||
/// Points are sorted along the line direction and split wherever
|
||||
/// consecutive points have a gap larger than `max_point_gap`.
|
||||
fn split_at_gaps(
|
||||
inlier_points: &[Point2D],
|
||||
inlier_indices: &[usize],
|
||||
config: &RansacLineConfig,
|
||||
) -> Vec<(Vec<Point2D>, Vec<usize>)> {
|
||||
if inlier_points.len() < 2 {
|
||||
return vec![(inlier_points.to_vec(), inlier_indices.to_vec())];
|
||||
}
|
||||
|
||||
// Compute principal direction using PCA-like approach
|
||||
let n = inlier_points.len() as f32;
|
||||
let mean_x: f32 = inlier_points.iter().map(|p| p.x).sum::<f32>() / n;
|
||||
let mean_y: f32 = inlier_points.iter().map(|p| p.y).sum::<f32>() / n;
|
||||
|
||||
// Compute covariance matrix elements
|
||||
let mut cxx = 0.0f32;
|
||||
let mut cxy = 0.0f32;
|
||||
let mut cyy = 0.0f32;
|
||||
for p in inlier_points {
|
||||
let dx = p.x - mean_x;
|
||||
let dy = p.y - mean_y;
|
||||
cxx += dx * dx;
|
||||
cxy += dx * dy;
|
||||
cyy += dy * dy;
|
||||
}
|
||||
|
||||
// Principal direction from dominant eigenvector
|
||||
let theta = 0.5 * (2.0 * cxy).atan2(cxx - cyy);
|
||||
let dir_x = theta.cos();
|
||||
let dir_y = theta.sin();
|
||||
|
||||
// Project points onto principal direction and sort
|
||||
let mut indexed: Vec<(usize, f32)> = inlier_points
|
||||
.iter()
|
||||
.enumerate()
|
||||
.map(|(i, p)| {
|
||||
let proj = (p.x - mean_x) * dir_x + (p.y - mean_y) * dir_y;
|
||||
(i, proj)
|
||||
})
|
||||
.collect();
|
||||
|
||||
indexed.sort_by(|a, b| a.1.partial_cmp(&b.1).unwrap_or(std::cmp::Ordering::Equal));
|
||||
|
||||
// Split at gaps
|
||||
let mut segments = Vec::new();
|
||||
let mut current_points = Vec::new();
|
||||
let mut current_indices = Vec::new();
|
||||
|
||||
for (i, &(orig_idx, _proj)) in indexed.iter().enumerate() {
|
||||
current_points.push(inlier_points[orig_idx]);
|
||||
current_indices.push(inlier_indices[orig_idx]);
|
||||
|
||||
// Check if there's a gap to the next point
|
||||
if i + 1 < indexed.len() {
|
||||
let next_orig_idx = indexed[i + 1].0;
|
||||
let dist = inlier_points[orig_idx].distance(inlier_points[next_orig_idx]);
|
||||
|
||||
if dist > config.max_point_gap {
|
||||
// Gap found - save current segment and start new one
|
||||
if !current_points.is_empty() {
|
||||
segments.push((
|
||||
std::mem::take(&mut current_points),
|
||||
std::mem::take(&mut current_indices),
|
||||
));
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Add final segment
|
||||
if !current_points.is_empty() {
|
||||
segments.push((current_points, current_indices));
|
||||
}
|
||||
|
||||
segments
|
||||
}
|
||||
|
||||
/// Extract lines using RANSAC.
|
||||
///
|
||||
/// Returns extracted lines and indices of points not matched to any line.
|
||||
///
|
||||
/// # Arguments
|
||||
/// * `points` - Input point cloud
|
||||
/// * `config` - RANSAC configuration
|
||||
///
|
||||
/// # Returns
|
||||
/// Tuple of (extracted_lines, remaining_point_indices)
|
||||
pub fn extract_lines_ransac(
|
||||
points: &[Point2D],
|
||||
config: &RansacLineConfig,
|
||||
) -> (Vec<Line2D>, Vec<usize>) {
|
||||
if points.len() < config.min_inliers {
|
||||
return (Vec::new(), (0..points.len()).collect());
|
||||
}
|
||||
|
||||
let mut remaining_mask = vec![true; points.len()];
|
||||
let mut lines = Vec::new();
|
||||
let mut rng = SimpleRng::new(config.seed);
|
||||
|
||||
loop {
|
||||
// Check max lines limit
|
||||
if config.max_lines > 0 && lines.len() >= config.max_lines {
|
||||
break;
|
||||
}
|
||||
|
||||
// Collect remaining point indices
|
||||
let remaining: Vec<usize> = remaining_mask
|
||||
.iter()
|
||||
.enumerate()
|
||||
.filter_map(|(i, &active)| if active { Some(i) } else { None })
|
||||
.collect();
|
||||
|
||||
if remaining.len() < config.min_inliers {
|
||||
break;
|
||||
}
|
||||
|
||||
// Run RANSAC to find best line
|
||||
let mut best_inliers: Vec<usize> = Vec::new();
|
||||
|
||||
for _ in 0..config.iterations {
|
||||
// Random sample of 2 distinct points
|
||||
let idx1 = remaining[rng.gen_range(remaining.len())];
|
||||
let idx2 = remaining[rng.gen_range(remaining.len())];
|
||||
if idx1 == idx2 {
|
||||
continue;
|
||||
}
|
||||
|
||||
let p1 = points[idx1];
|
||||
let p2 = points[idx2];
|
||||
|
||||
// Check minimum distance between sample points
|
||||
let sample_dist = p1.distance(p2);
|
||||
if sample_dist < config.min_line_length * 0.3 {
|
||||
continue;
|
||||
}
|
||||
|
||||
// Create candidate line
|
||||
let line = Line2D::new(p1, p2);
|
||||
|
||||
// Count inliers
|
||||
let inliers: Vec<usize> = remaining
|
||||
.iter()
|
||||
.copied()
|
||||
.filter(|&i| line.distance_to_point(points[i]) < config.inlier_threshold)
|
||||
.collect();
|
||||
|
||||
if inliers.len() > best_inliers.len() {
|
||||
best_inliers = inliers;
|
||||
}
|
||||
}
|
||||
|
||||
// Check if we found enough inliers
|
||||
if best_inliers.len() < config.min_inliers {
|
||||
break;
|
||||
}
|
||||
|
||||
// Get inlier points
|
||||
let inlier_points: Vec<Point2D> = best_inliers.iter().map(|&i| points[i]).collect();
|
||||
|
||||
// Split inliers at gaps to avoid connecting separate wall segments
|
||||
let segments = split_at_gaps(&inlier_points, &best_inliers, config);
|
||||
|
||||
let mut added_any = false;
|
||||
for (segment_points, segment_indices) in segments {
|
||||
if segment_points.len() < config.min_inliers {
|
||||
continue;
|
||||
}
|
||||
|
||||
if let Some(refined_line) = fit_line(&segment_points) {
|
||||
// Check minimum length
|
||||
if refined_line.length() >= config.min_line_length {
|
||||
lines.push(refined_line);
|
||||
added_any = true;
|
||||
|
||||
// Mark segment inliers as used
|
||||
for &i in &segment_indices {
|
||||
remaining_mask[i] = false;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// If we couldn't add any valid lines from this iteration, stop
|
||||
if !added_any {
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
// Collect remaining point indices
|
||||
let remaining: Vec<usize> = remaining_mask
|
||||
.iter()
|
||||
.enumerate()
|
||||
.filter_map(|(i, &active)| if active { Some(i) } else { None })
|
||||
.collect();
|
||||
|
||||
(lines, remaining)
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
mod tests {
|
||||
use super::*;
|
||||
use approx::assert_relative_eq;
|
||||
|
||||
#[test]
|
||||
fn test_config_default() {
|
||||
let config = RansacLineConfig::default();
|
||||
assert_eq!(config.iterations, 100);
|
||||
assert_relative_eq!(config.inlier_threshold, 0.02);
|
||||
assert_eq!(config.min_inliers, 5);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_config_builder() {
|
||||
let config = RansacLineConfig::new()
|
||||
.with_iterations(50)
|
||||
.with_inlier_threshold(0.05)
|
||||
.with_min_inliers(3)
|
||||
.with_seed(42);
|
||||
|
||||
assert_eq!(config.iterations, 50);
|
||||
assert_relative_eq!(config.inlier_threshold, 0.05);
|
||||
assert_eq!(config.min_inliers, 3);
|
||||
assert_eq!(config.seed, 42);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_extract_empty_points() {
|
||||
let config = RansacLineConfig::default();
|
||||
let (lines, remaining) = extract_lines_ransac(&[], &config);
|
||||
|
||||
assert!(lines.is_empty());
|
||||
assert!(remaining.is_empty());
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_extract_too_few_points() {
|
||||
let points = vec![Point2D::new(0.0, 0.0), Point2D::new(1.0, 0.0)];
|
||||
let config = RansacLineConfig::default().with_min_inliers(5);
|
||||
let (lines, remaining) = extract_lines_ransac(&points, &config);
|
||||
|
||||
assert!(lines.is_empty());
|
||||
assert_eq!(remaining.len(), 2);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_extract_single_line() {
|
||||
// Points along a horizontal line (5cm spacing, under 10cm gap threshold)
|
||||
let points: Vec<Point2D> = (0..20)
|
||||
.map(|i| Point2D::new(i as f32 * 0.05, 0.0))
|
||||
.collect();
|
||||
|
||||
let config = RansacLineConfig::default()
|
||||
.with_seed(42)
|
||||
.with_min_inliers(5);
|
||||
|
||||
let (lines, remaining) = extract_lines_ransac(&points, &config);
|
||||
|
||||
assert_eq!(lines.len(), 1);
|
||||
// Most points should be consumed
|
||||
assert!(remaining.len() < 5);
|
||||
// Line should be roughly horizontal
|
||||
let angle = lines[0].angle().abs();
|
||||
assert!(angle < 0.1 || (std::f32::consts::PI - angle).abs() < 0.1);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_extract_two_lines() {
|
||||
// Points along a horizontal line (5cm spacing)
|
||||
let mut points: Vec<Point2D> = (0..15)
|
||||
.map(|i| Point2D::new(i as f32 * 0.05, 0.0))
|
||||
.collect();
|
||||
|
||||
// Points along a vertical line (5cm spacing)
|
||||
for i in 0..15 {
|
||||
points.push(Point2D::new(0.0, i as f32 * 0.05));
|
||||
}
|
||||
|
||||
let config = RansacLineConfig::default()
|
||||
.with_seed(42)
|
||||
.with_min_inliers(5)
|
||||
.with_max_lines(2);
|
||||
|
||||
let (lines, _remaining) = extract_lines_ransac(&points, &config);
|
||||
|
||||
assert_eq!(lines.len(), 2);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_extract_with_noise() {
|
||||
// Points along a horizontal line with some noise (5cm spacing)
|
||||
let mut points: Vec<Point2D> = (0..20)
|
||||
.map(|i| Point2D::new(i as f32 * 0.05, (i as f32 * 0.05).sin() * 0.01))
|
||||
.collect();
|
||||
|
||||
// Add some outliers
|
||||
points.push(Point2D::new(0.5, 1.0));
|
||||
points.push(Point2D::new(1.0, -1.0));
|
||||
|
||||
let config = RansacLineConfig::default()
|
||||
.with_seed(42)
|
||||
.with_inlier_threshold(0.03)
|
||||
.with_min_inliers(5);
|
||||
|
||||
let (lines, _remaining) = extract_lines_ransac(&points, &config);
|
||||
|
||||
// Should find the main line despite outliers
|
||||
assert!(!lines.is_empty());
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_max_lines_limit() {
|
||||
// Many points that could form multiple lines (5cm spacing)
|
||||
let mut points: Vec<Point2D> = Vec::new();
|
||||
for j in 0..4 {
|
||||
for i in 0..10 {
|
||||
points.push(Point2D::new(i as f32 * 0.05, j as f32 * 0.5));
|
||||
}
|
||||
}
|
||||
|
||||
let config = RansacLineConfig::default()
|
||||
.with_seed(42)
|
||||
.with_min_inliers(5)
|
||||
.with_max_lines(2);
|
||||
|
||||
let (lines, _remaining) = extract_lines_ransac(&points, &config);
|
||||
|
||||
assert!(lines.len() <= 2);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_gap_splitting_no_gap() {
|
||||
// Continuous line with no gaps (3cm spacing, well under 10cm threshold)
|
||||
let points: Vec<Point2D> = (0..20)
|
||||
.map(|i| Point2D::new(i as f32 * 0.03, 0.0)) // 3cm spacing
|
||||
.collect();
|
||||
let indices: Vec<usize> = (0..20).collect();
|
||||
|
||||
let config = RansacLineConfig::default().with_max_point_gap(0.10);
|
||||
|
||||
let segments = split_at_gaps(&points, &indices, &config);
|
||||
|
||||
// Should result in a single segment (all points within gap threshold)
|
||||
assert_eq!(segments.len(), 1);
|
||||
assert_eq!(segments[0].0.len(), 20);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_gap_splitting_with_gap() {
|
||||
// Two wall segments separated by a doorway (gap > 10cm)
|
||||
let mut points = Vec::new();
|
||||
// First wall segment: 5 points with 3cm spacing
|
||||
for i in 0..5 {
|
||||
points.push(Point2D::new(i as f32 * 0.03, 0.0));
|
||||
}
|
||||
// Second wall segment: starts at 0.35m (gap of ~23cm from last point)
|
||||
for i in 0..5 {
|
||||
points.push(Point2D::new(0.35 + i as f32 * 0.03, 0.0));
|
||||
}
|
||||
let indices: Vec<usize> = (0..10).collect();
|
||||
|
||||
let config = RansacLineConfig::default().with_max_point_gap(0.10);
|
||||
|
||||
let segments = split_at_gaps(&points, &indices, &config);
|
||||
|
||||
// Should split into two segments due to gap
|
||||
assert_eq!(segments.len(), 2);
|
||||
assert_eq!(segments[0].0.len(), 5);
|
||||
assert_eq!(segments[1].0.len(), 5);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_gap_splitting_multiple_gaps() {
|
||||
// Three wall segments with gaps
|
||||
let mut points = Vec::new();
|
||||
// Segment 1: 3 points with 3cm spacing (0.0 to 0.06m)
|
||||
for i in 0..3 {
|
||||
points.push(Point2D::new(i as f32 * 0.03, 0.0));
|
||||
}
|
||||
// Gap of ~19cm
|
||||
// Segment 2: 3 points starting at 0.25m
|
||||
for i in 0..3 {
|
||||
points.push(Point2D::new(0.25 + i as f32 * 0.03, 0.0));
|
||||
}
|
||||
// Gap of ~19cm
|
||||
// Segment 3: 3 points starting at 0.50m
|
||||
for i in 0..3 {
|
||||
points.push(Point2D::new(0.50 + i as f32 * 0.03, 0.0));
|
||||
}
|
||||
let indices: Vec<usize> = (0..9).collect();
|
||||
|
||||
let config = RansacLineConfig::default().with_max_point_gap(0.10);
|
||||
|
||||
let segments = split_at_gaps(&points, &indices, &config);
|
||||
|
||||
// Should split into three segments
|
||||
assert_eq!(segments.len(), 3);
|
||||
for segment in &segments {
|
||||
assert_eq!(segment.0.len(), 3);
|
||||
}
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_extract_lines_respects_gap() {
|
||||
// Collinear points with a gap (simulating doorway between walls)
|
||||
let mut points = Vec::new();
|
||||
// Left wall segment: 10 points with 3cm spacing (0.27m length)
|
||||
for i in 0..10 {
|
||||
points.push(Point2D::new(i as f32 * 0.03, 0.0));
|
||||
}
|
||||
// Gap of ~25cm
|
||||
// Right wall segment: 10 points with 3cm spacing starting at 0.52m
|
||||
for i in 0..10 {
|
||||
points.push(Point2D::new(0.52 + i as f32 * 0.03, 0.0));
|
||||
}
|
||||
|
||||
let config = RansacLineConfig::default()
|
||||
.with_seed(42)
|
||||
.with_min_inliers(5)
|
||||
.with_max_point_gap(0.10);
|
||||
|
||||
let (lines, _remaining) = extract_lines_ransac(&points, &config);
|
||||
|
||||
// Should find two separate lines, not one long line
|
||||
assert_eq!(lines.len(), 2);
|
||||
|
||||
// Check each line is approximately the expected length
|
||||
for line in &lines {
|
||||
// Each wall segment is ~0.27m (10 points * 0.03m spacing)
|
||||
assert!(line.length() < 0.35, "Line too long: {}", line.length());
|
||||
assert!(line.length() > 0.15, "Line too short: {}", line.length());
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -115,11 +115,13 @@ pub mod features;
|
||||
pub mod integration;
|
||||
pub mod loop_closure;
|
||||
pub mod matching;
|
||||
pub mod motion_model;
|
||||
pub mod query;
|
||||
pub mod simd;
|
||||
pub mod vector_map;
|
||||
|
||||
// Re-export main types at crate root
|
||||
pub use motion_model::MotionModel;
|
||||
pub use vector_map::{VectorMap, VectorMapConfig};
|
||||
|
||||
// Re-export extensibility traits
|
||||
|
||||
@@ -220,6 +220,11 @@ impl CorrespondenceSet {
|
||||
self.correspondences.iter()
|
||||
}
|
||||
|
||||
/// Iterate over correspondences mutably.
|
||||
pub fn iter_mut(&mut self) -> impl Iterator<Item = &mut Correspondence> {
|
||||
self.correspondences.iter_mut()
|
||||
}
|
||||
|
||||
/// Apply weights to correspondences based on range using a noise model.
|
||||
///
|
||||
/// The range for each correspondence is computed from the original robot-frame points
|
||||
@@ -255,6 +260,166 @@ impl CorrespondenceSet {
|
||||
}
|
||||
}
|
||||
|
||||
/// A correspondence between a scan point and a map corner.
|
||||
///
|
||||
/// Used for point-to-point constraints in ICP, which provide
|
||||
/// better angular accuracy than point-to-line constraints alone.
|
||||
#[derive(Clone, Copy, Debug)]
|
||||
pub struct CornerCorrespondence {
|
||||
/// Index of the point in the scan.
|
||||
pub point_idx: usize,
|
||||
/// Index of the corner in the map.
|
||||
pub corner_idx: usize,
|
||||
/// The scan point position (in world frame after initial transform).
|
||||
pub point: Point2D,
|
||||
/// The map corner position.
|
||||
pub corner: Point2D,
|
||||
/// Distance from point to corner.
|
||||
pub distance: f32,
|
||||
/// Weight for this correspondence (1/σ² based on measurement uncertainty).
|
||||
pub weight: f32,
|
||||
}
|
||||
|
||||
impl CornerCorrespondence {
|
||||
/// Create a new corner correspondence with default weight (1.0).
|
||||
#[inline]
|
||||
pub fn new(
|
||||
point_idx: usize,
|
||||
corner_idx: usize,
|
||||
point: Point2D,
|
||||
corner: Point2D,
|
||||
distance: f32,
|
||||
) -> Self {
|
||||
Self {
|
||||
point_idx,
|
||||
corner_idx,
|
||||
point,
|
||||
corner,
|
||||
distance,
|
||||
weight: 1.0,
|
||||
}
|
||||
}
|
||||
|
||||
/// Create a new corner correspondence with explicit weight.
|
||||
#[inline]
|
||||
pub fn with_weight(
|
||||
point_idx: usize,
|
||||
corner_idx: usize,
|
||||
point: Point2D,
|
||||
corner: Point2D,
|
||||
distance: f32,
|
||||
weight: f32,
|
||||
) -> Self {
|
||||
Self {
|
||||
point_idx,
|
||||
corner_idx,
|
||||
point,
|
||||
corner,
|
||||
distance,
|
||||
weight,
|
||||
}
|
||||
}
|
||||
|
||||
/// Check if this is a valid correspondence (within distance threshold).
|
||||
#[inline]
|
||||
pub fn is_valid(&self, max_distance: f32) -> bool {
|
||||
self.distance <= max_distance
|
||||
}
|
||||
}
|
||||
|
||||
/// Collection of corner correspondences for a scan match.
|
||||
#[derive(Clone, Debug, Default)]
|
||||
pub struct CornerCorrespondenceSet {
|
||||
/// All corner correspondences found.
|
||||
pub correspondences: Vec<CornerCorrespondence>,
|
||||
}
|
||||
|
||||
impl CornerCorrespondenceSet {
|
||||
/// Create a new empty corner correspondence set.
|
||||
pub fn new() -> Self {
|
||||
Self::default()
|
||||
}
|
||||
|
||||
/// Create with capacity.
|
||||
pub fn with_capacity(capacity: usize) -> Self {
|
||||
Self {
|
||||
correspondences: Vec::with_capacity(capacity),
|
||||
}
|
||||
}
|
||||
|
||||
/// Add a correspondence.
|
||||
#[inline]
|
||||
pub fn push(&mut self, corr: CornerCorrespondence) {
|
||||
self.correspondences.push(corr);
|
||||
}
|
||||
|
||||
/// Number of correspondences.
|
||||
#[inline]
|
||||
pub fn len(&self) -> usize {
|
||||
self.correspondences.len()
|
||||
}
|
||||
|
||||
/// Check if empty.
|
||||
#[inline]
|
||||
pub fn is_empty(&self) -> bool {
|
||||
self.correspondences.is_empty()
|
||||
}
|
||||
|
||||
/// Filter correspondences by maximum distance.
|
||||
pub fn filter_by_distance(&self, max_distance: f32) -> Self {
|
||||
Self {
|
||||
correspondences: self
|
||||
.correspondences
|
||||
.iter()
|
||||
.filter(|c| c.distance <= max_distance)
|
||||
.copied()
|
||||
.collect(),
|
||||
}
|
||||
}
|
||||
|
||||
/// Get the mean correspondence distance.
|
||||
pub fn mean_distance(&self) -> f32 {
|
||||
if self.is_empty() {
|
||||
return 0.0;
|
||||
}
|
||||
let sum: f32 = self.correspondences.iter().map(|c| c.distance).sum();
|
||||
sum / self.len() as f32
|
||||
}
|
||||
|
||||
/// Get the RMS (root mean square) correspondence distance.
|
||||
pub fn rms_distance(&self) -> f32 {
|
||||
if self.is_empty() {
|
||||
return 0.0;
|
||||
}
|
||||
let sum_sq: f32 = self
|
||||
.correspondences
|
||||
.iter()
|
||||
.map(|c| c.distance * c.distance)
|
||||
.sum();
|
||||
(sum_sq / self.len() as f32).sqrt()
|
||||
}
|
||||
|
||||
/// Get the total weight of all correspondences.
|
||||
pub fn total_weight(&self) -> f32 {
|
||||
self.correspondences.iter().map(|c| c.weight).sum()
|
||||
}
|
||||
|
||||
/// Clear all correspondences.
|
||||
pub fn clear(&mut self) {
|
||||
self.correspondences.clear();
|
||||
}
|
||||
|
||||
/// Iterate over correspondences.
|
||||
pub fn iter(&self) -> impl Iterator<Item = &CornerCorrespondence> {
|
||||
self.correspondences.iter()
|
||||
}
|
||||
|
||||
/// Iterate over correspondences mutably.
|
||||
pub fn iter_mut(&mut self) -> impl Iterator<Item = &mut CornerCorrespondence> {
|
||||
self.correspondences.iter_mut()
|
||||
}
|
||||
}
|
||||
|
||||
/// 3x3 covariance matrix for pose uncertainty [x, y, theta].
|
||||
pub type PoseCovariance = [[f32; 3]; 3];
|
||||
|
||||
@@ -464,4 +629,112 @@ mod tests {
|
||||
assert!(result.confidence > 0.5);
|
||||
assert!(result.is_good_match(0.3));
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_corner_correspondence_new() {
|
||||
let point = Point2D::new(1.0, 2.0);
|
||||
let corner = Point2D::new(1.1, 2.05);
|
||||
let corr = CornerCorrespondence::new(0, 1, point, corner, 0.1);
|
||||
|
||||
assert_eq!(corr.point_idx, 0);
|
||||
assert_eq!(corr.corner_idx, 1);
|
||||
assert_eq!(corr.distance, 0.1);
|
||||
assert_eq!(corr.weight, 1.0);
|
||||
assert!(corr.is_valid(0.15));
|
||||
assert!(!corr.is_valid(0.05));
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_corner_correspondence_with_weight() {
|
||||
let point = Point2D::new(1.0, 2.0);
|
||||
let corner = Point2D::new(1.1, 2.05);
|
||||
let corr = CornerCorrespondence::with_weight(0, 1, point, corner, 0.1, 0.5);
|
||||
|
||||
assert_eq!(corr.weight, 0.5);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_corner_correspondence_set_statistics() {
|
||||
let mut set = CornerCorrespondenceSet::new();
|
||||
set.push(CornerCorrespondence::new(
|
||||
0,
|
||||
0,
|
||||
Point2D::zero(),
|
||||
Point2D::new(0.1, 0.0),
|
||||
0.1,
|
||||
));
|
||||
set.push(CornerCorrespondence::new(
|
||||
1,
|
||||
1,
|
||||
Point2D::zero(),
|
||||
Point2D::new(0.2, 0.0),
|
||||
0.2,
|
||||
));
|
||||
set.push(CornerCorrespondence::new(
|
||||
2,
|
||||
2,
|
||||
Point2D::zero(),
|
||||
Point2D::new(0.3, 0.0),
|
||||
0.3,
|
||||
));
|
||||
|
||||
assert_eq!(set.len(), 3);
|
||||
assert!(!set.is_empty());
|
||||
assert!((set.mean_distance() - 0.2).abs() < 1e-6);
|
||||
|
||||
// RMS = sqrt((0.01 + 0.04 + 0.09) / 3) = sqrt(0.14/3) ≈ 0.216
|
||||
assert!((set.rms_distance() - 0.216).abs() < 0.01);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_corner_correspondence_set_filter() {
|
||||
let mut set = CornerCorrespondenceSet::new();
|
||||
set.push(CornerCorrespondence::new(
|
||||
0,
|
||||
0,
|
||||
Point2D::zero(),
|
||||
Point2D::new(0.05, 0.0),
|
||||
0.05,
|
||||
));
|
||||
set.push(CornerCorrespondence::new(
|
||||
1,
|
||||
1,
|
||||
Point2D::zero(),
|
||||
Point2D::new(0.15, 0.0),
|
||||
0.15,
|
||||
));
|
||||
set.push(CornerCorrespondence::new(
|
||||
2,
|
||||
2,
|
||||
Point2D::zero(),
|
||||
Point2D::new(0.25, 0.0),
|
||||
0.25,
|
||||
));
|
||||
|
||||
let filtered = set.filter_by_distance(0.1);
|
||||
assert_eq!(filtered.len(), 1);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_corner_correspondence_set_total_weight() {
|
||||
let mut set = CornerCorrespondenceSet::new();
|
||||
set.push(CornerCorrespondence::with_weight(
|
||||
0,
|
||||
0,
|
||||
Point2D::zero(),
|
||||
Point2D::new(0.1, 0.0),
|
||||
0.1,
|
||||
0.5,
|
||||
));
|
||||
set.push(CornerCorrespondence::with_weight(
|
||||
1,
|
||||
1,
|
||||
Point2D::zero(),
|
||||
Point2D::new(0.2, 0.0),
|
||||
0.2,
|
||||
0.3,
|
||||
));
|
||||
|
||||
assert!((set.total_weight() - 0.8).abs() < 1e-6);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -1,8 +1,13 @@
|
||||
//! Gauss-Newton solver for point-to-line ICP.
|
||||
//! Levenberg-Marquardt solver for point-to-line ICP.
|
||||
//!
|
||||
//! Solves the nonlinear least squares problem of finding the pose
|
||||
//! that minimizes the sum of squared point-to-line distances.
|
||||
//!
|
||||
//! Uses Levenberg-Marquardt algorithm for robust convergence. LM blends
|
||||
//! between Gauss-Newton (fast convergence near solution) and gradient
|
||||
//! descent (robust far from solution). Set `lm_initial_lambda = 0.0`
|
||||
//! to disable LM damping and use pure Gauss-Newton.
|
||||
//!
|
||||
//! # Performance
|
||||
//!
|
||||
//! Two versions are provided:
|
||||
@@ -17,7 +22,10 @@ use crate::features::{Line2D, LineCollection};
|
||||
|
||||
use super::correspondence::CorrespondenceSet;
|
||||
|
||||
/// Configuration for Gauss-Newton solver.
|
||||
/// Configuration for Levenberg-Marquardt solver.
|
||||
///
|
||||
/// LM blends between Gauss-Newton and gradient descent for robust convergence.
|
||||
/// Set `lm_initial_lambda = 0.0` to disable LM damping (pure Gauss-Newton).
|
||||
#[derive(Clone, Debug)]
|
||||
pub struct GaussNewtonConfig {
|
||||
/// Maximum number of iterations.
|
||||
@@ -41,6 +49,23 @@ pub struct GaussNewtonConfig {
|
||||
/// Regularization factor for ill-conditioned systems.
|
||||
/// Default: 1e-4
|
||||
pub regularization: f32,
|
||||
|
||||
/// Initial LM damping factor (λ).
|
||||
/// Set to 0.0 to disable LM and use pure Gauss-Newton.
|
||||
/// Default: 0.001
|
||||
pub lm_initial_lambda: f32,
|
||||
|
||||
/// Factor to scale λ up (on bad step) or down (on good step).
|
||||
/// Default: 10.0
|
||||
pub lm_lambda_factor: f32,
|
||||
|
||||
/// Minimum λ value (floor after successful steps).
|
||||
/// Default: 1e-7
|
||||
pub lm_min_lambda: f32,
|
||||
|
||||
/// Maximum λ value (triggers failure if exceeded).
|
||||
/// Default: 1e7
|
||||
pub lm_max_lambda: f32,
|
||||
}
|
||||
|
||||
impl Default for GaussNewtonConfig {
|
||||
@@ -52,6 +77,11 @@ impl Default for GaussNewtonConfig {
|
||||
convergence_threshold_sq: convergence_threshold * convergence_threshold,
|
||||
min_eigenvalue_ratio: 1e-6,
|
||||
regularization: 1e-4,
|
||||
// LM parameters
|
||||
lm_initial_lambda: 0.001,
|
||||
lm_lambda_factor: 10.0,
|
||||
lm_min_lambda: 1e-7,
|
||||
lm_max_lambda: 1e7,
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -74,6 +104,19 @@ impl GaussNewtonConfig {
|
||||
self.convergence_threshold_sq = threshold * threshold;
|
||||
self
|
||||
}
|
||||
|
||||
/// Builder-style setter for initial LM lambda.
|
||||
/// Set to 0.0 to disable LM (pure Gauss-Newton).
|
||||
pub fn with_lm_lambda(mut self, lambda: f32) -> Self {
|
||||
self.lm_initial_lambda = lambda;
|
||||
self
|
||||
}
|
||||
|
||||
/// Disable LM damping (use pure Gauss-Newton).
|
||||
pub fn without_lm(mut self) -> Self {
|
||||
self.lm_initial_lambda = 0.0;
|
||||
self
|
||||
}
|
||||
}
|
||||
|
||||
use super::correspondence::PoseCovariance;
|
||||
@@ -343,6 +386,91 @@ fn compute_covariance(
|
||||
(covariance, condition_number)
|
||||
}
|
||||
|
||||
/// Compute covariance matrix with degrees-of-freedom correction.
|
||||
///
|
||||
/// Uses unbiased variance estimate: σ² = Σw·r² / (n - 3)
|
||||
/// where n is the number of correspondences and 3 is the number of parameters.
|
||||
fn compute_covariance_with_dof(
|
||||
hessian: &[[f32; 3]; 3],
|
||||
sum_weighted_sq_residuals: f32,
|
||||
num_correspondences: usize,
|
||||
regularization: f32,
|
||||
) -> (PoseCovariance, f32) {
|
||||
// Degrees of freedom correction: n - 3 (3 parameters: x, y, theta)
|
||||
let n = num_correspondences as f32;
|
||||
let dof = (n - 3.0).max(1.0);
|
||||
|
||||
// Unbiased variance estimate
|
||||
let sigma_sq = sum_weighted_sq_residuals / dof;
|
||||
|
||||
// Add regularization to diagonal
|
||||
let mut h = *hessian;
|
||||
h[0][0] += regularization;
|
||||
h[1][1] += regularization;
|
||||
h[2][2] += regularization;
|
||||
|
||||
// Compute determinant
|
||||
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 {
|
||||
// Singular matrix - return identity covariance with high condition number
|
||||
return (IDENTITY_COVARIANCE, f32::MAX);
|
||||
}
|
||||
|
||||
let inv_det = 1.0 / det;
|
||||
|
||||
// Compute inverse (H⁻¹)
|
||||
let 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,
|
||||
],
|
||||
];
|
||||
|
||||
// Scale by unbiased variance to get covariance: Cov = σ² × H⁻¹
|
||||
let covariance = [
|
||||
[
|
||||
sigma_sq * inv[0][0],
|
||||
sigma_sq * inv[0][1],
|
||||
sigma_sq * inv[0][2],
|
||||
],
|
||||
[
|
||||
sigma_sq * inv[1][0],
|
||||
sigma_sq * inv[1][1],
|
||||
sigma_sq * inv[1][2],
|
||||
],
|
||||
[
|
||||
sigma_sq * inv[2][0],
|
||||
sigma_sq * inv[2][1],
|
||||
sigma_sq * inv[2][2],
|
||||
],
|
||||
];
|
||||
|
||||
// Compute condition number
|
||||
let diag_max = h[0][0].max(h[1][1]).max(h[2][2]);
|
||||
let diag_min = h[0][0].min(h[1][1]).min(h[2][2]);
|
||||
let condition_number = if diag_min > 1e-10 {
|
||||
diag_max / diag_min
|
||||
} else {
|
||||
f32::MAX
|
||||
};
|
||||
|
||||
(covariance, condition_number)
|
||||
}
|
||||
|
||||
/// Solve a 3x3 linear system Ax = b using Cramer's rule.
|
||||
///
|
||||
/// Returns None if the matrix is singular.
|
||||
@@ -421,11 +549,19 @@ fn compute_residual(correspondences: &CorrespondenceSet, lines: &[Line2D], pose:
|
||||
// Fast versions using pre-computed LineCollection normals
|
||||
// ============================================================================
|
||||
|
||||
/// Optimize pose using pre-computed normals (faster hot-path version).
|
||||
/// Optimize pose using Levenberg-Marquardt with pre-computed normals.
|
||||
///
|
||||
/// This version uses `LineCollection` with pre-computed normals and inverse
|
||||
/// lengths, avoiding redundant sqrt computations in the inner loop.
|
||||
/// Also uses weighted least squares when correspondences have weights.
|
||||
/// Uses LM damping for robust convergence from poor initial guesses.
|
||||
///
|
||||
/// # Algorithm
|
||||
///
|
||||
/// LM adds damping (λI) to the Hessian diagonal:
|
||||
/// - Good step (error decreases): accept step, decrease λ
|
||||
/// - Bad step (error increases): reject step, increase λ
|
||||
///
|
||||
/// Set `config.lm_initial_lambda = 0.0` to disable LM (pure Gauss-Newton).
|
||||
///
|
||||
/// # Performance
|
||||
///
|
||||
@@ -447,20 +583,30 @@ pub fn optimize_pose_fast(
|
||||
};
|
||||
}
|
||||
|
||||
// Pre-fetch normals for all lines used in correspondences
|
||||
let (normal_xs, normal_ys) = lines.normals();
|
||||
let inv_lengths = lines.inv_lengths();
|
||||
|
||||
let mut pose = initial_pose;
|
||||
let mut converged = false;
|
||||
let mut iterations = 0;
|
||||
let mut final_hessian = [[0.0f32; 3]; 3];
|
||||
let mut final_sum_weighted_sq = 0.0f32;
|
||||
|
||||
// Pre-fetch normals for all lines used in correspondences
|
||||
let (normal_xs, normal_ys) = lines.normals();
|
||||
let inv_lengths = lines.inv_lengths();
|
||||
// LM state
|
||||
let use_lm = config.lm_initial_lambda > 0.0;
|
||||
let mut lambda = config.lm_initial_lambda;
|
||||
let mut prev_residual = if use_lm {
|
||||
compute_residual_fast(correspondences, lines, inv_lengths, &pose)
|
||||
} else {
|
||||
f32::MAX
|
||||
};
|
||||
|
||||
for iter in 0..config.max_iterations {
|
||||
iterations = iter + 1;
|
||||
|
||||
// Build weighted linear system using pre-computed normals
|
||||
let (h, g, residual) = build_linear_system_fast_weighted(
|
||||
let (h, g, sum_weighted_sq, _sum_weights) = build_linear_system_fast_weighted_extended(
|
||||
correspondences,
|
||||
lines,
|
||||
normal_xs,
|
||||
@@ -469,37 +615,77 @@ pub fn optimize_pose_fast(
|
||||
&pose,
|
||||
);
|
||||
final_hessian = h;
|
||||
final_sum_weighted_sq = sum_weighted_sq;
|
||||
|
||||
// Add LM damping to Hessian diagonal
|
||||
let mut h_damped = h;
|
||||
if use_lm {
|
||||
h_damped[0][0] += lambda;
|
||||
h_damped[1][1] += lambda;
|
||||
h_damped[2][2] += lambda;
|
||||
}
|
||||
|
||||
// Solve for Δx using direct 3x3 solve
|
||||
let Some(delta) = solve_3x3(&h, &g, config.regularization) else {
|
||||
let Some(delta) = solve_3x3(&h_damped, &g, config.regularization) else {
|
||||
break;
|
||||
};
|
||||
|
||||
// Update pose
|
||||
pose.x += delta[0];
|
||||
pose.y += delta[1];
|
||||
pose.theta += delta[2];
|
||||
pose.theta = crate::core::math::normalize_angle(pose.theta);
|
||||
// Compute trial pose
|
||||
let trial_pose = Pose2D::new(
|
||||
pose.x + delta[0],
|
||||
pose.y + delta[1],
|
||||
crate::core::math::normalize_angle(pose.theta + delta[2]),
|
||||
);
|
||||
|
||||
// Check convergence using squared comparison to avoid sqrt()
|
||||
let delta_norm_sq = delta[0] * delta[0] + delta[1] * delta[1] + delta[2] * delta[2];
|
||||
if delta_norm_sq < config.convergence_threshold_sq {
|
||||
converged = true;
|
||||
break;
|
||||
}
|
||||
if use_lm {
|
||||
// Evaluate step quality
|
||||
let trial_residual =
|
||||
compute_residual_fast(correspondences, lines, inv_lengths, &trial_pose);
|
||||
|
||||
if residual < config.convergence_threshold {
|
||||
converged = true;
|
||||
break;
|
||||
if trial_residual < prev_residual {
|
||||
// Good step - accept and decrease lambda
|
||||
pose = trial_pose;
|
||||
prev_residual = trial_residual;
|
||||
lambda = (lambda / config.lm_lambda_factor).max(config.lm_min_lambda);
|
||||
|
||||
// Check convergence
|
||||
let delta_norm_sq = delta[0] * delta[0] + delta[1] * delta[1] + delta[2] * delta[2];
|
||||
if delta_norm_sq < config.convergence_threshold_sq {
|
||||
converged = true;
|
||||
break;
|
||||
}
|
||||
} else {
|
||||
// Bad step - reject and increase lambda
|
||||
lambda *= config.lm_lambda_factor;
|
||||
if lambda > config.lm_max_lambda {
|
||||
// Failed to converge
|
||||
break;
|
||||
}
|
||||
// Don't update pose, try again with higher damping
|
||||
continue;
|
||||
}
|
||||
} else {
|
||||
// Pure Gauss-Newton (no LM damping)
|
||||
pose = trial_pose;
|
||||
|
||||
// Check convergence
|
||||
let delta_norm_sq = delta[0] * delta[0] + delta[1] * delta[1] + delta[2] * delta[2];
|
||||
if delta_norm_sq < config.convergence_threshold_sq {
|
||||
converged = true;
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
let final_residual = compute_residual_fast(correspondences, lines, inv_lengths, &pose);
|
||||
|
||||
// Compute covariance from inverse Hessian
|
||||
let residual_variance = final_residual * final_residual;
|
||||
let (covariance, condition_number) =
|
||||
compute_covariance(&final_hessian, residual_variance, config.regularization);
|
||||
// Compute covariance with DOF correction
|
||||
let (covariance, condition_number) = compute_covariance_with_dof(
|
||||
&final_hessian,
|
||||
final_sum_weighted_sq,
|
||||
correspondences.len(),
|
||||
config.regularization,
|
||||
);
|
||||
|
||||
GaussNewtonResult {
|
||||
pose,
|
||||
@@ -512,14 +698,15 @@ pub fn optimize_pose_fast(
|
||||
}
|
||||
|
||||
/// Build weighted linear system using pre-computed normals.
|
||||
fn build_linear_system_fast_weighted(
|
||||
/// Returns (H, g, sum_weighted_sq_error, sum_weights) for covariance computation.
|
||||
fn build_linear_system_fast_weighted_extended(
|
||||
correspondences: &CorrespondenceSet,
|
||||
lines: &LineCollection,
|
||||
normal_xs: &[f32],
|
||||
normal_ys: &[f32],
|
||||
inv_lengths: &[f32],
|
||||
pose: &Pose2D,
|
||||
) -> ([[f32; 3]; 3], [f32; 3], f32) {
|
||||
) -> ([[f32; 3]; 3], [f32; 3], f32, f32) {
|
||||
let (sin, cos) = pose.theta.sin_cos();
|
||||
|
||||
// Pre-compute -sin and -cos for Jacobian
|
||||
@@ -585,13 +772,7 @@ fn build_linear_system_fast_weighted(
|
||||
h[2][0] = h[0][2];
|
||||
h[2][1] = h[1][2];
|
||||
|
||||
let rms_error = if sum_weights > 0.0 {
|
||||
(sum_weighted_sq_error / sum_weights).sqrt()
|
||||
} else {
|
||||
0.0
|
||||
};
|
||||
|
||||
(h, g, rms_error)
|
||||
(h, g, sum_weighted_sq_error, sum_weights)
|
||||
}
|
||||
|
||||
/// Compute RMS residual using pre-computed inverse lengths.
|
||||
|
||||
@@ -2,7 +2,7 @@
|
||||
|
||||
use crate::config::LidarNoiseModel;
|
||||
|
||||
use super::super::ransac::RansacConfig;
|
||||
use super::super::robust::RobustCostFunction;
|
||||
|
||||
/// Configuration for Point-to-Line ICP.
|
||||
#[derive(Clone, Debug)]
|
||||
@@ -25,9 +25,16 @@ pub struct IcpConfig {
|
||||
/// Default: 0.5m
|
||||
pub max_correspondence_distance: f32,
|
||||
|
||||
/// Outlier rejection method.
|
||||
/// Default: DistanceThreshold
|
||||
pub outlier_rejection: OutlierRejection,
|
||||
/// Robust cost function for outlier handling.
|
||||
/// Uses M-estimators to down-weight outliers based on residual magnitude.
|
||||
/// Default: Huber with delta = 0.03 (3cm)
|
||||
pub robust_cost: RobustCostFunction,
|
||||
|
||||
/// Enable Iteratively Reweighted Least Squares (IRLS).
|
||||
/// When true, correspondence weights are updated each iteration
|
||||
/// based on residuals and the robust cost function.
|
||||
/// Default: true
|
||||
pub use_irls: bool,
|
||||
|
||||
/// Minimum number of correspondences required.
|
||||
/// Default: 10
|
||||
@@ -38,7 +45,7 @@ pub struct IcpConfig {
|
||||
pub min_confidence: f32,
|
||||
|
||||
/// Whether to use RANSAC for initial pose estimate.
|
||||
/// Default: false
|
||||
/// Default: true
|
||||
pub use_ransac_init: bool,
|
||||
|
||||
/// Whether to use batch (SIMD) correspondence search.
|
||||
@@ -57,17 +64,11 @@ pub struct IcpConfig {
|
||||
/// Only used if use_coarse_search is true.
|
||||
/// Default: 0.5
|
||||
pub coarse_search_confidence_threshold: f32,
|
||||
}
|
||||
|
||||
/// Outlier rejection method.
|
||||
#[derive(Clone, Debug)]
|
||||
pub enum OutlierRejection {
|
||||
/// Reject by distance threshold (fast).
|
||||
DistanceThreshold(f32),
|
||||
/// Use RANSAC for robust rejection (slower but more robust).
|
||||
Ransac(RansacConfig),
|
||||
/// No outlier rejection.
|
||||
None,
|
||||
/// Multi-resolution ICP configuration (coarse-to-fine matching).
|
||||
/// When enabled, matching starts with subsampled points and refines
|
||||
/// with progressively more points for better convergence.
|
||||
pub multi_resolution: MultiResolutionConfig,
|
||||
}
|
||||
|
||||
impl Default for IcpConfig {
|
||||
@@ -78,14 +79,16 @@ impl Default for IcpConfig {
|
||||
convergence_threshold,
|
||||
convergence_threshold_sq: convergence_threshold * convergence_threshold,
|
||||
max_correspondence_distance: 0.5,
|
||||
outlier_rejection: OutlierRejection::DistanceThreshold(0.15),
|
||||
robust_cost: RobustCostFunction::default(), // Huber with delta = 0.03
|
||||
use_irls: true,
|
||||
min_correspondences: 10,
|
||||
min_confidence: 0.3,
|
||||
use_ransac_init: false,
|
||||
use_ransac_init: true,
|
||||
use_batch_search: true,
|
||||
noise_model: LidarNoiseModel::default(),
|
||||
use_coarse_search: true, // Enabled: triggers when previous match confidence < threshold
|
||||
coarse_search_confidence_threshold: 0.5,
|
||||
multi_resolution: MultiResolutionConfig::default(),
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -115,9 +118,21 @@ impl IcpConfig {
|
||||
self
|
||||
}
|
||||
|
||||
/// Builder-style setter for outlier rejection method.
|
||||
pub fn with_outlier_rejection(mut self, method: OutlierRejection) -> Self {
|
||||
self.outlier_rejection = method;
|
||||
/// Builder-style setter for robust cost function.
|
||||
pub fn with_robust_cost(mut self, cost: RobustCostFunction) -> Self {
|
||||
self.robust_cost = cost;
|
||||
self
|
||||
}
|
||||
|
||||
/// Builder-style setter for IRLS (Iteratively Reweighted Least Squares).
|
||||
pub fn with_irls(mut self, enabled: bool) -> Self {
|
||||
self.use_irls = enabled;
|
||||
self
|
||||
}
|
||||
|
||||
/// Builder-style setter for batch search.
|
||||
pub fn with_batch_search(mut self, enabled: bool) -> Self {
|
||||
self.use_batch_search = enabled;
|
||||
self
|
||||
}
|
||||
|
||||
@@ -150,6 +165,18 @@ impl IcpConfig {
|
||||
self.coarse_search_confidence_threshold = threshold;
|
||||
self
|
||||
}
|
||||
|
||||
/// Builder-style setter for multi-resolution configuration.
|
||||
pub fn with_multi_resolution(mut self, config: MultiResolutionConfig) -> Self {
|
||||
self.multi_resolution = config;
|
||||
self
|
||||
}
|
||||
|
||||
/// Disable multi-resolution matching.
|
||||
pub fn without_multi_resolution(mut self) -> Self {
|
||||
self.multi_resolution.enabled = false;
|
||||
self
|
||||
}
|
||||
}
|
||||
|
||||
/// Configuration for coarse search initialization.
|
||||
@@ -190,3 +217,95 @@ impl Default for CoarseSearchConfig {
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// Configuration for multi-resolution ICP (coarse-to-fine matching).
|
||||
///
|
||||
/// Multi-resolution ICP improves convergence by starting with subsampled
|
||||
/// points (coarse) and progressively refining with more points (fine).
|
||||
/// This helps escape local minima when the initial guess is poor.
|
||||
#[derive(Clone, Debug)]
|
||||
pub struct MultiResolutionConfig {
|
||||
/// Enable multi-resolution matching.
|
||||
/// Default: true
|
||||
pub enabled: bool,
|
||||
|
||||
/// Number of pyramid levels.
|
||||
/// Level 0 is coarsest, level (levels-1) is finest (full resolution).
|
||||
/// Default: 3
|
||||
pub levels: usize,
|
||||
|
||||
/// Subsampling factor between levels.
|
||||
/// Points at level i use every (subsample_factor^(levels-1-i)) points.
|
||||
/// Example with levels=3, factor=2: coarse=1/4, medium=1/2, fine=full
|
||||
/// Default: 2
|
||||
pub subsample_factor: usize,
|
||||
|
||||
/// ICP iterations at each level (from coarsest to finest).
|
||||
/// Length should match `levels`. If shorter, last value is repeated.
|
||||
/// Default: [3, 5, 10]
|
||||
pub iterations_per_level: Vec<usize>,
|
||||
}
|
||||
|
||||
impl Default for MultiResolutionConfig {
|
||||
fn default() -> Self {
|
||||
Self {
|
||||
enabled: true,
|
||||
levels: 3,
|
||||
subsample_factor: 2,
|
||||
iterations_per_level: vec![3, 5, 10],
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
impl MultiResolutionConfig {
|
||||
/// Create a new multi-resolution config with default values.
|
||||
pub fn new() -> Self {
|
||||
Self::default()
|
||||
}
|
||||
|
||||
/// Create an enabled config with default pyramid settings.
|
||||
pub fn enabled() -> Self {
|
||||
Self {
|
||||
enabled: true,
|
||||
..Self::default()
|
||||
}
|
||||
}
|
||||
|
||||
/// Builder-style setter for number of levels.
|
||||
pub fn with_levels(mut self, levels: usize) -> Self {
|
||||
self.levels = levels.max(1);
|
||||
self
|
||||
}
|
||||
|
||||
/// Builder-style setter for subsample factor.
|
||||
pub fn with_subsample_factor(mut self, factor: usize) -> Self {
|
||||
self.subsample_factor = factor.max(1);
|
||||
self
|
||||
}
|
||||
|
||||
/// Builder-style setter for iterations per level.
|
||||
pub fn with_iterations(mut self, iterations: Vec<usize>) -> Self {
|
||||
self.iterations_per_level = iterations;
|
||||
self
|
||||
}
|
||||
|
||||
/// Get iterations for a given level (0 = coarsest).
|
||||
pub fn iterations_for_level(&self, level: usize) -> usize {
|
||||
if level < self.iterations_per_level.len() {
|
||||
self.iterations_per_level[level]
|
||||
} else {
|
||||
// Use last value if not enough entries
|
||||
*self.iterations_per_level.last().unwrap_or(&10)
|
||||
}
|
||||
}
|
||||
|
||||
/// Get subsample rate for a given level (0 = coarsest).
|
||||
///
|
||||
/// Returns the step size for point sampling at this level.
|
||||
pub fn subsample_for_level(&self, level: usize) -> usize {
|
||||
if level >= self.levels {
|
||||
return 1;
|
||||
}
|
||||
self.subsample_factor.pow((self.levels - 1 - level) as u32)
|
||||
}
|
||||
}
|
||||
|
||||
@@ -13,7 +13,7 @@ use super::super::nearest_neighbor::{
|
||||
use super::super::ransac::RansacConfig;
|
||||
use super::super::ransac::estimate_pose_ransac;
|
||||
use super::super::scratch::IcpScratchSpace;
|
||||
use super::config::{CoarseSearchConfig, IcpConfig, OutlierRejection};
|
||||
use super::config::{CoarseSearchConfig, IcpConfig};
|
||||
|
||||
/// Point-to-Line ICP matcher.
|
||||
///
|
||||
@@ -222,8 +222,9 @@ impl PointToLineIcp {
|
||||
);
|
||||
}
|
||||
|
||||
// 2. Reject outliers
|
||||
let filtered = self.reject_outliers(&correspondences, lines);
|
||||
// 2. Filter by max distance and apply IRLS weights
|
||||
let mut filtered = self.filter_by_max_distance(&correspondences);
|
||||
self.apply_irls_weights(&mut filtered);
|
||||
|
||||
if filtered.len() < self.config.min_correspondences {
|
||||
return MatchResult::with_covariance(
|
||||
@@ -237,7 +238,7 @@ impl PointToLineIcp {
|
||||
);
|
||||
}
|
||||
|
||||
// 3. Optimize pose using Gauss-Newton (with covariance output)
|
||||
// 3. Optimize pose using Gauss-Newton with weighted correspondences
|
||||
let gn_result = optimize_pose(&filtered, lines, Pose2D::identity(), &self.gn_config);
|
||||
final_covariance = gn_result.covariance;
|
||||
final_condition_number = gn_result.condition_number;
|
||||
@@ -309,6 +310,17 @@ impl PointToLineIcp {
|
||||
return MatchResult::new(initial_pose, CorrespondenceSet::new(), 0, false);
|
||||
}
|
||||
|
||||
// Use multi-resolution ICP if enabled
|
||||
if self.config.multi_resolution.enabled {
|
||||
return match_scan_multiresolution(
|
||||
robot_frame_points,
|
||||
lines,
|
||||
initial_pose,
|
||||
&self.config.multi_resolution,
|
||||
&self.config,
|
||||
);
|
||||
}
|
||||
|
||||
// Convert lines to collection for batch operations
|
||||
let line_collection = LineCollection::from_lines(lines);
|
||||
|
||||
@@ -383,8 +395,10 @@ impl PointToLineIcp {
|
||||
);
|
||||
}
|
||||
|
||||
// 2. Reject outliers
|
||||
let filtered = self.reject_outliers(&correspondences, lines);
|
||||
// 2. Filter by max distance and apply IRLS weights
|
||||
// Note: Range-based weights already applied above via apply_weights()
|
||||
let mut filtered = self.filter_by_max_distance(&correspondences);
|
||||
self.apply_irls_weights(&mut filtered);
|
||||
|
||||
if filtered.len() < self.config.min_correspondences {
|
||||
return MatchResult::with_covariance(
|
||||
@@ -398,7 +412,7 @@ impl PointToLineIcp {
|
||||
);
|
||||
}
|
||||
|
||||
// 3. Optimize pose using Gauss-Newton (uses weighted correspondences)
|
||||
// 3. Optimize pose using Gauss-Newton with weighted correspondences
|
||||
let gn_result = optimize_pose(&filtered, lines, Pose2D::identity(), &self.gn_config);
|
||||
final_covariance = gn_result.covariance;
|
||||
final_condition_number = gn_result.condition_number;
|
||||
@@ -474,8 +488,6 @@ impl PointToLineIcp {
|
||||
return MatchResult::new(initial_pose, CorrespondenceSet::new(), 0, false);
|
||||
}
|
||||
|
||||
// Convert to Line2D for outlier rejection (uses Line2D methods)
|
||||
let lines_vec = lines.to_lines();
|
||||
let mut pose = initial_pose;
|
||||
let mut prev_pose = pose;
|
||||
let mut final_correspondences = CorrespondenceSet::new();
|
||||
@@ -513,9 +525,10 @@ impl PointToLineIcp {
|
||||
);
|
||||
}
|
||||
|
||||
// 2. Get correspondences and reject outliers
|
||||
// 2. Get correspondences, filter by distance, and apply IRLS weights
|
||||
let correspondences = scratch.correspondences();
|
||||
let filtered = self.reject_outliers(correspondences, &lines_vec);
|
||||
let mut filtered = self.filter_by_max_distance(correspondences);
|
||||
self.apply_irls_weights(&mut filtered);
|
||||
|
||||
if filtered.len() < self.config.min_correspondences {
|
||||
return MatchResult::with_covariance(
|
||||
@@ -529,7 +542,7 @@ impl PointToLineIcp {
|
||||
);
|
||||
}
|
||||
|
||||
// 3. Optimize pose using fast Gauss-Newton (uses pre-computed normals)
|
||||
// 3. Optimize pose using fast Gauss-Newton with weighted correspondences
|
||||
let gn_result =
|
||||
optimize_pose_fast(&filtered, lines, Pose2D::identity(), &self.gn_config);
|
||||
final_covariance = gn_result.covariance;
|
||||
@@ -596,24 +609,28 @@ impl PointToLineIcp {
|
||||
}
|
||||
}
|
||||
|
||||
/// Reject outliers from correspondences.
|
||||
fn reject_outliers(
|
||||
&self,
|
||||
correspondences: &CorrespondenceSet,
|
||||
lines: &[Line2D],
|
||||
) -> CorrespondenceSet {
|
||||
match &self.config.outlier_rejection {
|
||||
OutlierRejection::None => correspondences.clone(),
|
||||
OutlierRejection::DistanceThreshold(threshold) => {
|
||||
correspondences.filter_by_distance(*threshold)
|
||||
}
|
||||
OutlierRejection::Ransac(config) => {
|
||||
match estimate_pose_ransac(correspondences, lines, config) {
|
||||
Some(result) => result.inliers,
|
||||
None => correspondences.filter_by_distance(0.2),
|
||||
}
|
||||
}
|
||||
/// Apply IRLS robust weights to correspondences.
|
||||
///
|
||||
/// Multiplies each correspondence weight by a robust weight computed
|
||||
/// from its residual (distance). This down-weights outliers softly
|
||||
/// rather than removing them entirely.
|
||||
fn apply_irls_weights(&self, correspondences: &mut CorrespondenceSet) {
|
||||
if !self.config.use_irls || self.config.robust_cost.is_none() {
|
||||
return;
|
||||
}
|
||||
|
||||
for corr in correspondences.iter_mut() {
|
||||
let robust_weight = self.config.robust_cost.weight(corr.distance);
|
||||
corr.weight *= robust_weight; // Multiply range-based weight with robust weight
|
||||
}
|
||||
}
|
||||
|
||||
/// Filter correspondences by max correspondence distance.
|
||||
///
|
||||
/// This is a hard filter applied before IRLS weighting to remove
|
||||
/// obviously bad correspondences (beyond max search distance).
|
||||
fn filter_by_max_distance(&self, correspondences: &CorrespondenceSet) -> CorrespondenceSet {
|
||||
correspondences.filter_by_distance(self.config.max_correspondence_distance)
|
||||
}
|
||||
}
|
||||
|
||||
@@ -648,10 +665,97 @@ pub fn match_scan_with_config(
|
||||
icp.match_scan(points, lines, initial_pose)
|
||||
}
|
||||
|
||||
/// Match scan using multi-resolution (coarse-to-fine) ICP.
|
||||
///
|
||||
/// This method improves convergence by starting with subsampled points
|
||||
/// (coarse level) and progressively refining with more points (fine level).
|
||||
/// Useful when the initial pose guess may be poor.
|
||||
///
|
||||
/// # Arguments
|
||||
/// * `points` - Scan points in robot frame
|
||||
/// * `lines` - Map lines in world frame
|
||||
/// * `initial_pose` - Initial pose estimate
|
||||
/// * `multi_config` - Multi-resolution pyramid configuration
|
||||
/// * `icp_config` - Base ICP configuration (iterations overridden per level)
|
||||
///
|
||||
/// # Returns
|
||||
/// Match result with optimized pose from finest level.
|
||||
///
|
||||
/// # Example
|
||||
/// ```rust,ignore
|
||||
/// use vastu_map::matching::{match_scan_multiresolution, MultiResolutionConfig, IcpConfig};
|
||||
///
|
||||
/// let multi_config = MultiResolutionConfig::enabled()
|
||||
/// .with_levels(3)
|
||||
/// .with_iterations(vec![3, 5, 10]);
|
||||
/// let icp_config = IcpConfig::default();
|
||||
///
|
||||
/// let result = match_scan_multiresolution(
|
||||
/// &points, &lines, initial_pose, &multi_config, &icp_config
|
||||
/// );
|
||||
/// ```
|
||||
pub fn match_scan_multiresolution(
|
||||
points: &[Point2D],
|
||||
lines: &[Line2D],
|
||||
initial_pose: Pose2D,
|
||||
multi_config: &super::config::MultiResolutionConfig,
|
||||
icp_config: &IcpConfig,
|
||||
) -> MatchResult {
|
||||
if points.is_empty() || lines.is_empty() {
|
||||
return MatchResult::new(initial_pose, CorrespondenceSet::new(), 0, false);
|
||||
}
|
||||
|
||||
// If multi-resolution disabled, fall back to standard ICP
|
||||
if !multi_config.enabled || multi_config.levels == 0 {
|
||||
return match_scan_with_config(points, lines, initial_pose, icp_config.clone());
|
||||
}
|
||||
|
||||
let mut pose = initial_pose;
|
||||
let mut total_iterations = 0;
|
||||
let mut final_result = None;
|
||||
|
||||
// Process from coarsest (level 0) to finest (level N-1)
|
||||
for level in 0..multi_config.levels {
|
||||
let subsample = multi_config.subsample_for_level(level);
|
||||
let level_iterations = multi_config.iterations_for_level(level);
|
||||
|
||||
// Subsample points deterministically using step_by
|
||||
let subsampled: Vec<Point2D> = points.iter().step_by(subsample.max(1)).copied().collect();
|
||||
|
||||
// Skip if too few points after subsampling
|
||||
if subsampled.len() < icp_config.min_correspondences {
|
||||
continue;
|
||||
}
|
||||
|
||||
// Configure ICP for this level
|
||||
let mut level_config = icp_config.clone();
|
||||
level_config.max_iterations = level_iterations;
|
||||
|
||||
// Run ICP at this level
|
||||
let icp = PointToLineIcp::with_config(level_config);
|
||||
let result = icp.match_scan(&subsampled, lines, pose);
|
||||
|
||||
// Update pose for next level
|
||||
pose = result.pose;
|
||||
total_iterations += result.iterations;
|
||||
final_result = Some(result);
|
||||
}
|
||||
|
||||
// Return result from finest level (or initial if no levels ran)
|
||||
match final_result {
|
||||
Some(mut result) => {
|
||||
result.iterations = total_iterations;
|
||||
result
|
||||
}
|
||||
None => MatchResult::new(pose, CorrespondenceSet::new(), 0, false),
|
||||
}
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
mod tests {
|
||||
use super::*;
|
||||
use crate::core::Point2D;
|
||||
use crate::matching::RobustCostFunction;
|
||||
use approx::assert_relative_eq;
|
||||
|
||||
fn make_room_lines() -> Vec<Line2D> {
|
||||
@@ -764,7 +868,7 @@ mod tests {
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_icp_with_ransac() {
|
||||
fn test_icp_with_tukey_robust_cost() {
|
||||
let lines = make_room_lines();
|
||||
let mut points = make_scan_points_at_origin();
|
||||
|
||||
@@ -773,33 +877,37 @@ mod tests {
|
||||
points.push(Point2D::new(-10.0, -10.0));
|
||||
points.push(Point2D::new(5.0, -5.0));
|
||||
|
||||
// Use Tukey for aggressive outlier rejection (zero weight beyond cutoff)
|
||||
let config = IcpConfig::default()
|
||||
.with_outlier_rejection(OutlierRejection::Ransac(RansacConfig::default()));
|
||||
.with_robust_cost(RobustCostFunction::Tukey { c: 0.1 })
|
||||
.with_irls(true);
|
||||
|
||||
let icp = PointToLineIcp::with_config(config);
|
||||
let result = icp.match_scan(&points, &lines, Pose2D::identity());
|
||||
|
||||
// Should still converge despite outliers
|
||||
// Should still converge despite outliers (they get zero weight)
|
||||
assert!(result.converged);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_icp_distance_threshold() {
|
||||
fn test_icp_with_huber_robust_cost() {
|
||||
let lines = make_room_lines();
|
||||
let mut points = make_scan_points_at_origin();
|
||||
|
||||
// Add outliers far from lines
|
||||
points.push(Point2D::new(0.0, 0.0)); // Center - far from all walls
|
||||
|
||||
let config =
|
||||
IcpConfig::default().with_outlier_rejection(OutlierRejection::DistanceThreshold(0.5));
|
||||
// Use Huber for soft outlier down-weighting
|
||||
let config = IcpConfig::default()
|
||||
.with_robust_cost(RobustCostFunction::Huber { delta: 0.03 })
|
||||
.with_irls(true);
|
||||
|
||||
let icp = PointToLineIcp::with_config(config);
|
||||
let result = icp.match_scan(&points, &lines, Pose2D::identity());
|
||||
|
||||
assert!(result.converged);
|
||||
// Center point should be filtered out
|
||||
assert!(result.correspondences.len() < points.len());
|
||||
// Outlier should have low weight but still be included
|
||||
assert!(!result.correspondences.is_empty());
|
||||
}
|
||||
|
||||
#[test]
|
||||
|
||||
@@ -3,8 +3,8 @@
|
||||
//! This module implements the full scan-to-map matching pipeline:
|
||||
//!
|
||||
//! 1. **Correspondence Search**: Find nearest map lines for each scan point
|
||||
//! 2. **Outlier Rejection**: Use RANSAC or distance threshold
|
||||
//! 3. **Pose Optimization**: Gauss-Newton solver for point-to-line error
|
||||
//! 2. **Robust Weighting**: Apply M-estimator weights via IRLS to down-weight outliers
|
||||
//! 3. **Pose Optimization**: Levenberg-Marquardt solver for weighted point-to-line error
|
||||
//! 4. **Iteration**: Repeat until convergence
|
||||
//!
|
||||
//! # Point-to-Line vs Point-to-Point
|
||||
@@ -13,10 +13,18 @@
|
||||
//! - Points can slide along lines in the tangent direction
|
||||
//! - Only the perpendicular distance matters
|
||||
//! - Better handles structured environments (walls, edges)
|
||||
//!
|
||||
//! # Robust Cost Functions
|
||||
//!
|
||||
//! Use `RobustCostFunction` with IRLS for soft outlier rejection:
|
||||
//! - **Huber** (default): Good balance between robustness and efficiency
|
||||
//! - **Cauchy**: Heavy-tailed, good for cluttered environments
|
||||
//! - **Tukey**: Zero weight for large residuals (aggressive rejection)
|
||||
//! - **Geman-McClure**: Redescending M-estimator for multimodal errors
|
||||
|
||||
mod config;
|
||||
mod matcher;
|
||||
|
||||
// Re-export public API
|
||||
pub use config::{CoarseSearchConfig, IcpConfig, OutlierRejection};
|
||||
pub use matcher::{PointToLineIcp, match_scan, match_scan_with_config};
|
||||
pub use config::{CoarseSearchConfig, IcpConfig, MultiResolutionConfig};
|
||||
pub use matcher::{PointToLineIcp, match_scan, match_scan_multiresolution, match_scan_with_config};
|
||||
|
||||
@@ -8,8 +8,9 @@
|
||||
//! - **Correspondence**: Types for point-to-line associations
|
||||
//! - **Nearest Neighbor**: Find correspondences by distance
|
||||
//! - **RANSAC**: Robust pose estimation with outlier rejection
|
||||
//! - **Gauss-Newton**: Nonlinear least squares optimization
|
||||
//! - **Point-to-Line ICP**: Full scan matching pipeline
|
||||
//! - **Gauss-Newton/Levenberg-Marquardt**: Nonlinear least squares optimization
|
||||
//! - **Robust Cost Functions**: M-estimators for outlier rejection (Huber, Cauchy, Tukey)
|
||||
//! - **Point-to-Line ICP**: Full scan matching pipeline with IRLS support
|
||||
//! - **Scratch Space**: Pre-allocated buffers for zero-allocation ICP
|
||||
//!
|
||||
//! # Usage
|
||||
@@ -63,24 +64,28 @@ pub mod gauss_newton;
|
||||
pub mod icp;
|
||||
pub mod nearest_neighbor;
|
||||
pub mod ransac;
|
||||
pub mod robust;
|
||||
pub mod scratch;
|
||||
pub mod traits;
|
||||
|
||||
// Re-export main types
|
||||
pub use correspondence::{
|
||||
Correspondence, CorrespondenceSet, IDENTITY_COVARIANCE, MatchResult, PoseCovariance,
|
||||
CornerCorrespondence, CornerCorrespondenceSet, Correspondence, CorrespondenceSet,
|
||||
IDENTITY_COVARIANCE, MatchResult, PoseCovariance,
|
||||
};
|
||||
pub use gauss_newton::{GaussNewtonConfig, GaussNewtonResult, optimize_pose_fast};
|
||||
pub use nearest_neighbor::{
|
||||
NearestNeighborConfig, find_correspondences, find_correspondences_batch,
|
||||
CornerMatchConfig, NearestNeighborConfig, find_corner_correspondences,
|
||||
find_corner_correspondences_weighted, find_correspondences, find_correspondences_batch,
|
||||
find_correspondences_spatial, find_correspondences_weighted, find_correspondences_with_angle,
|
||||
};
|
||||
// Re-export from icp submodule
|
||||
pub use icp::{
|
||||
CoarseSearchConfig, IcpConfig, OutlierRejection, PointToLineIcp, match_scan,
|
||||
match_scan_with_config,
|
||||
CoarseSearchConfig, IcpConfig, MultiResolutionConfig, PointToLineIcp, match_scan,
|
||||
match_scan_multiresolution, match_scan_with_config,
|
||||
};
|
||||
pub use ransac::{RansacConfig, RansacResult, estimate_pose_ransac};
|
||||
pub use robust::RobustCostFunction;
|
||||
pub use scratch::IcpScratchSpace;
|
||||
// Re-export traits
|
||||
pub use traits::ScanMatcher;
|
||||
|
||||
@@ -19,10 +19,12 @@
|
||||
|
||||
use crate::config::LidarNoiseModel;
|
||||
use crate::core::Point2D;
|
||||
use crate::features::{Line2D, LineCollection};
|
||||
use crate::features::{Corner2D, Line2D, LineCollection};
|
||||
use crate::integration::SpatialIndex;
|
||||
|
||||
use super::correspondence::{Correspondence, CorrespondenceSet};
|
||||
use super::correspondence::{
|
||||
CornerCorrespondence, CornerCorrespondenceSet, Correspondence, CorrespondenceSet,
|
||||
};
|
||||
|
||||
/// Configuration for nearest neighbor matching.
|
||||
///
|
||||
@@ -505,6 +507,151 @@ pub fn find_correspondences_with_angle(
|
||||
result
|
||||
}
|
||||
|
||||
/// Configuration for corner correspondence matching.
|
||||
#[derive(Clone, Debug)]
|
||||
pub struct CornerMatchConfig {
|
||||
/// Maximum distance from point to corner for a valid correspondence (meters).
|
||||
/// Default: 0.15m (15cm)
|
||||
pub max_distance: f32,
|
||||
|
||||
/// Whether to only keep the best correspondence per point.
|
||||
/// Default: true
|
||||
pub unique_per_point: bool,
|
||||
}
|
||||
|
||||
impl Default for CornerMatchConfig {
|
||||
fn default() -> Self {
|
||||
Self {
|
||||
max_distance: 0.15,
|
||||
unique_per_point: true,
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
impl CornerMatchConfig {
|
||||
/// Create a new configuration with default values.
|
||||
pub fn new() -> Self {
|
||||
Self::default()
|
||||
}
|
||||
|
||||
/// Builder-style setter for maximum distance.
|
||||
pub fn with_max_distance(mut self, max_distance: f32) -> Self {
|
||||
self.max_distance = max_distance;
|
||||
self
|
||||
}
|
||||
|
||||
/// Builder-style setter for unique per point.
|
||||
pub fn with_unique_per_point(mut self, unique: bool) -> Self {
|
||||
self.unique_per_point = unique;
|
||||
self
|
||||
}
|
||||
}
|
||||
|
||||
/// Find correspondences between scan points and map corners.
|
||||
///
|
||||
/// Uses simple nearest-neighbor matching to associate scan points with
|
||||
/// map corners within a distance threshold.
|
||||
///
|
||||
/// # Arguments
|
||||
/// * `points` - Scan points in world frame (after initial pose transform)
|
||||
/// * `corners` - Map corners to match against
|
||||
/// * `config` - Matching configuration
|
||||
///
|
||||
/// # Returns
|
||||
/// Set of corner correspondences, where each point is matched to its
|
||||
/// nearest corner within the distance threshold.
|
||||
///
|
||||
/// # Example
|
||||
/// ```rust,ignore
|
||||
/// use vastu_map::matching::{find_corner_correspondences, CornerMatchConfig};
|
||||
///
|
||||
/// let corners = vec![/* map corners */];
|
||||
/// let points = vec![/* scan points in world frame */];
|
||||
/// let config = CornerMatchConfig::default();
|
||||
///
|
||||
/// let corrs = find_corner_correspondences(&points, &corners, &config);
|
||||
/// println!("Found {} corner matches", corrs.len());
|
||||
/// ```
|
||||
pub fn find_corner_correspondences(
|
||||
points: &[Point2D],
|
||||
corners: &[Corner2D],
|
||||
config: &CornerMatchConfig,
|
||||
) -> CornerCorrespondenceSet {
|
||||
if points.is_empty() || corners.is_empty() {
|
||||
return CornerCorrespondenceSet::new();
|
||||
}
|
||||
|
||||
let mut result = CornerCorrespondenceSet::with_capacity(corners.len().min(points.len()));
|
||||
let max_dist_sq = config.max_distance * config.max_distance;
|
||||
|
||||
for (point_idx, &point) in points.iter().enumerate() {
|
||||
let mut best_distance_sq = max_dist_sq;
|
||||
let mut best_corner_idx: Option<usize> = None;
|
||||
|
||||
for (corner_idx, corner) in corners.iter().enumerate() {
|
||||
let dx = point.x - corner.position.x;
|
||||
let dy = point.y - corner.position.y;
|
||||
let distance_sq = dx * dx + dy * dy;
|
||||
|
||||
if distance_sq < best_distance_sq {
|
||||
best_distance_sq = distance_sq;
|
||||
best_corner_idx = Some(corner_idx);
|
||||
|
||||
// Early exit if we find a very close match
|
||||
if distance_sq < 0.001 {
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if let Some(corner_idx) = best_corner_idx {
|
||||
let distance = best_distance_sq.sqrt();
|
||||
let corner = &corners[corner_idx];
|
||||
result.push(CornerCorrespondence::new(
|
||||
point_idx,
|
||||
corner_idx,
|
||||
point,
|
||||
corner.position,
|
||||
distance,
|
||||
));
|
||||
}
|
||||
}
|
||||
|
||||
result
|
||||
}
|
||||
|
||||
/// Find corner correspondences with weighting based on range.
|
||||
///
|
||||
/// Similar to `find_corner_correspondences`, but applies weights based on
|
||||
/// the range from the sensor to each point using a lidar noise model.
|
||||
///
|
||||
/// # Arguments
|
||||
/// * `points` - Scan points in world frame
|
||||
/// * `robot_frame_points` - Original scan points in robot frame (for range computation)
|
||||
/// * `corners` - Map corners to match against
|
||||
/// * `config` - Matching configuration
|
||||
/// * `noise_model` - Lidar noise model for weight computation
|
||||
pub fn find_corner_correspondences_weighted(
|
||||
points: &[Point2D],
|
||||
robot_frame_points: &[Point2D],
|
||||
corners: &[Corner2D],
|
||||
config: &CornerMatchConfig,
|
||||
noise_model: &LidarNoiseModel,
|
||||
) -> CornerCorrespondenceSet {
|
||||
let mut result = find_corner_correspondences(points, corners, config);
|
||||
|
||||
// Apply weights based on range
|
||||
for corr in result.iter_mut() {
|
||||
if corr.point_idx < robot_frame_points.len() {
|
||||
let p = robot_frame_points[corr.point_idx];
|
||||
let range = (p.x * p.x + p.y * p.y).sqrt();
|
||||
corr.weight = noise_model.weight(range);
|
||||
}
|
||||
}
|
||||
|
||||
result
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
mod tests {
|
||||
use super::*;
|
||||
@@ -909,4 +1056,93 @@ mod tests {
|
||||
// Should match all three lines (distances: 0.1, 0.0, 0.1)
|
||||
assert_eq!(corrs.len(), 3, "Should match all 3 parallel lines");
|
||||
}
|
||||
|
||||
// === Corner correspondence tests ===
|
||||
|
||||
fn make_corner(x: f32, y: f32) -> Corner2D {
|
||||
Corner2D::new(Point2D::new(x, y), 0, 1, std::f32::consts::FRAC_PI_2)
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_find_corner_correspondences_basic() {
|
||||
let corners = vec![
|
||||
make_corner(0.0, 0.0),
|
||||
make_corner(5.0, 0.0),
|
||||
make_corner(5.0, 5.0),
|
||||
make_corner(0.0, 5.0),
|
||||
];
|
||||
|
||||
// Point near corner 0
|
||||
let points = vec![Point2D::new(0.05, 0.02)];
|
||||
|
||||
let config = CornerMatchConfig::default();
|
||||
let corrs = find_corner_correspondences(&points, &corners, &config);
|
||||
|
||||
assert_eq!(corrs.len(), 1);
|
||||
assert_eq!(corrs.correspondences[0].corner_idx, 0);
|
||||
// Distance should be sqrt(0.05^2 + 0.02^2) ≈ 0.054
|
||||
assert!(corrs.correspondences[0].distance < 0.06);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_find_corner_correspondences_multiple_points() {
|
||||
let corners = vec![
|
||||
make_corner(0.0, 0.0),
|
||||
make_corner(5.0, 0.0),
|
||||
make_corner(5.0, 5.0),
|
||||
make_corner(0.0, 5.0),
|
||||
];
|
||||
|
||||
// Points near each corner
|
||||
let points = vec![
|
||||
Point2D::new(0.05, 0.02), // Near corner 0
|
||||
Point2D::new(4.98, 0.01), // Near corner 1
|
||||
Point2D::new(5.02, 4.97), // Near corner 2
|
||||
Point2D::new(0.01, 5.03), // Near corner 3
|
||||
];
|
||||
|
||||
let config = CornerMatchConfig::default();
|
||||
let corrs = find_corner_correspondences(&points, &corners, &config);
|
||||
|
||||
assert_eq!(corrs.len(), 4);
|
||||
// Check that each point matched the expected corner
|
||||
assert_eq!(corrs.correspondences[0].corner_idx, 0);
|
||||
assert_eq!(corrs.correspondences[1].corner_idx, 1);
|
||||
assert_eq!(corrs.correspondences[2].corner_idx, 2);
|
||||
assert_eq!(corrs.correspondences[3].corner_idx, 3);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_find_corner_correspondences_no_match() {
|
||||
let corners = vec![make_corner(0.0, 0.0)];
|
||||
let points = vec![Point2D::new(1.0, 1.0)]; // Too far from corner
|
||||
|
||||
let config = CornerMatchConfig::default().with_max_distance(0.1);
|
||||
let corrs = find_corner_correspondences(&points, &corners, &config);
|
||||
|
||||
assert!(corrs.is_empty());
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_find_corner_correspondences_empty_inputs() {
|
||||
let config = CornerMatchConfig::default();
|
||||
|
||||
// Empty points
|
||||
let corrs1 = find_corner_correspondences(&[], &[make_corner(0.0, 0.0)], &config);
|
||||
assert!(corrs1.is_empty());
|
||||
|
||||
// Empty corners
|
||||
let corrs2 = find_corner_correspondences(&[Point2D::new(0.0, 0.0)], &[], &config);
|
||||
assert!(corrs2.is_empty());
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_corner_match_config_builder() {
|
||||
let config = CornerMatchConfig::new()
|
||||
.with_max_distance(0.2)
|
||||
.with_unique_per_point(false);
|
||||
|
||||
assert_eq!(config.max_distance, 0.2);
|
||||
assert!(!config.unique_per_point);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -0,0 +1,297 @@
|
||||
//! Robust cost functions for outlier rejection in ICP.
|
||||
//!
|
||||
//! These M-estimators compute weights that down-weight outliers based on
|
||||
//! residual magnitude, enabling soft outlier rejection without hard thresholds.
|
||||
//!
|
||||
//! # Usage
|
||||
//!
|
||||
//! ```rust,ignore
|
||||
//! use vastu_map::matching::RobustCostFunction;
|
||||
//!
|
||||
//! // Huber is recommended for most cases
|
||||
//! let cost = RobustCostFunction::Huber { delta: 0.03 };
|
||||
//!
|
||||
//! // Compute weight for a residual
|
||||
//! let weight = cost.weight(0.05); // Returns ~0.6 (down-weighted)
|
||||
//! ```
|
||||
//!
|
||||
//! # M-Estimator Comparison
|
||||
//!
|
||||
//! | Estimator | Behavior | Best For |
|
||||
//! |-----------|----------|----------|
|
||||
//! | Huber | Linear beyond delta | General use, balanced |
|
||||
//! | Cauchy | Heavy-tailed | Cluttered environments |
|
||||
//! | Tukey | Zero weight for outliers | Aggressive rejection |
|
||||
//! | Geman-McClure | Redescending | Multimodal errors |
|
||||
|
||||
/// Robust cost function for weighted least squares (IRLS).
|
||||
///
|
||||
/// Computes weights that down-weight outliers based on residual magnitude.
|
||||
/// Use with ICP to improve accuracy in presence of noise and clutter.
|
||||
#[derive(Clone, Debug, PartialEq)]
|
||||
pub enum RobustCostFunction {
|
||||
/// No robust weighting (standard least squares).
|
||||
/// Weight is always 1.0 regardless of residual.
|
||||
None,
|
||||
|
||||
/// Huber loss: quadratic near zero, linear beyond delta.
|
||||
///
|
||||
/// Good balance between robustness and efficiency.
|
||||
/// Recommended as default choice.
|
||||
///
|
||||
/// Weight: 1.0 if |r| ≤ δ, else δ/|r|
|
||||
Huber {
|
||||
/// Threshold where loss transitions from quadratic to linear.
|
||||
/// Default: 0.03 (3cm)
|
||||
delta: f32,
|
||||
},
|
||||
|
||||
/// Cauchy loss (Lorentzian): heavy-tailed distribution.
|
||||
///
|
||||
/// More aggressive outlier rejection than Huber.
|
||||
/// Good for cluttered environments with many outliers.
|
||||
///
|
||||
/// Weight: 1 / (1 + (r/scale)²)
|
||||
Cauchy {
|
||||
/// Scale parameter controlling tail heaviness.
|
||||
/// Default: 0.05
|
||||
scale: f32,
|
||||
},
|
||||
|
||||
/// Tukey biweight: zero weight for large residuals.
|
||||
///
|
||||
/// Most aggressive - completely ignores outliers beyond threshold.
|
||||
/// Use when outliers are clearly distinguishable.
|
||||
///
|
||||
/// Weight: (1 - (r/c)²)² if |r| ≤ c, else 0
|
||||
Tukey {
|
||||
/// Cutoff threshold. Residuals beyond this get zero weight.
|
||||
/// Default: 0.1
|
||||
c: f32,
|
||||
},
|
||||
|
||||
/// Geman-McClure: redescending M-estimator.
|
||||
///
|
||||
/// Good for multimodal error distributions.
|
||||
/// Weights decrease rapidly for large residuals.
|
||||
///
|
||||
/// Weight: 1 / (1 + (r/scale)²)²
|
||||
GemanMcClure {
|
||||
/// Scale parameter.
|
||||
/// Default: 0.05
|
||||
scale: f32,
|
||||
},
|
||||
}
|
||||
|
||||
impl RobustCostFunction {
|
||||
/// Compute weight for IRLS (Iteratively Reweighted Least Squares).
|
||||
///
|
||||
/// Returns a weight in [0, 1] that should multiply the correspondence weight.
|
||||
/// Small residuals get weight ≈ 1, large residuals get down-weighted.
|
||||
///
|
||||
/// # Arguments
|
||||
/// * `residual` - The point-to-line distance (can be negative for signed distance)
|
||||
///
|
||||
/// # Returns
|
||||
/// Weight in [0, 1] to apply to this correspondence
|
||||
#[inline]
|
||||
pub fn weight(&self, residual: f32) -> f32 {
|
||||
let r = residual.abs();
|
||||
match self {
|
||||
Self::None => 1.0,
|
||||
Self::Huber { delta } => {
|
||||
if r <= *delta {
|
||||
1.0
|
||||
} else {
|
||||
*delta / r
|
||||
}
|
||||
}
|
||||
Self::Cauchy { scale } => {
|
||||
let u = r / scale;
|
||||
1.0 / (1.0 + u * u)
|
||||
}
|
||||
Self::Tukey { c } => {
|
||||
if r <= *c {
|
||||
let u = r / c;
|
||||
let v = 1.0 - u * u;
|
||||
v * v
|
||||
} else {
|
||||
0.0
|
||||
}
|
||||
}
|
||||
Self::GemanMcClure { scale } => {
|
||||
let u = r / scale;
|
||||
let v = 1.0 + u * u;
|
||||
1.0 / (v * v)
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// Create Huber cost with default threshold (3cm).
|
||||
#[inline]
|
||||
pub fn default_huber() -> Self {
|
||||
Self::Huber { delta: 0.03 }
|
||||
}
|
||||
|
||||
/// Create Cauchy cost with default scale (5cm).
|
||||
#[inline]
|
||||
pub fn default_cauchy() -> Self {
|
||||
Self::Cauchy { scale: 0.05 }
|
||||
}
|
||||
|
||||
/// Create Tukey cost with default cutoff (10cm).
|
||||
#[inline]
|
||||
pub fn default_tukey() -> Self {
|
||||
Self::Tukey { c: 0.1 }
|
||||
}
|
||||
|
||||
/// Create Geman-McClure cost with default scale (5cm).
|
||||
#[inline]
|
||||
pub fn default_geman_mcclure() -> Self {
|
||||
Self::GemanMcClure { scale: 0.05 }
|
||||
}
|
||||
|
||||
/// Check if this is the None variant (no robust weighting).
|
||||
#[inline]
|
||||
pub fn is_none(&self) -> bool {
|
||||
matches!(self, Self::None)
|
||||
}
|
||||
|
||||
/// Check if this cost function is enabled (not None).
|
||||
#[inline]
|
||||
pub fn is_enabled(&self) -> bool {
|
||||
!self.is_none()
|
||||
}
|
||||
}
|
||||
|
||||
impl Default for RobustCostFunction {
|
||||
/// Default to Huber with 3cm threshold.
|
||||
fn default() -> Self {
|
||||
Self::default_huber()
|
||||
}
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
mod tests {
|
||||
use super::*;
|
||||
use approx::assert_relative_eq;
|
||||
|
||||
#[test]
|
||||
fn test_none_always_returns_one() {
|
||||
let cost = RobustCostFunction::None;
|
||||
assert_eq!(cost.weight(0.0), 1.0);
|
||||
assert_eq!(cost.weight(0.1), 1.0);
|
||||
assert_eq!(cost.weight(1.0), 1.0);
|
||||
assert_eq!(cost.weight(-0.5), 1.0);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_huber_within_delta() {
|
||||
let cost = RobustCostFunction::Huber { delta: 0.05 };
|
||||
assert_eq!(cost.weight(0.0), 1.0);
|
||||
assert_eq!(cost.weight(0.03), 1.0);
|
||||
assert_eq!(cost.weight(0.05), 1.0);
|
||||
assert_eq!(cost.weight(-0.03), 1.0);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_huber_beyond_delta() {
|
||||
let cost = RobustCostFunction::Huber { delta: 0.05 };
|
||||
// At r = 0.1, weight = 0.05 / 0.1 = 0.5
|
||||
assert_relative_eq!(cost.weight(0.1), 0.5, epsilon = 1e-6);
|
||||
// At r = 0.2, weight = 0.05 / 0.2 = 0.25
|
||||
assert_relative_eq!(cost.weight(0.2), 0.25, epsilon = 1e-6);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_cauchy() {
|
||||
let cost = RobustCostFunction::Cauchy { scale: 0.05 };
|
||||
// At r = 0, weight = 1
|
||||
assert_eq!(cost.weight(0.0), 1.0);
|
||||
// At r = scale, weight = 1 / (1 + 1) = 0.5
|
||||
assert_relative_eq!(cost.weight(0.05), 0.5, epsilon = 1e-6);
|
||||
// At r = 2*scale, weight = 1 / (1 + 4) = 0.2
|
||||
assert_relative_eq!(cost.weight(0.1), 0.2, epsilon = 1e-6);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_tukey_within_cutoff() {
|
||||
let cost = RobustCostFunction::Tukey { c: 0.1 };
|
||||
// At r = 0, weight = 1
|
||||
assert_eq!(cost.weight(0.0), 1.0);
|
||||
// At r = c/2, weight = (1 - 0.25)^2 = 0.5625
|
||||
assert_relative_eq!(cost.weight(0.05), 0.5625, epsilon = 1e-6);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_tukey_beyond_cutoff() {
|
||||
let cost = RobustCostFunction::Tukey { c: 0.1 };
|
||||
// Beyond cutoff, weight = 0
|
||||
assert_eq!(cost.weight(0.1), 0.0);
|
||||
assert_eq!(cost.weight(0.2), 0.0);
|
||||
assert_eq!(cost.weight(1.0), 0.0);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_geman_mcclure() {
|
||||
let cost = RobustCostFunction::GemanMcClure { scale: 0.05 };
|
||||
// At r = 0, weight = 1
|
||||
assert_eq!(cost.weight(0.0), 1.0);
|
||||
// At r = scale, weight = 1 / (1 + 1)^2 = 0.25
|
||||
assert_relative_eq!(cost.weight(0.05), 0.25, epsilon = 1e-6);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_default_is_huber() {
|
||||
let cost = RobustCostFunction::default();
|
||||
assert!(matches!(cost, RobustCostFunction::Huber { delta } if (delta - 0.03).abs() < 1e-6));
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_is_none() {
|
||||
assert!(RobustCostFunction::None.is_none());
|
||||
assert!(!RobustCostFunction::default_huber().is_none());
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_is_enabled() {
|
||||
assert!(!RobustCostFunction::None.is_enabled());
|
||||
assert!(RobustCostFunction::default_huber().is_enabled());
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_weights_are_symmetric() {
|
||||
let costs = [
|
||||
RobustCostFunction::None,
|
||||
RobustCostFunction::default_huber(),
|
||||
RobustCostFunction::default_cauchy(),
|
||||
RobustCostFunction::default_tukey(),
|
||||
RobustCostFunction::default_geman_mcclure(),
|
||||
];
|
||||
|
||||
for cost in &costs {
|
||||
assert_eq!(cost.weight(0.03), cost.weight(-0.03));
|
||||
assert_eq!(cost.weight(0.1), cost.weight(-0.1));
|
||||
}
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_weights_decrease_with_residual() {
|
||||
let costs = [
|
||||
RobustCostFunction::default_huber(),
|
||||
RobustCostFunction::default_cauchy(),
|
||||
RobustCostFunction::default_geman_mcclure(),
|
||||
];
|
||||
|
||||
for cost in &costs {
|
||||
let w0 = cost.weight(0.0);
|
||||
let w1 = cost.weight(0.05);
|
||||
let w2 = cost.weight(0.1);
|
||||
let w3 = cost.weight(0.2);
|
||||
|
||||
assert!(w0 >= w1, "Weight should decrease: {} >= {}", w0, w1);
|
||||
assert!(w1 >= w2, "Weight should decrease: {} >= {}", w1, w2);
|
||||
assert!(w2 >= w3, "Weight should decrease: {} >= {}", w2, w3);
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,290 @@
|
||||
//! Motion model for pose prediction.
|
||||
//!
|
||||
//! This is a utility struct for callers (e.g., dhruva-slam) to use when
|
||||
//! computing initial pose estimates for ICP when odometry is unavailable
|
||||
//! or unreliable.
|
||||
//!
|
||||
//! # Usage
|
||||
//!
|
||||
//! ```rust,ignore
|
||||
//! use vastu_map::MotionModel;
|
||||
//!
|
||||
//! let mut motion = MotionModel::new();
|
||||
//!
|
||||
//! // After each successful ICP:
|
||||
//! motion.update(icp_result.pose, current_time);
|
||||
//!
|
||||
//! // For next scan, if odometry unavailable:
|
||||
//! let predicted = motion.predict(next_time);
|
||||
//! let result = icp.match_scan(&points, &lines, predicted, &config);
|
||||
//! ```
|
||||
|
||||
use crate::core::{Pose2D, math::angle_diff};
|
||||
|
||||
/// Constant-velocity motion model for pose prediction.
|
||||
///
|
||||
/// Maintains an exponentially-smoothed velocity estimate based on
|
||||
/// observed pose changes, and uses this to predict future poses.
|
||||
#[derive(Clone, Debug)]
|
||||
pub struct MotionModel {
|
||||
/// Last observed pose.
|
||||
last_pose: Pose2D,
|
||||
/// Smoothed velocity estimate (vx, vy, omega).
|
||||
velocity: (f32, f32, f32),
|
||||
/// Timestamp of last observation.
|
||||
last_time: f64,
|
||||
/// Smoothing factor for velocity updates (0-1).
|
||||
/// Higher = more smoothing (slower response to changes).
|
||||
smoothing: f32,
|
||||
/// Whether the model has been initialized with at least one observation.
|
||||
initialized: bool,
|
||||
}
|
||||
|
||||
impl MotionModel {
|
||||
/// Create a new motion model with default smoothing (0.7).
|
||||
pub fn new() -> Self {
|
||||
Self {
|
||||
last_pose: Pose2D::identity(),
|
||||
velocity: (0.0, 0.0, 0.0),
|
||||
last_time: 0.0,
|
||||
smoothing: 0.7,
|
||||
initialized: false,
|
||||
}
|
||||
}
|
||||
|
||||
/// Create a motion model with custom smoothing factor.
|
||||
///
|
||||
/// # Arguments
|
||||
/// * `smoothing` - Smoothing factor in [0, 1).
|
||||
/// - 0.0 = no smoothing (velocity tracks instantly)
|
||||
/// - 0.9 = high smoothing (slow response to velocity changes)
|
||||
/// Default is 0.7 for a balance of responsiveness and stability.
|
||||
pub fn with_smoothing(smoothing: f32) -> Self {
|
||||
Self {
|
||||
smoothing: smoothing.clamp(0.0, 0.99),
|
||||
..Self::new()
|
||||
}
|
||||
}
|
||||
|
||||
/// Predict pose at a given time based on current velocity estimate.
|
||||
///
|
||||
/// Returns identity pose if not yet initialized.
|
||||
///
|
||||
/// # Arguments
|
||||
/// * `current_time` - Time at which to predict (seconds)
|
||||
pub fn predict(&self, current_time: f64) -> Pose2D {
|
||||
if !self.initialized {
|
||||
return Pose2D::identity();
|
||||
}
|
||||
|
||||
let dt = (current_time - self.last_time) as f32;
|
||||
if dt <= 0.0 {
|
||||
return self.last_pose;
|
||||
}
|
||||
|
||||
Pose2D::new(
|
||||
self.last_pose.x + self.velocity.0 * dt,
|
||||
self.last_pose.y + self.velocity.1 * dt,
|
||||
self.last_pose.theta + self.velocity.2 * dt,
|
||||
)
|
||||
}
|
||||
|
||||
/// Update the motion model with a new pose observation.
|
||||
///
|
||||
/// This updates the internal velocity estimate using exponential smoothing.
|
||||
///
|
||||
/// # Arguments
|
||||
/// * `new_pose` - Observed pose (typically from ICP result)
|
||||
/// * `current_time` - Time of observation (seconds)
|
||||
pub fn update(&mut self, new_pose: Pose2D, current_time: f64) {
|
||||
if self.initialized {
|
||||
let dt = (current_time - self.last_time) as f32;
|
||||
if dt > 0.001 {
|
||||
// Compute instantaneous velocity
|
||||
let new_vx = (new_pose.x - self.last_pose.x) / dt;
|
||||
let new_vy = (new_pose.y - self.last_pose.y) / dt;
|
||||
let new_omega = angle_diff(self.last_pose.theta, new_pose.theta) / dt;
|
||||
|
||||
// Apply exponential smoothing
|
||||
let s = self.smoothing;
|
||||
self.velocity.0 = s * self.velocity.0 + (1.0 - s) * new_vx;
|
||||
self.velocity.1 = s * self.velocity.1 + (1.0 - s) * new_vy;
|
||||
self.velocity.2 = s * self.velocity.2 + (1.0 - s) * new_omega;
|
||||
}
|
||||
}
|
||||
|
||||
self.last_pose = new_pose;
|
||||
self.last_time = current_time;
|
||||
self.initialized = true;
|
||||
}
|
||||
|
||||
/// Reset the motion model to uninitialized state.
|
||||
pub fn reset(&mut self) {
|
||||
*self = Self::with_smoothing(self.smoothing);
|
||||
}
|
||||
|
||||
/// Get the current velocity estimate (vx, vy, omega).
|
||||
///
|
||||
/// Returns (0, 0, 0) if not initialized.
|
||||
pub fn velocity(&self) -> (f32, f32, f32) {
|
||||
if self.initialized {
|
||||
self.velocity
|
||||
} else {
|
||||
(0.0, 0.0, 0.0)
|
||||
}
|
||||
}
|
||||
|
||||
/// Get the linear speed (magnitude of translational velocity).
|
||||
pub fn speed(&self) -> f32 {
|
||||
let (vx, vy, _) = self.velocity();
|
||||
(vx * vx + vy * vy).sqrt()
|
||||
}
|
||||
|
||||
/// Get the angular velocity (omega).
|
||||
pub fn angular_velocity(&self) -> f32 {
|
||||
self.velocity().2
|
||||
}
|
||||
|
||||
/// Check if the model has been initialized.
|
||||
pub fn is_initialized(&self) -> bool {
|
||||
self.initialized
|
||||
}
|
||||
|
||||
/// Get the last observed pose.
|
||||
pub fn last_pose(&self) -> Pose2D {
|
||||
self.last_pose
|
||||
}
|
||||
|
||||
/// Get the timestamp of the last observation.
|
||||
pub fn last_time(&self) -> f64 {
|
||||
self.last_time
|
||||
}
|
||||
|
||||
/// Set the smoothing factor.
|
||||
pub fn set_smoothing(&mut self, smoothing: f32) {
|
||||
self.smoothing = smoothing.clamp(0.0, 0.99);
|
||||
}
|
||||
}
|
||||
|
||||
impl Default for MotionModel {
|
||||
fn default() -> Self {
|
||||
Self::new()
|
||||
}
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
mod tests {
|
||||
use super::*;
|
||||
use approx::assert_relative_eq;
|
||||
|
||||
#[test]
|
||||
fn test_new_model_not_initialized() {
|
||||
let model = MotionModel::new();
|
||||
assert!(!model.is_initialized());
|
||||
assert_eq!(model.velocity(), (0.0, 0.0, 0.0));
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_predict_uninitialized_returns_identity() {
|
||||
let model = MotionModel::new();
|
||||
let predicted = model.predict(1.0);
|
||||
assert_relative_eq!(predicted.x, 0.0);
|
||||
assert_relative_eq!(predicted.y, 0.0);
|
||||
assert_relative_eq!(predicted.theta, 0.0);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_first_update_initializes() {
|
||||
let mut model = MotionModel::new();
|
||||
model.update(Pose2D::new(1.0, 2.0, 0.5), 0.0);
|
||||
|
||||
assert!(model.is_initialized());
|
||||
assert_relative_eq!(model.last_pose().x, 1.0);
|
||||
assert_relative_eq!(model.last_pose().y, 2.0);
|
||||
// First update has no velocity estimate
|
||||
assert_eq!(model.velocity(), (0.0, 0.0, 0.0));
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_velocity_estimation() {
|
||||
let mut model = MotionModel::with_smoothing(0.0); // No smoothing for predictable test
|
||||
|
||||
// First observation
|
||||
model.update(Pose2D::new(0.0, 0.0, 0.0), 0.0);
|
||||
|
||||
// Second observation: moved 1m in x over 1 second
|
||||
model.update(Pose2D::new(1.0, 0.0, 0.0), 1.0);
|
||||
|
||||
let (vx, vy, omega) = model.velocity();
|
||||
assert_relative_eq!(vx, 1.0, epsilon = 0.01);
|
||||
assert_relative_eq!(vy, 0.0, epsilon = 0.01);
|
||||
assert_relative_eq!(omega, 0.0, epsilon = 0.01);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_prediction() {
|
||||
let mut model = MotionModel::with_smoothing(0.0);
|
||||
|
||||
model.update(Pose2D::new(0.0, 0.0, 0.0), 0.0);
|
||||
model.update(Pose2D::new(1.0, 0.0, 0.0), 1.0);
|
||||
|
||||
// Predict 1 second ahead
|
||||
let predicted = model.predict(2.0);
|
||||
assert_relative_eq!(predicted.x, 2.0, epsilon = 0.01);
|
||||
assert_relative_eq!(predicted.y, 0.0, epsilon = 0.01);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_smoothing_effect() {
|
||||
let mut model = MotionModel::with_smoothing(0.5);
|
||||
|
||||
model.update(Pose2D::new(0.0, 0.0, 0.0), 0.0);
|
||||
model.update(Pose2D::new(1.0, 0.0, 0.0), 1.0); // vx = 1.0
|
||||
|
||||
// With smoothing=0.5: new_vx = 0.5 * 0 + 0.5 * 1.0 = 0.5
|
||||
let (vx, _, _) = model.velocity();
|
||||
assert_relative_eq!(vx, 0.5, epsilon = 0.01);
|
||||
|
||||
// Another update with same velocity
|
||||
model.update(Pose2D::new(2.0, 0.0, 0.0), 2.0); // instantaneous vx = 1.0
|
||||
|
||||
// new_vx = 0.5 * 0.5 + 0.5 * 1.0 = 0.75
|
||||
let (vx, _, _) = model.velocity();
|
||||
assert_relative_eq!(vx, 0.75, epsilon = 0.01);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_reset() {
|
||||
let mut model = MotionModel::with_smoothing(0.8);
|
||||
model.update(Pose2D::new(1.0, 1.0, 1.0), 1.0);
|
||||
model.update(Pose2D::new(2.0, 2.0, 2.0), 2.0);
|
||||
|
||||
model.reset();
|
||||
|
||||
assert!(!model.is_initialized());
|
||||
assert_eq!(model.velocity(), (0.0, 0.0, 0.0));
|
||||
// Smoothing should be preserved
|
||||
assert_relative_eq!(model.smoothing, 0.8);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_speed() {
|
||||
let mut model = MotionModel::with_smoothing(0.0);
|
||||
|
||||
model.update(Pose2D::new(0.0, 0.0, 0.0), 0.0);
|
||||
model.update(Pose2D::new(3.0, 4.0, 0.0), 1.0);
|
||||
|
||||
// Speed should be sqrt(3^2 + 4^2) = 5
|
||||
assert_relative_eq!(model.speed(), 5.0, epsilon = 0.01);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_angular_velocity() {
|
||||
let mut model = MotionModel::with_smoothing(0.0);
|
||||
|
||||
model.update(Pose2D::new(0.0, 0.0, 0.0), 0.0);
|
||||
model.update(Pose2D::new(0.0, 0.0, 0.5), 1.0);
|
||||
|
||||
assert_relative_eq!(model.angular_velocity(), 0.5, epsilon = 0.01);
|
||||
}
|
||||
}
|
||||
@@ -1,6 +1,6 @@
|
||||
//! Configuration for VectorMap.
|
||||
|
||||
use crate::extraction::{CornerConfig, SplitMergeConfig};
|
||||
use crate::extraction::{CornerConfig, RansacLineConfig, SplitMergeConfig};
|
||||
use crate::integration::{AssociationConfig, MergerConfig};
|
||||
use crate::loop_closure::LoopClosureConfig;
|
||||
use crate::matching::IcpConfig;
|
||||
@@ -9,9 +9,19 @@ use crate::query::{FrontierConfig, OccupancyConfig};
|
||||
/// Configuration for VectorMap.
|
||||
#[derive(Clone, Debug)]
|
||||
pub struct VectorMapConfig {
|
||||
/// Configuration for line extraction.
|
||||
/// Configuration for line extraction (split-merge).
|
||||
pub extraction: SplitMergeConfig,
|
||||
|
||||
/// Configuration for RANSAC line extraction.
|
||||
/// Used when `use_hybrid_extraction` is true.
|
||||
pub ransac_extraction: RansacLineConfig,
|
||||
|
||||
/// Whether to use hybrid RANSAC + split-merge extraction.
|
||||
/// When true, RANSAC extracts dominant lines first, then split-merge
|
||||
/// processes remaining points. This is more robust to outliers.
|
||||
/// Default: true
|
||||
pub use_hybrid_extraction: bool,
|
||||
|
||||
/// Configuration for corner detection.
|
||||
pub corner: CornerConfig,
|
||||
|
||||
@@ -52,6 +62,8 @@ impl Default for VectorMapConfig {
|
||||
fn default() -> Self {
|
||||
Self {
|
||||
extraction: SplitMergeConfig::default(),
|
||||
ransac_extraction: RansacLineConfig::default(),
|
||||
use_hybrid_extraction: true,
|
||||
corner: CornerConfig::default(),
|
||||
matching: IcpConfig::default(),
|
||||
association: AssociationConfig::default(),
|
||||
@@ -192,4 +204,23 @@ impl VectorMapConfig {
|
||||
self.loop_closure_enabled = enabled;
|
||||
self
|
||||
}
|
||||
|
||||
/// Builder-style setter for hybrid RANSAC + split-merge extraction.
|
||||
///
|
||||
/// When enabled, line extraction uses RANSAC to find dominant lines first,
|
||||
/// then split-merge to process remaining points. This is more robust to
|
||||
/// outliers and cluttered environments.
|
||||
pub fn with_hybrid_extraction(mut self, enabled: bool) -> Self {
|
||||
self.use_hybrid_extraction = enabled;
|
||||
self
|
||||
}
|
||||
|
||||
/// Builder-style setter for multi-resolution ICP.
|
||||
///
|
||||
/// When enabled, scan matching uses coarse-to-fine matching with
|
||||
/// subsampled point clouds for faster convergence from poor initial guesses.
|
||||
pub fn with_multi_resolution_icp(mut self, enabled: bool) -> Self {
|
||||
self.matching.multi_resolution.enabled = enabled;
|
||||
self
|
||||
}
|
||||
}
|
||||
|
||||
@@ -1,7 +1,7 @@
|
||||
//! VectorMap: The main SLAM map implementation.
|
||||
|
||||
use crate::core::{Bounds, Point2D, PointCloud2D, Pose2D};
|
||||
use crate::extraction::{detect_corners, extract_lines};
|
||||
use crate::extraction::{detect_corners, extract_lines, extract_lines_hybrid};
|
||||
use crate::features::{Corner2D, FeatureSet, Line2D};
|
||||
use crate::integration::{
|
||||
batch_merge, create_new_line, find_associations, find_unmatched_scan_lines,
|
||||
@@ -322,8 +322,18 @@ impl VectorMap {
|
||||
fn extract_features(&self, scan_robot_frame: &PointCloud2D, pose: &Pose2D) -> FeatureSet {
|
||||
let points: Vec<Point2D> = scan_robot_frame.iter().collect();
|
||||
|
||||
// Extract lines in robot frame (preserves angular ordering for split-merge)
|
||||
let lines_robot = extract_lines(&points, &self.config.extraction);
|
||||
// Extract lines in robot frame
|
||||
let lines_robot = if self.config.use_hybrid_extraction {
|
||||
// Hybrid: RANSAC for dominant lines, then split-merge for remaining
|
||||
extract_lines_hybrid(
|
||||
&points,
|
||||
&self.config.ransac_extraction,
|
||||
&self.config.extraction,
|
||||
)
|
||||
} else {
|
||||
// Default: split-merge (preserves angular ordering)
|
||||
extract_lines(&points, &self.config.extraction)
|
||||
};
|
||||
|
||||
// Transform lines to world frame
|
||||
let lines_world: Vec<Line2D> = lines_robot.iter().map(|l| l.transform(pose)).collect();
|
||||
|
||||
@@ -3,7 +3,7 @@
|
||||
//! sangam-io's lidar simulator produces `Vec<(f32, f32, u8)>` which maps
|
||||
//! directly to vastu-map's `PolarScan.points` format.
|
||||
|
||||
use vastu_map::core::{PointCloud2D, PolarScan, Pose2D};
|
||||
use vastu_map::core::{PointCloud2D, PolarScan};
|
||||
|
||||
/// Convert lidar scan data from sangam-io format to vastu-map PointCloud2D.
|
||||
///
|
||||
@@ -44,11 +44,6 @@ pub fn lidar_to_point_cloud_full(
|
||||
polar.to_cartesian(min_quality, min_range, max_range)
|
||||
}
|
||||
|
||||
/// Create a vastu-map Pose2D from physics state.
|
||||
pub fn physics_to_pose(x: f32, y: f32, theta: f32) -> Pose2D {
|
||||
Pose2D::new(x, y, theta)
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
mod tests {
|
||||
use super::*;
|
||||
|
||||
@@ -46,6 +46,8 @@ pub struct HarnessConfig {
|
||||
pub dt: f32,
|
||||
/// Lidar scan interval (in simulation steps)
|
||||
pub lidar_interval: usize,
|
||||
/// Fast mode: skip intermediate physics, run only at lidar rate (~22x speedup)
|
||||
pub fast_mode: bool,
|
||||
}
|
||||
|
||||
impl Default for HarnessConfig {
|
||||
@@ -75,6 +77,7 @@ impl Default for HarnessConfig {
|
||||
max_range: 8.0,
|
||||
dt: 1.0 / 110.0, // 110Hz simulation
|
||||
lidar_interval: 22, // ~5Hz lidar (110/22 = 5)
|
||||
fast_mode: true, // Skip physics substeps for faster tests
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -135,20 +138,10 @@ pub struct ObservationRecord {
|
||||
pub truth_pose: Pose2D,
|
||||
/// Estimated pose from SLAM.
|
||||
pub slam_pose: Pose2D,
|
||||
/// Simulation time at this observation.
|
||||
pub sim_time: f32,
|
||||
/// Observation index (0-based).
|
||||
pub observation_idx: usize,
|
||||
/// ICP iterations for this observation.
|
||||
pub icp_iterations: usize,
|
||||
/// Whether ICP converged.
|
||||
pub icp_converged: bool,
|
||||
/// Match confidence (0.0-1.0).
|
||||
pub confidence: f32,
|
||||
/// Drift magnitude (translation error in meters).
|
||||
pub drift_translation: f32,
|
||||
/// Drift magnitude (rotation error in radians).
|
||||
pub drift_rotation: f32,
|
||||
/// Lidar scan at this observation (robot frame) - stored at intervals for visualization.
|
||||
pub scan: Option<vastu_map::core::PointCloud2D>,
|
||||
}
|
||||
@@ -171,57 +164,20 @@ impl TrajectoryHistory {
|
||||
&mut self,
|
||||
truth_pose: Pose2D,
|
||||
slam_pose: Pose2D,
|
||||
sim_time: f32,
|
||||
icp_iterations: usize,
|
||||
icp_converged: bool,
|
||||
confidence: f32,
|
||||
scan: Option<vastu_map::core::PointCloud2D>,
|
||||
) {
|
||||
let drift_translation =
|
||||
((truth_pose.x - slam_pose.x).powi(2) + (truth_pose.y - slam_pose.y).powi(2)).sqrt();
|
||||
let drift_rotation = normalize_angle(truth_pose.theta - slam_pose.theta).abs();
|
||||
|
||||
self.observations.push(ObservationRecord {
|
||||
truth_pose,
|
||||
slam_pose,
|
||||
sim_time,
|
||||
observation_idx: self.observations.len(),
|
||||
icp_iterations,
|
||||
icp_converged,
|
||||
confidence,
|
||||
drift_translation,
|
||||
drift_rotation,
|
||||
scan,
|
||||
});
|
||||
}
|
||||
|
||||
/// Get ground truth path as a list of poses.
|
||||
pub fn ground_truth_path(&self) -> Vec<Pose2D> {
|
||||
self.observations.iter().map(|o| o.truth_pose).collect()
|
||||
}
|
||||
|
||||
/// Get estimated path as a list of poses.
|
||||
pub fn estimated_path(&self) -> Vec<Pose2D> {
|
||||
self.observations.iter().map(|o| o.slam_pose).collect()
|
||||
}
|
||||
|
||||
/// Get maximum drift (translation error) across all observations.
|
||||
pub fn max_drift(&self) -> f32 {
|
||||
self.observations
|
||||
.iter()
|
||||
.map(|o| o.drift_translation)
|
||||
.fold(0.0, f32::max)
|
||||
}
|
||||
|
||||
/// Check if trajectory is empty.
|
||||
pub fn is_empty(&self) -> bool {
|
||||
self.observations.is_empty()
|
||||
}
|
||||
|
||||
/// Get number of observations.
|
||||
pub fn len(&self) -> usize {
|
||||
self.observations.len()
|
||||
}
|
||||
}
|
||||
|
||||
/// Main test harness for running integration tests.
|
||||
@@ -306,26 +262,48 @@ impl TestHarness {
|
||||
|
||||
/// Run a single path segment.
|
||||
fn run_segment(&mut self, segment: &PathSegment) {
|
||||
let steps = (segment.duration / self.config.dt) as usize;
|
||||
if self.config.fast_mode {
|
||||
// Fast mode: skip intermediate physics, run only at lidar rate (~22x speedup)
|
||||
// Each step covers the time of one lidar interval
|
||||
let lidar_dt = self.config.dt * self.config.lidar_interval as f32;
|
||||
let lidar_steps = (segment.duration / lidar_dt).max(1.0) as usize;
|
||||
|
||||
for _ in 0..steps {
|
||||
// Update physics
|
||||
self.physics.update(
|
||||
self.config.dt,
|
||||
segment.linear_vel,
|
||||
segment.angular_vel,
|
||||
&self.map,
|
||||
&self.config.robot,
|
||||
);
|
||||
for _ in 0..lidar_steps {
|
||||
// Single physics update covering the full lidar interval
|
||||
self.physics.update(
|
||||
lidar_dt,
|
||||
segment.linear_vel,
|
||||
segment.angular_vel,
|
||||
&self.map,
|
||||
&self.config.robot,
|
||||
);
|
||||
|
||||
self.tick_count += 1;
|
||||
self.sim_time += self.config.dt;
|
||||
|
||||
// Generate lidar scan at configured interval
|
||||
if self.tick_count >= self.config.lidar_interval {
|
||||
self.tick_count = 0;
|
||||
self.sim_time += lidar_dt;
|
||||
self.process_lidar_scan();
|
||||
}
|
||||
} else {
|
||||
// Normal mode: full physics simulation at configured rate
|
||||
let steps = (segment.duration / self.config.dt) as usize;
|
||||
|
||||
for _ in 0..steps {
|
||||
// Update physics
|
||||
self.physics.update(
|
||||
self.config.dt,
|
||||
segment.linear_vel,
|
||||
segment.angular_vel,
|
||||
&self.map,
|
||||
&self.config.robot,
|
||||
);
|
||||
|
||||
self.tick_count += 1;
|
||||
self.sim_time += self.config.dt;
|
||||
|
||||
// Generate lidar scan at configured interval
|
||||
if self.tick_count >= self.config.lidar_interval {
|
||||
self.tick_count = 0;
|
||||
self.process_lidar_scan();
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@@ -385,15 +363,8 @@ impl TestHarness {
|
||||
None
|
||||
};
|
||||
|
||||
self.trajectory.record(
|
||||
current_truth,
|
||||
result.pose,
|
||||
self.sim_time,
|
||||
result.icp_iterations,
|
||||
result.icp_converged,
|
||||
result.confidence,
|
||||
scan_to_store,
|
||||
);
|
||||
self.trajectory
|
||||
.record(current_truth, result.pose, result.confidence, scan_to_store);
|
||||
|
||||
log::debug!(
|
||||
"Observation {}: truth=({:.2}, {:.2}, {:.1}°), slam=({:.2}, {:.2}, {:.1}°), conf={:.2}, icp_iters={}",
|
||||
|
||||
@@ -5,17 +5,13 @@
|
||||
|
||||
use vastu_map::core::Pose2D;
|
||||
|
||||
/// Odometry noise/drift configuration.
|
||||
/// Odometry drift configuration.
|
||||
#[derive(Clone, Debug)]
|
||||
pub struct OdometryConfig {
|
||||
/// Systematic drift per meter traveled (e.g., 0.02 = 2% drift)
|
||||
pub drift_per_meter: f32,
|
||||
/// Angular drift per radian rotated
|
||||
pub angular_drift_per_rad: f32,
|
||||
/// Random position noise standard deviation (meters)
|
||||
pub position_noise_stddev: f32,
|
||||
/// Random angular noise standard deviation (radians)
|
||||
pub angular_noise_stddev: f32,
|
||||
}
|
||||
|
||||
impl Default for OdometryConfig {
|
||||
@@ -23,27 +19,15 @@ impl Default for OdometryConfig {
|
||||
Self {
|
||||
drift_per_meter: 0.0,
|
||||
angular_drift_per_rad: 0.0,
|
||||
position_noise_stddev: 0.0,
|
||||
angular_noise_stddev: 0.0,
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
impl OdometryConfig {
|
||||
/// Create a clean odometry config with no drift or noise.
|
||||
/// Create a clean odometry config with no drift.
|
||||
pub fn clean() -> Self {
|
||||
Self::default()
|
||||
}
|
||||
|
||||
/// Create a noisy odometry config for testing robustness.
|
||||
pub fn noisy() -> Self {
|
||||
Self {
|
||||
drift_per_meter: 0.02, // 2% systematic drift
|
||||
angular_drift_per_rad: 0.01, // 1% angular drift
|
||||
position_noise_stddev: 0.005, // 5mm random noise
|
||||
angular_noise_stddev: 0.002, // ~0.1 degree noise
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// Odometry estimator that tracks robot pose from encoder ticks.
|
||||
@@ -147,38 +131,6 @@ impl OdometryEstimator {
|
||||
|
||||
self.accumulated_pose
|
||||
}
|
||||
|
||||
/// Get the relative pose change since the last call to `relative_update`.
|
||||
///
|
||||
/// This is useful for feeding to VectorMap::observe() which expects
|
||||
/// relative odometry.
|
||||
pub fn relative_update(&mut self, left_ticks: u16, right_ticks: u16) -> Pose2D {
|
||||
let prev_pose = self.accumulated_pose;
|
||||
let new_pose = self.update(left_ticks, right_ticks);
|
||||
|
||||
// Compute relative transformation
|
||||
let dx = new_pose.x - prev_pose.x;
|
||||
let dy = new_pose.y - prev_pose.y;
|
||||
let dtheta = normalize_angle(new_pose.theta - prev_pose.theta);
|
||||
|
||||
// Transform back to robot frame (inverse rotation)
|
||||
let cos_theta = prev_pose.theta.cos();
|
||||
let sin_theta = prev_pose.theta.sin();
|
||||
let dx_robot = dx * cos_theta + dy * sin_theta;
|
||||
let dy_robot = -dx * sin_theta + dy * cos_theta;
|
||||
|
||||
Pose2D::new(dx_robot, dy_robot, dtheta)
|
||||
}
|
||||
|
||||
/// Get the current accumulated pose estimate.
|
||||
pub fn pose(&self) -> Pose2D {
|
||||
self.accumulated_pose
|
||||
}
|
||||
|
||||
/// Reset the odometry to a given pose.
|
||||
pub fn reset(&mut self, pose: Pose2D) {
|
||||
self.accumulated_pose = pose;
|
||||
}
|
||||
}
|
||||
|
||||
/// Normalize angle to [-PI, PI] range.
|
||||
|
||||
@@ -38,10 +38,6 @@ mod colors {
|
||||
pub const LIDAR_SCAN: &str = "#56B4E9";
|
||||
/// Lidar scan history - lighter, semi-transparent
|
||||
pub const LIDAR_SCAN_HISTORY: &str = "#56B4E9";
|
||||
/// High confidence - green
|
||||
pub const CONFIDENCE_HIGH: &str = "#00FF00";
|
||||
/// Low confidence - red
|
||||
pub const CONFIDENCE_LOW: &str = "#FF0000";
|
||||
}
|
||||
|
||||
/// Interval for displaying drift vectors (every Nth observation).
|
||||
@@ -462,69 +458,6 @@ impl Visualizer {
|
||||
group
|
||||
}
|
||||
|
||||
/// Render the SLAM result to an SVG file (legacy method for backward compatibility).
|
||||
pub fn render(
|
||||
&self,
|
||||
slam: &VectorMap,
|
||||
map: &SimulationMap,
|
||||
final_pose: Pose2D,
|
||||
truth_pose: Pose2D,
|
||||
last_scan: Option<&PointCloud2D>,
|
||||
) {
|
||||
// Compute bounds from map
|
||||
let map_width_m = map.width() as f32 * map.resolution();
|
||||
let map_height_m = map.height() as f32 * map.resolution();
|
||||
|
||||
let width = (map_width_m * self.scale + 2.0 * self.margin) as i32;
|
||||
let height = (map_height_m * self.scale + 2.0 * self.margin) as i32;
|
||||
|
||||
// Create document
|
||||
let mut doc = Document::new()
|
||||
.set("width", width)
|
||||
.set("height", height)
|
||||
.set("viewBox", (0, 0, width, height));
|
||||
|
||||
// Background
|
||||
doc = doc.add(
|
||||
Rectangle::new()
|
||||
.set("x", 0)
|
||||
.set("y", 0)
|
||||
.set("width", width)
|
||||
.set("height", height)
|
||||
.set("fill", "white"),
|
||||
);
|
||||
|
||||
// Map walls (gray)
|
||||
let map_group = self.render_map_walls(map, height as f32);
|
||||
doc = doc.add(map_group);
|
||||
|
||||
// Lidar scan points (cyan) - transformed to world frame using final pose
|
||||
if let Some(scan) = last_scan {
|
||||
let scan_group = self.render_lidar_scan(scan, final_pose, height as f32);
|
||||
doc = doc.add(scan_group);
|
||||
}
|
||||
|
||||
// Detected walls (blue solid)
|
||||
let detected_group = self.render_detected(slam, height as f32);
|
||||
doc = doc.add(detected_group);
|
||||
|
||||
// Poses
|
||||
let pose_group = self.render_poses(final_pose, truth_pose, height as f32);
|
||||
doc = doc.add(pose_group);
|
||||
|
||||
// Legend
|
||||
let legend = self.render_legend(last_scan.is_some());
|
||||
doc = doc.add(legend);
|
||||
|
||||
// Ensure output directory exists
|
||||
if let Some(parent) = std::path::Path::new(&self.output_path).parent() {
|
||||
std::fs::create_dir_all(parent).ok();
|
||||
}
|
||||
|
||||
// Save
|
||||
svg::save(&self.output_path, &doc).expect("Failed to save SVG");
|
||||
}
|
||||
|
||||
/// Render map walls by sampling the occupancy grid.
|
||||
fn render_map_walls(&self, map: &SimulationMap, svg_height: f32) -> Group {
|
||||
let mut group = Group::new().set("id", "map_walls");
|
||||
@@ -627,23 +560,6 @@ impl Visualizer {
|
||||
group
|
||||
}
|
||||
|
||||
fn render_legend(&self, has_scan: bool) -> Group {
|
||||
let legend_text = if has_scan {
|
||||
"Blue=Detected, Cyan=LidarScan, Gray=MapWalls, Green=TruePose, Red=EstimatedPose"
|
||||
} else {
|
||||
"Blue=Detected, Gray=MapWalls, Green=TruePose, Red=EstimatedPose"
|
||||
};
|
||||
|
||||
Group::new().set("id", "legend").add(
|
||||
Text::new(legend_text)
|
||||
.set("x", 10)
|
||||
.set("y", 20)
|
||||
.set("font-size", 12)
|
||||
.set("font-family", "monospace")
|
||||
.set("fill", "black"),
|
||||
)
|
||||
}
|
||||
|
||||
fn transform_point(
|
||||
&self,
|
||||
p: Point2D,
|
||||
|
||||
Reference in New Issue
Block a user