Add zero-allocation APIs, ICP presets, and configuration validation
- Add zero-allocation methods: compute_weights_into, compute_weights_for_cloud, to_cartesian_into for hot-path performance - Add FitError enum and fit_line_checked for explicit error handling in line fitting - Add Line2D helpers: perpendicular_at, extended, contains_projection_extended - Extract validate_correspondence_candidate helper for cleaner matching code - Add ICP preset configurations: fast, accurate, robust, balanced - Add configurable quality score weights (distance_weight, angle_weight) to AssociationConfig - Add ConfigError and VectorMapConfig::validate() for configuration validation
This commit is contained in:
codetiger
@@ -60,6 +60,63 @@ impl LidarNoiseModel {
|
||||
pub fn weights(&self, ranges: &[f32]) -> Vec<f32> {
|
||||
ranges.iter().map(|&r| self.weight(r)).collect()
|
||||
}
|
||||
|
||||
/// Compute weights for a batch of ranges into a pre-allocated buffer (zero-allocation).
|
||||
///
|
||||
/// # Panics
|
||||
/// Panics if `weights.len() != ranges.len()`.
|
||||
///
|
||||
/// # Example
|
||||
/// ```rust,ignore
|
||||
/// let model = LidarNoiseModel::default();
|
||||
/// let ranges = vec![1.0, 2.0, 3.0];
|
||||
/// let mut weights = vec![0.0; ranges.len()];
|
||||
///
|
||||
/// model.compute_weights_into(&ranges, &mut weights);
|
||||
/// ```
|
||||
#[inline]
|
||||
pub fn compute_weights_into(&self, ranges: &[f32], weights: &mut [f32]) {
|
||||
debug_assert_eq!(
|
||||
ranges.len(),
|
||||
weights.len(),
|
||||
"weights buffer must match ranges length"
|
||||
);
|
||||
for (r, w) in ranges.iter().zip(weights.iter_mut()) {
|
||||
*w = self.weight(*r);
|
||||
}
|
||||
}
|
||||
|
||||
/// Compute weights for a PointCloud2D based on point distances from origin.
|
||||
///
|
||||
/// This is useful for weighting correspondences during scan matching.
|
||||
///
|
||||
/// # Arguments
|
||||
/// * `xs` - X coordinates of points
|
||||
/// * `ys` - Y coordinates of points
|
||||
/// * `weights` - Output buffer for weights (must match points length)
|
||||
///
|
||||
/// # Example
|
||||
/// ```rust,ignore
|
||||
/// let model = LidarNoiseModel::default();
|
||||
/// let cloud = PointCloud2D::from_points(&points);
|
||||
/// let mut weights = vec![0.0; cloud.len()];
|
||||
///
|
||||
/// model.compute_weights_for_cloud(&cloud.xs, &cloud.ys, &mut weights);
|
||||
/// ```
|
||||
#[inline]
|
||||
pub fn compute_weights_for_cloud(&self, xs: &[f32], ys: &[f32], weights: &mut [f32]) {
|
||||
debug_assert_eq!(xs.len(), ys.len(), "xs and ys must have same length");
|
||||
debug_assert_eq!(
|
||||
xs.len(),
|
||||
weights.len(),
|
||||
"weights buffer must match points length"
|
||||
);
|
||||
|
||||
for i in 0..xs.len() {
|
||||
let range = (xs[i] * xs[i] + ys[i] * ys[i]).sqrt();
|
||||
weights[i] = self.weight(range);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// Configuration for VectorMap SLAM.
|
||||
@@ -363,6 +420,43 @@ mod tests {
|
||||
assert!(weights[2] > weights[3]);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_compute_weights_into() {
|
||||
let model = LidarNoiseModel::default();
|
||||
let ranges = vec![0.5, 1.0, 2.0, 5.0];
|
||||
let mut weights = vec![0.0; ranges.len()];
|
||||
|
||||
model.compute_weights_into(&ranges, &mut weights);
|
||||
|
||||
// Should match allocating version
|
||||
let expected = model.weights(&ranges);
|
||||
for (w, e) in weights.iter().zip(expected.iter()) {
|
||||
assert!((w - e).abs() < 1e-6);
|
||||
}
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_compute_weights_for_cloud() {
|
||||
let model = LidarNoiseModel::default();
|
||||
let xs = vec![1.0, 0.0, 3.0, 4.0];
|
||||
let ys = vec![0.0, 2.0, 4.0, 0.0];
|
||||
let mut weights = vec![0.0; xs.len()];
|
||||
|
||||
model.compute_weights_for_cloud(&xs, &ys, &mut weights);
|
||||
|
||||
// Check against manual range computation
|
||||
let ranges: Vec<f32> = xs
|
||||
.iter()
|
||||
.zip(ys.iter())
|
||||
.map(|(x, y)| (x * x + y * y).sqrt())
|
||||
.collect();
|
||||
let expected = model.weights(&ranges);
|
||||
|
||||
for (w, e) in weights.iter().zip(expected.iter()) {
|
||||
assert!((w - e).abs() < 1e-6);
|
||||
}
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_config_with_noise_model() {
|
||||
let custom_model = LidarNoiseModel::new(0.02, 0.002);
|
||||
|
||||
@@ -78,6 +78,47 @@ impl PolarScan {
|
||||
|
||||
cloud
|
||||
}
|
||||
|
||||
/// Convert to cartesian coordinates into a pre-existing PointCloud2D (zero-allocation).
|
||||
///
|
||||
/// Clears the target cloud and fills it with converted points.
|
||||
/// Filters by quality threshold and valid range.
|
||||
///
|
||||
/// # Arguments
|
||||
/// * `min_quality` - Minimum quality value to include
|
||||
/// * `min_range` - Minimum valid range in meters
|
||||
/// * `max_range` - Maximum valid range in meters
|
||||
/// * `target` - Pre-allocated point cloud to fill (cleared first)
|
||||
///
|
||||
/// # Example
|
||||
/// ```rust,ignore
|
||||
/// let mut cloud = PointCloud2D::with_capacity(360);
|
||||
///
|
||||
/// for scan in scans {
|
||||
/// // Reuse cloud buffer to avoid allocations
|
||||
/// scan.to_cartesian_into(50, 0.1, 12.0, &mut cloud);
|
||||
/// process(&cloud);
|
||||
/// }
|
||||
/// ```
|
||||
pub fn to_cartesian_into(
|
||||
&self,
|
||||
min_quality: u8,
|
||||
min_range: f32,
|
||||
max_range: f32,
|
||||
target: &mut PointCloud2D,
|
||||
) {
|
||||
target.clear();
|
||||
target.xs.reserve(self.points.len());
|
||||
target.ys.reserve(self.points.len());
|
||||
|
||||
for &(angle, dist, quality) in &self.points {
|
||||
if quality >= min_quality && dist >= min_range && dist <= max_range {
|
||||
let (sin, cos) = angle.sin_cos();
|
||||
target.xs.push(dist * cos);
|
||||
target.ys.push(dist * sin);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// Cartesian point cloud with SoA (Struct of Arrays) layout.
|
||||
@@ -395,6 +436,36 @@ mod tests {
|
||||
assert_relative_eq!(cloud.xs[0], 0.5, epsilon = 1e-6);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_polar_scan_to_cartesian_into() {
|
||||
let mut scan = PolarScan::new();
|
||||
scan.push(0.0, 1.0, 100); // Forward, 1m
|
||||
scan.push(FRAC_PI_2, 2.0, 100); // Left, 2m
|
||||
|
||||
// First conversion
|
||||
let mut cloud = PointCloud2D::new();
|
||||
scan.to_cartesian_into(50, 0.0, 10.0, &mut cloud);
|
||||
|
||||
assert_eq!(cloud.len(), 2);
|
||||
assert_relative_eq!(cloud.xs[0], 1.0, epsilon = 1e-6);
|
||||
assert_relative_eq!(cloud.ys[0], 0.0, epsilon = 1e-6);
|
||||
|
||||
// Reuse buffer - should be cleared and refilled
|
||||
scan.push(PI, 3.0, 100); // Add another point
|
||||
scan.to_cartesian_into(50, 0.0, 10.0, &mut cloud);
|
||||
|
||||
assert_eq!(cloud.len(), 3);
|
||||
// Buffer reused - no allocation
|
||||
|
||||
// Verify equivalence to allocating version
|
||||
let cloud_alloc = scan.to_cartesian(50, 0.0, 10.0);
|
||||
assert_eq!(cloud.len(), cloud_alloc.len());
|
||||
for i in 0..cloud.len() {
|
||||
assert_relative_eq!(cloud.xs[i], cloud_alloc.xs[i], epsilon = 1e-6);
|
||||
assert_relative_eq!(cloud.ys[i], cloud_alloc.ys[i], epsilon = 1e-6);
|
||||
}
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_point_cloud_from_points() {
|
||||
let points = vec![
|
||||
|
||||
@@ -18,11 +18,91 @@
|
||||
//! let points = vec![/* lidar hits */];
|
||||
//! let line = fit_line_from_sensor(&points, sensor_pos, None);
|
||||
//! ```
|
||||
//!
|
||||
//! # Error Handling
|
||||
//!
|
||||
//! For explicit error handling, use the `_checked` variants that return `Result`:
|
||||
//!
|
||||
//! ```rust,ignore
|
||||
//! use vastu_map::extraction::{fit_line_checked, FitError};
|
||||
//!
|
||||
//! match fit_line_checked(&points) {
|
||||
//! Ok(line) => println!("Fitted line: {:?}", line),
|
||||
//! Err(FitError::InsufficientPoints { got, required }) => {
|
||||
//! println!("Need {} points, got {}", required, got);
|
||||
//! }
|
||||
//! Err(FitError::DegenerateGeometry) => {
|
||||
//! println!("Points are collocated or form a circle");
|
||||
//! }
|
||||
//! }
|
||||
//! ```
|
||||
|
||||
use crate::config::LidarNoiseModel;
|
||||
use crate::core::Point2D;
|
||||
use crate::features::Line2D;
|
||||
|
||||
// ============================================================================
|
||||
// Error Types
|
||||
// ============================================================================
|
||||
|
||||
/// Errors that can occur during line fitting.
|
||||
#[derive(Debug, Clone, Copy, PartialEq)]
|
||||
pub enum FitError {
|
||||
/// Not enough points to fit a line.
|
||||
InsufficientPoints {
|
||||
/// Number of points provided.
|
||||
got: usize,
|
||||
/// Minimum required (2).
|
||||
required: usize,
|
||||
},
|
||||
/// Points are collocated, form a circle, or are otherwise degenerate.
|
||||
DegenerateGeometry,
|
||||
/// Weights array length doesn't match points array length.
|
||||
WeightsMismatch {
|
||||
/// Number of points.
|
||||
points_len: usize,
|
||||
/// Number of weights.
|
||||
weights_len: usize,
|
||||
},
|
||||
/// Total weight is zero or negative.
|
||||
InvalidWeights,
|
||||
}
|
||||
|
||||
impl std::fmt::Display for FitError {
|
||||
fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result {
|
||||
match self {
|
||||
FitError::InsufficientPoints { got, required } => {
|
||||
write!(
|
||||
f,
|
||||
"insufficient points for line fitting: got {}, need {}",
|
||||
got, required
|
||||
)
|
||||
}
|
||||
FitError::DegenerateGeometry => {
|
||||
write!(
|
||||
f,
|
||||
"degenerate geometry: points are collocated or form a circle"
|
||||
)
|
||||
}
|
||||
FitError::WeightsMismatch {
|
||||
points_len,
|
||||
weights_len,
|
||||
} => {
|
||||
write!(
|
||||
f,
|
||||
"weights length mismatch: {} points, {} weights",
|
||||
points_len, weights_len
|
||||
)
|
||||
}
|
||||
FitError::InvalidWeights => {
|
||||
write!(f, "invalid weights: total weight is zero or negative")
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
impl std::error::Error for FitError {}
|
||||
|
||||
/// Fit a line to a set of points using Total Least Squares.
|
||||
///
|
||||
/// The algorithm:
|
||||
@@ -135,6 +215,112 @@ pub fn fit_line(points: &[Point2D]) -> Option<Line2D> {
|
||||
Some(Line2D::new(start, end))
|
||||
}
|
||||
|
||||
/// Fit a line to a set of points using Total Least Squares (with explicit errors).
|
||||
///
|
||||
/// This is the error-returning variant of [`fit_line`]. Use this when you need
|
||||
/// to distinguish between different failure modes.
|
||||
///
|
||||
/// # Errors
|
||||
///
|
||||
/// Returns `Err(FitError::InsufficientPoints)` if fewer than 2 points provided.
|
||||
/// Returns `Err(FitError::DegenerateGeometry)` if points are collocated or form a circle.
|
||||
///
|
||||
/// # Example
|
||||
/// ```
|
||||
/// use vastu_map::extraction::{fit_line_checked, FitError};
|
||||
/// use vastu_map::core::Point2D;
|
||||
///
|
||||
/// let points = vec![Point2D::new(0.0, 0.0)]; // Only one point
|
||||
/// let result = fit_line_checked(&points);
|
||||
/// assert!(matches!(result, Err(FitError::InsufficientPoints { .. })));
|
||||
/// ```
|
||||
pub fn fit_line_checked(points: &[Point2D]) -> Result<Line2D, FitError> {
|
||||
if points.len() < 2 {
|
||||
return Err(FitError::InsufficientPoints {
|
||||
got: points.len(),
|
||||
required: 2,
|
||||
});
|
||||
}
|
||||
|
||||
// Compute centroid
|
||||
let n = points.len() as f32;
|
||||
let mut sum_x: f32 = 0.0;
|
||||
let mut sum_y: f32 = 0.0;
|
||||
|
||||
for p in points {
|
||||
sum_x += p.x;
|
||||
sum_y += p.y;
|
||||
}
|
||||
|
||||
let centroid = Point2D::new(sum_x / n, sum_y / n);
|
||||
|
||||
// Compute covariance matrix elements
|
||||
let mut cxx: f32 = 0.0;
|
||||
let mut cyy: f32 = 0.0;
|
||||
let mut cxy: f32 = 0.0;
|
||||
|
||||
for p in points {
|
||||
let dx = p.x - centroid.x;
|
||||
let dy = p.y - centroid.y;
|
||||
cxx += dx * dx;
|
||||
cyy += dy * dy;
|
||||
cxy += dx * dy;
|
||||
}
|
||||
|
||||
// Find eigenvectors of 2x2 covariance matrix
|
||||
let trace_half = (cxx + cyy) / 2.0;
|
||||
let det_sqrt = ((cxx - cyy) / 2.0).powi(2) + cxy * cxy;
|
||||
|
||||
if det_sqrt < f32::EPSILON {
|
||||
// Degenerate case - try simple fallback
|
||||
return fit_line_simple_checked(points);
|
||||
}
|
||||
|
||||
let det_sqrt = det_sqrt.sqrt();
|
||||
let lambda1 = trace_half + det_sqrt;
|
||||
|
||||
// Eigenvector for larger eigenvalue (line direction)
|
||||
let direction = if cxy.abs() > f32::EPSILON {
|
||||
Point2D::new(lambda1 - cyy, cxy).normalized()
|
||||
} else if cxx > cyy {
|
||||
Point2D::new(1.0, 0.0)
|
||||
} else {
|
||||
Point2D::new(0.0, 1.0)
|
||||
};
|
||||
|
||||
// Find extent of points along line direction
|
||||
let mut t_min = f32::MAX;
|
||||
let mut t_max = f32::MIN;
|
||||
|
||||
for p in points {
|
||||
let t = (p.x - centroid.x) * direction.x + (p.y - centroid.y) * direction.y;
|
||||
t_min = t_min.min(t);
|
||||
t_max = t_max.max(t);
|
||||
}
|
||||
|
||||
let start = Point2D::new(
|
||||
centroid.x + t_min * direction.x,
|
||||
centroid.y + t_min * direction.y,
|
||||
);
|
||||
let end = Point2D::new(
|
||||
centroid.x + t_max * direction.x,
|
||||
centroid.y + t_max * direction.y,
|
||||
);
|
||||
|
||||
Ok(Line2D::new(start, end))
|
||||
}
|
||||
|
||||
/// Simple line fitting fallback (checked version).
|
||||
fn fit_line_simple_checked(points: &[Point2D]) -> Result<Line2D, FitError> {
|
||||
if points.len() < 2 {
|
||||
return Err(FitError::InsufficientPoints {
|
||||
got: points.len(),
|
||||
required: 2,
|
||||
});
|
||||
}
|
||||
Ok(Line2D::new(points[0], points[points.len() - 1]))
|
||||
}
|
||||
|
||||
/// Simple line fitting fallback using first and last points.
|
||||
fn fit_line_simple(points: &[Point2D]) -> Option<Line2D> {
|
||||
if points.len() < 2 {
|
||||
|
||||
@@ -52,8 +52,8 @@ pub use corner_detection::{
|
||||
detect_curvature_corners, detect_hybrid_corners,
|
||||
};
|
||||
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,
|
||||
FitError, compute_range_weights, fit_line, fit_line_checked, 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::{
|
||||
|
||||
@@ -93,6 +93,80 @@ impl Line2D {
|
||||
)
|
||||
}
|
||||
|
||||
/// Get a point along the line at parameter t.
|
||||
///
|
||||
/// - `t = 0`: returns start point
|
||||
/// - `t = 1`: returns end point
|
||||
/// - `t = 0.5`: returns midpoint
|
||||
#[inline]
|
||||
pub fn point_at(&self, t: f32) -> Point2D {
|
||||
Point2D::new(
|
||||
self.start.x + t * (self.end.x - self.start.x),
|
||||
self.start.y + t * (self.end.y - self.start.y),
|
||||
)
|
||||
}
|
||||
|
||||
/// Get a perpendicular line at parameter t along this line.
|
||||
///
|
||||
/// Returns a unit-length line perpendicular to this line,
|
||||
/// centered at `point_at(t)`.
|
||||
///
|
||||
/// # Arguments
|
||||
/// * `t` - Parameter along line (0=start, 1=end, 0.5=midpoint)
|
||||
/// * `length` - Length of the perpendicular line (centered on the point)
|
||||
///
|
||||
/// # Example
|
||||
/// ```rust,ignore
|
||||
/// let line = Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(4.0, 0.0));
|
||||
/// let perp = line.perpendicular_at(0.5, 2.0);
|
||||
/// // perp goes from (2, -1) to (2, 1) - perpendicular at midpoint
|
||||
/// ```
|
||||
#[inline]
|
||||
pub fn perpendicular_at(&self, t: f32, length: f32) -> Line2D {
|
||||
let center = self.point_at(t);
|
||||
let normal = self.normal();
|
||||
let half_len = length * 0.5;
|
||||
|
||||
Line2D::new(
|
||||
Point2D::new(
|
||||
center.x - normal.x * half_len,
|
||||
center.y - normal.y * half_len,
|
||||
),
|
||||
Point2D::new(
|
||||
center.x + normal.x * half_len,
|
||||
center.y + normal.y * half_len,
|
||||
),
|
||||
)
|
||||
}
|
||||
|
||||
/// Extend this line by a factor in both directions.
|
||||
///
|
||||
/// # Arguments
|
||||
/// * `factor` - Extension factor (1.0 = no change, 2.0 = double length)
|
||||
///
|
||||
/// The line is extended symmetrically from its midpoint.
|
||||
///
|
||||
/// # Example
|
||||
/// ```rust,ignore
|
||||
/// let line = Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(2.0, 0.0));
|
||||
/// let extended = line.extended(2.0);
|
||||
/// // extended goes from (-1, 0) to (3, 0) - doubled in length
|
||||
/// ```
|
||||
#[inline]
|
||||
pub fn extended(&self, factor: f32) -> Line2D {
|
||||
let mid = self.midpoint();
|
||||
let half_dir = Point2D::new(
|
||||
(self.end.x - self.start.x) * 0.5 * factor,
|
||||
(self.end.y - self.start.y) * 0.5 * factor,
|
||||
);
|
||||
|
||||
Line2D::with_observation_count(
|
||||
Point2D::new(mid.x - half_dir.x, mid.y - half_dir.y),
|
||||
Point2D::new(mid.x + half_dir.x, mid.y + half_dir.y),
|
||||
self.observation_count,
|
||||
)
|
||||
}
|
||||
|
||||
/// Angle of the line direction from X-axis (in radians, [-π, π]).
|
||||
#[inline]
|
||||
pub fn angle(&self) -> f32 {
|
||||
@@ -172,12 +246,26 @@ impl Line2D {
|
||||
(0.0..=1.0).contains(&t)
|
||||
}
|
||||
|
||||
/// Get the point on the line at parameter t.
|
||||
/// Check if a projection parameter t falls within the segment with extension tolerance.
|
||||
///
|
||||
/// `t = 0` gives start, `t = 1` gives end.
|
||||
/// Allows points to project slightly beyond line endpoints by `extension` amount.
|
||||
/// The valid range becomes `[-extension, 1 + extension]`.
|
||||
///
|
||||
/// # Arguments
|
||||
/// * `t` - Projection parameter (0 = start, 1 = end)
|
||||
/// * `extension` - Tolerance for projection beyond endpoints (e.g., 0.2 = 20% extension)
|
||||
///
|
||||
/// # Example
|
||||
/// ```rust,ignore
|
||||
/// let line = Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(10.0, 0.0));
|
||||
/// let t = line.project_point(Point2D::new(-1.0, 0.5)); // t ≈ -0.1
|
||||
///
|
||||
/// assert!(!line.contains_projection(t)); // Outside [0, 1]
|
||||
/// assert!(line.contains_projection_extended(t, 0.2)); // Inside [-0.2, 1.2]
|
||||
/// ```
|
||||
#[inline]
|
||||
pub fn point_at(&self, t: f32) -> Point2D {
|
||||
self.start.lerp(self.end, t)
|
||||
pub fn contains_projection_extended(&self, t: f32, extension: f32) -> bool {
|
||||
t >= -extension && t <= 1.0 + extension
|
||||
}
|
||||
|
||||
/// Distance from a point to the line segment (not infinite line).
|
||||
@@ -417,6 +505,24 @@ mod tests {
|
||||
assert!(!line.contains_projection(1.1));
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_contains_projection_extended() {
|
||||
let line = Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(10.0, 0.0));
|
||||
|
||||
// With 0.2 extension, valid range is [-0.2, 1.2]
|
||||
assert!(line.contains_projection_extended(0.0, 0.2));
|
||||
assert!(line.contains_projection_extended(0.5, 0.2));
|
||||
assert!(line.contains_projection_extended(1.0, 0.2));
|
||||
assert!(line.contains_projection_extended(-0.1, 0.2)); // Within extension
|
||||
assert!(line.contains_projection_extended(1.1, 0.2)); // Within extension
|
||||
assert!(!line.contains_projection_extended(-0.3, 0.2)); // Beyond extension
|
||||
assert!(!line.contains_projection_extended(1.3, 0.2)); // Beyond extension
|
||||
|
||||
// Zero extension = strict [0, 1]
|
||||
assert!(!line.contains_projection_extended(-0.1, 0.0));
|
||||
assert!(!line.contains_projection_extended(1.1, 0.0));
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_point_at() {
|
||||
let line = Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(10.0, 0.0));
|
||||
@@ -426,6 +532,50 @@ mod tests {
|
||||
assert_eq!(line.point_at(0.5), Point2D::new(5.0, 0.0));
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_perpendicular_at() {
|
||||
// Horizontal line
|
||||
let line = Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(4.0, 0.0));
|
||||
|
||||
// Perpendicular at midpoint with length 2
|
||||
let perp = line.perpendicular_at(0.5, 2.0);
|
||||
|
||||
// Center should be at (2, 0)
|
||||
assert_relative_eq!(perp.midpoint().x, 2.0, epsilon = 1e-6);
|
||||
assert_relative_eq!(perp.midpoint().y, 0.0, epsilon = 1e-6);
|
||||
|
||||
// Should be vertical (length 2)
|
||||
assert_relative_eq!(perp.length(), 2.0, epsilon = 1e-6);
|
||||
|
||||
// Endpoints should be at (2, -1) and (2, 1)
|
||||
assert_relative_eq!(perp.start.x, 2.0, epsilon = 1e-6);
|
||||
assert_relative_eq!(perp.end.x, 2.0, epsilon = 1e-6);
|
||||
assert_relative_eq!((perp.start.y - perp.end.y).abs(), 2.0, epsilon = 1e-6);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_extended() {
|
||||
let line = Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(2.0, 0.0));
|
||||
|
||||
// Double the length
|
||||
let ext = line.extended(2.0);
|
||||
|
||||
// Midpoint should be same
|
||||
assert_relative_eq!(ext.midpoint().x, 1.0, epsilon = 1e-6);
|
||||
assert_relative_eq!(ext.midpoint().y, 0.0, epsilon = 1e-6);
|
||||
|
||||
// Length should be doubled
|
||||
assert_relative_eq!(ext.length(), 4.0, epsilon = 1e-6);
|
||||
|
||||
// Endpoints should be at (-1, 0) and (3, 0)
|
||||
assert_relative_eq!(ext.start.x, -1.0, epsilon = 1e-6);
|
||||
assert_relative_eq!(ext.end.x, 3.0, epsilon = 1e-6);
|
||||
|
||||
// No change when factor is 1.0
|
||||
let same = line.extended(1.0);
|
||||
assert_relative_eq!(same.length(), 2.0, epsilon = 1e-6);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_distance_to_point_segment() {
|
||||
let line = Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(10.0, 0.0));
|
||||
|
||||
@@ -30,6 +30,20 @@ pub struct AssociationConfig {
|
||||
/// Lines that don't overlap but are close enough may still associate.
|
||||
/// Default: 0.3m
|
||||
pub max_endpoint_gap: f32,
|
||||
|
||||
// === Quality Score Weights ===
|
||||
// These control how the quality score is computed for ranking associations.
|
||||
/// Weight for distance component in quality score.
|
||||
/// Higher values make distance more important in ranking.
|
||||
/// Formula: `1.0 / (1.0 + distance * distance_weight)`
|
||||
/// Default: 10.0
|
||||
pub distance_weight: f32,
|
||||
|
||||
/// Weight for angle component in quality score.
|
||||
/// Higher values make angular alignment more important in ranking.
|
||||
/// Formula: `1.0 / (1.0 + angle_diff * angle_weight)`
|
||||
/// Default: 5.0
|
||||
pub angle_weight: f32,
|
||||
}
|
||||
|
||||
impl Default for AssociationConfig {
|
||||
@@ -39,6 +53,8 @@ impl Default for AssociationConfig {
|
||||
max_angle_difference: 0.2,
|
||||
min_overlap_ratio: 0.3,
|
||||
max_endpoint_gap: 0.3,
|
||||
distance_weight: 10.0,
|
||||
angle_weight: 5.0,
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -66,6 +82,18 @@ impl AssociationConfig {
|
||||
self.min_overlap_ratio = ratio;
|
||||
self
|
||||
}
|
||||
|
||||
/// Builder-style setter for distance weight in quality scoring.
|
||||
pub fn with_distance_weight(mut self, weight: f32) -> Self {
|
||||
self.distance_weight = weight;
|
||||
self
|
||||
}
|
||||
|
||||
/// Builder-style setter for angle weight in quality scoring.
|
||||
pub fn with_angle_weight(mut self, weight: f32) -> Self {
|
||||
self.angle_weight = weight;
|
||||
self
|
||||
}
|
||||
}
|
||||
|
||||
/// Result of line association.
|
||||
@@ -106,10 +134,43 @@ impl Association {
|
||||
}
|
||||
|
||||
/// Compute a quality score for the association (higher is better).
|
||||
///
|
||||
/// Uses default weights (distance=10.0, angle=5.0).
|
||||
/// For custom weights, use `quality_score_with_config`.
|
||||
pub fn quality_score(&self) -> f32 {
|
||||
// Weighted combination of factors
|
||||
let dist_score = 1.0 / (1.0 + self.distance * 10.0);
|
||||
let angle_score = 1.0 / (1.0 + self.angle_diff * 5.0);
|
||||
self.quality_score_with_weights(10.0, 5.0)
|
||||
}
|
||||
|
||||
/// Compute a quality score using custom weights from config.
|
||||
///
|
||||
/// # Arguments
|
||||
/// * `config` - Association configuration containing weight parameters
|
||||
///
|
||||
/// # Example
|
||||
/// ```rust,ignore
|
||||
/// let config = AssociationConfig::default()
|
||||
/// .with_distance_weight(20.0) // Prioritize distance
|
||||
/// .with_angle_weight(2.0); // Relax angle constraint
|
||||
///
|
||||
/// let score = association.quality_score_with_config(&config);
|
||||
/// ```
|
||||
pub fn quality_score_with_config(&self, config: &AssociationConfig) -> f32 {
|
||||
self.quality_score_with_weights(config.distance_weight, config.angle_weight)
|
||||
}
|
||||
|
||||
/// Compute a quality score with explicit weights.
|
||||
///
|
||||
/// # Formula
|
||||
/// ```text
|
||||
/// score = dist_score * angle_score * overlap_ratio
|
||||
/// where:
|
||||
/// dist_score = 1.0 / (1.0 + distance * distance_weight)
|
||||
/// angle_score = 1.0 / (1.0 + angle_diff * angle_weight)
|
||||
/// ```
|
||||
#[inline]
|
||||
pub fn quality_score_with_weights(&self, distance_weight: f32, angle_weight: f32) -> f32 {
|
||||
let dist_score = 1.0 / (1.0 + self.distance * distance_weight);
|
||||
let angle_score = 1.0 / (1.0 + self.angle_diff * angle_weight);
|
||||
let overlap_score = self.overlap_ratio;
|
||||
|
||||
dist_score * angle_score * overlap_score
|
||||
@@ -324,6 +385,29 @@ mod tests {
|
||||
assert!(good.quality_score() > bad.quality_score());
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_association_quality_score_with_weights() {
|
||||
let assoc = Association::new(0, 0, 0.1, 0.1, 1.0, 0.8);
|
||||
|
||||
// Default weights
|
||||
let default_score = assoc.quality_score();
|
||||
|
||||
// Higher distance weight = more penalty for distance
|
||||
let high_dist_weight = assoc.quality_score_with_weights(20.0, 5.0);
|
||||
assert!(high_dist_weight < default_score);
|
||||
|
||||
// Lower angle weight = less penalty for angle difference
|
||||
let low_angle_weight = assoc.quality_score_with_weights(10.0, 2.0);
|
||||
assert!(low_angle_weight > default_score);
|
||||
|
||||
// Using config
|
||||
let config = AssociationConfig::default()
|
||||
.with_distance_weight(5.0)
|
||||
.with_angle_weight(2.0);
|
||||
let config_score = assoc.quality_score_with_config(&config);
|
||||
assert!(config_score > default_score);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_find_associations_identical_lines() {
|
||||
let scan_lines = vec![Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(5.0, 0.0))];
|
||||
|
||||
@@ -123,7 +123,7 @@ pub mod vector_map;
|
||||
|
||||
// Re-export main types at crate root
|
||||
pub use motion_model::MotionModel;
|
||||
pub use vector_map::{VectorMap, VectorMapConfig};
|
||||
pub use vector_map::{ConfigError, VectorMap, VectorMapConfig};
|
||||
|
||||
// Re-export extensibility traits
|
||||
pub use extraction::{LineExtractor, SplitMergeExtractor};
|
||||
|
||||
@@ -99,6 +99,113 @@ impl IcpConfig {
|
||||
Self::default()
|
||||
}
|
||||
|
||||
// ===== Preset Configurations =====
|
||||
|
||||
/// Fast preset: Optimized for speed at the cost of some accuracy.
|
||||
///
|
||||
/// Good for:
|
||||
/// - Real-time applications with tight timing constraints
|
||||
/// - Situations where odometry is reliable
|
||||
/// - High-frequency (>10Hz) scan matching
|
||||
///
|
||||
/// Trade-offs:
|
||||
/// - Fewer iterations may not converge for poor initial guesses
|
||||
/// - No multi-resolution (faster but less robust)
|
||||
/// - Larger correspondence distance (fewer false negatives)
|
||||
pub fn fast() -> Self {
|
||||
let convergence_threshold = 5e-4;
|
||||
Self {
|
||||
max_iterations: 15,
|
||||
convergence_threshold,
|
||||
convergence_threshold_sq: convergence_threshold * convergence_threshold,
|
||||
max_correspondence_distance: 0.6,
|
||||
robust_cost: RobustCostFunction::Huber { delta: 0.05 },
|
||||
use_irls: false, // Skip IRLS for speed
|
||||
min_correspondences: 8,
|
||||
min_confidence: 0.25,
|
||||
use_ransac_init: false, // Trust odometry
|
||||
use_batch_search: true,
|
||||
noise_model: LidarNoiseModel::default(),
|
||||
use_coarse_search: false,
|
||||
coarse_search_confidence_threshold: 0.5,
|
||||
multi_resolution: MultiResolutionConfig {
|
||||
enabled: false,
|
||||
..Default::default()
|
||||
},
|
||||
}
|
||||
}
|
||||
|
||||
/// Accurate preset: Optimized for accuracy at the cost of speed.
|
||||
///
|
||||
/// Good for:
|
||||
/// - Mapping applications where precision matters
|
||||
/// - Situations with poor odometry
|
||||
/// - Loop closure verification
|
||||
///
|
||||
/// Trade-offs:
|
||||
/// - More iterations = longer compute time
|
||||
/// - Tighter thresholds may cause false rejections
|
||||
pub fn accurate() -> Self {
|
||||
let convergence_threshold = 1e-5;
|
||||
Self {
|
||||
max_iterations: 50,
|
||||
convergence_threshold,
|
||||
convergence_threshold_sq: convergence_threshold * convergence_threshold,
|
||||
max_correspondence_distance: 0.4,
|
||||
robust_cost: RobustCostFunction::Tukey { c: 0.02 }, // Aggressive outlier rejection
|
||||
use_irls: true,
|
||||
min_correspondences: 15,
|
||||
min_confidence: 0.4,
|
||||
use_ransac_init: true,
|
||||
use_batch_search: true,
|
||||
noise_model: LidarNoiseModel::default(),
|
||||
use_coarse_search: true,
|
||||
coarse_search_confidence_threshold: 0.6,
|
||||
multi_resolution: MultiResolutionConfig {
|
||||
enabled: true,
|
||||
levels: 3,
|
||||
subsample_factor: 2,
|
||||
iterations_per_level: vec![5, 10, 15],
|
||||
},
|
||||
}
|
||||
}
|
||||
|
||||
/// Balanced preset: Good trade-off between speed and accuracy.
|
||||
///
|
||||
/// This is the same as `default()` but with a more descriptive name.
|
||||
/// Suitable for most indoor navigation applications.
|
||||
pub fn balanced() -> Self {
|
||||
Self::default()
|
||||
}
|
||||
|
||||
/// Robust preset: Optimized for challenging environments with outliers.
|
||||
///
|
||||
/// Good for:
|
||||
/// - Environments with dynamic obstacles
|
||||
/// - Cluttered or partially occluded scenes
|
||||
/// - Glass walls or reflective surfaces
|
||||
///
|
||||
/// Uses aggressive outlier rejection but maintains accuracy.
|
||||
pub fn robust() -> Self {
|
||||
let convergence_threshold = 1e-4;
|
||||
Self {
|
||||
max_iterations: 40,
|
||||
convergence_threshold,
|
||||
convergence_threshold_sq: convergence_threshold * convergence_threshold,
|
||||
max_correspondence_distance: 0.5,
|
||||
robust_cost: RobustCostFunction::GemanMcClure { scale: 0.02 }, // Heavy outlier rejection
|
||||
use_irls: true,
|
||||
min_correspondences: 12,
|
||||
min_confidence: 0.35,
|
||||
use_ransac_init: true,
|
||||
use_batch_search: true,
|
||||
noise_model: LidarNoiseModel::default(),
|
||||
use_coarse_search: true,
|
||||
coarse_search_confidence_threshold: 0.5,
|
||||
multi_resolution: MultiResolutionConfig::default(),
|
||||
}
|
||||
}
|
||||
|
||||
/// Builder-style setter for maximum iterations.
|
||||
pub fn with_max_iterations(mut self, iterations: usize) -> Self {
|
||||
self.max_iterations = iterations;
|
||||
|
||||
@@ -77,10 +77,11 @@ pub use gauss_newton::{
|
||||
GaussNewtonConfig, GaussNewtonResult, optimize_pose_fast, optimize_pose_simd,
|
||||
};
|
||||
pub use nearest_neighbor::{
|
||||
CornerMatchConfig, NearestNeighborConfig, find_corner_correspondences,
|
||||
CornerMatchConfig, CorrespondenceCandidate, NearestNeighborConfig, find_corner_correspondences,
|
||||
find_corner_correspondences_weighted, find_correspondences, find_correspondences_batch,
|
||||
find_correspondences_soa, find_correspondences_soa_unweighted, find_correspondences_spatial,
|
||||
find_correspondences_weighted, find_correspondences_with_angle,
|
||||
validate_correspondence_candidate,
|
||||
};
|
||||
// Re-export from icp submodule
|
||||
pub use icp::{
|
||||
|
||||
@@ -27,6 +27,73 @@ use super::correspondence::{
|
||||
CorrespondenceSoA,
|
||||
};
|
||||
|
||||
// ============================================================================
|
||||
// Correspondence Validation Helper
|
||||
// ============================================================================
|
||||
|
||||
/// Result of validating a potential correspondence.
|
||||
///
|
||||
/// Contains the projection parameter and perpendicular distance needed
|
||||
/// to create a `Correspondence` if the validation passes.
|
||||
#[derive(Clone, Copy, Debug)]
|
||||
pub struct CorrespondenceCandidate {
|
||||
/// Projection parameter along the line (0 = start, 1 = end).
|
||||
pub t: f32,
|
||||
/// Perpendicular distance from point to line.
|
||||
pub distance: f32,
|
||||
}
|
||||
|
||||
/// Validate a potential correspondence between a point and a line.
|
||||
///
|
||||
/// Checks:
|
||||
/// 1. Projection bounds: point projects within the line segment (with extension tolerance)
|
||||
/// 2. Distance threshold: perpendicular distance is within the maximum allowed
|
||||
///
|
||||
/// # Arguments
|
||||
/// * `point` - The scan point to validate
|
||||
/// * `line` - The map line to match against
|
||||
/// * `max_distance` - Maximum allowed perpendicular distance (meters)
|
||||
/// * `max_projection_extension` - Tolerance for projection beyond line endpoints
|
||||
///
|
||||
/// # Returns
|
||||
/// `Some(CorrespondenceCandidate)` if valid, `None` if rejected.
|
||||
///
|
||||
/// # Example
|
||||
/// ```rust,ignore
|
||||
/// let line = Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(5.0, 0.0));
|
||||
/// let point = Point2D::new(2.5, 0.1); // Near middle of line
|
||||
///
|
||||
/// if let Some(candidate) = validate_correspondence_candidate(point, &line, 0.5, 0.2) {
|
||||
/// // Point is valid: distance=0.1, t=0.5
|
||||
/// let corr = Correspondence::new(point_idx, line_idx, point, candidate.distance, candidate.t);
|
||||
/// }
|
||||
/// ```
|
||||
#[inline]
|
||||
pub fn validate_correspondence_candidate(
|
||||
point: Point2D,
|
||||
line: &Line2D,
|
||||
max_distance: f32,
|
||||
max_projection_extension: f32,
|
||||
) -> Option<CorrespondenceCandidate> {
|
||||
// Compute projection parameter
|
||||
let t = line.project_point(point);
|
||||
|
||||
// Check projection bounds using Line2D helper
|
||||
if !line.contains_projection_extended(t, max_projection_extension) {
|
||||
return None;
|
||||
}
|
||||
|
||||
// Compute perpendicular distance
|
||||
let distance = line.distance_to_point(point);
|
||||
|
||||
// Check distance threshold
|
||||
if distance > max_distance {
|
||||
return None;
|
||||
}
|
||||
|
||||
Some(CorrespondenceCandidate { t, distance })
|
||||
}
|
||||
|
||||
/// Configuration for nearest neighbor matching.
|
||||
///
|
||||
/// # Geometric Concepts
|
||||
@@ -142,39 +209,43 @@ pub fn find_correspondences(
|
||||
}
|
||||
|
||||
let mut result = CorrespondenceSet::with_capacity(points.len());
|
||||
let min_t = -config.max_projection_extension;
|
||||
let max_t = 1.0 + config.max_projection_extension;
|
||||
|
||||
for (point_idx, &point) in points.iter().enumerate() {
|
||||
let mut best_distance = config.max_distance;
|
||||
let mut best_corr: Option<Correspondence> = None;
|
||||
|
||||
for (line_idx, line) in lines.iter().enumerate() {
|
||||
// Compute projection parameter
|
||||
let t = line.project_point(point);
|
||||
|
||||
// Check projection bounds
|
||||
if t < min_t || t > max_t {
|
||||
// Use validation helper to check projection and distance constraints
|
||||
let Some(candidate) = validate_correspondence_candidate(
|
||||
point,
|
||||
line,
|
||||
config.max_distance,
|
||||
config.max_projection_extension,
|
||||
) else {
|
||||
continue;
|
||||
}
|
||||
|
||||
// Compute perpendicular distance
|
||||
let distance = line.distance_to_point(point);
|
||||
|
||||
// Check distance threshold
|
||||
if distance > config.max_distance {
|
||||
continue;
|
||||
}
|
||||
};
|
||||
|
||||
if config.unique_per_point {
|
||||
// Keep only the best match
|
||||
if distance < best_distance {
|
||||
best_distance = distance;
|
||||
best_corr = Some(Correspondence::new(point_idx, line_idx, point, distance, t));
|
||||
if candidate.distance < best_distance {
|
||||
best_distance = candidate.distance;
|
||||
best_corr = Some(Correspondence::new(
|
||||
point_idx,
|
||||
line_idx,
|
||||
point,
|
||||
candidate.distance,
|
||||
candidate.t,
|
||||
));
|
||||
}
|
||||
} else {
|
||||
// Add all valid matches
|
||||
result.push(Correspondence::new(point_idx, line_idx, point, distance, t));
|
||||
result.push(Correspondence::new(
|
||||
point_idx,
|
||||
line_idx,
|
||||
point,
|
||||
candidate.distance,
|
||||
candidate.t,
|
||||
));
|
||||
}
|
||||
}
|
||||
|
||||
@@ -532,8 +603,6 @@ pub fn find_correspondences_spatial(
|
||||
}
|
||||
|
||||
let mut result = CorrespondenceSet::with_capacity(points.len());
|
||||
let min_t = -config.max_projection_extension;
|
||||
let max_t = 1.0 + config.max_projection_extension;
|
||||
|
||||
for (point_idx, &point) in points.iter().enumerate() {
|
||||
// Use spatial index to find candidate lines within distance threshold
|
||||
@@ -550,11 +619,9 @@ pub fn find_correspondences_spatial(
|
||||
for (line_idx, distance) in candidates {
|
||||
let line = &lines[line_idx];
|
||||
|
||||
// Compute projection parameter
|
||||
// Check projection bounds only (distance already validated by spatial index)
|
||||
let t = line.project_point(point);
|
||||
|
||||
// Check projection bounds
|
||||
if t < min_t || t > max_t {
|
||||
if !line.contains_projection_extended(t, config.max_projection_extension) {
|
||||
continue;
|
||||
}
|
||||
|
||||
@@ -601,8 +668,6 @@ pub fn find_correspondences_with_angle(
|
||||
}
|
||||
|
||||
let mut result = CorrespondenceSet::with_capacity(points.len());
|
||||
let min_t = -config.max_projection_extension;
|
||||
let max_t = 1.0 + config.max_projection_extension;
|
||||
let cos_threshold = max_angle_deviation.cos();
|
||||
|
||||
for (point_idx, &point) in points.iter().enumerate() {
|
||||
@@ -613,7 +678,7 @@ pub fn find_correspondences_with_angle(
|
||||
let mut best_corr: Option<Correspondence> = None;
|
||||
|
||||
for (line_idx, line) in lines.iter().enumerate() {
|
||||
// Check angle constraint
|
||||
// Check angle constraint first (cheap)
|
||||
let line_normal = line.normal();
|
||||
let dot = bearing.dot(line_normal).abs();
|
||||
|
||||
@@ -622,29 +687,35 @@ pub fn find_correspondences_with_angle(
|
||||
continue;
|
||||
}
|
||||
|
||||
// Compute projection parameter
|
||||
let t = line.project_point(point);
|
||||
|
||||
// Check projection bounds
|
||||
if t < min_t || t > max_t {
|
||||
// Use validation helper for projection and distance constraints
|
||||
let Some(candidate) = validate_correspondence_candidate(
|
||||
point,
|
||||
line,
|
||||
config.max_distance,
|
||||
config.max_projection_extension,
|
||||
) else {
|
||||
continue;
|
||||
}
|
||||
|
||||
// Compute perpendicular distance
|
||||
let distance = line.distance_to_point(point);
|
||||
|
||||
// Check distance threshold
|
||||
if distance > config.max_distance {
|
||||
continue;
|
||||
}
|
||||
};
|
||||
|
||||
if config.unique_per_point {
|
||||
if distance < best_distance {
|
||||
best_distance = distance;
|
||||
best_corr = Some(Correspondence::new(point_idx, line_idx, point, distance, t));
|
||||
if candidate.distance < best_distance {
|
||||
best_distance = candidate.distance;
|
||||
best_corr = Some(Correspondence::new(
|
||||
point_idx,
|
||||
line_idx,
|
||||
point,
|
||||
candidate.distance,
|
||||
candidate.t,
|
||||
));
|
||||
}
|
||||
} else {
|
||||
result.push(Correspondence::new(point_idx, line_idx, point, distance, t));
|
||||
result.push(Correspondence::new(
|
||||
point_idx,
|
||||
line_idx,
|
||||
point,
|
||||
candidate.distance,
|
||||
candidate.t,
|
||||
));
|
||||
}
|
||||
}
|
||||
|
||||
@@ -815,6 +886,60 @@ mod tests {
|
||||
Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(0.0, 2.0))
|
||||
}
|
||||
|
||||
// === Tests for validate_correspondence_candidate helper ===
|
||||
|
||||
#[test]
|
||||
fn test_validate_correspondence_candidate_valid() {
|
||||
let line = make_horizontal_line(); // (0,0) to (2,0)
|
||||
let point = Point2D::new(1.0, 0.1); // Above middle of line
|
||||
|
||||
let result = validate_correspondence_candidate(point, &line, 0.5, 0.2);
|
||||
|
||||
assert!(result.is_some());
|
||||
let candidate = result.unwrap();
|
||||
assert!((candidate.t - 0.5).abs() < 0.01); // Projects to middle
|
||||
assert!((candidate.distance - 0.1).abs() < 0.01); // 0.1m perpendicular
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_validate_correspondence_candidate_too_far() {
|
||||
let line = make_horizontal_line();
|
||||
let point = Point2D::new(1.0, 1.0); // 1m away from line
|
||||
|
||||
let result = validate_correspondence_candidate(point, &line, 0.5, 0.2);
|
||||
|
||||
assert!(result.is_none()); // Rejected: distance > 0.5
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_validate_correspondence_candidate_outside_projection() {
|
||||
let line = make_horizontal_line(); // (0,0) to (2,0)
|
||||
let point = Point2D::new(-1.0, 0.1); // Projects before line start
|
||||
|
||||
// Strict: no extension
|
||||
let result_strict = validate_correspondence_candidate(point, &line, 0.5, 0.0);
|
||||
assert!(result_strict.is_none());
|
||||
|
||||
// Relaxed: 0.5 extension (projects at t=-0.5, within [-0.5, 1.5])
|
||||
let result_relaxed = validate_correspondence_candidate(point, &line, 0.5, 0.6);
|
||||
assert!(result_relaxed.is_some());
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_validate_correspondence_candidate_at_endpoints() {
|
||||
let line = make_horizontal_line(); // (0,0) to (2,0)
|
||||
|
||||
// At start
|
||||
let at_start = validate_correspondence_candidate(Point2D::new(0.0, 0.1), &line, 0.5, 0.0);
|
||||
assert!(at_start.is_some());
|
||||
assert!((at_start.unwrap().t - 0.0).abs() < 0.01);
|
||||
|
||||
// At end
|
||||
let at_end = validate_correspondence_candidate(Point2D::new(2.0, 0.1), &line, 0.5, 0.0);
|
||||
assert!(at_end.is_some());
|
||||
assert!((at_end.unwrap().t - 1.0).abs() < 0.01);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_find_correspondences_single_point() {
|
||||
let lines = vec![make_horizontal_line()];
|
||||
|
||||
@@ -6,6 +6,82 @@ use crate::loop_closure::LoopClosureConfig;
|
||||
use crate::matching::IcpConfig;
|
||||
use crate::query::{FrontierConfig, OccupancyConfig};
|
||||
|
||||
// ============================================================================
|
||||
// Configuration Validation
|
||||
// ============================================================================
|
||||
|
||||
/// Errors that can occur during configuration validation.
|
||||
#[derive(Debug, Clone, PartialEq)]
|
||||
pub enum ConfigError {
|
||||
/// A threshold value is invalid (negative or zero when positive required).
|
||||
InvalidThreshold {
|
||||
/// Name of the configuration field.
|
||||
field: &'static str,
|
||||
/// The invalid value.
|
||||
value: f32,
|
||||
/// Description of the constraint violated.
|
||||
constraint: &'static str,
|
||||
},
|
||||
/// An iteration count is invalid (zero or too high).
|
||||
InvalidIterations {
|
||||
/// Name of the configuration field.
|
||||
field: &'static str,
|
||||
/// The invalid value.
|
||||
value: usize,
|
||||
/// Description of the constraint violated.
|
||||
constraint: &'static str,
|
||||
},
|
||||
/// A probability value is out of range [0, 1].
|
||||
InvalidProbability {
|
||||
/// Name of the configuration field.
|
||||
field: &'static str,
|
||||
/// The invalid value.
|
||||
value: f32,
|
||||
},
|
||||
/// Configuration values are inconsistent with each other.
|
||||
InconsistentConfig {
|
||||
/// Description of the inconsistency.
|
||||
message: String,
|
||||
},
|
||||
}
|
||||
|
||||
impl std::fmt::Display for ConfigError {
|
||||
fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result {
|
||||
match self {
|
||||
ConfigError::InvalidThreshold {
|
||||
field,
|
||||
value,
|
||||
constraint,
|
||||
} => {
|
||||
write!(f, "invalid threshold {}: {} ({})", field, value, constraint)
|
||||
}
|
||||
ConfigError::InvalidIterations {
|
||||
field,
|
||||
value,
|
||||
constraint,
|
||||
} => {
|
||||
write!(
|
||||
f,
|
||||
"invalid iteration count {}: {} ({})",
|
||||
field, value, constraint
|
||||
)
|
||||
}
|
||||
ConfigError::InvalidProbability { field, value } => {
|
||||
write!(
|
||||
f,
|
||||
"invalid probability {}: {} (must be in [0, 1])",
|
||||
field, value
|
||||
)
|
||||
}
|
||||
ConfigError::InconsistentConfig { message } => {
|
||||
write!(f, "inconsistent configuration: {}", message)
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
impl std::error::Error for ConfigError {}
|
||||
|
||||
/// Configuration for VectorMap.
|
||||
#[derive(Clone, Debug)]
|
||||
pub struct VectorMapConfig {
|
||||
@@ -223,4 +299,159 @@ impl VectorMapConfig {
|
||||
self.matching.multi_resolution.enabled = enabled;
|
||||
self
|
||||
}
|
||||
|
||||
// ===== Configuration Validation =====
|
||||
|
||||
/// Validate the configuration parameters.
|
||||
///
|
||||
/// Checks that all parameters are within valid ranges and consistent with each other.
|
||||
/// Call this after building a configuration to catch errors early.
|
||||
///
|
||||
/// # Returns
|
||||
/// `Ok(())` if configuration is valid, `Err(ConfigError)` otherwise.
|
||||
///
|
||||
/// # Example
|
||||
/// ```rust
|
||||
/// use vastu_map::VectorMapConfig;
|
||||
///
|
||||
/// let config = VectorMapConfig::default();
|
||||
/// assert!(config.validate().is_ok());
|
||||
///
|
||||
/// // Invalid config with zero iterations would fail
|
||||
/// let mut bad_config = VectorMapConfig::default();
|
||||
/// bad_config.matching.max_iterations = 0;
|
||||
/// assert!(bad_config.validate().is_err());
|
||||
/// ```
|
||||
pub fn validate(&self) -> Result<(), ConfigError> {
|
||||
// === ICP Configuration ===
|
||||
|
||||
if self.matching.max_iterations == 0 {
|
||||
return Err(ConfigError::InvalidIterations {
|
||||
field: "matching.max_iterations",
|
||||
value: 0,
|
||||
constraint: "must be at least 1",
|
||||
});
|
||||
}
|
||||
|
||||
if self.matching.max_correspondence_distance <= 0.0 {
|
||||
return Err(ConfigError::InvalidThreshold {
|
||||
field: "matching.max_correspondence_distance",
|
||||
value: self.matching.max_correspondence_distance,
|
||||
constraint: "must be positive",
|
||||
});
|
||||
}
|
||||
|
||||
if self.matching.convergence_threshold <= 0.0 {
|
||||
return Err(ConfigError::InvalidThreshold {
|
||||
field: "matching.convergence_threshold",
|
||||
value: self.matching.convergence_threshold,
|
||||
constraint: "must be positive",
|
||||
});
|
||||
}
|
||||
|
||||
if self.matching.min_correspondences == 0 {
|
||||
return Err(ConfigError::InvalidIterations {
|
||||
field: "matching.min_correspondences",
|
||||
value: 0,
|
||||
constraint: "must be at least 1",
|
||||
});
|
||||
}
|
||||
|
||||
// === RANSAC Configuration ===
|
||||
|
||||
if self.ransac_extraction.iterations == 0 {
|
||||
return Err(ConfigError::InvalidIterations {
|
||||
field: "ransac_extraction.iterations",
|
||||
value: 0,
|
||||
constraint: "must be at least 1",
|
||||
});
|
||||
}
|
||||
|
||||
if self.ransac_extraction.inlier_threshold <= 0.0 {
|
||||
return Err(ConfigError::InvalidThreshold {
|
||||
field: "ransac_extraction.inlier_threshold",
|
||||
value: self.ransac_extraction.inlier_threshold,
|
||||
constraint: "must be positive",
|
||||
});
|
||||
}
|
||||
|
||||
// === Association Configuration ===
|
||||
|
||||
if self.association.max_perpendicular_distance <= 0.0 {
|
||||
return Err(ConfigError::InvalidThreshold {
|
||||
field: "association.max_perpendicular_distance",
|
||||
value: self.association.max_perpendicular_distance,
|
||||
constraint: "must be positive",
|
||||
});
|
||||
}
|
||||
|
||||
if self.association.max_angle_difference <= 0.0
|
||||
|| self.association.max_angle_difference > std::f32::consts::PI
|
||||
{
|
||||
return Err(ConfigError::InvalidThreshold {
|
||||
field: "association.max_angle_difference",
|
||||
value: self.association.max_angle_difference,
|
||||
constraint: "must be in (0, π]",
|
||||
});
|
||||
}
|
||||
|
||||
// === Confidence Threshold ===
|
||||
|
||||
if self.min_match_confidence < 0.0 || self.min_match_confidence > 1.0 {
|
||||
return Err(ConfigError::InvalidProbability {
|
||||
field: "min_match_confidence",
|
||||
value: self.min_match_confidence,
|
||||
});
|
||||
}
|
||||
|
||||
// === Split-Merge Configuration ===
|
||||
|
||||
if self.extraction.min_length <= 0.0 {
|
||||
return Err(ConfigError::InvalidThreshold {
|
||||
field: "extraction.min_length",
|
||||
value: self.extraction.min_length,
|
||||
constraint: "must be positive",
|
||||
});
|
||||
}
|
||||
|
||||
if self.extraction.split_threshold <= 0.0 {
|
||||
return Err(ConfigError::InvalidThreshold {
|
||||
field: "extraction.split_threshold",
|
||||
value: self.extraction.split_threshold,
|
||||
constraint: "must be positive",
|
||||
});
|
||||
}
|
||||
|
||||
// === Corner Detection ===
|
||||
|
||||
if self.corner.min_angle <= 0.0 || self.corner.min_angle >= std::f32::consts::PI {
|
||||
return Err(ConfigError::InvalidThreshold {
|
||||
field: "corner.min_angle",
|
||||
value: self.corner.min_angle,
|
||||
constraint: "must be in (0, π)",
|
||||
});
|
||||
}
|
||||
|
||||
if self.corner.max_angle <= self.corner.min_angle
|
||||
|| self.corner.max_angle > std::f32::consts::PI
|
||||
{
|
||||
return Err(ConfigError::InconsistentConfig {
|
||||
message: format!(
|
||||
"corner.max_angle ({}) must be > corner.min_angle ({}) and <= π",
|
||||
self.corner.max_angle, self.corner.min_angle
|
||||
),
|
||||
});
|
||||
}
|
||||
|
||||
Ok(())
|
||||
}
|
||||
|
||||
/// Validate and return self for builder pattern.
|
||||
///
|
||||
/// # Panics
|
||||
/// Panics if validation fails. Use `validate()` for error handling.
|
||||
pub fn validated(self) -> Self {
|
||||
self.validate().expect("VectorMapConfig validation failed");
|
||||
self
|
||||
}
|
||||
}
|
||||
|
||||
@@ -32,6 +32,6 @@ mod line_store;
|
||||
mod map;
|
||||
|
||||
// Re-export public API
|
||||
pub use config::VectorMapConfig;
|
||||
pub use config::{ConfigError, VectorMapConfig};
|
||||
pub use line_store::LineStore;
|
||||
pub use map::VectorMap;
|
||||
|
||||
Reference in New Issue
Block a user