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

Add YAML configuration file support for VectorMap

Browse Source 
Enable loading algorithm parameters from YAML files for reproducible
experiments and easier parameter tuning without recompilation.

- Add yaml.rs module with from_yaml_file/to_yaml_file methods
- Add Serialize/Deserialize derives to all config structs
- Add finalize() method to recompute derived fields after deserialization
- Add configs/default.yaml reference configuration with documentation
- Update test harness to load config from VASTU_MAP_CONFIG env var
- Move serde/serde_yaml to main dependencies
This commit is contained in:
codetiger
2025-12-22 17:27:24 +05:30
parent 6f9df02675
commit 68242ca310
25 changed files with 920 additions and 214 deletions
Download Patch File Download Diff File Expand all files Collapse all files
vastu-map/Cargo.toml
+3 -3
View File
@@ -7,24 +7,24 @@ license = "MIT"
[features]
default = []
integration-tests = ["dep:sangam-io", "dep:svg", "dep:image", "dep:serde_yaml"]
integration-tests = ["dep:sangam-io", "dep:svg", "dep:image"]
[dependencies]
rstar = "0.12" # R-tree spatial index
log = "0.4" # Logging
rand = "0.9" # RANSAC randomness
serde = { version = "1.0", features = ["derive"] } # Config serialization
serde_yaml = "0.9" # YAML config files
# Integration test dependencies (optional)
sangam-io = { path = "../sangam-io", features = ["mock"], optional = true }
svg = { version = "0.18", optional = true }
image = { version = "0.25", default-features = false, features = ["png", "pnm"], optional = true }
serde_yaml = { version = "0.9", optional = true }
[dev-dependencies]
criterion = "0.8" # Benchmarks
env_logger = "0.11" # Test logging
approx = "0.5" # Approximate comparisons (tests only)
serde = { version = "1.0", features = ["derive"] } # Test config parsing
[[test]]
name = "integration"
vastu-map/configs/default.yaml
+225
View File
@@ -0,0 +1,225 @@
# VastuMap YAML Configuration
# All values must be specified (no preset inheritance)
# Units: meters (m), radians (rad), counts (n)
# ============================================================================
# Line Extraction (Split-Merge Algorithm)
# ============================================================================
extraction:
# Maximum perpendicular distance for splitting (m)
split_threshold: 0.05
# Minimum points to form a valid line
min_points: 5
# Minimum line length to keep (m)
min_length: 0.10
# Maximum gap between consecutive points (m)
max_point_gap: 0.30
# Merge threshold for collinear segments (m)
merge_threshold: 0.05
# Coefficient for range-adaptive threshold scaling
adaptive_range_scale: 0.03
# ============================================================================
# RANSAC Line Extraction (for hybrid mode)
# ============================================================================
ransac_extraction:
# Number of RANSAC iterations per line
iterations: 100
# Inlier distance threshold (m)
inlier_threshold: 0.02
# Minimum inliers for valid line
min_inliers: 5
# Minimum line length (m)
min_line_length: 0.1
# Maximum gap between inliers (m) - prevents connecting walls across doorways
max_point_gap: 0.10
# Maximum lines to extract (0 = unlimited)
max_lines: 0
# Random seed (0 = random)
seed: 0
# Whether to use RANSAC + Split-Merge hybrid extraction
use_hybrid_extraction: true
# ============================================================================
# Corner Detection
# ============================================================================
corner:
# Minimum angle between lines to form corner (rad)
min_angle: 0.524 # 30 degrees
# Maximum angle between lines to form corner (rad)
max_angle: 2.618 # 150 degrees
# Maximum endpoint distance for corner detection (m)
max_endpoint_distance: 0.05
# Curvature detection window size (points on each side)
curvature_window_size: 3
# Minimum curvature angle for corner candidate (rad)
curvature_threshold: 0.785 # 45 degrees
# Merge distance for deduplicating corners (m)
merge_distance: 0.10
# Non-maximum suppression radius (m)
nms_radius: 0.15
# ============================================================================
# ICP Scan Matching
# ============================================================================
matching:
# Maximum ICP iterations
max_iterations: 30
# Convergence threshold for pose change
convergence_threshold: 0.0001
# Maximum correspondence distance (m)
max_correspondence_distance: 0.5
# Robust cost function for outlier handling
# Options: None, Huber, Cauchy, Tukey, GemanMcClure
robust_cost:
type: Huber
delta: 0.03
# Use Iteratively Reweighted Least Squares
use_irls: true
# Minimum correspondences required
min_correspondences: 10
# Minimum match confidence to accept
min_confidence: 0.3
# Use RANSAC for initial pose estimate
use_ransac_init: true
# Use batch (SIMD) correspondence search
use_batch_search: true
# Lidar noise model
noise_model:
sigma_base: 0.01
sigma_range: 0.001
# Coarse search when confidence is low
use_coarse_search: true
coarse_search_confidence_threshold: 0.5
# Multi-resolution ICP (coarse-to-fine)
multi_resolution:
enabled: true
levels: 3
subsample_factor: 2
iterations_per_level: [3, 5, 10]
# ============================================================================
# Line Association (Map Integration)
# ============================================================================
association:
# Maximum perpendicular distance for association (m)
max_perpendicular_distance: 0.15
# Maximum angular difference (rad)
max_angle_difference: 0.2
# Minimum overlap ratio for valid association
min_overlap_ratio: 0.3
# Maximum gap between line endpoints (m)
max_endpoint_gap: 0.3
# Weight for distance in quality scoring
distance_weight: 10.0
# Weight for angle in quality scoring
angle_weight: 5.0
# ============================================================================
# Line Merging
# ============================================================================
merger:
# Weight given to existing map line (0-1)
map_weight: 0.8
# Whether to extend line endpoints
extend_endpoints: true
# Maximum extension per observation (m)
max_extension: 0.5
# Minimum extension to apply (m)
min_extension: 0.05
# ============================================================================
# Frontier Detection
# ============================================================================
frontier:
# Maximum endpoint distance for "connected" (m)
connection_distance: 0.3
# Minimum distance from robot for valid frontier (m)
min_distance_from_robot: 0.5
# Minimum openness score for valid frontier
min_openness: 0.3
# ============================================================================
# Occupancy Queries
# ============================================================================
occupancy:
# Distance threshold for "occupied" (m)
obstacle_distance: 0.1
# Distance threshold for "known" (m)
known_distance: 2.0
# Use winding number for inside/outside determination
use_winding_number: false
# ============================================================================
# Path Planning
# ============================================================================
path_planning:
# Robot radius for collision checking (m)
robot_radius: 0.15
# Maximum visibility graph nodes
max_nodes: 1000
# Only plan through known free space
conservative: true
# ============================================================================
# Loop Closure Detection
# ============================================================================
loop_closure:
# Minimum travel distance before checking loop closure (m)
min_travel_distance: 5.0
# Maximum descriptor distance for candidates
max_descriptor_distance: 0.5
# Minimum ICP confidence to accept closure
min_verification_confidence: 0.6
# Distance between keyframes (m)
keyframe_interval: 2.0
# Radius for computing descriptors (m)
descriptor_radius: 2.0
# Maximum candidates to verify per keyframe
max_candidates_to_verify: 3
# Minimum lines for valid keyframe
min_lines_for_keyframe: 3
# Minimum corners for valid keyframe
min_corners_for_keyframe: 2
# ICP config for loop closure verification (nested)
icp_config:
max_iterations: 30
convergence_threshold: 0.0001
max_correspondence_distance: 0.5
robust_cost:
type: Huber
delta: 0.03
use_irls: true
min_correspondences: 10
min_confidence: 0.3
use_ransac_init: true
use_batch_search: true
noise_model:
sigma_base: 0.01
sigma_range: 0.001
use_coarse_search: true
coarse_search_confidence_threshold: 0.5
multi_resolution:
enabled: true
levels: 3
subsample_factor: 2
iterations_per_level: [3, 5, 10]
# ============================================================================
# Global Settings
# ============================================================================
# Minimum match confidence to use scan matching (vs. odometry)
min_match_confidence: 0.3
# Whether to update map with new observations
mapping_enabled: true
# Whether loop closure detection is enabled
loop_closure_enabled: true
vastu-map/src/config.rs
+4 -3
View File
@@ -2,6 +2,7 @@
//!
//! All configuration parameters have sensible defaults for indoor robot navigation.
use serde::{Deserialize, Serialize};
use std::f32::consts::PI;
/// Lidar measurement noise model.
@@ -11,7 +12,7 @@ use std::f32::consts::PI;
///
/// This allows weighting correspondences by their measurement quality.
/// Close-range measurements are more reliable than far-range ones.
#[derive(Clone, Copy, Debug)]
#[derive(Clone, Copy, Debug, Serialize, Deserialize)]
pub struct LidarNoiseModel {
/// Base measurement standard deviation (meters).
/// Minimum uncertainty even at zero range.
@@ -135,7 +136,7 @@ impl LidarNoiseModel {
///
/// map.enable_exploration_mode(config);
/// ```
#[derive(Clone, Debug)]
#[derive(Clone, Debug, Serialize, Deserialize)]
pub struct ExplorationConfig {
/// Re-fit lines every N observations.
/// Default: 10
@@ -215,7 +216,7 @@ impl ExplorationConfig {
///
/// All parameters have sensible defaults. Create with `Default::default()`
/// and modify as needed.
#[derive(Clone, Debug)]
#[derive(Clone, Debug, Serialize, Deserialize)]
pub struct VectorMapConfig {
// ─────────────────────────────────────────────────────────────────────────
// Input Filtering
vastu-map/src/extraction/corner_detection.rs
+3 -1
View File
@@ -23,12 +23,14 @@
//! let corners = detect_hybrid_corners(&points, &lines, &corner_config);
//! ```
use serde::{Deserialize, Serialize};
use crate::core::Point2D;
use crate::features::{Corner2D, Line2D};
use std::f32::consts::PI;
/// Configuration for corner detection.
#[derive(Clone, Debug)]
#[derive(Clone, Debug, Serialize, Deserialize)]
pub struct CornerConfig {
/// Minimum angle between lines to form a corner (radians).
/// Lines that are nearly parallel (angle below this) don't form corners.
vastu-map/src/extraction/line_fitting.rs
+26 -59
View File
@@ -61,7 +61,7 @@ use crate::features::Line2D;
///
/// // Lines longer than 1m will be split into smaller segments
/// ```
#[derive(Debug, Clone)]
#[derive(Debug, Clone, Default)]
pub struct LineFitConfig {
/// Maximum length for a single line segment.
///
@@ -73,14 +73,6 @@ pub struct LineFitConfig {
pub max_line_length: Option<f32>,
}
impl Default for LineFitConfig {
fn default() -> Self {
Self {
max_line_length: None,
}
}
}
impl LineFitConfig {
/// Create a new configuration with default settings.
pub fn new() -> Self {
@@ -126,7 +118,7 @@ pub fn split_line_by_length(line: &Line2D, max_length: f32) -> Vec<Line2D> {
let length = line.length();
if length <= max_length || max_length <= 0.0 {
return vec![line.clone()];
return vec![*line];
}
// Calculate number of segments needed
@@ -157,16 +149,14 @@ pub fn split_line_by_length(line: &Line2D, max_length: f32) -> Vec<Line2D> {
// Use exact endpoint for last segment to avoid floating point drift
line.end
} else {
Point2D::new(
line.start.x + dir.x * t_end,
line.start.y + dir.y * t_end,
)
Point2D::new(line.start.x + dir.x * t_end, line.start.y + dir.y * t_end)
};
// Each segment inherits observation_count=1, proportional point_count
let segment_point_count = if i == num_segments - 1 {
// Last segment gets remaining points
line.point_count.saturating_sub(points_per_segment * (num_segments - 1) as u32)
line.point_count
.saturating_sub(points_per_segment * (num_segments - 1) as u32)
} else {
points_per_segment
};
@@ -1180,11 +1170,7 @@ mod tests {
#[test]
fn test_split_line_short_line_no_split() {
let line = Line2D::with_point_count(
Point2D::new(0.0, 0.0),
Point2D::new(0.5, 0.0),
10,
);
let line = Line2D::with_point_count(Point2D::new(0.0, 0.0), Point2D::new(0.5, 0.0), 10);
let segments = split_line_by_length(&line, 1.0);
@@ -1196,11 +1182,7 @@ mod tests {
#[test]
fn test_split_line_exact_max_length() {
let line = Line2D::with_point_count(
Point2D::new(0.0, 0.0),
Point2D::new(1.0, 0.0),
10,
);
let line = Line2D::with_point_count(Point2D::new(0.0, 0.0), Point2D::new(1.0, 0.0), 10);
let segments = split_line_by_length(&line, 1.0);
@@ -1209,11 +1191,7 @@ mod tests {
#[test]
fn test_split_line_into_two() {
let line = Line2D::with_point_count(
Point2D::new(0.0, 0.0),
Point2D::new(2.0, 0.0),
20,
);
let line = Line2D::with_point_count(Point2D::new(0.0, 0.0), Point2D::new(2.0, 0.0), 20);
let segments = split_line_by_length(&line, 1.0);
@@ -1232,11 +1210,7 @@ mod tests {
#[test]
fn test_split_line_into_multiple() {
let line = Line2D::with_point_count(
Point2D::new(0.0, 0.0),
Point2D::new(5.0, 0.0),
50,
);
let line = Line2D::with_point_count(Point2D::new(0.0, 0.0), Point2D::new(5.0, 0.0), 50);
let segments = split_line_by_length(&line, 1.0);
@@ -1254,11 +1228,7 @@ mod tests {
#[test]
fn test_split_line_diagonal() {
// Diagonal line of length 5*sqrt(2) ≈ 7.07
let line = Line2D::with_point_count(
Point2D::new(0.0, 0.0),
Point2D::new(5.0, 5.0),
30,
);
let line = Line2D::with_point_count(Point2D::new(0.0, 0.0), Point2D::new(5.0, 5.0), 30);
let segments = split_line_by_length(&line, 2.0);
@@ -1279,11 +1249,7 @@ mod tests {
#[test]
fn test_split_line_zero_max_length() {
let line = Line2D::with_point_count(
Point2D::new(0.0, 0.0),
Point2D::new(2.0, 0.0),
10,
);
let line = Line2D::with_point_count(Point2D::new(0.0, 0.0), Point2D::new(2.0, 0.0), 10);
// Zero or negative max_length should return original line
let segments = split_line_by_length(&line, 0.0);
@@ -1318,17 +1284,14 @@ mod tests {
#[test]
fn test_line_fit_config_builder() {
let config = LineFitConfig::new()
.with_max_line_length(1.5);
let config = LineFitConfig::new().with_max_line_length(1.5);
assert_eq!(config.max_line_length, Some(1.5));
}
#[test]
fn test_fit_line_with_config_no_split() {
let points: Vec<Point2D> = (0..10)
.map(|i| Point2D::new(i as f32 * 0.1, 0.0))
.collect();
let points: Vec<Point2D> = (0..10).map(|i| Point2D::new(i as f32 * 0.1, 0.0)).collect();
// Config with no max length
let config = LineFitConfig::new();
@@ -1340,9 +1303,7 @@ mod tests {
#[test]
fn test_fit_line_with_config_with_split() {
// Create a 5m horizontal line
let points: Vec<Point2D> = (0..51)
.map(|i| Point2D::new(i as f32 * 0.1, 0.0))
.collect();
let points: Vec<Point2D> = (0..51).map(|i| Point2D::new(i as f32 * 0.1, 0.0)).collect();
// Config with 1m max length
let config = LineFitConfig::new().with_max_line_length(1.0);
@@ -1353,7 +1314,11 @@ mod tests {
// Each segment should be approximately 1m
for line in &lines {
assert!(line.length() <= 1.01, "Line length {} > 1.01", line.length());
assert!(
line.length() <= 1.01,
"Line length {} > 1.01",
line.length()
);
}
}
@@ -1371,14 +1336,18 @@ mod tests {
fn test_fit_line_from_sensor_with_config() {
let sensor_pos = Point2D::new(0.0, 0.0);
let points: Vec<Point2D> = (10..40)
.map(|i| Point2D::new(i as f32 * 0.1, 1.0)) // 3m wall at y=1
.map(|i| Point2D::new(i as f32 * 0.1, 1.0)) // 3m wall at y=1
.collect();
let config = LineFitConfig::new().with_max_line_length(0.5);
let lines = fit_line_from_sensor_with_config(&points, sensor_pos, None, &config);
// 3m line should split into 6 segments of ~0.5m
assert!(lines.len() >= 5, "Expected >= 5 segments, got {}", lines.len());
assert!(
lines.len() >= 5,
"Expected >= 5 segments, got {}",
lines.len()
);
// All segments should be approximately horizontal
for line in &lines {
@@ -1392,9 +1361,7 @@ mod tests {
#[test]
fn test_fit_line_weighted_with_config() {
let points: Vec<Point2D> = (0..31)
.map(|i| Point2D::new(i as f32 * 0.1, 0.0))
.collect();
let points: Vec<Point2D> = (0..31).map(|i| Point2D::new(i as f32 * 0.1, 0.0)).collect();
let weights: Vec<f32> = vec![1.0; 31];
let config = LineFitConfig::new().with_max_line_length(1.0);
vastu-map/src/extraction/ransac_lines.rs
+3 -1
View File
@@ -19,6 +19,8 @@
//! let (lines2, remaining2) = extract_lines_ransac_with_scratch(&points2, &config, &mut scratch);
//! ```
use serde::{Deserialize, Serialize};
use crate::core::Point2D;
use crate::features::Line2D;
@@ -148,7 +150,7 @@ impl Default for RansacScratchSpace {
}
/// Configuration for RANSAC line extraction.
#[derive(Clone, Debug)]
#[derive(Clone, Debug, Serialize, Deserialize)]
pub struct RansacLineConfig {
/// Number of RANSAC iterations per line.
/// More iterations = better chance of finding optimal line.
vastu-map/src/extraction/split_merge/config.rs
+3 -1
View File
@@ -1,7 +1,9 @@
//! Configuration for Split-and-Merge line extraction.
use serde::{Deserialize, Serialize};
/// Configuration for Split-and-Merge algorithm.
#[derive(Clone, Debug)]
#[derive(Clone, Debug, Serialize, Deserialize)]
pub struct SplitMergeConfig {
/// Maximum perpendicular distance for a point to be considered on the line.
/// If any point deviates more than this, the segment is split.
vastu-map/src/features/line.rs
+1 -6
View File
@@ -72,12 +72,7 @@ impl Line2D {
/// Create a line segment with all fields specified.
#[inline]
pub fn full(
start: Point2D,
end: Point2D,
observation_count: u32,
point_count: u32,
) -> Self {
pub fn full(start: Point2D, end: Point2D, observation_count: u32, point_count: u32) -> Self {
Self {
start,
end,
vastu-map/src/integration/association.rs
+3 -1
View File
@@ -5,13 +5,15 @@
//! - Angular similarity
//! - Overlap extent
use serde::{Deserialize, Serialize};
use crate::core::math::angle_diff;
use crate::features::Line2D;
use super::spatial_index::SpatialIndex;
/// Configuration for line association.
#[derive(Clone, Debug)]
#[derive(Clone, Debug, Serialize, Deserialize)]
pub struct AssociationConfig {
/// Maximum perpendicular distance for association (meters).
/// Default: 0.15m
vastu-map/src/integration/merger.rs
+15 -42
View File
@@ -3,13 +3,15 @@
//! Merges associated lines into updated map lines, extending
//! endpoints and updating parameters based on observations.
use serde::{Deserialize, Serialize};
use crate::core::Point2D;
use crate::features::Line2D;
use super::association::Association;
/// Configuration for line merging.
#[derive(Clone, Debug)]
#[derive(Clone, Debug, Serialize, Deserialize)]
pub struct MergerConfig {
/// Weight given to existing map line (vs. scan line).
/// Higher values make the map more stable but slower to adapt.
@@ -70,7 +72,7 @@ impl MergerConfig {
///
/// Used by [`optimize_coplanar_lines`] to merge multiple lines
/// that lie on the same infinite line (within tolerances).
#[derive(Clone, Debug)]
#[derive(Clone, Debug, Serialize, Deserialize)]
pub struct CoplanarMergeConfig {
/// Maximum angle difference for lines to be considered coplanar (radians).
/// Default: 0.052 rad (~3°)
@@ -96,9 +98,9 @@ pub struct CoplanarMergeConfig {
impl Default for CoplanarMergeConfig {
fn default() -> Self {
Self {
max_angle_diff: 0.052, // ~3 degrees
max_perpendicular_dist: 0.05, // 5cm
max_gap: 0.3, // 30cm
max_angle_diff: 0.052, // ~3 degrees
max_perpendicular_dist: 0.05, // 5cm
max_gap: 0.3, // 30cm
min_dominant_point_count: 10,
}
}
@@ -576,8 +578,7 @@ pub fn optimize_coplanar_lines(lines: &mut Vec<Line2D>, config: &CoplanarMergeCo
}
// Collect groups
let mut groups: std::collections::HashMap<usize, Vec<usize>> =
std::collections::HashMap::new();
let mut groups: std::collections::HashMap<usize, Vec<usize>> = std::collections::HashMap::new();
for i in 0..n {
let root = find(&mut parent, i);
groups.entry(root).or_default().push(i);
@@ -862,11 +863,7 @@ mod tests {
Point2D::new(3.0, 0.0),
15, // Above threshold
),
Line2D::with_point_count(
Point2D::new(2.0, 0.0),
Point2D::new(5.0, 0.0),
12,
),
Line2D::with_point_count(Point2D::new(2.0, 0.0), Point2D::new(5.0, 0.0), 12),
];
let config = CoplanarMergeConfig::default();
@@ -881,11 +878,7 @@ mod tests {
#[test]
fn test_optimize_coplanar_lines_merges_with_gap() {
let mut lines = vec![
Line2D::with_point_count(
Point2D::new(0.0, 0.0),
Point2D::new(2.0, 0.0),
12,
),
Line2D::with_point_count(Point2D::new(0.0, 0.0), Point2D::new(2.0, 0.0), 12),
Line2D::with_point_count(
Point2D::new(2.2, 0.0), // 0.2m gap, within default 0.3m
Point2D::new(5.0, 0.0),
@@ -930,11 +923,7 @@ mod tests {
#[test]
fn test_optimize_coplanar_lines_respects_max_gap() {
let mut lines = vec![
Line2D::with_point_count(
Point2D::new(0.0, 0.0),
Point2D::new(2.0, 0.0),
15,
),
Line2D::with_point_count(Point2D::new(0.0, 0.0), Point2D::new(2.0, 0.0), 15),
Line2D::with_point_count(
Point2D::new(4.0, 0.0), // 2.0m gap, above default 0.3m
Point2D::new(6.0, 0.0),
@@ -977,27 +966,11 @@ mod tests {
// Two separate walls (horizontal and vertical)
let mut lines = vec![
// Horizontal wall (will merge)
Line2D::with_point_count(
Point2D::new(0.0, 0.0),
Point2D::new(2.0, 0.0),
15,
),
Line2D::with_point_count(
Point2D::new(2.0, 0.0),
Point2D::new(4.0, 0.0),
12,
),
Line2D::with_point_count(Point2D::new(0.0, 0.0), Point2D::new(2.0, 0.0), 15),
Line2D::with_point_count(Point2D::new(2.0, 0.0), Point2D::new(4.0, 0.0), 12),
// Vertical wall (will merge)
Line2D::with_point_count(
Point2D::new(5.0, 0.0),
Point2D::new(5.0, 2.0),
15,
),
Line2D::with_point_count(
Point2D::new(5.0, 2.0),
Point2D::new(5.0, 4.0),
12,
),
Line2D::with_point_count(Point2D::new(5.0, 0.0), Point2D::new(5.0, 2.0), 15),
Line2D::with_point_count(Point2D::new(5.0, 2.0), Point2D::new(5.0, 4.0), 12),
];
let config = CoplanarMergeConfig::default();
vastu-map/src/integration/point_association.rs
+35 -41
View File
@@ -171,20 +171,21 @@ pub fn associate_scan_points(
let mut total_unassociated = 0usize;
for &world_point in points {
if let Some((line_idx, distance)) = index.nearest_line(world_point) {
if distance <= config.max_distance && line_idx < num_lines {
let line = &lines[line_idx];
let t = line.project_point(world_point);
if line.contains_projection_extended(t, config.max_projection_extension) {
line_points[line_idx].push(AssociatedPoint {
point: world_point,
scan_id,
distance,
projection_t: t,
});
total_associated += 1;
continue;
}
if let Some((line_idx, distance)) = index.nearest_line(world_point)
&& distance <= config.max_distance
&& line_idx < num_lines
{
let line = &lines[line_idx];
let t = line.project_point(world_point);
if line.contains_projection_extended(t, config.max_projection_extension) {
line_points[line_idx].push(AssociatedPoint {
point: world_point,
scan_id,
distance,
projection_t: t,
});
total_associated += 1;
continue;
}
}
total_unassociated += 1;
@@ -213,10 +214,7 @@ pub fn lines_with_enough_points(result: &PointAssociationResult, min_points: usi
/// Extract just the points for a specific line (without metadata).
///
/// Useful when only the point coordinates are needed for fitting.
pub fn extract_points_for_line(
result: &PointAssociationResult,
line_idx: usize,
) -> Vec<Point2D> {
pub fn extract_points_for_line(result: &PointAssociationResult, line_idx: usize) -> Vec<Point2D> {
result
.line_points
.get(line_idx)
@@ -291,9 +289,9 @@ mod tests {
// Points close to line 0 (y=0)
let store = make_scan_store_with_points(&[
(2.0, 0.05), // Very close to y=0
(5.0, 0.02), // Very close to y=0
(8.0, 4.98), // Very close to y=5
(2.0, 0.05), // Very close to y=0
(5.0, 0.02), // Very close to y=0
(8.0, 4.98), // Very close to y=5
]);
let result = associate_points_to_lines(&store, &lines, &index, &config);
@@ -311,8 +309,8 @@ mod tests {
// Points too far from line
let store = make_scan_store_with_points(&[
(5.0, 0.5), // 0.5m away, too far
(5.0, 1.0), // 1.0m away, too far
(5.0, 0.5), // 0.5m away, too far
(5.0, 1.0), // 1.0m away, too far
]);
let result = associate_points_to_lines(&store, &lines, &index, &config);
@@ -332,9 +330,9 @@ mod tests {
// Points beyond line endpoints
let store = make_scan_store_with_points(&[
(-2.0, 0.0), // Before start
(12.0, 0.0), // After end
(5.0, 0.0), // On segment (should associate)
(-2.0, 0.0), // Before start
(12.0, 0.0), // After end
(5.0, 0.0), // On segment (should associate)
]);
let result = associate_points_to_lines(&store, &lines, &index, &config);
@@ -354,9 +352,9 @@ mod tests {
.with_max_projection_extension(0.1);
let store = make_scan_store_with_points(&[
(-0.5, 0.0), // 5% before start, within extension
(10.5, 0.0), // 5% after end, within extension
(-2.0, 0.0), // 20% before start, outside extension
(-0.5, 0.0), // 5% before start, within extension
(10.5, 0.0), // 5% after end, within extension
(-2.0, 0.0), // 20% before start, outside extension
]);
let result = associate_points_to_lines(&store, &lines, &index, &config);
@@ -459,9 +457,9 @@ mod tests {
let config = PointAssociationConfig::default().with_max_distance(0.5);
let points = vec![
Point2D::new(5.0, 0.05), // Near line 0
Point2D::new(9.95, 5.0), // Near line 1
Point2D::new(5.0, 5.0), // Far from both
Point2D::new(5.0, 0.05), // Near line 0
Point2D::new(9.95, 5.0), // Near line 1
Point2D::new(5.0, 5.0), // Far from both
];
let result = associate_scan_points(&points, 42, &lines, &index, &config);
@@ -512,11 +510,7 @@ mod tests {
let index = SpatialIndex::new(&lines);
let config = PointAssociationConfig::default().with_max_distance(0.5);
let store = make_scan_store_with_points(&[
(2.0, 0.05),
(5.0, 0.05),
(8.0, 0.05),
]);
let store = make_scan_store_with_points(&[(2.0, 0.05), (5.0, 0.05), (8.0, 0.05)]);
let result = associate_points_to_lines(&store, &lines, &index, &config);
let points = extract_points_for_line(&result, 0);
@@ -534,10 +528,10 @@ mod tests {
let config = PointAssociationConfig::default().with_max_distance(0.1);
let store = make_scan_store_with_points(&[
(5.0, 0.05), // Close enough
(5.0, 0.5), // Too far
(5.0, 0.08), // Close enough
(5.0, 1.0), // Too far
(5.0, 0.05), // Close enough
(5.0, 0.5), // Too far
(5.0, 0.08), // Close enough
(5.0, 1.0), // Too far
]);
let result = associate_points_to_lines(&store, &lines, &index, &config);
vastu-map/src/integration/refitter.rs
+9 -9
View File
@@ -80,7 +80,7 @@ impl RefitResult {
/// Get all lines from the result.
pub fn lines(&self) -> Vec<Line2D> {
match self {
RefitResult::Single(line) => vec![line.clone()],
RefitResult::Single(line) => vec![*line],
RefitResult::Split(lines) => lines.clone(),
RefitResult::Insufficient => vec![],
}
@@ -262,8 +262,7 @@ fn split_and_fit(
// Fit each segment
for (range_start, range_end) in ranges {
let segment_points: Vec<&AssociatedPoint> =
sorted_points[range_start..range_end].to_vec();
let segment_points: Vec<&AssociatedPoint> = sorted_points[range_start..range_end].to_vec();
if segment_points.len() < config.min_points {
continue;
@@ -340,11 +339,7 @@ mod tests {
use super::*;
fn make_horizontal_line(y: f32, x_start: f32, x_end: f32) -> Line2D {
Line2D::with_observation_count(
Point2D::new(x_start, y),
Point2D::new(x_end, y),
5,
)
Line2D::with_observation_count(Point2D::new(x_start, y), Point2D::new(x_end, y), 5)
}
fn make_associated_point(x: f32, y: f32, scan_id: u32, t: f32) -> AssociatedPoint {
@@ -374,7 +369,12 @@ mod tests {
}
/// Create a dense point cloud (no gaps > threshold).
fn make_dense_points(x_start: f32, x_end: f32, y: f32, gap_threshold: f32) -> Vec<AssociatedPoint> {
fn make_dense_points(
x_start: f32,
x_end: f32,
y: f32,
gap_threshold: f32,
) -> Vec<AssociatedPoint> {
let line_length = (x_end - x_start).abs();
// Create points with spacing less than gap_threshold
let spacing = gap_threshold * 0.5;
vastu-map/src/integration/scan_store.rs
+1 -3
View File
@@ -591,9 +591,7 @@ mod tests {
use super::*;
fn make_test_points(n: usize) -> Vec<Point2D> {
(0..n)
.map(|i| Point2D::new(i as f32 * 0.1, 0.0))
.collect()
(0..n).map(|i| Point2D::new(i as f32 * 0.1, 0.0)).collect()
}
fn make_point_cloud(n: usize) -> PointCloud2D {
vastu-map/src/lib.rs
+1 -1
View File
@@ -126,7 +126,7 @@ pub mod vector_map;
// Re-export main types at crate root
pub use motion_model::MotionModel;
pub use vector_map::{ConfigError, VectorMap, VectorMapConfig};
pub use vector_map::{ConfigError, VectorMap, VectorMapConfig, YamlConfigError};
// Re-export exploration types
pub use exploration::{
vastu-map/src/loop_closure/mod.rs
+3 -1
View File
@@ -29,6 +29,8 @@
//! }
//! ```
use serde::{Deserialize, Serialize};
use crate::core::{Point2D, Pose2D};
use crate::features::{Corner2D, Line2D, ScanDescriptor};
use crate::matching::{IcpConfig, match_scan};
@@ -38,7 +40,7 @@ use crate::matching::{IcpConfig, match_scan};
// ─────────────────────────────────────────────────────────────────────────────
/// Configuration for loop closure detection.
#[derive(Clone, Debug)]
#[derive(Clone, Debug, Serialize, Deserialize)]
pub struct LoopClosureConfig {
/// Minimum travel distance before considering loop closure (meters).
/// Prevents detecting loops immediately after keyframe creation.
vastu-map/src/matching/icp/config.rs
+13 -3
View File
@@ -1,11 +1,13 @@
//! Configuration for Point-to-Line ICP algorithm.
use serde::{Deserialize, Serialize};
use crate::config::LidarNoiseModel;
use super::super::robust::RobustCostFunction;
/// Configuration for Point-to-Line ICP.
#[derive(Clone, Debug)]
#[derive(Clone, Debug, Serialize, Deserialize)]
pub struct IcpConfig {
/// Maximum number of ICP iterations.
/// Default: 30
@@ -18,6 +20,7 @@ pub struct IcpConfig {
/// Pre-computed squared convergence threshold for efficient comparison.
/// Avoids sqrt() in hot loop by comparing squared distances.
#[serde(skip)]
pub(crate) convergence_threshold_sq: f32,
/// Maximum correspondence distance (meters).
@@ -99,6 +102,13 @@ impl IcpConfig {
Self::default()
}
/// Recompute derived fields after deserialization.
///
/// Call this after loading from YAML to ensure computed fields are correct.
pub fn finalize(&mut self) {
self.convergence_threshold_sq = self.convergence_threshold * self.convergence_threshold;
}
// ===== Preset Configurations =====
/// Fast preset: Optimized for speed at the cost of some accuracy.
@@ -287,7 +297,7 @@ impl IcpConfig {
}
/// Configuration for coarse search initialization.
#[derive(Clone, Debug)]
#[derive(Clone, Debug, Serialize, Deserialize)]
pub struct CoarseSearchConfig {
/// Maximum translation search range (meters).
/// Search from -range to +range in both x and y.
@@ -330,7 +340,7 @@ impl Default for CoarseSearchConfig {
/// 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)]
#[derive(Clone, Debug, Serialize, Deserialize)]
pub struct MultiResolutionConfig {
/// Enable multi-resolution matching.
/// Default: true
vastu-map/src/matching/robust.rs
+4 -1
View File
@@ -24,11 +24,14 @@
//! | Tukey | Zero weight for outliers | Aggressive rejection |
//! | Geman-McClure | Redescending | Multimodal errors |
use serde::{Deserialize, Serialize};
/// 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)]
#[derive(Clone, Debug, PartialEq, Serialize, Deserialize)]
#[serde(tag = "type")]
pub enum RobustCostFunction {
/// No robust weighting (standard least squares).
/// Weight is always 1.0 regardless of residual.
vastu-map/src/query/frontier.rs
+2 -1
View File
@@ -22,6 +22,7 @@
//! let frontiers = detector.detect(&new_lines, &config);
//! ```
use serde::{Deserialize, Serialize};
use std::collections::HashMap;
use crate::Frontier;
@@ -257,7 +258,7 @@ impl FrontierDetector {
}
/// Configuration for frontier detection.
#[derive(Clone, Debug)]
#[derive(Clone, Debug, Serialize, Deserialize)]
pub struct FrontierConfig {
/// Maximum distance for endpoints to be considered "connected" (meters).
/// Endpoints closer than this are not frontiers.
vastu-map/src/query/occupancy.rs
+3 -1
View File
@@ -3,13 +3,15 @@
//! Determines whether a point is in free space, occupied, or unknown
//! based on the map's line features.
use serde::{Deserialize, Serialize};
use crate::Occupancy;
use crate::core::{Bounds, Point2D};
use crate::features::Line2D;
use crate::integration::SpatialIndex;
/// Configuration for occupancy queries.
#[derive(Clone, Debug)]
#[derive(Clone, Debug, Serialize, Deserialize)]
pub struct OccupancyConfig {
/// Distance threshold for considering a point "on" a line (meters).
/// Points within this distance of a line are considered occupied.
vastu-map/src/query/path_planning.rs
+3 -1
View File
@@ -28,6 +28,8 @@
//! );
//! ```
use serde::{Deserialize, Serialize};
use crate::core::{Bounds, Point2D};
use crate::features::Line2D;
use crate::query::occupancy::OccupancyConfig;
@@ -36,7 +38,7 @@ use std::cmp::Ordering;
use std::collections::BinaryHeap;
/// Configuration for path planning.
#[derive(Clone, Debug)]
#[derive(Clone, Debug, Serialize, Deserialize)]
pub struct PathPlanningConfig {
/// Robot radius for obstacle inflation (meters).
/// Paths will avoid coming within this distance of obstacles.
vastu-map/src/vector_map/config.rs
+12 -2
View File
@@ -1,5 +1,7 @@
//! Configuration for VectorMap.
use serde::{Deserialize, Serialize};
use crate::extraction::{CornerConfig, RansacLineConfig, SplitMergeConfig};
use crate::integration::{AssociationConfig, MergerConfig};
use crate::loop_closure::LoopClosureConfig;
@@ -11,7 +13,7 @@ use crate::query::{FrontierConfig, OccupancyConfig, PathPlanningConfig};
// ============================================================================
/// Errors that can occur during configuration validation.
#[derive(Debug, Clone, PartialEq)]
#[derive(Debug, Clone, PartialEq, Serialize, Deserialize)]
pub enum ConfigError {
/// A threshold value is invalid (negative or zero when positive required).
InvalidThreshold {
@@ -83,7 +85,7 @@ impl std::fmt::Display for ConfigError {
impl std::error::Error for ConfigError {}
/// Configuration for VectorMap.
#[derive(Clone, Debug)]
#[derive(Clone, Debug, Serialize, Deserialize)]
pub struct VectorMapConfig {
/// Configuration for line extraction (split-merge).
pub extraction: SplitMergeConfig,
@@ -464,6 +466,14 @@ impl VectorMapConfig {
Ok(())
}
/// Recompute derived fields after deserialization.
///
/// Call this after loading from YAML to ensure computed fields are correct.
pub fn finalize(&mut self) {
self.matching.finalize();
self.loop_closure.icp_config.finalize();
}
/// Validate and return self for builder pattern.
///
/// # Panics
vastu-map/src/vector_map/map.rs
+20 -30
View File
@@ -7,9 +7,9 @@ use crate::core::{Bounds, Point2D, PointCloud2D, Pose2D};
use crate::extraction::{detect_corners, extract_lines, extract_lines_hybrid};
use crate::features::{Corner2D, FeatureSet, Line2D};
use crate::integration::{
PointAssociationConfig, RefitConfig, RefitResult, RefitStats, ScanStore, ScanStoreConfig,
associate_points_to_lines, batch_merge, create_new_line, find_associations,
find_unmatched_scan_lines, refit_line, PointAssociationConfig, RefitConfig, RefitResult,
RefitStats, ScanStore, ScanStoreConfig,
find_unmatched_scan_lines, refit_line,
};
use crate::loop_closure::{LoopClosure, LoopClosureDetector};
use crate::matching::PointToLineIcp;
@@ -420,9 +420,7 @@ impl VectorMap {
// ─── Scan Storage (Exploration Mode) ───
if let Some(ref mut store) = self.scan_store {
store.add_scan(
odometry,
final_pose,
scan,
odometry, final_pose, scan,
0, // features_extracted - will be updated after extraction
confidence,
);
@@ -440,15 +438,16 @@ impl VectorMap {
};
// ─── Periodic Line Re-fitting (Exploration Mode) ───
if let Some(ref config) = self.exploration_config {
if config.refit_interval > 0 && self.observation_count % config.refit_interval == 0 {
log::debug!(
"Triggering line re-fit at observation {}",
self.observation_count
);
// Trigger line re-fitting from accumulated scans
let _ = self.refit_lines_from_scans();
}
if let Some(ref config) = self.exploration_config
&& config.refit_interval > 0
&& self.observation_count.is_multiple_of(config.refit_interval)
{
log::debug!(
"Triggering line re-fit at observation {}",
self.observation_count
);
// Trigger line re-fitting from accumulated scans
let _ = self.refit_lines_from_scans();
}
// ─── Loop Closure ───
@@ -503,10 +502,8 @@ impl VectorMap {
// ─── Scan Storage (Exploration Mode) ───
if let Some(ref mut store) = self.scan_store {
store.add_scan(
odometry,
odometry, // First scan: odometry = estimated pose
scan,
0, // features_extracted - unknown at this point
odometry, odometry, // First scan: odometry = estimated pose
scan, 0, // features_extracted - unknown at this point
0.0, // confidence - no ICP for first scan
);
}
@@ -674,11 +671,9 @@ impl VectorMap {
/// let merged = map.optimize_lines(&config);
/// println!("Merged {} redundant lines", merged);
/// ```
pub fn optimize_lines(
&mut self,
config: &crate::integration::CoplanarMergeConfig,
) -> usize {
let merged = crate::integration::optimize_coplanar_lines(self.line_store.lines_mut(), config);
pub fn optimize_lines(&mut self, config: &crate::integration::CoplanarMergeConfig) -> usize {
let merged =
crate::integration::optimize_coplanar_lines(self.line_store.lines_mut(), config);
if merged > 0 {
// Rebuild auxiliary structures
@@ -763,12 +758,7 @@ impl VectorMap {
for (line_idx, associated_points) in associations.line_points.iter().enumerate() {
let original = &self.line_store.lines()[line_idx];
let result = refit_line(
associated_points,
original,
&robot_poses,
&refit_config,
);
let result = refit_line(associated_points, original, &robot_poses, &refit_config);
match result {
RefitResult::Single(line) => {
@@ -782,7 +772,7 @@ impl VectorMap {
}
RefitResult::Insufficient => {
// Keep original line
new_lines.push(original.clone());
new_lines.push(*original);
stats.lines_insufficient += 1;
}
}
vastu-map/src/vector_map/mod.rs
+2
View File
@@ -30,8 +30,10 @@
mod config;
mod line_store;
mod map;
mod yaml;
// Re-export public API
pub use config::{ConfigError, VectorMapConfig};
pub use line_store::LineStore;
pub use map::VectorMap;
pub use yaml::YamlConfigError;
vastu-map/src/vector_map/yaml.rs
+455
View File
@@ -0,0 +1,455 @@
//! YAML configuration file loading for VectorMap.
//!
//! Load algorithm configuration from YAML files for reproducible experiments
//! and easy parameter tuning.
//!
//! # Example
//!
//! ```rust,ignore
//! use vastu_map::VectorMapConfig;
//!
//! // Load from file
//! let config = VectorMapConfig::from_yaml_file("config.yaml")?;
//!
//! // Validate before use
//! config.validate()?;
//!
//! let map = VectorMap::new(config);
//! ```
use std::io;
use std::path::Path;
use crate::vector_map::config::{ConfigError, VectorMapConfig};
/// Errors that can occur when loading YAML configuration.
#[derive(Debug)]
pub enum YamlConfigError {
/// IO error reading the file.
Io(io::Error),
/// YAML parsing error.
Parse(serde_yaml::Error),
/// Configuration validation failed.
Validation(ConfigError),
}
impl std::fmt::Display for YamlConfigError {
fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result {
match self {
YamlConfigError::Io(e) => write!(f, "IO error: {}", e),
YamlConfigError::Parse(e) => write!(f, "YAML parse error: {}", e),
YamlConfigError::Validation(e) => write!(f, "Config validation error: {}", e),
}
}
}
impl std::error::Error for YamlConfigError {
fn source(&self) -> Option<&(dyn std::error::Error + 'static)> {
match self {
YamlConfigError::Io(e) => Some(e),
YamlConfigError::Parse(e) => Some(e),
YamlConfigError::Validation(e) => Some(e),
}
}
}
impl From<io::Error> for YamlConfigError {
fn from(err: io::Error) -> Self {
YamlConfigError::Io(err)
}
}
impl From<serde_yaml::Error> for YamlConfigError {
fn from(err: serde_yaml::Error) -> Self {
YamlConfigError::Parse(err)
}
}
impl From<ConfigError> for YamlConfigError {
fn from(err: ConfigError) -> Self {
YamlConfigError::Validation(err)
}
}
impl VectorMapConfig {
/// Load configuration from a YAML file.
///
/// Parses the file and validates the configuration.
///
/// # Arguments
/// * `path` - Path to the YAML configuration file
///
/// # Errors
/// Returns `YamlConfigError` if the file cannot be read, parsed, or validated.
///
/// # Example
/// ```rust,ignore
/// let config = VectorMapConfig::from_yaml_file("config.yaml")?;
/// let map = VectorMap::new(config);
/// ```
pub fn from_yaml_file<P: AsRef<Path>>(path: P) -> Result<Self, YamlConfigError> {
let contents = std::fs::read_to_string(path)?;
Self::from_yaml_str(&contents)
}
/// Load configuration from a YAML string.
///
/// Parses the string and validates the configuration.
///
/// # Example
/// ```rust,ignore
/// let yaml = r#"
/// extraction:
/// split_threshold: 0.05
/// "#;
/// let config = VectorMapConfig::from_yaml_str(yaml)?;
/// ```
pub fn from_yaml_str(yaml: &str) -> Result<Self, YamlConfigError> {
let mut config: VectorMapConfig = serde_yaml::from_str(yaml)?;
// Finalize derived fields (like convergence_threshold_sq)
config.finalize();
// Validate configuration
config.validate()?;
Ok(config)
}
/// Load configuration from a YAML file without validation.
///
/// Use this if you want to manually validate or modify the config after loading.
///
/// # Example
/// ```rust,ignore
/// let mut config = VectorMapConfig::from_yaml_file_unchecked("config.yaml")?;
/// config.matching.max_iterations = 50; // Modify
/// config.validate()?; // Manually validate
/// ```
pub fn from_yaml_file_unchecked<P: AsRef<Path>>(path: P) -> Result<Self, YamlConfigError> {
let contents = std::fs::read_to_string(path)?;
Self::from_yaml_str_unchecked(&contents)
}
/// Load configuration from a YAML string without validation.
pub fn from_yaml_str_unchecked(yaml: &str) -> Result<Self, YamlConfigError> {
let mut config: VectorMapConfig = serde_yaml::from_str(yaml)?;
config.finalize();
Ok(config)
}
/// Serialize configuration to a YAML string.
///
/// Useful for saving configurations or generating templates.
///
/// # Example
/// ```rust,ignore
/// let config = VectorMapConfig::default();
/// let yaml = config.to_yaml_string()?;
/// println!("{}", yaml);
/// ```
pub fn to_yaml_string(&self) -> Result<String, serde_yaml::Error> {
serde_yaml::to_string(self)
}
/// Save configuration to a YAML file.
///
/// # Example
/// ```rust,ignore
/// let config = VectorMapConfig::default();
/// config.to_yaml_file("reference.yaml")?;
/// ```
pub fn to_yaml_file<P: AsRef<Path>>(&self, path: P) -> Result<(), YamlConfigError> {
let yaml = self.to_yaml_string()?;
std::fs::write(path, yaml)?;
Ok(())
}
}
#[cfg(test)]
mod tests {
use super::*;
#[test]
fn test_roundtrip_default_config() {
let original = VectorMapConfig::default();
let yaml = original.to_yaml_string().expect("Failed to serialize");
let loaded = VectorMapConfig::from_yaml_str(&yaml).expect("Failed to deserialize");
// Key fields should match
assert_eq!(
original.matching.max_iterations,
loaded.matching.max_iterations
);
assert_eq!(original.use_hybrid_extraction, loaded.use_hybrid_extraction);
assert_eq!(original.mapping_enabled, loaded.mapping_enabled);
}
#[test]
fn test_validation_called_on_load() {
// Create invalid YAML with zero iterations
let yaml = r#"
extraction:
split_threshold: 0.05
min_points: 5
min_length: 0.1
max_point_gap: 0.3
merge_threshold: 0.05
adaptive_range_scale: 0.03
ransac_extraction:
iterations: 0
inlier_threshold: 0.02
min_inliers: 5
min_line_length: 0.1
max_point_gap: 0.1
max_lines: 0
seed: 0
use_hybrid_extraction: true
corner:
min_angle: 0.524
max_angle: 2.618
max_endpoint_distance: 0.05
curvature_window_size: 3
curvature_threshold: 0.785
merge_distance: 0.1
nms_radius: 0.15
matching:
max_iterations: 30
convergence_threshold: 0.0001
max_correspondence_distance: 0.5
robust_cost:
type: Huber
delta: 0.03
use_irls: true
min_correspondences: 10
min_confidence: 0.3
use_ransac_init: true
use_batch_search: true
noise_model:
sigma_base: 0.01
sigma_range: 0.001
use_coarse_search: true
coarse_search_confidence_threshold: 0.5
multi_resolution:
enabled: true
levels: 3
subsample_factor: 2
iterations_per_level: [3, 5, 10]
association:
max_perpendicular_distance: 0.15
max_angle_difference: 0.2
min_overlap_ratio: 0.3
max_endpoint_gap: 0.3
distance_weight: 10.0
angle_weight: 5.0
merger:
map_weight: 0.8
extend_endpoints: true
max_extension: 0.5
min_extension: 0.05
frontier:
connection_distance: 0.3
min_distance_from_robot: 0.5
min_openness: 0.3
occupancy:
obstacle_distance: 0.1
known_distance: 2.0
use_winding_number: false
path_planning:
robot_radius: 0.15
max_nodes: 1000
conservative: true
loop_closure:
min_travel_distance: 5.0
max_descriptor_distance: 0.5
min_verification_confidence: 0.6
keyframe_interval: 2.0
descriptor_radius: 2.0
max_candidates_to_verify: 3
min_lines_for_keyframe: 3
min_corners_for_keyframe: 2
icp_config:
max_iterations: 30
convergence_threshold: 0.0001
max_correspondence_distance: 0.5
robust_cost:
type: Huber
delta: 0.03
use_irls: true
min_correspondences: 10
min_confidence: 0.3
use_ransac_init: true
use_batch_search: true
noise_model:
sigma_base: 0.01
sigma_range: 0.001
use_coarse_search: true
coarse_search_confidence_threshold: 0.5
multi_resolution:
enabled: true
levels: 3
subsample_factor: 2
iterations_per_level: [3, 5, 10]
min_match_confidence: 0.3
mapping_enabled: true
loop_closure_enabled: true
"#;
let result = VectorMapConfig::from_yaml_str(yaml);
assert!(result.is_err());
assert!(matches!(
result.unwrap_err(),
YamlConfigError::Validation(_)
));
}
#[test]
fn test_unchecked_load_skips_validation() {
// Same invalid YAML but using unchecked method
let yaml = r#"
extraction:
split_threshold: 0.05
min_points: 5
min_length: 0.1
max_point_gap: 0.3
merge_threshold: 0.05
adaptive_range_scale: 0.03
ransac_extraction:
iterations: 0
inlier_threshold: 0.02
min_inliers: 5
min_line_length: 0.1
max_point_gap: 0.1
max_lines: 0
seed: 0
use_hybrid_extraction: true
corner:
min_angle: 0.524
max_angle: 2.618
max_endpoint_distance: 0.05
curvature_window_size: 3
curvature_threshold: 0.785
merge_distance: 0.1
nms_radius: 0.15
matching:
max_iterations: 30
convergence_threshold: 0.0001
max_correspondence_distance: 0.5
robust_cost:
type: Huber
delta: 0.03
use_irls: true
min_correspondences: 10
min_confidence: 0.3
use_ransac_init: true
use_batch_search: true
noise_model:
sigma_base: 0.01
sigma_range: 0.001
use_coarse_search: true
coarse_search_confidence_threshold: 0.5
multi_resolution:
enabled: true
levels: 3
subsample_factor: 2
iterations_per_level: [3, 5, 10]
association:
max_perpendicular_distance: 0.15
max_angle_difference: 0.2
min_overlap_ratio: 0.3
max_endpoint_gap: 0.3
distance_weight: 10.0
angle_weight: 5.0
merger:
map_weight: 0.8
extend_endpoints: true
max_extension: 0.5
min_extension: 0.05
frontier:
connection_distance: 0.3
min_distance_from_robot: 0.5
min_openness: 0.3
occupancy:
obstacle_distance: 0.1
known_distance: 2.0
use_winding_number: false
path_planning:
robot_radius: 0.15
max_nodes: 1000
conservative: true
loop_closure:
min_travel_distance: 5.0
max_descriptor_distance: 0.5
min_verification_confidence: 0.6
keyframe_interval: 2.0
descriptor_radius: 2.0
max_candidates_to_verify: 3
min_lines_for_keyframe: 3
min_corners_for_keyframe: 2
icp_config:
max_iterations: 30
convergence_threshold: 0.0001
max_correspondence_distance: 0.5
robust_cost:
type: Huber
delta: 0.03
use_irls: true
min_correspondences: 10
min_confidence: 0.3
use_ransac_init: true
use_batch_search: true
noise_model:
sigma_base: 0.01
sigma_range: 0.001
use_coarse_search: true
coarse_search_confidence_threshold: 0.5
multi_resolution:
enabled: true
levels: 3
subsample_factor: 2
iterations_per_level: [3, 5, 10]
min_match_confidence: 0.3
mapping_enabled: true
loop_closure_enabled: true
"#;
// Should succeed with unchecked method
let result = VectorMapConfig::from_yaml_str_unchecked(yaml);
assert!(result.is_ok());
// But validation should fail
let config = result.unwrap();
assert!(config.validate().is_err());
}
#[test]
fn test_empty_yaml_fails() {
let result = VectorMapConfig::from_yaml_str("");
assert!(result.is_err());
}
#[test]
fn test_io_error_conversion() {
let io_err = io::Error::new(io::ErrorKind::NotFound, "file not found");
let yaml_err: YamlConfigError = io_err.into();
assert!(matches!(yaml_err, YamlConfigError::Io(_)));
}
#[test]
fn test_load_default_yaml_file() {
// Load the reference config file from configs/default.yaml
let config_path = std::path::Path::new(env!("CARGO_MANIFEST_DIR"))
.join("configs")
.join("default.yaml");
let config = VectorMapConfig::from_yaml_file(&config_path)
.expect("Failed to load configs/default.yaml");
// Verify key values match expected defaults
assert_eq!(config.matching.max_iterations, 30);
assert!(config.use_hybrid_extraction);
assert!(config.mapping_enabled);
assert!(config.loop_closure_enabled);
assert!(config.validate().is_ok());
}
}
vastu-map/tests/integration/harness.rs
+71 -3
View File
@@ -2,6 +2,24 @@
//!
//! Provides synchronous simulation using sangam-io's mock device components
//! directly (without spawning daemon threads).
//!
//! # Configuration
//!
//! SLAM algorithm parameters can be customized via YAML configuration:
//!
//! - Set `VASTU_MAP_CONFIG` environment variable to specify a custom config file
//! - Falls back to `configs/default.yaml` in the project root
//! - Falls back to `VectorMapConfig::default()` if no YAML file is found
//!
//! ```bash
//! # Use custom config
//! VASTU_MAP_CONFIG=my_config.yaml cargo test --features integration-tests
//!
//! # Use default config file
//! cargo test --features integration-tests
//! ```
use std::path::Path;
use sangam_io::devices::mock::config::{LidarConfig, RobotConfig};
use sangam_io::devices::mock::lidar_sim::LidarSimulator;
@@ -55,6 +73,56 @@ pub struct HarnessConfig {
pub fast_mode: bool,
}
/// Environment variable name for custom SLAM config file path.
const SLAM_CONFIG_ENV_VAR: &str = "VASTU_MAP_CONFIG";
/// Load VectorMapConfig from YAML file.
///
/// Priority:
/// 1. `VASTU_MAP_CONFIG` environment variable (if set)
/// 2. `configs/default.yaml` in project root
/// 3. `VectorMapConfig::default()` as fallback
fn load_slam_config() -> VectorMapConfig {
// Check environment variable first
if let Ok(config_path) = std::env::var(SLAM_CONFIG_ENV_VAR) {
match VectorMapConfig::from_yaml_file(&config_path) {
Ok(config) => {
log::info!("Loaded SLAM config from env var: {}", config_path);
return config;
}
Err(e) => {
log::warn!(
"Failed to load SLAM config from {}: {}, using default",
config_path,
e
);
}
}
}
// Try default config file in project root
let default_config_path = Path::new(env!("CARGO_MANIFEST_DIR")).join("configs/default.yaml");
if default_config_path.exists() {
match VectorMapConfig::from_yaml_file(&default_config_path) {
Ok(config) => {
log::info!("Loaded SLAM config from: {}", default_config_path.display());
return config;
}
Err(e) => {
log::warn!(
"Failed to load default SLAM config from {}: {}, using builtin defaults",
default_config_path.display(),
e
);
}
}
}
// Fall back to builtin defaults
log::info!("Using builtin VectorMapConfig::default()");
VectorMapConfig::default()
}
impl Default for HarnessConfig {
fn default() -> Self {
// Use lidar config with zero offsets for test accuracy
@@ -75,7 +143,7 @@ impl Default for HarnessConfig {
start_theta: 0.0,
robot: RobotConfig::default(),
lidar,
slam: VectorMapConfig::default(),
slam: load_slam_config(),
random_seed: 42, // Fixed seed for reproducibility
min_quality: 50,
min_range: 0.15,
@@ -126,8 +194,8 @@ impl HarnessConfig {
env!("CARGO_MANIFEST_DIR"),
"/tests/integration/scenarios/maps/medium_room.yaml"
);
// Use non-conservative path planning for exploration
let slam = VectorMapConfig::default()
// Load base config from YAML, then override path planning for exploration
let slam = load_slam_config()
.with_path_planning(PathPlanningConfig::default().with_conservative(false));
Self {
map_file: map_file.to_string(),