Add exploration mode with point-cloud-based line re-fitting
Implements a new exploration mode that stores raw scan data and periodically re-fits lines from accumulated point clouds, eliminating first-scan bias in line extraction. New modules: - scan_store: Retains raw scans with pose tracking and memory management - point_association: Associates world-frame points with existing map lines - refitter: Re-fits lines from accumulated points with gap detection/splitting Key features: - ExplorationConfig for controlling re-fit intervals and thresholds - LineFitConfig with max_line_length splitting for robustness - Line2D point_count field for tracking scan point origins - Gap-based line splitting during re-fitting - Range-based weighting using noise model during fitting
This commit is contained in:
codetiger
@@ -119,6 +119,98 @@ impl LidarNoiseModel {
|
||||
}
|
||||
}
|
||||
|
||||
/// Configuration for exploration mode with point-cloud-based line re-fitting.
|
||||
///
|
||||
/// When exploration mode is enabled, VectorMap stores raw scan data and
|
||||
/// periodically re-fits lines from accumulated point clouds. This eliminates
|
||||
/// first-scan bias in line extraction.
|
||||
///
|
||||
/// # Example
|
||||
/// ```rust,ignore
|
||||
/// use vastu_map::config::ExplorationConfig;
|
||||
///
|
||||
/// let config = ExplorationConfig::default()
|
||||
/// .with_refit_interval(10)
|
||||
/// .with_gap_threshold(0.3);
|
||||
///
|
||||
/// map.enable_exploration_mode(config);
|
||||
/// ```
|
||||
#[derive(Clone, Debug)]
|
||||
pub struct ExplorationConfig {
|
||||
/// Re-fit lines every N observations.
|
||||
/// Default: 10
|
||||
pub refit_interval: usize,
|
||||
|
||||
/// Minimum points per line to trigger re-fitting.
|
||||
/// Lines with fewer associated points keep their current geometry.
|
||||
/// Default: 10
|
||||
pub min_points_per_line: usize,
|
||||
|
||||
/// Maximum perpendicular distance for point-to-line association (meters).
|
||||
/// Points farther than this from any line are not associated.
|
||||
/// Default: 0.10m (10cm)
|
||||
pub max_point_distance: f32,
|
||||
|
||||
/// Gap threshold for splitting lines (meters).
|
||||
/// If consecutive points along a line have a gap larger than this,
|
||||
/// the line is split into multiple segments.
|
||||
/// Default: 0.30m (30cm)
|
||||
pub gap_threshold: f32,
|
||||
|
||||
/// Noise model for range-based weighting during re-fitting.
|
||||
/// Closer points get higher weight.
|
||||
pub noise_model: LidarNoiseModel,
|
||||
}
|
||||
|
||||
impl Default for ExplorationConfig {
|
||||
fn default() -> Self {
|
||||
Self {
|
||||
refit_interval: 10,
|
||||
min_points_per_line: 10,
|
||||
max_point_distance: 0.10,
|
||||
gap_threshold: 0.30,
|
||||
noise_model: LidarNoiseModel::default(),
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
impl ExplorationConfig {
|
||||
/// Create a new configuration with default values.
|
||||
pub fn new() -> Self {
|
||||
Self::default()
|
||||
}
|
||||
|
||||
/// Set the re-fit interval (every N observations).
|
||||
pub fn with_refit_interval(mut self, interval: usize) -> Self {
|
||||
self.refit_interval = interval;
|
||||
self
|
||||
}
|
||||
|
||||
/// Set the minimum points per line for re-fitting.
|
||||
pub fn with_min_points(mut self, min_points: usize) -> Self {
|
||||
self.min_points_per_line = min_points;
|
||||
self
|
||||
}
|
||||
|
||||
/// Set the maximum point-to-line distance for association.
|
||||
pub fn with_max_point_distance(mut self, distance: f32) -> Self {
|
||||
self.max_point_distance = distance;
|
||||
self
|
||||
}
|
||||
|
||||
/// Set the gap threshold for line splitting.
|
||||
pub fn with_gap_threshold(mut self, gap: f32) -> Self {
|
||||
self.gap_threshold = gap;
|
||||
self
|
||||
}
|
||||
|
||||
/// Set the noise model for range-based weighting.
|
||||
pub fn with_noise_model(mut self, model: LidarNoiseModel) -> Self {
|
||||
self.noise_model = model;
|
||||
self
|
||||
}
|
||||
}
|
||||
|
||||
/// Configuration for VectorMap SLAM.
|
||||
///
|
||||
/// All parameters have sensible defaults. Create with `Default::default()`
|
||||
@@ -465,4 +557,28 @@ mod tests {
|
||||
assert_eq!(config.noise_model.sigma_base, 0.02);
|
||||
assert_eq!(config.noise_model.sigma_range, 0.002);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_exploration_config_default() {
|
||||
let config = ExplorationConfig::default();
|
||||
|
||||
assert_eq!(config.refit_interval, 10);
|
||||
assert_eq!(config.min_points_per_line, 10);
|
||||
assert!((config.max_point_distance - 0.10).abs() < 1e-6);
|
||||
assert!((config.gap_threshold - 0.30).abs() < 1e-6);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_exploration_config_builder() {
|
||||
let config = ExplorationConfig::new()
|
||||
.with_refit_interval(20)
|
||||
.with_min_points(15)
|
||||
.with_max_point_distance(0.15)
|
||||
.with_gap_threshold(0.5);
|
||||
|
||||
assert_eq!(config.refit_interval, 20);
|
||||
assert_eq!(config.min_points_per_line, 15);
|
||||
assert!((config.max_point_distance - 0.15).abs() < 1e-6);
|
||||
assert!((config.gap_threshold - 0.5).abs() < 1e-6);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -41,6 +41,153 @@ use crate::config::LidarNoiseModel;
|
||||
use crate::core::Point2D;
|
||||
use crate::features::Line2D;
|
||||
|
||||
// ============================================================================
|
||||
// Configuration
|
||||
// ============================================================================
|
||||
|
||||
/// Configuration for line fitting operations.
|
||||
///
|
||||
/// Controls how lines are fitted to point sets, including maximum line length
|
||||
/// constraints that can improve robustness by creating smaller line segments
|
||||
/// that are later merged by the integration layer.
|
||||
///
|
||||
/// # Example
|
||||
/// ```
|
||||
/// use vastu_map::extraction::LineFitConfig;
|
||||
///
|
||||
/// // Create config with 1m max line length
|
||||
/// let config = LineFitConfig::new()
|
||||
/// .with_max_line_length(1.0);
|
||||
///
|
||||
/// // Lines longer than 1m will be split into smaller segments
|
||||
/// ```
|
||||
#[derive(Debug, Clone)]
|
||||
pub struct LineFitConfig {
|
||||
/// Maximum length for a single line segment.
|
||||
///
|
||||
/// If a fitted line exceeds this length, it will be split into multiple
|
||||
/// smaller segments. This can improve robustness as shorter segments are
|
||||
/// less prone to fitting errors from outliers.
|
||||
///
|
||||
/// Set to `None` (default) for no length limit.
|
||||
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 {
|
||||
Self::default()
|
||||
}
|
||||
|
||||
/// Set the maximum line length.
|
||||
///
|
||||
/// Lines longer than this will be split into smaller segments.
|
||||
/// Pass `None` to disable length limiting (default).
|
||||
///
|
||||
/// # Arguments
|
||||
/// * `length` - Maximum length in meters, or None for no limit
|
||||
pub fn with_max_line_length(mut self, length: f32) -> Self {
|
||||
self.max_line_length = Some(length);
|
||||
self
|
||||
}
|
||||
|
||||
/// Set the maximum line length (optional variant).
|
||||
pub fn with_max_line_length_opt(mut self, length: Option<f32>) -> Self {
|
||||
self.max_line_length = length;
|
||||
self
|
||||
}
|
||||
}
|
||||
|
||||
// ============================================================================
|
||||
// Line Splitting
|
||||
// ============================================================================
|
||||
|
||||
/// Split a line into smaller segments if it exceeds the maximum length.
|
||||
///
|
||||
/// Returns a vector of line segments, each no longer than `max_length`.
|
||||
/// The point counts are distributed proportionally across segments.
|
||||
///
|
||||
/// # Arguments
|
||||
/// * `line` - The line to potentially split
|
||||
/// * `max_length` - Maximum length for each segment
|
||||
///
|
||||
/// # Returns
|
||||
/// Vector of line segments. If the line is already short enough,
|
||||
/// returns a vector containing just the original line.
|
||||
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()];
|
||||
}
|
||||
|
||||
// Calculate number of segments needed
|
||||
let num_segments = (length / max_length).ceil() as usize;
|
||||
|
||||
// Direction vector (not normalized - used with t parameter 0..1)
|
||||
let dir = line.direction();
|
||||
|
||||
let mut segments = Vec::with_capacity(num_segments);
|
||||
|
||||
// Distribute point count proportionally
|
||||
let points_per_segment = if line.point_count > 0 {
|
||||
(line.point_count as f32 / num_segments as f32).ceil() as u32
|
||||
} else {
|
||||
0
|
||||
};
|
||||
|
||||
for i in 0..num_segments {
|
||||
// Use t parameter (0 to 1) to interpolate along line
|
||||
let t_start = i as f32 / num_segments as f32;
|
||||
let t_end = (i + 1) as f32 / num_segments as f32;
|
||||
|
||||
let start = Point2D::new(
|
||||
line.start.x + dir.x * t_start,
|
||||
line.start.y + dir.y * t_start,
|
||||
);
|
||||
let end = if i == num_segments - 1 {
|
||||
// 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,
|
||||
)
|
||||
};
|
||||
|
||||
// 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)
|
||||
} else {
|
||||
points_per_segment
|
||||
};
|
||||
|
||||
segments.push(Line2D::full(start, end, 1, segment_point_count));
|
||||
}
|
||||
|
||||
segments
|
||||
}
|
||||
|
||||
/// Split multiple lines by maximum length.
|
||||
///
|
||||
/// Convenience function that applies `split_line_by_length` to all lines.
|
||||
pub fn split_lines_by_length(lines: &[Line2D], max_length: f32) -> Vec<Line2D> {
|
||||
let mut result = Vec::new();
|
||||
for line in lines {
|
||||
result.extend(split_line_by_length(line, max_length));
|
||||
}
|
||||
result
|
||||
}
|
||||
|
||||
// ============================================================================
|
||||
// Error Types
|
||||
// ============================================================================
|
||||
@@ -212,7 +359,7 @@ pub fn fit_line(points: &[Point2D]) -> Option<Line2D> {
|
||||
centroid.y + t_max * direction.y,
|
||||
);
|
||||
|
||||
Some(Line2D::new(start, end))
|
||||
Some(Line2D::with_point_count(start, end, points.len() as u32))
|
||||
}
|
||||
|
||||
/// Fit a line to a set of points using Total Least Squares (with explicit errors).
|
||||
@@ -307,7 +454,7 @@ pub fn fit_line_checked(points: &[Point2D]) -> Result<Line2D, FitError> {
|
||||
centroid.y + t_max * direction.y,
|
||||
);
|
||||
|
||||
Ok(Line2D::new(start, end))
|
||||
Ok(Line2D::with_point_count(start, end, points.len() as u32))
|
||||
}
|
||||
|
||||
/// Simple line fitting fallback (checked version).
|
||||
@@ -318,7 +465,11 @@ fn fit_line_simple_checked(points: &[Point2D]) -> Result<Line2D, FitError> {
|
||||
required: 2,
|
||||
});
|
||||
}
|
||||
Ok(Line2D::new(points[0], points[points.len() - 1]))
|
||||
Ok(Line2D::with_point_count(
|
||||
points[0],
|
||||
points[points.len() - 1],
|
||||
points.len() as u32,
|
||||
))
|
||||
}
|
||||
|
||||
/// Simple line fitting fallback using first and last points.
|
||||
@@ -326,7 +477,11 @@ fn fit_line_simple(points: &[Point2D]) -> Option<Line2D> {
|
||||
if points.len() < 2 {
|
||||
return None;
|
||||
}
|
||||
Some(Line2D::new(points[0], points[points.len() - 1]))
|
||||
Some(Line2D::with_point_count(
|
||||
points[0],
|
||||
points[points.len() - 1],
|
||||
points.len() as u32,
|
||||
))
|
||||
}
|
||||
|
||||
/// Compute range-based weights for line fitting.
|
||||
@@ -453,7 +608,7 @@ pub fn fit_line_weighted(points: &[Point2D], weights: &[f32]) -> Option<Line2D>
|
||||
centroid.y + t_max * direction.y,
|
||||
);
|
||||
|
||||
Some(Line2D::new(start, end))
|
||||
Some(Line2D::with_point_count(start, end, points.len() as u32))
|
||||
}
|
||||
|
||||
/// Fit a line to points using range-based weighting from sensor position.
|
||||
@@ -494,6 +649,119 @@ pub fn fit_line_from_sensor(
|
||||
fit_line_weighted(points, &weights)
|
||||
}
|
||||
|
||||
// ============================================================================
|
||||
// Config-based Line Fitting (with max length splitting)
|
||||
// ============================================================================
|
||||
|
||||
/// Fit a line to points with configuration, potentially splitting into segments.
|
||||
///
|
||||
/// If `config.max_line_length` is set, lines longer than this threshold
|
||||
/// will be split into smaller segments for improved robustness.
|
||||
///
|
||||
/// # Arguments
|
||||
/// * `points` - Points to fit line to
|
||||
/// * `config` - Configuration controlling fitting behavior
|
||||
///
|
||||
/// # Returns
|
||||
/// Vector of fitted line segments. Returns empty if fitting fails.
|
||||
///
|
||||
/// # Example
|
||||
/// ```
|
||||
/// use vastu_map::extraction::{fit_line_with_config, LineFitConfig};
|
||||
/// use vastu_map::core::Point2D;
|
||||
///
|
||||
/// let points: Vec<Point2D> = (0..50)
|
||||
/// .map(|i| Point2D::new(i as f32 * 0.1, 0.0)) // 5m line
|
||||
/// .collect();
|
||||
///
|
||||
/// let config = LineFitConfig::new().with_max_line_length(1.0);
|
||||
/// let lines = fit_line_with_config(&points, &config);
|
||||
///
|
||||
/// // Line is split into ~5 segments of ~1m each
|
||||
/// assert!(lines.len() >= 4);
|
||||
/// ```
|
||||
pub fn fit_line_with_config(points: &[Point2D], config: &LineFitConfig) -> Vec<Line2D> {
|
||||
let Some(line) = fit_line(points) else {
|
||||
return Vec::new();
|
||||
};
|
||||
|
||||
match config.max_line_length {
|
||||
Some(max_len) if max_len > 0.0 => split_line_by_length(&line, max_len),
|
||||
_ => vec![line],
|
||||
}
|
||||
}
|
||||
|
||||
/// Fit a line to points with weighting and configuration.
|
||||
///
|
||||
/// Combines weighted fitting with max length splitting for robust line extraction.
|
||||
///
|
||||
/// # Arguments
|
||||
/// * `points` - Points to fit line to
|
||||
/// * `weights` - Weight for each point
|
||||
/// * `config` - Configuration controlling fitting behavior
|
||||
///
|
||||
/// # Returns
|
||||
/// Vector of fitted line segments. Returns empty if fitting fails.
|
||||
pub fn fit_line_weighted_with_config(
|
||||
points: &[Point2D],
|
||||
weights: &[f32],
|
||||
config: &LineFitConfig,
|
||||
) -> Vec<Line2D> {
|
||||
let Some(line) = fit_line_weighted(points, weights) else {
|
||||
return Vec::new();
|
||||
};
|
||||
|
||||
match config.max_line_length {
|
||||
Some(max_len) if max_len > 0.0 => split_line_by_length(&line, max_len),
|
||||
_ => vec![line],
|
||||
}
|
||||
}
|
||||
|
||||
/// Fit a line using sensor-based weighting with configuration.
|
||||
///
|
||||
/// Combines range-based weighting with max length splitting.
|
||||
///
|
||||
/// # Arguments
|
||||
/// * `points` - Points to fit line to
|
||||
/// * `sensor_pos` - Position of the sensor (typically robot position)
|
||||
/// * `noise_model` - Lidar noise model parameters (uses default if None)
|
||||
/// * `config` - Configuration controlling fitting behavior
|
||||
///
|
||||
/// # Returns
|
||||
/// Vector of fitted line segments. Returns empty if fitting fails.
|
||||
///
|
||||
/// # Example
|
||||
/// ```
|
||||
/// use vastu_map::extraction::{fit_line_from_sensor_with_config, LineFitConfig};
|
||||
/// use vastu_map::core::Point2D;
|
||||
///
|
||||
/// let sensor_pos = Point2D::new(0.0, 0.0);
|
||||
/// let points: Vec<Point2D> = (1..30)
|
||||
/// .map(|i| Point2D::new(i as f32 * 0.1, 1.0)) // ~3m wall
|
||||
/// .collect();
|
||||
///
|
||||
/// let config = LineFitConfig::new().with_max_line_length(0.5);
|
||||
/// let lines = fit_line_from_sensor_with_config(&points, sensor_pos, None, &config);
|
||||
///
|
||||
/// // Wall is split into ~6 segments of ~0.5m each
|
||||
/// assert!(lines.len() >= 5);
|
||||
/// ```
|
||||
pub fn fit_line_from_sensor_with_config(
|
||||
points: &[Point2D],
|
||||
sensor_pos: Point2D,
|
||||
noise_model: Option<&LidarNoiseModel>,
|
||||
config: &LineFitConfig,
|
||||
) -> Vec<Line2D> {
|
||||
let Some(line) = fit_line_from_sensor(points, sensor_pos, noise_model) else {
|
||||
return Vec::new();
|
||||
};
|
||||
|
||||
match config.max_line_length {
|
||||
Some(max_len) if max_len > 0.0 => split_line_by_length(&line, max_len),
|
||||
_ => vec![line],
|
||||
}
|
||||
}
|
||||
|
||||
/// Compute weighted fitting error (RMS distance to line, weighted by measurement quality).
|
||||
///
|
||||
/// Returns the weighted root-mean-square of perpendicular distances.
|
||||
@@ -905,4 +1173,234 @@ mod tests {
|
||||
weighted_error
|
||||
);
|
||||
}
|
||||
|
||||
// ========================================
|
||||
// Line Splitting 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 segments = split_line_by_length(&line, 1.0);
|
||||
|
||||
assert_eq!(segments.len(), 1);
|
||||
assert_relative_eq!(segments[0].start.x, 0.0, epsilon = 1e-6);
|
||||
assert_relative_eq!(segments[0].end.x, 0.5, epsilon = 1e-6);
|
||||
assert_eq!(segments[0].point_count, 10);
|
||||
}
|
||||
|
||||
#[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 segments = split_line_by_length(&line, 1.0);
|
||||
|
||||
assert_eq!(segments.len(), 1);
|
||||
}
|
||||
|
||||
#[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 segments = split_line_by_length(&line, 1.0);
|
||||
|
||||
assert_eq!(segments.len(), 2);
|
||||
|
||||
// First segment
|
||||
assert_relative_eq!(segments[0].start.x, 0.0, epsilon = 1e-6);
|
||||
assert_relative_eq!(segments[0].end.x, 1.0, epsilon = 1e-6);
|
||||
assert_eq!(segments[0].point_count, 10);
|
||||
|
||||
// Second segment
|
||||
assert_relative_eq!(segments[1].start.x, 1.0, epsilon = 1e-6);
|
||||
assert_relative_eq!(segments[1].end.x, 2.0, epsilon = 1e-6);
|
||||
assert_eq!(segments[1].point_count, 10);
|
||||
}
|
||||
|
||||
#[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 segments = split_line_by_length(&line, 1.0);
|
||||
|
||||
assert_eq!(segments.len(), 5);
|
||||
|
||||
// Total length should be preserved
|
||||
let total_length: f32 = segments.iter().map(|s| s.length()).sum();
|
||||
assert_relative_eq!(total_length, 5.0, epsilon = 1e-5);
|
||||
|
||||
// Total point count should be preserved
|
||||
let total_points: u32 = segments.iter().map(|s| s.point_count).sum();
|
||||
assert_eq!(total_points, 50);
|
||||
}
|
||||
|
||||
#[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 segments = split_line_by_length(&line, 2.0);
|
||||
|
||||
// 7.07 / 2.0 = 3.54, so 4 segments
|
||||
assert_eq!(segments.len(), 4);
|
||||
|
||||
// Segments should be collinear
|
||||
for seg in &segments {
|
||||
assert_relative_eq!(seg.angle(), FRAC_PI_4, epsilon = 0.01);
|
||||
}
|
||||
|
||||
// Endpoints should be preserved
|
||||
assert_relative_eq!(segments[0].start.x, 0.0, epsilon = 1e-5);
|
||||
assert_relative_eq!(segments[0].start.y, 0.0, epsilon = 1e-5);
|
||||
assert_relative_eq!(segments.last().unwrap().end.x, 5.0, epsilon = 1e-5);
|
||||
assert_relative_eq!(segments.last().unwrap().end.y, 5.0, epsilon = 1e-5);
|
||||
}
|
||||
|
||||
#[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,
|
||||
);
|
||||
|
||||
// Zero or negative max_length should return original line
|
||||
let segments = split_line_by_length(&line, 0.0);
|
||||
assert_eq!(segments.len(), 1);
|
||||
|
||||
let segments_neg = split_line_by_length(&line, -1.0);
|
||||
assert_eq!(segments_neg.len(), 1);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_split_lines_by_length() {
|
||||
let lines = vec![
|
||||
Line2D::with_point_count(Point2D::new(0.0, 0.0), Point2D::new(3.0, 0.0), 30),
|
||||
Line2D::with_point_count(Point2D::new(0.0, 1.0), Point2D::new(0.5, 1.0), 5),
|
||||
];
|
||||
|
||||
let segments = split_lines_by_length(&lines, 1.0);
|
||||
|
||||
// First line splits into 3, second line stays as 1
|
||||
assert_eq!(segments.len(), 4);
|
||||
}
|
||||
|
||||
// ========================================
|
||||
// Config-based Fitting Tests
|
||||
// ========================================
|
||||
|
||||
#[test]
|
||||
fn test_line_fit_config_default() {
|
||||
let config = LineFitConfig::default();
|
||||
assert!(config.max_line_length.is_none());
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_line_fit_config_builder() {
|
||||
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();
|
||||
|
||||
// Config with no max length
|
||||
let config = LineFitConfig::new();
|
||||
let lines = fit_line_with_config(&points, &config);
|
||||
|
||||
assert_eq!(lines.len(), 1);
|
||||
}
|
||||
|
||||
#[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();
|
||||
|
||||
// Config with 1m max length
|
||||
let config = LineFitConfig::new().with_max_line_length(1.0);
|
||||
let lines = fit_line_with_config(&points, &config);
|
||||
|
||||
// 5m line should split into 5 segments
|
||||
assert_eq!(lines.len(), 5);
|
||||
|
||||
// Each segment should be approximately 1m
|
||||
for line in &lines {
|
||||
assert!(line.length() <= 1.01, "Line length {} > 1.01", line.length());
|
||||
}
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_fit_line_with_config_empty_input() {
|
||||
let points: Vec<Point2D> = vec![];
|
||||
let config = LineFitConfig::new().with_max_line_length(1.0);
|
||||
|
||||
let lines = fit_line_with_config(&points, &config);
|
||||
|
||||
assert!(lines.is_empty());
|
||||
}
|
||||
|
||||
#[test]
|
||||
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
|
||||
.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());
|
||||
|
||||
// All segments should be approximately horizontal
|
||||
for line in &lines {
|
||||
assert!(
|
||||
line.angle().abs() < 0.1,
|
||||
"Line angle {} is not horizontal",
|
||||
line.angle()
|
||||
);
|
||||
}
|
||||
}
|
||||
|
||||
#[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 weights: Vec<f32> = vec![1.0; 31];
|
||||
|
||||
let config = LineFitConfig::new().with_max_line_length(1.0);
|
||||
let lines = fit_line_weighted_with_config(&points, &weights, &config);
|
||||
|
||||
// 3m line should split into 3 segments
|
||||
assert_eq!(lines.len(), 3);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -52,8 +52,10 @@ pub use corner_detection::{
|
||||
detect_curvature_corners, detect_hybrid_corners,
|
||||
};
|
||||
pub use line_fitting::{
|
||||
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,
|
||||
FitError, LineFitConfig, compute_range_weights, fit_line, fit_line_checked,
|
||||
fit_line_from_sensor, fit_line_from_sensor_with_config, fit_line_segment, fit_line_weighted,
|
||||
fit_line_weighted_with_config, fit_line_with_config, fitting_error, fitting_error_weighted,
|
||||
max_distance_point, split_line_by_length, split_lines_by_length,
|
||||
};
|
||||
// Re-export from ransac_lines module
|
||||
pub use ransac_lines::{
|
||||
|
||||
@@ -59,7 +59,7 @@ impl FeatureSet {
|
||||
/// Add a line from components.
|
||||
#[inline]
|
||||
pub fn add_line_from_points(&mut self, start: Point2D, end: Point2D) {
|
||||
self.lines.push(start.x, start.y, end.x, end.y, 1);
|
||||
self.lines.push_full(start.x, start.y, end.x, end.y, 1, 0);
|
||||
}
|
||||
|
||||
/// Number of lines.
|
||||
|
||||
@@ -29,6 +29,8 @@ pub struct Line2D {
|
||||
pub end: Point2D,
|
||||
/// Number of times this line has been observed (for merging).
|
||||
pub observation_count: u32,
|
||||
/// Number of scan points that formed this line (for dominance).
|
||||
pub point_count: u32,
|
||||
}
|
||||
|
||||
impl Line2D {
|
||||
@@ -39,6 +41,7 @@ impl Line2D {
|
||||
start,
|
||||
end,
|
||||
observation_count: 1,
|
||||
point_count: 0,
|
||||
}
|
||||
}
|
||||
|
||||
@@ -49,6 +52,37 @@ impl Line2D {
|
||||
start,
|
||||
end,
|
||||
observation_count: count,
|
||||
point_count: 0,
|
||||
}
|
||||
}
|
||||
|
||||
/// Create a line segment with a specific point count.
|
||||
///
|
||||
/// Used when extracting lines from scan points where point_count
|
||||
/// indicates how many scan points formed this line.
|
||||
#[inline]
|
||||
pub fn with_point_count(start: Point2D, end: Point2D, point_count: u32) -> Self {
|
||||
Self {
|
||||
start,
|
||||
end,
|
||||
observation_count: 1,
|
||||
point_count,
|
||||
}
|
||||
}
|
||||
|
||||
/// Create a line segment with all fields specified.
|
||||
#[inline]
|
||||
pub fn full(
|
||||
start: Point2D,
|
||||
end: Point2D,
|
||||
observation_count: u32,
|
||||
point_count: u32,
|
||||
) -> Self {
|
||||
Self {
|
||||
start,
|
||||
end,
|
||||
observation_count,
|
||||
point_count,
|
||||
}
|
||||
}
|
||||
|
||||
@@ -160,10 +194,11 @@ impl Line2D {
|
||||
(self.end.y - self.start.y) * 0.5 * factor,
|
||||
);
|
||||
|
||||
Line2D::with_observation_count(
|
||||
Line2D::full(
|
||||
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,
|
||||
self.point_count,
|
||||
)
|
||||
}
|
||||
|
||||
@@ -284,6 +319,7 @@ impl Line2D {
|
||||
start: pose.transform_point(self.start),
|
||||
end: pose.transform_point(self.end),
|
||||
observation_count: self.observation_count,
|
||||
point_count: self.point_count,
|
||||
}
|
||||
}
|
||||
|
||||
@@ -392,6 +428,7 @@ impl Default for Line2D {
|
||||
start: Point2D::zero(),
|
||||
end: Point2D::zero(),
|
||||
observation_count: 0,
|
||||
point_count: 0,
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@@ -59,6 +59,8 @@ pub struct LineCollection {
|
||||
pub end_ys: Vec<f32>,
|
||||
/// Observation counts for each line.
|
||||
pub observation_counts: Vec<u32>,
|
||||
/// Point counts for each line (scan points that formed the line).
|
||||
pub point_counts: Vec<u32>,
|
||||
|
||||
// === Pre-computed cached values for performance ===
|
||||
/// X components of unit normal vectors.
|
||||
@@ -84,6 +86,7 @@ impl LineCollection {
|
||||
end_xs: Vec::with_capacity(capacity),
|
||||
end_ys: Vec::with_capacity(capacity),
|
||||
observation_counts: Vec::with_capacity(capacity),
|
||||
point_counts: Vec::with_capacity(capacity),
|
||||
normal_xs: Vec::with_capacity(capacity),
|
||||
normal_ys: Vec::with_capacity(capacity),
|
||||
inv_lengths: Vec::with_capacity(capacity),
|
||||
@@ -126,14 +129,23 @@ impl LineCollection {
|
||||
|
||||
/// Add a line to the collection.
|
||||
#[inline]
|
||||
pub fn push(&mut self, start_x: f32, start_y: f32, end_x: f32, end_y: f32, count: u32) {
|
||||
pub fn push_full(
|
||||
&mut self,
|
||||
start_x: f32,
|
||||
start_y: f32,
|
||||
end_x: f32,
|
||||
end_y: f32,
|
||||
obs_count: u32,
|
||||
pt_count: u32,
|
||||
) {
|
||||
let (nx, ny, inv_len) = Self::compute_cached_values(start_x, start_y, end_x, end_y);
|
||||
|
||||
self.start_xs.push(start_x);
|
||||
self.start_ys.push(start_y);
|
||||
self.end_xs.push(end_x);
|
||||
self.end_ys.push(end_y);
|
||||
self.observation_counts.push(count);
|
||||
self.observation_counts.push(obs_count);
|
||||
self.point_counts.push(pt_count);
|
||||
self.normal_xs.push(nx);
|
||||
self.normal_ys.push(ny);
|
||||
self.inv_lengths.push(inv_len);
|
||||
@@ -142,12 +154,13 @@ impl LineCollection {
|
||||
/// Add a Line2D to the collection.
|
||||
#[inline]
|
||||
pub fn push_line(&mut self, line: &Line2D) {
|
||||
self.push(
|
||||
self.push_full(
|
||||
line.start.x,
|
||||
line.start.y,
|
||||
line.end.x,
|
||||
line.end.y,
|
||||
line.observation_count,
|
||||
line.point_count,
|
||||
);
|
||||
}
|
||||
|
||||
@@ -195,10 +208,11 @@ impl LineCollection {
|
||||
#[inline]
|
||||
pub fn get(&self, index: usize) -> Option<Line2D> {
|
||||
if index < self.len() {
|
||||
Some(Line2D::with_observation_count(
|
||||
Some(Line2D::full(
|
||||
Point2D::new(self.start_xs[index], self.start_ys[index]),
|
||||
Point2D::new(self.end_xs[index], self.end_ys[index]),
|
||||
self.observation_counts[index],
|
||||
self.point_counts[index],
|
||||
))
|
||||
} else {
|
||||
None
|
||||
@@ -209,10 +223,11 @@ impl LineCollection {
|
||||
pub fn to_lines(&self) -> Vec<Line2D> {
|
||||
(0..self.len())
|
||||
.map(|i| {
|
||||
Line2D::with_observation_count(
|
||||
Line2D::full(
|
||||
Point2D::new(self.start_xs[i], self.start_ys[i]),
|
||||
Point2D::new(self.end_xs[i], self.end_ys[i]),
|
||||
self.observation_counts[i],
|
||||
self.point_counts[i],
|
||||
)
|
||||
})
|
||||
.collect()
|
||||
@@ -600,6 +615,7 @@ impl LineCollection {
|
||||
result.normal_xs.resize(n, 0.0);
|
||||
result.normal_ys.resize(n, 0.0);
|
||||
result.observation_counts = self.observation_counts.clone();
|
||||
result.point_counts = self.point_counts.clone();
|
||||
// Inverse lengths are preserved by rotation+translation
|
||||
result.inv_lengths = self.inv_lengths.clone();
|
||||
|
||||
@@ -675,6 +691,7 @@ impl LineCollection {
|
||||
self.end_xs.clear();
|
||||
self.end_ys.clear();
|
||||
self.observation_counts.clear();
|
||||
self.point_counts.clear();
|
||||
self.normal_xs.clear();
|
||||
self.normal_ys.clear();
|
||||
self.inv_lengths.clear();
|
||||
@@ -687,6 +704,7 @@ impl LineCollection {
|
||||
self.end_xs.swap_remove(index);
|
||||
self.end_ys.swap_remove(index);
|
||||
self.observation_counts.swap_remove(index);
|
||||
self.point_counts.swap_remove(index);
|
||||
self.normal_xs.swap_remove(index);
|
||||
self.normal_ys.swap_remove(index);
|
||||
self.inv_lengths.swap_remove(index);
|
||||
@@ -695,10 +713,11 @@ impl LineCollection {
|
||||
/// Iterate over lines.
|
||||
pub fn iter(&self) -> impl Iterator<Item = Line2D> + '_ {
|
||||
(0..self.len()).map(move |i| {
|
||||
Line2D::with_observation_count(
|
||||
Line2D::full(
|
||||
Point2D::new(self.start_xs[i], self.start_ys[i]),
|
||||
Point2D::new(self.end_xs[i], self.end_ys[i]),
|
||||
self.observation_counts[i],
|
||||
self.point_counts[i],
|
||||
)
|
||||
})
|
||||
}
|
||||
@@ -729,8 +748,8 @@ mod tests {
|
||||
#[test]
|
||||
fn test_to_lines() {
|
||||
let mut collection = LineCollection::new();
|
||||
collection.push(0.0, 0.0, 1.0, 0.0, 1);
|
||||
collection.push(0.0, 1.0, 1.0, 1.0, 2);
|
||||
collection.push_full(0.0, 0.0, 1.0, 0.0, 1, 10);
|
||||
collection.push_full(0.0, 1.0, 1.0, 1.0, 2, 20);
|
||||
|
||||
let lines = collection.to_lines();
|
||||
|
||||
@@ -738,7 +757,9 @@ mod tests {
|
||||
assert_eq!(lines[0].start, Point2D::new(0.0, 0.0));
|
||||
assert_eq!(lines[0].end, Point2D::new(1.0, 0.0));
|
||||
assert_eq!(lines[0].observation_count, 1);
|
||||
assert_eq!(lines[0].point_count, 10);
|
||||
assert_eq!(lines[1].observation_count, 2);
|
||||
assert_eq!(lines[1].point_count, 20);
|
||||
}
|
||||
|
||||
#[test]
|
||||
|
||||
@@ -66,6 +66,75 @@ impl MergerConfig {
|
||||
}
|
||||
}
|
||||
|
||||
/// Configuration for coplanar line merging optimization.
|
||||
///
|
||||
/// Used by [`optimize_coplanar_lines`] to merge multiple lines
|
||||
/// that lie on the same infinite line (within tolerances).
|
||||
#[derive(Clone, Debug)]
|
||||
pub struct CoplanarMergeConfig {
|
||||
/// Maximum angle difference for lines to be considered coplanar (radians).
|
||||
/// Default: 0.052 rad (~3°)
|
||||
pub max_angle_diff: f32,
|
||||
|
||||
/// Maximum perpendicular distance between lines (meters).
|
||||
/// Default: 0.05m (5cm)
|
||||
pub max_perpendicular_dist: f32,
|
||||
|
||||
/// Maximum gap between line endpoints to allow merging (meters).
|
||||
/// Lines that are coplanar but separated by more than this gap
|
||||
/// will not be merged.
|
||||
/// Default: 0.3m (30cm)
|
||||
pub max_gap: f32,
|
||||
|
||||
/// Minimum point count for a line to become the dominant line.
|
||||
/// Lines with fewer scan points may be absorbed but won't be chosen
|
||||
/// as the base direction for merging.
|
||||
/// Default: 10
|
||||
pub min_dominant_point_count: u32,
|
||||
}
|
||||
|
||||
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
|
||||
min_dominant_point_count: 10,
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
impl CoplanarMergeConfig {
|
||||
/// Create a new configuration with default values.
|
||||
pub fn new() -> Self {
|
||||
Self::default()
|
||||
}
|
||||
|
||||
/// Builder-style setter for maximum angle difference.
|
||||
pub fn with_max_angle_diff(mut self, radians: f32) -> Self {
|
||||
self.max_angle_diff = radians;
|
||||
self
|
||||
}
|
||||
|
||||
/// Builder-style setter for maximum perpendicular distance.
|
||||
pub fn with_max_perpendicular_dist(mut self, meters: f32) -> Self {
|
||||
self.max_perpendicular_dist = meters;
|
||||
self
|
||||
}
|
||||
|
||||
/// Builder-style setter for maximum gap.
|
||||
pub fn with_max_gap(mut self, meters: f32) -> Self {
|
||||
self.max_gap = meters;
|
||||
self
|
||||
}
|
||||
|
||||
/// Builder-style setter for minimum dominant point count.
|
||||
pub fn with_min_dominant_point_count(mut self, count: u32) -> Self {
|
||||
self.min_dominant_point_count = count;
|
||||
self
|
||||
}
|
||||
}
|
||||
|
||||
/// Result of merging a line.
|
||||
#[derive(Clone, Debug)]
|
||||
pub struct MergeResult {
|
||||
@@ -191,11 +260,12 @@ pub fn merge_lines(
|
||||
let final_start = new_start + perp * merged_offset;
|
||||
let final_end = new_end + perp * merged_offset;
|
||||
|
||||
// Create merged line with incremented observation count
|
||||
let merged_line = Line2D::with_observation_count(
|
||||
// Create merged line with incremented observation count and summed point count
|
||||
let merged_line = Line2D::full(
|
||||
final_start,
|
||||
final_end,
|
||||
map_line.observation_count.saturating_add(1),
|
||||
map_line.point_count.saturating_add(scan_line.point_count),
|
||||
);
|
||||
|
||||
MergeResult {
|
||||
@@ -243,8 +313,9 @@ pub fn batch_merge(
|
||||
/// Create a new line from a scan observation (for unmatched scan lines).
|
||||
///
|
||||
/// This is used when adding new features to the map.
|
||||
/// Preserves the scan line's point_count for dominance tracking.
|
||||
pub fn create_new_line(scan_line: &Line2D) -> Line2D {
|
||||
Line2D::with_observation_count(scan_line.start, scan_line.end, 1)
|
||||
Line2D::full(scan_line.start, scan_line.end, 1, scan_line.point_count)
|
||||
}
|
||||
|
||||
/// Merge two line segments that represent the same physical feature.
|
||||
@@ -286,11 +357,270 @@ pub fn merge_collinear_lines(line1: &Line2D, line2: &Line2D) -> Option<Line2D> {
|
||||
let new_start = reference + dir * min_t;
|
||||
let new_end = reference + dir * max_t;
|
||||
|
||||
let count = line1
|
||||
let obs_count = line1
|
||||
.observation_count
|
||||
.saturating_add(line2.observation_count);
|
||||
let pt_count = line1.point_count.saturating_add(line2.point_count);
|
||||
|
||||
Some(Line2D::with_observation_count(new_start, new_end, count))
|
||||
Some(Line2D::full(new_start, new_end, obs_count, pt_count))
|
||||
}
|
||||
|
||||
/// Merge two line segments using configurable thresholds.
|
||||
///
|
||||
/// This is like [`merge_collinear_lines`] but uses explicit configuration
|
||||
/// instead of hardcoded values.
|
||||
pub fn merge_collinear_lines_with_config(
|
||||
line1: &Line2D,
|
||||
line2: &Line2D,
|
||||
config: &CoplanarMergeConfig,
|
||||
) -> Option<Line2D> {
|
||||
if !are_lines_coplanar(line1, line2, config) {
|
||||
return None;
|
||||
}
|
||||
|
||||
// Merge by finding the extended endpoints
|
||||
let dir = line1.unit_direction();
|
||||
let reference = line1.midpoint();
|
||||
|
||||
let project = |p: Point2D| -> f32 { (p - reference).dot(dir) };
|
||||
|
||||
let t1 = project(line1.start);
|
||||
let t2 = project(line1.end);
|
||||
let t3 = project(line2.start);
|
||||
let t4 = project(line2.end);
|
||||
|
||||
let min_t = t1.min(t2).min(t3).min(t4);
|
||||
let max_t = t1.max(t2).max(t3).max(t4);
|
||||
|
||||
let new_start = reference + dir * min_t;
|
||||
let new_end = reference + dir * max_t;
|
||||
|
||||
let obs_count = line1
|
||||
.observation_count
|
||||
.saturating_add(line2.observation_count);
|
||||
let pt_count = line1.point_count.saturating_add(line2.point_count);
|
||||
|
||||
Some(Line2D::full(new_start, new_end, obs_count, pt_count))
|
||||
}
|
||||
|
||||
/// Check if two lines are coplanar (on the same infinite line within tolerances).
|
||||
///
|
||||
/// Two lines are considered coplanar if:
|
||||
/// 1. Their angles differ by less than `max_angle_diff` (handling 180° ambiguity)
|
||||
/// 2. The perpendicular distance between them is less than `max_perpendicular_dist`
|
||||
fn are_lines_coplanar(line1: &Line2D, line2: &Line2D, config: &CoplanarMergeConfig) -> bool {
|
||||
// Check angular compatibility
|
||||
let angle_diff = crate::core::math::angle_diff(line1.angle(), line2.angle()).abs();
|
||||
let angle_diff = angle_diff.min(std::f32::consts::PI - angle_diff);
|
||||
|
||||
if angle_diff > config.max_angle_diff {
|
||||
return false;
|
||||
}
|
||||
|
||||
// Check perpendicular distance using midpoint of line2
|
||||
let dist = line1.distance_to_point(line2.midpoint());
|
||||
dist <= config.max_perpendicular_dist
|
||||
}
|
||||
|
||||
/// Check if two coplanar lines can be merged (overlap or small gap).
|
||||
///
|
||||
/// Lines can be merged if they are coplanar AND either:
|
||||
/// - They overlap, OR
|
||||
/// - The gap between their closest endpoints is less than `max_gap`
|
||||
fn lines_can_merge(line1: &Line2D, line2: &Line2D, config: &CoplanarMergeConfig) -> bool {
|
||||
if !are_lines_coplanar(line1, line2, config) {
|
||||
return false;
|
||||
}
|
||||
|
||||
// Project line2's endpoints onto line1's direction to check overlap/gap
|
||||
let dir = line1.unit_direction();
|
||||
let reference = line1.midpoint();
|
||||
|
||||
let project = |p: Point2D| -> f32 { (p - reference).dot(dir) };
|
||||
|
||||
let t1_min = project(line1.start).min(project(line1.end));
|
||||
let t1_max = project(line1.start).max(project(line1.end));
|
||||
let t2_min = project(line2.start).min(project(line2.end));
|
||||
let t2_max = project(line2.start).max(project(line2.end));
|
||||
|
||||
// Check for overlap
|
||||
if t1_max >= t2_min && t2_max >= t1_min {
|
||||
return true;
|
||||
}
|
||||
|
||||
// Check gap
|
||||
let gap = if t1_max < t2_min {
|
||||
t2_min - t1_max
|
||||
} else {
|
||||
t1_min - t2_max
|
||||
};
|
||||
|
||||
gap <= config.max_gap
|
||||
}
|
||||
|
||||
/// Merge a group of coplanar lines into a single extended line.
|
||||
///
|
||||
/// Uses the line with most scan points as the base direction (for robust dominance).
|
||||
/// Falls back to observation_count if no line has point_count set.
|
||||
/// Both observation_count and point_count are summed.
|
||||
///
|
||||
/// # Panics
|
||||
/// Panics if `lines` is empty.
|
||||
fn merge_coplanar_group(lines: &[&Line2D]) -> Line2D {
|
||||
assert!(!lines.is_empty(), "Cannot merge empty group");
|
||||
|
||||
if lines.len() == 1 {
|
||||
return *lines[0];
|
||||
}
|
||||
|
||||
// Find the dominant line (most scan points, fallback to observation count)
|
||||
let dominant = lines
|
||||
.iter()
|
||||
.max_by_key(|l| {
|
||||
// Use point_count if available, otherwise fall back to observation_count
|
||||
if l.point_count > 0 {
|
||||
l.point_count
|
||||
} else {
|
||||
l.observation_count
|
||||
}
|
||||
})
|
||||
.unwrap();
|
||||
|
||||
let dir = dominant.unit_direction();
|
||||
let reference = dominant.midpoint();
|
||||
|
||||
let project = |p: Point2D| -> f32 { (p - reference).dot(dir) };
|
||||
|
||||
// Find extent across all lines and sum counts
|
||||
let mut min_t = f32::MAX;
|
||||
let mut max_t = f32::MIN;
|
||||
let mut total_obs_count: u32 = 0;
|
||||
let mut total_pt_count: u32 = 0;
|
||||
|
||||
for line in lines {
|
||||
let t_start = project(line.start);
|
||||
let t_end = project(line.end);
|
||||
min_t = min_t.min(t_start).min(t_end);
|
||||
max_t = max_t.max(t_start).max(t_end);
|
||||
total_obs_count = total_obs_count.saturating_add(line.observation_count);
|
||||
total_pt_count = total_pt_count.saturating_add(line.point_count);
|
||||
}
|
||||
|
||||
let new_start = reference + dir * min_t;
|
||||
let new_end = reference + dir * max_t;
|
||||
|
||||
Line2D::full(new_start, new_end, total_obs_count, total_pt_count)
|
||||
}
|
||||
|
||||
/// Optimize map lines by merging coplanar segments.
|
||||
///
|
||||
/// Groups lines by coplanarity (similar angle + perpendicular offset) and merges
|
||||
/// each group into a single extended line using the most-observed line as the base.
|
||||
///
|
||||
/// # Arguments
|
||||
/// * `lines` - Mutable vector of lines to optimize (modified in place)
|
||||
/// * `config` - Coplanar merge configuration
|
||||
///
|
||||
/// # Returns
|
||||
/// Number of lines merged (removed from the vector)
|
||||
///
|
||||
/// # Example
|
||||
/// ```rust,ignore
|
||||
/// use vastu_map::integration::{optimize_coplanar_lines, CoplanarMergeConfig};
|
||||
///
|
||||
/// let mut lines = vec![...]; // Multiple lines on the same wall
|
||||
/// let config = CoplanarMergeConfig::default();
|
||||
/// let merged_count = optimize_coplanar_lines(&mut lines, &config);
|
||||
/// println!("Merged {} lines", merged_count);
|
||||
/// ```
|
||||
pub fn optimize_coplanar_lines(lines: &mut Vec<Line2D>, config: &CoplanarMergeConfig) -> usize {
|
||||
if lines.len() < 2 {
|
||||
return 0;
|
||||
}
|
||||
|
||||
let n = lines.len();
|
||||
|
||||
// Union-Find for grouping coplanar lines
|
||||
let mut parent: Vec<usize> = (0..n).collect();
|
||||
let mut rank: Vec<usize> = vec![0; n];
|
||||
|
||||
fn find(parent: &mut [usize], i: usize) -> usize {
|
||||
if parent[i] != i {
|
||||
parent[i] = find(parent, parent[i]);
|
||||
}
|
||||
parent[i]
|
||||
}
|
||||
|
||||
fn union(parent: &mut [usize], rank: &mut [usize], i: usize, j: usize) {
|
||||
let pi = find(parent, i);
|
||||
let pj = find(parent, j);
|
||||
if pi != pj {
|
||||
if rank[pi] < rank[pj] {
|
||||
parent[pi] = pj;
|
||||
} else if rank[pi] > rank[pj] {
|
||||
parent[pj] = pi;
|
||||
} else {
|
||||
parent[pj] = pi;
|
||||
rank[pi] += 1;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Group coplanar lines that can be merged
|
||||
for i in 0..n {
|
||||
for j in (i + 1)..n {
|
||||
if lines_can_merge(&lines[i], &lines[j], config) {
|
||||
union(&mut parent, &mut rank, i, j);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Collect groups
|
||||
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);
|
||||
}
|
||||
|
||||
// Process each group and build result
|
||||
let mut result: Vec<Line2D> = Vec::with_capacity(n);
|
||||
let mut merged_count = 0;
|
||||
|
||||
for (_root, indices) in groups {
|
||||
if indices.len() == 1 {
|
||||
// Single line, keep as-is
|
||||
result.push(lines[indices[0]]);
|
||||
} else {
|
||||
// Check if any line in the group has enough scan points to be dominant
|
||||
let has_dominant = indices.iter().any(|&i| {
|
||||
let line = &lines[i];
|
||||
// Use point_count if available, fall back to observation_count
|
||||
let confidence = if line.point_count > 0 {
|
||||
line.point_count
|
||||
} else {
|
||||
line.observation_count
|
||||
};
|
||||
confidence >= config.min_dominant_point_count
|
||||
});
|
||||
|
||||
if !has_dominant {
|
||||
// No dominant line, keep all lines as-is
|
||||
for &idx in &indices {
|
||||
result.push(lines[idx]);
|
||||
}
|
||||
} else {
|
||||
// Merge the group
|
||||
let group_lines: Vec<&Line2D> = indices.iter().map(|&i| &lines[i]).collect();
|
||||
let merged = merge_coplanar_group(&group_lines);
|
||||
result.push(merged);
|
||||
merged_count += indices.len() - 1;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
*lines = result;
|
||||
merged_count
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
@@ -418,4 +748,297 @@ mod tests {
|
||||
|
||||
assert!(merged.is_none());
|
||||
}
|
||||
|
||||
// ===== Coplanar merge config tests =====
|
||||
|
||||
#[test]
|
||||
fn test_coplanar_merge_config_default() {
|
||||
let config = CoplanarMergeConfig::default();
|
||||
|
||||
assert_relative_eq!(config.max_angle_diff, 0.052, epsilon = 0.001);
|
||||
assert_relative_eq!(config.max_perpendicular_dist, 0.05, epsilon = 0.001);
|
||||
assert_relative_eq!(config.max_gap, 0.3, epsilon = 0.001);
|
||||
assert_eq!(config.min_dominant_point_count, 10);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_coplanar_merge_config_builders() {
|
||||
let config = CoplanarMergeConfig::new()
|
||||
.with_max_angle_diff(0.2)
|
||||
.with_max_perpendicular_dist(0.08)
|
||||
.with_max_gap(1.0)
|
||||
.with_min_dominant_point_count(5);
|
||||
|
||||
assert_relative_eq!(config.max_angle_diff, 0.2, epsilon = 0.001);
|
||||
assert_relative_eq!(config.max_perpendicular_dist, 0.08, epsilon = 0.001);
|
||||
assert_relative_eq!(config.max_gap, 1.0, epsilon = 0.001);
|
||||
assert_eq!(config.min_dominant_point_count, 5);
|
||||
}
|
||||
|
||||
// ===== are_lines_coplanar tests =====
|
||||
|
||||
#[test]
|
||||
fn test_are_lines_coplanar_same_line() {
|
||||
let line1 = Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(5.0, 0.0));
|
||||
let line2 = Line2D::new(Point2D::new(3.0, 0.0), Point2D::new(8.0, 0.0));
|
||||
let config = CoplanarMergeConfig::default();
|
||||
|
||||
assert!(are_lines_coplanar(&line1, &line2, &config));
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_are_lines_coplanar_parallel_close() {
|
||||
let line1 = Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(5.0, 0.0));
|
||||
let line2 = Line2D::new(Point2D::new(0.0, 0.05), Point2D::new(5.0, 0.05)); // 5cm offset
|
||||
let config = CoplanarMergeConfig::default();
|
||||
|
||||
assert!(are_lines_coplanar(&line1, &line2, &config));
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_are_lines_coplanar_rejects_angle() {
|
||||
let line1 = Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(5.0, 0.0));
|
||||
let line2 = Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(5.0, 1.0)); // ~11° angle
|
||||
let config = CoplanarMergeConfig::default();
|
||||
|
||||
assert!(!are_lines_coplanar(&line1, &line2, &config));
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_are_lines_coplanar_rejects_distance() {
|
||||
let line1 = Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(5.0, 0.0));
|
||||
let line2 = Line2D::new(Point2D::new(0.0, 0.5), Point2D::new(5.0, 0.5)); // 50cm offset
|
||||
let config = CoplanarMergeConfig::default();
|
||||
|
||||
assert!(!are_lines_coplanar(&line1, &line2, &config));
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_are_lines_coplanar_opposite_direction() {
|
||||
// Lines pointing in opposite directions should still be coplanar
|
||||
let line1 = Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(5.0, 0.0));
|
||||
let line2 = Line2D::new(Point2D::new(10.0, 0.0), Point2D::new(6.0, 0.0)); // Opposite direction
|
||||
let config = CoplanarMergeConfig::default();
|
||||
|
||||
assert!(are_lines_coplanar(&line1, &line2, &config));
|
||||
}
|
||||
|
||||
// ===== lines_can_merge tests =====
|
||||
|
||||
#[test]
|
||||
fn test_lines_can_merge_overlapping() {
|
||||
let line1 = Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(5.0, 0.0));
|
||||
let line2 = Line2D::new(Point2D::new(3.0, 0.0), Point2D::new(8.0, 0.0));
|
||||
let config = CoplanarMergeConfig::default();
|
||||
|
||||
assert!(lines_can_merge(&line1, &line2, &config));
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_lines_can_merge_with_small_gap() {
|
||||
let line1 = Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(3.0, 0.0));
|
||||
let line2 = Line2D::new(Point2D::new(3.3, 0.0), Point2D::new(6.0, 0.0)); // 0.3m gap
|
||||
let config = CoplanarMergeConfig::default();
|
||||
|
||||
assert!(lines_can_merge(&line1, &line2, &config));
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_lines_can_merge_rejects_large_gap() {
|
||||
let line1 = Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(3.0, 0.0));
|
||||
let line2 = Line2D::new(Point2D::new(4.0, 0.0), Point2D::new(7.0, 0.0)); // 1.0m gap
|
||||
let config = CoplanarMergeConfig::default();
|
||||
|
||||
assert!(!lines_can_merge(&line1, &line2, &config));
|
||||
}
|
||||
|
||||
// ===== optimize_coplanar_lines tests =====
|
||||
|
||||
#[test]
|
||||
fn test_optimize_coplanar_lines_merges_overlapping() {
|
||||
let mut lines = vec![
|
||||
Line2D::with_point_count(
|
||||
Point2D::new(0.0, 0.0),
|
||||
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,
|
||||
),
|
||||
];
|
||||
let config = CoplanarMergeConfig::default();
|
||||
|
||||
let merged_count = optimize_coplanar_lines(&mut lines, &config);
|
||||
|
||||
assert_eq!(merged_count, 1);
|
||||
assert_eq!(lines.len(), 1);
|
||||
assert_relative_eq!(lines[0].length(), 5.0, epsilon = 0.01);
|
||||
assert_eq!(lines[0].point_count, 27); // 15 + 12
|
||||
}
|
||||
|
||||
#[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(2.2, 0.0), // 0.2m gap, within default 0.3m
|
||||
Point2D::new(5.0, 0.0),
|
||||
15,
|
||||
),
|
||||
];
|
||||
let config = CoplanarMergeConfig::default();
|
||||
|
||||
let merged_count = optimize_coplanar_lines(&mut lines, &config);
|
||||
|
||||
assert_eq!(merged_count, 1);
|
||||
assert_eq!(lines.len(), 1);
|
||||
assert_relative_eq!(lines[0].length(), 5.0, epsilon = 0.01);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_optimize_coplanar_lines_uses_dominant() {
|
||||
// The line with most points should determine the direction
|
||||
let mut lines = vec![
|
||||
Line2D::with_point_count(
|
||||
Point2D::new(0.0, 0.0),
|
||||
Point2D::new(2.0, 0.0),
|
||||
25, // Dominant (most points)
|
||||
),
|
||||
Line2D::with_point_count(
|
||||
Point2D::new(2.0, 0.0),
|
||||
Point2D::new(4.0, 0.01), // Slightly off
|
||||
10,
|
||||
),
|
||||
];
|
||||
let config = CoplanarMergeConfig::default();
|
||||
|
||||
let merged_count = optimize_coplanar_lines(&mut lines, &config);
|
||||
|
||||
assert_eq!(merged_count, 1);
|
||||
assert_eq!(lines.len(), 1);
|
||||
// The merged line should use the dominant line's direction
|
||||
// So it should be close to horizontal
|
||||
assert!(lines[0].angle().abs() < 0.02);
|
||||
}
|
||||
|
||||
#[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(4.0, 0.0), // 2.0m gap, above default 0.3m
|
||||
Point2D::new(6.0, 0.0),
|
||||
15,
|
||||
),
|
||||
];
|
||||
let config = CoplanarMergeConfig::default(); // max_gap = 0.3m
|
||||
|
||||
let merged_count = optimize_coplanar_lines(&mut lines, &config);
|
||||
|
||||
assert_eq!(merged_count, 0);
|
||||
assert_eq!(lines.len(), 2);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_optimize_coplanar_lines_keeps_non_dominant() {
|
||||
// When no line has enough points, don't merge
|
||||
let mut lines = vec![
|
||||
Line2D::with_point_count(
|
||||
Point2D::new(0.0, 0.0),
|
||||
Point2D::new(2.0, 0.0),
|
||||
5, // Below threshold of 10
|
||||
),
|
||||
Line2D::with_point_count(
|
||||
Point2D::new(2.0, 0.0),
|
||||
Point2D::new(4.0, 0.0),
|
||||
5, // Below threshold of 10
|
||||
),
|
||||
];
|
||||
let config = CoplanarMergeConfig::default(); // min_dominant_point_count = 10
|
||||
|
||||
let merged_count = optimize_coplanar_lines(&mut lines, &config);
|
||||
|
||||
assert_eq!(merged_count, 0);
|
||||
assert_eq!(lines.len(), 2);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_optimize_coplanar_lines_multiple_groups() {
|
||||
// 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,
|
||||
),
|
||||
// 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,
|
||||
),
|
||||
];
|
||||
let config = CoplanarMergeConfig::default();
|
||||
|
||||
let merged_count = optimize_coplanar_lines(&mut lines, &config);
|
||||
|
||||
assert_eq!(merged_count, 2); // Merged 2 lines (one from each group)
|
||||
assert_eq!(lines.len(), 2); // Two merged lines remain
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_optimize_coplanar_lines_empty() {
|
||||
let mut lines: Vec<Line2D> = vec![];
|
||||
let config = CoplanarMergeConfig::default();
|
||||
|
||||
let merged_count = optimize_coplanar_lines(&mut lines, &config);
|
||||
|
||||
assert_eq!(merged_count, 0);
|
||||
assert_eq!(lines.len(), 0);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_optimize_coplanar_lines_single() {
|
||||
let mut lines = vec![Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(5.0, 0.0))];
|
||||
let config = CoplanarMergeConfig::default();
|
||||
|
||||
let merged_count = optimize_coplanar_lines(&mut lines, &config);
|
||||
|
||||
assert_eq!(merged_count, 0);
|
||||
assert_eq!(lines.len(), 1);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_merge_collinear_lines_with_config() {
|
||||
let line1 = Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(3.0, 0.0));
|
||||
let line2 = Line2D::new(Point2D::new(2.0, 0.0), Point2D::new(5.0, 0.0));
|
||||
let config = CoplanarMergeConfig::default();
|
||||
|
||||
let merged = merge_collinear_lines_with_config(&line1, &line2, &config);
|
||||
|
||||
assert!(merged.is_some());
|
||||
let merged = merged.unwrap();
|
||||
assert_relative_eq!(merged.length(), 5.0, epsilon = 0.01);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -41,6 +41,9 @@
|
||||
|
||||
pub mod association;
|
||||
pub mod merger;
|
||||
pub mod point_association;
|
||||
pub mod refitter;
|
||||
pub mod scan_store;
|
||||
pub mod spatial_index;
|
||||
|
||||
// Re-export main types
|
||||
@@ -49,6 +52,13 @@ pub use association::{
|
||||
find_unmatched_scan_lines,
|
||||
};
|
||||
pub use merger::{
|
||||
MergeResult, MergerConfig, batch_merge, create_new_line, merge_collinear_lines, merge_lines,
|
||||
CoplanarMergeConfig, MergeResult, MergerConfig, batch_merge, create_new_line,
|
||||
merge_collinear_lines, merge_collinear_lines_with_config, merge_lines, optimize_coplanar_lines,
|
||||
};
|
||||
pub use point_association::{
|
||||
AssociatedPoint, PointAssociationConfig, PointAssociationResult, associate_points_to_lines,
|
||||
associate_scan_points, extract_points_for_line, lines_with_enough_points,
|
||||
};
|
||||
pub use refitter::{RefitConfig, RefitResult, RefitStats, refit_line, refit_line_simple};
|
||||
pub use scan_store::{ScanStore, ScanStoreConfig, ScanStoreStats, StoredScan};
|
||||
pub use spatial_index::{IndexedLine, SpatialIndex};
|
||||
|
||||
@@ -0,0 +1,550 @@
|
||||
//! Point-to-line association for line re-fitting.
|
||||
//!
|
||||
//! Associates world-frame points from ScanStore with existing map lines,
|
||||
//! enabling periodic re-fitting of lines from accumulated point clouds.
|
||||
|
||||
use crate::core::Point2D;
|
||||
use crate::features::Line2D;
|
||||
use crate::integration::{ScanStore, SpatialIndex};
|
||||
|
||||
/// Configuration for point-to-line association.
|
||||
#[derive(Clone, Debug)]
|
||||
pub struct PointAssociationConfig {
|
||||
/// Maximum perpendicular distance from line to associate a point (meters).
|
||||
/// Points further than this from all lines are unassociated.
|
||||
/// Default: 0.10m (10cm)
|
||||
pub max_distance: f32,
|
||||
|
||||
/// Maximum projection extension beyond line endpoints.
|
||||
/// Points can project up to this fraction beyond line ends.
|
||||
/// 0.0 = strict segment bounds, 0.2 = 20% extension allowed.
|
||||
/// Default: 0.1 (10% extension)
|
||||
pub max_projection_extension: f32,
|
||||
}
|
||||
|
||||
impl Default for PointAssociationConfig {
|
||||
fn default() -> Self {
|
||||
Self {
|
||||
max_distance: 0.10,
|
||||
max_projection_extension: 0.1,
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
impl PointAssociationConfig {
|
||||
/// Create a new configuration with defaults.
|
||||
pub fn new() -> Self {
|
||||
Self::default()
|
||||
}
|
||||
|
||||
/// Set maximum association distance.
|
||||
pub fn with_max_distance(mut self, meters: f32) -> Self {
|
||||
self.max_distance = meters;
|
||||
self
|
||||
}
|
||||
|
||||
/// Set maximum projection extension.
|
||||
pub fn with_max_projection_extension(mut self, fraction: f32) -> Self {
|
||||
self.max_projection_extension = fraction;
|
||||
self
|
||||
}
|
||||
}
|
||||
|
||||
/// A point associated with a map line.
|
||||
#[derive(Clone, Debug)]
|
||||
pub struct AssociatedPoint {
|
||||
/// World-frame point.
|
||||
pub point: Point2D,
|
||||
/// Scan ID the point came from.
|
||||
pub scan_id: u32,
|
||||
/// Perpendicular distance to the line.
|
||||
pub distance: f32,
|
||||
/// Projection parameter on the line (0=start, 1=end).
|
||||
pub projection_t: f32,
|
||||
}
|
||||
|
||||
/// Result of point-to-line association.
|
||||
#[derive(Clone, Debug)]
|
||||
pub struct PointAssociationResult {
|
||||
/// Points associated with each map line, indexed by line index.
|
||||
pub line_points: Vec<Vec<AssociatedPoint>>,
|
||||
/// Number of points associated.
|
||||
pub total_associated: usize,
|
||||
/// Number of points not associated (too far from all lines).
|
||||
pub total_unassociated: usize,
|
||||
}
|
||||
|
||||
impl PointAssociationResult {
|
||||
/// Get the total number of points processed.
|
||||
pub fn total_points(&self) -> usize {
|
||||
self.total_associated + self.total_unassociated
|
||||
}
|
||||
|
||||
/// Get association rate (0.0 - 1.0).
|
||||
pub fn association_rate(&self) -> f32 {
|
||||
let total = self.total_points();
|
||||
if total == 0 {
|
||||
0.0
|
||||
} else {
|
||||
self.total_associated as f32 / total as f32
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// Associate all points from ScanStore with map lines.
|
||||
///
|
||||
/// Each point is associated with its nearest line if within `max_distance`
|
||||
/// and within projection bounds (considering extension).
|
||||
///
|
||||
/// # Arguments
|
||||
/// * `scan_store` - Storage containing accumulated scans
|
||||
/// * `lines` - Map lines to associate points with
|
||||
/// * `index` - Spatial index for efficient line lookup
|
||||
/// * `config` - Association configuration
|
||||
///
|
||||
/// # Returns
|
||||
/// Association result with points grouped by line index.
|
||||
pub fn associate_points_to_lines(
|
||||
scan_store: &ScanStore,
|
||||
lines: &[Line2D],
|
||||
index: &SpatialIndex,
|
||||
config: &PointAssociationConfig,
|
||||
) -> PointAssociationResult {
|
||||
let num_lines = lines.len();
|
||||
let mut line_points: Vec<Vec<AssociatedPoint>> = vec![Vec::new(); num_lines];
|
||||
let mut total_associated = 0usize;
|
||||
let mut total_unassociated = 0usize;
|
||||
|
||||
// Iterate through all scans and their world-frame points
|
||||
for scan in scan_store.iter() {
|
||||
let pose = &scan.estimated_pose;
|
||||
|
||||
for robot_point in scan.points() {
|
||||
// Transform to world frame
|
||||
let world_point = pose.transform_point(*robot_point);
|
||||
|
||||
// Find nearest line using spatial index
|
||||
if let Some((line_idx, distance)) = index.nearest_line(world_point) {
|
||||
// Check distance threshold
|
||||
if distance <= config.max_distance && line_idx < num_lines {
|
||||
let line = &lines[line_idx];
|
||||
|
||||
// Check projection bounds
|
||||
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: scan.scan_id,
|
||||
distance,
|
||||
projection_t: t,
|
||||
});
|
||||
total_associated += 1;
|
||||
continue;
|
||||
}
|
||||
}
|
||||
}
|
||||
total_unassociated += 1;
|
||||
}
|
||||
}
|
||||
|
||||
PointAssociationResult {
|
||||
line_points,
|
||||
total_associated,
|
||||
total_unassociated,
|
||||
}
|
||||
}
|
||||
|
||||
/// Associate points from a single scan with map lines.
|
||||
///
|
||||
/// Similar to `associate_points_to_lines` but for a single scan.
|
||||
/// Useful for incremental association during observations.
|
||||
pub fn associate_scan_points(
|
||||
points: &[Point2D],
|
||||
scan_id: u32,
|
||||
lines: &[Line2D],
|
||||
index: &SpatialIndex,
|
||||
config: &PointAssociationConfig,
|
||||
) -> PointAssociationResult {
|
||||
let num_lines = lines.len();
|
||||
let mut line_points: Vec<Vec<AssociatedPoint>> = vec![Vec::new(); num_lines];
|
||||
let mut total_associated = 0usize;
|
||||
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;
|
||||
}
|
||||
}
|
||||
}
|
||||
total_unassociated += 1;
|
||||
}
|
||||
|
||||
PointAssociationResult {
|
||||
line_points,
|
||||
total_associated,
|
||||
total_unassociated,
|
||||
}
|
||||
}
|
||||
|
||||
/// Get line indices that have enough points for re-fitting.
|
||||
///
|
||||
/// Returns indices of lines with at least `min_points` associated points.
|
||||
pub fn lines_with_enough_points(result: &PointAssociationResult, min_points: usize) -> Vec<usize> {
|
||||
result
|
||||
.line_points
|
||||
.iter()
|
||||
.enumerate()
|
||||
.filter(|(_, points)| points.len() >= min_points)
|
||||
.map(|(idx, _)| idx)
|
||||
.collect()
|
||||
}
|
||||
|
||||
/// 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> {
|
||||
result
|
||||
.line_points
|
||||
.get(line_idx)
|
||||
.map(|pts| pts.iter().map(|ap| ap.point).collect())
|
||||
.unwrap_or_default()
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
mod tests {
|
||||
use super::*;
|
||||
use crate::core::{PointCloud2D, Pose2D};
|
||||
|
||||
fn make_horizontal_line(y: f32, x_start: f32, x_end: f32) -> Line2D {
|
||||
Line2D::new(Point2D::new(x_start, y), Point2D::new(x_end, y))
|
||||
}
|
||||
|
||||
fn make_vertical_line(x: f32, y_start: f32, y_end: f32) -> Line2D {
|
||||
Line2D::new(Point2D::new(x, y_start), Point2D::new(x, y_end))
|
||||
}
|
||||
|
||||
fn make_scan_store_with_points(points: &[(f32, f32)]) -> ScanStore {
|
||||
use crate::integration::ScanStoreConfig;
|
||||
|
||||
let mut store = ScanStore::new(ScanStoreConfig::default());
|
||||
let xs: Vec<f32> = points.iter().map(|(x, _)| *x).collect();
|
||||
let ys: Vec<f32> = points.iter().map(|(_, y)| *y).collect();
|
||||
let cloud = PointCloud2D { xs, ys };
|
||||
|
||||
// Use identity pose so robot-frame = world-frame
|
||||
store.add_scan(Pose2D::identity(), Pose2D::identity(), &cloud, 0, 1.0);
|
||||
store
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_config_defaults() {
|
||||
let config = PointAssociationConfig::default();
|
||||
assert!((config.max_distance - 0.10).abs() < 0.001);
|
||||
assert!((config.max_projection_extension - 0.1).abs() < 0.001);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_config_builder() {
|
||||
let config = PointAssociationConfig::new()
|
||||
.with_max_distance(0.15)
|
||||
.with_max_projection_extension(0.2);
|
||||
assert!((config.max_distance - 0.15).abs() < 0.001);
|
||||
assert!((config.max_projection_extension - 0.2).abs() < 0.001);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_empty_inputs() {
|
||||
let store = ScanStore::default();
|
||||
let lines: Vec<Line2D> = vec![];
|
||||
let index = SpatialIndex::empty();
|
||||
let config = PointAssociationConfig::default();
|
||||
|
||||
let result = associate_points_to_lines(&store, &lines, &index, &config);
|
||||
assert_eq!(result.total_associated, 0);
|
||||
assert_eq!(result.total_unassociated, 0);
|
||||
assert!(result.line_points.is_empty());
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_points_associate_to_nearest_line() {
|
||||
// Create two horizontal lines
|
||||
let lines = vec![
|
||||
make_horizontal_line(0.0, 0.0, 10.0), // y=0
|
||||
make_horizontal_line(5.0, 0.0, 10.0), // y=5
|
||||
];
|
||||
let index = SpatialIndex::new(&lines);
|
||||
let config = PointAssociationConfig::default().with_max_distance(0.5);
|
||||
|
||||
// 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
|
||||
]);
|
||||
|
||||
let result = associate_points_to_lines(&store, &lines, &index, &config);
|
||||
|
||||
assert_eq!(result.total_associated, 3);
|
||||
assert_eq!(result.line_points[0].len(), 2); // Two points near y=0
|
||||
assert_eq!(result.line_points[1].len(), 1); // One point near y=5
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_points_outside_max_distance() {
|
||||
let lines = vec![make_horizontal_line(0.0, 0.0, 10.0)];
|
||||
let index = SpatialIndex::new(&lines);
|
||||
let config = PointAssociationConfig::default().with_max_distance(0.1);
|
||||
|
||||
// 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
|
||||
]);
|
||||
|
||||
let result = associate_points_to_lines(&store, &lines, &index, &config);
|
||||
|
||||
assert_eq!(result.total_associated, 0);
|
||||
assert_eq!(result.total_unassociated, 2);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_points_outside_projection_bounds() {
|
||||
let lines = vec![make_horizontal_line(0.0, 0.0, 10.0)];
|
||||
let index = SpatialIndex::new(&lines);
|
||||
// Strict projection bounds (no extension)
|
||||
let config = PointAssociationConfig::default()
|
||||
.with_max_distance(0.5)
|
||||
.with_max_projection_extension(0.0);
|
||||
|
||||
// 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)
|
||||
]);
|
||||
|
||||
let result = associate_points_to_lines(&store, &lines, &index, &config);
|
||||
|
||||
assert_eq!(result.total_associated, 1);
|
||||
assert_eq!(result.total_unassociated, 2);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_projection_extension() {
|
||||
// Line from (0,0) to (10,0), length 10
|
||||
let lines = vec![make_horizontal_line(0.0, 0.0, 10.0)];
|
||||
let index = SpatialIndex::new(&lines);
|
||||
// Allow 10% extension = 1m beyond endpoints
|
||||
let config = PointAssociationConfig::default()
|
||||
.with_max_distance(0.5)
|
||||
.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
|
||||
]);
|
||||
|
||||
let result = associate_points_to_lines(&store, &lines, &index, &config);
|
||||
|
||||
assert_eq!(result.total_associated, 2);
|
||||
assert_eq!(result.total_unassociated, 1);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_associated_point_metadata() {
|
||||
let lines = vec![make_horizontal_line(0.0, 0.0, 10.0)];
|
||||
let index = SpatialIndex::new(&lines);
|
||||
let config = PointAssociationConfig::default().with_max_distance(0.5);
|
||||
|
||||
let store = make_scan_store_with_points(&[(5.0, 0.1)]);
|
||||
|
||||
let result = associate_points_to_lines(&store, &lines, &index, &config);
|
||||
|
||||
assert_eq!(result.line_points[0].len(), 1);
|
||||
let ap = &result.line_points[0][0];
|
||||
assert!((ap.point.x - 5.0).abs() < 0.01);
|
||||
assert!((ap.point.y - 0.1).abs() < 0.01);
|
||||
assert_eq!(ap.scan_id, 0);
|
||||
assert!((ap.distance - 0.1).abs() < 0.01); // Perpendicular distance
|
||||
assert!((ap.projection_t - 0.5).abs() < 0.01); // Midpoint of line
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_multiple_scans() {
|
||||
use crate::integration::ScanStoreConfig;
|
||||
|
||||
let lines = vec![make_horizontal_line(0.0, 0.0, 10.0)];
|
||||
let index = SpatialIndex::new(&lines);
|
||||
let config = PointAssociationConfig::default().with_max_distance(0.5);
|
||||
|
||||
let mut store = ScanStore::new(ScanStoreConfig::default());
|
||||
|
||||
// Add first scan
|
||||
let cloud1 = PointCloud2D {
|
||||
xs: vec![2.0, 3.0],
|
||||
ys: vec![0.05, 0.05],
|
||||
};
|
||||
store.add_scan(Pose2D::identity(), Pose2D::identity(), &cloud1, 0, 1.0);
|
||||
|
||||
// Add second scan
|
||||
let cloud2 = PointCloud2D {
|
||||
xs: vec![7.0, 8.0],
|
||||
ys: vec![0.05, 0.05],
|
||||
};
|
||||
store.add_scan(Pose2D::identity(), Pose2D::identity(), &cloud2, 0, 1.0);
|
||||
|
||||
let result = associate_points_to_lines(&store, &lines, &index, &config);
|
||||
|
||||
assert_eq!(result.total_associated, 4);
|
||||
assert_eq!(result.line_points[0].len(), 4);
|
||||
|
||||
// Verify scan IDs are preserved
|
||||
let scan_ids: Vec<u32> = result.line_points[0].iter().map(|ap| ap.scan_id).collect();
|
||||
assert!(scan_ids.contains(&0));
|
||||
assert!(scan_ids.contains(&1));
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_transformed_poses() {
|
||||
use crate::integration::ScanStoreConfig;
|
||||
|
||||
// Line at y=10 in world frame
|
||||
let lines = vec![make_horizontal_line(10.0, 0.0, 10.0)];
|
||||
let index = SpatialIndex::new(&lines);
|
||||
let config = PointAssociationConfig::default().with_max_distance(0.5);
|
||||
|
||||
let mut store = ScanStore::new(ScanStoreConfig::default());
|
||||
|
||||
// Robot-frame points at (5, 0) - directly in front of robot
|
||||
let cloud = PointCloud2D {
|
||||
xs: vec![5.0],
|
||||
ys: vec![0.0],
|
||||
};
|
||||
|
||||
// Robot at (0, 10), facing +X -> world point at (5, 10)
|
||||
let pose = Pose2D::new(0.0, 10.0, 0.0);
|
||||
store.add_scan(Pose2D::identity(), pose, &cloud, 0, 1.0);
|
||||
|
||||
let result = associate_points_to_lines(&store, &lines, &index, &config);
|
||||
|
||||
// Point at (5, 10) should associate with line at y=10
|
||||
assert_eq!(result.total_associated, 1);
|
||||
let ap = &result.line_points[0][0];
|
||||
assert!((ap.point.x - 5.0).abs() < 0.01);
|
||||
assert!((ap.point.y - 10.0).abs() < 0.01);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_associate_scan_points() {
|
||||
let lines = vec![
|
||||
make_horizontal_line(0.0, 0.0, 10.0),
|
||||
make_vertical_line(10.0, 0.0, 10.0),
|
||||
];
|
||||
let index = SpatialIndex::new(&lines);
|
||||
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
|
||||
];
|
||||
|
||||
let result = associate_scan_points(&points, 42, &lines, &index, &config);
|
||||
|
||||
assert_eq!(result.total_associated, 2);
|
||||
assert_eq!(result.total_unassociated, 1);
|
||||
assert_eq!(result.line_points[0].len(), 1);
|
||||
assert_eq!(result.line_points[1].len(), 1);
|
||||
assert_eq!(result.line_points[0][0].scan_id, 42);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_lines_with_enough_points() {
|
||||
let lines = vec![
|
||||
make_horizontal_line(0.0, 0.0, 10.0),
|
||||
make_horizontal_line(5.0, 0.0, 10.0),
|
||||
make_horizontal_line(10.0, 0.0, 10.0),
|
||||
];
|
||||
let index = SpatialIndex::new(&lines);
|
||||
let config = PointAssociationConfig::default().with_max_distance(0.5);
|
||||
|
||||
// Create points: 5 near line 0, 3 near line 1, 10 near line 2
|
||||
let mut points = vec![];
|
||||
for i in 0..5 {
|
||||
points.push((i as f32, 0.05)); // Near line 0
|
||||
}
|
||||
for i in 0..3 {
|
||||
points.push((i as f32, 5.05)); // Near line 1
|
||||
}
|
||||
for i in 0..10 {
|
||||
points.push((i as f32, 10.05)); // Near line 2
|
||||
}
|
||||
|
||||
let store = make_scan_store_with_points(&points);
|
||||
let result = associate_points_to_lines(&store, &lines, &index, &config);
|
||||
|
||||
// Lines with at least 5 points
|
||||
let enough = lines_with_enough_points(&result, 5);
|
||||
assert_eq!(enough.len(), 2);
|
||||
assert!(enough.contains(&0));
|
||||
assert!(enough.contains(&2));
|
||||
assert!(!enough.contains(&1)); // Only 3 points
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_extract_points_for_line() {
|
||||
let lines = vec![make_horizontal_line(0.0, 0.0, 10.0)];
|
||||
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 result = associate_points_to_lines(&store, &lines, &index, &config);
|
||||
let points = extract_points_for_line(&result, 0);
|
||||
|
||||
assert_eq!(points.len(), 3);
|
||||
assert!((points[0].x - 2.0).abs() < 0.01);
|
||||
assert!((points[1].x - 5.0).abs() < 0.01);
|
||||
assert!((points[2].x - 8.0).abs() < 0.01);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_association_rate() {
|
||||
let lines = vec![make_horizontal_line(0.0, 0.0, 10.0)];
|
||||
let index = SpatialIndex::new(&lines);
|
||||
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
|
||||
]);
|
||||
|
||||
let result = associate_points_to_lines(&store, &lines, &index, &config);
|
||||
|
||||
assert_eq!(result.total_associated, 2);
|
||||
assert_eq!(result.total_unassociated, 2);
|
||||
assert!((result.association_rate() - 0.5).abs() < 0.001);
|
||||
assert_eq!(result.total_points(), 4);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,706 @@
|
||||
//! Line re-fitting from accumulated point clouds.
|
||||
//!
|
||||
//! Re-fits existing map lines using points accumulated in ScanStore,
|
||||
//! eliminating first-scan bias in line geometry.
|
||||
|
||||
use std::collections::HashMap;
|
||||
|
||||
use crate::config::LidarNoiseModel;
|
||||
use crate::core::{Point2D, Pose2D};
|
||||
use crate::extraction::fit_line_weighted;
|
||||
use crate::features::Line2D;
|
||||
use crate::integration::point_association::AssociatedPoint;
|
||||
|
||||
/// Configuration for line re-fitting.
|
||||
#[derive(Clone, Debug)]
|
||||
pub struct RefitConfig {
|
||||
/// Minimum points required to re-fit a line.
|
||||
/// Lines with fewer points keep their original geometry.
|
||||
/// Default: 10
|
||||
pub min_points: usize,
|
||||
|
||||
/// Gap threshold for splitting lines (meters).
|
||||
/// If consecutive points (sorted by projection) have a gap larger than this,
|
||||
/// the line is split into multiple segments.
|
||||
/// Default: 0.30m (30cm)
|
||||
pub gap_threshold: f32,
|
||||
|
||||
/// Noise model for range-based weighting.
|
||||
/// Closer points get higher weights during fitting.
|
||||
pub noise_model: LidarNoiseModel,
|
||||
}
|
||||
|
||||
impl Default for RefitConfig {
|
||||
fn default() -> Self {
|
||||
Self {
|
||||
min_points: 10,
|
||||
gap_threshold: 0.30,
|
||||
noise_model: LidarNoiseModel::default(),
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
impl RefitConfig {
|
||||
/// Create a new configuration with defaults.
|
||||
pub fn new() -> Self {
|
||||
Self::default()
|
||||
}
|
||||
|
||||
/// Set minimum points for re-fitting.
|
||||
pub fn with_min_points(mut self, min: usize) -> Self {
|
||||
self.min_points = min;
|
||||
self
|
||||
}
|
||||
|
||||
/// Set gap threshold for line splitting.
|
||||
pub fn with_gap_threshold(mut self, gap: f32) -> Self {
|
||||
self.gap_threshold = gap;
|
||||
self
|
||||
}
|
||||
|
||||
/// Set the noise model for weighting.
|
||||
pub fn with_noise_model(mut self, model: LidarNoiseModel) -> Self {
|
||||
self.noise_model = model;
|
||||
self
|
||||
}
|
||||
}
|
||||
|
||||
/// Result of re-fitting a line.
|
||||
#[derive(Clone, Debug)]
|
||||
pub enum RefitResult {
|
||||
/// Single line fitted (no gaps detected).
|
||||
Single(Line2D),
|
||||
/// Line was split due to gaps in point cloud.
|
||||
Split(Vec<Line2D>),
|
||||
/// Not enough points to re-fit (keeps original geometry).
|
||||
Insufficient,
|
||||
}
|
||||
|
||||
impl RefitResult {
|
||||
/// Get all lines from the result.
|
||||
pub fn lines(&self) -> Vec<Line2D> {
|
||||
match self {
|
||||
RefitResult::Single(line) => vec![line.clone()],
|
||||
RefitResult::Split(lines) => lines.clone(),
|
||||
RefitResult::Insufficient => vec![],
|
||||
}
|
||||
}
|
||||
|
||||
/// Get number of lines produced.
|
||||
pub fn line_count(&self) -> usize {
|
||||
match self {
|
||||
RefitResult::Single(_) => 1,
|
||||
RefitResult::Split(lines) => lines.len(),
|
||||
RefitResult::Insufficient => 0,
|
||||
}
|
||||
}
|
||||
|
||||
/// Check if re-fitting was successful (not insufficient).
|
||||
pub fn is_success(&self) -> bool {
|
||||
!matches!(self, RefitResult::Insufficient)
|
||||
}
|
||||
}
|
||||
|
||||
/// Statistics from a batch re-fitting operation.
|
||||
#[derive(Clone, Debug, Default)]
|
||||
pub struct RefitStats {
|
||||
/// Number of lines re-fitted as single segments.
|
||||
pub lines_refitted_single: usize,
|
||||
/// Number of lines that were split due to gaps.
|
||||
pub lines_split: usize,
|
||||
/// Number of lines with insufficient points (kept original).
|
||||
pub lines_insufficient: usize,
|
||||
/// Total number of new line segments created (from splits).
|
||||
pub total_segments_created: usize,
|
||||
}
|
||||
|
||||
impl RefitStats {
|
||||
/// Total lines processed.
|
||||
pub fn total_processed(&self) -> usize {
|
||||
self.lines_refitted_single + self.lines_split + self.lines_insufficient
|
||||
}
|
||||
|
||||
/// Success rate (refitted / total).
|
||||
pub fn success_rate(&self) -> f32 {
|
||||
let total = self.total_processed();
|
||||
if total == 0 {
|
||||
0.0
|
||||
} else {
|
||||
(self.lines_refitted_single + self.lines_split) as f32 / total as f32
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// Re-fit a line from associated points with gap detection.
|
||||
///
|
||||
/// # Arguments
|
||||
/// * `points` - Points associated with this line (from point_association)
|
||||
/// * `original` - Original line geometry (for fallback and observation_count)
|
||||
/// * `robot_poses` - Map of scan_id -> robot pose (for computing weights)
|
||||
/// * `config` - Re-fitting configuration
|
||||
///
|
||||
/// # Returns
|
||||
/// - `RefitResult::Single(line)` if fitted as a single line
|
||||
/// - `RefitResult::Split(lines)` if gaps were detected and line was split
|
||||
/// - `RefitResult::Insufficient` if not enough points
|
||||
pub fn refit_line(
|
||||
points: &[AssociatedPoint],
|
||||
original: &Line2D,
|
||||
robot_poses: &HashMap<u32, Pose2D>,
|
||||
config: &RefitConfig,
|
||||
) -> RefitResult {
|
||||
// Check minimum points
|
||||
if points.len() < config.min_points {
|
||||
return RefitResult::Insufficient;
|
||||
}
|
||||
|
||||
// Sort points by projection onto original line direction
|
||||
let mut sorted_points: Vec<&AssociatedPoint> = points.iter().collect();
|
||||
sorted_points.sort_by(|a, b| {
|
||||
a.projection_t
|
||||
.partial_cmp(&b.projection_t)
|
||||
.unwrap_or(std::cmp::Ordering::Equal)
|
||||
});
|
||||
|
||||
// Detect gaps and find split indices
|
||||
let split_indices = find_gap_splits(&sorted_points, config.gap_threshold, original);
|
||||
|
||||
if split_indices.is_empty() {
|
||||
// No gaps - fit as single line
|
||||
let line = fit_segment(
|
||||
&sorted_points,
|
||||
robot_poses,
|
||||
original.observation_count,
|
||||
&config.noise_model,
|
||||
);
|
||||
match line {
|
||||
Some(l) => RefitResult::Single(l),
|
||||
None => RefitResult::Insufficient,
|
||||
}
|
||||
} else {
|
||||
// Gaps detected - split into segments
|
||||
let segments = split_and_fit(
|
||||
&sorted_points,
|
||||
&split_indices,
|
||||
robot_poses,
|
||||
original.observation_count,
|
||||
config,
|
||||
);
|
||||
if segments.is_empty() {
|
||||
RefitResult::Insufficient
|
||||
} else if segments.len() == 1 {
|
||||
RefitResult::Single(segments.into_iter().next().unwrap())
|
||||
} else {
|
||||
RefitResult::Split(segments)
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// Find indices where the point cloud should be split due to gaps.
|
||||
///
|
||||
/// Returns a list of indices where splits should occur.
|
||||
/// Each index represents the start of a new segment.
|
||||
fn find_gap_splits(
|
||||
sorted_points: &[&AssociatedPoint],
|
||||
gap_threshold: f32,
|
||||
original: &Line2D,
|
||||
) -> Vec<usize> {
|
||||
if sorted_points.len() < 2 {
|
||||
return vec![];
|
||||
}
|
||||
|
||||
let line_length = original.length();
|
||||
if line_length < f32::EPSILON {
|
||||
return vec![];
|
||||
}
|
||||
|
||||
let mut splits = Vec::new();
|
||||
|
||||
for i in 1..sorted_points.len() {
|
||||
let prev = sorted_points[i - 1];
|
||||
let curr = sorted_points[i];
|
||||
|
||||
// Gap in projection space (as fraction of line length)
|
||||
let t_gap = (curr.projection_t - prev.projection_t).abs();
|
||||
|
||||
// Convert to world distance
|
||||
let world_gap = t_gap * line_length;
|
||||
|
||||
if world_gap > gap_threshold {
|
||||
splits.push(i);
|
||||
}
|
||||
}
|
||||
|
||||
splits
|
||||
}
|
||||
|
||||
/// Split points at gap indices and fit each segment.
|
||||
fn split_and_fit(
|
||||
sorted_points: &[&AssociatedPoint],
|
||||
split_indices: &[usize],
|
||||
robot_poses: &HashMap<u32, Pose2D>,
|
||||
observation_count: u32,
|
||||
config: &RefitConfig,
|
||||
) -> Vec<Line2D> {
|
||||
let mut segments = Vec::new();
|
||||
|
||||
// Create segment ranges
|
||||
let mut ranges: Vec<(usize, usize)> = Vec::new();
|
||||
let mut start = 0;
|
||||
|
||||
for &split_idx in split_indices {
|
||||
if split_idx > start {
|
||||
ranges.push((start, split_idx));
|
||||
}
|
||||
start = split_idx;
|
||||
}
|
||||
|
||||
// Add final segment
|
||||
if start < sorted_points.len() {
|
||||
ranges.push((start, sorted_points.len()));
|
||||
}
|
||||
|
||||
// Fit each segment
|
||||
for (range_start, range_end) in ranges {
|
||||
let segment_points: Vec<&AssociatedPoint> =
|
||||
sorted_points[range_start..range_end].to_vec();
|
||||
|
||||
if segment_points.len() < config.min_points {
|
||||
continue;
|
||||
}
|
||||
|
||||
if let Some(line) = fit_segment(
|
||||
&segment_points,
|
||||
robot_poses,
|
||||
observation_count,
|
||||
&config.noise_model,
|
||||
) {
|
||||
segments.push(line);
|
||||
}
|
||||
}
|
||||
|
||||
segments
|
||||
}
|
||||
|
||||
/// Fit a line segment from points using weighted TLS.
|
||||
fn fit_segment(
|
||||
points: &[&AssociatedPoint],
|
||||
robot_poses: &HashMap<u32, Pose2D>,
|
||||
observation_count: u32,
|
||||
noise_model: &LidarNoiseModel,
|
||||
) -> Option<Line2D> {
|
||||
if points.len() < 2 {
|
||||
return None;
|
||||
}
|
||||
|
||||
// Extract Point2D and compute weights
|
||||
let mut point_vec: Vec<Point2D> = Vec::with_capacity(points.len());
|
||||
let mut weights: Vec<f32> = Vec::with_capacity(points.len());
|
||||
|
||||
for ap in points {
|
||||
point_vec.push(ap.point);
|
||||
|
||||
// Compute weight based on range from robot
|
||||
let weight = if let Some(pose) = robot_poses.get(&ap.scan_id) {
|
||||
let robot_pos = Point2D::new(pose.x, pose.y);
|
||||
let range = ap.point.distance(robot_pos);
|
||||
noise_model.weight(range)
|
||||
} else {
|
||||
// Fallback: use default weight
|
||||
1.0
|
||||
};
|
||||
weights.push(weight);
|
||||
}
|
||||
|
||||
// Fit using weighted TLS
|
||||
let mut line = fit_line_weighted(&point_vec, &weights)?;
|
||||
|
||||
// Preserve observation count from original
|
||||
line.observation_count = observation_count;
|
||||
line.point_count = points.len() as u32;
|
||||
|
||||
Some(line)
|
||||
}
|
||||
|
||||
/// Re-fit a line from points using simple (unweighted) fitting.
|
||||
///
|
||||
/// Use this when robot poses are not available.
|
||||
pub fn refit_line_simple(
|
||||
points: &[AssociatedPoint],
|
||||
original: &Line2D,
|
||||
config: &RefitConfig,
|
||||
) -> RefitResult {
|
||||
// Use empty pose map for simple fitting
|
||||
let empty_poses: HashMap<u32, Pose2D> = HashMap::new();
|
||||
refit_line(points, original, &empty_poses, config)
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
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,
|
||||
)
|
||||
}
|
||||
|
||||
fn make_associated_point(x: f32, y: f32, scan_id: u32, t: f32) -> AssociatedPoint {
|
||||
AssociatedPoint {
|
||||
point: Point2D::new(x, y),
|
||||
scan_id,
|
||||
distance: 0.0,
|
||||
projection_t: t,
|
||||
}
|
||||
}
|
||||
|
||||
fn make_points_along_line(
|
||||
x_start: f32,
|
||||
x_end: f32,
|
||||
y: f32,
|
||||
count: usize,
|
||||
) -> Vec<AssociatedPoint> {
|
||||
let line_length = (x_end - x_start).abs();
|
||||
(0..count)
|
||||
.map(|i| {
|
||||
let t = i as f32 / (count - 1).max(1) as f32;
|
||||
let x = x_start + t * (x_end - x_start);
|
||||
// projection_t is the fraction along the line
|
||||
make_associated_point(x, y, 0, x / line_length)
|
||||
})
|
||||
.collect()
|
||||
}
|
||||
|
||||
/// 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> {
|
||||
let line_length = (x_end - x_start).abs();
|
||||
// Create points with spacing less than gap_threshold
|
||||
let spacing = gap_threshold * 0.5;
|
||||
let count = ((line_length / spacing) as usize).max(2);
|
||||
(0..count)
|
||||
.map(|i| {
|
||||
let x = x_start + i as f32 * spacing;
|
||||
let t = x / line_length;
|
||||
make_associated_point(x.min(x_end), y, 0, t.min(1.0))
|
||||
})
|
||||
.collect()
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_config_defaults() {
|
||||
let config = RefitConfig::default();
|
||||
assert_eq!(config.min_points, 10);
|
||||
assert!((config.gap_threshold - 0.30).abs() < 0.001);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_config_builder() {
|
||||
let config = RefitConfig::new()
|
||||
.with_min_points(5)
|
||||
.with_gap_threshold(0.5);
|
||||
assert_eq!(config.min_points, 5);
|
||||
assert!((config.gap_threshold - 0.5).abs() < 0.001);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_refit_insufficient_points() {
|
||||
let original = make_horizontal_line(0.0, 0.0, 10.0);
|
||||
let points = make_points_along_line(0.0, 10.0, 0.0, 5); // Only 5 points
|
||||
let config = RefitConfig::default().with_min_points(10);
|
||||
let poses = HashMap::new();
|
||||
|
||||
let result = refit_line(&points, &original, &poses, &config);
|
||||
|
||||
assert!(matches!(result, RefitResult::Insufficient));
|
||||
assert!(!result.is_success());
|
||||
assert_eq!(result.line_count(), 0);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_refit_single_line() {
|
||||
let original = make_horizontal_line(0.0, 0.0, 10.0);
|
||||
// Create dense points along the line with small noise (spacing < gap_threshold)
|
||||
let gap_threshold = 0.30;
|
||||
let spacing = gap_threshold * 0.5; // 0.15m spacing
|
||||
let count = (10.0 / spacing) as usize; // ~67 points for 10m line
|
||||
|
||||
let mut points: Vec<AssociatedPoint> = Vec::new();
|
||||
for i in 0..count {
|
||||
let x = i as f32 * spacing;
|
||||
let t = x / 10.0;
|
||||
let y = if i % 2 == 0 { 0.01 } else { -0.01 }; // Small noise
|
||||
points.push(make_associated_point(x, y, 0, t));
|
||||
}
|
||||
|
||||
let config = RefitConfig::default()
|
||||
.with_min_points(10)
|
||||
.with_gap_threshold(gap_threshold);
|
||||
let poses = HashMap::new();
|
||||
|
||||
let result = refit_line(&points, &original, &poses, &config);
|
||||
|
||||
assert!(result.is_success());
|
||||
assert!(matches!(result, RefitResult::Single(_)));
|
||||
|
||||
let lines = result.lines();
|
||||
assert_eq!(lines.len(), 1);
|
||||
|
||||
// Verify line is approximately horizontal
|
||||
let line = &lines[0];
|
||||
assert!(line.angle().abs() < 0.1);
|
||||
// Verify point count is set
|
||||
assert_eq!(line.point_count as usize, count);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_refit_with_gap_split() {
|
||||
let original = make_horizontal_line(0.0, 0.0, 10.0);
|
||||
let gap_threshold = 0.5;
|
||||
let spacing = 0.1; // Dense points within each segment
|
||||
|
||||
// Create points with a gap in the middle
|
||||
let mut points: Vec<AssociatedPoint> = Vec::new();
|
||||
|
||||
// First segment: 0-3m (dense points)
|
||||
let mut x = 0.0;
|
||||
while x <= 3.0 {
|
||||
let t = x / 10.0;
|
||||
points.push(make_associated_point(x, 0.0, 0, t));
|
||||
x += spacing;
|
||||
}
|
||||
|
||||
// GAP: 3m to 7m (4m gap > 0.5m threshold)
|
||||
|
||||
// Second segment: 7-10m (dense points)
|
||||
x = 7.0;
|
||||
while x <= 10.0 {
|
||||
let t = x / 10.0;
|
||||
points.push(make_associated_point(x, 0.0, 1, t));
|
||||
x += spacing;
|
||||
}
|
||||
|
||||
let config = RefitConfig::default()
|
||||
.with_min_points(5)
|
||||
.with_gap_threshold(gap_threshold);
|
||||
let poses = HashMap::new();
|
||||
|
||||
let result = refit_line(&points, &original, &poses, &config);
|
||||
|
||||
assert!(result.is_success());
|
||||
assert!(
|
||||
matches!(result, RefitResult::Split(_)),
|
||||
"Expected Split, got {:?}",
|
||||
result
|
||||
);
|
||||
|
||||
let lines = result.lines();
|
||||
assert_eq!(lines.len(), 2);
|
||||
|
||||
// First line should cover ~0-3m
|
||||
assert!(lines[0].start.x < 1.0);
|
||||
assert!(lines[0].end.x < 4.0);
|
||||
|
||||
// Second line should cover ~7-10m
|
||||
assert!(lines[1].start.x > 6.0);
|
||||
assert!(lines[1].end.x > 9.0);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_refit_with_poses_weighting() {
|
||||
let original = make_horizontal_line(0.0, 0.0, 10.0);
|
||||
let gap_threshold = 0.5;
|
||||
let spacing = 0.1; // Dense points
|
||||
|
||||
// Create dense points with noise
|
||||
let mut points: Vec<AssociatedPoint> = Vec::new();
|
||||
|
||||
// Points close to robot (scan 0 at origin) - on y=0
|
||||
let mut x = 0.0;
|
||||
while x <= 5.0 {
|
||||
let t = x / 10.0;
|
||||
points.push(make_associated_point(x, 0.0, 0, t));
|
||||
x += spacing;
|
||||
}
|
||||
|
||||
// Points far from robot (scan 1 at x=10) - with noise above y=0
|
||||
x = 5.0 + spacing;
|
||||
while x <= 10.0 {
|
||||
let t = x / 10.0;
|
||||
points.push(make_associated_point(x, 0.5, 1, t));
|
||||
x += spacing;
|
||||
}
|
||||
|
||||
let mut poses = HashMap::new();
|
||||
poses.insert(0, Pose2D::new(0.0, 0.0, 0.0)); // Robot at origin for scan 0
|
||||
poses.insert(1, Pose2D::new(10.0, 0.0, 0.0)); // Robot at x=10 for scan 1
|
||||
|
||||
let config = RefitConfig::default()
|
||||
.with_min_points(10)
|
||||
.with_gap_threshold(gap_threshold);
|
||||
|
||||
let result = refit_line(&points, &original, &poses, &config);
|
||||
|
||||
assert!(result.is_success());
|
||||
let lines = result.lines();
|
||||
assert_eq!(lines.len(), 1);
|
||||
|
||||
// Line should be closer to y=0 due to close points having higher weight
|
||||
let line = &lines[0];
|
||||
let midpoint = line.midpoint();
|
||||
assert!(
|
||||
midpoint.y < 0.3,
|
||||
"Midpoint y={} should be < 0.3 (closer to close points)",
|
||||
midpoint.y
|
||||
);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_refit_preserves_observation_count() {
|
||||
let mut original = make_horizontal_line(0.0, 0.0, 10.0);
|
||||
original.observation_count = 42;
|
||||
|
||||
// Use dense points to avoid gap splitting
|
||||
let points = make_dense_points(0.0, 10.0, 0.0, 0.3);
|
||||
let config = RefitConfig::default()
|
||||
.with_min_points(10)
|
||||
.with_gap_threshold(0.3);
|
||||
let poses = HashMap::new();
|
||||
|
||||
let result = refit_line(&points, &original, &poses, &config);
|
||||
|
||||
let lines = result.lines();
|
||||
assert_eq!(lines.len(), 1, "Expected 1 line, got {}", lines.len());
|
||||
assert_eq!(lines[0].observation_count, 42);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_refit_simple() {
|
||||
let original = make_horizontal_line(0.0, 0.0, 10.0);
|
||||
// Use dense points to avoid gap splitting
|
||||
let points = make_dense_points(0.0, 10.0, 0.05, 0.3);
|
||||
let config = RefitConfig::default()
|
||||
.with_min_points(10)
|
||||
.with_gap_threshold(0.3);
|
||||
|
||||
let result = refit_line_simple(&points, &original, &config);
|
||||
|
||||
assert!(result.is_success());
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_refit_stats() {
|
||||
let stats = RefitStats {
|
||||
lines_refitted_single: 5,
|
||||
lines_split: 2,
|
||||
lines_insufficient: 3,
|
||||
total_segments_created: 5, // 2 splits creating ~2.5 segments each
|
||||
};
|
||||
|
||||
assert_eq!(stats.total_processed(), 10);
|
||||
assert!((stats.success_rate() - 0.7).abs() < 0.001);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_find_gap_splits_no_gaps() {
|
||||
let original = make_horizontal_line(0.0, 0.0, 10.0);
|
||||
let gap_threshold = 0.5;
|
||||
let spacing = 0.2; // Smaller than gap_threshold
|
||||
|
||||
// Create dense points with no gaps
|
||||
let points: Vec<AssociatedPoint> = (0..50)
|
||||
.map(|i| {
|
||||
let x = i as f32 * spacing;
|
||||
let t = x / 10.0;
|
||||
make_associated_point(x.min(10.0), 0.0, 0, t.min(1.0))
|
||||
})
|
||||
.collect();
|
||||
|
||||
let sorted: Vec<&AssociatedPoint> = points.iter().collect();
|
||||
let splits = find_gap_splits(&sorted, gap_threshold, &original);
|
||||
|
||||
assert!(splits.is_empty(), "Expected no splits, got {:?}", splits);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_find_gap_splits_with_gap() {
|
||||
let original = make_horizontal_line(0.0, 0.0, 10.0);
|
||||
let gap_threshold = 0.5;
|
||||
|
||||
// Create points with a clear gap
|
||||
let points = vec![
|
||||
make_associated_point(0.0, 0.0, 0, 0.0),
|
||||
make_associated_point(0.2, 0.0, 0, 0.02),
|
||||
make_associated_point(0.4, 0.0, 0, 0.04),
|
||||
// Gap here: 0.4 to 5.0 = 4.6m > 0.5m threshold
|
||||
make_associated_point(5.0, 0.0, 0, 0.5),
|
||||
make_associated_point(5.2, 0.0, 0, 0.52),
|
||||
];
|
||||
|
||||
let sorted: Vec<&AssociatedPoint> = points.iter().collect();
|
||||
let splits = find_gap_splits(&sorted, gap_threshold, &original);
|
||||
|
||||
assert_eq!(splits.len(), 1, "Expected 1 split, got {:?}", splits);
|
||||
assert_eq!(splits[0], 3); // Split before the 4th point (index 3)
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_refitresult_lines() {
|
||||
let line = make_horizontal_line(0.0, 0.0, 10.0);
|
||||
|
||||
let single = RefitResult::Single(line.clone());
|
||||
assert_eq!(single.lines().len(), 1);
|
||||
assert_eq!(single.line_count(), 1);
|
||||
assert!(single.is_success());
|
||||
|
||||
let split = RefitResult::Split(vec![line.clone(), line.clone()]);
|
||||
assert_eq!(split.lines().len(), 2);
|
||||
assert_eq!(split.line_count(), 2);
|
||||
assert!(split.is_success());
|
||||
|
||||
let insufficient = RefitResult::Insufficient;
|
||||
assert!(insufficient.lines().is_empty());
|
||||
assert_eq!(insufficient.line_count(), 0);
|
||||
assert!(!insufficient.is_success());
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_multiple_gaps_creates_multiple_segments() {
|
||||
let original = make_horizontal_line(0.0, 0.0, 10.0);
|
||||
|
||||
// Create three clusters of points with gaps between
|
||||
let mut points: Vec<AssociatedPoint> = Vec::new();
|
||||
|
||||
// Cluster 1: t = 0.0 to 0.2 (10 points)
|
||||
for i in 0..10 {
|
||||
let t = i as f32 / 50.0;
|
||||
points.push(make_associated_point(t * 10.0, 0.0, 0, t));
|
||||
}
|
||||
|
||||
// Cluster 2: t = 0.4 to 0.6 (10 points) - gap before
|
||||
for i in 0..10 {
|
||||
let t = 0.4 + i as f32 / 50.0;
|
||||
points.push(make_associated_point(t * 10.0, 0.0, 0, t));
|
||||
}
|
||||
|
||||
// Cluster 3: t = 0.8 to 1.0 (10 points) - gap before
|
||||
for i in 0..10 {
|
||||
let t = 0.8 + i as f32 / 50.0;
|
||||
points.push(make_associated_point(t * 10.0, 0.0, 0, t));
|
||||
}
|
||||
|
||||
let config = RefitConfig::default()
|
||||
.with_min_points(5)
|
||||
.with_gap_threshold(0.3); // 0.3m < 2m gaps
|
||||
|
||||
let poses = HashMap::new();
|
||||
let result = refit_line(&points, &original, &poses, &config);
|
||||
|
||||
assert!(result.is_success());
|
||||
assert!(matches!(result, RefitResult::Split(_)));
|
||||
|
||||
let lines = result.lines();
|
||||
assert_eq!(lines.len(), 3);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,909 @@
|
||||
//! Scan storage for exploration mode.
|
||||
//!
|
||||
//! Retains raw scan data during exploration for visualization,
|
||||
//! debugging, and future point-cloud-based line fitting.
|
||||
|
||||
use crate::core::{Point2D, PointCloud2D, Pose2D};
|
||||
|
||||
/// A stored scan with associated pose information.
|
||||
#[derive(Clone, Debug)]
|
||||
pub struct StoredScan {
|
||||
/// Unique scan identifier (sequential).
|
||||
pub scan_id: u32,
|
||||
|
||||
/// Raw odometry pose (input to observe).
|
||||
pub odometry: Pose2D,
|
||||
|
||||
/// Estimated pose after ICP matching.
|
||||
/// This is the corrected pose used for map integration.
|
||||
pub estimated_pose: Pose2D,
|
||||
|
||||
/// Scan points in robot frame.
|
||||
/// Transform with `estimated_pose` to get world-frame points.
|
||||
points: Vec<Point2D>,
|
||||
|
||||
/// Number of features extracted from this scan.
|
||||
pub features_extracted: usize,
|
||||
|
||||
/// ICP matching confidence (0.0 - 1.0).
|
||||
pub match_confidence: f32,
|
||||
}
|
||||
|
||||
impl StoredScan {
|
||||
/// Create a new stored scan.
|
||||
pub fn new(
|
||||
scan_id: u32,
|
||||
odometry: Pose2D,
|
||||
estimated_pose: Pose2D,
|
||||
points: Vec<Point2D>,
|
||||
features_extracted: usize,
|
||||
match_confidence: f32,
|
||||
) -> Self {
|
||||
Self {
|
||||
scan_id,
|
||||
odometry,
|
||||
estimated_pose,
|
||||
points,
|
||||
features_extracted,
|
||||
match_confidence,
|
||||
}
|
||||
}
|
||||
|
||||
/// Get robot-frame points.
|
||||
#[inline]
|
||||
pub fn points(&self) -> &[Point2D] {
|
||||
&self.points
|
||||
}
|
||||
|
||||
/// Get the number of points in this scan.
|
||||
#[inline]
|
||||
pub fn point_count(&self) -> usize {
|
||||
self.points.len()
|
||||
}
|
||||
|
||||
/// Get world-frame points (transformed by estimated pose).
|
||||
pub fn world_points(&self) -> Vec<Point2D> {
|
||||
self.points
|
||||
.iter()
|
||||
.map(|p| self.estimated_pose.transform_point(*p))
|
||||
.collect()
|
||||
}
|
||||
|
||||
/// Fill a buffer with world-frame points (zero-allocation if buffer is large enough).
|
||||
pub fn world_points_into(&self, buffer: &mut Vec<Point2D>) {
|
||||
buffer.clear();
|
||||
buffer.reserve(self.points.len());
|
||||
for p in &self.points {
|
||||
buffer.push(self.estimated_pose.transform_point(*p));
|
||||
}
|
||||
}
|
||||
|
||||
/// Get the pose correction applied by ICP.
|
||||
/// Returns the difference between estimated and odometry-predicted pose.
|
||||
pub fn pose_correction(&self) -> Pose2D {
|
||||
// correction = estimated * odometry.inverse()
|
||||
// This gives the transform that was applied to correct odometry
|
||||
self.estimated_pose.compose(self.odometry.inverse())
|
||||
}
|
||||
|
||||
/// Memory usage in bytes (approximate).
|
||||
pub fn memory_bytes(&self) -> usize {
|
||||
std::mem::size_of::<Self>() + self.points.len() * std::mem::size_of::<Point2D>()
|
||||
}
|
||||
}
|
||||
|
||||
/// Configuration for scan storage.
|
||||
#[derive(Clone, Debug)]
|
||||
pub struct ScanStoreConfig {
|
||||
/// Maximum number of scans to store.
|
||||
/// Oldest scans are removed when limit is reached.
|
||||
/// None = unlimited. Default: None
|
||||
pub max_scans: Option<usize>,
|
||||
|
||||
/// Whether to store point data.
|
||||
/// If false, only poses are stored (much less memory).
|
||||
/// Default: true
|
||||
pub store_points: bool,
|
||||
|
||||
/// Minimum distance traveled to store a new scan (meters).
|
||||
/// Helps reduce redundant scans when robot is stationary.
|
||||
/// Default: 0.0 (store all scans)
|
||||
pub min_distance: f32,
|
||||
|
||||
/// Minimum rotation to store a new scan (radians).
|
||||
/// Default: 0.0 (store all scans)
|
||||
pub min_rotation: f32,
|
||||
}
|
||||
|
||||
impl Default for ScanStoreConfig {
|
||||
fn default() -> Self {
|
||||
Self {
|
||||
max_scans: None,
|
||||
store_points: true,
|
||||
min_distance: 0.0,
|
||||
min_rotation: 0.0,
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
impl ScanStoreConfig {
|
||||
/// Create a new configuration with defaults.
|
||||
pub fn new() -> Self {
|
||||
Self::default()
|
||||
}
|
||||
|
||||
/// Set maximum number of scans to store.
|
||||
pub fn with_max_scans(mut self, max: usize) -> Self {
|
||||
self.max_scans = Some(max);
|
||||
self
|
||||
}
|
||||
|
||||
/// Set unlimited scan storage.
|
||||
pub fn with_unlimited_scans(mut self) -> Self {
|
||||
self.max_scans = None;
|
||||
self
|
||||
}
|
||||
|
||||
/// Enable or disable point storage.
|
||||
pub fn with_store_points(mut self, store: bool) -> Self {
|
||||
self.store_points = store;
|
||||
self
|
||||
}
|
||||
|
||||
/// Set minimum distance between stored scans.
|
||||
pub fn with_min_distance(mut self, meters: f32) -> Self {
|
||||
self.min_distance = meters;
|
||||
self
|
||||
}
|
||||
|
||||
/// Set minimum rotation between stored scans.
|
||||
pub fn with_min_rotation(mut self, radians: f32) -> Self {
|
||||
self.min_rotation = radians;
|
||||
self
|
||||
}
|
||||
}
|
||||
|
||||
/// Storage for scans during exploration.
|
||||
///
|
||||
/// Retains raw scan data for visualization, debugging, and
|
||||
/// point-cloud-based algorithms.
|
||||
///
|
||||
/// # Example
|
||||
///
|
||||
/// ```rust,ignore
|
||||
/// use vastu_map::integration::{ScanStore, ScanStoreConfig};
|
||||
///
|
||||
/// let config = ScanStoreConfig::default().with_max_scans(1000);
|
||||
/// let mut store = ScanStore::new(config);
|
||||
///
|
||||
/// // After each observation...
|
||||
/// store.add_scan(odometry, estimated_pose, &scan_points, features, confidence);
|
||||
///
|
||||
/// // For visualization
|
||||
/// for scan in store.iter() {
|
||||
/// let world_points = scan.world_points();
|
||||
/// draw_points(&world_points);
|
||||
/// }
|
||||
/// ```
|
||||
#[derive(Clone, Debug)]
|
||||
pub struct ScanStore {
|
||||
/// Stored scans (oldest first).
|
||||
scans: Vec<StoredScan>,
|
||||
|
||||
/// Next scan ID to assign.
|
||||
next_scan_id: u32,
|
||||
|
||||
/// Configuration.
|
||||
config: ScanStoreConfig,
|
||||
|
||||
/// Last stored pose (for distance/rotation filtering).
|
||||
last_stored_pose: Option<Pose2D>,
|
||||
}
|
||||
|
||||
impl ScanStore {
|
||||
/// Create a new empty scan store.
|
||||
pub fn new(config: ScanStoreConfig) -> Self {
|
||||
Self {
|
||||
scans: Vec::new(),
|
||||
next_scan_id: 0,
|
||||
config,
|
||||
last_stored_pose: None,
|
||||
}
|
||||
}
|
||||
|
||||
/// Create with default configuration.
|
||||
pub fn with_defaults() -> Self {
|
||||
Self::new(ScanStoreConfig::default())
|
||||
}
|
||||
|
||||
/// Get the configuration.
|
||||
pub fn config(&self) -> &ScanStoreConfig {
|
||||
&self.config
|
||||
}
|
||||
|
||||
/// Get the number of stored scans.
|
||||
#[inline]
|
||||
pub fn len(&self) -> usize {
|
||||
self.scans.len()
|
||||
}
|
||||
|
||||
/// Check if the store is empty.
|
||||
#[inline]
|
||||
pub fn is_empty(&self) -> bool {
|
||||
self.scans.is_empty()
|
||||
}
|
||||
|
||||
/// Get the next scan ID that will be assigned.
|
||||
#[inline]
|
||||
pub fn next_scan_id(&self) -> u32 {
|
||||
self.next_scan_id
|
||||
}
|
||||
|
||||
/// Add a scan to the store.
|
||||
///
|
||||
/// Returns the assigned scan_id, or None if the scan was filtered
|
||||
/// (due to min_distance/min_rotation constraints).
|
||||
pub fn add_scan(
|
||||
&mut self,
|
||||
odometry: Pose2D,
|
||||
estimated_pose: Pose2D,
|
||||
points: &PointCloud2D,
|
||||
features_extracted: usize,
|
||||
match_confidence: f32,
|
||||
) -> Option<u32> {
|
||||
// Check distance/rotation filter
|
||||
if let Some(last_pose) = self.last_stored_pose {
|
||||
let dx = estimated_pose.x - last_pose.x;
|
||||
let dy = estimated_pose.y - last_pose.y;
|
||||
let dist = (dx * dx + dy * dy).sqrt();
|
||||
let dtheta = crate::core::math::angle_diff(estimated_pose.theta, last_pose.theta).abs();
|
||||
|
||||
// Determine which constraints are active (> 0 means enabled)
|
||||
let distance_check_enabled = self.config.min_distance > 0.0;
|
||||
let rotation_check_enabled = self.config.min_rotation > 0.0;
|
||||
|
||||
let passes_distance = dist >= self.config.min_distance;
|
||||
let passes_rotation = dtheta >= self.config.min_rotation;
|
||||
|
||||
// Filter logic:
|
||||
// - If only distance filter: require distance
|
||||
// - If only rotation filter: require rotation
|
||||
// - If both filters: require either (OR semantics)
|
||||
// - If no filters: store all
|
||||
let should_store = match (distance_check_enabled, rotation_check_enabled) {
|
||||
(false, false) => true,
|
||||
(true, false) => passes_distance,
|
||||
(false, true) => passes_rotation,
|
||||
(true, true) => passes_distance || passes_rotation,
|
||||
};
|
||||
|
||||
if !should_store {
|
||||
return None;
|
||||
}
|
||||
}
|
||||
|
||||
// Enforce max_scans limit
|
||||
if let Some(max) = self.config.max_scans {
|
||||
while self.scans.len() >= max {
|
||||
self.scans.remove(0); // Remove oldest
|
||||
}
|
||||
}
|
||||
|
||||
let scan_id = self.next_scan_id;
|
||||
self.next_scan_id += 1;
|
||||
|
||||
let stored_points = if self.config.store_points {
|
||||
points.iter().collect()
|
||||
} else {
|
||||
Vec::new()
|
||||
};
|
||||
|
||||
let scan = StoredScan::new(
|
||||
scan_id,
|
||||
odometry,
|
||||
estimated_pose,
|
||||
stored_points,
|
||||
features_extracted,
|
||||
match_confidence,
|
||||
);
|
||||
|
||||
self.last_stored_pose = Some(estimated_pose);
|
||||
self.scans.push(scan);
|
||||
|
||||
Some(scan_id)
|
||||
}
|
||||
|
||||
/// Add a scan from raw points (not PointCloud2D).
|
||||
pub fn add_scan_from_points(
|
||||
&mut self,
|
||||
odometry: Pose2D,
|
||||
estimated_pose: Pose2D,
|
||||
points: &[Point2D],
|
||||
features_extracted: usize,
|
||||
match_confidence: f32,
|
||||
) -> Option<u32> {
|
||||
// Check distance/rotation filter
|
||||
if let Some(last_pose) = self.last_stored_pose {
|
||||
let dx = estimated_pose.x - last_pose.x;
|
||||
let dy = estimated_pose.y - last_pose.y;
|
||||
let dist = (dx * dx + dy * dy).sqrt();
|
||||
let dtheta = crate::core::math::angle_diff(estimated_pose.theta, last_pose.theta).abs();
|
||||
|
||||
// Determine which constraints are active (> 0 means enabled)
|
||||
let distance_check_enabled = self.config.min_distance > 0.0;
|
||||
let rotation_check_enabled = self.config.min_rotation > 0.0;
|
||||
|
||||
let passes_distance = dist >= self.config.min_distance;
|
||||
let passes_rotation = dtheta >= self.config.min_rotation;
|
||||
|
||||
let should_store = match (distance_check_enabled, rotation_check_enabled) {
|
||||
(false, false) => true,
|
||||
(true, false) => passes_distance,
|
||||
(false, true) => passes_rotation,
|
||||
(true, true) => passes_distance || passes_rotation,
|
||||
};
|
||||
|
||||
if !should_store {
|
||||
return None;
|
||||
}
|
||||
}
|
||||
|
||||
// Enforce max_scans limit
|
||||
if let Some(max) = self.config.max_scans {
|
||||
while self.scans.len() >= max {
|
||||
self.scans.remove(0);
|
||||
}
|
||||
}
|
||||
|
||||
let scan_id = self.next_scan_id;
|
||||
self.next_scan_id += 1;
|
||||
|
||||
let stored_points = if self.config.store_points {
|
||||
points.to_vec()
|
||||
} else {
|
||||
Vec::new()
|
||||
};
|
||||
|
||||
let scan = StoredScan::new(
|
||||
scan_id,
|
||||
odometry,
|
||||
estimated_pose,
|
||||
stored_points,
|
||||
features_extracted,
|
||||
match_confidence,
|
||||
);
|
||||
|
||||
self.last_stored_pose = Some(estimated_pose);
|
||||
self.scans.push(scan);
|
||||
|
||||
Some(scan_id)
|
||||
}
|
||||
|
||||
/// Get a scan by its ID.
|
||||
pub fn get(&self, scan_id: u32) -> Option<&StoredScan> {
|
||||
self.scans.iter().find(|s| s.scan_id == scan_id)
|
||||
}
|
||||
|
||||
/// Get a scan by index (0 = oldest).
|
||||
pub fn get_by_index(&self, index: usize) -> Option<&StoredScan> {
|
||||
self.scans.get(index)
|
||||
}
|
||||
|
||||
/// Get the most recent scan.
|
||||
pub fn latest(&self) -> Option<&StoredScan> {
|
||||
self.scans.last()
|
||||
}
|
||||
|
||||
/// Get the oldest scan.
|
||||
pub fn oldest(&self) -> Option<&StoredScan> {
|
||||
self.scans.first()
|
||||
}
|
||||
|
||||
/// Iterate over all stored scans (oldest first).
|
||||
pub fn iter(&self) -> impl Iterator<Item = &StoredScan> {
|
||||
self.scans.iter()
|
||||
}
|
||||
|
||||
/// Iterate over scans in reverse order (newest first).
|
||||
pub fn iter_rev(&self) -> impl Iterator<Item = &StoredScan> {
|
||||
self.scans.iter().rev()
|
||||
}
|
||||
|
||||
/// Iterate over scans within a pose range.
|
||||
///
|
||||
/// Returns scans whose estimated_pose is within `radius` of `center`.
|
||||
pub fn iter_near(&self, center: Point2D, radius: f32) -> impl Iterator<Item = &StoredScan> {
|
||||
let radius_sq = radius * radius;
|
||||
self.scans.iter().filter(move |s| {
|
||||
let dx = s.estimated_pose.x - center.x;
|
||||
let dy = s.estimated_pose.y - center.y;
|
||||
dx * dx + dy * dy <= radius_sq
|
||||
})
|
||||
}
|
||||
|
||||
/// Get all scans as a slice.
|
||||
pub fn scans(&self) -> &[StoredScan] {
|
||||
&self.scans
|
||||
}
|
||||
|
||||
/// Get all estimated poses.
|
||||
pub fn poses(&self) -> Vec<Pose2D> {
|
||||
self.scans.iter().map(|s| s.estimated_pose).collect()
|
||||
}
|
||||
|
||||
/// Get the trajectory as a list of points.
|
||||
pub fn trajectory(&self) -> Vec<Point2D> {
|
||||
self.scans
|
||||
.iter()
|
||||
.map(|s| Point2D::new(s.estimated_pose.x, s.estimated_pose.y))
|
||||
.collect()
|
||||
}
|
||||
|
||||
/// Get all world-frame points from all scans.
|
||||
///
|
||||
/// Warning: This can be memory-intensive for large stores.
|
||||
pub fn all_world_points(&self) -> Vec<Point2D> {
|
||||
let total_points: usize = self.scans.iter().map(|s| s.point_count()).sum();
|
||||
let mut result = Vec::with_capacity(total_points);
|
||||
|
||||
for scan in &self.scans {
|
||||
for p in scan.points() {
|
||||
result.push(scan.estimated_pose.transform_point(*p));
|
||||
}
|
||||
}
|
||||
|
||||
result
|
||||
}
|
||||
|
||||
/// Get world-frame points with scan IDs.
|
||||
///
|
||||
/// Returns (point, scan_id) pairs for all stored points.
|
||||
pub fn all_world_points_with_ids(&self) -> Vec<(Point2D, u32)> {
|
||||
let total_points: usize = self.scans.iter().map(|s| s.point_count()).sum();
|
||||
let mut result = Vec::with_capacity(total_points);
|
||||
|
||||
for scan in &self.scans {
|
||||
for p in scan.points() {
|
||||
result.push((scan.estimated_pose.transform_point(*p), scan.scan_id));
|
||||
}
|
||||
}
|
||||
|
||||
result
|
||||
}
|
||||
|
||||
/// Update the estimated pose for a scan (after loop closure correction).
|
||||
///
|
||||
/// Returns true if the scan was found and updated.
|
||||
pub fn update_pose(&mut self, scan_id: u32, new_pose: Pose2D) -> bool {
|
||||
if let Some(scan) = self.scans.iter_mut().find(|s| s.scan_id == scan_id) {
|
||||
scan.estimated_pose = new_pose;
|
||||
true
|
||||
} else {
|
||||
false
|
||||
}
|
||||
}
|
||||
|
||||
/// Apply pose corrections to multiple scans.
|
||||
///
|
||||
/// Takes a map of scan_id -> corrected_pose.
|
||||
/// Returns the number of scans updated.
|
||||
pub fn apply_pose_corrections(
|
||||
&mut self,
|
||||
corrections: &std::collections::HashMap<u32, Pose2D>,
|
||||
) -> usize {
|
||||
let mut updated = 0;
|
||||
for scan in &mut self.scans {
|
||||
if let Some(new_pose) = corrections.get(&scan.scan_id) {
|
||||
scan.estimated_pose = *new_pose;
|
||||
updated += 1;
|
||||
}
|
||||
}
|
||||
updated
|
||||
}
|
||||
|
||||
/// Clear all stored scans.
|
||||
pub fn clear(&mut self) {
|
||||
self.scans.clear();
|
||||
self.last_stored_pose = None;
|
||||
// Note: next_scan_id is NOT reset to allow ID continuity
|
||||
}
|
||||
|
||||
/// Reset the store completely (including scan ID counter).
|
||||
pub fn reset(&mut self) {
|
||||
self.scans.clear();
|
||||
self.next_scan_id = 0;
|
||||
self.last_stored_pose = None;
|
||||
}
|
||||
|
||||
/// Total number of points stored across all scans.
|
||||
pub fn total_points(&self) -> usize {
|
||||
self.scans.iter().map(|s| s.point_count()).sum()
|
||||
}
|
||||
|
||||
/// Memory usage statistics.
|
||||
pub fn memory_stats(&self) -> ScanStoreStats {
|
||||
let total_points = self.total_points();
|
||||
let point_bytes = total_points * std::mem::size_of::<Point2D>();
|
||||
let scan_overhead = self.scans.len() * std::mem::size_of::<StoredScan>();
|
||||
let vec_overhead = self.scans.capacity() * std::mem::size_of::<StoredScan>();
|
||||
|
||||
ScanStoreStats {
|
||||
scan_count: self.scans.len(),
|
||||
total_points,
|
||||
point_bytes,
|
||||
total_bytes: point_bytes + scan_overhead + vec_overhead,
|
||||
avg_points_per_scan: if self.scans.is_empty() {
|
||||
0.0
|
||||
} else {
|
||||
total_points as f32 / self.scans.len() as f32
|
||||
},
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
impl Default for ScanStore {
|
||||
fn default() -> Self {
|
||||
Self::with_defaults()
|
||||
}
|
||||
}
|
||||
|
||||
/// Memory usage statistics for ScanStore.
|
||||
#[derive(Clone, Debug)]
|
||||
pub struct ScanStoreStats {
|
||||
/// Number of stored scans.
|
||||
pub scan_count: usize,
|
||||
/// Total number of points across all scans.
|
||||
pub total_points: usize,
|
||||
/// Memory used by points (bytes).
|
||||
pub point_bytes: usize,
|
||||
/// Total estimated memory usage (bytes).
|
||||
pub total_bytes: usize,
|
||||
/// Average points per scan.
|
||||
pub avg_points_per_scan: f32,
|
||||
}
|
||||
|
||||
impl ScanStoreStats {
|
||||
/// Get memory usage in kilobytes.
|
||||
pub fn total_kb(&self) -> f32 {
|
||||
self.total_bytes as f32 / 1024.0
|
||||
}
|
||||
|
||||
/// Get memory usage in megabytes.
|
||||
pub fn total_mb(&self) -> f32 {
|
||||
self.total_bytes as f32 / (1024.0 * 1024.0)
|
||||
}
|
||||
}
|
||||
|
||||
impl std::fmt::Display for ScanStoreStats {
|
||||
fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result {
|
||||
write!(
|
||||
f,
|
||||
"ScanStore: {} scans, {} points, {:.2} KB",
|
||||
self.scan_count,
|
||||
self.total_points,
|
||||
self.total_kb()
|
||||
)
|
||||
}
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
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()
|
||||
}
|
||||
|
||||
fn make_point_cloud(n: usize) -> PointCloud2D {
|
||||
let xs: Vec<f32> = (0..n).map(|i| i as f32 * 0.1).collect();
|
||||
let ys: Vec<f32> = vec![0.0; n];
|
||||
PointCloud2D { xs, ys }
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_stored_scan_creation() {
|
||||
let scan = StoredScan::new(
|
||||
0,
|
||||
Pose2D::identity(),
|
||||
Pose2D::new(1.0, 0.0, 0.0),
|
||||
make_test_points(10),
|
||||
5,
|
||||
0.95,
|
||||
);
|
||||
|
||||
assert_eq!(scan.scan_id, 0);
|
||||
assert_eq!(scan.point_count(), 10);
|
||||
assert_eq!(scan.features_extracted, 5);
|
||||
assert!((scan.match_confidence - 0.95).abs() < 0.001);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_stored_scan_world_points() {
|
||||
let points = vec![Point2D::new(1.0, 0.0), Point2D::new(0.0, 1.0)];
|
||||
let pose = Pose2D::new(10.0, 20.0, std::f32::consts::FRAC_PI_2); // 90° rotation
|
||||
|
||||
let scan = StoredScan::new(0, Pose2D::identity(), pose, points, 0, 1.0);
|
||||
|
||||
let world = scan.world_points();
|
||||
assert_eq!(world.len(), 2);
|
||||
|
||||
// (1, 0) rotated 90° + translated = (10 + 0, 20 + 1) = (10, 21)
|
||||
assert!((world[0].x - 10.0).abs() < 0.01);
|
||||
assert!((world[0].y - 21.0).abs() < 0.01);
|
||||
|
||||
// (0, 1) rotated 90° + translated = (10 - 1, 20 + 0) = (9, 20)
|
||||
assert!((world[1].x - 9.0).abs() < 0.01);
|
||||
assert!((world[1].y - 20.0).abs() < 0.01);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_scan_store_basic() {
|
||||
let mut store = ScanStore::with_defaults();
|
||||
assert!(store.is_empty());
|
||||
assert_eq!(store.len(), 0);
|
||||
|
||||
let cloud = make_point_cloud(50);
|
||||
let id = store.add_scan(
|
||||
Pose2D::identity(),
|
||||
Pose2D::new(1.0, 0.0, 0.0),
|
||||
&cloud,
|
||||
3,
|
||||
0.9,
|
||||
);
|
||||
|
||||
assert_eq!(id, Some(0));
|
||||
assert_eq!(store.len(), 1);
|
||||
assert!(!store.is_empty());
|
||||
|
||||
let scan = store.get(0).unwrap();
|
||||
assert_eq!(scan.point_count(), 50);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_scan_store_max_scans() {
|
||||
let config = ScanStoreConfig::default().with_max_scans(3);
|
||||
let mut store = ScanStore::new(config);
|
||||
|
||||
for i in 0..5 {
|
||||
let cloud = make_point_cloud(10);
|
||||
store.add_scan(
|
||||
Pose2D::identity(),
|
||||
Pose2D::new(i as f32, 0.0, 0.0),
|
||||
&cloud,
|
||||
0,
|
||||
1.0,
|
||||
);
|
||||
}
|
||||
|
||||
assert_eq!(store.len(), 3);
|
||||
// Oldest scans (0, 1) should be removed
|
||||
assert!(store.get(0).is_none());
|
||||
assert!(store.get(1).is_none());
|
||||
assert!(store.get(2).is_some());
|
||||
assert!(store.get(3).is_some());
|
||||
assert!(store.get(4).is_some());
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_scan_store_distance_filter() {
|
||||
let config = ScanStoreConfig::default().with_min_distance(0.5);
|
||||
let mut store = ScanStore::new(config);
|
||||
|
||||
let cloud = make_point_cloud(10);
|
||||
|
||||
// First scan always stored
|
||||
let id1 = store.add_scan(
|
||||
Pose2D::identity(),
|
||||
Pose2D::new(0.0, 0.0, 0.0),
|
||||
&cloud,
|
||||
0,
|
||||
1.0,
|
||||
);
|
||||
assert!(id1.is_some());
|
||||
|
||||
// Second scan too close, filtered
|
||||
let id2 = store.add_scan(
|
||||
Pose2D::identity(),
|
||||
Pose2D::new(0.1, 0.0, 0.0),
|
||||
&cloud,
|
||||
0,
|
||||
1.0,
|
||||
);
|
||||
assert!(id2.is_none());
|
||||
|
||||
// Third scan far enough
|
||||
let id3 = store.add_scan(
|
||||
Pose2D::identity(),
|
||||
Pose2D::new(1.0, 0.0, 0.0),
|
||||
&cloud,
|
||||
0,
|
||||
1.0,
|
||||
);
|
||||
assert!(id3.is_some());
|
||||
|
||||
assert_eq!(store.len(), 2);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_scan_store_rotation_filter() {
|
||||
let config = ScanStoreConfig::default().with_min_rotation(0.5);
|
||||
let mut store = ScanStore::new(config);
|
||||
|
||||
let cloud = make_point_cloud(10);
|
||||
|
||||
store.add_scan(
|
||||
Pose2D::identity(),
|
||||
Pose2D::new(0.0, 0.0, 0.0),
|
||||
&cloud,
|
||||
0,
|
||||
1.0,
|
||||
);
|
||||
|
||||
// Small rotation, filtered
|
||||
let id = store.add_scan(
|
||||
Pose2D::identity(),
|
||||
Pose2D::new(0.0, 0.0, 0.1),
|
||||
&cloud,
|
||||
0,
|
||||
1.0,
|
||||
);
|
||||
assert!(id.is_none());
|
||||
|
||||
// Large rotation, stored
|
||||
let id = store.add_scan(
|
||||
Pose2D::identity(),
|
||||
Pose2D::new(0.0, 0.0, 1.0),
|
||||
&cloud,
|
||||
0,
|
||||
1.0,
|
||||
);
|
||||
assert!(id.is_some());
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_scan_store_no_points() {
|
||||
let config = ScanStoreConfig::default().with_store_points(false);
|
||||
let mut store = ScanStore::new(config);
|
||||
|
||||
let cloud = make_point_cloud(100);
|
||||
store.add_scan(Pose2D::identity(), Pose2D::identity(), &cloud, 0, 1.0);
|
||||
|
||||
let scan = store.latest().unwrap();
|
||||
assert_eq!(scan.point_count(), 0); // Points not stored
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_scan_store_trajectory() {
|
||||
let mut store = ScanStore::with_defaults();
|
||||
|
||||
for i in 0..5 {
|
||||
let cloud = make_point_cloud(10);
|
||||
store.add_scan(
|
||||
Pose2D::identity(),
|
||||
Pose2D::new(i as f32, i as f32 * 2.0, 0.0),
|
||||
&cloud,
|
||||
0,
|
||||
1.0,
|
||||
);
|
||||
}
|
||||
|
||||
let traj = store.trajectory();
|
||||
assert_eq!(traj.len(), 5);
|
||||
assert!((traj[2].x - 2.0).abs() < 0.01);
|
||||
assert!((traj[2].y - 4.0).abs() < 0.01);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_scan_store_iter_near() {
|
||||
let mut store = ScanStore::with_defaults();
|
||||
let cloud = make_point_cloud(10);
|
||||
|
||||
// Add scans at various positions
|
||||
for i in 0..10 {
|
||||
store.add_scan(
|
||||
Pose2D::identity(),
|
||||
Pose2D::new(i as f32, 0.0, 0.0),
|
||||
&cloud,
|
||||
0,
|
||||
1.0,
|
||||
);
|
||||
}
|
||||
|
||||
// Find scans near (5, 0) with radius 1.5
|
||||
let near: Vec<_> = store.iter_near(Point2D::new(5.0, 0.0), 1.5).collect();
|
||||
assert_eq!(near.len(), 3); // scans at x=4, 5, 6
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_scan_store_update_pose() {
|
||||
let mut store = ScanStore::with_defaults();
|
||||
let cloud = make_point_cloud(10);
|
||||
|
||||
store.add_scan(
|
||||
Pose2D::identity(),
|
||||
Pose2D::new(1.0, 0.0, 0.0),
|
||||
&cloud,
|
||||
0,
|
||||
1.0,
|
||||
);
|
||||
|
||||
let new_pose = Pose2D::new(1.5, 0.5, 0.1);
|
||||
assert!(store.update_pose(0, new_pose));
|
||||
|
||||
let scan = store.get(0).unwrap();
|
||||
assert!((scan.estimated_pose.x - 1.5).abs() < 0.01);
|
||||
assert!((scan.estimated_pose.y - 0.5).abs() < 0.01);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_scan_store_memory_stats() {
|
||||
let mut store = ScanStore::with_defaults();
|
||||
|
||||
for _ in 0..10 {
|
||||
let cloud = make_point_cloud(100);
|
||||
store.add_scan(Pose2D::identity(), Pose2D::identity(), &cloud, 0, 1.0);
|
||||
}
|
||||
|
||||
let stats = store.memory_stats();
|
||||
assert_eq!(stats.scan_count, 10);
|
||||
assert_eq!(stats.total_points, 1000);
|
||||
assert!((stats.avg_points_per_scan - 100.0).abs() < 0.1);
|
||||
assert!(stats.total_kb() > 0.0);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_scan_store_all_world_points() {
|
||||
let mut store = ScanStore::with_defaults();
|
||||
|
||||
// Add two scans at different positions
|
||||
let cloud1 = make_point_cloud(5); // Points at (0,0), (0.1,0), ...
|
||||
store.add_scan(
|
||||
Pose2D::identity(),
|
||||
Pose2D::new(10.0, 0.0, 0.0), // Translate by (10, 0)
|
||||
&cloud1,
|
||||
0,
|
||||
1.0,
|
||||
);
|
||||
|
||||
let cloud2 = make_point_cloud(5);
|
||||
store.add_scan(
|
||||
Pose2D::identity(),
|
||||
Pose2D::new(20.0, 0.0, 0.0), // Translate by (20, 0)
|
||||
&cloud2,
|
||||
0,
|
||||
1.0,
|
||||
);
|
||||
|
||||
let all_points = store.all_world_points();
|
||||
assert_eq!(all_points.len(), 10);
|
||||
|
||||
// First 5 points should be around x=10
|
||||
assert!((all_points[0].x - 10.0).abs() < 0.01);
|
||||
// Last 5 points should be around x=20
|
||||
assert!((all_points[5].x - 20.0).abs() < 0.01);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_scan_store_clear_and_reset() {
|
||||
let mut store = ScanStore::with_defaults();
|
||||
let cloud = make_point_cloud(10);
|
||||
|
||||
store.add_scan(Pose2D::identity(), Pose2D::identity(), &cloud, 0, 1.0);
|
||||
store.add_scan(Pose2D::identity(), Pose2D::identity(), &cloud, 0, 1.0);
|
||||
|
||||
assert_eq!(store.next_scan_id(), 2);
|
||||
|
||||
store.clear();
|
||||
assert!(store.is_empty());
|
||||
assert_eq!(store.next_scan_id(), 2); // ID preserved
|
||||
|
||||
store.add_scan(Pose2D::identity(), Pose2D::identity(), &cloud, 0, 1.0);
|
||||
assert_eq!(store.latest().unwrap().scan_id, 2);
|
||||
|
||||
store.reset();
|
||||
assert!(store.is_empty());
|
||||
assert_eq!(store.next_scan_id(), 0); // ID reset
|
||||
}
|
||||
}
|
||||
@@ -139,6 +139,9 @@ pub use extraction::{LineExtractor, SplitMergeExtractor};
|
||||
pub use loop_closure::LoopDetector;
|
||||
pub use matching::ScanMatcher;
|
||||
|
||||
// Re-export scan storage for exploration
|
||||
pub use integration::{ScanStore, ScanStoreConfig, ScanStoreStats, StoredScan};
|
||||
|
||||
// ─────────────────────────────────────────────────────────────────────────────
|
||||
// Map Trait and Supporting Types
|
||||
// ─────────────────────────────────────────────────────────────────────────────
|
||||
|
||||
@@ -2,11 +2,14 @@
|
||||
|
||||
use std::time::Instant;
|
||||
|
||||
use crate::config::ExplorationConfig;
|
||||
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::{
|
||||
batch_merge, create_new_line, find_associations, find_unmatched_scan_lines,
|
||||
associate_points_to_lines, batch_merge, create_new_line, find_associations,
|
||||
find_unmatched_scan_lines, refit_line, PointAssociationConfig, RefitConfig, RefitResult,
|
||||
RefitStats, ScanStore, ScanStoreConfig,
|
||||
};
|
||||
use crate::loop_closure::{LoopClosure, LoopClosureDetector};
|
||||
use crate::matching::PointToLineIcp;
|
||||
@@ -158,6 +161,13 @@ pub struct VectorMap {
|
||||
|
||||
/// Pre-allocated scratch space for observe pipeline (zero-allocation).
|
||||
observe_scratch: ObserveScratchSpace,
|
||||
|
||||
/// Scan storage for exploration mode.
|
||||
/// When enabled, stores raw scan data for point-cloud-based line re-fitting.
|
||||
scan_store: Option<ScanStore>,
|
||||
|
||||
/// Exploration configuration (None if exploration mode disabled).
|
||||
exploration_config: Option<ExplorationConfig>,
|
||||
}
|
||||
|
||||
impl VectorMap {
|
||||
@@ -175,6 +185,8 @@ impl VectorMap {
|
||||
loop_closures: Vec::new(),
|
||||
icp_matcher,
|
||||
observe_scratch: ObserveScratchSpace::default_capacity(),
|
||||
scan_store: None,
|
||||
exploration_config: None,
|
||||
}
|
||||
}
|
||||
|
||||
@@ -230,6 +242,75 @@ impl VectorMap {
|
||||
self.loop_closures.clear();
|
||||
}
|
||||
|
||||
// ─────────────────────────────────────────────────────────────────────────
|
||||
// Exploration Mode
|
||||
// ─────────────────────────────────────────────────────────────────────────
|
||||
|
||||
/// Enable exploration mode with the given configuration.
|
||||
///
|
||||
/// In exploration mode, VectorMap stores raw scan data and periodically
|
||||
/// re-fits lines from accumulated point clouds, eliminating first-scan bias.
|
||||
///
|
||||
/// # Arguments
|
||||
/// * `config` - Exploration configuration
|
||||
///
|
||||
/// # Example
|
||||
/// ```rust,ignore
|
||||
/// use vastu_map::config::ExplorationConfig;
|
||||
///
|
||||
/// let config = ExplorationConfig::default()
|
||||
/// .with_refit_interval(10)
|
||||
/// .with_gap_threshold(0.3);
|
||||
///
|
||||
/// map.enable_exploration_mode(config);
|
||||
/// ```
|
||||
pub fn enable_exploration_mode(&mut self, config: ExplorationConfig) {
|
||||
let scan_config = ScanStoreConfig::default();
|
||||
self.scan_store = Some(ScanStore::new(scan_config));
|
||||
self.exploration_config = Some(config);
|
||||
}
|
||||
|
||||
/// Enable exploration mode with custom scan store configuration.
|
||||
///
|
||||
/// Allows configuring max scans, distance/rotation filtering, etc.
|
||||
pub fn enable_exploration_mode_with_scan_config(
|
||||
&mut self,
|
||||
exploration_config: ExplorationConfig,
|
||||
scan_config: ScanStoreConfig,
|
||||
) {
|
||||
self.scan_store = Some(ScanStore::new(scan_config));
|
||||
self.exploration_config = Some(exploration_config);
|
||||
}
|
||||
|
||||
/// Disable exploration mode and return the accumulated scan store.
|
||||
///
|
||||
/// Returns the ScanStore containing all accumulated scans, or None
|
||||
/// if exploration mode was not enabled.
|
||||
pub fn disable_exploration_mode(&mut self) -> Option<ScanStore> {
|
||||
self.exploration_config = None;
|
||||
self.scan_store.take()
|
||||
}
|
||||
|
||||
/// Check if exploration mode is enabled.
|
||||
pub fn is_exploration_mode(&self) -> bool {
|
||||
self.exploration_config.is_some()
|
||||
}
|
||||
|
||||
/// Get the exploration configuration (if exploration mode is enabled).
|
||||
pub fn exploration_config(&self) -> Option<&ExplorationConfig> {
|
||||
self.exploration_config.as_ref()
|
||||
}
|
||||
|
||||
/// Get the scan store (if exploration mode is enabled).
|
||||
pub fn scan_store(&self) -> Option<&ScanStore> {
|
||||
self.scan_store.as_ref()
|
||||
}
|
||||
|
||||
/// Get mutable access to the scan store (if exploration mode is enabled).
|
||||
pub fn scan_store_mut(&mut self) -> Option<&mut ScanStore> {
|
||||
self.scan_store.as_mut()
|
||||
}
|
||||
|
||||
/// Add a line to the map.
|
||||
pub fn add_line(&mut self, line: Line2D) {
|
||||
self.line_store.add(line);
|
||||
@@ -336,6 +417,17 @@ impl VectorMap {
|
||||
|
||||
self.current_pose = final_pose;
|
||||
|
||||
// ─── Scan Storage (Exploration Mode) ───
|
||||
if let Some(ref mut store) = self.scan_store {
|
||||
store.add_scan(
|
||||
odometry,
|
||||
final_pose,
|
||||
scan,
|
||||
0, // features_extracted - will be updated after extraction
|
||||
confidence,
|
||||
);
|
||||
}
|
||||
|
||||
// ─── Feature Extraction ───
|
||||
// (robot_frame_points already filled in scratch above)
|
||||
let features = self.extract_features_timed(&final_pose, &mut timing);
|
||||
@@ -347,6 +439,18 @@ impl VectorMap {
|
||||
(0, 0)
|
||||
};
|
||||
|
||||
// ─── 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();
|
||||
}
|
||||
}
|
||||
|
||||
// ─── Loop Closure ───
|
||||
let loop_closure_detected = if self.config.loop_closure_enabled {
|
||||
let lc_start = Instant::now();
|
||||
@@ -396,6 +500,17 @@ impl VectorMap {
|
||||
// Fill scratch with robot frame points (used by feature extraction and loop closure)
|
||||
self.observe_scratch.fill_robot_frame_points(scan);
|
||||
|
||||
// ─── 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
|
||||
0.0, // confidence - no ICP for first scan
|
||||
);
|
||||
}
|
||||
|
||||
// Extract features from robot-frame scan, transform to world using odometry
|
||||
let features = self.extract_features_timed(&odometry, &mut timing);
|
||||
|
||||
@@ -536,6 +651,158 @@ impl VectorMap {
|
||||
fn update_corners(&mut self) {
|
||||
self.corners = detect_corners(self.line_store.lines(), &self.config.corner);
|
||||
}
|
||||
|
||||
/// Optimize map by merging coplanar line segments.
|
||||
///
|
||||
/// Call this periodically (e.g., after N observations) to clean up
|
||||
/// redundant line segments that represent the same wall.
|
||||
///
|
||||
/// # Arguments
|
||||
/// * `config` - Configuration for coplanar merging thresholds
|
||||
///
|
||||
/// # Returns
|
||||
/// Number of lines merged (removed from the map)
|
||||
///
|
||||
/// # Example
|
||||
/// ```rust,ignore
|
||||
/// use vastu_map::integration::CoplanarMergeConfig;
|
||||
///
|
||||
/// // After exploration...
|
||||
/// let config = CoplanarMergeConfig::default()
|
||||
/// .with_max_angle_diff(0.0175) // 1 degree
|
||||
/// .with_max_perpendicular_dist(0.02); // 2cm
|
||||
/// 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);
|
||||
|
||||
if merged > 0 {
|
||||
// Rebuild auxiliary structures
|
||||
self.line_store.rebuild_all();
|
||||
// Update corners based on new line set
|
||||
self.update_corners();
|
||||
}
|
||||
|
||||
merged
|
||||
}
|
||||
|
||||
// ─────────────────────────────────────────────────────────────────────────
|
||||
// Line Re-fitting from Accumulated Scans
|
||||
// ─────────────────────────────────────────────────────────────────────────
|
||||
|
||||
/// Re-fit all map lines from accumulated scan points.
|
||||
///
|
||||
/// This method uses points from the ScanStore to re-fit line geometry,
|
||||
/// eliminating first-scan bias. Call this periodically during exploration.
|
||||
///
|
||||
/// Requires exploration mode to be enabled.
|
||||
///
|
||||
/// # Returns
|
||||
/// Statistics about the re-fitting operation, or None if exploration mode
|
||||
/// is not enabled.
|
||||
///
|
||||
/// # Example
|
||||
/// ```rust,ignore
|
||||
/// if map.is_exploration_mode() && map.observation_count() % 10 == 0 {
|
||||
/// if let Some(stats) = map.refit_lines_from_scans() {
|
||||
/// println!("Refitted lines: {}/{} success rate",
|
||||
/// stats.lines_refitted_single + stats.lines_split,
|
||||
/// stats.total_processed());
|
||||
/// }
|
||||
/// }
|
||||
/// ```
|
||||
pub fn refit_lines_from_scans(&mut self) -> Option<RefitStats> {
|
||||
// Get exploration config and scan store
|
||||
let exploration_config = self.exploration_config.as_ref()?;
|
||||
let scan_store = self.scan_store.as_ref()?;
|
||||
|
||||
if self.line_store.is_empty() || scan_store.is_empty() {
|
||||
return Some(RefitStats::default());
|
||||
}
|
||||
|
||||
// Build association config from exploration config
|
||||
let assoc_config = PointAssociationConfig::default()
|
||||
.with_max_distance(exploration_config.max_point_distance);
|
||||
|
||||
// Associate all stored points with map lines
|
||||
let associations = associate_points_to_lines(
|
||||
scan_store,
|
||||
self.line_store.lines(),
|
||||
self.line_store.index(),
|
||||
&assoc_config,
|
||||
);
|
||||
|
||||
log::debug!(
|
||||
"Refit: {} scans, {} lines, {} points associated ({:.1}% rate)",
|
||||
scan_store.len(),
|
||||
self.line_store.lines().len(),
|
||||
associations.total_associated,
|
||||
associations.association_rate() * 100.0
|
||||
);
|
||||
|
||||
// Build robot pose map for weighted fitting
|
||||
let robot_poses: std::collections::HashMap<u32, Pose2D> = scan_store
|
||||
.iter()
|
||||
.map(|scan| (scan.scan_id, scan.estimated_pose))
|
||||
.collect();
|
||||
|
||||
// Build refit config from exploration config
|
||||
let refit_config = RefitConfig::default()
|
||||
.with_min_points(exploration_config.min_points_per_line)
|
||||
.with_gap_threshold(exploration_config.gap_threshold)
|
||||
.with_noise_model(exploration_config.noise_model);
|
||||
|
||||
// Re-fit each line
|
||||
let mut stats = RefitStats::default();
|
||||
let mut new_lines: Vec<Line2D> = Vec::new();
|
||||
|
||||
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,
|
||||
);
|
||||
|
||||
match result {
|
||||
RefitResult::Single(line) => {
|
||||
new_lines.push(line);
|
||||
stats.lines_refitted_single += 1;
|
||||
}
|
||||
RefitResult::Split(lines) => {
|
||||
stats.total_segments_created += lines.len();
|
||||
new_lines.extend(lines);
|
||||
stats.lines_split += 1;
|
||||
}
|
||||
RefitResult::Insufficient => {
|
||||
// Keep original line
|
||||
new_lines.push(original.clone());
|
||||
stats.lines_insufficient += 1;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Replace all lines and rebuild
|
||||
*self.line_store.lines_mut() = new_lines;
|
||||
self.line_store.rebuild_all();
|
||||
self.update_corners();
|
||||
|
||||
log::debug!(
|
||||
"Refit complete: {} single, {} split, {} insufficient, {} new segments",
|
||||
stats.lines_refitted_single,
|
||||
stats.lines_split,
|
||||
stats.lines_insufficient,
|
||||
stats.total_segments_created
|
||||
);
|
||||
|
||||
Some(stats)
|
||||
}
|
||||
}
|
||||
|
||||
impl Map for VectorMap {
|
||||
|
||||
@@ -9,8 +9,10 @@ use sangam_io::devices::mock::map_loader::SimulationMap;
|
||||
use sangam_io::devices::mock::noise::NoiseGenerator;
|
||||
use sangam_io::devices::mock::physics::PhysicsState;
|
||||
|
||||
use vastu_map::config::ExplorationConfig;
|
||||
use vastu_map::core::Pose2D;
|
||||
use vastu_map::core::math::normalize_angle;
|
||||
use vastu_map::integration::{ScanStore, ScanStoreConfig};
|
||||
use vastu_map::odometry::WheelOdometry;
|
||||
use vastu_map::query::PathPlanningConfig;
|
||||
use vastu_map::{Map, VectorMap, VectorMapConfig};
|
||||
@@ -166,9 +168,6 @@ pub struct TestResult {
|
||||
pub sim_time: f32,
|
||||
}
|
||||
|
||||
/// Interval for storing lidar scans in trajectory history.
|
||||
const SCAN_STORAGE_INTERVAL: usize = 5;
|
||||
|
||||
/// Per-observation record for trajectory visualization.
|
||||
#[derive(Clone, Debug)]
|
||||
pub struct ObservationRecord {
|
||||
@@ -246,6 +245,8 @@ pub struct TestHarness {
|
||||
left_encoder_ticks: i32,
|
||||
/// Simulated right encoder ticks (accumulated)
|
||||
right_encoder_ticks: i32,
|
||||
/// Scan storage for visualization (stores ALL scans)
|
||||
scan_store: ScanStore,
|
||||
}
|
||||
|
||||
impl TestHarness {
|
||||
@@ -277,6 +278,9 @@ impl TestHarness {
|
||||
let wheel_odometry =
|
||||
WheelOdometry::new(config.robot.wheel_base, config.robot.ticks_per_meter);
|
||||
|
||||
// Create scan store for visualization (stores all scans)
|
||||
let scan_store = ScanStore::new(ScanStoreConfig::default());
|
||||
|
||||
Ok(Self {
|
||||
config,
|
||||
map,
|
||||
@@ -296,6 +300,7 @@ impl TestHarness {
|
||||
wheel_odometry,
|
||||
left_encoder_ticks: 0,
|
||||
right_encoder_ticks: 0,
|
||||
scan_store,
|
||||
})
|
||||
}
|
||||
|
||||
@@ -305,6 +310,25 @@ impl TestHarness {
|
||||
self
|
||||
}
|
||||
|
||||
/// Enable exploration mode on the SLAM map.
|
||||
///
|
||||
/// In exploration mode, VectorMap stores raw scan data and periodically
|
||||
/// re-fits lines from accumulated point clouds, eliminating first-scan bias.
|
||||
///
|
||||
/// # Arguments
|
||||
/// * `config` - Exploration configuration (uses default if None)
|
||||
///
|
||||
/// # Example
|
||||
/// ```rust,ignore
|
||||
/// let harness = TestHarness::new(HarnessConfig::simple_room())?
|
||||
/// .with_exploration_mode(None); // Use default config
|
||||
/// ```
|
||||
pub fn with_exploration_mode(mut self, config: Option<ExplorationConfig>) -> Self {
|
||||
let exploration_config = config.unwrap_or_default();
|
||||
self.slam.enable_exploration_mode(exploration_config);
|
||||
self
|
||||
}
|
||||
|
||||
/// Run simulation along a path.
|
||||
pub fn run_path(&mut self, segments: &[PathSegment]) -> TestResult {
|
||||
for segment in segments {
|
||||
@@ -425,6 +449,15 @@ impl TestHarness {
|
||||
self.last_physics_pose = current_truth; // Update for metrics computation
|
||||
self.observations += 1;
|
||||
|
||||
// Store scan in scan store for visualization (stores ALL scans)
|
||||
self.scan_store.add_scan(
|
||||
odometry,
|
||||
result.pose,
|
||||
&cloud,
|
||||
result.features_extracted,
|
||||
result.confidence,
|
||||
);
|
||||
|
||||
// Record ICP convergence stats
|
||||
self.convergence_stats
|
||||
.record(result.icp_iterations, result.icp_converged);
|
||||
@@ -433,15 +466,9 @@ impl TestHarness {
|
||||
self.timing_stats.record(&result.timing);
|
||||
|
||||
// Record to trajectory history for visualization
|
||||
// Store scan at intervals to avoid excessive memory usage
|
||||
let scan_to_store = if self.observations % SCAN_STORAGE_INTERVAL == 0 {
|
||||
Some(cloud.clone())
|
||||
} else {
|
||||
None
|
||||
};
|
||||
|
||||
// Note: We no longer store scans here since ScanStore handles it
|
||||
self.trajectory
|
||||
.record(current_truth, result.pose, result.confidence, scan_to_store);
|
||||
.record(current_truth, result.pose, result.confidence, None);
|
||||
|
||||
log::debug!(
|
||||
"Observation {}: truth=({:.2}, {:.2}, {:.1}°), slam=({:.2}, {:.2}, {:.1}°), conf={:.2}, icp_iters={}",
|
||||
@@ -527,7 +554,7 @@ impl TestHarness {
|
||||
&self.map,
|
||||
final_pose,
|
||||
ground_truth_pose,
|
||||
self.last_scan_robot_frame.as_ref(),
|
||||
&self.scan_store,
|
||||
&self.trajectory,
|
||||
&metrics,
|
||||
);
|
||||
@@ -685,6 +712,11 @@ impl TestHarness {
|
||||
pub fn observation_count(&self) -> usize {
|
||||
self.observations
|
||||
}
|
||||
|
||||
/// Get scan store for visualization.
|
||||
pub fn scan_store(&self) -> &ScanStore {
|
||||
&self.scan_store
|
||||
}
|
||||
}
|
||||
|
||||
/// Result of a single simulation step.
|
||||
|
||||
@@ -26,8 +26,10 @@
|
||||
|
||||
use crate::{HarnessConfig, TestHarness};
|
||||
use vastu_map::Map;
|
||||
use vastu_map::config::ExplorationConfig as MapExplorationConfig;
|
||||
use vastu_map::core::Point2D;
|
||||
use vastu_map::exploration::{CollisionEvent, CollisionType, ExplorationConfig, VelocityCommand};
|
||||
use vastu_map::integration::CoplanarMergeConfig;
|
||||
|
||||
/// Maximum exploration cycles before timeout.
|
||||
const MAX_CYCLES: usize = 200;
|
||||
@@ -57,8 +59,12 @@ pub struct ExplorationTestResult {
|
||||
pub collisions: usize,
|
||||
/// Number of virtual walls added.
|
||||
pub virtual_walls: usize,
|
||||
/// Final map line count.
|
||||
/// Map line count before optimization.
|
||||
pub map_lines_before: usize,
|
||||
/// Final map line count (after optimization).
|
||||
pub map_lines: usize,
|
||||
/// Number of lines merged during optimization.
|
||||
pub lines_merged: usize,
|
||||
/// Initial frontier count.
|
||||
pub initial_frontiers: usize,
|
||||
/// Final frontier count (should be 0 on success).
|
||||
@@ -86,11 +92,19 @@ mod tests {
|
||||
|
||||
// Setup harness with medium_room map and visualization
|
||||
let harness_config = HarnessConfig::medium_room();
|
||||
|
||||
// Enable VectorMap exploration mode for point-cloud-based line re-fitting
|
||||
// This will re-fit lines every 10 observations using accumulated scan data
|
||||
let map_exploration_config = MapExplorationConfig::default()
|
||||
.with_refit_interval(10)
|
||||
.with_gap_threshold(0.3);
|
||||
|
||||
let mut harness = TestHarness::new(harness_config)
|
||||
.expect("Failed to create harness")
|
||||
.with_exploration_mode(Some(map_exploration_config))
|
||||
.with_visualization("results/exploration_test.svg");
|
||||
|
||||
// Create exploration config
|
||||
// Create robot exploration config (for path following)
|
||||
let exploration_config = ExplorationConfig::default()
|
||||
.with_max_linear_speed(0.3)
|
||||
.with_max_angular_speed(1.0)
|
||||
@@ -107,7 +121,10 @@ mod tests {
|
||||
println!("Cycles: {}", result.cycles);
|
||||
println!("Collisions handled: {}", result.collisions);
|
||||
println!("Virtual walls added: {}", result.virtual_walls);
|
||||
println!("Final map lines: {}", result.map_lines);
|
||||
println!(
|
||||
"Map lines: {} before optimization -> {} after ({} merged)",
|
||||
result.map_lines_before, result.map_lines, result.lines_merged
|
||||
);
|
||||
println!(
|
||||
"Frontiers: {} initial -> {} remaining",
|
||||
result.initial_frontiers, result.remaining_frontiers
|
||||
@@ -317,6 +334,25 @@ mod tests {
|
||||
);
|
||||
}
|
||||
|
||||
// Get line count before optimization
|
||||
let map_lines_before = harness.slam().lines().len();
|
||||
|
||||
// Optimize coplanar lines with tight thresholds:
|
||||
// - max_angle_diff: 1 degree = 0.01745 rad
|
||||
// - max_perpendicular_dist: 2cm = 0.02m
|
||||
let coplanar_config = CoplanarMergeConfig::default()
|
||||
.with_max_angle_diff(3.0 * std::f32::consts::PI / 180.0) // 1 degree
|
||||
.with_max_perpendicular_dist(0.05); // 2cm
|
||||
|
||||
let lines_merged = harness.slam_mut().optimize_lines(&coplanar_config);
|
||||
|
||||
println!(
|
||||
"\n Line optimization: {} lines -> {} lines ({} merged)",
|
||||
map_lines_before,
|
||||
harness.slam().lines().len(),
|
||||
lines_merged
|
||||
);
|
||||
|
||||
// Finalize to generate visualization
|
||||
let _test_result = harness.finalize();
|
||||
|
||||
@@ -326,7 +362,9 @@ mod tests {
|
||||
cycles,
|
||||
collisions,
|
||||
virtual_walls,
|
||||
map_lines_before,
|
||||
map_lines: harness.slam().lines().len(),
|
||||
lines_merged,
|
||||
initial_frontiers,
|
||||
remaining_frontiers: harness.slam().frontiers().len(),
|
||||
}
|
||||
|
||||
@@ -17,7 +17,8 @@ use svg::node::element::{
|
||||
};
|
||||
|
||||
use vastu_map::VectorMap;
|
||||
use vastu_map::core::{Point2D, PointCloud2D, Pose2D};
|
||||
use vastu_map::core::{Point2D, Pose2D};
|
||||
use vastu_map::integration::ScanStore;
|
||||
|
||||
use crate::harness::TrajectoryHistory;
|
||||
use crate::metrics::TestMetrics;
|
||||
@@ -91,7 +92,7 @@ impl Visualizer {
|
||||
map: &SimulationMap,
|
||||
final_pose: Pose2D,
|
||||
truth_pose: Pose2D,
|
||||
last_scan: Option<&PointCloud2D>,
|
||||
scan_store: &ScanStore,
|
||||
trajectory: &TrajectoryHistory,
|
||||
metrics: &TestMetrics,
|
||||
) {
|
||||
@@ -125,12 +126,13 @@ impl Visualizer {
|
||||
// Layer 1: Map walls (gray)
|
||||
doc = doc.add(self.render_map_walls(map, height as f32));
|
||||
|
||||
// Layer 2: Scan history (semi-transparent)
|
||||
doc = doc.add(self.render_scan_history(trajectory, height as f32));
|
||||
// Layer 2: All scans from scan store (semi-transparent)
|
||||
doc = doc.add(self.render_all_scans(scan_store, height as f32));
|
||||
|
||||
// Layer 3: Final lidar scan (full opacity)
|
||||
if let Some(scan) = last_scan {
|
||||
doc = doc.add(self.render_lidar_scan(scan, final_pose, height as f32));
|
||||
// Layer 3: Final lidar scan (full opacity) - use last scan from store
|
||||
if let Some(last_scan) = scan_store.latest() {
|
||||
let world_points = last_scan.world_points();
|
||||
doc = doc.add(self.render_world_points(&world_points, height as f32));
|
||||
}
|
||||
|
||||
// Layer 4: Detected walls (teal)
|
||||
@@ -277,29 +279,55 @@ impl Visualizer {
|
||||
group
|
||||
}
|
||||
|
||||
/// Render lidar scans at key points along the trajectory.
|
||||
fn render_scan_history(&self, trajectory: &TrajectoryHistory, svg_height: f32) -> Group {
|
||||
/// Render all scans from the scan store.
|
||||
fn render_all_scans(&self, scan_store: &ScanStore, svg_height: f32) -> Group {
|
||||
let mut group = Group::new().set("id", "scan_history");
|
||||
|
||||
for obs in &trajectory.observations {
|
||||
if let Some(ref scan) = obs.scan {
|
||||
// Transform scan from robot frame to world frame
|
||||
let world_scan = scan.transform(&obs.slam_pose);
|
||||
|
||||
for i in 0..world_scan.len() {
|
||||
let point = Point2D::new(world_scan.xs[i], world_scan.ys[i]);
|
||||
let (sx, sy) = self.transform_point(point, 0.0, 0.0, svg_height);
|
||||
|
||||
let circle = Circle::new()
|
||||
.set("cx", sx)
|
||||
.set("cy", sy)
|
||||
.set("r", 2.0)
|
||||
.set("fill", colors::LIDAR_SCAN_HISTORY)
|
||||
.set("opacity", SCAN_HISTORY_OPACITY);
|
||||
|
||||
group = group.add(circle);
|
||||
}
|
||||
// Skip the last scan as it will be rendered at full opacity separately
|
||||
let scan_count = scan_store.len();
|
||||
for (idx, scan) in scan_store.iter().enumerate() {
|
||||
// Skip the last scan (rendered separately at full opacity)
|
||||
if idx == scan_count - 1 {
|
||||
continue;
|
||||
}
|
||||
|
||||
// Get world-frame points directly from stored scan
|
||||
let world_points = scan.world_points();
|
||||
|
||||
for point in world_points {
|
||||
let (sx, sy) = self.transform_point(point, 0.0, 0.0, svg_height);
|
||||
|
||||
let circle = Circle::new()
|
||||
.set("cx", sx)
|
||||
.set("cy", sy)
|
||||
.set("r", 2.0)
|
||||
.set("fill", colors::LIDAR_SCAN_HISTORY)
|
||||
.set("opacity", SCAN_HISTORY_OPACITY);
|
||||
|
||||
group = group.add(circle);
|
||||
}
|
||||
}
|
||||
|
||||
group
|
||||
}
|
||||
|
||||
/// Render world-frame points (for final scan).
|
||||
fn render_world_points(&self, points: &[Point2D], svg_height: f32) -> Group {
|
||||
let mut group = Group::new().set("id", "lidar_scan");
|
||||
|
||||
for point in points {
|
||||
let (sx, sy) = self.transform_point(*point, 0.0, 0.0, svg_height);
|
||||
|
||||
let circle = Circle::new()
|
||||
.set("cx", sx)
|
||||
.set("cy", sy)
|
||||
.set("r", 3.0)
|
||||
.set("fill", colors::LIDAR_SCAN)
|
||||
.set("stroke", colors::TRUTH_PATH)
|
||||
.set("stroke-width", 0.5)
|
||||
.set("opacity", 0.8);
|
||||
|
||||
group = group.add(circle);
|
||||
}
|
||||
|
||||
group
|
||||
@@ -515,32 +543,6 @@ impl Visualizer {
|
||||
group
|
||||
}
|
||||
|
||||
/// Render lidar scan points transformed to world frame.
|
||||
fn render_lidar_scan(&self, scan: &PointCloud2D, pose: Pose2D, svg_height: f32) -> Group {
|
||||
let mut group = Group::new().set("id", "lidar_scan");
|
||||
|
||||
// Transform scan from robot frame to world frame
|
||||
let world_scan = scan.transform(&pose);
|
||||
|
||||
for i in 0..world_scan.len() {
|
||||
let point = Point2D::new(world_scan.xs[i], world_scan.ys[i]);
|
||||
let (sx, sy) = self.transform_point(point, 0.0, 0.0, svg_height);
|
||||
|
||||
let circle = Circle::new()
|
||||
.set("cx", sx)
|
||||
.set("cy", sy)
|
||||
.set("r", 3.0)
|
||||
.set("fill", colors::LIDAR_SCAN)
|
||||
.set("stroke", colors::TRUTH_PATH)
|
||||
.set("stroke-width", 0.5)
|
||||
.set("opacity", 0.8);
|
||||
|
||||
group = group.add(circle);
|
||||
}
|
||||
|
||||
group
|
||||
}
|
||||
|
||||
fn render_poses(&self, estimated: Pose2D, truth: Pose2D, svg_height: f32) -> Group {
|
||||
let mut group = Group::new().set("id", "poses");
|
||||
|
||||
|
||||
Reference in New Issue
Block a user