Fix clippy warnings in vastu-map

- Remove unused max_points/max_lines fields from IcpScratchSpace
- Add #[allow(clippy::needless_range_loop)] for SoA array indexing
- Add EndpointGrid type alias to reduce type complexity

vastu-map/.cargo/config.toml

@@ -0,0 +1,47 @@
+# Cross-compilation configuration for ARM targets
+#
+# Baseline target: Allwinner A33 (Cortex-A7) with NEON + VFPv4
+# This configuration also works on:
+# - Cortex-A8, A9, A15 (ARMv7-A with NEON)
+# - Cortex-A53, A55, A72 (ARMv8-A, backward compatible)
+#
+# Performance Optimizations:
+# - target-cpu=cortex-a7: Tune scheduling for Cortex-A7
+# - +neon: Enable NEON SIMD (128-bit vectors)
+# - +vfpv4: Enable VFPv4 floating point with FMA
+# - opt-level=3: Maximum optimization
+# - llvm-args: Enable fast-math and loop vectorization
+
+[target.armv7-unknown-linux-musleabihf]
+linker = "arm-linux-gnueabihf-gcc"
+rustflags = [
+    # CPU targeting
+    "-C", "target-cpu=cortex-a7",
+    "-C", "target-feature=+neon,+vfpv4",
+
+    # Optimization flags
+    "-C", "opt-level=3",
+
+    # LLVM optimization passes for better auto-vectorization
+    "-C", "llvm-args=-enable-unsafe-fp-math",
+    "-C", "llvm-args=-vectorize-loops",
+    "-C", "llvm-args=-vectorize-slp",
+
+    # Linker
+    "-C", "link-arg=-lgcc",
+]
+
+# Host machine (macOS/Linux x86_64) - enable AVX2 for development testing
+[target.x86_64-apple-darwin]
+rustflags = [
+    "-C", "target-cpu=native",
+]
+
+[target.aarch64-apple-darwin]
+rustflags = [
+    "-C", "target-cpu=native",
+]
+
+# Uncomment for ARM-only development
+# [build]
+# target = "armv7-unknown-linux-musleabihf"

vastu-map/Cargo.toml

@@ -0,0 +1,50 @@
+[package]
+name = "vastu-map"
+version = "0.1.0"
+edition = "2024"
+description = "Feature-based 2D SLAM library optimized for ARM NEON"
+license = "MIT"
+
+[features]
+default = []
+integration-tests = ["dep:sangam-io", "dep:svg", "dep:image", "dep:serde_yaml"]
+
+[dependencies]
+rstar = "0.12"           # R-tree spatial index
+log = "0.4"              # Logging
+approx = "0.5"           # Approximate comparisons
+rand = "0.9"             # RANSAC randomness
+
+# Integration test dependencies (optional)
+sangam-io = { path = "../sangam-io", features = ["mock"], optional = true }
+svg = { version = "0.17", optional = true }
+image = { version = "0.25", default-features = false, features = ["png", "pnm"], optional = true }
+serde_yaml = { version = "0.9", optional = true }
+serde = { version = "1.0", features = ["derive"] }
+
+[dev-dependencies]
+criterion = "0.5"        # Benchmarks
+env_logger = "0.11"      # Test logging
+
+[[test]]
+name = "integration"
+path = "tests/integration/mod.rs"
+required-features = ["integration-tests"]
+
+[profile.release]
+opt-level = 3
+lto = true
+codegen-units = 1        # Better optimization, slower compile
+panic = "abort"          # Smaller binary, no unwinding overhead
+strip = true             # Strip debug symbols for smaller binary
+
+# Release with debug info (for profiling)
+[profile.release-with-debug]
+inherits = "release"
+debug = true
+strip = false
+
+# Benchmarks (uncomment when ready)
+# [[bench]]
+# name = "simd_bench"
+# harness = false

vastu-map/src/config.rs

@@ -0,0 +1,247 @@
+//! Configuration for VectorMap SLAM.
+//!
+//! All configuration parameters have sensible defaults for indoor robot navigation.
+
+use std::f32::consts::PI;
+
+/// Configuration for VectorMap SLAM.
+///
+/// All parameters have sensible defaults. Create with `Default::default()`
+/// and modify as needed.
+#[derive(Clone, Debug)]
+pub struct VectorMapConfig {
+    // ─────────────────────────────────────────────────────────────────────────
+    // Input Filtering
+    // ─────────────────────────────────────────────────────────────────────────
+    /// Minimum quality value for scan points (0-255).
+    /// Points below this threshold are filtered out.
+    /// Default: 10
+    pub min_scan_quality: u8,
+
+    /// Minimum valid range in meters.
+    /// Points closer than this are filtered out.
+    /// Default: 0.15m
+    pub min_scan_range: f32,
+
+    /// Maximum valid range in meters.
+    /// Points farther than this are filtered out.
+    /// Default: 12.0m
+    pub max_scan_range: f32,
+
+    // ─────────────────────────────────────────────────────────────────────────
+    // Line Extraction (Split-and-Merge)
+    // ─────────────────────────────────────────────────────────────────────────
+    /// Maximum perpendicular distance from point to line for splitting.
+    /// Points deviating more than this trigger a split.
+    /// Default: 0.05m (5cm)
+    pub split_distance_threshold: f32,
+
+    /// Minimum line length to keep.
+    /// Shorter lines are discarded.
+    /// Default: 0.10m (10cm)
+    pub min_line_length: f32,
+
+    /// Minimum number of points to form a line.
+    /// Default: 5
+    pub min_points_per_line: usize,
+
+    /// Maximum gap between consecutive points to be considered continuous.
+    /// Gaps larger than this start a new line segment.
+    /// Default: 0.30m (30cm)
+    pub max_point_gap: f32,
+
+    // ─────────────────────────────────────────────────────────────────────────
+    // Corner Detection
+    // ─────────────────────────────────────────────────────────────────────────
+    /// Minimum angle between lines to form a corner (radians).
+    /// Default: 30° (π/6)
+    pub corner_angle_min: f32,
+
+    /// Maximum angle between lines to form a corner (radians).
+    /// Default: 150° (5π/6)
+    pub corner_angle_max: f32,
+
+    /// Maximum distance between line endpoints to form a corner.
+    /// Default: 0.05m (5cm)
+    pub corner_distance_threshold: f32,
+
+    // ─────────────────────────────────────────────────────────────────────────
+    // Line Association (Map Integration)
+    // ─────────────────────────────────────────────────────────────────────────
+    /// Maximum angular difference between line directions for association (radians).
+    /// Default: 10° (π/18)
+    pub association_angle_tolerance: f32,
+
+    /// Maximum perpendicular distance between lines for association.
+    /// Default: 0.05m (5cm)
+    pub association_distance_tolerance: f32,
+
+    /// Minimum overlap ratio required for merging lines.
+    /// Default: 0.3 (30%)
+    pub association_overlap_ratio: f32,
+
+    // ─────────────────────────────────────────────────────────────────────────
+    // Scan Matching
+    // ─────────────────────────────────────────────────────────────────────────
+    /// Maximum iterations for Point-to-Line ICP.
+    /// Default: 50
+    pub icp_max_iterations: usize,
+
+    /// Convergence threshold for ICP (translation change in meters).
+    /// ICP stops when pose change is below this.
+    /// Default: 0.001m (1mm)
+    pub icp_convergence_threshold: f32,
+
+    /// Maximum rotation change for ICP convergence (radians).
+    /// Default: 0.001 rad (~0.06°)
+    pub icp_rotation_threshold: f32,
+
+    /// Maximum correspondence distance for ICP.
+    /// Points farther from any line are treated as outliers.
+    /// Default: 0.30m (30cm)
+    pub icp_max_correspondence_dist: f32,
+
+    /// Minimum percentage of points with valid correspondences.
+    /// Below this, scan matching is considered failed.
+    /// Default: 0.3 (30%)
+    pub icp_min_correspondence_ratio: f32,
+
+    /// Number of RANSAC iterations for coarse alignment.
+    /// Default: 100
+    pub ransac_iterations: usize,
+
+    /// Inlier distance threshold for RANSAC.
+    /// Default: 0.05m (5cm)
+    pub ransac_inlier_threshold: f32,
+
+    // ─────────────────────────────────────────────────────────────────────────
+    // Confidence Thresholds
+    // ─────────────────────────────────────────────────────────────────────────
+    /// Minimum confidence to accept a scan match.
+    /// Below this, odometry is used instead.
+    /// Default: 0.3
+    pub min_match_confidence: f32,
+}
+
+impl Default for VectorMapConfig {
+    fn default() -> Self {
+        Self {
+            // Input filtering
+            min_scan_quality: 10,
+            min_scan_range: 0.15,
+            max_scan_range: 12.0,
+
+            // Line extraction
+            split_distance_threshold: 0.05,
+            min_line_length: 0.10,
+            min_points_per_line: 5,
+            max_point_gap: 0.30,
+
+            // Corner detection
+            corner_angle_min: PI / 6.0,       // 30°
+            corner_angle_max: 5.0 * PI / 6.0, // 150°
+            corner_distance_threshold: 0.05,
+
+            // Line association
+            association_angle_tolerance: PI / 18.0, // 10°
+            association_distance_tolerance: 0.05,
+            association_overlap_ratio: 0.3,
+
+            // Scan matching
+            icp_max_iterations: 50,
+            icp_convergence_threshold: 0.001,
+            icp_rotation_threshold: 0.001,
+            icp_max_correspondence_dist: 0.30,
+            icp_min_correspondence_ratio: 0.3,
+            ransac_iterations: 100,
+            ransac_inlier_threshold: 0.05,
+
+            // Confidence
+            min_match_confidence: 0.3,
+        }
+    }
+}
+
+impl VectorMapConfig {
+    /// Create a new configuration with default values.
+    pub fn new() -> Self {
+        Self::default()
+    }
+
+    /// Builder-style setter for min_scan_quality.
+    pub fn with_min_scan_quality(mut self, value: u8) -> Self {
+        self.min_scan_quality = value;
+        self
+    }
+
+    /// Builder-style setter for scan range.
+    pub fn with_scan_range(mut self, min: f32, max: f32) -> Self {
+        self.min_scan_range = min;
+        self.max_scan_range = max;
+        self
+    }
+
+    /// Builder-style setter for split distance threshold.
+    pub fn with_split_threshold(mut self, value: f32) -> Self {
+        self.split_distance_threshold = value;
+        self
+    }
+
+    /// Builder-style setter for minimum line length.
+    pub fn with_min_line_length(mut self, value: f32) -> Self {
+        self.min_line_length = value;
+        self
+    }
+
+    /// Builder-style setter for ICP parameters.
+    pub fn with_icp_params(mut self, max_iterations: usize, convergence_threshold: f32) -> Self {
+        self.icp_max_iterations = max_iterations;
+        self.icp_convergence_threshold = convergence_threshold;
+        self
+    }
+
+    /// Builder-style setter for RANSAC iterations.
+    pub fn with_ransac_iterations(mut self, value: usize) -> Self {
+        self.ransac_iterations = value;
+        self
+    }
+}
+
+#[cfg(test)]
+mod tests {
+    use super::*;
+
+    #[test]
+    fn test_default_config() {
+        let config = VectorMapConfig::default();
+
+        assert_eq!(config.min_scan_quality, 10);
+        assert_eq!(config.min_scan_range, 0.15);
+        assert_eq!(config.max_scan_range, 12.0);
+        assert_eq!(config.split_distance_threshold, 0.05);
+        assert_eq!(config.min_line_length, 0.10);
+        assert_eq!(config.min_points_per_line, 5);
+        assert_eq!(config.icp_max_iterations, 50);
+        assert_eq!(config.ransac_iterations, 100);
+    }
+
+    #[test]
+    fn test_builder_pattern() {
+        let config = VectorMapConfig::new()
+            .with_min_scan_quality(20)
+            .with_scan_range(0.10, 8.0)
+            .with_split_threshold(0.03)
+            .with_min_line_length(0.15)
+            .with_icp_params(100, 0.0005)
+            .with_ransac_iterations(200);
+
+        assert_eq!(config.min_scan_quality, 20);
+        assert_eq!(config.min_scan_range, 0.10);
+        assert_eq!(config.max_scan_range, 8.0);
+        assert_eq!(config.split_distance_threshold, 0.03);
+        assert_eq!(config.min_line_length, 0.15);
+        assert_eq!(config.icp_max_iterations, 100);
+        assert_eq!(config.icp_convergence_threshold, 0.0005);
+        assert_eq!(config.ransac_iterations, 200);
+    }
+}

vastu-map/src/core/bounds.rs

@@ -0,0 +1,332 @@
+//! Bounding box type for map extents.
+
+use super::point::Point2D;
+
+/// Axis-aligned bounding box.
+///
+/// Represents a rectangular region in 2D space.
+#[derive(Clone, Copy, Debug, Default, PartialEq)]
+pub struct Bounds {
+    /// Minimum corner (smallest x and y values).
+    pub min: Point2D,
+    /// Maximum corner (largest x and y values).
+    pub max: Point2D,
+}
+
+impl Bounds {
+    /// Create a new bounding box from min and max corners.
+    #[inline]
+    pub const fn new(min: Point2D, max: Point2D) -> Self {
+        Self { min, max }
+    }
+
+    /// Create an empty (invalid) bounding box.
+    ///
+    /// The empty bounds has min > max, so it will expand to fit any point.
+    #[inline]
+    pub fn empty() -> Self {
+        Self {
+            min: Point2D::new(f32::INFINITY, f32::INFINITY),
+            max: Point2D::new(f32::NEG_INFINITY, f32::NEG_INFINITY),
+        }
+    }
+
+    /// Check if the bounds are empty (invalid).
+    #[inline]
+    pub fn is_empty(&self) -> bool {
+        self.min.x > self.max.x || self.min.y > self.max.y
+    }
+
+    /// Create bounds from a single point.
+    #[inline]
+    pub fn from_point(point: Point2D) -> Self {
+        Self {
+            min: point,
+            max: point,
+        }
+    }
+
+    /// Create bounds from center and half-size.
+    #[inline]
+    pub fn from_center_half_size(center: Point2D, half_size: Point2D) -> Self {
+        Self {
+            min: center - half_size,
+            max: center + half_size,
+        }
+    }
+
+    /// Width of the bounding box (x extent).
+    #[inline]
+    pub fn width(&self) -> f32 {
+        self.max.x - self.min.x
+    }
+
+    /// Height of the bounding box (y extent).
+    #[inline]
+    pub fn height(&self) -> f32 {
+        self.max.y - self.min.y
+    }
+
+    /// Size of the bounding box as a Point2D (width, height).
+    #[inline]
+    pub fn size(&self) -> Point2D {
+        Point2D::new(self.width(), self.height())
+    }
+
+    /// Center of the bounding box.
+    #[inline]
+    pub fn center(&self) -> Point2D {
+        Point2D::new(
+            (self.min.x + self.max.x) * 0.5,
+            (self.min.y + self.max.y) * 0.5,
+        )
+    }
+
+    /// Half-size of the bounding box.
+    #[inline]
+    pub fn half_size(&self) -> Point2D {
+        Point2D::new(self.width() * 0.5, self.height() * 0.5)
+    }
+
+    /// Area of the bounding box.
+    #[inline]
+    pub fn area(&self) -> f32 {
+        self.width() * self.height()
+    }
+
+    /// Check if a point is inside the bounding box.
+    #[inline]
+    pub fn contains(&self, point: Point2D) -> bool {
+        point.x >= self.min.x
+            && point.x <= self.max.x
+            && point.y >= self.min.y
+            && point.y <= self.max.y
+    }
+
+    /// Check if this bounds intersects with another.
+    #[inline]
+    pub fn intersects(&self, other: &Bounds) -> bool {
+        self.min.x <= other.max.x
+            && self.max.x >= other.min.x
+            && self.min.y <= other.max.y
+            && self.max.y >= other.min.y
+    }
+
+    /// Compute the intersection of two bounds.
+    ///
+    /// Returns empty bounds if they don't intersect.
+    #[inline]
+    pub fn intersection(&self, other: &Bounds) -> Self {
+        Self {
+            min: self.min.max(other.min),
+            max: self.max.min(other.max),
+        }
+    }
+
+    /// Compute the union of two bounds (smallest box containing both).
+    #[inline]
+    pub fn union(&self, other: &Bounds) -> Self {
+        if self.is_empty() {
+            return *other;
+        }
+        if other.is_empty() {
+            return *self;
+        }
+        Self {
+            min: self.min.min(other.min),
+            max: self.max.max(other.max),
+        }
+    }
+
+    /// Expand bounds to include a point.
+    #[inline]
+    pub fn expand_to_include(&mut self, point: Point2D) {
+        self.min = self.min.min(point);
+        self.max = self.max.max(point);
+    }
+
+    /// Expand bounds by a margin on all sides.
+    #[inline]
+    pub fn expand(&self, margin: f32) -> Self {
+        Self {
+            min: Point2D::new(self.min.x - margin, self.min.y - margin),
+            max: Point2D::new(self.max.x + margin, self.max.y + margin),
+        }
+    }
+
+    /// Clamp a point to be inside the bounds.
+    #[inline]
+    pub fn clamp(&self, point: Point2D) -> Point2D {
+        Point2D::new(
+            point.x.max(self.min.x).min(self.max.x),
+            point.y.max(self.min.y).min(self.max.y),
+        )
+    }
+
+    /// Get corners of the bounding box.
+    ///
+    /// Returns [min, (max.x, min.y), max, (min.x, max.y)] in CCW order.
+    #[inline]
+    pub fn corners(&self) -> [Point2D; 4] {
+        [
+            self.min,
+            Point2D::new(self.max.x, self.min.y),
+            self.max,
+            Point2D::new(self.min.x, self.max.y),
+        ]
+    }
+}
+
+#[cfg(test)]
+mod tests {
+    use super::*;
+
+    #[test]
+    fn test_new() {
+        let bounds = Bounds::new(Point2D::new(0.0, 0.0), Point2D::new(10.0, 20.0));
+
+        assert_eq!(bounds.min, Point2D::new(0.0, 0.0));
+        assert_eq!(bounds.max, Point2D::new(10.0, 20.0));
+    }
+
+    #[test]
+    fn test_empty() {
+        let bounds = Bounds::empty();
+        assert!(bounds.is_empty());
+
+        let valid = Bounds::new(Point2D::new(0.0, 0.0), Point2D::new(1.0, 1.0));
+        assert!(!valid.is_empty());
+    }
+
+    #[test]
+    fn test_from_point() {
+        let bounds = Bounds::from_point(Point2D::new(5.0, 10.0));
+
+        assert_eq!(bounds.min, Point2D::new(5.0, 10.0));
+        assert_eq!(bounds.max, Point2D::new(5.0, 10.0));
+        assert_eq!(bounds.width(), 0.0);
+        assert_eq!(bounds.height(), 0.0);
+    }
+
+    #[test]
+    fn test_from_center_half_size() {
+        let bounds = Bounds::from_center_half_size(Point2D::new(5.0, 5.0), Point2D::new(2.0, 3.0));
+
+        assert_eq!(bounds.min, Point2D::new(3.0, 2.0));
+        assert_eq!(bounds.max, Point2D::new(7.0, 8.0));
+    }
+
+    #[test]
+    fn test_dimensions() {
+        let bounds = Bounds::new(Point2D::new(1.0, 2.0), Point2D::new(5.0, 8.0));
+
+        assert_eq!(bounds.width(), 4.0);
+        assert_eq!(bounds.height(), 6.0);
+        assert_eq!(bounds.size(), Point2D::new(4.0, 6.0));
+        assert_eq!(bounds.center(), Point2D::new(3.0, 5.0));
+        assert_eq!(bounds.half_size(), Point2D::new(2.0, 3.0));
+        assert_eq!(bounds.area(), 24.0);
+    }
+
+    #[test]
+    fn test_contains() {
+        let bounds = Bounds::new(Point2D::new(0.0, 0.0), Point2D::new(10.0, 10.0));
+
+        assert!(bounds.contains(Point2D::new(5.0, 5.0)));
+        assert!(bounds.contains(Point2D::new(0.0, 0.0))); // Edge
+        assert!(bounds.contains(Point2D::new(10.0, 10.0))); // Edge
+        assert!(!bounds.contains(Point2D::new(-1.0, 5.0)));
+        assert!(!bounds.contains(Point2D::new(5.0, 11.0)));
+    }
+
+    #[test]
+    fn test_intersects() {
+        let a = Bounds::new(Point2D::new(0.0, 0.0), Point2D::new(10.0, 10.0));
+        let b = Bounds::new(Point2D::new(5.0, 5.0), Point2D::new(15.0, 15.0));
+        let c = Bounds::new(Point2D::new(20.0, 20.0), Point2D::new(30.0, 30.0));
+
+        assert!(a.intersects(&b));
+        assert!(b.intersects(&a));
+        assert!(!a.intersects(&c));
+        assert!(!c.intersects(&a));
+    }
+
+    #[test]
+    fn test_intersection() {
+        let a = Bounds::new(Point2D::new(0.0, 0.0), Point2D::new(10.0, 10.0));
+        let b = Bounds::new(Point2D::new(5.0, 5.0), Point2D::new(15.0, 15.0));
+
+        let inter = a.intersection(&b);
+
+        assert_eq!(inter.min, Point2D::new(5.0, 5.0));
+        assert_eq!(inter.max, Point2D::new(10.0, 10.0));
+    }
+
+    #[test]
+    fn test_union() {
+        let a = Bounds::new(Point2D::new(0.0, 0.0), Point2D::new(10.0, 10.0));
+        let b = Bounds::new(Point2D::new(5.0, 5.0), Point2D::new(15.0, 15.0));
+
+        let u = a.union(&b);
+
+        assert_eq!(u.min, Point2D::new(0.0, 0.0));
+        assert_eq!(u.max, Point2D::new(15.0, 15.0));
+    }
+
+    #[test]
+    fn test_union_with_empty() {
+        let a = Bounds::new(Point2D::new(0.0, 0.0), Point2D::new(10.0, 10.0));
+        let empty = Bounds::empty();
+
+        assert_eq!(a.union(&empty), a);
+        assert_eq!(empty.union(&a), a);
+    }
+
+    #[test]
+    fn test_expand_to_include() {
+        let mut bounds = Bounds::empty();
+
+        bounds.expand_to_include(Point2D::new(5.0, 5.0));
+        assert_eq!(bounds.min, Point2D::new(5.0, 5.0));
+        assert_eq!(bounds.max, Point2D::new(5.0, 5.0));
+
+        bounds.expand_to_include(Point2D::new(0.0, 10.0));
+        assert_eq!(bounds.min, Point2D::new(0.0, 5.0));
+        assert_eq!(bounds.max, Point2D::new(5.0, 10.0));
+    }
+
+    #[test]
+    fn test_expand() {
+        let bounds = Bounds::new(Point2D::new(5.0, 5.0), Point2D::new(10.0, 10.0));
+        let expanded = bounds.expand(2.0);
+
+        assert_eq!(expanded.min, Point2D::new(3.0, 3.0));
+        assert_eq!(expanded.max, Point2D::new(12.0, 12.0));
+    }
+
+    #[test]
+    fn test_clamp() {
+        let bounds = Bounds::new(Point2D::new(0.0, 0.0), Point2D::new(10.0, 10.0));
+
+        assert_eq!(bounds.clamp(Point2D::new(5.0, 5.0)), Point2D::new(5.0, 5.0));
+        assert_eq!(
+            bounds.clamp(Point2D::new(-5.0, 5.0)),
+            Point2D::new(0.0, 5.0)
+        );
+        assert_eq!(
+            bounds.clamp(Point2D::new(15.0, 15.0)),
+            Point2D::new(10.0, 10.0)
+        );
+    }
+
+    #[test]
+    fn test_corners() {
+        let bounds = Bounds::new(Point2D::new(0.0, 0.0), Point2D::new(10.0, 20.0));
+        let corners = bounds.corners();
+
+        assert_eq!(corners[0], Point2D::new(0.0, 0.0));
+        assert_eq!(corners[1], Point2D::new(10.0, 0.0));
+        assert_eq!(corners[2], Point2D::new(10.0, 20.0));
+        assert_eq!(corners[3], Point2D::new(0.0, 20.0));
+    }
+}

vastu-map/src/core/math.rs

@@ -0,0 +1,173 @@
+//! Mathematical utilities for angles and geometry.
+//!
+//! All angles are in radians. Coordinate frame follows ROS REP-103:
+//! - X-forward, Y-left, Z-up
+//! - Counter-clockwise positive rotation
+
+use std::f32::consts::PI;
+
+/// Two times PI (full circle in radians).
+pub const TWO_PI: f32 = 2.0 * PI;
+
+/// Normalize angle to [-π, π).
+///
+/// # Example
+/// ```
+/// use vastu_map::core::math::normalize_angle;
+/// use std::f32::consts::PI;
+///
+/// // Values near ±π may normalize to either +π or -π due to floating-point
+/// assert!(normalize_angle(3.0 * PI).abs() - PI < 1e-5);
+/// assert!(normalize_angle(-3.0 * PI).abs() - PI < 1e-5);
+/// assert!((normalize_angle(PI / 2.0) - PI / 2.0).abs() < 1e-6);
+/// ```
+#[inline]
+pub fn normalize_angle(angle: f32) -> f32 {
+    let mut a = angle % TWO_PI;
+    if a >= PI {
+        a -= TWO_PI;
+    } else if a < -PI {
+        a += TWO_PI;
+    }
+    a
+}
+
+/// Compute the signed angular difference between two angles.
+///
+/// Returns the shortest angular distance from `from` to `to`,
+/// in the range [-π, π).
+///
+/// Positive result means counter-clockwise rotation from `from` to `to`.
+///
+/// # Example
+/// ```
+/// use vastu_map::core::math::angle_diff;
+/// use std::f32::consts::PI;
+///
+/// // 90° difference
+/// let diff = angle_diff(0.0, PI / 2.0);
+/// assert!((diff - PI / 2.0).abs() < 1e-6);
+///
+/// // Crossing -π/π boundary
+/// let diff = angle_diff(-0.9 * PI, 0.9 * PI);
+/// assert!((diff - (-0.2 * PI)).abs() < 1e-5);
+/// ```
+#[inline]
+pub fn angle_diff(from: f32, to: f32) -> f32 {
+    normalize_angle(to - from)
+}
+
+/// Check if two angles are approximately equal (within tolerance).
+///
+/// Handles wrap-around at ±π correctly.
+///
+/// # Example
+/// ```
+/// use vastu_map::core::math::angles_approx_equal;
+/// use std::f32::consts::PI;
+///
+/// assert!(angles_approx_equal(PI - 0.001, -PI + 0.001, 0.01));
+/// assert!(!angles_approx_equal(0.0, PI, 0.1));
+/// ```
+#[inline]
+pub fn angles_approx_equal(a: f32, b: f32, tolerance: f32) -> bool {
+    angle_diff(a, b).abs() <= tolerance
+}
+
+/// Convert degrees to radians.
+#[inline]
+pub fn deg_to_rad(deg: f32) -> f32 {
+    deg * PI / 180.0
+}
+
+/// Convert radians to degrees.
+#[inline]
+pub fn rad_to_deg(rad: f32) -> f32 {
+    rad * 180.0 / PI
+}
+
+/// Clamp a value to a range.
+#[inline]
+pub fn clamp(value: f32, min: f32, max: f32) -> f32 {
+    value.max(min).min(max)
+}
+
+/// Linear interpolation between two values.
+#[inline]
+pub fn lerp(a: f32, b: f32, t: f32) -> f32 {
+    a + (b - a) * t
+}
+
+/// Square of a value. Useful for avoiding `pow(x, 2)`.
+#[inline]
+pub fn sq(x: f32) -> f32 {
+    x * x
+}
+
+#[cfg(test)]
+mod tests {
+    use super::*;
+    use approx::assert_relative_eq;
+
+    #[test]
+    fn test_normalize_angle() {
+        assert_relative_eq!(normalize_angle(0.0), 0.0, epsilon = 1e-6);
+        // At ±π boundary, floating-point may give +π or -π; both are valid
+        assert!(normalize_angle(PI).abs() - PI < 1e-6);
+        assert!(normalize_angle(-PI).abs() - PI < 1e-6);
+        assert_relative_eq!(normalize_angle(TWO_PI), 0.0, epsilon = 1e-6);
+        // For 3π, should normalize to approximately ±π
+        assert!(normalize_angle(3.0 * PI).abs() - PI < 1e-5);
+        assert!(normalize_angle(-3.0 * PI).abs() - PI < 1e-5);
+        assert_relative_eq!(normalize_angle(PI / 2.0), PI / 2.0, epsilon = 1e-6);
+        assert_relative_eq!(normalize_angle(-PI / 2.0), -PI / 2.0, epsilon = 1e-6);
+    }
+
+    #[test]
+    fn test_angle_diff() {
+        assert_relative_eq!(angle_diff(0.0, PI / 2.0), PI / 2.0, epsilon = 1e-6);
+        assert_relative_eq!(angle_diff(PI / 2.0, 0.0), -PI / 2.0, epsilon = 1e-6);
+        assert_relative_eq!(angle_diff(0.0, PI), -PI, epsilon = 1e-6);
+        assert_relative_eq!(angle_diff(0.0, -PI), -PI, epsilon = 1e-6);
+
+        // Crossing boundary
+        assert_relative_eq!(angle_diff(-0.9 * PI, 0.9 * PI), -0.2 * PI, epsilon = 1e-5);
+        assert_relative_eq!(angle_diff(0.9 * PI, -0.9 * PI), 0.2 * PI, epsilon = 1e-5);
+    }
+
+    #[test]
+    fn test_angles_approx_equal() {
+        assert!(angles_approx_equal(0.0, 0.001, 0.01));
+        assert!(angles_approx_equal(PI - 0.001, -PI + 0.001, 0.01));
+        assert!(!angles_approx_equal(0.0, PI, 0.1));
+    }
+
+    #[test]
+    fn test_deg_rad_conversion() {
+        assert_relative_eq!(deg_to_rad(180.0), PI, epsilon = 1e-6);
+        assert_relative_eq!(deg_to_rad(90.0), PI / 2.0, epsilon = 1e-6);
+        assert_relative_eq!(rad_to_deg(PI), 180.0, epsilon = 1e-6);
+        assert_relative_eq!(rad_to_deg(PI / 2.0), 90.0, epsilon = 1e-6);
+    }
+
+    #[test]
+    fn test_clamp() {
+        assert_eq!(clamp(5.0, 0.0, 10.0), 5.0);
+        assert_eq!(clamp(-5.0, 0.0, 10.0), 0.0);
+        assert_eq!(clamp(15.0, 0.0, 10.0), 10.0);
+    }
+
+    #[test]
+    fn test_lerp() {
+        assert_eq!(lerp(0.0, 10.0, 0.0), 0.0);
+        assert_eq!(lerp(0.0, 10.0, 1.0), 10.0);
+        assert_eq!(lerp(0.0, 10.0, 0.5), 5.0);
+    }
+
+    #[test]
+    fn test_sq() {
+        assert_eq!(sq(2.0), 4.0);
+        assert_eq!(sq(-3.0), 9.0);
+        assert_eq!(sq(0.0), 0.0);
+    }
+}

vastu-map/src/core/mod.rs

@@ -0,0 +1,23 @@
+//! Core types for vastu-map.
+//!
+//! This module provides fundamental geometric types:
+//! - [`Point2D`]: 2D point/vector
+//! - [`Pose2D`]: 2D position and orientation
+//! - [`PolarScan`]: Lidar scan in polar coordinates
+//! - [`PointCloud2D`]: Point cloud with SoA layout
+//! - [`Bounds`]: Axis-aligned bounding box
+//!
+//! All coordinates use the ROS REP-103 convention:
+//! - X-forward, Y-left, Z-up (right-handed)
+//! - Counter-clockwise positive rotation
+
+pub mod bounds;
+pub mod math;
+pub mod point;
+pub mod pose;
+pub mod scan;
+
+pub use bounds::Bounds;
+pub use point::Point2D;
+pub use pose::Pose2D;
+pub use scan::{PointCloud2D, PolarScan};

vastu-map/src/core/point.rs

@@ -0,0 +1,387 @@
+//! 2D point type for geometric operations.
+//!
+//! Coordinate frame follows ROS REP-103:
+//! - X-forward, Y-left, Z-up (right-handed)
+//! - Counter-clockwise positive rotation
+
+use std::ops::{Add, Div, Mul, Neg, Sub};
+
+/// A 2D point/vector in meters.
+///
+/// Uses the ROS REP-103 coordinate convention:
+/// - X: forward (positive ahead of robot)
+/// - Y: left (positive to the left of robot)
+///
+/// This type is used for both points and direction vectors.
+#[derive(Clone, Copy, Debug, Default, PartialEq)]
+pub struct Point2D {
+    /// X coordinate in meters (forward direction).
+    pub x: f32,
+    /// Y coordinate in meters (left direction).
+    pub y: f32,
+}
+
+impl Point2D {
+    /// Create a new point from x and y coordinates.
+    #[inline]
+    pub const fn new(x: f32, y: f32) -> Self {
+        Self { x, y }
+    }
+
+    /// Create a zero point (origin).
+    #[inline]
+    pub const fn zero() -> Self {
+        Self { x: 0.0, y: 0.0 }
+    }
+
+    /// Create a point from polar coordinates.
+    ///
+    /// # Arguments
+    /// * `angle` - Angle in radians from X axis (CCW positive)
+    /// * `distance` - Distance from origin in meters
+    #[inline]
+    pub fn from_polar(angle: f32, distance: f32) -> Self {
+        Self {
+            x: distance * angle.cos(),
+            y: distance * angle.sin(),
+        }
+    }
+
+    /// Squared distance from origin (avoids sqrt).
+    #[inline]
+    pub fn length_squared(self) -> f32 {
+        self.x * self.x + self.y * self.y
+    }
+
+    /// Distance from origin.
+    #[inline]
+    pub fn length(self) -> f32 {
+        self.length_squared().sqrt()
+    }
+
+    /// Squared distance to another point (avoids sqrt).
+    #[inline]
+    pub fn distance_squared(self, other: Point2D) -> f32 {
+        (self - other).length_squared()
+    }
+
+    /// Distance to another point.
+    #[inline]
+    pub fn distance(self, other: Point2D) -> f32 {
+        self.distance_squared(other).sqrt()
+    }
+
+    /// Normalize to unit length. Returns zero vector if length is zero.
+    #[inline]
+    pub fn normalized(self) -> Self {
+        let len = self.length();
+        if len > f32::EPSILON {
+            self / len
+        } else {
+            Self::zero()
+        }
+    }
+
+    /// Dot product with another vector.
+    #[inline]
+    pub fn dot(self, other: Point2D) -> f32 {
+        self.x * other.x + self.y * other.y
+    }
+
+    /// 2D cross product (returns scalar z-component).
+    ///
+    /// This is the signed area of the parallelogram formed by the two vectors.
+    /// Positive if `other` is counter-clockwise from `self`.
+    #[inline]
+    pub fn cross(self, other: Point2D) -> f32 {
+        self.x * other.y - self.y * other.x
+    }
+
+    /// Perpendicular vector (90° counter-clockwise rotation).
+    #[inline]
+    pub fn perpendicular(self) -> Self {
+        Self {
+            x: -self.y,
+            y: self.x,
+        }
+    }
+
+    /// Angle from X axis (in radians, range [-π, π]).
+    #[inline]
+    pub fn angle(self) -> f32 {
+        self.y.atan2(self.x)
+    }
+
+    /// Rotate point by angle (radians, counter-clockwise positive).
+    #[inline]
+    pub fn rotate(self, angle: f32) -> Self {
+        let (sin, cos) = angle.sin_cos();
+        Self {
+            x: self.x * cos - self.y * sin,
+            y: self.x * sin + self.y * cos,
+        }
+    }
+
+    /// Linear interpolation between this point and another.
+    #[inline]
+    pub fn lerp(self, other: Point2D, t: f32) -> Self {
+        Self {
+            x: self.x + (other.x - self.x) * t,
+            y: self.y + (other.y - self.y) * t,
+        }
+    }
+
+    /// Component-wise minimum.
+    #[inline]
+    pub fn min(self, other: Point2D) -> Self {
+        Self {
+            x: self.x.min(other.x),
+            y: self.y.min(other.y),
+        }
+    }
+
+    /// Component-wise maximum.
+    #[inline]
+    pub fn max(self, other: Point2D) -> Self {
+        Self {
+            x: self.x.max(other.x),
+            y: self.y.max(other.y),
+        }
+    }
+
+    /// Check if this point is approximately equal to another.
+    #[inline]
+    pub fn approx_eq(self, other: Point2D, epsilon: f32) -> bool {
+        (self.x - other.x).abs() <= epsilon && (self.y - other.y).abs() <= epsilon
+    }
+}
+
+impl Add for Point2D {
+    type Output = Self;
+
+    #[inline]
+    fn add(self, rhs: Self) -> Self {
+        Self {
+            x: self.x + rhs.x,
+            y: self.y + rhs.y,
+        }
+    }
+}
+
+impl Sub for Point2D {
+    type Output = Self;
+
+    #[inline]
+    fn sub(self, rhs: Self) -> Self {
+        Self {
+            x: self.x - rhs.x,
+            y: self.y - rhs.y,
+        }
+    }
+}
+
+impl Mul<f32> for Point2D {
+    type Output = Self;
+
+    #[inline]
+    fn mul(self, rhs: f32) -> Self {
+        Self {
+            x: self.x * rhs,
+            y: self.y * rhs,
+        }
+    }
+}
+
+impl Mul<Point2D> for f32 {
+    type Output = Point2D;
+
+    #[inline]
+    fn mul(self, rhs: Point2D) -> Point2D {
+        Point2D {
+            x: self * rhs.x,
+            y: self * rhs.y,
+        }
+    }
+}
+
+impl Div<f32> for Point2D {
+    type Output = Self;
+
+    #[inline]
+    fn div(self, rhs: f32) -> Self {
+        Self {
+            x: self.x / rhs,
+            y: self.y / rhs,
+        }
+    }
+}
+
+impl Neg for Point2D {
+    type Output = Self;
+
+    #[inline]
+    fn neg(self) -> Self {
+        Self {
+            x: -self.x,
+            y: -self.y,
+        }
+    }
+}
+
+#[cfg(test)]
+mod tests {
+    use super::*;
+    use approx::assert_relative_eq;
+    use std::f32::consts::PI;
+
+    #[test]
+    fn test_new_and_zero() {
+        let p = Point2D::new(1.0, 2.0);
+        assert_eq!(p.x, 1.0);
+        assert_eq!(p.y, 2.0);
+
+        let z = Point2D::zero();
+        assert_eq!(z.x, 0.0);
+        assert_eq!(z.y, 0.0);
+    }
+
+    #[test]
+    fn test_from_polar() {
+        let p = Point2D::from_polar(0.0, 1.0);
+        assert_relative_eq!(p.x, 1.0, epsilon = 1e-6);
+        assert_relative_eq!(p.y, 0.0, epsilon = 1e-6);
+
+        let p = Point2D::from_polar(PI / 2.0, 1.0);
+        assert_relative_eq!(p.x, 0.0, epsilon = 1e-6);
+        assert_relative_eq!(p.y, 1.0, epsilon = 1e-6);
+
+        let p = Point2D::from_polar(PI / 4.0, 2.0_f32.sqrt());
+        assert_relative_eq!(p.x, 1.0, epsilon = 1e-6);
+        assert_relative_eq!(p.y, 1.0, epsilon = 1e-6);
+    }
+
+    #[test]
+    fn test_length() {
+        let p = Point2D::new(3.0, 4.0);
+        assert_eq!(p.length_squared(), 25.0);
+        assert_eq!(p.length(), 5.0);
+    }
+
+    #[test]
+    fn test_distance() {
+        let a = Point2D::new(0.0, 0.0);
+        let b = Point2D::new(3.0, 4.0);
+        assert_eq!(a.distance_squared(b), 25.0);
+        assert_eq!(a.distance(b), 5.0);
+    }
+
+    #[test]
+    fn test_normalized() {
+        let p = Point2D::new(3.0, 4.0);
+        let n = p.normalized();
+        assert_relative_eq!(n.length(), 1.0, epsilon = 1e-6);
+        assert_relative_eq!(n.x, 0.6, epsilon = 1e-6);
+        assert_relative_eq!(n.y, 0.8, epsilon = 1e-6);
+
+        // Zero vector stays zero
+        let z = Point2D::zero().normalized();
+        assert_eq!(z, Point2D::zero());
+    }
+
+    #[test]
+    fn test_dot() {
+        let a = Point2D::new(1.0, 0.0);
+        let b = Point2D::new(0.0, 1.0);
+        assert_eq!(a.dot(b), 0.0); // Perpendicular
+
+        let c = Point2D::new(1.0, 2.0);
+        let d = Point2D::new(3.0, 4.0);
+        assert_eq!(c.dot(d), 11.0); // 1*3 + 2*4 = 11
+    }
+
+    #[test]
+    fn test_cross() {
+        let a = Point2D::new(1.0, 0.0);
+        let b = Point2D::new(0.0, 1.0);
+        assert_eq!(a.cross(b), 1.0); // CCW
+        assert_eq!(b.cross(a), -1.0); // CW
+    }
+
+    #[test]
+    fn test_perpendicular() {
+        let p = Point2D::new(1.0, 0.0);
+        let perp = p.perpendicular();
+        assert_relative_eq!(perp.x, 0.0, epsilon = 1e-6);
+        assert_relative_eq!(perp.y, 1.0, epsilon = 1e-6);
+    }
+
+    #[test]
+    fn test_angle() {
+        assert_relative_eq!(Point2D::new(1.0, 0.0).angle(), 0.0, epsilon = 1e-6);
+        assert_relative_eq!(Point2D::new(0.0, 1.0).angle(), PI / 2.0, epsilon = 1e-6);
+        assert_relative_eq!(Point2D::new(-1.0, 0.0).angle(), PI, epsilon = 1e-6);
+        assert_relative_eq!(Point2D::new(0.0, -1.0).angle(), -PI / 2.0, epsilon = 1e-6);
+    }
+
+    #[test]
+    fn test_rotate() {
+        let p = Point2D::new(1.0, 0.0);
+
+        let r90 = p.rotate(PI / 2.0);
+        assert_relative_eq!(r90.x, 0.0, epsilon = 1e-6);
+        assert_relative_eq!(r90.y, 1.0, epsilon = 1e-6);
+
+        let r180 = p.rotate(PI);
+        assert_relative_eq!(r180.x, -1.0, epsilon = 1e-6);
+        assert_relative_eq!(r180.y, 0.0, epsilon = 1e-6);
+    }
+
+    #[test]
+    fn test_lerp() {
+        let a = Point2D::new(0.0, 0.0);
+        let b = Point2D::new(10.0, 20.0);
+
+        let mid = a.lerp(b, 0.5);
+        assert_eq!(mid.x, 5.0);
+        assert_eq!(mid.y, 10.0);
+
+        assert_eq!(a.lerp(b, 0.0), a);
+        assert_eq!(a.lerp(b, 1.0), b);
+    }
+
+    #[test]
+    fn test_min_max() {
+        let a = Point2D::new(1.0, 5.0);
+        let b = Point2D::new(3.0, 2.0);
+
+        let min = a.min(b);
+        assert_eq!(min.x, 1.0);
+        assert_eq!(min.y, 2.0);
+
+        let max = a.max(b);
+        assert_eq!(max.x, 3.0);
+        assert_eq!(max.y, 5.0);
+    }
+
+    #[test]
+    fn test_operators() {
+        let a = Point2D::new(1.0, 2.0);
+        let b = Point2D::new(3.0, 4.0);
+
+        assert_eq!(a + b, Point2D::new(4.0, 6.0));
+        assert_eq!(b - a, Point2D::new(2.0, 2.0));
+        assert_eq!(a * 2.0, Point2D::new(2.0, 4.0));
+        assert_eq!(2.0 * a, Point2D::new(2.0, 4.0));
+        assert_eq!(a / 2.0, Point2D::new(0.5, 1.0));
+        assert_eq!(-a, Point2D::new(-1.0, -2.0));
+    }
+
+    #[test]
+    fn test_approx_eq() {
+        let a = Point2D::new(1.0, 2.0);
+        let b = Point2D::new(1.001, 2.001);
+
+        assert!(a.approx_eq(b, 0.01));
+        assert!(!a.approx_eq(b, 0.0001));
+    }
+}

vastu-map/src/core/pose.rs

@@ -0,0 +1,343 @@
+//! 2D pose type for robot position and orientation.
+//!
+//! Coordinate frame follows ROS REP-103:
+//! - X-forward, Y-left, Z-up (right-handed)
+//! - Counter-clockwise positive rotation
+
+use super::math::normalize_angle;
+use super::point::Point2D;
+
+/// A 2D pose representing position and orientation.
+///
+/// Uses the ROS REP-103 coordinate convention:
+/// - Position: (x, y) in meters
+/// - Theta: heading angle in radians, counter-clockwise from X-axis
+///
+/// # Composition
+///
+/// Poses can be composed using `*` operator (chain transformations):
+/// ```
+/// use vastu_map::core::{Pose2D, Point2D};
+///
+/// let pose_a = Pose2D::new(1.0, 0.0, std::f32::consts::FRAC_PI_2);
+/// let pose_b = Pose2D::new(1.0, 0.0, 0.0);
+/// let combined = pose_a * pose_b;  // Apply pose_b in pose_a's frame
+/// ```
+#[derive(Clone, Copy, Debug, Default, PartialEq)]
+pub struct Pose2D {
+    /// X position in meters.
+    pub x: f32,
+    /// Y position in meters.
+    pub y: f32,
+    /// Heading angle in radians [-π, π), CCW positive from X-axis.
+    pub theta: f32,
+}
+
+impl Pose2D {
+    /// Create a new pose.
+    ///
+    /// # Arguments
+    /// * `x` - X position in meters
+    /// * `y` - Y position in meters
+    /// * `theta` - Heading angle in radians (will be normalized to [-π, π))
+    #[inline]
+    pub fn new(x: f32, y: f32, theta: f32) -> Self {
+        Self {
+            x,
+            y,
+            theta: normalize_angle(theta),
+        }
+    }
+
+    /// Create an identity pose (origin, facing forward).
+    #[inline]
+    pub const fn identity() -> Self {
+        Self {
+            x: 0.0,
+            y: 0.0,
+            theta: 0.0,
+        }
+    }
+
+    /// Create a pose from position and angle.
+    #[inline]
+    pub fn from_position_angle(position: Point2D, theta: f32) -> Self {
+        Self::new(position.x, position.y, theta)
+    }
+
+    /// Get the position as a Point2D.
+    #[inline]
+    pub fn position(self) -> Point2D {
+        Point2D::new(self.x, self.y)
+    }
+
+    /// Get the forward direction (unit vector).
+    #[inline]
+    pub fn forward(self) -> Point2D {
+        Point2D::new(self.theta.cos(), self.theta.sin())
+    }
+
+    /// Get the left direction (unit vector, perpendicular to forward).
+    #[inline]
+    pub fn left(self) -> Point2D {
+        Point2D::new(-self.theta.sin(), self.theta.cos())
+    }
+
+    /// Transform a point from this pose's local frame to world frame.
+    ///
+    /// # Example
+    /// ```
+    /// use vastu_map::core::{Pose2D, Point2D};
+    /// use std::f32::consts::FRAC_PI_2;
+    ///
+    /// let pose = Pose2D::new(1.0, 0.0, FRAC_PI_2);  // At (1,0), facing left
+    /// let local = Point2D::new(1.0, 0.0);  // 1m forward in local frame
+    /// let world = pose.transform_point(local);
+    /// // Should be at (1, 1) in world frame
+    /// ```
+    #[inline]
+    pub fn transform_point(self, point: Point2D) -> Point2D {
+        let (sin, cos) = self.theta.sin_cos();
+        Point2D {
+            x: self.x + point.x * cos - point.y * sin,
+            y: self.y + point.x * sin + point.y * cos,
+        }
+    }
+
+    /// Transform a point from world frame to this pose's local frame.
+    #[inline]
+    pub fn inverse_transform_point(self, point: Point2D) -> Point2D {
+        let (sin, cos) = self.theta.sin_cos();
+        let dx = point.x - self.x;
+        let dy = point.y - self.y;
+        Point2D {
+            x: dx * cos + dy * sin,
+            y: -dx * sin + dy * cos,
+        }
+    }
+
+    /// Compose this pose with another (chain transformations).
+    ///
+    /// Returns a new pose representing: apply `other` in `self`'s frame.
+    /// This is equivalent to matrix multiplication: self * other.
+    #[inline]
+    pub fn compose(self, other: Pose2D) -> Self {
+        let pos = self.transform_point(other.position());
+        Self::new(pos.x, pos.y, self.theta + other.theta)
+    }
+
+    /// Compute the inverse of this pose.
+    ///
+    /// The inverse pose, when composed with the original, yields identity:
+    /// `pose.compose(pose.inverse()) ≈ Pose2D::identity()`
+    #[inline]
+    pub fn inverse(self) -> Self {
+        let (sin, cos) = self.theta.sin_cos();
+        Self::new(
+            -self.x * cos - self.y * sin,
+            self.x * sin - self.y * cos,
+            -self.theta,
+        )
+    }
+
+    /// Compute the relative pose from `self` to `other`.
+    ///
+    /// Returns the pose that, when composed with `self`, yields `other`:
+    /// `self.compose(self.relative_to(other)) ≈ other`
+    #[inline]
+    pub fn relative_to(self, other: Pose2D) -> Self {
+        self.inverse().compose(other)
+    }
+
+    /// Linear interpolation between poses.
+    ///
+    /// Note: This interpolates theta linearly, which may not be ideal
+    /// for large angular differences. For smooth rotation interpolation,
+    /// consider using spherical interpolation.
+    #[inline]
+    pub fn lerp(self, other: Pose2D, t: f32) -> Self {
+        use super::math::angle_diff;
+        Self::new(
+            self.x + (other.x - self.x) * t,
+            self.y + (other.y - self.y) * t,
+            self.theta + angle_diff(self.theta, other.theta) * t,
+        )
+    }
+
+    /// Check if this pose is approximately equal to another.
+    #[inline]
+    pub fn approx_eq(self, other: Pose2D, pos_epsilon: f32, angle_epsilon: f32) -> bool {
+        use super::math::angles_approx_equal;
+        (self.x - other.x).abs() <= pos_epsilon
+            && (self.y - other.y).abs() <= pos_epsilon
+            && angles_approx_equal(self.theta, other.theta, angle_epsilon)
+    }
+}
+
+impl std::ops::Mul for Pose2D {
+    type Output = Self;
+
+    /// Compose two poses (same as `compose`).
+    #[inline]
+    fn mul(self, rhs: Self) -> Self {
+        self.compose(rhs)
+    }
+}
+
+#[cfg(test)]
+mod tests {
+    use super::*;
+    use approx::assert_relative_eq;
+    use std::f32::consts::{FRAC_PI_2, PI};
+
+    #[test]
+    fn test_new_normalizes_angle() {
+        // At ±π boundary, floating-point may give +π or -π; both are valid
+        let pose = Pose2D::new(0.0, 0.0, 3.0 * PI);
+        assert!(pose.theta.abs() - PI < 1e-5);
+
+        let pose = Pose2D::new(0.0, 0.0, -3.0 * PI);
+        assert!(pose.theta.abs() - PI < 1e-5);
+    }
+
+    #[test]
+    fn test_identity() {
+        let pose = Pose2D::identity();
+        assert_eq!(pose.x, 0.0);
+        assert_eq!(pose.y, 0.0);
+        assert_eq!(pose.theta, 0.0);
+    }
+
+    #[test]
+    fn test_position() {
+        let pose = Pose2D::new(1.0, 2.0, 0.5);
+        let pos = pose.position();
+        assert_eq!(pos.x, 1.0);
+        assert_eq!(pos.y, 2.0);
+    }
+
+    #[test]
+    fn test_forward_and_left() {
+        let pose = Pose2D::new(0.0, 0.0, 0.0);
+        let fwd = pose.forward();
+        assert_relative_eq!(fwd.x, 1.0, epsilon = 1e-6);
+        assert_relative_eq!(fwd.y, 0.0, epsilon = 1e-6);
+
+        let left = pose.left();
+        assert_relative_eq!(left.x, 0.0, epsilon = 1e-6);
+        assert_relative_eq!(left.y, 1.0, epsilon = 1e-6);
+
+        // At 90 degrees
+        let pose90 = Pose2D::new(0.0, 0.0, FRAC_PI_2);
+        let fwd90 = pose90.forward();
+        assert_relative_eq!(fwd90.x, 0.0, epsilon = 1e-6);
+        assert_relative_eq!(fwd90.y, 1.0, epsilon = 1e-6);
+    }
+
+    #[test]
+    fn test_transform_point() {
+        // At origin, no rotation
+        let pose = Pose2D::identity();
+        let local = Point2D::new(1.0, 0.0);
+        let world = pose.transform_point(local);
+        assert_relative_eq!(world.x, 1.0, epsilon = 1e-6);
+        assert_relative_eq!(world.y, 0.0, epsilon = 1e-6);
+
+        // At (1, 0), rotated 90 degrees
+        let pose = Pose2D::new(1.0, 0.0, FRAC_PI_2);
+        let local = Point2D::new(1.0, 0.0);
+        let world = pose.transform_point(local);
+        assert_relative_eq!(world.x, 1.0, epsilon = 1e-6);
+        assert_relative_eq!(world.y, 1.0, epsilon = 1e-6);
+    }
+
+    #[test]
+    fn test_inverse_transform_point() {
+        let pose = Pose2D::new(1.0, 2.0, FRAC_PI_2);
+        let world = Point2D::new(3.0, 4.0);
+
+        let local = pose.inverse_transform_point(world);
+        let back = pose.transform_point(local);
+
+        assert_relative_eq!(back.x, world.x, epsilon = 1e-6);
+        assert_relative_eq!(back.y, world.y, epsilon = 1e-6);
+    }
+
+    #[test]
+    fn test_compose() {
+        // Translate then rotate
+        let translate = Pose2D::new(1.0, 0.0, 0.0);
+        let rotate = Pose2D::new(0.0, 0.0, FRAC_PI_2);
+        let combined = translate.compose(rotate);
+
+        assert_relative_eq!(combined.x, 1.0, epsilon = 1e-6);
+        assert_relative_eq!(combined.y, 0.0, epsilon = 1e-6);
+        assert_relative_eq!(combined.theta, FRAC_PI_2, epsilon = 1e-6);
+
+        // Rotate then translate (in rotated frame)
+        let combined2 = rotate.compose(translate);
+        assert_relative_eq!(combined2.x, 0.0, epsilon = 1e-6);
+        assert_relative_eq!(combined2.y, 1.0, epsilon = 1e-6);
+        assert_relative_eq!(combined2.theta, FRAC_PI_2, epsilon = 1e-6);
+    }
+
+    #[test]
+    fn test_inverse() {
+        let pose = Pose2D::new(1.0, 2.0, 0.5);
+        let inv = pose.inverse();
+        let identity = pose.compose(inv);
+
+        assert_relative_eq!(identity.x, 0.0, epsilon = 1e-5);
+        assert_relative_eq!(identity.y, 0.0, epsilon = 1e-5);
+        assert_relative_eq!(identity.theta, 0.0, epsilon = 1e-5);
+    }
+
+    #[test]
+    fn test_relative_to() {
+        let pose_a = Pose2D::new(1.0, 0.0, 0.0);
+        let pose_b = Pose2D::new(2.0, 1.0, FRAC_PI_2);
+
+        let relative = pose_a.relative_to(pose_b);
+        let reconstructed = pose_a.compose(relative);
+
+        assert_relative_eq!(reconstructed.x, pose_b.x, epsilon = 1e-5);
+        assert_relative_eq!(reconstructed.y, pose_b.y, epsilon = 1e-5);
+        assert_relative_eq!(reconstructed.theta, pose_b.theta, epsilon = 1e-5);
+    }
+
+    #[test]
+    fn test_lerp() {
+        // Use non-boundary angle to avoid π ambiguity
+        let a = Pose2D::new(0.0, 0.0, 0.0);
+        let b = Pose2D::new(2.0, 4.0, FRAC_PI_2);
+
+        let mid = a.lerp(b, 0.5);
+        assert_relative_eq!(mid.x, 1.0, epsilon = 1e-6);
+        assert_relative_eq!(mid.y, 2.0, epsilon = 1e-6);
+        assert_relative_eq!(mid.theta, FRAC_PI_2 / 2.0, epsilon = 1e-6); // π/4
+
+        // Test full interpolation
+        assert_relative_eq!(a.lerp(b, 0.0).theta, 0.0, epsilon = 1e-6);
+        assert_relative_eq!(a.lerp(b, 1.0).theta, FRAC_PI_2, epsilon = 1e-6);
+    }
+
+    #[test]
+    fn test_mul_operator() {
+        let a = Pose2D::new(1.0, 0.0, FRAC_PI_2);
+        let b = Pose2D::new(1.0, 0.0, 0.0);
+
+        let composed = a.compose(b);
+        let multiplied = a * b;
+
+        assert_eq!(composed, multiplied);
+    }
+
+    #[test]
+    fn test_approx_eq() {
+        let a = Pose2D::new(1.0, 2.0, 0.5);
+        let b = Pose2D::new(1.001, 2.001, 0.501);
+
+        assert!(a.approx_eq(b, 0.01, 0.01));
+        assert!(!a.approx_eq(b, 0.0001, 0.0001));
+    }
+}

vastu-map/src/core/scan.rs

@@ -0,0 +1,590 @@
+//! Scan types for lidar data representation.
+//!
+//! This module provides:
+//! - `PolarScan`: Raw lidar scan in polar coordinates
+//! - `PointCloud2D`: Cartesian point cloud with SoA (Struct of Arrays) layout
+//!
+//! The SoA layout is optimized for SIMD operations on ARM NEON.
+
+use super::point::Point2D;
+use super::pose::Pose2D;
+use crate::simd::Float4;
+
+/// Raw lidar scan in polar coordinates.
+///
+/// Points are ordered by angle (sequential around scan).
+/// Coordinate frame follows ROS REP-103:
+/// - Angle 0 is forward (X-axis)
+/// - Positive angles are counter-clockwise (toward Y-axis)
+#[derive(Clone, Debug, Default)]
+pub struct PolarScan {
+    /// Points as (angle_rad, distance_m, quality).
+    /// - angle_rad: Angle from X-axis in radians, CCW positive
+    /// - distance_m: Range in meters
+    /// - quality: Measurement quality (0-255, higher is better)
+    pub points: Vec<(f32, f32, u8)>,
+}
+
+impl PolarScan {
+    /// Create a new empty polar scan.
+    pub fn new() -> Self {
+        Self { points: Vec::new() }
+    }
+
+    /// Create a polar scan with capacity.
+    pub fn with_capacity(capacity: usize) -> Self {
+        Self {
+            points: Vec::with_capacity(capacity),
+        }
+    }
+
+    /// Add a point to the scan.
+    #[inline]
+    pub fn push(&mut self, angle: f32, distance: f32, quality: u8) {
+        self.points.push((angle, distance, quality));
+    }
+
+    /// Number of points in the scan.
+    #[inline]
+    pub fn len(&self) -> usize {
+        self.points.len()
+    }
+
+    /// Check if the scan is empty.
+    #[inline]
+    pub fn is_empty(&self) -> bool {
+        self.points.is_empty()
+    }
+
+    /// Convert to cartesian PointCloud2D.
+    ///
+    /// Filters by quality threshold and valid range.
+    ///
+    /// # Arguments
+    /// * `min_quality` - Minimum quality value to include
+    /// * `min_range` - Minimum valid range in meters
+    /// * `max_range` - Maximum valid range in meters
+    pub fn to_cartesian(&self, min_quality: u8, min_range: f32, max_range: f32) -> PointCloud2D {
+        let mut cloud = PointCloud2D::with_capacity(self.points.len());
+
+        for &(angle, dist, quality) in &self.points {
+            if quality >= min_quality && dist >= min_range && dist <= max_range {
+                let (sin, cos) = angle.sin_cos();
+                cloud.xs.push(dist * cos);
+                cloud.ys.push(dist * sin);
+            }
+        }
+
+        cloud
+    }
+}
+
+/// Cartesian point cloud with SoA (Struct of Arrays) layout.
+///
+/// This layout enables efficient SIMD operations:
+/// - Transform operations can process 4 points per NEON instruction
+/// - Memory access patterns are cache-friendly for sequential processing
+///
+/// Coordinate frame follows ROS REP-103:
+/// - X-forward, Y-left
+#[derive(Clone, Debug, Default)]
+pub struct PointCloud2D {
+    /// X coordinates in meters (forward direction).
+    pub xs: Vec<f32>,
+    /// Y coordinates in meters (left direction).
+    pub ys: Vec<f32>,
+}
+
+impl PointCloud2D {
+    /// Create a new empty point cloud.
+    pub fn new() -> Self {
+        Self {
+            xs: Vec::new(),
+            ys: Vec::new(),
+        }
+    }
+
+    /// Create a point cloud with capacity.
+    pub fn with_capacity(capacity: usize) -> Self {
+        Self {
+            xs: Vec::with_capacity(capacity),
+            ys: Vec::with_capacity(capacity),
+        }
+    }
+
+    /// Create from a slice of Point2D.
+    pub fn from_points(points: &[Point2D]) -> Self {
+        let mut cloud = Self::with_capacity(points.len());
+        for p in points {
+            cloud.xs.push(p.x);
+            cloud.ys.push(p.y);
+        }
+        cloud
+    }
+
+    /// Add a point to the cloud.
+    #[inline]
+    pub fn push(&mut self, x: f32, y: f32) {
+        self.xs.push(x);
+        self.ys.push(y);
+    }
+
+    /// Add a Point2D to the cloud.
+    #[inline]
+    pub fn push_point(&mut self, point: Point2D) {
+        self.xs.push(point.x);
+        self.ys.push(point.y);
+    }
+
+    /// Number of points in the cloud.
+    #[inline]
+    pub fn len(&self) -> usize {
+        self.xs.len()
+    }
+
+    /// Check if the cloud is empty.
+    #[inline]
+    pub fn is_empty(&self) -> bool {
+        self.xs.is_empty()
+    }
+
+    /// Get a point by index.
+    #[inline]
+    pub fn get(&self, index: usize) -> Option<Point2D> {
+        if index < self.len() {
+            Some(Point2D::new(self.xs[index], self.ys[index]))
+        } else {
+            None
+        }
+    }
+
+    /// Get a point by index (unchecked).
+    ///
+    /// # Safety
+    /// Caller must ensure index is in bounds.
+    #[inline]
+    pub unsafe fn get_unchecked(&self, index: usize) -> Point2D {
+        // SAFETY: Caller guarantees index is in bounds
+        unsafe { Point2D::new(*self.xs.get_unchecked(index), *self.ys.get_unchecked(index)) }
+    }
+
+    /// Transform the point cloud by a pose.
+    ///
+    /// This is SIMD-optimized: processes 4 points per iteration using Float4.
+    ///
+    /// # Performance
+    /// Uses fused multiply-add (FMA) operations which map directly to
+    /// ARM NEON `vfmaq_f32` instructions on VFPv4 hardware.
+    pub fn transform(&self, pose: &Pose2D) -> Self {
+        let n = self.len();
+        let mut result = Self::with_capacity(n);
+        result.xs.resize(n, 0.0);
+        result.ys.resize(n, 0.0);
+
+        let (sin, cos) = pose.theta.sin_cos();
+        let cos4 = Float4::splat(cos);
+        let sin4 = Float4::splat(sin);
+        let tx4 = Float4::splat(pose.x);
+        let ty4 = Float4::splat(pose.y);
+
+        // Process 4 points at a time (SIMD-friendly)
+        let chunks = n / 4;
+        for i in 0..chunks {
+            let base = i * 4;
+
+            // Load 4 points
+            let xs = Float4::new([
+                self.xs[base],
+                self.xs[base + 1],
+                self.xs[base + 2],
+                self.xs[base + 3],
+            ]);
+            let ys = Float4::new([
+                self.ys[base],
+                self.ys[base + 1],
+                self.ys[base + 2],
+                self.ys[base + 3],
+            ]);
+
+            // Rotation + translation using FMA:
+            // x' = x*cos - y*sin + tx
+            // y' = x*sin + y*cos + ty
+            let new_xs = ys.neg_mul_add(sin4, xs.mul_add(cos4, tx4));
+            let new_ys = xs.mul_add(sin4, ys.mul_add(cos4, ty4));
+
+            // Store results
+            let new_xs_arr = new_xs.to_array();
+            let new_ys_arr = new_ys.to_array();
+            result.xs[base..base + 4].copy_from_slice(&new_xs_arr);
+            result.ys[base..base + 4].copy_from_slice(&new_ys_arr);
+        }
+
+        // Handle remainder (scalar)
+        for i in (chunks * 4)..n {
+            let x = self.xs[i];
+            let y = self.ys[i];
+            result.xs[i] = x * cos - y * sin + pose.x;
+            result.ys[i] = x * sin + y * cos + pose.y;
+        }
+
+        result
+    }
+
+    /// Compute bounding box of the point cloud.
+    ///
+    /// Returns (min, max) corners, or None if cloud is empty.
+    /// SIMD-optimized using Float4 min/max operations.
+    pub fn bounds(&self) -> Option<(Point2D, Point2D)> {
+        if self.is_empty() {
+            return None;
+        }
+
+        let n = self.len();
+
+        // Initialize with first point
+        let mut min_x = self.xs[0];
+        let mut max_x = self.xs[0];
+        let mut min_y = self.ys[0];
+        let mut max_y = self.ys[0];
+
+        // SIMD processing of remaining points
+        let chunks = (n - 1) / 4;
+        if chunks > 0 {
+            let mut min_x4 = Float4::splat(min_x);
+            let mut max_x4 = Float4::splat(max_x);
+            let mut min_y4 = Float4::splat(min_y);
+            let mut max_y4 = Float4::splat(max_y);
+
+            for i in 0..chunks {
+                let base = 1 + i * 4;
+
+                let xs = Float4::new([
+                    self.xs[base],
+                    self.xs[base + 1],
+                    self.xs[base + 2],
+                    self.xs[base + 3],
+                ]);
+                let ys = Float4::new([
+                    self.ys[base],
+                    self.ys[base + 1],
+                    self.ys[base + 2],
+                    self.ys[base + 3],
+                ]);
+
+                min_x4 = min_x4.min(xs);
+                max_x4 = max_x4.max(xs);
+                min_y4 = min_y4.min(ys);
+                max_y4 = max_y4.max(ys);
+            }
+
+            // Horizontal reduction
+            min_x = min_x4.horizontal_min();
+            max_x = max_x4.horizontal_max();
+            min_y = min_y4.horizontal_min();
+            max_y = max_y4.horizontal_max();
+        }
+
+        // Handle remainder (scalar)
+        let remainder_start = 1 + chunks * 4;
+        for i in remainder_start..n {
+            min_x = min_x.min(self.xs[i]);
+            max_x = max_x.max(self.xs[i]);
+            min_y = min_y.min(self.ys[i]);
+            max_y = max_y.max(self.ys[i]);
+        }
+
+        Some((Point2D::new(min_x, min_y), Point2D::new(max_x, max_y)))
+    }
+
+    /// Compute centroid (mean position) of the point cloud.
+    ///
+    /// Returns None if cloud is empty.
+    pub fn centroid(&self) -> Option<Point2D> {
+        if self.is_empty() {
+            return None;
+        }
+
+        let n = self.len();
+        let mut sum_x: f32 = 0.0;
+        let mut sum_y: f32 = 0.0;
+
+        // SIMD accumulation
+        let chunks = n / 4;
+        if chunks > 0 {
+            let mut sum_x4 = Float4::zero();
+            let mut sum_y4 = Float4::zero();
+
+            for i in 0..chunks {
+                let base = i * 4;
+                let xs = Float4::new([
+                    self.xs[base],
+                    self.xs[base + 1],
+                    self.xs[base + 2],
+                    self.xs[base + 3],
+                ]);
+                let ys = Float4::new([
+                    self.ys[base],
+                    self.ys[base + 1],
+                    self.ys[base + 2],
+                    self.ys[base + 3],
+                ]);
+
+                sum_x4 += xs;
+                sum_y4 += ys;
+            }
+
+            sum_x = sum_x4.sum();
+            sum_y = sum_y4.sum();
+        }
+
+        // Handle remainder
+        for i in (chunks * 4)..n {
+            sum_x += self.xs[i];
+            sum_y += self.ys[i];
+        }
+
+        Some(Point2D::new(sum_x / n as f32, sum_y / n as f32))
+    }
+
+    /// Convert to Vec<Point2D> (AoS layout).
+    pub fn to_points(&self) -> Vec<Point2D> {
+        self.xs
+            .iter()
+            .zip(&self.ys)
+            .map(|(&x, &y)| Point2D::new(x, y))
+            .collect()
+    }
+
+    /// Iterate over points.
+    pub fn iter(&self) -> impl Iterator<Item = Point2D> + '_ {
+        self.xs
+            .iter()
+            .zip(&self.ys)
+            .map(|(&x, &y)| Point2D::new(x, y))
+    }
+
+    /// Clear all points.
+    pub fn clear(&mut self) {
+        self.xs.clear();
+        self.ys.clear();
+    }
+}
+
+#[cfg(test)]
+mod tests {
+    use super::*;
+    use approx::assert_relative_eq;
+    use std::f32::consts::{FRAC_PI_2, PI};
+
+    #[test]
+    fn test_polar_scan_to_cartesian() {
+        let mut scan = PolarScan::new();
+        scan.push(0.0, 1.0, 100); // Forward, 1m
+        scan.push(FRAC_PI_2, 2.0, 100); // Left, 2m
+        scan.push(PI, 1.5, 50); // Behind, 1.5m, low quality
+
+        let cloud = scan.to_cartesian(60, 0.0, 10.0);
+
+        assert_eq!(cloud.len(), 2); // Third point filtered by quality
+
+        // First point: (1, 0)
+        assert_relative_eq!(cloud.xs[0], 1.0, epsilon = 1e-6);
+        assert_relative_eq!(cloud.ys[0], 0.0, epsilon = 1e-6);
+
+        // Second point: (0, 2)
+        assert_relative_eq!(cloud.xs[1], 0.0, epsilon = 1e-6);
+        assert_relative_eq!(cloud.ys[1], 2.0, epsilon = 1e-6);
+    }
+
+    #[test]
+    fn test_polar_scan_range_filter() {
+        let mut scan = PolarScan::new();
+        scan.push(0.0, 0.05, 100); // Too close
+        scan.push(0.0, 0.5, 100); // Good
+        scan.push(0.0, 15.0, 100); // Too far
+
+        let cloud = scan.to_cartesian(0, 0.1, 10.0);
+        assert_eq!(cloud.len(), 1);
+        assert_relative_eq!(cloud.xs[0], 0.5, epsilon = 1e-6);
+    }
+
+    #[test]
+    fn test_point_cloud_from_points() {
+        let points = vec![
+            Point2D::new(1.0, 2.0),
+            Point2D::new(3.0, 4.0),
+            Point2D::new(5.0, 6.0),
+        ];
+
+        let cloud = PointCloud2D::from_points(&points);
+
+        assert_eq!(cloud.len(), 3);
+        assert_eq!(cloud.xs, vec![1.0, 3.0, 5.0]);
+        assert_eq!(cloud.ys, vec![2.0, 4.0, 6.0]);
+    }
+
+    #[test]
+    fn test_point_cloud_transform_identity() {
+        let mut cloud = PointCloud2D::new();
+        cloud.push(1.0, 2.0);
+        cloud.push(3.0, 4.0);
+
+        let pose = Pose2D::identity();
+        let transformed = cloud.transform(&pose);
+
+        assert_eq!(transformed.len(), 2);
+        assert_relative_eq!(transformed.xs[0], 1.0, epsilon = 1e-6);
+        assert_relative_eq!(transformed.ys[0], 2.0, epsilon = 1e-6);
+    }
+
+    #[test]
+    fn test_point_cloud_transform_translation() {
+        let mut cloud = PointCloud2D::new();
+        cloud.push(1.0, 2.0);
+
+        let pose = Pose2D::new(10.0, 20.0, 0.0);
+        let transformed = cloud.transform(&pose);
+
+        assert_relative_eq!(transformed.xs[0], 11.0, epsilon = 1e-6);
+        assert_relative_eq!(transformed.ys[0], 22.0, epsilon = 1e-6);
+    }
+
+    #[test]
+    fn test_point_cloud_transform_rotation() {
+        let mut cloud = PointCloud2D::new();
+        cloud.push(1.0, 0.0);
+
+        let pose = Pose2D::new(0.0, 0.0, FRAC_PI_2);
+        let transformed = cloud.transform(&pose);
+
+        assert_relative_eq!(transformed.xs[0], 0.0, epsilon = 1e-6);
+        assert_relative_eq!(transformed.ys[0], 1.0, epsilon = 1e-6);
+    }
+
+    #[test]
+    fn test_point_cloud_transform_combined() {
+        let mut cloud = PointCloud2D::new();
+        cloud.push(1.0, 0.0);
+
+        // Rotate 90° then translate by (1, 0)
+        let pose = Pose2D::new(1.0, 0.0, FRAC_PI_2);
+        let transformed = cloud.transform(&pose);
+
+        // Point rotates to (0, 1), then translates to (1, 1)
+        assert_relative_eq!(transformed.xs[0], 1.0, epsilon = 1e-6);
+        assert_relative_eq!(transformed.ys[0], 1.0, epsilon = 1e-6);
+    }
+
+    #[test]
+    fn test_point_cloud_transform_simd() {
+        // Test with more than 4 points to exercise SIMD path
+        let mut cloud = PointCloud2D::new();
+        for i in 0..10 {
+            cloud.push(i as f32, 0.0);
+        }
+
+        let pose = Pose2D::new(0.0, 0.0, FRAC_PI_2);
+        let transformed = cloud.transform(&pose);
+
+        for i in 0..10 {
+            assert_relative_eq!(transformed.xs[i], 0.0, epsilon = 1e-5);
+            assert_relative_eq!(transformed.ys[i], i as f32, epsilon = 1e-5);
+        }
+    }
+
+    #[test]
+    fn test_point_cloud_bounds() {
+        let mut cloud = PointCloud2D::new();
+        cloud.push(1.0, 2.0);
+        cloud.push(-3.0, 4.0);
+        cloud.push(5.0, -6.0);
+        cloud.push(0.0, 0.0);
+
+        let (min, max) = cloud.bounds().unwrap();
+
+        assert_eq!(min.x, -3.0);
+        assert_eq!(min.y, -6.0);
+        assert_eq!(max.x, 5.0);
+        assert_eq!(max.y, 4.0);
+    }
+
+    #[test]
+    fn test_point_cloud_bounds_simd() {
+        // Test with more than 4 points
+        let mut cloud = PointCloud2D::new();
+        for i in 0..10 {
+            cloud.push(i as f32 - 5.0, i as f32 - 5.0);
+        }
+
+        let (min, max) = cloud.bounds().unwrap();
+
+        assert_eq!(min.x, -5.0);
+        assert_eq!(min.y, -5.0);
+        assert_eq!(max.x, 4.0);
+        assert_eq!(max.y, 4.0);
+    }
+
+    #[test]
+    fn test_point_cloud_bounds_empty() {
+        let cloud = PointCloud2D::new();
+        assert!(cloud.bounds().is_none());
+    }
+
+    #[test]
+    fn test_point_cloud_centroid() {
+        let mut cloud = PointCloud2D::new();
+        cloud.push(0.0, 0.0);
+        cloud.push(2.0, 0.0);
+        cloud.push(2.0, 2.0);
+        cloud.push(0.0, 2.0);
+
+        let centroid = cloud.centroid().unwrap();
+
+        assert_relative_eq!(centroid.x, 1.0, epsilon = 1e-6);
+        assert_relative_eq!(centroid.y, 1.0, epsilon = 1e-6);
+    }
+
+    #[test]
+    fn test_point_cloud_centroid_simd() {
+        // Test with more than 4 points
+        let mut cloud = PointCloud2D::new();
+        for _ in 0..8 {
+            cloud.push(4.0, 6.0);
+        }
+
+        let centroid = cloud.centroid().unwrap();
+
+        assert_relative_eq!(centroid.x, 4.0, epsilon = 1e-6);
+        assert_relative_eq!(centroid.y, 6.0, epsilon = 1e-6);
+    }
+
+    #[test]
+    fn test_point_cloud_centroid_empty() {
+        let cloud = PointCloud2D::new();
+        assert!(cloud.centroid().is_none());
+    }
+
+    #[test]
+    fn test_point_cloud_iter() {
+        let mut cloud = PointCloud2D::new();
+        cloud.push(1.0, 2.0);
+        cloud.push(3.0, 4.0);
+
+        let points: Vec<_> = cloud.iter().collect();
+
+        assert_eq!(points.len(), 2);
+        assert_eq!(points[0], Point2D::new(1.0, 2.0));
+        assert_eq!(points[1], Point2D::new(3.0, 4.0));
+    }
+
+    #[test]
+    fn test_point_cloud_get() {
+        let mut cloud = PointCloud2D::new();
+        cloud.push(1.0, 2.0);
+        cloud.push(3.0, 4.0);
+
+        assert_eq!(cloud.get(0), Some(Point2D::new(1.0, 2.0)));
+        assert_eq!(cloud.get(1), Some(Point2D::new(3.0, 4.0)));
+        assert_eq!(cloud.get(2), None);
+    }
+}

vastu-map/src/extraction/corner_detection.rs

@@ -0,0 +1,376 @@
+//! Corner detection from line segments.
+//!
+//! Corners are detected at intersections of adjacent line segments
+//! that meet certain angle and distance criteria.
+
+use crate::core::Point2D;
+use crate::features::{Corner2D, Line2D};
+use std::f32::consts::PI;
+
+/// Configuration for corner detection.
+#[derive(Clone, Debug)]
+pub struct CornerConfig {
+    /// Minimum angle between lines to form a corner (radians).
+    /// Lines that are nearly parallel (angle below this) don't form corners.
+    /// Default: π/6 (30°)
+    pub min_angle: f32,
+
+    /// Maximum angle between lines to form a corner (radians).
+    /// Lines that are nearly collinear (angle above this) don't form corners.
+    /// Default: 5π/6 (150°)
+    pub max_angle: f32,
+
+    /// Maximum distance between line endpoints to form a corner.
+    /// If the nearest endpoints are farther apart, no corner is detected.
+    /// Default: 0.05m (5cm)
+    pub max_endpoint_distance: f32,
+}
+
+impl Default for CornerConfig {
+    fn default() -> Self {
+        Self {
+            min_angle: PI / 6.0,         // 30°
+            max_angle: 5.0 * PI / 6.0,   // 150°
+            max_endpoint_distance: 0.05, // 5cm
+        }
+    }
+}
+
+impl CornerConfig {
+    /// Create a new configuration with default values.
+    pub fn new() -> Self {
+        Self::default()
+    }
+
+    /// Builder-style setter for minimum angle.
+    pub fn with_min_angle(mut self, radians: f32) -> Self {
+        self.min_angle = radians;
+        self
+    }
+
+    /// Builder-style setter for maximum angle.
+    pub fn with_max_angle(mut self, radians: f32) -> Self {
+        self.max_angle = radians;
+        self
+    }
+
+    /// Builder-style setter for maximum endpoint distance.
+    pub fn with_max_endpoint_distance(mut self, meters: f32) -> Self {
+        self.max_endpoint_distance = meters;
+        self
+    }
+}
+
+/// Detect corners from adjacent line segments.
+///
+/// Adjacent segments are checked for corner formation based on:
+/// - Endpoint proximity
+/// - Angle between lines
+///
+/// # Arguments
+/// * `lines` - Line segments (assumed to be in order, e.g., from split-merge)
+/// * `config` - Corner detection configuration
+///
+/// # Returns
+/// Vector of detected corners with line indices and angles.
+pub fn detect_corners(lines: &[Line2D], config: &CornerConfig) -> Vec<Corner2D> {
+    if lines.len() < 2 {
+        return Vec::new();
+    }
+
+    let mut corners = Vec::new();
+
+    for i in 0..lines.len() - 1 {
+        if let Some(corner) = detect_corner_between(&lines[i], &lines[i + 1], i, i + 1, config) {
+            corners.push(corner);
+        }
+    }
+
+    corners
+}
+
+/// Detect corners from all pairs of lines (not just adjacent).
+///
+/// This is useful for finding corners in maps where lines may not be
+/// in sequential order.
+///
+/// # Note
+/// This has O(n²) complexity, so use sparingly for large line sets.
+pub fn detect_all_corners(lines: &[Line2D], config: &CornerConfig) -> Vec<Corner2D> {
+    if lines.len() < 2 {
+        return Vec::new();
+    }
+
+    let mut corners = Vec::new();
+
+    for i in 0..lines.len() {
+        for j in i + 1..lines.len() {
+            if let Some(corner) = detect_corner_between(&lines[i], &lines[j], i, j, config) {
+                corners.push(corner);
+            }
+        }
+    }
+
+    corners
+}
+
+/// Detect corner between two specific lines.
+fn detect_corner_between(
+    line1: &Line2D,
+    line2: &Line2D,
+    idx1: usize,
+    idx2: usize,
+    config: &CornerConfig,
+) -> Option<Corner2D> {
+    // Find nearest endpoints
+    let endpoints1 = [line1.start, line1.end];
+    let endpoints2 = [line2.start, line2.end];
+
+    let mut min_dist = f32::MAX;
+    let mut corner_point = Point2D::zero();
+    let mut _ep1_idx = 0;
+    let mut _ep2_idx = 0;
+
+    for (i, &ep1) in endpoints1.iter().enumerate() {
+        for (j, &ep2) in endpoints2.iter().enumerate() {
+            let dist = ep1.distance(ep2);
+            if dist < min_dist {
+                min_dist = dist;
+                corner_point = Point2D::new((ep1.x + ep2.x) / 2.0, (ep1.y + ep2.y) / 2.0);
+                _ep1_idx = i;
+                _ep2_idx = j;
+            }
+        }
+    }
+
+    // Check distance threshold
+    if min_dist > config.max_endpoint_distance {
+        return None;
+    }
+
+    // Compute angle between lines
+    let angle = angle_between_lines(line1, line2);
+
+    // Check angle thresholds
+    if angle < config.min_angle || angle > config.max_angle {
+        return None;
+    }
+
+    // Alternatively, use the intersection point if lines actually intersect
+    if let Some(intersection) = line_intersection_point(line1, line2) {
+        // Use intersection if it's close to the endpoints
+        if intersection.distance(corner_point) < config.max_endpoint_distance * 2.0 {
+            corner_point = intersection;
+        }
+    }
+
+    Some(Corner2D::new(corner_point, idx1, idx2, angle))
+}
+
+/// Compute the angle between two lines (0 to π).
+///
+/// Returns the acute or obtuse angle at their intersection.
+fn angle_between_lines(line1: &Line2D, line2: &Line2D) -> f32 {
+    let dir1 = line1.unit_direction();
+    let dir2 = line2.unit_direction();
+
+    // Use absolute dot product to get angle regardless of direction
+    let dot = dir1.dot(dir2).abs();
+
+    // Clamp to handle floating-point errors
+    let dot = dot.clamp(-1.0, 1.0);
+
+    // acos gives angle between directions
+    // We want the supplementary angle if > 90°
+    dot.acos()
+}
+
+/// Find intersection point of two infinite lines.
+fn line_intersection_point(line1: &Line2D, line2: &Line2D) -> Option<Point2D> {
+    let d1 = line1.direction();
+    let d2 = line2.direction();
+
+    let cross = d1.cross(d2);
+    if cross.abs() < f32::EPSILON {
+        // Parallel lines
+        return None;
+    }
+
+    let diff = line2.start - line1.start;
+    let t = diff.cross(d2) / cross;
+
+    Some(line1.point_at(t))
+}
+
+/// Filter corners by removing duplicates within a tolerance.
+///
+/// Corners at approximately the same position are merged,
+/// keeping the one with highest observation count.
+pub fn deduplicate_corners(corners: &mut Vec<Corner2D>, distance_tolerance: f32) {
+    if corners.len() < 2 {
+        return;
+    }
+
+    // Sort by x coordinate for spatial coherence
+    corners.sort_by(|a, b| a.position.x.partial_cmp(&b.position.x).unwrap());
+
+    let mut keep = vec![true; corners.len()];
+
+    for i in 0..corners.len() {
+        if !keep[i] {
+            continue;
+        }
+
+        for j in i + 1..corners.len() {
+            if !keep[j] {
+                continue;
+            }
+
+            // Early exit if too far in x
+            if corners[j].position.x - corners[i].position.x > distance_tolerance {
+                break;
+            }
+
+            if corners[i].distance(&corners[j]) < distance_tolerance {
+                // Keep the one with higher observation count
+                if corners[j].observation_count > corners[i].observation_count {
+                    keep[i] = false;
+                } else {
+                    keep[j] = false;
+                }
+            }
+        }
+    }
+
+    let mut idx = 0;
+    corners.retain(|_| {
+        let result = keep[idx];
+        idx += 1;
+        result
+    });
+}
+
+#[cfg(test)]
+mod tests {
+    use super::*;
+    use approx::assert_relative_eq;
+    use std::f32::consts::{FRAC_PI_2, FRAC_PI_4};
+
+    #[test]
+    fn test_detect_corner_perpendicular() {
+        // Two perpendicular lines meeting at origin
+        let line1 = Line2D::new(Point2D::new(-1.0, 0.0), Point2D::new(0.0, 0.0));
+        let line2 = Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(0.0, 1.0));
+
+        let config = CornerConfig::default();
+        let corners = detect_corners(&[line1, line2], &config);
+
+        assert_eq!(corners.len(), 1);
+        assert_relative_eq!(corners[0].angle, FRAC_PI_2, epsilon = 0.1);
+        assert!(corners[0].position.distance(Point2D::zero()) < 0.1);
+    }
+
+    #[test]
+    fn test_detect_corner_45_degrees() {
+        // Two lines meeting at 45 degrees
+        let line1 = Line2D::new(Point2D::new(-1.0, 0.0), Point2D::new(0.0, 0.0));
+        let line2 = Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(1.0, 1.0));
+
+        let config = CornerConfig::default();
+        let corners = detect_corners(&[line1, line2], &config);
+
+        assert_eq!(corners.len(), 1);
+        assert_relative_eq!(corners[0].angle, FRAC_PI_4, epsilon = 0.1);
+    }
+
+    #[test]
+    fn test_no_corner_parallel_lines() {
+        // Parallel lines don't form corners
+        let line1 = Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(1.0, 0.0));
+        let line2 = Line2D::new(Point2D::new(1.01, 0.0), Point2D::new(2.0, 0.0));
+
+        let config = CornerConfig::default();
+        let corners = detect_corners(&[line1, line2], &config);
+
+        // Should be empty - lines are collinear
+        assert!(corners.is_empty());
+    }
+
+    #[test]
+    fn test_no_corner_too_far() {
+        // Lines too far apart
+        let line1 = Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(1.0, 0.0));
+        let line2 = Line2D::new(Point2D::new(0.0, 1.0), Point2D::new(0.0, 2.0));
+
+        let config = CornerConfig::default().with_max_endpoint_distance(0.05);
+        let corners = detect_corners(&[line1, line2], &config);
+
+        assert!(corners.is_empty());
+    }
+
+    #[test]
+    fn test_no_corner_angle_too_small() {
+        // Angle below threshold (nearly parallel)
+        let line1 = Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(1.0, 0.0));
+        let line2 = Line2D::new(Point2D::new(1.0, 0.0), Point2D::new(2.0, 0.1)); // ~5.7° deviation
+
+        let config = CornerConfig::default().with_min_angle(PI / 6.0); // 30°
+        let corners = detect_corners(&[line1, line2], &config);
+
+        assert!(corners.is_empty());
+    }
+
+    #[test]
+    fn test_detect_all_corners() {
+        // Three lines forming two corners
+        let lines = vec![
+            Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(1.0, 0.0)),
+            Line2D::new(Point2D::new(1.0, 0.0), Point2D::new(1.0, 1.0)),
+            Line2D::new(Point2D::new(1.0, 1.0), Point2D::new(0.0, 1.0)),
+        ];
+
+        let config = CornerConfig::default();
+        let corners = detect_corners(&lines, &config);
+
+        assert_eq!(corners.len(), 2);
+    }
+
+    #[test]
+    fn test_angle_between_lines() {
+        // Perpendicular lines
+        let line1 = Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(1.0, 0.0));
+        let line2 = Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(0.0, 1.0));
+
+        let angle = angle_between_lines(&line1, &line2);
+        assert_relative_eq!(angle, FRAC_PI_2, epsilon = 0.01);
+
+        // Parallel lines
+        let line3 = Line2D::new(Point2D::new(0.0, 1.0), Point2D::new(1.0, 1.0));
+        let angle2 = angle_between_lines(&line1, &line3);
+        assert_relative_eq!(angle2, 0.0, epsilon = 0.01);
+    }
+
+    #[test]
+    fn test_deduplicate_corners() {
+        let mut corners = vec![
+            Corner2D::new(Point2D::new(0.0, 0.0), 0, 1, FRAC_PI_2),
+            Corner2D::new(Point2D::new(0.01, 0.01), 0, 1, FRAC_PI_2), // Duplicate
+            Corner2D::new(Point2D::new(5.0, 5.0), 2, 3, FRAC_PI_2),
+        ];
+
+        deduplicate_corners(&mut corners, 0.05);
+
+        assert_eq!(corners.len(), 2);
+    }
+
+    #[test]
+    fn test_line_intersection() {
+        let line1 = Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(2.0, 2.0));
+        let line2 = Line2D::new(Point2D::new(0.0, 2.0), Point2D::new(2.0, 0.0));
+
+        let intersection = line_intersection_point(&line1, &line2).unwrap();
+
+        assert_relative_eq!(intersection.x, 1.0, epsilon = 0.01);
+        assert_relative_eq!(intersection.y, 1.0, epsilon = 0.01);
+    }
+}

vastu-map/src/extraction/line_fitting.rs

@@ -0,0 +1,346 @@
+//! Line fitting using Total Least Squares (Orthogonal Regression).
+//!
+//! TLS minimizes perpendicular distances to the line, not vertical distances
+//! like ordinary least squares. This is appropriate for lidar data where
+//! errors are isotropic.
+
+use crate::core::Point2D;
+use crate::features::Line2D;
+
+/// Fit a line to a set of points using Total Least Squares.
+///
+/// The algorithm:
+/// 1. Compute centroid of points
+/// 2. Compute covariance matrix
+/// 3. Find eigenvector of smallest eigenvalue (perpendicular to line)
+/// 4. Line direction is eigenvector of largest eigenvalue
+///
+/// Returns None if:
+/// - Fewer than 2 points
+/// - Points are collinear in one direction (degenerate case)
+///
+/// # Example
+/// ```
+/// use vastu_map::extraction::fit_line;
+/// use vastu_map::core::Point2D;
+///
+/// let points = vec![
+///     Point2D::new(0.0, 0.0),
+///     Point2D::new(1.0, 0.1),  // Slight noise
+///     Point2D::new(2.0, 0.0),
+///     Point2D::new(3.0, -0.1),
+///     Point2D::new(4.0, 0.0),
+/// ];
+///
+/// let line = fit_line(&points).unwrap();
+/// // Line should be approximately horizontal
+/// ```
+pub fn fit_line(points: &[Point2D]) -> Option<Line2D> {
+    if points.len() < 2 {
+        return None;
+    }
+
+    // Compute centroid
+    let n = points.len() as f32;
+    let mut sum_x: f32 = 0.0;
+    let mut sum_y: f32 = 0.0;
+
+    for p in points {
+        sum_x += p.x;
+        sum_y += p.y;
+    }
+
+    let centroid = Point2D::new(sum_x / n, sum_y / n);
+
+    // Compute covariance matrix elements
+    // Cov = | cxx  cxy |
+    //       | cxy  cyy |
+    let mut cxx: f32 = 0.0;
+    let mut cyy: f32 = 0.0;
+    let mut cxy: f32 = 0.0;
+
+    for p in points {
+        let dx = p.x - centroid.x;
+        let dy = p.y - centroid.y;
+        cxx += dx * dx;
+        cyy += dy * dy;
+        cxy += dx * dy;
+    }
+
+    // Find eigenvectors of 2x2 covariance matrix
+    // For 2x2 symmetric matrix, eigenvalues are:
+    // λ = (cxx + cyy)/2 ± sqrt(((cxx - cyy)/2)² + cxy²)
+
+    let trace_half = (cxx + cyy) / 2.0;
+    let det_sqrt = ((cxx - cyy) / 2.0).powi(2) + cxy * cxy;
+
+    if det_sqrt < f32::EPSILON {
+        // Degenerate case - points form a circle or single point
+        // Use simple linear regression fallback
+        return fit_line_simple(points);
+    }
+
+    let det_sqrt = det_sqrt.sqrt();
+    let lambda1 = trace_half + det_sqrt; // Larger eigenvalue
+    let _lambda2 = trace_half - det_sqrt; // Smaller eigenvalue
+
+    // Eigenvector for larger eigenvalue (line direction)
+    // For eigenvalue λ1, eigenvector satisfies (cxx - λ1)x + cxy*y = 0
+    // Direction: (cxy, λ1 - cxx) or (-cxy, cxx - λ1)
+
+    let direction = if cxy.abs() > f32::EPSILON {
+        Point2D::new(lambda1 - cyy, cxy).normalized()
+    } else if cxx > cyy {
+        Point2D::new(1.0, 0.0) // Horizontal line
+    } else {
+        Point2D::new(0.0, 1.0) // Vertical line
+    };
+
+    // Find extent of points along line direction
+    let mut t_min = f32::MAX;
+    let mut t_max = f32::MIN;
+
+    for p in points {
+        let t = (p.x - centroid.x) * direction.x + (p.y - centroid.y) * direction.y;
+        t_min = t_min.min(t);
+        t_max = t_max.max(t);
+    }
+
+    // Create line from extent
+    let start = Point2D::new(
+        centroid.x + t_min * direction.x,
+        centroid.y + t_min * direction.y,
+    );
+    let end = Point2D::new(
+        centroid.x + t_max * direction.x,
+        centroid.y + t_max * direction.y,
+    );
+
+    Some(Line2D::new(start, end))
+}
+
+/// Simple line fitting fallback using first and last points.
+fn fit_line_simple(points: &[Point2D]) -> Option<Line2D> {
+    if points.len() < 2 {
+        return None;
+    }
+    Some(Line2D::new(points[0], points[points.len() - 1]))
+}
+
+/// Compute the perpendicular fitting error (RMS distance to line).
+///
+/// Returns the root-mean-square of perpendicular distances from points to the line.
+pub fn fitting_error(points: &[Point2D], line: &Line2D) -> f32 {
+    if points.is_empty() {
+        return 0.0;
+    }
+
+    let mut sum_sq: f32 = 0.0;
+    for p in points {
+        let dist = line.distance_to_point(*p);
+        sum_sq += dist * dist;
+    }
+
+    (sum_sq / points.len() as f32).sqrt()
+}
+
+/// Find the point with maximum distance from a line.
+///
+/// Returns (index, distance) of the farthest point.
+/// Returns None if points is empty.
+pub fn max_distance_point(points: &[Point2D], line: &Line2D) -> Option<(usize, f32)> {
+    if points.is_empty() {
+        return None;
+    }
+
+    let mut max_idx = 0;
+    let mut max_dist: f32 = 0.0;
+
+    for (i, p) in points.iter().enumerate() {
+        let dist = line.distance_to_point(*p);
+        if dist > max_dist {
+            max_dist = dist;
+            max_idx = i;
+        }
+    }
+
+    Some((max_idx, max_dist))
+}
+
+/// Fit a line segment to points with known start and end indices.
+///
+/// The endpoints are snapped to actual point positions for accurate
+/// segment boundaries.
+pub fn fit_line_segment(points: &[Point2D], start_idx: usize, end_idx: usize) -> Option<Line2D> {
+    if start_idx >= points.len() || end_idx >= points.len() || start_idx >= end_idx {
+        return None;
+    }
+
+    let segment_points = &points[start_idx..=end_idx];
+    let mut line = fit_line(segment_points)?;
+
+    // Snap endpoints to actual point projections
+    // This ensures the segment covers exactly the input points
+    let start_proj = line.project_point_onto_line(points[start_idx]);
+    let end_proj = line.project_point_onto_line(points[end_idx]);
+
+    line.start = start_proj;
+    line.end = end_proj;
+
+    Some(line)
+}
+
+#[cfg(test)]
+mod tests {
+    use super::*;
+    use approx::assert_relative_eq;
+    use std::f32::consts::{FRAC_PI_2, FRAC_PI_4};
+
+    #[test]
+    fn test_fit_horizontal_line() {
+        let points = vec![
+            Point2D::new(0.0, 0.0),
+            Point2D::new(1.0, 0.0),
+            Point2D::new(2.0, 0.0),
+            Point2D::new(3.0, 0.0),
+            Point2D::new(4.0, 0.0),
+        ];
+
+        let line = fit_line(&points).unwrap();
+
+        // Line should be horizontal
+        assert_relative_eq!(line.angle(), 0.0, epsilon = 0.01);
+
+        // Should span from (0,0) to (4,0)
+        assert_relative_eq!(line.start.x, 0.0, epsilon = 0.01);
+        assert_relative_eq!(line.end.x, 4.0, epsilon = 0.01);
+        assert_relative_eq!(line.start.y, 0.0, epsilon = 0.01);
+        assert_relative_eq!(line.end.y, 0.0, epsilon = 0.01);
+    }
+
+    #[test]
+    fn test_fit_vertical_line() {
+        let points = vec![
+            Point2D::new(0.0, 0.0),
+            Point2D::new(0.0, 1.0),
+            Point2D::new(0.0, 2.0),
+            Point2D::new(0.0, 3.0),
+        ];
+
+        let line = fit_line(&points).unwrap();
+
+        // Line should be vertical
+        assert_relative_eq!(line.angle().abs(), FRAC_PI_2, epsilon = 0.01);
+    }
+
+    #[test]
+    fn test_fit_diagonal_line() {
+        let points = vec![
+            Point2D::new(0.0, 0.0),
+            Point2D::new(1.0, 1.0),
+            Point2D::new(2.0, 2.0),
+            Point2D::new(3.0, 3.0),
+        ];
+
+        let line = fit_line(&points).unwrap();
+
+        // Line should be at 45 degrees
+        assert_relative_eq!(line.angle().abs(), FRAC_PI_4, epsilon = 0.01);
+    }
+
+    #[test]
+    fn test_fit_with_noise() {
+        let points = vec![
+            Point2D::new(0.0, 0.02),
+            Point2D::new(1.0, -0.01),
+            Point2D::new(2.0, 0.03),
+            Point2D::new(3.0, -0.02),
+            Point2D::new(4.0, 0.01),
+        ];
+
+        let line = fit_line(&points).unwrap();
+
+        // Line should still be approximately horizontal
+        assert!(line.angle().abs() < 0.1);
+
+        // Fitting error should be small
+        let error = fitting_error(&points, &line);
+        assert!(error < 0.03);
+    }
+
+    #[test]
+    fn test_fit_two_points() {
+        let points = vec![Point2D::new(0.0, 0.0), Point2D::new(5.0, 3.0)];
+
+        let line = fit_line(&points).unwrap();
+
+        assert_relative_eq!(line.start.distance(points[0]), 0.0, epsilon = 0.01);
+        assert_relative_eq!(line.end.distance(points[1]), 0.0, epsilon = 0.01);
+    }
+
+    #[test]
+    fn test_fit_one_point() {
+        let points = vec![Point2D::new(0.0, 0.0)];
+        assert!(fit_line(&points).is_none());
+    }
+
+    #[test]
+    fn test_fit_empty() {
+        let points: Vec<Point2D> = vec![];
+        assert!(fit_line(&points).is_none());
+    }
+
+    #[test]
+    fn test_fitting_error() {
+        let line = Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(10.0, 0.0));
+
+        // Points on the line
+        let points_on = vec![
+            Point2D::new(0.0, 0.0),
+            Point2D::new(5.0, 0.0),
+            Point2D::new(10.0, 0.0),
+        ];
+        assert_relative_eq!(fitting_error(&points_on, &line), 0.0, epsilon = 1e-6);
+
+        // Points 1 unit away
+        let points_off = vec![
+            Point2D::new(0.0, 1.0),
+            Point2D::new(5.0, 1.0),
+            Point2D::new(10.0, 1.0),
+        ];
+        assert_relative_eq!(fitting_error(&points_off, &line), 1.0, epsilon = 1e-6);
+    }
+
+    #[test]
+    fn test_max_distance_point() {
+        let line = Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(10.0, 0.0));
+
+        let points = vec![
+            Point2D::new(2.0, 1.0),
+            Point2D::new(5.0, 3.0), // Farthest
+            Point2D::new(8.0, 2.0),
+        ];
+
+        let (idx, dist) = max_distance_point(&points, &line).unwrap();
+
+        assert_eq!(idx, 1);
+        assert_relative_eq!(dist, 3.0, epsilon = 1e-6);
+    }
+
+    #[test]
+    fn test_fit_line_segment() {
+        let points = vec![
+            Point2D::new(0.0, 0.0), // idx 0
+            Point2D::new(1.0, 0.0), // idx 1
+            Point2D::new(2.0, 0.0), // idx 2
+            Point2D::new(3.0, 0.0), // idx 3
+            Point2D::new(4.0, 0.0), // idx 4
+        ];
+
+        let line = fit_line_segment(&points, 1, 3).unwrap();
+
+        // Should span from point 1 to point 3
+        assert_relative_eq!(line.start.x, 1.0, epsilon = 0.01);
+        assert_relative_eq!(line.end.x, 3.0, epsilon = 0.01);
+    }
+}

vastu-map/src/extraction/mod.rs

@@ -0,0 +1,18 @@
+//! Feature extraction from point clouds.
+//!
+//! This module provides algorithms for extracting geometric features
+//! (lines and corners) from lidar point clouds.
+//!
+//! # Algorithms
+//!
+//! - **Line Fitting**: Total Least Squares (orthogonal regression)
+//! - **Line Extraction**: Split-and-Merge algorithm for ordered point sequences
+//! - **Corner Detection**: Find corners at line intersections
+
+pub mod corner_detection;
+pub mod line_fitting;
+pub mod split_merge;
+
+pub use corner_detection::{CornerConfig, deduplicate_corners, detect_all_corners, detect_corners};
+pub use line_fitting::{fit_line, fit_line_segment, fitting_error, max_distance_point};
+pub use split_merge::{SplitMergeConfig, extract_lines};

vastu-map/src/extraction/split_merge.rs

@@ -0,0 +1,538 @@
+//! Split-and-Merge line extraction algorithm.
+//!
+//! This algorithm extracts line segments from an ordered sequence of points
+//! (e.g., from a lidar scan). It works by:
+//!
+//! 1. **Split Phase**: Recursively split point sequences where the maximum
+//!    deviation from the fitted line exceeds a threshold.
+//!
+//! 2. **Merge Phase**: Adjacent segments that can be represented by a single
+//!    line within tolerance are merged.
+//!
+//! Requirements:
+//! - Points must be ordered (sequential around scan)
+//! - Works best with relatively uniform point density
+
+use crate::core::Point2D;
+use crate::features::Line2D;
+
+use super::line_fitting::{fit_line, max_distance_point};
+
+/// Configuration for Split-and-Merge algorithm.
+#[derive(Clone, Debug)]
+pub struct SplitMergeConfig {
+    /// Maximum perpendicular distance for a point to be considered on the line.
+    /// If any point deviates more than this, the segment is split.
+    /// Default: 0.05m (5cm)
+    pub split_threshold: f32,
+
+    /// Minimum number of points required to form a line segment.
+    /// Segments with fewer points are discarded.
+    /// Default: 5
+    pub min_points: usize,
+
+    /// Minimum line segment length.
+    /// Segments shorter than this are discarded.
+    /// Default: 0.10m (10cm)
+    pub min_length: f32,
+
+    /// Maximum gap between consecutive points.
+    /// If gap exceeds this, points are split into separate sequences.
+    /// Default: 0.30m (30cm)
+    pub max_point_gap: f32,
+
+    /// Merge threshold for adjacent segments.
+    /// Segments whose combined fit error is below this are merged.
+    /// Default: same as split_threshold
+    pub merge_threshold: f32,
+}
+
+impl Default for SplitMergeConfig {
+    fn default() -> Self {
+        Self {
+            split_threshold: 0.05,
+            min_points: 5,
+            min_length: 0.10,
+            max_point_gap: 0.30,
+            merge_threshold: 0.05,
+        }
+    }
+}
+
+impl SplitMergeConfig {
+    /// Create a new configuration with default values.
+    pub fn new() -> Self {
+        Self::default()
+    }
+
+    /// Builder-style setter for split threshold.
+    pub fn with_split_threshold(mut self, value: f32) -> Self {
+        self.split_threshold = value;
+        self
+    }
+
+    /// Builder-style setter for minimum points.
+    pub fn with_min_points(mut self, value: usize) -> Self {
+        self.min_points = value;
+        self
+    }
+
+    /// Builder-style setter for minimum length.
+    pub fn with_min_length(mut self, value: f32) -> Self {
+        self.min_length = value;
+        self
+    }
+
+    /// Builder-style setter for maximum point gap.
+    pub fn with_max_point_gap(mut self, value: f32) -> Self {
+        self.max_point_gap = value;
+        self
+    }
+}
+
+/// Extract line segments from an ordered sequence of points.
+///
+/// Points should be ordered (e.g., by angle in a lidar scan).
+/// Returns extracted line segments.
+///
+/// # Example
+/// ```
+/// use vastu_map::extraction::{SplitMergeConfig, extract_lines};
+/// use vastu_map::core::Point2D;
+///
+/// // Simulated lidar points forming two perpendicular walls
+/// let points = vec![
+///     // Horizontal wall
+///     Point2D::new(0.0, 1.0),
+///     Point2D::new(0.5, 1.0),
+///     Point2D::new(1.0, 1.0),
+///     Point2D::new(1.5, 1.0),
+///     Point2D::new(2.0, 1.0),
+///     // Corner transition
+///     // Vertical wall
+///     Point2D::new(2.0, 1.5),
+///     Point2D::new(2.0, 2.0),
+///     Point2D::new(2.0, 2.5),
+///     Point2D::new(2.0, 3.0),
+///     Point2D::new(2.0, 3.5),
+/// ];
+///
+/// let config = SplitMergeConfig::default();
+/// let lines = extract_lines(&points, &config);
+/// // Should extract approximately 2 lines
+/// ```
+pub fn extract_lines(points: &[Point2D], config: &SplitMergeConfig) -> Vec<Line2D> {
+    if points.len() < config.min_points {
+        return Vec::new();
+    }
+
+    // Split points into continuous sequences by index pairs (avoids copying)
+    let sequences = split_by_gaps_indices(points, config.max_point_gap);
+
+    let mut all_lines = Vec::new();
+
+    for (start, end) in sequences {
+        let seq_len = end - start;
+        if seq_len < config.min_points {
+            continue;
+        }
+
+        // Extract lines from this sequence using split-and-merge
+        let sequence = &points[start..end];
+        let lines = split_and_merge(sequence, config);
+        all_lines.extend(lines);
+    }
+
+    all_lines
+}
+
+/// Split points into continuous sequences based on gap threshold.
+/// Returns (start, end) index pairs instead of copying points.
+///
+/// This is a zero-copy optimization that returns index ranges into the
+/// original points slice.
+fn split_by_gaps_indices(points: &[Point2D], max_gap: f32) -> Vec<(usize, usize)> {
+    if points.is_empty() {
+        return Vec::new();
+    }
+
+    let mut sequences = Vec::new();
+    let mut seq_start = 0;
+
+    for i in 1..points.len() {
+        let gap = points[i].distance(points[i - 1]);
+        if gap > max_gap {
+            // End current sequence and start new one
+            if i > seq_start {
+                sequences.push((seq_start, i));
+            }
+            seq_start = i;
+        }
+    }
+
+    // Add final sequence
+    if points.len() > seq_start {
+        sequences.push((seq_start, points.len()));
+    }
+
+    sequences
+}
+
+/// Split points into continuous sequences based on gap threshold.
+/// Returns copied sequences (for compatibility with tests).
+#[cfg(test)]
+fn split_by_gaps(points: &[Point2D], max_gap: f32) -> Vec<Vec<Point2D>> {
+    split_by_gaps_indices(points, max_gap)
+        .into_iter()
+        .map(|(start, end)| points[start..end].to_vec())
+        .collect()
+}
+
+/// Main split-and-merge algorithm on a continuous sequence.
+fn split_and_merge(points: &[Point2D], config: &SplitMergeConfig) -> Vec<Line2D> {
+    // Split phase: find breakpoints
+    let mut breakpoints = vec![0];
+    split_recursive(points, 0, points.len() - 1, config, &mut breakpoints);
+    breakpoints.push(points.len() - 1);
+    breakpoints.sort_unstable();
+    breakpoints.dedup();
+
+    // Create line segments from breakpoints
+    let mut segments: Vec<(usize, usize, Line2D)> = Vec::new();
+
+    for i in 0..breakpoints.len() - 1 {
+        let start = breakpoints[i];
+        let end = breakpoints[i + 1];
+
+        if end - start + 1 < config.min_points {
+            continue;
+        }
+
+        let segment_points = &points[start..=end];
+        if let Some(line) = fit_line(segment_points)
+            && line.length() >= config.min_length
+        {
+            segments.push((start, end, line));
+        }
+    }
+
+    // Merge phase: combine adjacent segments that fit well together
+    let merged = merge_segments(points, segments, config);
+
+    merged.into_iter().map(|(_, _, line)| line).collect()
+}
+
+/// Recursive split function.
+fn split_recursive(
+    points: &[Point2D],
+    start: usize,
+    end: usize,
+    config: &SplitMergeConfig,
+    breakpoints: &mut Vec<usize>,
+) {
+    if end <= start + 1 {
+        return;
+    }
+
+    let segment_points = &points[start..=end];
+
+    // Fit line to segment
+    let Some(line) = fit_line(segment_points) else {
+        return;
+    };
+
+    // Find point with maximum deviation
+    let Some((max_idx, max_dist)) = max_distance_point(segment_points, &line) else {
+        return;
+    };
+
+    // If deviation exceeds threshold, split at this point
+    if max_dist > config.split_threshold {
+        let split_idx = start + max_idx;
+
+        // Prevent degenerate splits at boundaries that cause infinite recursion.
+        // This happens with continuous 360° lidar scans where max deviation point
+        // is at index 0, causing split_idx == start and identical recursive calls.
+        if split_idx == start || split_idx == end {
+            return;
+        }
+
+        breakpoints.push(split_idx);
+
+        // Recursively split both halves
+        split_recursive(points, start, split_idx, config, breakpoints);
+        split_recursive(points, split_idx, end, config, breakpoints);
+    }
+}
+
+/// Merge adjacent segments that can be represented by a single line.
+fn merge_segments(
+    points: &[Point2D],
+    mut segments: Vec<(usize, usize, Line2D)>,
+    config: &SplitMergeConfig,
+) -> Vec<(usize, usize, Line2D)> {
+    if segments.len() < 2 {
+        return segments;
+    }
+
+    let mut merged = true;
+    while merged {
+        merged = false;
+        let mut new_segments = Vec::new();
+        let mut i = 0;
+
+        while i < segments.len() {
+            if i + 1 < segments.len() {
+                let (start1, end1, _) = segments[i];
+                let (start2, end2, _) = segments[i + 1];
+
+                // Check if segments are adjacent
+                if end1 == start2 || end1 + 1 == start2 {
+                    // Try to merge
+                    let merged_points = &points[start1..=end2];
+                    if let Some(merged_line) = fit_line(merged_points) {
+                        // Check if merged line fits well
+                        if let Some((_, max_dist)) = max_distance_point(merged_points, &merged_line)
+                            && max_dist <= config.merge_threshold
+                            && merged_line.length() >= config.min_length
+                        {
+                            // Merge successful
+                            new_segments.push((start1, end2, merged_line));
+                            i += 2;
+                            merged = true;
+                            continue;
+                        }
+                    }
+                }
+            }
+
+            // Keep original segment
+            new_segments.push(segments[i]);
+            i += 1;
+        }
+
+        segments = new_segments;
+    }
+
+    segments
+}
+
+#[cfg(test)]
+mod tests {
+    use super::*;
+    use approx::assert_relative_eq;
+    use std::f32::consts::FRAC_PI_2;
+
+    #[test]
+    fn test_split_by_gaps() {
+        let points = vec![
+            Point2D::new(0.0, 0.0),
+            Point2D::new(0.1, 0.0),
+            Point2D::new(0.2, 0.0),
+            // Gap
+            Point2D::new(1.0, 0.0),
+            Point2D::new(1.1, 0.0),
+        ];
+
+        let sequences = split_by_gaps(&points, 0.3);
+
+        assert_eq!(sequences.len(), 2);
+        assert_eq!(sequences[0].len(), 3);
+        assert_eq!(sequences[1].len(), 2);
+    }
+
+    #[test]
+    fn test_extract_single_line() {
+        // Generate points along a horizontal line
+        let points: Vec<_> = (0..20).map(|i| Point2D::new(i as f32 * 0.1, 0.0)).collect();
+
+        let config = SplitMergeConfig::default();
+        let lines = extract_lines(&points, &config);
+
+        assert_eq!(lines.len(), 1);
+        assert_relative_eq!(lines[0].angle(), 0.0, epsilon = 0.1);
+    }
+
+    #[test]
+    fn test_extract_two_lines_corner() {
+        // L-shaped points: horizontal then vertical
+        let mut points = Vec::new();
+
+        // Horizontal segment (10 points)
+        for i in 0..10 {
+            points.push(Point2D::new(i as f32 * 0.2, 0.0));
+        }
+
+        // Vertical segment (10 points), starting from end of horizontal
+        for i in 1..10 {
+            points.push(Point2D::new(1.8, i as f32 * 0.2));
+        }
+
+        let config = SplitMergeConfig::default()
+            .with_min_points(5)
+            .with_min_length(0.5);
+
+        let lines = extract_lines(&points, &config);
+
+        // Should extract 2 lines
+        assert!(
+            lines.len() >= 1,
+            "Expected at least 1 line, got {}",
+            lines.len()
+        );
+
+        // One should be approximately horizontal, one approximately vertical
+        let angles: Vec<_> = lines.iter().map(|l| l.angle()).collect();
+
+        if lines.len() >= 2 {
+            // Check that we have both orientations
+            let has_horizontal = angles.iter().any(|a| a.abs() < 0.3);
+            let has_vertical = angles.iter().any(|a| (a.abs() - FRAC_PI_2).abs() < 0.3);
+
+            assert!(
+                has_horizontal || has_vertical,
+                "Expected horizontal or vertical lines, got angles: {:?}",
+                angles
+            );
+        }
+    }
+
+    #[test]
+    fn test_extract_noisy_line() {
+        // Points along a line with some noise
+        let points: Vec<_> = (0..20)
+            .map(|i| {
+                let noise = if i % 2 == 0 { 0.02 } else { -0.02 };
+                Point2D::new(i as f32 * 0.1, noise)
+            })
+            .collect();
+
+        let config = SplitMergeConfig::default().with_split_threshold(0.05);
+        let lines = extract_lines(&points, &config);
+
+        // Should still extract as single line (noise within threshold)
+        assert_eq!(lines.len(), 1);
+    }
+
+    #[test]
+    fn test_extract_min_points_filter() {
+        // Only 3 points - below minimum
+        let points = vec![
+            Point2D::new(0.0, 0.0),
+            Point2D::new(1.0, 0.0),
+            Point2D::new(2.0, 0.0),
+        ];
+
+        let config = SplitMergeConfig::default().with_min_points(5);
+        let lines = extract_lines(&points, &config);
+
+        assert!(lines.is_empty());
+    }
+
+    #[test]
+    fn test_extract_min_length_filter() {
+        // Short line
+        let points: Vec<_> = (0..10)
+            .map(|i| Point2D::new(i as f32 * 0.005, 0.0)) // Total length 0.045m
+            .collect();
+
+        let config = SplitMergeConfig::default()
+            .with_min_points(3)
+            .with_min_length(0.1);
+
+        let lines = extract_lines(&points, &config);
+
+        assert!(lines.is_empty());
+    }
+
+    #[test]
+    fn test_extract_with_gap() {
+        // Two separate line segments with a gap
+        let mut points = Vec::new();
+
+        // First segment
+        for i in 0..10 {
+            points.push(Point2D::new(i as f32 * 0.1, 0.0));
+        }
+
+        // Gap of 1.0m
+        // Second segment
+        for i in 0..10 {
+            points.push(Point2D::new(2.0 + i as f32 * 0.1, 1.0));
+        }
+
+        let config = SplitMergeConfig::default()
+            .with_max_point_gap(0.5)
+            .with_min_points(5);
+
+        let lines = extract_lines(&points, &config);
+
+        // Should extract 2 separate lines
+        assert_eq!(lines.len(), 2);
+    }
+
+    #[test]
+    fn test_empty_input() {
+        let points: Vec<Point2D> = vec![];
+        let config = SplitMergeConfig::default();
+        let lines = extract_lines(&points, &config);
+
+        assert!(lines.is_empty());
+    }
+
+    #[test]
+    fn test_circular_points_no_infinite_recursion() {
+        // Simulate a 360° lidar scan forming a rough circle.
+        // This previously caused infinite recursion when max deviation
+        // point was at index 0, causing split_idx == start.
+        use std::f32::consts::PI;
+
+        let num_points = 100;
+        let radius = 2.0;
+        let points: Vec<_> = (0..num_points)
+            .map(|i| {
+                let angle = 2.0 * PI * (i as f32) / (num_points as f32);
+                // Add some noise to create deviation
+                let r = radius + 0.1 * ((i % 5) as f32 - 2.0);
+                Point2D::new(r * angle.cos(), r * angle.sin())
+            })
+            .collect();
+
+        let config = SplitMergeConfig::default()
+            .with_min_points(3)
+            .with_split_threshold(0.1);
+
+        // This should complete without hanging (previously infinite recursion)
+        let lines = extract_lines(&points, &config);
+
+        // We should get some lines (exact number depends on noise pattern)
+        // The important thing is that it terminates without panic
+        let _ = lines.len();
+    }
+
+    #[test]
+    fn test_degenerate_split_at_boundary() {
+        // Test case where all points except first/last are collinear,
+        // forcing max deviation at boundary indices.
+        let points = vec![
+            Point2D::new(0.0, 1.0), // Outlier at start
+            Point2D::new(0.1, 0.0),
+            Point2D::new(0.2, 0.0),
+            Point2D::new(0.3, 0.0),
+            Point2D::new(0.4, 0.0),
+            Point2D::new(0.5, 0.0),
+            Point2D::new(0.6, 0.0),
+            Point2D::new(0.7, 0.0),
+            Point2D::new(0.8, 0.0),
+            Point2D::new(0.9, 1.0), // Outlier at end
+        ];
+
+        let config = SplitMergeConfig::default()
+            .with_min_points(3)
+            .with_split_threshold(0.05);
+
+        // Should terminate without infinite recursion
+        let lines = extract_lines(&points, &config);
+        let _ = lines.len();
+    }
+}

vastu-map/src/features/corner.rs

@@ -0,0 +1,359 @@
+//! Corner feature type representing line intersections.
+//!
+//! Corners are detected where two lines meet at a significant angle.
+
+use crate::core::{Point2D, Pose2D};
+
+/// A corner (vertex) feature detected at a line intersection.
+///
+/// Corners are stable features useful for localization because they're
+/// viewpoint-invariant and easy to detect consistently.
+#[derive(Clone, Copy, Debug, PartialEq)]
+pub struct Corner2D {
+    /// Position of the corner in world coordinates.
+    pub position: Point2D,
+    /// Index of the first line forming this corner.
+    pub line1_idx: usize,
+    /// Index of the second line forming this corner.
+    pub line2_idx: usize,
+    /// Angle between the two lines (in radians, 0 to π).
+    pub angle: f32,
+    /// Number of times this corner has been observed.
+    pub observation_count: u32,
+}
+
+impl Corner2D {
+    /// Create a new corner.
+    #[inline]
+    pub fn new(position: Point2D, line1_idx: usize, line2_idx: usize, angle: f32) -> Self {
+        Self {
+            position,
+            line1_idx,
+            line2_idx,
+            angle,
+            observation_count: 1,
+        }
+    }
+
+    /// Create a corner with a specific observation count.
+    #[inline]
+    pub fn with_observation_count(
+        position: Point2D,
+        line1_idx: usize,
+        line2_idx: usize,
+        angle: f32,
+        count: u32,
+    ) -> Self {
+        Self {
+            position,
+            line1_idx,
+            line2_idx,
+            angle,
+            observation_count: count,
+        }
+    }
+
+    /// Transform corner by a pose.
+    #[inline]
+    pub fn transform(&self, pose: &Pose2D) -> Self {
+        Self {
+            position: pose.transform_point(self.position),
+            line1_idx: self.line1_idx,
+            line2_idx: self.line2_idx,
+            angle: self.angle,
+            observation_count: self.observation_count,
+        }
+    }
+
+    /// Distance to another corner.
+    #[inline]
+    pub fn distance(&self, other: &Corner2D) -> f32 {
+        self.position.distance(other.position)
+    }
+
+    /// Check if this corner is approximately equal to another.
+    ///
+    /// Compares position and angle, ignoring line indices.
+    #[inline]
+    pub fn approx_eq(&self, other: &Corner2D, pos_epsilon: f32, angle_epsilon: f32) -> bool {
+        self.position.approx_eq(other.position, pos_epsilon)
+            && (self.angle - other.angle).abs() <= angle_epsilon
+    }
+
+    /// Check if the corner angle is valid (between min and max thresholds).
+    ///
+    /// # Arguments
+    /// * `min_angle` - Minimum valid angle in radians
+    /// * `max_angle` - Maximum valid angle in radians
+    #[inline]
+    pub fn is_valid_angle(&self, min_angle: f32, max_angle: f32) -> bool {
+        self.angle >= min_angle && self.angle <= max_angle
+    }
+}
+
+impl Default for Corner2D {
+    fn default() -> Self {
+        Self {
+            position: Point2D::zero(),
+            line1_idx: 0,
+            line2_idx: 0,
+            angle: 0.0,
+            observation_count: 0,
+        }
+    }
+}
+
+/// Collection of corners with efficient storage.
+#[derive(Clone, Debug, Default)]
+pub struct CornerCollection {
+    /// Positions of all corners (SoA: x coordinates).
+    pub xs: Vec<f32>,
+    /// Positions of all corners (SoA: y coordinates).
+    pub ys: Vec<f32>,
+    /// First line index for each corner.
+    pub line1_indices: Vec<usize>,
+    /// Second line index for each corner.
+    pub line2_indices: Vec<usize>,
+    /// Angles at each corner.
+    pub angles: Vec<f32>,
+    /// Observation counts.
+    pub observation_counts: Vec<u32>,
+}
+
+impl CornerCollection {
+    /// Create a new empty corner collection.
+    pub fn new() -> Self {
+        Self::default()
+    }
+
+    /// Create with capacity.
+    pub fn with_capacity(capacity: usize) -> Self {
+        Self {
+            xs: Vec::with_capacity(capacity),
+            ys: Vec::with_capacity(capacity),
+            line1_indices: Vec::with_capacity(capacity),
+            line2_indices: Vec::with_capacity(capacity),
+            angles: Vec::with_capacity(capacity),
+            observation_counts: Vec::with_capacity(capacity),
+        }
+    }
+
+    /// Create from a slice of Corner2D.
+    pub fn from_corners(corners: &[Corner2D]) -> Self {
+        let mut collection = Self::with_capacity(corners.len());
+        for corner in corners {
+            collection.push_corner(corner);
+        }
+        collection
+    }
+
+    /// Add a corner to the collection.
+    #[inline]
+    pub fn push_corner(&mut self, corner: &Corner2D) {
+        self.xs.push(corner.position.x);
+        self.ys.push(corner.position.y);
+        self.line1_indices.push(corner.line1_idx);
+        self.line2_indices.push(corner.line2_idx);
+        self.angles.push(corner.angle);
+        self.observation_counts.push(corner.observation_count);
+    }
+
+    /// Number of corners in the collection.
+    #[inline]
+    pub fn len(&self) -> usize {
+        self.xs.len()
+    }
+
+    /// Check if the collection is empty.
+    #[inline]
+    pub fn is_empty(&self) -> bool {
+        self.xs.is_empty()
+    }
+
+    /// Get a corner by index.
+    #[inline]
+    pub fn get(&self, index: usize) -> Option<Corner2D> {
+        if index < self.len() {
+            Some(Corner2D::with_observation_count(
+                Point2D::new(self.xs[index], self.ys[index]),
+                self.line1_indices[index],
+                self.line2_indices[index],
+                self.angles[index],
+                self.observation_counts[index],
+            ))
+        } else {
+            None
+        }
+    }
+
+    /// Convert to Vec<Corner2D>.
+    pub fn to_corners(&self) -> Vec<Corner2D> {
+        (0..self.len())
+            .map(|i| {
+                Corner2D::with_observation_count(
+                    Point2D::new(self.xs[i], self.ys[i]),
+                    self.line1_indices[i],
+                    self.line2_indices[i],
+                    self.angles[i],
+                    self.observation_counts[i],
+                )
+            })
+            .collect()
+    }
+
+    /// Find the nearest corner to a point.
+    ///
+    /// Returns (index, distance) of the nearest corner, or None if collection is empty.
+    pub fn nearest_corner(&self, point: Point2D) -> Option<(usize, f32)> {
+        if self.is_empty() {
+            return None;
+        }
+
+        let mut min_idx = 0;
+        let mut min_dist_sq = f32::MAX;
+
+        for i in 0..self.len() {
+            let dx = point.x - self.xs[i];
+            let dy = point.y - self.ys[i];
+            let dist_sq = dx * dx + dy * dy;
+
+            if dist_sq < min_dist_sq {
+                min_dist_sq = dist_sq;
+                min_idx = i;
+            }
+        }
+
+        Some((min_idx, min_dist_sq.sqrt()))
+    }
+
+    /// Clear all corners.
+    pub fn clear(&mut self) {
+        self.xs.clear();
+        self.ys.clear();
+        self.line1_indices.clear();
+        self.line2_indices.clear();
+        self.angles.clear();
+        self.observation_counts.clear();
+    }
+
+    /// Iterate over corners.
+    pub fn iter(&self) -> impl Iterator<Item = Corner2D> + '_ {
+        (0..self.len()).map(move |i| {
+            Corner2D::with_observation_count(
+                Point2D::new(self.xs[i], self.ys[i]),
+                self.line1_indices[i],
+                self.line2_indices[i],
+                self.angles[i],
+                self.observation_counts[i],
+            )
+        })
+    }
+}
+
+#[cfg(test)]
+mod tests {
+    use super::*;
+    use approx::assert_relative_eq;
+    use std::f32::consts::{FRAC_PI_2, FRAC_PI_4};
+
+    #[test]
+    fn test_corner_new() {
+        let corner = Corner2D::new(Point2D::new(1.0, 2.0), 0, 1, FRAC_PI_2);
+
+        assert_eq!(corner.position, Point2D::new(1.0, 2.0));
+        assert_eq!(corner.line1_idx, 0);
+        assert_eq!(corner.line2_idx, 1);
+        assert_relative_eq!(corner.angle, FRAC_PI_2, epsilon = 1e-6);
+        assert_eq!(corner.observation_count, 1);
+    }
+
+    #[test]
+    fn test_corner_transform() {
+        let corner = Corner2D::new(Point2D::new(1.0, 0.0), 0, 1, FRAC_PI_2);
+        let pose = Pose2D::new(0.0, 0.0, FRAC_PI_2);
+
+        let transformed = corner.transform(&pose);
+
+        assert_relative_eq!(transformed.position.x, 0.0, epsilon = 1e-6);
+        assert_relative_eq!(transformed.position.y, 1.0, epsilon = 1e-6);
+        // Angle stays the same (corner angle, not orientation)
+        assert_relative_eq!(transformed.angle, FRAC_PI_2, epsilon = 1e-6);
+    }
+
+    #[test]
+    fn test_corner_distance() {
+        let c1 = Corner2D::new(Point2D::new(0.0, 0.0), 0, 1, FRAC_PI_2);
+        let c2 = Corner2D::new(Point2D::new(3.0, 4.0), 2, 3, FRAC_PI_2);
+
+        assert_eq!(c1.distance(&c2), 5.0);
+    }
+
+    #[test]
+    fn test_corner_is_valid_angle() {
+        let corner = Corner2D::new(Point2D::new(0.0, 0.0), 0, 1, FRAC_PI_2);
+
+        assert!(corner.is_valid_angle(FRAC_PI_4, std::f32::consts::PI - FRAC_PI_4));
+        assert!(!corner.is_valid_angle(FRAC_PI_2 + 0.1, std::f32::consts::PI));
+    }
+
+    #[test]
+    fn test_corner_collection_from_corners() {
+        let corners = vec![
+            Corner2D::new(Point2D::new(0.0, 0.0), 0, 1, FRAC_PI_2),
+            Corner2D::new(Point2D::new(1.0, 1.0), 1, 2, FRAC_PI_4),
+        ];
+
+        let collection = CornerCollection::from_corners(&corners);
+
+        assert_eq!(collection.len(), 2);
+        assert_eq!(collection.xs, vec![0.0, 1.0]);
+        assert_eq!(collection.ys, vec![0.0, 1.0]);
+    }
+
+    #[test]
+    fn test_corner_collection_to_corners() {
+        let corners = vec![
+            Corner2D::new(Point2D::new(0.0, 0.0), 0, 1, FRAC_PI_2),
+            Corner2D::new(Point2D::new(1.0, 1.0), 1, 2, FRAC_PI_4),
+        ];
+
+        let collection = CornerCollection::from_corners(&corners);
+        let recovered = collection.to_corners();
+
+        assert_eq!(recovered.len(), 2);
+        assert_eq!(recovered[0].position, corners[0].position);
+        assert_eq!(recovered[1].position, corners[1].position);
+    }
+
+    #[test]
+    fn test_corner_collection_nearest() {
+        let corners = vec![
+            Corner2D::new(Point2D::new(0.0, 0.0), 0, 1, FRAC_PI_2),
+            Corner2D::new(Point2D::new(10.0, 0.0), 1, 2, FRAC_PI_2),
+            Corner2D::new(Point2D::new(5.0, 5.0), 2, 3, FRAC_PI_2),
+        ];
+
+        let collection = CornerCollection::from_corners(&corners);
+        let point = Point2D::new(4.0, 4.0);
+
+        let (idx, dist) = collection.nearest_corner(point).unwrap();
+
+        assert_eq!(idx, 2); // (5, 5) is nearest
+        assert_relative_eq!(dist, 2.0_f32.sqrt(), epsilon = 1e-6);
+    }
+
+    #[test]
+    fn test_corner_collection_iter() {
+        let corners = vec![
+            Corner2D::new(Point2D::new(0.0, 0.0), 0, 1, FRAC_PI_2),
+            Corner2D::new(Point2D::new(1.0, 1.0), 1, 2, FRAC_PI_4),
+        ];
+
+        let collection = CornerCollection::from_corners(&corners);
+        let collected: Vec<_> = collection.iter().collect();
+
+        assert_eq!(collected.len(), 2);
+        assert_eq!(collected[0].position, Point2D::new(0.0, 0.0));
+        assert_eq!(collected[1].position, Point2D::new(1.0, 1.0));
+    }
+}

vastu-map/src/features/feature_set.rs

@@ -0,0 +1,386 @@
+//! Feature set container holding lines and corners.
+//!
+//! A FeatureSet represents all features extracted from a single scan
+//! or the accumulated features in a map.
+
+use super::corner::{Corner2D, CornerCollection};
+use super::line::Line2D;
+use super::line_collection::LineCollection;
+use crate::core::{Bounds, Point2D, Pose2D};
+
+/// A set of geometric features (lines and corners).
+///
+/// Used to represent:
+/// - Features extracted from a single lidar scan
+/// - Accumulated features in a map
+///
+/// Provides both AoS (Array of Structs) and SoA (Struct of Arrays) access
+/// for flexibility between ease of use and SIMD performance.
+#[derive(Clone, Debug, Default)]
+pub struct FeatureSet {
+    /// Lines stored in SoA layout for SIMD operations.
+    lines: LineCollection,
+    /// Corners stored in SoA layout.
+    corners: CornerCollection,
+}
+
+impl FeatureSet {
+    /// Create a new empty feature set.
+    pub fn new() -> Self {
+        Self::default()
+    }
+
+    /// Create a feature set with capacity hints.
+    pub fn with_capacity(line_capacity: usize, corner_capacity: usize) -> Self {
+        Self {
+            lines: LineCollection::with_capacity(line_capacity),
+            corners: CornerCollection::with_capacity(corner_capacity),
+        }
+    }
+
+    /// Create from lines and corners.
+    pub fn from_features(lines: &[Line2D], corners: &[Corner2D]) -> Self {
+        Self {
+            lines: LineCollection::from_lines(lines),
+            corners: CornerCollection::from_corners(corners),
+        }
+    }
+
+    // ─────────────────────────────────────────────────────────────────────────
+    // Line operations
+    // ─────────────────────────────────────────────────────────────────────────
+
+    /// Add a line to the feature set.
+    #[inline]
+    pub fn add_line(&mut self, line: Line2D) {
+        self.lines.push_line(&line);
+    }
+
+    /// 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);
+    }
+
+    /// Number of lines.
+    #[inline]
+    pub fn line_count(&self) -> usize {
+        self.lines.len()
+    }
+
+    /// Get a line by index.
+    #[inline]
+    pub fn get_line(&self, index: usize) -> Option<Line2D> {
+        self.lines.get(index)
+    }
+
+    /// Get all lines as a vector.
+    pub fn lines(&self) -> Vec<Line2D> {
+        self.lines.to_lines()
+    }
+
+    /// Get reference to the line collection (SoA layout).
+    #[inline]
+    pub fn line_collection(&self) -> &LineCollection {
+        &self.lines
+    }
+
+    /// Get mutable reference to the line collection.
+    #[inline]
+    pub fn line_collection_mut(&mut self) -> &mut LineCollection {
+        &mut self.lines
+    }
+
+    /// Iterate over lines.
+    pub fn iter_lines(&self) -> impl Iterator<Item = Line2D> + '_ {
+        self.lines.iter()
+    }
+
+    /// Remove a line by index (swap-remove).
+    pub fn remove_line(&mut self, index: usize) {
+        self.lines.swap_remove(index);
+        // Note: This invalidates corner line indices!
+        // Caller should update or clear corners after removing lines.
+    }
+
+    // ─────────────────────────────────────────────────────────────────────────
+    // Corner operations
+    // ─────────────────────────────────────────────────────────────────────────
+
+    /// Add a corner to the feature set.
+    #[inline]
+    pub fn add_corner(&mut self, corner: Corner2D) {
+        self.corners.push_corner(&corner);
+    }
+
+    /// Number of corners.
+    #[inline]
+    pub fn corner_count(&self) -> usize {
+        self.corners.len()
+    }
+
+    /// Get a corner by index.
+    #[inline]
+    pub fn get_corner(&self, index: usize) -> Option<Corner2D> {
+        self.corners.get(index)
+    }
+
+    /// Get all corners as a vector.
+    pub fn corners(&self) -> Vec<Corner2D> {
+        self.corners.to_corners()
+    }
+
+    /// Get reference to the corner collection.
+    #[inline]
+    pub fn corner_collection(&self) -> &CornerCollection {
+        &self.corners
+    }
+
+    /// Get mutable reference to the corner collection.
+    #[inline]
+    pub fn corner_collection_mut(&mut self) -> &mut CornerCollection {
+        &mut self.corners
+    }
+
+    /// Iterate over corners.
+    pub fn iter_corners(&self) -> impl Iterator<Item = Corner2D> + '_ {
+        self.corners.iter()
+    }
+
+    // ─────────────────────────────────────────────────────────────────────────
+    // Combined operations
+    // ─────────────────────────────────────────────────────────────────────────
+
+    /// Total number of features (lines + corners).
+    #[inline]
+    pub fn total_count(&self) -> usize {
+        self.line_count() + self.corner_count()
+    }
+
+    /// Check if the feature set is empty.
+    #[inline]
+    pub fn is_empty(&self) -> bool {
+        self.lines.is_empty() && self.corners.is_empty()
+    }
+
+    /// Transform all features by a pose.
+    pub fn transform(&self, pose: &Pose2D) -> Self {
+        Self {
+            lines: self.lines.transform(pose),
+            corners: {
+                let mut transformed = CornerCollection::with_capacity(self.corners.len());
+                for corner in self.corners.iter() {
+                    transformed.push_corner(&corner.transform(pose));
+                }
+                transformed
+            },
+        }
+    }
+
+    /// Compute bounding box of all features.
+    pub fn bounds(&self) -> Bounds {
+        let mut bounds = Bounds::empty();
+
+        // Include all line endpoints
+        for i in 0..self.lines.len() {
+            bounds.expand_to_include(Point2D::new(self.lines.start_xs[i], self.lines.start_ys[i]));
+            bounds.expand_to_include(Point2D::new(self.lines.end_xs[i], self.lines.end_ys[i]));
+        }
+
+        // Include all corner positions
+        for i in 0..self.corners.len() {
+            bounds.expand_to_include(Point2D::new(self.corners.xs[i], self.corners.ys[i]));
+        }
+
+        bounds
+    }
+
+    /// Clear all features.
+    pub fn clear(&mut self) {
+        self.lines.clear();
+        self.corners.clear();
+    }
+
+    /// Clear only corners (useful after modifying lines).
+    pub fn clear_corners(&mut self) {
+        self.corners.clear();
+    }
+
+    /// Find the nearest line to a point.
+    ///
+    /// Returns (index, distance) or None if no lines.
+    #[inline]
+    pub fn nearest_line(&self, point: Point2D) -> Option<(usize, f32)> {
+        self.lines.nearest_line(point)
+    }
+
+    /// Find the nearest corner to a point.
+    ///
+    /// Returns (index, distance) or None if no corners.
+    #[inline]
+    pub fn nearest_corner(&self, point: Point2D) -> Option<(usize, f32)> {
+        self.corners.nearest_corner(point)
+    }
+
+    /// Compute distances from a point to all lines.
+    #[inline]
+    pub fn distances_to_lines(&self, point: Point2D) -> Vec<f32> {
+        self.lines.distances_to_point(point)
+    }
+
+    /// Merge another feature set into this one.
+    ///
+    /// Note: This is a simple append. For intelligent merging with
+    /// association and deduplication, use the integration module.
+    pub fn append(&mut self, other: &FeatureSet) {
+        for line in other.lines.iter() {
+            self.add_line(line);
+        }
+        for corner in other.corners.iter() {
+            self.add_corner(corner);
+        }
+    }
+}
+
+#[cfg(test)]
+mod tests {
+    use super::*;
+    use approx::assert_relative_eq;
+    use std::f32::consts::FRAC_PI_2;
+
+    #[test]
+    fn test_new_empty() {
+        let fs = FeatureSet::new();
+        assert!(fs.is_empty());
+        assert_eq!(fs.line_count(), 0);
+        assert_eq!(fs.corner_count(), 0);
+        assert_eq!(fs.total_count(), 0);
+    }
+
+    #[test]
+    fn test_add_line() {
+        let mut fs = FeatureSet::new();
+        fs.add_line(Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(1.0, 0.0)));
+        fs.add_line_from_points(Point2D::new(0.0, 1.0), Point2D::new(1.0, 1.0));
+
+        assert_eq!(fs.line_count(), 2);
+        assert!(!fs.is_empty());
+
+        let line = fs.get_line(0).unwrap();
+        assert_eq!(line.start, Point2D::new(0.0, 0.0));
+    }
+
+    #[test]
+    fn test_add_corner() {
+        let mut fs = FeatureSet::new();
+        fs.add_corner(Corner2D::new(Point2D::new(5.0, 5.0), 0, 1, FRAC_PI_2));
+
+        assert_eq!(fs.corner_count(), 1);
+
+        let corner = fs.get_corner(0).unwrap();
+        assert_eq!(corner.position, Point2D::new(5.0, 5.0));
+    }
+
+    #[test]
+    fn test_from_features() {
+        let lines = vec![
+            Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(1.0, 0.0)),
+            Line2D::new(Point2D::new(0.0, 1.0), Point2D::new(1.0, 1.0)),
+        ];
+        let corners = vec![Corner2D::new(Point2D::new(0.0, 0.0), 0, 1, FRAC_PI_2)];
+
+        let fs = FeatureSet::from_features(&lines, &corners);
+
+        assert_eq!(fs.line_count(), 2);
+        assert_eq!(fs.corner_count(), 1);
+        assert_eq!(fs.total_count(), 3);
+    }
+
+    #[test]
+    fn test_transform() {
+        let mut fs = FeatureSet::new();
+        fs.add_line(Line2D::new(Point2D::new(1.0, 0.0), Point2D::new(2.0, 0.0)));
+        fs.add_corner(Corner2D::new(Point2D::new(1.0, 0.0), 0, 0, FRAC_PI_2));
+
+        let pose = Pose2D::new(0.0, 0.0, FRAC_PI_2);
+        let transformed = fs.transform(&pose);
+
+        let line = transformed.get_line(0).unwrap();
+        assert_relative_eq!(line.start.x, 0.0, epsilon = 1e-6);
+        assert_relative_eq!(line.start.y, 1.0, epsilon = 1e-6);
+
+        let corner = transformed.get_corner(0).unwrap();
+        assert_relative_eq!(corner.position.x, 0.0, epsilon = 1e-6);
+        assert_relative_eq!(corner.position.y, 1.0, epsilon = 1e-6);
+    }
+
+    #[test]
+    fn test_bounds() {
+        let mut fs = FeatureSet::new();
+        fs.add_line(Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(10.0, 0.0)));
+        fs.add_line(Line2D::new(Point2D::new(5.0, -5.0), Point2D::new(5.0, 5.0)));
+
+        let bounds = fs.bounds();
+
+        assert_eq!(bounds.min, Point2D::new(0.0, -5.0));
+        assert_eq!(bounds.max, Point2D::new(10.0, 5.0));
+    }
+
+    #[test]
+    fn test_bounds_empty() {
+        let fs = FeatureSet::new();
+        let bounds = fs.bounds();
+        assert!(bounds.is_empty());
+    }
+
+    #[test]
+    fn test_nearest_line() {
+        let mut fs = FeatureSet::new();
+        fs.add_line(Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(10.0, 0.0)));
+        fs.add_line(Line2D::new(Point2D::new(0.0, 5.0), Point2D::new(10.0, 5.0)));
+
+        let point = Point2D::new(5.0, 2.0);
+        let (idx, dist) = fs.nearest_line(point).unwrap();
+
+        assert_eq!(idx, 0);
+        assert_relative_eq!(dist, 2.0, epsilon = 1e-6);
+    }
+
+    #[test]
+    fn test_iter_lines() {
+        let mut fs = FeatureSet::new();
+        fs.add_line(Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(1.0, 0.0)));
+        fs.add_line(Line2D::new(Point2D::new(0.0, 1.0), Point2D::new(1.0, 1.0)));
+
+        let lines: Vec<_> = fs.iter_lines().collect();
+        assert_eq!(lines.len(), 2);
+    }
+
+    #[test]
+    fn test_append() {
+        let mut fs1 = FeatureSet::new();
+        fs1.add_line(Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(1.0, 0.0)));
+
+        let mut fs2 = FeatureSet::new();
+        fs2.add_line(Line2D::new(Point2D::new(0.0, 1.0), Point2D::new(1.0, 1.0)));
+        fs2.add_corner(Corner2D::new(Point2D::new(0.0, 0.0), 0, 0, FRAC_PI_2));
+
+        fs1.append(&fs2);
+
+        assert_eq!(fs1.line_count(), 2);
+        assert_eq!(fs1.corner_count(), 1);
+    }
+
+    #[test]
+    fn test_clear() {
+        let mut fs = FeatureSet::new();
+        fs.add_line(Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(1.0, 0.0)));
+        fs.add_corner(Corner2D::new(Point2D::new(0.0, 0.0), 0, 0, FRAC_PI_2));
+
+        fs.clear();
+
+        assert!(fs.is_empty());
+        assert_eq!(fs.line_count(), 0);
+        assert_eq!(fs.corner_count(), 0);
+    }
+}

vastu-map/src/features/line.rs

@@ -0,0 +1,517 @@
+//! Line segment type using endpoint representation.
+//!
+//! Lines are represented by their start and end points only (no parametric form).
+//! This design choice simplifies transforms and enables better SIMD compatibility.
+
+use crate::core::{Point2D, Pose2D};
+
+/// A 2D line segment defined by its endpoints.
+///
+/// # Design Decision: Endpoint-Only Representation
+///
+/// We use endpoints instead of parametric (ρ,θ) form because:
+/// - **Simpler transforms**: Just rotate+translate two points
+/// - **Better SIMD layout**: 4 homogeneous floats per line
+/// - **No angle wrapping**: No discontinuity at ±π
+/// - **Built-in bounds**: Segment extent is implicit
+///
+/// # Point-to-Line Distance
+///
+/// Uses cross product formula (no trig functions):
+/// ```text
+/// distance = |cross(p - start, direction)| / |direction|
+/// ```
+#[derive(Clone, Copy, Debug, PartialEq)]
+pub struct Line2D {
+    /// Start point of the line segment.
+    pub start: Point2D,
+    /// End point of the line segment.
+    pub end: Point2D,
+    /// Number of times this line has been observed (for merging).
+    pub observation_count: u32,
+}
+
+impl Line2D {
+    /// Create a new line segment from two points.
+    #[inline]
+    pub fn new(start: Point2D, end: Point2D) -> Self {
+        Self {
+            start,
+            end,
+            observation_count: 1,
+        }
+    }
+
+    /// Create a line segment with a specific observation count.
+    #[inline]
+    pub fn with_observation_count(start: Point2D, end: Point2D, count: u32) -> Self {
+        Self {
+            start,
+            end,
+            observation_count: count,
+        }
+    }
+
+    /// Direction vector from start to end (not normalized).
+    #[inline]
+    pub fn direction(&self) -> Point2D {
+        self.end - self.start
+    }
+
+    /// Unit direction vector from start to end.
+    #[inline]
+    pub fn unit_direction(&self) -> Point2D {
+        self.direction().normalized()
+    }
+
+    /// Normal vector (perpendicular to direction, pointing left).
+    ///
+    /// If the line goes from left to right, the normal points up.
+    #[inline]
+    pub fn normal(&self) -> Point2D {
+        self.unit_direction().perpendicular()
+    }
+
+    /// Length of the line segment.
+    #[inline]
+    pub fn length(&self) -> f32 {
+        self.direction().length()
+    }
+
+    /// Squared length of the line segment (avoids sqrt).
+    #[inline]
+    pub fn length_squared(&self) -> f32 {
+        self.direction().length_squared()
+    }
+
+    /// Midpoint of the line segment.
+    #[inline]
+    pub fn midpoint(&self) -> Point2D {
+        Point2D::new(
+            (self.start.x + self.end.x) * 0.5,
+            (self.start.y + self.end.y) * 0.5,
+        )
+    }
+
+    /// Angle of the line direction from X-axis (in radians, [-π, π]).
+    #[inline]
+    pub fn angle(&self) -> f32 {
+        self.direction().angle()
+    }
+
+    /// Perpendicular distance from a point to the infinite line.
+    ///
+    /// Uses the cross product formula:
+    /// ```text
+    /// distance = |cross(p - start, end - start)| / |end - start|
+    /// ```
+    ///
+    /// This is always non-negative.
+    #[inline]
+    pub fn distance_to_point(&self, point: Point2D) -> f32 {
+        let dir = self.direction();
+        let len_sq = dir.length_squared();
+
+        if len_sq < f32::EPSILON {
+            // Degenerate line (zero length)
+            return point.distance(self.start);
+        }
+
+        let to_point = point - self.start;
+        (to_point.cross(dir)).abs() / len_sq.sqrt()
+    }
+
+    /// Signed perpendicular distance from a point to the infinite line.
+    ///
+    /// Positive if the point is to the left of the line (in direction of normal),
+    /// negative if to the right.
+    #[inline]
+    pub fn signed_distance_to_point(&self, point: Point2D) -> f32 {
+        let dir = self.direction();
+        let len_sq = dir.length_squared();
+
+        if len_sq < f32::EPSILON {
+            return point.distance(self.start);
+        }
+
+        let to_point = point - self.start;
+        // Cross product: dir × to_point gives positive when point is to the left
+        dir.cross(to_point) / len_sq.sqrt()
+    }
+
+    /// Project a point onto the infinite line, returning parameter t.
+    ///
+    /// The projection point is: `start + t * (end - start)`
+    /// - `t = 0`: projection is at start
+    /// - `t = 1`: projection is at end
+    /// - `0 < t < 1`: projection is between endpoints
+    /// - `t < 0` or `t > 1`: projection is outside segment
+    #[inline]
+    pub fn project_point(&self, point: Point2D) -> f32 {
+        let dir = self.direction();
+        let len_sq = dir.length_squared();
+
+        if len_sq < f32::EPSILON {
+            return 0.0;
+        }
+
+        let to_point = point - self.start;
+        to_point.dot(dir) / len_sq
+    }
+
+    /// Get the projected point on the infinite line.
+    #[inline]
+    pub fn project_point_onto_line(&self, point: Point2D) -> Point2D {
+        let t = self.project_point(point);
+        self.point_at(t)
+    }
+
+    /// Check if a projection parameter t falls within the segment [0, 1].
+    #[inline]
+    pub fn contains_projection(&self, t: f32) -> bool {
+        (0.0..=1.0).contains(&t)
+    }
+
+    /// Get the point on the line at parameter t.
+    ///
+    /// `t = 0` gives start, `t = 1` gives end.
+    #[inline]
+    pub fn point_at(&self, t: f32) -> Point2D {
+        self.start.lerp(self.end, t)
+    }
+
+    /// Distance from a point to the line segment (not infinite line).
+    ///
+    /// Returns the minimum distance to any point on the segment.
+    #[inline]
+    pub fn distance_to_point_segment(&self, point: Point2D) -> f32 {
+        let t = self.project_point(point).clamp(0.0, 1.0);
+        point.distance(self.point_at(t))
+    }
+
+    /// Transform line by a pose (rotate and translate both endpoints).
+    #[inline]
+    pub fn transform(&self, pose: &Pose2D) -> Self {
+        Self {
+            start: pose.transform_point(self.start),
+            end: pose.transform_point(self.end),
+            observation_count: self.observation_count,
+        }
+    }
+
+    /// Compute the overlap between two line segments.
+    ///
+    /// Returns the length of the overlap when projected onto this line.
+    /// Lines must be roughly parallel for meaningful overlap.
+    ///
+    /// Returns 0 if lines don't overlap or aren't parallel.
+    pub fn overlap_length(&self, other: &Line2D, angle_tolerance: f32) -> f32 {
+        use crate::core::math::angle_diff;
+
+        // Check if lines are roughly parallel
+        let angle_diff = angle_diff(self.angle(), other.angle()).abs();
+        if angle_diff > angle_tolerance && angle_diff < std::f32::consts::PI - angle_tolerance {
+            return 0.0;
+        }
+
+        // Project other line's endpoints onto this line
+        let t1 = self.project_point(other.start);
+        let t2 = self.project_point(other.end);
+
+        // Find overlap in parameter space
+        let (t_min, t_max) = if t1 < t2 { (t1, t2) } else { (t2, t1) };
+
+        // Clamp to [0, 1] (this line's extent)
+        let overlap_start = t_min.max(0.0);
+        let overlap_end = t_max.min(1.0);
+
+        if overlap_end <= overlap_start {
+            return 0.0;
+        }
+
+        (overlap_end - overlap_start) * self.length()
+    }
+
+    /// Check if a ray intersects this line segment.
+    ///
+    /// Returns the distance along the ray to the intersection point,
+    /// or None if no intersection.
+    ///
+    /// # Arguments
+    /// * `ray_origin` - Origin of the ray
+    /// * `ray_direction` - Direction of the ray (will be normalized internally)
+    pub fn ray_intersection(&self, ray_origin: Point2D, ray_direction: Point2D) -> Option<f32> {
+        let dir = ray_direction.normalized();
+        let seg = self.direction();
+
+        // Use parametric form: ray_origin + t*dir = start + s*seg
+        // Solve using cross products
+        let cross_dir_seg = dir.cross(seg);
+
+        if cross_dir_seg.abs() < f32::EPSILON {
+            // Parallel lines
+            return None;
+        }
+
+        let origin_to_start = self.start - ray_origin;
+        let t = origin_to_start.cross(seg) / cross_dir_seg;
+        let s = origin_to_start.cross(dir) / cross_dir_seg;
+
+        // t must be positive (forward along ray)
+        // s must be in [0, 1] (within segment)
+        if t >= 0.0 && (0.0..=1.0).contains(&s) {
+            Some(t)
+        } else {
+            None
+        }
+    }
+
+    /// Find intersection point with another line segment.
+    ///
+    /// Returns the intersection point if the segments intersect,
+    /// None otherwise.
+    pub fn intersection(&self, other: &Line2D) -> Option<Point2D> {
+        let d1 = self.direction();
+        let d2 = other.direction();
+
+        let cross = d1.cross(d2);
+        if cross.abs() < f32::EPSILON {
+            // Parallel lines
+            return None;
+        }
+
+        let origin_diff = other.start - self.start;
+        let t = origin_diff.cross(d2) / cross;
+        let s = origin_diff.cross(d1) / cross;
+
+        if (0.0..=1.0).contains(&t) && (0.0..=1.0).contains(&s) {
+            Some(self.point_at(t))
+        } else {
+            None
+        }
+    }
+
+    /// Check if this line approximately equals another.
+    #[inline]
+    pub fn approx_eq(&self, other: &Line2D, epsilon: f32) -> bool {
+        self.start.approx_eq(other.start, epsilon) && self.end.approx_eq(other.end, epsilon)
+    }
+}
+
+impl Default for Line2D {
+    fn default() -> Self {
+        Self {
+            start: Point2D::zero(),
+            end: Point2D::zero(),
+            observation_count: 0,
+        }
+    }
+}
+
+#[cfg(test)]
+mod tests {
+    use super::*;
+    use approx::assert_relative_eq;
+    use std::f32::consts::{FRAC_PI_2, FRAC_PI_4};
+
+    #[test]
+    fn test_new() {
+        let line = Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(1.0, 0.0));
+        assert_eq!(line.start, Point2D::new(0.0, 0.0));
+        assert_eq!(line.end, Point2D::new(1.0, 0.0));
+        assert_eq!(line.observation_count, 1);
+    }
+
+    #[test]
+    fn test_direction_and_length() {
+        let line = Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(3.0, 4.0));
+
+        assert_eq!(line.direction(), Point2D::new(3.0, 4.0));
+        assert_eq!(line.length(), 5.0);
+        assert_eq!(line.length_squared(), 25.0);
+
+        let unit = line.unit_direction();
+        assert_relative_eq!(unit.x, 0.6, epsilon = 1e-6);
+        assert_relative_eq!(unit.y, 0.8, epsilon = 1e-6);
+    }
+
+    #[test]
+    fn test_midpoint() {
+        let line = Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(4.0, 6.0));
+        let mid = line.midpoint();
+        assert_eq!(mid.x, 2.0);
+        assert_eq!(mid.y, 3.0);
+    }
+
+    #[test]
+    fn test_angle() {
+        let horizontal = Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(1.0, 0.0));
+        assert_relative_eq!(horizontal.angle(), 0.0, epsilon = 1e-6);
+
+        let vertical = Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(0.0, 1.0));
+        assert_relative_eq!(vertical.angle(), FRAC_PI_2, epsilon = 1e-6);
+
+        let diagonal = Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(1.0, 1.0));
+        assert_relative_eq!(diagonal.angle(), FRAC_PI_4, epsilon = 1e-6);
+    }
+
+    #[test]
+    fn test_distance_to_point() {
+        let line = Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(10.0, 0.0));
+
+        // Point above the line
+        let dist = line.distance_to_point(Point2D::new(5.0, 3.0));
+        assert_relative_eq!(dist, 3.0, epsilon = 1e-6);
+
+        // Point below the line
+        let dist = line.distance_to_point(Point2D::new(5.0, -4.0));
+        assert_relative_eq!(dist, 4.0, epsilon = 1e-6);
+
+        // Point on the line
+        let dist = line.distance_to_point(Point2D::new(5.0, 0.0));
+        assert_relative_eq!(dist, 0.0, epsilon = 1e-6);
+    }
+
+    #[test]
+    fn test_signed_distance() {
+        let line = Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(10.0, 0.0));
+
+        // Point above (left of line direction) - positive
+        let dist = line.signed_distance_to_point(Point2D::new(5.0, 3.0));
+        assert_relative_eq!(dist, 3.0, epsilon = 1e-6);
+
+        // Point below (right of line direction) - negative
+        let dist = line.signed_distance_to_point(Point2D::new(5.0, -3.0));
+        assert_relative_eq!(dist, -3.0, epsilon = 1e-6);
+    }
+
+    #[test]
+    fn test_project_point() {
+        let line = Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(10.0, 0.0));
+
+        // Project point to midpoint
+        let t = line.project_point(Point2D::new(5.0, 3.0));
+        assert_relative_eq!(t, 0.5, epsilon = 1e-6);
+
+        // Project point at start
+        let t = line.project_point(Point2D::new(0.0, 5.0));
+        assert_relative_eq!(t, 0.0, epsilon = 1e-6);
+
+        // Project point beyond end
+        let t = line.project_point(Point2D::new(15.0, 0.0));
+        assert_relative_eq!(t, 1.5, epsilon = 1e-6);
+
+        // Project point before start
+        let t = line.project_point(Point2D::new(-5.0, 0.0));
+        assert_relative_eq!(t, -0.5, epsilon = 1e-6);
+    }
+
+    #[test]
+    fn test_contains_projection() {
+        let line = Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(10.0, 0.0));
+
+        assert!(line.contains_projection(0.0));
+        assert!(line.contains_projection(0.5));
+        assert!(line.contains_projection(1.0));
+        assert!(!line.contains_projection(-0.1));
+        assert!(!line.contains_projection(1.1));
+    }
+
+    #[test]
+    fn test_point_at() {
+        let line = Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(10.0, 0.0));
+
+        assert_eq!(line.point_at(0.0), line.start);
+        assert_eq!(line.point_at(1.0), line.end);
+        assert_eq!(line.point_at(0.5), Point2D::new(5.0, 0.0));
+    }
+
+    #[test]
+    fn test_distance_to_point_segment() {
+        let line = Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(10.0, 0.0));
+
+        // Point above segment (closest to segment)
+        let dist = line.distance_to_point_segment(Point2D::new(5.0, 3.0));
+        assert_relative_eq!(dist, 3.0, epsilon = 1e-6);
+
+        // Point beyond end (closest to endpoint)
+        let dist = line.distance_to_point_segment(Point2D::new(13.0, 4.0));
+        assert_relative_eq!(dist, 5.0, epsilon = 1e-6); // Distance to (10, 0)
+    }
+
+    #[test]
+    fn test_transform() {
+        let line = Line2D::new(Point2D::new(1.0, 0.0), Point2D::new(2.0, 0.0));
+
+        // Rotate 90 degrees at origin
+        let pose = Pose2D::new(0.0, 0.0, FRAC_PI_2);
+        let transformed = line.transform(&pose);
+
+        assert_relative_eq!(transformed.start.x, 0.0, epsilon = 1e-6);
+        assert_relative_eq!(transformed.start.y, 1.0, epsilon = 1e-6);
+        assert_relative_eq!(transformed.end.x, 0.0, epsilon = 1e-6);
+        assert_relative_eq!(transformed.end.y, 2.0, epsilon = 1e-6);
+    }
+
+    #[test]
+    fn test_ray_intersection() {
+        let line = Line2D::new(Point2D::new(5.0, -5.0), Point2D::new(5.0, 5.0));
+
+        // Ray from origin pointing right
+        let dist = line.ray_intersection(Point2D::new(0.0, 0.0), Point2D::new(1.0, 0.0));
+        assert_relative_eq!(dist.unwrap(), 5.0, epsilon = 1e-6);
+
+        // Ray pointing away from line
+        let dist = line.ray_intersection(Point2D::new(0.0, 0.0), Point2D::new(-1.0, 0.0));
+        assert!(dist.is_none());
+
+        // Ray parallel to line
+        let dist = line.ray_intersection(Point2D::new(0.0, 0.0), Point2D::new(0.0, 1.0));
+        assert!(dist.is_none());
+    }
+
+    #[test]
+    fn test_segment_intersection() {
+        let line1 = Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(10.0, 0.0));
+        let line2 = Line2D::new(Point2D::new(5.0, -5.0), Point2D::new(5.0, 5.0));
+
+        let intersection = line1.intersection(&line2);
+        assert!(intersection.is_some());
+        let p = intersection.unwrap();
+        assert_relative_eq!(p.x, 5.0, epsilon = 1e-6);
+        assert_relative_eq!(p.y, 0.0, epsilon = 1e-6);
+
+        // Non-intersecting segments
+        let line3 = Line2D::new(Point2D::new(20.0, 0.0), Point2D::new(30.0, 0.0));
+        assert!(line1.intersection(&line3).is_none());
+    }
+
+    #[test]
+    fn test_overlap_length() {
+        let line1 = Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(10.0, 0.0));
+        let line2 = Line2D::new(Point2D::new(5.0, 0.1), Point2D::new(15.0, 0.1)); // Slightly offset, parallel
+
+        let overlap = line1.overlap_length(&line2, 0.1);
+        assert_relative_eq!(overlap, 5.0, epsilon = 0.1);
+
+        // Non-overlapping
+        let line3 = Line2D::new(Point2D::new(15.0, 0.0), Point2D::new(25.0, 0.0));
+        let overlap = line1.overlap_length(&line3, 0.1);
+        assert_eq!(overlap, 0.0);
+
+        // Perpendicular lines (not parallel)
+        let line4 = Line2D::new(Point2D::new(5.0, -5.0), Point2D::new(5.0, 5.0));
+        let overlap = line1.overlap_length(&line4, 0.1);
+        assert_eq!(overlap, 0.0);
+    }
+
+    #[test]
+    fn test_normal() {
+        let line = Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(1.0, 0.0));
+        let normal = line.normal();
+
+        // For a line pointing right, normal points up
+        assert_relative_eq!(normal.x, 0.0, epsilon = 1e-6);
+        assert_relative_eq!(normal.y, 1.0, epsilon = 1e-6);
+    }
+}

vastu-map/src/features/line_collection.rs

@@ -0,0 +1,945 @@
+//! Line collection with SoA (Struct of Arrays) layout for SIMD operations.
+//!
+//! This layout enables efficient batch operations on ARM NEON:
+//! - 4 floats per line = clean SIMD alignment
+//! - Batch distance computations
+//! - Parallel transforms
+//!
+//! # Performance Optimizations
+//!
+//! Pre-computed cached values avoid redundant calculations in hot paths:
+//! - `normal_xs`, `normal_ys`: Unit normal vectors (avoids sqrt per distance calc)
+//! - `inv_lengths`: Reciprocal of line lengths (avoids division in inner loops)
+
+use super::line::Line2D;
+use crate::core::{Point2D, Pose2D};
+use crate::simd::Float4;
+
+/// Collection of line segments with SoA layout for SIMD operations.
+///
+/// Each line is represented by 4 floats: (start_x, start_y, end_x, end_y).
+/// This layout enables efficient batch operations using 4-wide SIMD.
+///
+/// # Pre-computed Values
+///
+/// For performance, the collection caches:
+/// - **Normals**: Unit normal vectors for each line
+/// - **Inverse lengths**: `1.0 / length` to avoid division in inner loops
+///
+/// These are automatically computed when lines are added.
+///
+/// # Example
+/// ```
+/// use vastu_map::features::{Line2D, LineCollection};
+/// use vastu_map::core::Point2D;
+///
+/// let lines = vec![
+///     Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(1.0, 0.0)),
+///     Line2D::new(Point2D::new(0.0, 1.0), Point2D::new(1.0, 1.0)),
+/// ];
+///
+/// let collection = LineCollection::from_lines(&lines);
+/// assert_eq!(collection.len(), 2);
+/// ```
+#[derive(Clone, Debug, Default)]
+pub struct LineCollection {
+    /// X coordinates of line start points.
+    pub start_xs: Vec<f32>,
+    /// Y coordinates of line start points.
+    pub start_ys: Vec<f32>,
+    /// X coordinates of line end points.
+    pub end_xs: Vec<f32>,
+    /// Y coordinates of line end points.
+    pub end_ys: Vec<f32>,
+    /// Observation counts for each line.
+    pub observation_counts: Vec<u32>,
+
+    // === Pre-computed cached values for performance ===
+    /// X components of unit normal vectors.
+    normal_xs: Vec<f32>,
+    /// Y components of unit normal vectors.
+    normal_ys: Vec<f32>,
+    /// Reciprocal of line lengths (1.0 / length).
+    /// Used to avoid division in distance calculations.
+    inv_lengths: Vec<f32>,
+}
+
+impl LineCollection {
+    /// Create a new empty line collection.
+    pub fn new() -> Self {
+        Self::default()
+    }
+
+    /// Create a line collection with capacity.
+    pub fn with_capacity(capacity: usize) -> Self {
+        Self {
+            start_xs: Vec::with_capacity(capacity),
+            start_ys: Vec::with_capacity(capacity),
+            end_xs: Vec::with_capacity(capacity),
+            end_ys: Vec::with_capacity(capacity),
+            observation_counts: Vec::with_capacity(capacity),
+            normal_xs: Vec::with_capacity(capacity),
+            normal_ys: Vec::with_capacity(capacity),
+            inv_lengths: Vec::with_capacity(capacity),
+        }
+    }
+
+    /// Create from a slice of Line2D.
+    pub fn from_lines(lines: &[Line2D]) -> Self {
+        let mut collection = Self::with_capacity(lines.len());
+        for line in lines {
+            collection.push_line(line);
+        }
+        collection
+    }
+
+    /// Compute and cache normal and inverse length for a line.
+    #[inline]
+    fn compute_cached_values(
+        start_x: f32,
+        start_y: f32,
+        end_x: f32,
+        end_y: f32,
+    ) -> (f32, f32, f32) {
+        let dx = end_x - start_x;
+        let dy = end_y - start_y;
+        let len_sq = dx * dx + dy * dy;
+
+        if len_sq < f32::EPSILON {
+            // Degenerate line - use arbitrary normal and large inv_length
+            (0.0, 1.0, 0.0)
+        } else {
+            let len = len_sq.sqrt();
+            let inv_len = 1.0 / len;
+            // Normal is perpendicular to direction: (-dy, dx) normalized
+            let nx = -dy * inv_len;
+            let ny = dx * inv_len;
+            (nx, ny, inv_len)
+        }
+    }
+
+    /// 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) {
+        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.normal_xs.push(nx);
+        self.normal_ys.push(ny);
+        self.inv_lengths.push(inv_len);
+    }
+
+    /// Add a Line2D to the collection.
+    #[inline]
+    pub fn push_line(&mut self, line: &Line2D) {
+        self.push(
+            line.start.x,
+            line.start.y,
+            line.end.x,
+            line.end.y,
+            line.observation_count,
+        );
+    }
+
+    /// Get the pre-computed normal vector for a line.
+    #[inline]
+    pub fn normal(&self, index: usize) -> Option<Point2D> {
+        if index < self.len() {
+            Some(Point2D::new(self.normal_xs[index], self.normal_ys[index]))
+        } else {
+            None
+        }
+    }
+
+    /// Get the pre-computed inverse length for a line.
+    #[inline]
+    pub fn inv_length(&self, index: usize) -> Option<f32> {
+        self.inv_lengths.get(index).copied()
+    }
+
+    /// Get all normals as slices (for batch operations).
+    #[inline]
+    pub fn normals(&self) -> (&[f32], &[f32]) {
+        (&self.normal_xs, &self.normal_ys)
+    }
+
+    /// Get all inverse lengths (for batch operations).
+    #[inline]
+    pub fn inv_lengths(&self) -> &[f32] {
+        &self.inv_lengths
+    }
+
+    /// Number of lines in the collection.
+    #[inline]
+    pub fn len(&self) -> usize {
+        self.start_xs.len()
+    }
+
+    /// Check if the collection is empty.
+    #[inline]
+    pub fn is_empty(&self) -> bool {
+        self.start_xs.is_empty()
+    }
+
+    /// Get a line by index.
+    #[inline]
+    pub fn get(&self, index: usize) -> Option<Line2D> {
+        if index < self.len() {
+            Some(Line2D::with_observation_count(
+                Point2D::new(self.start_xs[index], self.start_ys[index]),
+                Point2D::new(self.end_xs[index], self.end_ys[index]),
+                self.observation_counts[index],
+            ))
+        } else {
+            None
+        }
+    }
+
+    /// Convert to Vec<Line2D>.
+    pub fn to_lines(&self) -> Vec<Line2D> {
+        (0..self.len())
+            .map(|i| {
+                Line2D::with_observation_count(
+                    Point2D::new(self.start_xs[i], self.start_ys[i]),
+                    Point2D::new(self.end_xs[i], self.end_ys[i]),
+                    self.observation_counts[i],
+                )
+            })
+            .collect()
+    }
+
+    /// Compute perpendicular distances from a point to all lines.
+    ///
+    /// Uses SIMD-optimized batch computation with pre-computed inverse lengths.
+    /// Returns a vector of distances (always non-negative).
+    ///
+    /// # Formula
+    /// ```text
+    /// distance = |cross(p - start, end - start)| * inv_length
+    ///          = |((px - sx) * (ey - sy) - (py - sy) * (ex - sx))| * inv_length
+    /// ```
+    ///
+    /// # Performance
+    /// Uses pre-computed `inv_lengths` to avoid sqrt/division in the inner loop.
+    pub fn distances_to_point(&self, point: Point2D) -> Vec<f32> {
+        let n = self.len();
+        let mut distances = vec![0.0; n];
+
+        let px = Float4::splat(point.x);
+        let py = Float4::splat(point.y);
+
+        // Process 4 lines at a time
+        let chunks = n / 4;
+        for i in 0..chunks {
+            let base = i * 4;
+
+            // Load line data
+            let sx = Float4::new([
+                self.start_xs[base],
+                self.start_xs[base + 1],
+                self.start_xs[base + 2],
+                self.start_xs[base + 3],
+            ]);
+            let sy = Float4::new([
+                self.start_ys[base],
+                self.start_ys[base + 1],
+                self.start_ys[base + 2],
+                self.start_ys[base + 3],
+            ]);
+            let ex = Float4::new([
+                self.end_xs[base],
+                self.end_xs[base + 1],
+                self.end_xs[base + 2],
+                self.end_xs[base + 3],
+            ]);
+            let ey = Float4::new([
+                self.end_ys[base],
+                self.end_ys[base + 1],
+                self.end_ys[base + 2],
+                self.end_ys[base + 3],
+            ]);
+
+            // Load pre-computed inverse lengths
+            let inv_len = Float4::new([
+                self.inv_lengths[base],
+                self.inv_lengths[base + 1],
+                self.inv_lengths[base + 2],
+                self.inv_lengths[base + 3],
+            ]);
+
+            // Compute direction vectors
+            let dx = ex - sx;
+            let dy = ey - sy;
+
+            // Compute vectors from start to point
+            let to_px = px - sx;
+            let to_py = py - sy;
+
+            // Cross product: (px - sx) * (ey - sy) - (py - sy) * (ex - sx)
+            // = to_px * dy - to_py * dx
+            let cross = to_px * dy - to_py * dx;
+
+            // Distance = |cross| * inv_length (no sqrt needed!)
+            let cross_arr = cross.to_array();
+            let inv_len_arr = inv_len.to_array();
+
+            for j in 0..4 {
+                distances[base + j] = cross_arr[j].abs() * inv_len_arr[j];
+            }
+        }
+
+        // Handle remainder (scalar) using pre-computed values
+        // Using index loop to access multiple parallel arrays at same position
+        #[allow(clippy::needless_range_loop)]
+        for i in (chunks * 4)..n {
+            let sx = self.start_xs[i];
+            let sy = self.start_ys[i];
+            let ex = self.end_xs[i];
+            let ey = self.end_ys[i];
+            let inv_len = self.inv_lengths[i];
+
+            let dx = ex - sx;
+            let dy = ey - sy;
+            let to_px = point.x - sx;
+            let to_py = point.y - sy;
+            let cross = to_px * dy - to_py * dx;
+
+            distances[i] = cross.abs() * inv_len;
+        }
+
+        distances
+    }
+
+    /// Compute projection parameters for a point onto all lines.
+    ///
+    /// Returns a vector of projection parameters t, where:
+    /// - t = 0 means the projection is at line start
+    /// - t = 1 means the projection is at line end
+    /// - t < 0 or t > 1 means the projection is outside the segment
+    ///
+    /// # Formula
+    /// ```text
+    /// t = dot(point - start, end - start) / |end - start|²
+    /// ```
+    pub fn project_point(&self, point: Point2D) -> Vec<f32> {
+        let n = self.len();
+        let mut projections = vec![0.0; n];
+
+        let px = Float4::splat(point.x);
+        let py = Float4::splat(point.y);
+
+        // Process 4 lines at a time
+        let chunks = n / 4;
+        for i in 0..chunks {
+            let base = i * 4;
+
+            // Load line data
+            let sx = Float4::new([
+                self.start_xs[base],
+                self.start_xs[base + 1],
+                self.start_xs[base + 2],
+                self.start_xs[base + 3],
+            ]);
+            let sy = Float4::new([
+                self.start_ys[base],
+                self.start_ys[base + 1],
+                self.start_ys[base + 2],
+                self.start_ys[base + 3],
+            ]);
+            let ex = Float4::new([
+                self.end_xs[base],
+                self.end_xs[base + 1],
+                self.end_xs[base + 2],
+                self.end_xs[base + 3],
+            ]);
+            let ey = Float4::new([
+                self.end_ys[base],
+                self.end_ys[base + 1],
+                self.end_ys[base + 2],
+                self.end_ys[base + 3],
+            ]);
+
+            // Compute direction vectors
+            let dx = ex - sx;
+            let dy = ey - sy;
+
+            // Compute vectors from start to point
+            let to_px = px - sx;
+            let to_py = py - sy;
+
+            // Dot product: (px - sx) * dx + (py - sy) * dy
+            let dot = to_px * dx + to_py * dy;
+
+            // Length squared: dx*dx + dy*dy
+            let len_sq = dx * dx + dy * dy;
+
+            // Compute projection t = dot / len_sq
+            let dot_arr = dot.to_array();
+            let len_sq_arr = len_sq.to_array();
+
+            for j in 0..4 {
+                projections[base + j] = if len_sq_arr[j] > f32::EPSILON {
+                    dot_arr[j] / len_sq_arr[j]
+                } else {
+                    // Degenerate line - return 0.5 (midpoint)
+                    0.5
+                };
+            }
+        }
+
+        // Handle remainder (scalar)
+        for (i, proj) in projections.iter_mut().enumerate().skip(chunks * 4) {
+            let line = self.get(i).unwrap();
+            *proj = line.project_point(point);
+        }
+
+        projections
+    }
+
+    /// Find the nearest line to a point.
+    ///
+    /// Returns (index, distance) of the nearest line, or None if collection is empty.
+    pub fn nearest_line(&self, point: Point2D) -> Option<(usize, f32)> {
+        if self.is_empty() {
+            return None;
+        }
+
+        let distances = self.distances_to_point(point);
+
+        let mut min_idx = 0;
+        let mut min_dist = distances[0];
+
+        for (i, &d) in distances.iter().enumerate().skip(1) {
+            if d < min_dist {
+                min_dist = d;
+                min_idx = i;
+            }
+        }
+
+        Some((min_idx, min_dist))
+    }
+
+    /// Compute perpendicular distances from a point to all lines into a pre-allocated buffer.
+    ///
+    /// Zero-allocation variant of `distances_to_point()` for hot paths.
+    /// Uses SIMD-optimized batch computation with pre-computed inverse lengths.
+    ///
+    /// # Panics
+    /// Panics if `buffer.len() < self.len()`.
+    pub fn distances_to_point_into(&self, point: Point2D, buffer: &mut [f32]) {
+        let n = self.len();
+        debug_assert!(
+            buffer.len() >= n,
+            "Buffer too small: {} < {}",
+            buffer.len(),
+            n
+        );
+
+        let px = Float4::splat(point.x);
+        let py = Float4::splat(point.y);
+
+        // Process 4 lines at a time
+        let chunks = n / 4;
+        for i in 0..chunks {
+            let base = i * 4;
+
+            // Load line data
+            let sx = Float4::new([
+                self.start_xs[base],
+                self.start_xs[base + 1],
+                self.start_xs[base + 2],
+                self.start_xs[base + 3],
+            ]);
+            let sy = Float4::new([
+                self.start_ys[base],
+                self.start_ys[base + 1],
+                self.start_ys[base + 2],
+                self.start_ys[base + 3],
+            ]);
+            let ex = Float4::new([
+                self.end_xs[base],
+                self.end_xs[base + 1],
+                self.end_xs[base + 2],
+                self.end_xs[base + 3],
+            ]);
+            let ey = Float4::new([
+                self.end_ys[base],
+                self.end_ys[base + 1],
+                self.end_ys[base + 2],
+                self.end_ys[base + 3],
+            ]);
+
+            // Load pre-computed inverse lengths
+            let inv_len = Float4::new([
+                self.inv_lengths[base],
+                self.inv_lengths[base + 1],
+                self.inv_lengths[base + 2],
+                self.inv_lengths[base + 3],
+            ]);
+
+            // Compute direction vectors
+            let dx = ex - sx;
+            let dy = ey - sy;
+
+            // Compute vectors from start to point
+            let to_px = px - sx;
+            let to_py = py - sy;
+
+            // Cross product: (px - sx) * (ey - sy) - (py - sy) * (ex - sx)
+            // = to_px * dy - to_py * dx
+            let cross = to_px * dy - to_py * dx;
+
+            // Distance = |cross| * inv_length (no sqrt needed!)
+            let cross_arr = cross.to_array();
+            let inv_len_arr = inv_len.to_array();
+
+            for j in 0..4 {
+                buffer[base + j] = cross_arr[j].abs() * inv_len_arr[j];
+            }
+        }
+
+        // Handle remainder (scalar) using pre-computed values
+        // Using index loop to access multiple parallel arrays at same position
+        #[allow(clippy::needless_range_loop)]
+        for i in (chunks * 4)..n {
+            let sx = self.start_xs[i];
+            let sy = self.start_ys[i];
+            let ex = self.end_xs[i];
+            let ey = self.end_ys[i];
+            let inv_len = self.inv_lengths[i];
+
+            let dx = ex - sx;
+            let dy = ey - sy;
+            let to_px = point.x - sx;
+            let to_py = point.y - sy;
+            let cross = to_px * dy - to_py * dx;
+
+            buffer[i] = cross.abs() * inv_len;
+        }
+    }
+
+    /// Find the nearest line to a point using a reusable buffer.
+    ///
+    /// Zero-allocation variant of `nearest_line()` for hot paths like ICP.
+    /// The buffer will be resized if needed.
+    ///
+    /// Returns (index, distance) of the nearest line, or None if collection is empty.
+    pub fn nearest_line_with_buffer(
+        &self,
+        point: Point2D,
+        buffer: &mut Vec<f32>,
+    ) -> Option<(usize, f32)> {
+        if self.is_empty() {
+            return None;
+        }
+
+        let n = self.len();
+        buffer.resize(n, 0.0);
+        self.distances_to_point_into(point, buffer);
+
+        let mut min_idx = 0;
+        let mut min_dist = buffer[0];
+
+        for (i, &d) in buffer.iter().enumerate().skip(1).take(n - 1) {
+            if d < min_dist {
+                min_dist = d;
+                min_idx = i;
+            }
+        }
+
+        Some((min_idx, min_dist))
+    }
+
+    /// Transform all lines by a pose.
+    ///
+    /// SIMD-optimized: processes 4 points per iteration.
+    /// Normals are rotated (not recomputed) since rotation preserves angles.
+    /// Inverse lengths are preserved since rotation+translation preserves length.
+    pub fn transform(&self, pose: &Pose2D) -> Self {
+        let n = self.len();
+        let mut result = Self::with_capacity(n);
+        result.start_xs.resize(n, 0.0);
+        result.start_ys.resize(n, 0.0);
+        result.end_xs.resize(n, 0.0);
+        result.end_ys.resize(n, 0.0);
+        result.normal_xs.resize(n, 0.0);
+        result.normal_ys.resize(n, 0.0);
+        result.observation_counts = self.observation_counts.clone();
+        // Inverse lengths are preserved by rotation+translation
+        result.inv_lengths = self.inv_lengths.clone();
+
+        let (sin, cos) = pose.theta.sin_cos();
+        let cos4 = Float4::splat(cos);
+        let sin4 = Float4::splat(sin);
+        let tx4 = Float4::splat(pose.x);
+        let ty4 = Float4::splat(pose.y);
+
+        // Process 4 start points at a time
+        let chunks = n / 4;
+        for i in 0..chunks {
+            let base = i * 4;
+
+            // Transform start points
+            let sx = Float4::new([
+                self.start_xs[base],
+                self.start_xs[base + 1],
+                self.start_xs[base + 2],
+                self.start_xs[base + 3],
+            ]);
+            let sy = Float4::new([
+                self.start_ys[base],
+                self.start_ys[base + 1],
+                self.start_ys[base + 2],
+                self.start_ys[base + 3],
+            ]);
+
+            let new_sx = sy.neg_mul_add(sin4, sx.mul_add(cos4, tx4));
+            let new_sy = sx.mul_add(sin4, sy.mul_add(cos4, ty4));
+
+            result.start_xs[base..base + 4].copy_from_slice(&new_sx.to_array());
+            result.start_ys[base..base + 4].copy_from_slice(&new_sy.to_array());
+
+            // Transform end points
+            let ex = Float4::new([
+                self.end_xs[base],
+                self.end_xs[base + 1],
+                self.end_xs[base + 2],
+                self.end_xs[base + 3],
+            ]);
+            let ey = Float4::new([
+                self.end_ys[base],
+                self.end_ys[base + 1],
+                self.end_ys[base + 2],
+                self.end_ys[base + 3],
+            ]);
+
+            let new_ex = ey.neg_mul_add(sin4, ex.mul_add(cos4, tx4));
+            let new_ey = ex.mul_add(sin4, ey.mul_add(cos4, ty4));
+
+            result.end_xs[base..base + 4].copy_from_slice(&new_ex.to_array());
+            result.end_ys[base..base + 4].copy_from_slice(&new_ey.to_array());
+
+            // Rotate normals (no translation needed for unit vectors)
+            let nx = Float4::new([
+                self.normal_xs[base],
+                self.normal_xs[base + 1],
+                self.normal_xs[base + 2],
+                self.normal_xs[base + 3],
+            ]);
+            let ny = Float4::new([
+                self.normal_ys[base],
+                self.normal_ys[base + 1],
+                self.normal_ys[base + 2],
+                self.normal_ys[base + 3],
+            ]);
+
+            // Rotate normal: n' = R * n (no translation)
+            let new_nx = ny.neg_mul_add(sin4, nx * cos4);
+            let new_ny = nx.mul_add(sin4, ny * cos4);
+
+            result.normal_xs[base..base + 4].copy_from_slice(&new_nx.to_array());
+            result.normal_ys[base..base + 4].copy_from_slice(&new_ny.to_array());
+        }
+
+        // Handle remainder (scalar)
+        for i in (chunks * 4)..n {
+            let sx = self.start_xs[i];
+            let sy = self.start_ys[i];
+            let ex = self.end_xs[i];
+            let ey = self.end_ys[i];
+            let nx = self.normal_xs[i];
+            let ny = self.normal_ys[i];
+
+            result.start_xs[i] = sx * cos - sy * sin + pose.x;
+            result.start_ys[i] = sx * sin + sy * cos + pose.y;
+            result.end_xs[i] = ex * cos - ey * sin + pose.x;
+            result.end_ys[i] = ex * sin + ey * cos + pose.y;
+            result.normal_xs[i] = nx * cos - ny * sin;
+            result.normal_ys[i] = nx * sin + ny * cos;
+        }
+
+        result
+    }
+
+    /// Clear all lines.
+    pub fn clear(&mut self) {
+        self.start_xs.clear();
+        self.start_ys.clear();
+        self.end_xs.clear();
+        self.end_ys.clear();
+        self.observation_counts.clear();
+        self.normal_xs.clear();
+        self.normal_ys.clear();
+        self.inv_lengths.clear();
+    }
+
+    /// Remove a line by index (swap-remove for efficiency).
+    pub fn swap_remove(&mut self, index: usize) {
+        self.start_xs.swap_remove(index);
+        self.start_ys.swap_remove(index);
+        self.end_xs.swap_remove(index);
+        self.end_ys.swap_remove(index);
+        self.observation_counts.swap_remove(index);
+        self.normal_xs.swap_remove(index);
+        self.normal_ys.swap_remove(index);
+        self.inv_lengths.swap_remove(index);
+    }
+
+    /// Iterate over lines.
+    pub fn iter(&self) -> impl Iterator<Item = Line2D> + '_ {
+        (0..self.len()).map(move |i| {
+            Line2D::with_observation_count(
+                Point2D::new(self.start_xs[i], self.start_ys[i]),
+                Point2D::new(self.end_xs[i], self.end_ys[i]),
+                self.observation_counts[i],
+            )
+        })
+    }
+}
+
+#[cfg(test)]
+mod tests {
+    use super::*;
+    use approx::assert_relative_eq;
+    use std::f32::consts::FRAC_PI_2;
+
+    #[test]
+    fn test_from_lines() {
+        let lines = vec![
+            Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(1.0, 0.0)),
+            Line2D::new(Point2D::new(0.0, 1.0), Point2D::new(1.0, 1.0)),
+        ];
+
+        let collection = LineCollection::from_lines(&lines);
+
+        assert_eq!(collection.len(), 2);
+        assert_eq!(collection.start_xs, vec![0.0, 0.0]);
+        assert_eq!(collection.start_ys, vec![0.0, 1.0]);
+        assert_eq!(collection.end_xs, vec![1.0, 1.0]);
+        assert_eq!(collection.end_ys, vec![0.0, 1.0]);
+    }
+
+    #[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);
+
+        let lines = collection.to_lines();
+
+        assert_eq!(lines.len(), 2);
+        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[1].observation_count, 2);
+    }
+
+    #[test]
+    fn test_distances_to_point() {
+        let lines = vec![
+            Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(10.0, 0.0)), // Horizontal at y=0
+            Line2D::new(Point2D::new(0.0, 5.0), Point2D::new(10.0, 5.0)), // Horizontal at y=5
+        ];
+
+        let collection = LineCollection::from_lines(&lines);
+        let point = Point2D::new(5.0, 2.0);
+
+        let distances = collection.distances_to_point(point);
+
+        assert_eq!(distances.len(), 2);
+        assert_relative_eq!(distances[0], 2.0, epsilon = 1e-6); // 2 units above first line
+        assert_relative_eq!(distances[1], 3.0, epsilon = 1e-6); // 3 units below second line
+    }
+
+    #[test]
+    fn test_distances_to_point_simd() {
+        // Test with more than 4 lines to exercise SIMD path
+        let mut lines = Vec::new();
+        for i in 0..8 {
+            lines.push(Line2D::new(
+                Point2D::new(0.0, i as f32),
+                Point2D::new(10.0, i as f32),
+            ));
+        }
+
+        let collection = LineCollection::from_lines(&lines);
+        let point = Point2D::new(5.0, 3.5);
+
+        let distances = collection.distances_to_point(point);
+
+        assert_eq!(distances.len(), 8);
+        assert_relative_eq!(distances[3], 0.5, epsilon = 1e-6); // Closest to y=3 line
+        assert_relative_eq!(distances[4], 0.5, epsilon = 1e-6); // Equal distance to y=4 line
+    }
+
+    #[test]
+    fn test_nearest_line() {
+        let lines = vec![
+            Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(10.0, 0.0)),
+            Line2D::new(Point2D::new(0.0, 5.0), Point2D::new(10.0, 5.0)),
+            Line2D::new(Point2D::new(0.0, 2.0), Point2D::new(10.0, 2.0)),
+        ];
+
+        let collection = LineCollection::from_lines(&lines);
+        let point = Point2D::new(5.0, 1.8);
+
+        let (idx, dist) = collection.nearest_line(point).unwrap();
+
+        assert_eq!(idx, 2); // y=2 line is nearest
+        assert_relative_eq!(dist, 0.2, epsilon = 1e-6);
+    }
+
+    #[test]
+    fn test_transform() {
+        let lines = vec![Line2D::new(Point2D::new(1.0, 0.0), Point2D::new(2.0, 0.0))];
+
+        let collection = LineCollection::from_lines(&lines);
+
+        // Rotate 90 degrees
+        let pose = Pose2D::new(0.0, 0.0, FRAC_PI_2);
+        let transformed = collection.transform(&pose);
+
+        assert_relative_eq!(transformed.start_xs[0], 0.0, epsilon = 1e-6);
+        assert_relative_eq!(transformed.start_ys[0], 1.0, epsilon = 1e-6);
+        assert_relative_eq!(transformed.end_xs[0], 0.0, epsilon = 1e-6);
+        assert_relative_eq!(transformed.end_ys[0], 2.0, epsilon = 1e-6);
+    }
+
+    #[test]
+    fn test_transform_simd() {
+        // Test with more than 4 lines
+        let mut lines = Vec::new();
+        for i in 0..8 {
+            lines.push(Line2D::new(
+                Point2D::new(i as f32, 0.0),
+                Point2D::new(i as f32 + 1.0, 0.0),
+            ));
+        }
+
+        let collection = LineCollection::from_lines(&lines);
+        let pose = Pose2D::new(10.0, 20.0, 0.0); // Just translation
+
+        let transformed = collection.transform(&pose);
+
+        for i in 0..8 {
+            assert_relative_eq!(transformed.start_xs[i], i as f32 + 10.0, epsilon = 1e-5);
+            assert_relative_eq!(transformed.start_ys[i], 20.0, epsilon = 1e-5);
+        }
+    }
+
+    #[test]
+    fn test_swap_remove() {
+        let lines = vec![
+            Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(1.0, 0.0)),
+            Line2D::new(Point2D::new(0.0, 1.0), Point2D::new(1.0, 1.0)),
+            Line2D::new(Point2D::new(0.0, 2.0), Point2D::new(1.0, 2.0)),
+        ];
+
+        let mut collection = LineCollection::from_lines(&lines);
+        collection.swap_remove(0);
+
+        assert_eq!(collection.len(), 2);
+        // First element is now the last one (swap-remove)
+        assert_eq!(collection.start_ys[0], 2.0);
+        assert_eq!(collection.start_ys[1], 1.0);
+    }
+
+    #[test]
+    fn test_iter() {
+        let lines = vec![
+            Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(1.0, 0.0)),
+            Line2D::new(Point2D::new(0.0, 1.0), Point2D::new(1.0, 1.0)),
+        ];
+
+        let collection = LineCollection::from_lines(&lines);
+        let collected: Vec<_> = collection.iter().collect();
+
+        assert_eq!(collected.len(), 2);
+        assert_eq!(collected[0].start, Point2D::new(0.0, 0.0));
+        assert_eq!(collected[1].start, Point2D::new(0.0, 1.0));
+    }
+
+    #[test]
+    fn test_distances_to_point_into() {
+        let lines = vec![
+            Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(10.0, 0.0)), // Horizontal at y=0
+            Line2D::new(Point2D::new(0.0, 5.0), Point2D::new(10.0, 5.0)), // Horizontal at y=5
+        ];
+
+        let collection = LineCollection::from_lines(&lines);
+        let point = Point2D::new(5.0, 2.0);
+
+        // Test buffer reuse variant
+        let mut buffer = vec![0.0; 10]; // Oversized buffer
+        collection.distances_to_point_into(point, &mut buffer);
+
+        assert_relative_eq!(buffer[0], 2.0, epsilon = 1e-6);
+        assert_relative_eq!(buffer[1], 3.0, epsilon = 1e-6);
+
+        // Verify matches allocating version
+        let distances = collection.distances_to_point(point);
+        assert_relative_eq!(buffer[0], distances[0], epsilon = 1e-6);
+        assert_relative_eq!(buffer[1], distances[1], epsilon = 1e-6);
+    }
+
+    #[test]
+    fn test_nearest_line_with_buffer() {
+        let lines = vec![
+            Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(10.0, 0.0)),
+            Line2D::new(Point2D::new(0.0, 5.0), Point2D::new(10.0, 5.0)),
+            Line2D::new(Point2D::new(0.0, 2.0), Point2D::new(10.0, 2.0)),
+        ];
+
+        let collection = LineCollection::from_lines(&lines);
+        let point = Point2D::new(5.0, 1.8);
+
+        // Test buffer reuse variant
+        let mut buffer = Vec::new();
+        let (idx, dist) = collection
+            .nearest_line_with_buffer(point, &mut buffer)
+            .unwrap();
+
+        assert_eq!(idx, 2); // y=2 line is nearest
+        assert_relative_eq!(dist, 0.2, epsilon = 1e-6);
+
+        // Buffer should be reused on subsequent calls
+        let (idx2, dist2) = collection
+            .nearest_line_with_buffer(point, &mut buffer)
+            .unwrap();
+        assert_eq!(idx, idx2);
+        assert_relative_eq!(dist, dist2, epsilon = 1e-6);
+
+        // Verify matches allocating version
+        let (idx_alloc, dist_alloc) = collection.nearest_line(point).unwrap();
+        assert_eq!(idx, idx_alloc);
+        assert_relative_eq!(dist, dist_alloc, epsilon = 1e-6);
+    }
+
+    #[test]
+    fn test_distances_to_point_into_simd() {
+        // Test with more than 4 lines to exercise SIMD path
+        let mut lines = Vec::new();
+        for i in 0..8 {
+            lines.push(Line2D::new(
+                Point2D::new(0.0, i as f32),
+                Point2D::new(10.0, i as f32),
+            ));
+        }
+
+        let collection = LineCollection::from_lines(&lines);
+        let point = Point2D::new(5.0, 3.5);
+
+        let mut buffer = vec![0.0; 8];
+        collection.distances_to_point_into(point, &mut buffer);
+
+        // Verify matches allocating version
+        let distances = collection.distances_to_point(point);
+        for i in 0..8 {
+            assert_relative_eq!(buffer[i], distances[i], epsilon = 1e-6);
+        }
+    }
+}

vastu-map/src/features/mod.rs

@@ -0,0 +1,18 @@
+//! Geometric feature types for VectorMap SLAM.
+//!
+//! This module provides:
+//! - [`Line2D`]: Line segment defined by endpoints
+//! - [`LineCollection`]: SoA collection for SIMD operations
+//! - [`Corner2D`]: Corner feature at line intersections
+//! - [`CornerCollection`]: SoA collection for corners
+//! - [`FeatureSet`]: Combined container for lines and corners
+
+pub mod corner;
+pub mod feature_set;
+pub mod line;
+pub mod line_collection;
+
+pub use corner::{Corner2D, CornerCollection};
+pub use feature_set::FeatureSet;
+pub use line::Line2D;
+pub use line_collection::LineCollection;

vastu-map/src/integration/association.rs

@@ -0,0 +1,458 @@
+//! Line association for map integration.
+//!
+//! Associates new scan lines with existing map lines based on:
+//! - Spatial proximity
+//! - Angular similarity
+//! - Overlap extent
+
+use crate::core::math::angle_diff;
+use crate::features::Line2D;
+
+use super::spatial_index::SpatialIndex;
+
+/// Configuration for line association.
+#[derive(Clone, Debug)]
+pub struct AssociationConfig {
+    /// Maximum perpendicular distance for association (meters).
+    /// Default: 0.15m
+    pub max_perpendicular_distance: f32,
+
+    /// Maximum angular difference for association (radians).
+    /// Default: 0.2 (~11°)
+    pub max_angle_difference: f32,
+
+    /// Minimum overlap ratio for valid association.
+    /// Overlap is measured as a fraction of the shorter line's length.
+    /// Default: 0.3 (30%)
+    pub min_overlap_ratio: f32,
+
+    /// Maximum gap between lines for association (meters).
+    /// Lines that don't overlap but are close enough may still associate.
+    /// Default: 0.3m
+    pub max_endpoint_gap: f32,
+}
+
+impl Default for AssociationConfig {
+    fn default() -> Self {
+        Self {
+            max_perpendicular_distance: 0.15,
+            max_angle_difference: 0.2,
+            min_overlap_ratio: 0.3,
+            max_endpoint_gap: 0.3,
+        }
+    }
+}
+
+impl AssociationConfig {
+    /// Create a new configuration with default values.
+    pub fn new() -> Self {
+        Self::default()
+    }
+
+    /// Builder-style setter for maximum perpendicular distance.
+    pub fn with_max_perpendicular_distance(mut self, meters: f32) -> Self {
+        self.max_perpendicular_distance = meters;
+        self
+    }
+
+    /// Builder-style setter for maximum angle difference.
+    pub fn with_max_angle_difference(mut self, radians: f32) -> Self {
+        self.max_angle_difference = radians;
+        self
+    }
+
+    /// Builder-style setter for minimum overlap ratio.
+    pub fn with_min_overlap_ratio(mut self, ratio: f32) -> Self {
+        self.min_overlap_ratio = ratio;
+        self
+    }
+}
+
+/// Result of line association.
+#[derive(Clone, Debug)]
+pub struct Association {
+    /// Index of the scan line.
+    pub scan_line_idx: usize,
+    /// Index of the associated map line.
+    pub map_line_idx: usize,
+    /// Perpendicular distance between lines.
+    pub distance: f32,
+    /// Angular difference between lines (radians).
+    pub angle_diff: f32,
+    /// Overlap length (meters).
+    pub overlap_length: f32,
+    /// Overlap ratio (0 to 1).
+    pub overlap_ratio: f32,
+}
+
+impl Association {
+    /// Create a new association.
+    pub fn new(
+        scan_line_idx: usize,
+        map_line_idx: usize,
+        distance: f32,
+        angle_diff: f32,
+        overlap_length: f32,
+        overlap_ratio: f32,
+    ) -> Self {
+        Self {
+            scan_line_idx,
+            map_line_idx,
+            distance,
+            angle_diff,
+            overlap_length,
+            overlap_ratio,
+        }
+    }
+
+    /// Compute a quality score for the association (higher is better).
+    pub fn quality_score(&self) -> f32 {
+        // Weighted combination of factors
+        let dist_score = 1.0 / (1.0 + self.distance * 10.0);
+        let angle_score = 1.0 / (1.0 + self.angle_diff * 5.0);
+        let overlap_score = self.overlap_ratio;
+
+        dist_score * angle_score * overlap_score
+    }
+}
+
+/// Find associations between scan lines and map lines.
+///
+/// # Arguments
+/// * `scan_lines` - Lines extracted from the current scan
+/// * `map_lines` - Existing map lines
+/// * `index` - Spatial index of map lines (optional, for efficiency)
+/// * `config` - Association configuration
+///
+/// # Returns
+/// Vector of associations, one per scan line that found a match.
+/// Scan lines without matches are not included.
+pub fn find_associations(
+    scan_lines: &[Line2D],
+    map_lines: &[Line2D],
+    index: Option<&SpatialIndex>,
+    config: &AssociationConfig,
+) -> Vec<Association> {
+    if scan_lines.is_empty() || map_lines.is_empty() {
+        return Vec::new();
+    }
+
+    let mut associations = Vec::with_capacity(scan_lines.len());
+
+    for (scan_idx, scan_line) in scan_lines.iter().enumerate() {
+        // Find candidate map lines using spatial index if available
+        let candidates: Vec<usize> = if let Some(idx) = index {
+            idx.potential_matches(scan_line, config.max_perpendicular_distance * 2.0)
+        } else {
+            (0..map_lines.len()).collect()
+        };
+
+        let mut best_association: Option<Association> = None;
+        let mut best_score = 0.0;
+
+        for &map_idx in &candidates {
+            let map_line = &map_lines[map_idx];
+
+            if let Some(assoc) = try_associate(scan_idx, scan_line, map_idx, map_line, config) {
+                let score = assoc.quality_score();
+                if score > best_score {
+                    best_score = score;
+                    best_association = Some(assoc);
+                }
+            }
+        }
+
+        if let Some(assoc) = best_association {
+            associations.push(assoc);
+        }
+    }
+
+    associations
+}
+
+/// Try to associate two lines.
+///
+/// Returns Some(Association) if lines are compatible, None otherwise.
+fn try_associate(
+    scan_idx: usize,
+    scan_line: &Line2D,
+    map_idx: usize,
+    map_line: &Line2D,
+    config: &AssociationConfig,
+) -> Option<Association> {
+    // Check angular compatibility
+    let scan_angle = scan_line.angle();
+    let map_angle = map_line.angle();
+    let angle_difference = angle_diff(scan_angle, map_angle).abs();
+
+    // Lines can be oriented either way, so check both directions
+    let angle_difference = angle_difference.min(std::f32::consts::PI - angle_difference);
+
+    if angle_difference > config.max_angle_difference {
+        return None;
+    }
+
+    // Compute perpendicular distance (average of endpoint distances)
+    let dist1 = map_line.distance_to_point(scan_line.start);
+    let dist2 = map_line.distance_to_point(scan_line.end);
+    let avg_distance = (dist1 + dist2) / 2.0;
+
+    if avg_distance > config.max_perpendicular_distance {
+        return None;
+    }
+
+    // Compute overlap
+    let overlap = compute_overlap(scan_line, map_line);
+    let scan_len = scan_line.length();
+    let map_len = map_line.length();
+    let shorter_len = scan_len.min(map_len);
+
+    // Handle case where lines don't overlap but are close
+    if overlap <= 0.0 {
+        let gap = compute_endpoint_gap(scan_line, map_line);
+        if gap > config.max_endpoint_gap {
+            return None;
+        }
+        // Allow association with zero overlap if endpoints are close
+        return Some(Association::new(
+            scan_idx,
+            map_idx,
+            avg_distance,
+            angle_difference,
+            0.0,
+            0.0,
+        ));
+    }
+
+    let overlap_ratio = if shorter_len > 0.0 {
+        overlap / shorter_len
+    } else {
+        0.0
+    };
+
+    if overlap_ratio < config.min_overlap_ratio {
+        return None;
+    }
+
+    Some(Association::new(
+        scan_idx,
+        map_idx,
+        avg_distance,
+        angle_difference,
+        overlap,
+        overlap_ratio,
+    ))
+}
+
+/// Compute overlap length between two lines.
+///
+/// Projects both lines onto a common axis and computes intersection.
+fn compute_overlap(line1: &Line2D, line2: &Line2D) -> f32 {
+    // Use Line2D's overlap_length method with default angle tolerance
+    line1.overlap_length(line2, 0.3)
+}
+
+/// Compute minimum gap between line endpoints.
+fn compute_endpoint_gap(line1: &Line2D, line2: &Line2D) -> f32 {
+    let d1 = line1.start.distance(line2.start);
+    let d2 = line1.start.distance(line2.end);
+    let d3 = line1.end.distance(line2.start);
+    let d4 = line1.end.distance(line2.end);
+
+    d1.min(d2).min(d3).min(d4)
+}
+
+/// Associate scan lines to map lines, ensuring each map line
+/// has at most one association (best match wins).
+///
+/// This is useful when updating the map to avoid duplicate associations.
+pub fn find_unique_associations(
+    scan_lines: &[Line2D],
+    map_lines: &[Line2D],
+    index: Option<&SpatialIndex>,
+    config: &AssociationConfig,
+) -> Vec<Association> {
+    let all_associations = find_associations(scan_lines, map_lines, index, config);
+
+    // Group by map line and keep best match
+    let mut best_per_map: std::collections::HashMap<usize, Association> =
+        std::collections::HashMap::new();
+
+    for assoc in all_associations {
+        let entry = best_per_map.entry(assoc.map_line_idx);
+        entry
+            .and_modify(|existing| {
+                if assoc.quality_score() > existing.quality_score() {
+                    *existing = assoc.clone();
+                }
+            })
+            .or_insert(assoc);
+    }
+
+    best_per_map.into_values().collect()
+}
+
+/// Find scan lines that don't match any map lines.
+///
+/// These are candidates for new features to add to the map.
+pub fn find_unmatched_scan_lines(
+    scan_lines: &[Line2D],
+    map_lines: &[Line2D],
+    index: Option<&SpatialIndex>,
+    config: &AssociationConfig,
+) -> Vec<usize> {
+    let associations = find_associations(scan_lines, map_lines, index, config);
+    let matched: std::collections::HashSet<usize> =
+        associations.iter().map(|a| a.scan_line_idx).collect();
+
+    (0..scan_lines.len())
+        .filter(|i| !matched.contains(i))
+        .collect()
+}
+
+#[cfg(test)]
+mod tests {
+    use super::*;
+    use crate::core::Point2D;
+    use approx::assert_relative_eq;
+
+    #[test]
+    fn test_association_quality_score() {
+        let good = Association::new(0, 0, 0.01, 0.01, 1.0, 0.9);
+        let bad = Association::new(0, 0, 0.5, 0.5, 0.1, 0.1);
+
+        assert!(good.quality_score() > bad.quality_score());
+    }
+
+    #[test]
+    fn test_find_associations_identical_lines() {
+        let scan_lines = vec![Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(5.0, 0.0))];
+        let map_lines = vec![Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(5.0, 0.0))];
+
+        let config = AssociationConfig::default();
+        let assocs = find_associations(&scan_lines, &map_lines, None, &config);
+
+        assert_eq!(assocs.len(), 1);
+        assert_eq!(assocs[0].scan_line_idx, 0);
+        assert_eq!(assocs[0].map_line_idx, 0);
+        assert!(assocs[0].distance < 0.01);
+        assert!(assocs[0].overlap_ratio > 0.99);
+    }
+
+    #[test]
+    fn test_find_associations_parallel_lines() {
+        // Parallel lines with small offset
+        let scan_lines = vec![Line2D::new(Point2D::new(0.0, 0.1), Point2D::new(5.0, 0.1))];
+        let map_lines = vec![Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(5.0, 0.0))];
+
+        let config = AssociationConfig::default();
+        let assocs = find_associations(&scan_lines, &map_lines, None, &config);
+
+        assert_eq!(assocs.len(), 1);
+        assert_relative_eq!(assocs[0].distance, 0.1, epsilon = 0.01);
+    }
+
+    #[test]
+    fn test_find_associations_no_match_angle() {
+        // Perpendicular lines
+        let scan_lines = vec![Line2D::new(Point2D::new(2.5, -1.0), Point2D::new(2.5, 1.0))];
+        let map_lines = vec![Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(5.0, 0.0))];
+
+        let config = AssociationConfig::default();
+        let assocs = find_associations(&scan_lines, &map_lines, None, &config);
+
+        // Should not match due to angle difference
+        assert!(assocs.is_empty());
+    }
+
+    #[test]
+    fn test_find_associations_no_match_distance() {
+        // Parallel but far apart
+        let scan_lines = vec![Line2D::new(Point2D::new(0.0, 1.0), Point2D::new(5.0, 1.0))];
+        let map_lines = vec![Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(5.0, 0.0))];
+
+        let config = AssociationConfig::default().with_max_perpendicular_distance(0.5);
+        let assocs = find_associations(&scan_lines, &map_lines, None, &config);
+
+        // Should not match due to distance
+        assert!(assocs.is_empty());
+    }
+
+    #[test]
+    fn test_find_associations_partial_overlap() {
+        // Overlapping lines
+        let scan_lines = vec![Line2D::new(Point2D::new(2.0, 0.0), Point2D::new(7.0, 0.0))];
+        let map_lines = vec![Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(5.0, 0.0))];
+
+        let config = AssociationConfig::default();
+        let assocs = find_associations(&scan_lines, &map_lines, None, &config);
+
+        assert_eq!(assocs.len(), 1);
+        assert!(assocs[0].overlap_length > 2.0); // Should overlap by 3 meters
+    }
+
+    #[test]
+    fn test_find_associations_multiple() {
+        let scan_lines = vec![
+            Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(5.0, 0.0)),
+            Line2D::new(Point2D::new(5.0, 0.0), Point2D::new(5.0, 5.0)),
+        ];
+        let map_lines = vec![
+            Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(5.0, 0.0)),
+            Line2D::new(Point2D::new(5.0, 0.0), Point2D::new(5.0, 5.0)),
+        ];
+
+        let config = AssociationConfig::default();
+        let assocs = find_associations(&scan_lines, &map_lines, None, &config);
+
+        assert_eq!(assocs.len(), 2);
+    }
+
+    #[test]
+    fn test_find_unique_associations() {
+        // Two scan lines that could match the same map line
+        let scan_lines = vec![
+            Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(2.5, 0.0)),
+            Line2D::new(Point2D::new(2.5, 0.0), Point2D::new(5.0, 0.0)),
+        ];
+        let map_lines = vec![Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(5.0, 0.0))];
+
+        let config = AssociationConfig::default();
+        let assocs = find_unique_associations(&scan_lines, &map_lines, None, &config);
+
+        // Should only have one association (best match)
+        assert_eq!(assocs.len(), 1);
+    }
+
+    #[test]
+    fn test_find_unmatched() {
+        let scan_lines = vec![
+            Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(5.0, 0.0)),
+            Line2D::new(Point2D::new(0.0, 10.0), Point2D::new(5.0, 10.0)), // No match
+        ];
+        let map_lines = vec![Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(5.0, 0.0))];
+
+        let config = AssociationConfig::default();
+        let unmatched = find_unmatched_scan_lines(&scan_lines, &map_lines, None, &config);
+
+        assert_eq!(unmatched.len(), 1);
+        assert_eq!(unmatched[0], 1); // Second scan line is unmatched
+    }
+
+    #[test]
+    fn test_with_spatial_index() {
+        let scan_lines = vec![Line2D::new(Point2D::new(0.0, 0.1), Point2D::new(5.0, 0.1))];
+        let map_lines = vec![
+            Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(5.0, 0.0)),
+            Line2D::new(Point2D::new(0.0, 10.0), Point2D::new(5.0, 10.0)),
+        ];
+
+        let index = SpatialIndex::new(&map_lines);
+        let config = AssociationConfig::default();
+
+        let assocs = find_associations(&scan_lines, &map_lines, Some(&index), &config);
+
+        assert_eq!(assocs.len(), 1);
+        assert_eq!(assocs[0].map_line_idx, 0);
+    }
+}

vastu-map/src/integration/merger.rs

@@ -0,0 +1,421 @@
+//! Feature merging for map integration.
+//!
+//! Merges associated lines into updated map lines, extending
+//! endpoints and updating parameters based on observations.
+
+use crate::core::Point2D;
+use crate::features::Line2D;
+
+use super::association::Association;
+
+/// Configuration for line merging.
+#[derive(Clone, Debug)]
+pub struct MergerConfig {
+    /// Weight given to existing map line (vs. scan line).
+    /// Higher values make the map more stable but slower to adapt.
+    /// Default: 0.8 (map line has 80% weight)
+    pub map_weight: f32,
+
+    /// Whether to extend line endpoints based on observations.
+    /// Default: true
+    pub extend_endpoints: bool,
+
+    /// Maximum endpoint extension per observation (meters).
+    /// Default: 0.5m
+    pub max_extension: f32,
+
+    /// Minimum extension required to actually extend (meters).
+    /// Small extensions are ignored to avoid noise accumulation.
+    /// Default: 0.05m
+    pub min_extension: f32,
+}
+
+impl Default for MergerConfig {
+    fn default() -> Self {
+        Self {
+            map_weight: 0.8,
+            extend_endpoints: true,
+            max_extension: 0.5,
+            min_extension: 0.05,
+        }
+    }
+}
+
+impl MergerConfig {
+    /// Create a new configuration with default values.
+    pub fn new() -> Self {
+        Self::default()
+    }
+
+    /// Builder-style setter for map weight.
+    pub fn with_map_weight(mut self, weight: f32) -> Self {
+        self.map_weight = weight;
+        self
+    }
+
+    /// Builder-style setter for endpoint extension.
+    pub fn with_extend_endpoints(mut self, extend: bool) -> Self {
+        self.extend_endpoints = extend;
+        self
+    }
+
+    /// Builder-style setter for maximum extension.
+    pub fn with_max_extension(mut self, meters: f32) -> Self {
+        self.max_extension = meters;
+        self
+    }
+}
+
+/// Result of merging a line.
+#[derive(Clone, Debug)]
+pub struct MergeResult {
+    /// The merged line.
+    pub line: Line2D,
+    /// Whether the line was extended at the start.
+    pub extended_start: bool,
+    /// Whether the line was extended at the end.
+    pub extended_end: bool,
+    /// Amount extended at start (meters, positive = extended).
+    pub start_extension: f32,
+    /// Amount extended at end (meters, positive = extended).
+    pub end_extension: f32,
+}
+
+impl MergeResult {
+    /// Create a new merge result with no extension.
+    pub fn new(line: Line2D) -> Self {
+        Self {
+            line,
+            extended_start: false,
+            extended_end: false,
+            start_extension: 0.0,
+            end_extension: 0.0,
+        }
+    }
+}
+
+/// Merge a scan line with a map line based on their association.
+///
+/// # Arguments
+/// * `scan_line` - The new observation
+/// * `map_line` - The existing map line
+/// * `association` - Association between the lines
+/// * `config` - Merger configuration
+///
+/// # Returns
+/// Merge result with the updated line.
+pub fn merge_lines(
+    scan_line: &Line2D,
+    map_line: &Line2D,
+    _association: &Association,
+    config: &MergerConfig,
+) -> MergeResult {
+    let scan_weight = 1.0 - config.map_weight;
+
+    // Merge direction using weighted average
+    let map_dir = map_line.unit_direction();
+    let scan_dir = scan_line.unit_direction();
+
+    // Ensure directions are aligned (not pointing opposite ways)
+    let dot = map_dir.dot(scan_dir);
+    let aligned_scan_dir = if dot < 0.0 { -scan_dir } else { scan_dir };
+
+    // Weighted average of directions
+    let merged_dir = Point2D::new(
+        map_dir.x * config.map_weight + aligned_scan_dir.x * scan_weight,
+        map_dir.y * config.map_weight + aligned_scan_dir.y * scan_weight,
+    )
+    .normalized();
+
+    // Get all four endpoints
+    let scan_start = scan_line.start;
+    let scan_end = scan_line.end;
+    let map_start = map_line.start;
+    let map_end = map_line.end;
+
+    // Project all endpoints onto the merged direction line
+    // Use the map line's midpoint as reference
+    let reference = map_line.midpoint();
+
+    let project = |p: Point2D| -> f32 { (p - reference).dot(merged_dir) };
+
+    let scan_start_t = project(scan_start);
+    let scan_end_t = project(scan_end);
+    let map_start_t = project(map_start);
+    let map_end_t = project(map_end);
+
+    // Find the extent of the merged line
+    let mut min_t = map_start_t.min(map_end_t);
+    let mut max_t = map_start_t.max(map_end_t);
+
+    let mut extended_start = false;
+    let mut extended_end = false;
+    let mut start_extension = 0.0f32;
+    let mut end_extension = 0.0f32;
+
+    if config.extend_endpoints {
+        // Check if scan extends beyond map
+        let scan_min_t = scan_start_t.min(scan_end_t);
+        let scan_max_t = scan_start_t.max(scan_end_t);
+
+        if scan_min_t < min_t {
+            let extension = (min_t - scan_min_t).min(config.max_extension);
+            if extension >= config.min_extension {
+                min_t -= extension;
+                extended_start = true;
+                start_extension = extension;
+            }
+        }
+
+        if scan_max_t > max_t {
+            let extension = (scan_max_t - max_t).min(config.max_extension);
+            if extension >= config.min_extension {
+                max_t += extension;
+                extended_end = true;
+                end_extension = extension;
+            }
+        }
+    }
+
+    // Compute new endpoints
+    let new_start = reference + merged_dir * min_t;
+    let new_end = reference + merged_dir * max_t;
+
+    // Average perpendicular offset (to preserve line position)
+    let map_perp = map_line.normal();
+    let map_offset = (map_start - reference).dot(map_perp);
+    let scan_perp_offset = (scan_line.midpoint() - reference).dot(map_perp);
+    let merged_offset = map_offset * config.map_weight + scan_perp_offset * scan_weight;
+
+    let perp = Point2D::new(-merged_dir.y, merged_dir.x);
+    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(
+        final_start,
+        final_end,
+        map_line.observation_count.saturating_add(1),
+    );
+
+    MergeResult {
+        line: merged_line,
+        extended_start,
+        extended_end,
+        start_extension,
+        end_extension,
+    }
+}
+
+/// Batch merge multiple associations.
+///
+/// # Arguments
+/// * `scan_lines` - All scan lines
+/// * `map_lines` - All map lines (mutable for in-place update)
+/// * `associations` - Associations between scan and map lines
+/// * `config` - Merger configuration
+///
+/// # Returns
+/// Vector of (map_line_idx, MergeResult) for each merge performed.
+pub fn batch_merge(
+    scan_lines: &[Line2D],
+    map_lines: &mut [Line2D],
+    associations: &[Association],
+    config: &MergerConfig,
+) -> Vec<(usize, MergeResult)> {
+    let mut results = Vec::with_capacity(associations.len());
+
+    for assoc in associations {
+        let scan_line = &scan_lines[assoc.scan_line_idx];
+        let map_line = &map_lines[assoc.map_line_idx];
+
+        let result = merge_lines(scan_line, map_line, assoc, config);
+
+        // Update map line in place
+        map_lines[assoc.map_line_idx] = result.line;
+
+        results.push((assoc.map_line_idx, result));
+    }
+
+    results
+}
+
+/// Create a new line from a scan observation (for unmatched scan lines).
+///
+/// This is used when adding new features to the map.
+pub fn create_new_line(scan_line: &Line2D) -> Line2D {
+    Line2D::with_observation_count(scan_line.start, scan_line.end, 1)
+}
+
+/// Merge two line segments that represent the same physical feature.
+///
+/// This is a simpler merge that just extends the endpoints.
+pub fn merge_collinear_lines(line1: &Line2D, line2: &Line2D) -> Option<Line2D> {
+    // Check if lines are approximately collinear
+    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 > 0.1 {
+        // ~5.7 degrees
+        return None;
+    }
+
+    // Check perpendicular distance
+    let dist1 = line1.distance_to_point(line2.start);
+    let dist2 = line1.distance_to_point(line2.end);
+    let max_dist = dist1.max(dist2);
+
+    if max_dist > 0.1 {
+        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 count = line1
+        .observation_count
+        .saturating_add(line2.observation_count);
+
+    Some(Line2D::with_observation_count(new_start, new_end, count))
+}
+
+#[cfg(test)]
+mod tests {
+    use super::*;
+    use approx::assert_relative_eq;
+
+    #[test]
+    fn test_merge_identical_lines() {
+        let scan = Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(5.0, 0.0));
+        let map = Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(5.0, 0.0));
+        let assoc = Association::new(0, 0, 0.0, 0.0, 5.0, 1.0);
+
+        let config = MergerConfig::default();
+        let result = merge_lines(&scan, &map, &assoc, &config);
+
+        assert_relative_eq!(result.line.length(), 5.0, epsilon = 0.01);
+        assert!(!result.extended_start);
+        assert!(!result.extended_end);
+    }
+
+    #[test]
+    fn test_merge_extends_line() {
+        let scan = Line2D::new(Point2D::new(-1.0, 0.0), Point2D::new(6.0, 0.0));
+        let map = Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(5.0, 0.0));
+        let assoc = Association::new(0, 0, 0.0, 0.0, 5.0, 1.0);
+
+        let config = MergerConfig::default();
+        let result = merge_lines(&scan, &map, &assoc, &config);
+
+        assert!(result.line.length() > 5.0);
+        assert!(result.extended_start || result.extended_end);
+    }
+
+    #[test]
+    fn test_merge_respects_max_extension() {
+        let scan = Line2D::new(Point2D::new(-5.0, 0.0), Point2D::new(10.0, 0.0));
+        let map = Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(5.0, 0.0));
+        let assoc = Association::new(0, 0, 0.0, 0.0, 5.0, 1.0);
+
+        let config = MergerConfig::default().with_max_extension(0.5);
+        let result = merge_lines(&scan, &map, &assoc, &config);
+
+        // Should not extend by more than 0.5m on each side
+        assert!(result.line.length() <= 6.0); // Original 5.0 + max 0.5 + 0.5
+    }
+
+    #[test]
+    fn test_merge_weighted_average() {
+        // Scan line slightly offset from map line
+        let scan = Line2D::new(Point2D::new(0.0, 0.1), Point2D::new(5.0, 0.1));
+        let map = Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(5.0, 0.0));
+        let assoc = Association::new(0, 0, 0.1, 0.0, 5.0, 1.0);
+
+        let config = MergerConfig::default().with_map_weight(0.8);
+        let result = merge_lines(&scan, &map, &assoc, &config);
+
+        // Result should be closer to map line (weighted 80%)
+        let mid_y = result.line.midpoint().y;
+        assert!(mid_y < 0.05); // Closer to map's y=0 than scan's y=0.1
+    }
+
+    #[test]
+    fn test_batch_merge() {
+        let scan_lines = vec![
+            Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(5.0, 0.0)),
+            Line2D::new(Point2D::new(5.0, 0.0), Point2D::new(5.0, 5.0)),
+        ];
+        let mut map_lines = vec![
+            Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(5.0, 0.0)),
+            Line2D::new(Point2D::new(5.0, 0.0), Point2D::new(5.0, 5.0)),
+        ];
+        let associations = vec![
+            Association::new(0, 0, 0.0, 0.0, 5.0, 1.0),
+            Association::new(1, 1, 0.0, 0.0, 5.0, 1.0),
+        ];
+
+        let config = MergerConfig::default();
+        let results = batch_merge(&scan_lines, &mut map_lines, &associations, &config);
+
+        assert_eq!(results.len(), 2);
+        // Observation counts should be incremented
+        assert!(map_lines[0].observation_count > 1);
+        assert!(map_lines[1].observation_count > 1);
+    }
+
+    #[test]
+    fn test_create_new_line() {
+        let scan = Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(5.0, 0.0));
+        let new_line = create_new_line(&scan);
+
+        assert_eq!(new_line.start, scan.start);
+        assert_eq!(new_line.end, scan.end);
+        assert_eq!(new_line.observation_count, 1);
+    }
+
+    #[test]
+    fn test_merge_collinear_lines() {
+        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 merged = merge_collinear_lines(&line1, &line2);
+
+        assert!(merged.is_some());
+        let merged = merged.unwrap();
+        assert_relative_eq!(merged.length(), 5.0, epsilon = 0.01);
+    }
+
+    #[test]
+    fn test_merge_collinear_rejects_perpendicular() {
+        let line1 = Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(5.0, 0.0));
+        let line2 = Line2D::new(Point2D::new(2.5, -2.0), Point2D::new(2.5, 2.0));
+
+        let merged = merge_collinear_lines(&line1, &line2);
+
+        assert!(merged.is_none());
+    }
+
+    #[test]
+    fn test_merge_collinear_rejects_far() {
+        let line1 = Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(5.0, 0.0));
+        let line2 = Line2D::new(Point2D::new(0.0, 1.0), Point2D::new(5.0, 1.0));
+
+        let merged = merge_collinear_lines(&line1, &line2);
+
+        assert!(merged.is_none());
+    }
+}

vastu-map/src/integration/mod.rs

@@ -0,0 +1,54 @@
+//! Map integration for combining scan features with the map.
+//!
+//! This module provides algorithms for:
+//! - Spatial indexing (R-tree) for efficient queries
+//! - Line association (matching scan lines to map lines)
+//! - Feature merging (updating map lines with observations)
+//!
+//! # Integration Pipeline
+//!
+//! 1. **Associate**: Find correspondences between scan and map lines
+//! 2. **Merge**: Update matched map lines with new observations
+//! 3. **Add**: Insert unmatched scan lines as new map features
+//!
+//! # Example
+//!
+//! ```rust,ignore
+//! use vastu_map::integration::{
+//!     SpatialIndex, find_associations, batch_merge, create_new_line,
+//!     AssociationConfig, MergerConfig,
+//! };
+//! use vastu_map::features::Line2D;
+//!
+//! // Extract features from scan
+//! let scan_lines: Vec<Line2D> = extract_from_scan();
+//!
+//! // Find associations with map
+//! let index = SpatialIndex::new(&map_lines);
+//! let assoc_config = AssociationConfig::default();
+//! let associations = find_associations(&scan_lines, &map_lines, Some(&index), &assoc_config);
+//!
+//! // Merge matched lines
+//! let merger_config = MergerConfig::default();
+//! batch_merge(&scan_lines, &mut map_lines, &associations, &merger_config);
+//!
+//! // Add unmatched lines as new features
+//! let unmatched = find_unmatched_scan_lines(&scan_lines, &map_lines, Some(&index), &assoc_config);
+//! for idx in unmatched {
+//!     map_lines.push(create_new_line(&scan_lines[idx]));
+//! }
+//! ```
+
+pub mod association;
+pub mod merger;
+pub mod spatial_index;
+
+// Re-export main types
+pub use association::{
+    Association, AssociationConfig, find_associations, find_unique_associations,
+    find_unmatched_scan_lines,
+};
+pub use merger::{
+    MergeResult, MergerConfig, batch_merge, create_new_line, merge_collinear_lines, merge_lines,
+};
+pub use spatial_index::{IndexedLine, SpatialIndex};

vastu-map/src/integration/spatial_index.rs

@@ -0,0 +1,406 @@
+//! Spatial indexing for efficient map queries.
+//!
+//! Uses an R-tree to enable fast spatial queries:
+//! - Nearest line to a point
+//! - Lines within a bounding box
+//! - Lines within a radius
+
+use rstar::{AABB, PointDistance, RTree, RTreeObject};
+
+use crate::core::{Bounds, Point2D};
+use crate::features::Line2D;
+
+/// An indexed line segment for R-tree storage.
+#[derive(Clone, Debug)]
+pub struct IndexedLine {
+    /// The line segment.
+    pub line: Line2D,
+    /// Index of this line in the original collection.
+    pub index: usize,
+}
+
+impl IndexedLine {
+    /// Create a new indexed line.
+    pub fn new(line: Line2D, index: usize) -> Self {
+        Self { line, index }
+    }
+}
+
+impl RTreeObject for IndexedLine {
+    type Envelope = AABB<[f32; 2]>;
+
+    fn envelope(&self) -> Self::Envelope {
+        let min_x = self.line.start.x.min(self.line.end.x);
+        let max_x = self.line.start.x.max(self.line.end.x);
+        let min_y = self.line.start.y.min(self.line.end.y);
+        let max_y = self.line.start.y.max(self.line.end.y);
+
+        AABB::from_corners([min_x, min_y], [max_x, max_y])
+    }
+}
+
+impl PointDistance for IndexedLine {
+    fn distance_2(&self, point: &[f32; 2]) -> f32 {
+        let p = Point2D::new(point[0], point[1]);
+        let dist = self.line.distance_to_point(p);
+        dist * dist // Squared distance
+    }
+
+    fn contains_point(&self, _point: &[f32; 2]) -> bool {
+        false // Lines don't contain points (zero area)
+    }
+}
+
+/// Spatial index for line segments using R-tree.
+///
+/// Enables efficient spatial queries for large maps.
+///
+/// # Example
+/// ```rust
+/// use vastu_map::integration::SpatialIndex;
+/// use vastu_map::features::Line2D;
+/// use vastu_map::core::Point2D;
+///
+/// let lines = vec![
+///     Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(5.0, 0.0)),
+///     Line2D::new(Point2D::new(5.0, 0.0), Point2D::new(5.0, 5.0)),
+/// ];
+///
+/// let index = SpatialIndex::new(&lines);
+///
+/// // Find nearest line to a point
+/// if let Some((idx, dist)) = index.nearest_line(Point2D::new(3.0, 0.5)) {
+///     println!("Nearest line: {}, distance: {:.3}", idx, dist);
+/// }
+/// ```
+#[derive(Clone)]
+pub struct SpatialIndex {
+    tree: RTree<IndexedLine>,
+}
+
+impl SpatialIndex {
+    /// Create a new spatial index from lines.
+    pub fn new(lines: &[Line2D]) -> Self {
+        let indexed: Vec<IndexedLine> = lines
+            .iter()
+            .enumerate()
+            .map(|(i, line)| IndexedLine::new(*line, i))
+            .collect();
+
+        Self {
+            tree: RTree::bulk_load(indexed),
+        }
+    }
+
+    /// Create an empty spatial index.
+    pub fn empty() -> Self {
+        Self { tree: RTree::new() }
+    }
+
+    /// Number of lines in the index.
+    pub fn len(&self) -> usize {
+        self.tree.size()
+    }
+
+    /// Check if index is empty.
+    pub fn is_empty(&self) -> bool {
+        self.tree.size() == 0
+    }
+
+    /// Insert a line into the index.
+    ///
+    /// O(log n) insertion time.
+    pub fn insert(&mut self, line: Line2D, index: usize) {
+        self.tree.insert(IndexedLine::new(line, index));
+    }
+
+    /// Insert multiple lines into the index.
+    ///
+    /// For small batches (<20), uses incremental insert.
+    /// For large batches, triggers a rebuild for better tree balance.
+    ///
+    /// # Arguments
+    /// * `lines` - Slice of lines to insert
+    /// * `start_index` - Starting index for the new lines
+    pub fn insert_batch(&mut self, lines: &[Line2D], start_index: usize) {
+        // Threshold for incremental vs rebuild
+        const REBUILD_THRESHOLD: usize = 50;
+
+        if lines.len() < REBUILD_THRESHOLD {
+            // Incremental insert for small batches
+            for (i, line) in lines.iter().enumerate() {
+                self.tree.insert(IndexedLine::new(*line, start_index + i));
+            }
+        } else {
+            // For large batches, it's more efficient to rebuild
+            // Extract existing items and combine with new ones
+            let existing: Vec<IndexedLine> = self.tree.iter().cloned().collect();
+            let new: Vec<IndexedLine> = lines
+                .iter()
+                .enumerate()
+                .map(|(i, line)| IndexedLine::new(*line, start_index + i))
+                .collect();
+
+            let mut all = existing;
+            all.extend(new);
+            self.tree = RTree::bulk_load(all);
+        }
+    }
+
+    /// Find the nearest line to a point.
+    ///
+    /// Returns (index, distance) of the nearest line, or None if index is empty.
+    pub fn nearest_line(&self, point: Point2D) -> Option<(usize, f32)> {
+        let p = [point.x, point.y];
+        self.tree.nearest_neighbor(&p).map(|indexed| {
+            let dist = indexed.line.distance_to_point(point);
+            (indexed.index, dist)
+        })
+    }
+
+    /// Find the k nearest lines to a point.
+    ///
+    /// Returns vector of (index, distance) pairs, sorted by distance.
+    pub fn k_nearest_lines(&self, point: Point2D, k: usize) -> Vec<(usize, f32)> {
+        let p = [point.x, point.y];
+        self.tree
+            .nearest_neighbor_iter(&p)
+            .take(k)
+            .map(|indexed| {
+                let dist = indexed.line.distance_to_point(point);
+                (indexed.index, dist)
+            })
+            .collect()
+    }
+
+    /// Find all lines within a given distance of a point.
+    pub fn lines_within_distance(&self, point: Point2D, max_distance: f32) -> Vec<(usize, f32)> {
+        let p = [point.x, point.y];
+        let max_dist_sq = max_distance * max_distance;
+
+        self.tree
+            .nearest_neighbor_iter(&p)
+            .take_while(|indexed| {
+                let dist = indexed.line.distance_to_point(point);
+                dist * dist <= max_dist_sq
+            })
+            .map(|indexed| {
+                let dist = indexed.line.distance_to_point(point);
+                (indexed.index, dist)
+            })
+            .collect()
+    }
+
+    /// Find all lines intersecting a bounding box.
+    pub fn lines_in_bounds(&self, bounds: &Bounds) -> Vec<usize> {
+        let aabb = AABB::from_corners([bounds.min.x, bounds.min.y], [bounds.max.x, bounds.max.y]);
+
+        self.tree
+            .locate_in_envelope_intersecting(&aabb)
+            .map(|indexed| indexed.index)
+            .collect()
+    }
+
+    /// Find all lines that could potentially match a query line.
+    ///
+    /// Uses the query line's bounding box expanded by a margin.
+    pub fn potential_matches(&self, query: &Line2D, margin: f32) -> Vec<usize> {
+        let min_x = query.start.x.min(query.end.x) - margin;
+        let max_x = query.start.x.max(query.end.x) + margin;
+        let min_y = query.start.y.min(query.end.y) - margin;
+        let max_y = query.start.y.max(query.end.y) + margin;
+
+        let aabb = AABB::from_corners([min_x, min_y], [max_x, max_y]);
+
+        self.tree
+            .locate_in_envelope_intersecting(&aabb)
+            .map(|indexed| indexed.index)
+            .collect()
+    }
+
+    /// Rebuild the index with new lines.
+    pub fn rebuild(&mut self, lines: &[Line2D]) {
+        let indexed: Vec<IndexedLine> = lines
+            .iter()
+            .enumerate()
+            .map(|(i, line)| IndexedLine::new(*line, i))
+            .collect();
+
+        self.tree = RTree::bulk_load(indexed);
+    }
+
+    /// Clear the index.
+    pub fn clear(&mut self) {
+        self.tree = RTree::new();
+    }
+}
+
+impl Default for SpatialIndex {
+    fn default() -> Self {
+        Self::empty()
+    }
+}
+
+impl std::fmt::Debug for SpatialIndex {
+    fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result {
+        f.debug_struct("SpatialIndex")
+            .field("size", &self.tree.size())
+            .finish()
+    }
+}
+
+#[cfg(test)]
+mod tests {
+    use super::*;
+
+    fn make_test_lines() -> Vec<Line2D> {
+        vec![
+            Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(5.0, 0.0)), // Horizontal at y=0
+            Line2D::new(Point2D::new(5.0, 0.0), Point2D::new(5.0, 5.0)), // Vertical at x=5
+            Line2D::new(Point2D::new(5.0, 5.0), Point2D::new(0.0, 5.0)), // Horizontal at y=5
+            Line2D::new(Point2D::new(0.0, 5.0), Point2D::new(0.0, 0.0)), // Vertical at x=0
+            Line2D::new(Point2D::new(10.0, 0.0), Point2D::new(15.0, 0.0)), // Far horizontal
+        ]
+    }
+
+    #[test]
+    fn test_new_index() {
+        let lines = make_test_lines();
+        let index = SpatialIndex::new(&lines);
+
+        assert_eq!(index.len(), 5);
+        assert!(!index.is_empty());
+    }
+
+    #[test]
+    fn test_empty_index() {
+        let index = SpatialIndex::empty();
+
+        assert_eq!(index.len(), 0);
+        assert!(index.is_empty());
+        assert!(index.nearest_line(Point2D::zero()).is_none());
+    }
+
+    #[test]
+    fn test_nearest_line() {
+        let lines = make_test_lines();
+        let index = SpatialIndex::new(&lines);
+
+        // Point near bottom line
+        let (idx, dist) = index.nearest_line(Point2D::new(2.5, 0.5)).unwrap();
+        assert_eq!(idx, 0);
+        assert!((dist - 0.5).abs() < 0.01);
+
+        // Point near right line
+        let (idx, dist) = index.nearest_line(Point2D::new(4.5, 2.5)).unwrap();
+        assert_eq!(idx, 1);
+        assert!((dist - 0.5).abs() < 0.01);
+    }
+
+    #[test]
+    fn test_k_nearest() {
+        let lines = make_test_lines();
+        let index = SpatialIndex::new(&lines);
+
+        // Point at corner (near lines 0 and 3)
+        let nearest = index.k_nearest_lines(Point2D::new(0.5, 0.5), 2);
+
+        assert_eq!(nearest.len(), 2);
+        // Both should be at similar distances
+        assert!(nearest[0].1 < 1.0);
+        assert!(nearest[1].1 < 1.0);
+    }
+
+    #[test]
+    fn test_lines_within_distance() {
+        let lines = make_test_lines();
+        let index = SpatialIndex::new(&lines);
+
+        // Point at center of the room
+        let nearby = index.lines_within_distance(Point2D::new(2.5, 2.5), 3.0);
+
+        // Should find at least the 4 room walls (each at distance 2.5)
+        // The far line is at distance ~7.9 so should not be included
+        assert!(nearby.len() >= 4);
+        // Verify all returned distances are within threshold
+        for (_, dist) in &nearby {
+            assert!(*dist <= 3.0, "Distance {} exceeds threshold", dist);
+        }
+    }
+
+    #[test]
+    fn test_lines_in_bounds() {
+        let lines = make_test_lines();
+        let index = SpatialIndex::new(&lines);
+
+        // Bounds covering just the room
+        let bounds = Bounds::new(Point2D::new(-1.0, -1.0), Point2D::new(6.0, 6.0));
+        let in_bounds = index.lines_in_bounds(&bounds);
+
+        // Should find the 4 room walls
+        assert_eq!(in_bounds.len(), 4);
+
+        // Bounds covering everything
+        let big_bounds = Bounds::new(Point2D::new(-1.0, -1.0), Point2D::new(20.0, 10.0));
+        let all = index.lines_in_bounds(&big_bounds);
+        assert_eq!(all.len(), 5);
+    }
+
+    #[test]
+    fn test_potential_matches() {
+        let lines = make_test_lines();
+        let index = SpatialIndex::new(&lines);
+
+        // Query line similar to the bottom wall
+        let query = Line2D::new(Point2D::new(1.0, 0.1), Point2D::new(4.0, 0.1));
+        let matches = index.potential_matches(&query, 0.5);
+
+        // Should find the bottom wall
+        assert!(matches.contains(&0));
+    }
+
+    #[test]
+    fn test_insert() {
+        let mut index = SpatialIndex::empty();
+
+        index.insert(
+            Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(1.0, 0.0)),
+            0,
+        );
+        index.insert(
+            Line2D::new(Point2D::new(0.0, 1.0), Point2D::new(1.0, 1.0)),
+            1,
+        );
+
+        assert_eq!(index.len(), 2);
+
+        let (idx, _) = index.nearest_line(Point2D::new(0.5, 0.1)).unwrap();
+        assert_eq!(idx, 0);
+    }
+
+    #[test]
+    fn test_rebuild() {
+        let lines1 = vec![Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(1.0, 0.0))];
+        let lines2 = vec![
+            Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(1.0, 0.0)),
+            Line2D::new(Point2D::new(0.0, 1.0), Point2D::new(1.0, 1.0)),
+        ];
+
+        let mut index = SpatialIndex::new(&lines1);
+        assert_eq!(index.len(), 1);
+
+        index.rebuild(&lines2);
+        assert_eq!(index.len(), 2);
+    }
+
+    #[test]
+    fn test_clear() {
+        let lines = make_test_lines();
+        let mut index = SpatialIndex::new(&lines);
+
+        assert_eq!(index.len(), 5);
+
+        index.clear();
+        assert!(index.is_empty());
+    }
+}

vastu-map/src/lib.rs

@@ -0,0 +1,277 @@
+//! # Vastu-Map: Feature-Based 2D SLAM Library
+//!
+//! A high-performance SLAM (Simultaneous Localization and Mapping) library
+//! optimized for ARM NEON and designed for indoor robot navigation.
+//!
+//! ## Features
+//!
+//! - **VectorMap Representation**: Uses line segments and corners instead of
+//!   occupancy grids for memory-efficient mapping
+//! - **Full SLAM Pipeline**: Localization and mapping integrated into a clean API
+//! - **SIMD Optimized**: Designed for ARM NEON auto-vectorization (also works on x86)
+//! - **SoA Data Layout**: Struct-of-Arrays for cache-friendly SIMD operations
+//!
+//! ## Quick Start
+//!
+//! ```rust,no_run
+//! use vastu_map::{VectorMap, VectorMapConfig, Map};
+//! use vastu_map::core::{Pose2D, PointCloud2D, Point2D};
+//!
+//! // Create map with default configuration
+//! let config = VectorMapConfig::default();
+//! let mut map = VectorMap::new(config);
+//!
+//! // Process a scan observation
+//! let scan = PointCloud2D::from_points(&[
+//!     Point2D::new(1.0, 0.0),
+//!     Point2D::new(1.0, 0.1),
+//!     Point2D::new(1.0, 0.2),
+//! ]);
+//! let odometry = Pose2D::identity();
+//! let result = map.observe(&scan, odometry);
+//! println!("Pose: ({:.2}, {:.2}), confidence: {:.2}",
+//!     result.pose.x, result.pose.y, result.confidence);
+//! ```
+//!
+//! ## Coordinate Frame
+//!
+//! All coordinates follow the ROS REP-103 convention:
+//! - **X-forward**: Positive X is in front of the robot
+//! - **Y-left**: Positive Y is to the left of the robot
+//! - **Z-up**: Positive Z is upward (not used in 2D)
+//! - **Rotation**: Counter-clockwise positive
+//!
+//! ## Architecture
+//!
+//! The library is organized into modules:
+//!
+//! - [`core`]: Fundamental types (Point2D, Pose2D, PointCloud2D, etc.)
+//! - [`simd`]: SIMD primitives for auto-vectorization
+//! - [`config`]: Configuration types
+//! - [`features`]: Line and corner feature types
+//! - [`extraction`]: Line extraction from point clouds
+//! - [`matching`]: Scan-to-map matching algorithms (ICP, RANSAC)
+//! - [`integration`]: Map integration and merging
+//! - [`query`]: Map queries (raycast, occupancy, frontiers)
+//! - [`vector_map`]: Main VectorMap implementation
+//!
+//! ## Target Platform
+//!
+//! Primary target: ARM Cortex-A7 with NEON and VFPv4 (Allwinner A33).
+//! The library uses auto-vectorization patterns that LLVM can optimize
+//! for various SIMD architectures.
+
+pub mod config;
+pub mod core;
+pub mod extraction;
+pub mod features;
+pub mod integration;
+pub mod matching;
+pub mod query;
+pub mod simd;
+pub mod vector_map;
+
+// Re-export main types at crate root
+pub use vector_map::{VectorMap, VectorMapConfig};
+
+// ─────────────────────────────────────────────────────────────────────────────
+// Map Trait and Supporting Types
+// ─────────────────────────────────────────────────────────────────────────────
+
+use core::{Bounds, Point2D, PointCloud2D, Pose2D};
+
+/// Point occupancy state.
+#[derive(Debug, Clone, Copy, PartialEq, Eq)]
+pub enum Occupancy {
+    /// Point is in navigable free space.
+    Free,
+    /// Point is on or inside an obstacle.
+    Occupied,
+    /// Point has not been observed yet.
+    Unknown,
+}
+
+/// Exploration frontier (unconnected line endpoint).
+#[derive(Debug, Clone)]
+pub struct Frontier {
+    /// Position of the frontier point.
+    pub point: Point2D,
+    /// Index of the line this endpoint belongs to.
+    pub line_idx: usize,
+    /// True if this is the start endpoint, false if end.
+    pub is_start: bool,
+}
+
+/// A planned path through the map.
+#[derive(Debug, Clone)]
+pub struct Path {
+    /// Waypoints along the path.
+    pub points: Vec<Point2D>,
+    /// Total path length in meters.
+    pub length: f32,
+}
+
+impl Path {
+    /// Create a new empty path.
+    pub fn new() -> Self {
+        Self {
+            points: Vec::new(),
+            length: 0.0,
+        }
+    }
+
+    /// Check if the path is empty.
+    pub fn is_empty(&self) -> bool {
+        self.points.is_empty()
+    }
+}
+
+impl Default for Path {
+    fn default() -> Self {
+        Self::new()
+    }
+}
+
+/// Result of observing a scan.
+#[derive(Debug, Clone)]
+pub struct ObserveResult {
+    /// Localized pose in world frame.
+    pub pose: Pose2D,
+    /// Match confidence (0.0 = used odometry, 1.0 = excellent match).
+    pub confidence: f32,
+    /// Number of features extracted from scan.
+    pub features_extracted: usize,
+    /// Number of new features added to map.
+    pub features_added: usize,
+    /// Number of features merged with existing map features.
+    pub features_merged: usize,
+}
+
+impl ObserveResult {
+    /// Create a result with just pose and confidence.
+    pub fn from_pose(pose: Pose2D, confidence: f32) -> Self {
+        Self {
+            pose,
+            confidence,
+            features_extracted: 0,
+            features_added: 0,
+            features_merged: 0,
+        }
+    }
+}
+
+/// High-level map interface.
+///
+/// Encapsulates localization, mapping, and query algorithms.
+/// All coordinates in meters, all angles in radians.
+///
+/// # Usage
+///
+/// ```rust,ignore
+/// use vastu_map::{Map, core::{PointCloud2D, Pose2D, Point2D}};
+///
+/// fn robot_loop<M: Map>(map: &mut M, get_scan: impl Fn() -> PointCloud2D) {
+///     let goal = Point2D::new(5.0, 3.0);
+///
+///     loop {
+///         let scan = get_scan();
+///         let odom = Pose2D::identity(); // Get from odometry
+///
+///         // Observe and localize
+///         let result = map.observe(&scan, odom);
+///         println!("Position: ({:.2}, {:.2}), confidence: {:.2}",
+///             result.pose.x, result.pose.y, result.confidence);
+///
+///         // Plan path to goal
+///         if let Some(path) = map.get_path(result.pose.position(), goal) {
+///             println!("Path found: {} waypoints, {:.2}m", path.points.len(), path.length);
+///         }
+///     }
+/// }
+/// ```
+pub trait Map: Send + Sync {
+    /// Feed sensor observation and get localized pose.
+    ///
+    /// This is the main SLAM function. It:
+    /// 1. Extracts features from the scan
+    /// 2. Matches features against the map (localization)
+    /// 3. Integrates new/updated features (mapping)
+    ///
+    /// # Arguments
+    /// * `scan` - Point cloud in robot frame
+    /// * `odometry` - Odometry estimate (used as initial guess and fallback)
+    ///
+    /// # Returns
+    /// Observation result including localized pose and confidence.
+    /// On match failure, returns odometry pose with low confidence.
+    fn observe(&mut self, scan: &PointCloud2D, odometry: Pose2D) -> ObserveResult;
+
+    /// Cast a ray and return distance to first obstacle.
+    ///
+    /// # Arguments
+    /// * `from` - Ray origin in world frame
+    /// * `direction` - Ray direction (will be normalized internally)
+    /// * `max_range` - Maximum ray distance
+    ///
+    /// # Returns
+    /// Distance to first intersection, or `max_range` if no intersection.
+    fn raycast(&self, from: Point2D, direction: Point2D, max_range: f32) -> f32;
+
+    /// Query point occupancy.
+    ///
+    /// Uses contour-based occupancy (like font outlines):
+    /// - Inside navigable contour → Free
+    /// - On or outside boundary → Occupied
+    /// - Not yet mapped → Unknown
+    fn query(&self, point: Point2D) -> Occupancy;
+
+    /// Get exploration frontiers (unconnected line endpoints).
+    ///
+    /// Frontiers are line endpoints that don't connect to other lines,
+    /// indicating unexplored areas.
+    fn frontiers(&self) -> Vec<Frontier>;
+
+    /// Plan path between two points.
+    ///
+    /// # Returns
+    /// Path if one exists, None otherwise.
+    /// Note: Path planning is deferred in initial implementation.
+    fn get_path(&self, from: Point2D, to: Point2D) -> Option<Path>;
+
+    /// Get current map bounds.
+    ///
+    /// Returns the axis-aligned bounding box containing all map features.
+    /// The bounds grow dynamically as the robot explores.
+    fn bounds(&self) -> Bounds;
+
+    /// Clear all map data.
+    fn clear(&mut self);
+}
+
+#[cfg(test)]
+mod tests {
+    use super::*;
+
+    #[test]
+    fn test_occupancy_debug() {
+        assert_eq!(format!("{:?}", Occupancy::Free), "Free");
+        assert_eq!(format!("{:?}", Occupancy::Occupied), "Occupied");
+        assert_eq!(format!("{:?}", Occupancy::Unknown), "Unknown");
+    }
+
+    #[test]
+    fn test_path_new() {
+        let path = Path::new();
+        assert!(path.is_empty());
+        assert_eq!(path.length, 0.0);
+    }
+
+    #[test]
+    fn test_observe_result() {
+        let result = ObserveResult::from_pose(Pose2D::new(1.0, 2.0, 0.5), 0.8);
+        assert_eq!(result.pose.x, 1.0);
+        assert_eq!(result.pose.y, 2.0);
+        assert_eq!(result.confidence, 0.8);
+        assert_eq!(result.features_extracted, 0);
+    }
+}

vastu-map/src/matching/correspondence.rs

@@ -0,0 +1,321 @@
+//! Correspondence types for scan matching.
+//!
+//! A correspondence associates a point from the current scan with
+//! a line in the map.
+
+use crate::core::Point2D;
+
+/// A correspondence between a scan point and a map line.
+#[derive(Clone, Copy, Debug)]
+pub struct Correspondence {
+    /// Index of the point in the scan.
+    pub point_idx: usize,
+    /// Index of the line in the map.
+    pub line_idx: usize,
+    /// The scan point position (in world frame after initial transform).
+    pub point: Point2D,
+    /// Perpendicular distance from point to line.
+    pub distance: f32,
+    /// Projection parameter t along the line (0 = start, 1 = end).
+    pub projection_t: f32,
+}
+
+impl Correspondence {
+    /// Create a new correspondence.
+    #[inline]
+    pub fn new(
+        point_idx: usize,
+        line_idx: usize,
+        point: Point2D,
+        distance: f32,
+        projection_t: f32,
+    ) -> Self {
+        Self {
+            point_idx,
+            line_idx,
+            point,
+            distance,
+            projection_t,
+        }
+    }
+
+    /// Check if the projection falls within the line segment.
+    #[inline]
+    pub fn is_within_segment(&self) -> bool {
+        self.projection_t >= 0.0 && self.projection_t <= 1.0
+    }
+
+    /// Check if this is a valid correspondence (within distance threshold).
+    #[inline]
+    pub fn is_valid(&self, max_distance: f32) -> bool {
+        self.distance <= max_distance
+    }
+}
+
+/// Collection of correspondences for a scan match.
+#[derive(Clone, Debug, Default)]
+pub struct CorrespondenceSet {
+    /// All correspondences found.
+    pub correspondences: Vec<Correspondence>,
+}
+
+impl CorrespondenceSet {
+    /// Create a new empty correspondence set.
+    pub fn new() -> Self {
+        Self::default()
+    }
+
+    /// Create with capacity.
+    pub fn with_capacity(capacity: usize) -> Self {
+        Self {
+            correspondences: Vec::with_capacity(capacity),
+        }
+    }
+
+    /// Add a correspondence.
+    #[inline]
+    pub fn push(&mut self, corr: Correspondence) {
+        self.correspondences.push(corr);
+    }
+
+    /// Number of correspondences.
+    #[inline]
+    pub fn len(&self) -> usize {
+        self.correspondences.len()
+    }
+
+    /// Check if empty.
+    #[inline]
+    pub fn is_empty(&self) -> bool {
+        self.correspondences.is_empty()
+    }
+
+    /// Filter correspondences by maximum distance.
+    pub fn filter_by_distance(&self, max_distance: f32) -> Self {
+        Self {
+            correspondences: self
+                .correspondences
+                .iter()
+                .filter(|c| c.distance <= max_distance)
+                .copied()
+                .collect(),
+        }
+    }
+
+    /// Filter correspondences to only those within line segments.
+    pub fn filter_within_segment(&self) -> Self {
+        Self {
+            correspondences: self
+                .correspondences
+                .iter()
+                .filter(|c| c.is_within_segment())
+                .copied()
+                .collect(),
+        }
+    }
+
+    /// Get the mean correspondence distance.
+    pub fn mean_distance(&self) -> f32 {
+        if self.is_empty() {
+            return 0.0;
+        }
+        let sum: f32 = self.correspondences.iter().map(|c| c.distance).sum();
+        sum / self.len() as f32
+    }
+
+    /// Get the RMS (root mean square) correspondence distance.
+    pub fn rms_distance(&self) -> f32 {
+        if self.is_empty() {
+            return 0.0;
+        }
+        let sum_sq: f32 = self
+            .correspondences
+            .iter()
+            .map(|c| c.distance * c.distance)
+            .sum();
+        (sum_sq / self.len() as f32).sqrt()
+    }
+
+    /// Get the maximum correspondence distance.
+    pub fn max_distance(&self) -> f32 {
+        self.correspondences
+            .iter()
+            .map(|c| c.distance)
+            .fold(0.0, f32::max)
+    }
+
+    /// Compute inlier ratio (correspondences within threshold).
+    pub fn inlier_ratio(&self, threshold: f32) -> f32 {
+        if self.is_empty() {
+            return 0.0;
+        }
+        let inliers = self
+            .correspondences
+            .iter()
+            .filter(|c| c.distance <= threshold)
+            .count();
+        inliers as f32 / self.len() as f32
+    }
+
+    /// Clear all correspondences.
+    pub fn clear(&mut self) {
+        self.correspondences.clear();
+    }
+
+    /// Iterate over correspondences.
+    pub fn iter(&self) -> impl Iterator<Item = &Correspondence> {
+        self.correspondences.iter()
+    }
+}
+
+/// Result of a scan matching operation.
+#[derive(Clone, Debug)]
+pub struct MatchResult {
+    /// Estimated pose transformation.
+    pub pose: crate::core::Pose2D,
+    /// Correspondences used in the final match.
+    pub correspondences: CorrespondenceSet,
+    /// Number of iterations performed.
+    pub iterations: usize,
+    /// Whether the matching converged.
+    pub converged: bool,
+    /// Final RMS error.
+    pub rms_error: f32,
+    /// Match confidence (0.0 to 1.0).
+    pub confidence: f32,
+}
+
+impl MatchResult {
+    /// Create a new match result.
+    pub fn new(
+        pose: crate::core::Pose2D,
+        correspondences: CorrespondenceSet,
+        iterations: usize,
+        converged: bool,
+    ) -> Self {
+        let rms_error = correspondences.rms_distance();
+        let confidence = Self::compute_confidence(&correspondences, converged);
+
+        Self {
+            pose,
+            correspondences,
+            iterations,
+            converged,
+            rms_error,
+            confidence,
+        }
+    }
+
+    /// Compute match confidence based on various factors.
+    fn compute_confidence(correspondences: &CorrespondenceSet, converged: bool) -> f32 {
+        if !converged || correspondences.is_empty() {
+            return 0.0;
+        }
+
+        // Factors affecting confidence:
+        // 1. Number of correspondences (more is better)
+        // 2. Mean distance (lower is better)
+        // 3. Inlier ratio (higher is better)
+
+        let n = correspondences.len() as f32;
+        let count_factor = (n / 50.0).min(1.0); // Saturates at 50 correspondences
+
+        let mean_dist = correspondences.mean_distance();
+        let dist_factor = (-mean_dist / 0.1).exp(); // Exponential decay, 0.1m = ~37%
+
+        let inlier_ratio = correspondences.inlier_ratio(0.1);
+
+        // Combine factors
+        (count_factor * dist_factor * inlier_ratio).clamp(0.0, 1.0)
+    }
+
+    /// Check if this is a good match.
+    pub fn is_good_match(&self, min_confidence: f32) -> bool {
+        self.converged && self.confidence >= min_confidence
+    }
+}
+
+#[cfg(test)]
+mod tests {
+    use super::*;
+    use crate::core::Pose2D;
+
+    #[test]
+    fn test_correspondence_new() {
+        let corr = Correspondence::new(0, 1, Point2D::new(1.0, 2.0), 0.05, 0.5);
+
+        assert_eq!(corr.point_idx, 0);
+        assert_eq!(corr.line_idx, 1);
+        assert_eq!(corr.distance, 0.05);
+        assert!(corr.is_within_segment());
+    }
+
+    #[test]
+    fn test_correspondence_within_segment() {
+        let inside = Correspondence::new(0, 0, Point2D::zero(), 0.0, 0.5);
+        assert!(inside.is_within_segment());
+
+        let at_start = Correspondence::new(0, 0, Point2D::zero(), 0.0, 0.0);
+        assert!(at_start.is_within_segment());
+
+        let at_end = Correspondence::new(0, 0, Point2D::zero(), 0.0, 1.0);
+        assert!(at_end.is_within_segment());
+
+        let outside = Correspondence::new(0, 0, Point2D::zero(), 0.0, 1.5);
+        assert!(!outside.is_within_segment());
+    }
+
+    #[test]
+    fn test_correspondence_set_statistics() {
+        let mut set = CorrespondenceSet::new();
+        set.push(Correspondence::new(0, 0, Point2D::zero(), 0.1, 0.5));
+        set.push(Correspondence::new(1, 0, Point2D::zero(), 0.2, 0.5));
+        set.push(Correspondence::new(2, 0, Point2D::zero(), 0.3, 0.5));
+
+        assert_eq!(set.len(), 3);
+        assert!((set.mean_distance() - 0.2).abs() < 1e-6);
+        assert_eq!(set.max_distance(), 0.3);
+
+        // RMS = sqrt((0.01 + 0.04 + 0.09) / 3) = sqrt(0.14/3) ≈ 0.216
+        assert!((set.rms_distance() - 0.216).abs() < 0.01);
+    }
+
+    #[test]
+    fn test_correspondence_set_filter() {
+        let mut set = CorrespondenceSet::new();
+        set.push(Correspondence::new(0, 0, Point2D::zero(), 0.05, 0.5));
+        set.push(Correspondence::new(1, 0, Point2D::zero(), 0.15, 0.5));
+        set.push(Correspondence::new(2, 0, Point2D::zero(), 0.25, 0.5));
+
+        let filtered = set.filter_by_distance(0.1);
+        assert_eq!(filtered.len(), 1);
+    }
+
+    #[test]
+    fn test_correspondence_set_inlier_ratio() {
+        let mut set = CorrespondenceSet::new();
+        set.push(Correspondence::new(0, 0, Point2D::zero(), 0.05, 0.5));
+        set.push(Correspondence::new(1, 0, Point2D::zero(), 0.05, 0.5));
+        set.push(Correspondence::new(2, 0, Point2D::zero(), 0.15, 0.5));
+        set.push(Correspondence::new(3, 0, Point2D::zero(), 0.25, 0.5));
+
+        let ratio = set.inlier_ratio(0.1);
+        assert!((ratio - 0.5).abs() < 1e-6); // 2 out of 4
+    }
+
+    #[test]
+    fn test_match_result() {
+        let mut corr = CorrespondenceSet::new();
+        for i in 0..50 {
+            corr.push(Correspondence::new(i, 0, Point2D::zero(), 0.02, 0.5));
+        }
+
+        let result = MatchResult::new(Pose2D::identity(), corr, 10, true);
+
+        assert!(result.converged);
+        // With 50 correspondences at 0.02m: count_factor=1.0, dist_factor≈0.82, inlier_ratio=1.0
+        // confidence ≈ 0.82
+        assert!(result.confidence > 0.5);
+        assert!(result.is_good_match(0.3));
+    }
+}

vastu-map/src/matching/gauss_newton.rs

@@ -0,0 +1,649 @@
+//! Gauss-Newton solver for point-to-line ICP.
+//!
+//! Solves the nonlinear least squares problem of finding the pose
+//! that minimizes the sum of squared point-to-line distances.
+//!
+//! # Performance
+//!
+//! Two versions are provided:
+//! - `optimize_pose`: Takes `&[Line2D]`, computes normals on-the-fly
+//! - `optimize_pose_fast`: Takes `&LineCollection` with pre-computed normals
+//!
+//! Use `optimize_pose_fast` in hot paths for ~15-20% speedup by avoiding
+//! redundant sqrt computations.
+
+use crate::core::{Point2D, Pose2D};
+use crate::features::{Line2D, LineCollection};
+
+use super::correspondence::CorrespondenceSet;
+
+/// Configuration for Gauss-Newton solver.
+#[derive(Clone, Debug)]
+pub struct GaussNewtonConfig {
+    /// Maximum number of iterations.
+    /// Default: 20
+    pub max_iterations: usize,
+
+    /// Convergence threshold for pose change.
+    /// If ‖Δpose‖ < threshold, iteration stops.
+    /// Default: 1e-6
+    pub convergence_threshold: f32,
+
+    /// Minimum eigenvalue ratio for matrix conditioning.
+    /// If smallest/largest eigenvalue < ratio, add regularization.
+    /// Default: 1e-6
+    pub min_eigenvalue_ratio: f32,
+
+    /// Regularization factor for ill-conditioned systems.
+    /// Default: 1e-4
+    pub regularization: f32,
+}
+
+impl Default for GaussNewtonConfig {
+    fn default() -> Self {
+        Self {
+            max_iterations: 20,
+            convergence_threshold: 1e-6,
+            min_eigenvalue_ratio: 1e-6,
+            regularization: 1e-4,
+        }
+    }
+}
+
+impl GaussNewtonConfig {
+    /// Create a new configuration with default values.
+    pub fn new() -> Self {
+        Self::default()
+    }
+
+    /// Builder-style setter for maximum iterations.
+    pub fn with_max_iterations(mut self, iterations: usize) -> Self {
+        self.max_iterations = iterations;
+        self
+    }
+
+    /// Builder-style setter for convergence threshold.
+    pub fn with_convergence_threshold(mut self, threshold: f32) -> Self {
+        self.convergence_threshold = threshold;
+        self
+    }
+}
+
+/// Result of Gauss-Newton optimization.
+#[derive(Clone, Debug)]
+pub struct GaussNewtonResult {
+    /// Optimized pose.
+    pub pose: Pose2D,
+    /// Number of iterations performed.
+    pub iterations: usize,
+    /// Final residual (RMS error).
+    pub residual: f32,
+    /// Whether optimization converged.
+    pub converged: bool,
+}
+
+/// Optimize pose using Gauss-Newton method for point-to-line ICP.
+///
+/// Minimizes: sum_i (n_i · (R(θ)·p_i + t - q_i))²
+///
+/// where:
+/// - p_i is the i-th scan point
+/// - q_i is the closest point on line i
+/// - n_i is the normal of line i
+/// - R(θ), t is the rotation and translation
+///
+/// # Arguments
+/// * `correspondences` - Point-to-line correspondences
+/// * `lines` - Map lines
+/// * `initial_pose` - Initial pose estimate
+/// * `config` - Solver configuration
+///
+/// # Returns
+/// Optimization result with final pose and convergence info.
+pub fn optimize_pose(
+    correspondences: &CorrespondenceSet,
+    lines: &[Line2D],
+    initial_pose: Pose2D,
+    config: &GaussNewtonConfig,
+) -> GaussNewtonResult {
+    if correspondences.is_empty() {
+        return GaussNewtonResult {
+            pose: initial_pose,
+            iterations: 0,
+            residual: 0.0,
+            converged: true,
+        };
+    }
+
+    let mut pose = initial_pose;
+    let mut converged = false;
+    let mut iterations = 0;
+
+    for iter in 0..config.max_iterations {
+        iterations = iter + 1;
+
+        // Build linear system: H·Δx = -g
+        // H = J^T·J (Hessian approximation)
+        // g = J^T·r (gradient)
+        let (h, g, residual) = build_linear_system(correspondences, lines, &pose);
+
+        // Solve for Δx using direct 3x3 solve
+        let Some(delta) = solve_3x3(&h, &g, config.regularization) else {
+            // System is singular - stop iteration
+            break;
+        };
+
+        // Update pose
+        pose.x += delta[0];
+        pose.y += delta[1];
+        pose.theta += delta[2];
+        pose.theta = crate::core::math::normalize_angle(pose.theta);
+
+        // Check convergence
+        let delta_norm = (delta[0] * delta[0] + delta[1] * delta[1] + delta[2] * delta[2]).sqrt();
+        if delta_norm < config.convergence_threshold {
+            converged = true;
+            break;
+        }
+
+        // Also check if residual is very small
+        if residual < config.convergence_threshold {
+            converged = true;
+            break;
+        }
+    }
+
+    // Compute final residual
+    let final_residual = compute_residual(correspondences, lines, &pose);
+
+    GaussNewtonResult {
+        pose,
+        iterations,
+        residual: final_residual,
+        converged,
+    }
+}
+
+/// Build the linear system for Gauss-Newton.
+///
+/// Returns (H, g, residual) where:
+/// - H is the 3x3 Hessian approximation (J^T·J)
+/// - g is the 3-element gradient (J^T·r)
+/// - residual is the RMS error
+fn build_linear_system(
+    correspondences: &CorrespondenceSet,
+    lines: &[Line2D],
+    pose: &Pose2D,
+) -> ([[f32; 3]; 3], [f32; 3], f32) {
+    let (sin, cos) = pose.theta.sin_cos();
+
+    // Initialize H and g to zero
+    let mut h = [[0.0f32; 3]; 3];
+    let mut g = [0.0f32; 3];
+    let mut sum_sq_error = 0.0f32;
+    let mut count = 0;
+
+    for corr in correspondences.iter() {
+        let p = corr.point;
+        let line = &lines[corr.line_idx];
+
+        // Transform point by current pose
+        let tx = p.x * cos - p.y * sin + pose.x;
+        let ty = p.x * sin + p.y * cos + pose.y;
+        let transformed = Point2D::new(tx, ty);
+
+        // Get line normal
+        let normal = line.normal();
+
+        // Compute residual: signed distance from transformed point to line
+        let residual = line.signed_distance_to_point(transformed);
+        sum_sq_error += residual * residual;
+        count += 1;
+
+        // Jacobian of residual w.r.t. pose (x, y, theta)
+        // r = n · (R(θ)·p + t - q)
+        // ∂r/∂x = n_x
+        // ∂r/∂y = n_y
+        // ∂r/∂θ = n · ∂(R(θ)·p)/∂θ = n · [-p_x·sin(θ) - p_y·cos(θ), p_x·cos(θ) - p_y·sin(θ)]
+        //       = n_x·(-p_x·sin - p_y·cos) + n_y·(p_x·cos - p_y·sin)
+
+        let j0 = normal.x;
+        let j1 = normal.y;
+        let j2 = normal.x * (-p.x * sin - p.y * cos) + normal.y * (p.x * cos - p.y * sin);
+
+        let jacobian = [j0, j1, j2];
+
+        // Accumulate H = J^T·J
+        for i in 0..3 {
+            for j in 0..3 {
+                h[i][j] += jacobian[i] * jacobian[j];
+            }
+        }
+
+        // Accumulate g = J^T·r
+        for i in 0..3 {
+            g[i] += jacobian[i] * residual;
+        }
+    }
+
+    let rms_error = if count > 0 {
+        (sum_sq_error / count as f32).sqrt()
+    } else {
+        0.0
+    };
+
+    (h, g, rms_error)
+}
+
+/// Solve a 3x3 linear system Ax = b using Cramer's rule.
+///
+/// Returns None if the matrix is singular.
+fn solve_3x3(a: &[[f32; 3]; 3], b: &[f32; 3], regularization: f32) -> Option<[f32; 3]> {
+    // Add regularization to diagonal
+    let mut ar = *a;
+    ar[0][0] += regularization;
+    ar[1][1] += regularization;
+    ar[2][2] += regularization;
+
+    // Compute determinant
+    let det = ar[0][0] * (ar[1][1] * ar[2][2] - ar[1][2] * ar[2][1])
+        - ar[0][1] * (ar[1][0] * ar[2][2] - ar[1][2] * ar[2][0])
+        + ar[0][2] * (ar[1][0] * ar[2][1] - ar[1][1] * ar[2][0]);
+
+    if det.abs() < 1e-10 {
+        return None;
+    }
+
+    let inv_det = 1.0 / det;
+
+    // Compute inverse using cofactors
+    let inv = [
+        [
+            (ar[1][1] * ar[2][2] - ar[1][2] * ar[2][1]) * inv_det,
+            (ar[0][2] * ar[2][1] - ar[0][1] * ar[2][2]) * inv_det,
+            (ar[0][1] * ar[1][2] - ar[0][2] * ar[1][1]) * inv_det,
+        ],
+        [
+            (ar[1][2] * ar[2][0] - ar[1][0] * ar[2][2]) * inv_det,
+            (ar[0][0] * ar[2][2] - ar[0][2] * ar[2][0]) * inv_det,
+            (ar[0][2] * ar[1][0] - ar[0][0] * ar[1][2]) * inv_det,
+        ],
+        [
+            (ar[1][0] * ar[2][1] - ar[1][1] * ar[2][0]) * inv_det,
+            (ar[0][1] * ar[2][0] - ar[0][0] * ar[2][1]) * inv_det,
+            (ar[0][0] * ar[1][1] - ar[0][1] * ar[1][0]) * inv_det,
+        ],
+    ];
+
+    // x = A^(-1) · (-b)  (negative because we want to minimize)
+    Some([
+        -(inv[0][0] * b[0] + inv[0][1] * b[1] + inv[0][2] * b[2]),
+        -(inv[1][0] * b[0] + inv[1][1] * b[1] + inv[1][2] * b[2]),
+        -(inv[2][0] * b[0] + inv[2][1] * b[1] + inv[2][2] * b[2]),
+    ])
+}
+
+/// Compute RMS residual for current pose.
+fn compute_residual(correspondences: &CorrespondenceSet, lines: &[Line2D], pose: &Pose2D) -> f32 {
+    if correspondences.is_empty() {
+        return 0.0;
+    }
+
+    let (sin, cos) = pose.theta.sin_cos();
+    let mut sum_sq = 0.0;
+
+    for corr in correspondences.iter() {
+        let p = corr.point;
+        let line = &lines[corr.line_idx];
+
+        // Transform point
+        let tx = p.x * cos - p.y * sin + pose.x;
+        let ty = p.x * sin + p.y * cos + pose.y;
+        let transformed = Point2D::new(tx, ty);
+
+        // Compute distance
+        let dist = line.distance_to_point(transformed);
+        sum_sq += dist * dist;
+    }
+
+    (sum_sq / correspondences.len() as f32).sqrt()
+}
+
+// ============================================================================
+// Fast versions using pre-computed LineCollection normals
+// ============================================================================
+
+/// Optimize pose using pre-computed normals (faster hot-path version).
+///
+/// This version uses `LineCollection` with pre-computed normals and inverse
+/// lengths, avoiding redundant sqrt computations in the inner loop.
+///
+/// # Performance
+///
+/// ~15-20% faster than `optimize_pose` for typical correspondence sets.
+pub fn optimize_pose_fast(
+    correspondences: &CorrespondenceSet,
+    lines: &LineCollection,
+    initial_pose: Pose2D,
+    config: &GaussNewtonConfig,
+) -> GaussNewtonResult {
+    if correspondences.is_empty() {
+        return GaussNewtonResult {
+            pose: initial_pose,
+            iterations: 0,
+            residual: 0.0,
+            converged: true,
+        };
+    }
+
+    let mut pose = initial_pose;
+    let mut converged = false;
+    let mut iterations = 0;
+
+    // Pre-fetch normals for all lines used in correspondences
+    let (normal_xs, normal_ys) = lines.normals();
+    let inv_lengths = lines.inv_lengths();
+
+    for iter in 0..config.max_iterations {
+        iterations = iter + 1;
+
+        // Build linear system using pre-computed normals
+        let (h, g, residual) = build_linear_system_fast(
+            correspondences,
+            lines,
+            normal_xs,
+            normal_ys,
+            inv_lengths,
+            &pose,
+        );
+
+        // Solve for Δx using direct 3x3 solve
+        let Some(delta) = solve_3x3(&h, &g, config.regularization) else {
+            break;
+        };
+
+        // Update pose
+        pose.x += delta[0];
+        pose.y += delta[1];
+        pose.theta += delta[2];
+        pose.theta = crate::core::math::normalize_angle(pose.theta);
+
+        // Check convergence
+        let delta_norm = (delta[0] * delta[0] + delta[1] * delta[1] + delta[2] * delta[2]).sqrt();
+        if delta_norm < config.convergence_threshold {
+            converged = true;
+            break;
+        }
+
+        if residual < config.convergence_threshold {
+            converged = true;
+            break;
+        }
+    }
+
+    let final_residual = compute_residual_fast(correspondences, lines, inv_lengths, &pose);
+
+    GaussNewtonResult {
+        pose,
+        iterations,
+        residual: final_residual,
+        converged,
+    }
+}
+
+/// Build linear system using pre-computed normals (no sqrt in inner loop).
+fn build_linear_system_fast(
+    correspondences: &CorrespondenceSet,
+    lines: &LineCollection,
+    normal_xs: &[f32],
+    normal_ys: &[f32],
+    inv_lengths: &[f32],
+    pose: &Pose2D,
+) -> ([[f32; 3]; 3], [f32; 3], f32) {
+    let (sin, cos) = pose.theta.sin_cos();
+
+    // Pre-compute -sin and -cos for Jacobian
+    let neg_sin = -sin;
+    let neg_cos = -cos;
+
+    let mut h = [[0.0f32; 3]; 3];
+    let mut g = [0.0f32; 3];
+    let mut sum_sq_error = 0.0f32;
+    let mut count = 0;
+
+    for corr in correspondences.iter() {
+        let p = corr.point;
+        let line_idx = corr.line_idx;
+
+        // Transform point by current pose
+        let tx = p.x * cos - p.y * sin + pose.x;
+        let ty = p.x * sin + p.y * cos + pose.y;
+
+        // Get pre-computed normal (no sqrt!)
+        let nx = normal_xs[line_idx];
+        let ny = normal_ys[line_idx];
+
+        // Compute signed distance using pre-computed values
+        // signed_distance = (tx - start_x) * (-dy/len) + (ty - start_y) * (dx/len)
+        //                 = (tx - start_x) * nx + (ty - start_y) * ny
+        // But normal is perpendicular to direction, so:
+        // d = dot(transformed - start, perpendicular_direction) / length
+        //   = cross(transformed - start, direction) / length
+        //   = cross(transformed - start, direction) * inv_length
+        let sx = lines.start_xs[line_idx];
+        let sy = lines.start_ys[line_idx];
+        let ex = lines.end_xs[line_idx];
+        let ey = lines.end_ys[line_idx];
+
+        let dx = ex - sx;
+        let dy = ey - sy;
+        let to_px = tx - sx;
+        let to_py = ty - sy;
+
+        // Cross product gives signed perpendicular distance * length
+        let cross = dx * to_py - dy * to_px;
+        let residual = cross * inv_lengths[line_idx];
+
+        sum_sq_error += residual * residual;
+        count += 1;
+
+        // Jacobian using pre-computed normal
+        // j0 = n_x, j1 = n_y
+        // j2 = n_x * (-p.x * sin - p.y * cos) + n_y * (p.x * cos - p.y * sin)
+        //    = n_x * (p.x * neg_sin + p.y * neg_cos) + n_y * (p.x * cos + p.y * neg_sin)
+        let j0 = nx;
+        let j1 = ny;
+        let j2 = nx * (p.x * neg_sin + p.y * neg_cos) + ny * (p.x * cos + p.y * neg_sin);
+
+        // Accumulate H = J^T·J (symmetric, so only compute upper triangle + diagonal)
+        h[0][0] += j0 * j0;
+        h[0][1] += j0 * j1;
+        h[0][2] += j0 * j2;
+        h[1][1] += j1 * j1;
+        h[1][2] += j1 * j2;
+        h[2][2] += j2 * j2;
+
+        // Accumulate g = J^T·r
+        g[0] += j0 * residual;
+        g[1] += j1 * residual;
+        g[2] += j2 * residual;
+    }
+
+    // Fill symmetric entries
+    h[1][0] = h[0][1];
+    h[2][0] = h[0][2];
+    h[2][1] = h[1][2];
+
+    let rms_error = if count > 0 {
+        (sum_sq_error / count as f32).sqrt()
+    } else {
+        0.0
+    };
+
+    (h, g, rms_error)
+}
+
+/// Compute RMS residual using pre-computed inverse lengths.
+fn compute_residual_fast(
+    correspondences: &CorrespondenceSet,
+    lines: &LineCollection,
+    inv_lengths: &[f32],
+    pose: &Pose2D,
+) -> f32 {
+    if correspondences.is_empty() {
+        return 0.0;
+    }
+
+    let (sin, cos) = pose.theta.sin_cos();
+    let mut sum_sq = 0.0;
+
+    for corr in correspondences.iter() {
+        let p = corr.point;
+        let line_idx = corr.line_idx;
+
+        // Transform point
+        let tx = p.x * cos - p.y * sin + pose.x;
+        let ty = p.x * sin + p.y * cos + pose.y;
+
+        // Compute distance using pre-computed values
+        let sx = lines.start_xs[line_idx];
+        let sy = lines.start_ys[line_idx];
+        let ex = lines.end_xs[line_idx];
+        let ey = lines.end_ys[line_idx];
+
+        let dx = ex - sx;
+        let dy = ey - sy;
+        let to_px = tx - sx;
+        let to_py = ty - sy;
+
+        let cross = dx * to_py - dy * to_px;
+        let dist = cross.abs() * inv_lengths[line_idx];
+
+        sum_sq += dist * dist;
+    }
+
+    (sum_sq / correspondences.len() as f32).sqrt()
+}
+
+#[cfg(test)]
+mod tests {
+    use super::*;
+    use crate::core::Point2D;
+    use crate::matching::correspondence::Correspondence;
+    use approx::assert_relative_eq;
+
+    #[test]
+    fn test_optimize_translation_only() {
+        // Single horizontal line at y=0
+        let lines = vec![Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(10.0, 0.0))];
+
+        // Points that are offset from the line
+        let mut corrs = CorrespondenceSet::new();
+        corrs.push(Correspondence::new(0, 0, Point2D::new(1.0, 0.5), 0.5, 0.1));
+        corrs.push(Correspondence::new(1, 0, Point2D::new(3.0, 0.5), 0.5, 0.3));
+        corrs.push(Correspondence::new(2, 0, Point2D::new(5.0, 0.5), 0.5, 0.5));
+
+        let config = GaussNewtonConfig::default();
+        let result = optimize_pose(&corrs, &lines, Pose2D::identity(), &config);
+
+        // Should find translation to move points down to the line
+        assert!(result.converged);
+        assert_relative_eq!(result.pose.y, -0.5, epsilon = 0.1);
+        assert_relative_eq!(result.pose.x, 0.0, epsilon = 0.1);
+    }
+
+    #[test]
+    fn test_optimize_two_lines() {
+        // Two perpendicular lines
+        let lines = vec![
+            Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(10.0, 0.0)), // Horizontal
+            Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(0.0, 10.0)), // Vertical
+        ];
+
+        // Points offset from both lines
+        let mut corrs = CorrespondenceSet::new();
+        corrs.push(Correspondence::new(0, 0, Point2D::new(2.0, 0.3), 0.3, 0.2));
+        corrs.push(Correspondence::new(1, 0, Point2D::new(5.0, 0.3), 0.3, 0.5));
+        corrs.push(Correspondence::new(2, 1, Point2D::new(0.3, 2.0), 0.3, 0.2));
+        corrs.push(Correspondence::new(3, 1, Point2D::new(0.3, 5.0), 0.3, 0.5));
+
+        let config = GaussNewtonConfig::default();
+        let result = optimize_pose(&corrs, &lines, Pose2D::identity(), &config);
+
+        assert!(result.converged);
+        // Should translate points toward both lines
+        assert!(result.pose.x < 0.0); // Move left
+        assert!(result.pose.y < 0.0); // Move down
+    }
+
+    #[test]
+    fn test_optimize_empty_correspondences() {
+        let lines = vec![Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(10.0, 0.0))];
+        let corrs = CorrespondenceSet::new();
+
+        let initial = Pose2D::new(1.0, 2.0, 0.5);
+        let config = GaussNewtonConfig::default();
+        let result = optimize_pose(&corrs, &lines, initial, &config);
+
+        // Should return initial pose unchanged
+        assert_eq!(result.pose.x, initial.x);
+        assert_eq!(result.pose.y, initial.y);
+        assert_eq!(result.pose.theta, initial.theta);
+        assert!(result.converged);
+    }
+
+    #[test]
+    fn test_solve_3x3() {
+        // Simple identity system
+        let a = [[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]];
+        let b = [1.0, 2.0, 3.0];
+
+        let x = solve_3x3(&a, &b, 0.0).unwrap();
+
+        assert_relative_eq!(x[0], -1.0, epsilon = 1e-6);
+        assert_relative_eq!(x[1], -2.0, epsilon = 1e-6);
+        assert_relative_eq!(x[2], -3.0, epsilon = 1e-6);
+    }
+
+    #[test]
+    fn test_solve_3x3_with_regularization() {
+        // Nearly singular matrix
+        let a = [[1e-8, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]];
+        let b = [1.0, 2.0, 3.0];
+
+        // Without regularization might fail
+        let x = solve_3x3(&a, &b, 1e-4);
+        assert!(x.is_some());
+    }
+
+    #[test]
+    fn test_compute_residual() {
+        let lines = vec![Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(10.0, 0.0))];
+
+        let mut corrs = CorrespondenceSet::new();
+        corrs.push(Correspondence::new(0, 0, Point2D::new(1.0, 0.1), 0.1, 0.1));
+        corrs.push(Correspondence::new(1, 0, Point2D::new(5.0, 0.1), 0.1, 0.5));
+
+        let residual = compute_residual(&corrs, &lines, &Pose2D::identity());
+
+        // RMS of [0.1, 0.1] = sqrt(0.02/2) = 0.1
+        assert_relative_eq!(residual, 0.1, epsilon = 0.01);
+    }
+
+    #[test]
+    fn test_convergence() {
+        let lines = vec![Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(10.0, 0.0))];
+
+        // Points already on the line - should converge immediately
+        let mut corrs = CorrespondenceSet::new();
+        corrs.push(Correspondence::new(0, 0, Point2D::new(1.0, 0.0), 0.0, 0.1));
+        corrs.push(Correspondence::new(1, 0, Point2D::new(5.0, 0.0), 0.0, 0.5));
+
+        let config = GaussNewtonConfig::default();
+        let result = optimize_pose(&corrs, &lines, Pose2D::identity(), &config);
+
+        assert!(result.converged);
+        assert!(result.iterations <= 2); // Should converge quickly
+        assert!(result.residual < 1e-5);
+    }
+}

vastu-map/src/matching/mod.rs

@@ -0,0 +1,79 @@
+//! Scan-to-map matching algorithms.
+//!
+//! This module provides algorithms for matching lidar scans against a map
+//! of line features. The primary algorithm is Point-to-Line ICP.
+//!
+//! # Components
+//!
+//! - **Correspondence**: Types for point-to-line associations
+//! - **Nearest Neighbor**: Find correspondences by distance
+//! - **RANSAC**: Robust pose estimation with outlier rejection
+//! - **Gauss-Newton**: Nonlinear least squares optimization
+//! - **Point-to-Line ICP**: Full scan matching pipeline
+//! - **Scratch Space**: Pre-allocated buffers for zero-allocation ICP
+//!
+//! # Usage
+//!
+//! ```rust,ignore
+//! use vastu_map::matching::{match_scan, IcpConfig};
+//! use vastu_map::core::{Point2D, Pose2D};
+//! use vastu_map::features::Line2D;
+//!
+//! // Map lines
+//! let lines = vec![
+//!     Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(5.0, 0.0)),
+//!     Line2D::new(Point2D::new(5.0, 0.0), Point2D::new(5.0, 5.0)),
+//! ];
+//!
+//! // Scan points (in robot frame)
+//! let points = vec![
+//!     Point2D::new(1.0, 0.05),  // Near first line
+//!     Point2D::new(2.0, -0.02), // Near first line
+//!     Point2D::new(4.95, 2.0),  // Near second line
+//! ];
+//!
+//! // Match scan against map
+//! let initial_pose = Pose2D::new(0.0, 0.0, 0.0);  // From odometry
+//! let result = match_scan(&points, &lines, initial_pose);
+//!
+//! if result.converged {
+//!     println!("Matched pose: ({:.3}, {:.3}, {:.3})",
+//!         result.pose.x, result.pose.y, result.pose.theta);
+//!     println!("Confidence: {:.2}", result.confidence);
+//! }
+//! ```
+//!
+//! # Performance: Zero-Allocation Matching
+//!
+//! For best performance in hot paths, use `IcpScratchSpace` to reuse buffers:
+//!
+//! ```rust,ignore
+//! use vastu_map::matching::{IcpScratchSpace, PointToLineIcp};
+//!
+//! let mut scratch = IcpScratchSpace::default_capacity();
+//! let icp = PointToLineIcp::new();
+//!
+//! for scan in scans {
+//!     let result = icp.match_scan_with_scratch(&scan, &map, pose, &mut scratch);
+//! }
+//! ```
+
+pub mod correspondence;
+pub mod gauss_newton;
+pub mod nearest_neighbor;
+pub mod point_to_line_icp;
+pub mod ransac;
+pub mod scratch;
+
+// Re-export main types
+pub use correspondence::{Correspondence, CorrespondenceSet, MatchResult};
+pub use gauss_newton::{GaussNewtonConfig, GaussNewtonResult, optimize_pose_fast};
+pub use nearest_neighbor::{
+    NearestNeighborConfig, find_correspondences, find_correspondences_batch,
+    find_correspondences_spatial, find_correspondences_with_angle,
+};
+pub use point_to_line_icp::{
+    IcpConfig, OutlierRejection, PointToLineIcp, match_scan, match_scan_with_config,
+};
+pub use ransac::{RansacConfig, RansacResult, estimate_pose_ransac};
+pub use scratch::IcpScratchSpace;

vastu-map/src/matching/nearest_neighbor.rs

@@ -0,0 +1,781 @@
+//! Nearest neighbor matching for scan-to-map correspondence.
+//!
+//! Finds correspondences between scan points and map lines using
+//! distance-based matching with geometric constraints.
+//!
+//! # Performance
+//!
+//! Three variants are provided:
+//! - `find_correspondences`: Basic O(n×m) brute force
+//! - `find_correspondences_batch`: SIMD-optimized O(n×m) using LineCollection
+//! - `find_correspondences_spatial`: O(n×log(m)) using R-tree spatial index
+//!
+//! Use `find_correspondences_spatial` for large maps (>50 lines).
+
+use crate::core::Point2D;
+use crate::features::{Line2D, LineCollection};
+use crate::integration::SpatialIndex;
+
+use super::correspondence::{Correspondence, CorrespondenceSet};
+
+/// Configuration for nearest neighbor matching.
+#[derive(Clone, Debug)]
+pub struct NearestNeighborConfig {
+    /// Maximum distance for a valid correspondence (meters).
+    /// Points farther than this from all lines are rejected.
+    /// Default: 0.5m
+    pub max_distance: f32,
+
+    /// Maximum projection parameter for valid correspondence.
+    /// Points whose projection falls outside [−extension, 1+extension] are rejected.
+    /// Default: 0.2 (allow 20% extension beyond endpoints)
+    pub max_projection_extension: f32,
+
+    /// Whether to only keep the best correspondence per point.
+    /// If false, a point may correspond to multiple lines.
+    /// Default: true
+    pub unique_per_point: bool,
+}
+
+impl Default for NearestNeighborConfig {
+    fn default() -> Self {
+        Self {
+            max_distance: 0.5,
+            max_projection_extension: 0.2,
+            unique_per_point: true,
+        }
+    }
+}
+
+impl NearestNeighborConfig {
+    /// Create a new configuration with default values.
+    pub fn new() -> Self {
+        Self::default()
+    }
+
+    /// Builder-style setter for maximum distance.
+    pub fn with_max_distance(mut self, meters: f32) -> Self {
+        self.max_distance = meters;
+        self
+    }
+
+    /// Builder-style setter for maximum projection extension.
+    pub fn with_max_projection_extension(mut self, extension: f32) -> Self {
+        self.max_projection_extension = extension;
+        self
+    }
+
+    /// Builder-style setter for unique per point.
+    pub fn with_unique_per_point(mut self, unique: bool) -> Self {
+        self.unique_per_point = unique;
+        self
+    }
+}
+
+/// Find correspondences between scan points and map lines.
+///
+/// For each point, finds the nearest line within the distance threshold.
+/// Uses geometric constraints (projection bounds) to filter invalid matches.
+///
+/// # Arguments
+/// * `points` - Scan points in world frame
+/// * `lines` - Map lines to match against
+/// * `config` - Matching configuration
+///
+/// # Returns
+/// Set of valid correspondences.
+pub fn find_correspondences(
+    points: &[Point2D],
+    lines: &[Line2D],
+    config: &NearestNeighborConfig,
+) -> CorrespondenceSet {
+    if points.is_empty() || lines.is_empty() {
+        return CorrespondenceSet::new();
+    }
+
+    let mut result = CorrespondenceSet::with_capacity(points.len());
+    let min_t = -config.max_projection_extension;
+    let max_t = 1.0 + config.max_projection_extension;
+
+    for (point_idx, &point) in points.iter().enumerate() {
+        let mut best_distance = config.max_distance;
+        let mut best_corr: Option<Correspondence> = None;
+
+        for (line_idx, line) in lines.iter().enumerate() {
+            // Compute projection parameter
+            let t = line.project_point(point);
+
+            // Check projection bounds
+            if t < min_t || t > max_t {
+                continue;
+            }
+
+            // Compute perpendicular distance
+            let distance = line.distance_to_point(point);
+
+            // Check distance threshold
+            if distance > config.max_distance {
+                continue;
+            }
+
+            if config.unique_per_point {
+                // Keep only the best match
+                if distance < best_distance {
+                    best_distance = distance;
+                    best_corr = Some(Correspondence::new(point_idx, line_idx, point, distance, t));
+                }
+            } else {
+                // Add all valid matches
+                result.push(Correspondence::new(point_idx, line_idx, point, distance, t));
+            }
+        }
+
+        if config.unique_per_point
+            && let Some(corr) = best_corr
+        {
+            result.push(corr);
+        }
+    }
+
+    result
+}
+
+/// Find correspondences using SoA LineCollection (SIMD-optimized).
+///
+/// This version uses batch distance computation for better performance
+/// on large line sets.
+///
+/// # Arguments
+/// * `points` - Scan points in world frame
+/// * `lines` - Map lines in SoA format
+/// * `config` - Matching configuration
+///
+/// # Returns
+/// Set of valid correspondences.
+pub fn find_correspondences_batch(
+    points: &[Point2D],
+    lines: &LineCollection,
+    config: &NearestNeighborConfig,
+) -> CorrespondenceSet {
+    if points.is_empty() || lines.is_empty() {
+        return CorrespondenceSet::new();
+    }
+
+    let mut result = CorrespondenceSet::with_capacity(points.len());
+    let min_t = -config.max_projection_extension;
+    let max_t = 1.0 + config.max_projection_extension;
+    let n_lines = lines.len();
+
+    for (point_idx, &point) in points.iter().enumerate() {
+        // Compute distances to all lines at once (SIMD-friendly)
+        let distances = lines.distances_to_point(point);
+        let projections = lines.project_point(point);
+
+        let mut best_distance = config.max_distance;
+        let mut best_corr: Option<Correspondence> = None;
+
+        for line_idx in 0..n_lines {
+            let t = projections[line_idx];
+            let distance = distances[line_idx];
+
+            // Check projection bounds
+            if t < min_t || t > max_t {
+                continue;
+            }
+
+            // Check distance threshold
+            if distance > config.max_distance {
+                continue;
+            }
+
+            if config.unique_per_point {
+                if distance < best_distance {
+                    best_distance = distance;
+                    best_corr = Some(Correspondence::new(point_idx, line_idx, point, distance, t));
+                }
+            } else {
+                result.push(Correspondence::new(point_idx, line_idx, point, distance, t));
+            }
+        }
+
+        if config.unique_per_point
+            && let Some(corr) = best_corr
+        {
+            result.push(corr);
+        }
+    }
+
+    result
+}
+
+/// Find correspondences using R-tree spatial index (O(n log m) complexity).
+///
+/// This version uses a spatial index for fast nearest-neighbor queries,
+/// making it much faster than brute-force for large maps (>50 lines).
+///
+/// # Arguments
+/// * `points` - Scan points in world frame
+/// * `lines` - Map lines to match against
+/// * `index` - Pre-built spatial index for the lines
+/// * `config` - Matching configuration
+///
+/// # Performance
+///
+/// - For small maps (<50 lines): Use `find_correspondences_batch` instead
+/// - For large maps (>50 lines): This version is 5-10x faster
+///
+/// # Example
+/// ```rust,ignore
+/// use vastu_map::matching::{find_correspondences_spatial, NearestNeighborConfig};
+/// use vastu_map::integration::SpatialIndex;
+///
+/// let lines = vec![/* map lines */];
+/// let index = SpatialIndex::new(&lines);
+///
+/// for scan in scans {
+///     // Reuse index across scans
+///     let corrs = find_correspondences_spatial(&scan, &lines, &index, &config);
+/// }
+/// ```
+pub fn find_correspondences_spatial(
+    points: &[Point2D],
+    lines: &[Line2D],
+    index: &SpatialIndex,
+    config: &NearestNeighborConfig,
+) -> CorrespondenceSet {
+    if points.is_empty() || lines.is_empty() {
+        return CorrespondenceSet::new();
+    }
+
+    let mut result = CorrespondenceSet::with_capacity(points.len());
+    let min_t = -config.max_projection_extension;
+    let max_t = 1.0 + config.max_projection_extension;
+
+    for (point_idx, &point) in points.iter().enumerate() {
+        // Use spatial index to find candidate lines within distance threshold
+        // This is O(log n) instead of O(n)
+        let candidates = index.lines_within_distance(point, config.max_distance);
+
+        if candidates.is_empty() {
+            continue;
+        }
+
+        let mut best_distance = config.max_distance;
+        let mut best_corr: Option<Correspondence> = None;
+
+        for (line_idx, distance) in candidates {
+            let line = &lines[line_idx];
+
+            // Compute projection parameter
+            let t = line.project_point(point);
+
+            // Check projection bounds
+            if t < min_t || t > max_t {
+                continue;
+            }
+
+            if config.unique_per_point {
+                if distance < best_distance {
+                    best_distance = distance;
+                    best_corr = Some(Correspondence::new(point_idx, line_idx, point, distance, t));
+                }
+            } else {
+                result.push(Correspondence::new(point_idx, line_idx, point, distance, t));
+            }
+        }
+
+        if config.unique_per_point
+            && let Some(corr) = best_corr
+        {
+            result.push(corr);
+        }
+    }
+
+    result
+}
+
+/// Find correspondences with angle constraint.
+///
+/// Adds an additional constraint that the point's bearing from the sensor
+/// should be approximately perpendicular to the line normal.
+///
+/// # Arguments
+/// * `points` - Scan points in world frame
+/// * `lines` - Map lines to match against
+/// * `sensor_position` - Position of the sensor in world frame
+/// * `max_angle_deviation` - Maximum deviation from perpendicular (radians)
+/// * `config` - Matching configuration
+pub fn find_correspondences_with_angle(
+    points: &[Point2D],
+    lines: &[Line2D],
+    sensor_position: Point2D,
+    max_angle_deviation: f32,
+    config: &NearestNeighborConfig,
+) -> CorrespondenceSet {
+    if points.is_empty() || lines.is_empty() {
+        return CorrespondenceSet::new();
+    }
+
+    let mut result = CorrespondenceSet::with_capacity(points.len());
+    let min_t = -config.max_projection_extension;
+    let max_t = 1.0 + config.max_projection_extension;
+    let cos_threshold = max_angle_deviation.cos();
+
+    for (point_idx, &point) in points.iter().enumerate() {
+        // Compute bearing from sensor to point
+        let bearing = (point - sensor_position).normalized();
+
+        let mut best_distance = config.max_distance;
+        let mut best_corr: Option<Correspondence> = None;
+
+        for (line_idx, line) in lines.iter().enumerate() {
+            // Check angle constraint
+            let line_normal = line.normal();
+            let dot = bearing.dot(line_normal).abs();
+
+            // bearing should be roughly parallel to normal (hitting line head-on)
+            if dot < cos_threshold {
+                continue;
+            }
+
+            // Compute projection parameter
+            let t = line.project_point(point);
+
+            // Check projection bounds
+            if t < min_t || t > max_t {
+                continue;
+            }
+
+            // Compute perpendicular distance
+            let distance = line.distance_to_point(point);
+
+            // Check distance threshold
+            if distance > config.max_distance {
+                continue;
+            }
+
+            if config.unique_per_point {
+                if distance < best_distance {
+                    best_distance = distance;
+                    best_corr = Some(Correspondence::new(point_idx, line_idx, point, distance, t));
+                }
+            } else {
+                result.push(Correspondence::new(point_idx, line_idx, point, distance, t));
+            }
+        }
+
+        if config.unique_per_point
+            && let Some(corr) = best_corr
+        {
+            result.push(corr);
+        }
+    }
+
+    result
+}
+
+#[cfg(test)]
+mod tests {
+    use super::*;
+
+    fn make_horizontal_line() -> Line2D {
+        Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(2.0, 0.0))
+    }
+
+    fn make_vertical_line() -> Line2D {
+        Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(0.0, 2.0))
+    }
+
+    #[test]
+    fn test_find_correspondences_single_point() {
+        let lines = vec![make_horizontal_line()];
+        let points = vec![Point2D::new(1.0, 0.1)]; // Close to middle of line
+
+        let config = NearestNeighborConfig::default();
+        let corrs = find_correspondences(&points, &lines, &config);
+
+        assert_eq!(corrs.len(), 1);
+        assert_eq!(corrs.correspondences[0].point_idx, 0);
+        assert_eq!(corrs.correspondences[0].line_idx, 0);
+        assert!((corrs.correspondences[0].distance - 0.1).abs() < 0.01);
+    }
+
+    #[test]
+    fn test_find_correspondences_multiple_lines() {
+        let lines = vec![make_horizontal_line(), make_vertical_line()];
+        let points = vec![Point2D::new(0.05, 0.1)]; // Closer to vertical line
+
+        let config = NearestNeighborConfig::default();
+        let corrs = find_correspondences(&points, &lines, &config);
+
+        assert_eq!(corrs.len(), 1);
+        // Should match vertical line (distance 0.05) not horizontal (distance 0.1)
+        assert_eq!(corrs.correspondences[0].line_idx, 1);
+        assert!((corrs.correspondences[0].distance - 0.05).abs() < 0.01);
+    }
+
+    #[test]
+    fn test_find_correspondences_no_match_distance() {
+        let lines = vec![make_horizontal_line()];
+        let points = vec![Point2D::new(1.0, 1.0)]; // Too far from line
+
+        let config = NearestNeighborConfig::default().with_max_distance(0.5);
+        let corrs = find_correspondences(&points, &lines, &config);
+
+        assert!(corrs.is_empty());
+    }
+
+    #[test]
+    fn test_find_correspondences_no_match_projection() {
+        let lines = vec![make_horizontal_line()]; // From (0,0) to (2,0)
+        let points = vec![Point2D::new(-1.0, 0.1)]; // Projects before line start
+
+        let config = NearestNeighborConfig::default().with_max_projection_extension(0.0);
+        let corrs = find_correspondences(&points, &lines, &config);
+
+        assert!(corrs.is_empty());
+    }
+
+    #[test]
+    fn test_find_correspondences_projection_extension() {
+        let lines = vec![make_horizontal_line()]; // From (0,0) to (2,0)
+        let points = vec![Point2D::new(-0.2, 0.1)]; // Projects slightly before start
+
+        // Without extension: no match
+        let config_strict = NearestNeighborConfig::default().with_max_projection_extension(0.0);
+        let corrs_strict = find_correspondences(&points, &lines, &config_strict);
+        assert!(corrs_strict.is_empty());
+
+        // With extension: match
+        let config_relaxed = NearestNeighborConfig::default().with_max_projection_extension(0.2);
+        let corrs_relaxed = find_correspondences(&points, &lines, &config_relaxed);
+        assert_eq!(corrs_relaxed.len(), 1);
+    }
+
+    #[test]
+    fn test_find_correspondences_multiple_per_point() {
+        let lines = vec![
+            make_horizontal_line(),
+            Line2D::new(Point2D::new(0.0, 0.2), Point2D::new(2.0, 0.2)), // Parallel line
+        ];
+        let points = vec![Point2D::new(1.0, 0.1)]; // Between the two lines
+
+        // Unique: only best match
+        let config_unique = NearestNeighborConfig::default().with_unique_per_point(true);
+        let corrs_unique = find_correspondences(&points, &lines, &config_unique);
+        assert_eq!(corrs_unique.len(), 1);
+
+        // Non-unique: both matches
+        let config_multi = NearestNeighborConfig::default().with_unique_per_point(false);
+        let corrs_multi = find_correspondences(&points, &lines, &config_multi);
+        assert_eq!(corrs_multi.len(), 2);
+    }
+
+    #[test]
+    fn test_find_correspondences_empty_inputs() {
+        let config = NearestNeighborConfig::default();
+
+        // Empty points
+        let corrs1 = find_correspondences(&[], &[make_horizontal_line()], &config);
+        assert!(corrs1.is_empty());
+
+        // Empty lines
+        let corrs2 = find_correspondences(&[Point2D::new(0.0, 0.0)], &[], &config);
+        assert!(corrs2.is_empty());
+    }
+
+    #[test]
+    fn test_find_correspondences_batch() {
+        let mut collection = LineCollection::new();
+        collection.push_line(&make_horizontal_line());
+        collection.push_line(&make_vertical_line());
+
+        let points = vec![
+            Point2D::new(1.0, 0.1), // Near horizontal
+            Point2D::new(0.1, 1.0), // Near vertical
+        ];
+
+        let config = NearestNeighborConfig::default();
+        let corrs = find_correspondences_batch(&points, &collection, &config);
+
+        assert_eq!(corrs.len(), 2);
+    }
+
+    #[test]
+    fn test_projection_accuracy() {
+        let line = make_horizontal_line(); // From (0,0) to (2,0)
+        let points = vec![
+            Point2D::new(0.0, 0.1), // At start, t ≈ 0
+            Point2D::new(1.0, 0.1), // At middle, t ≈ 0.5
+            Point2D::new(2.0, 0.1), // At end, t ≈ 1
+        ];
+
+        let config = NearestNeighborConfig::default();
+        let corrs = find_correspondences(&points, &[line], &config);
+
+        assert_eq!(corrs.len(), 3);
+        assert!((corrs.correspondences[0].projection_t - 0.0).abs() < 0.01);
+        assert!((corrs.correspondences[1].projection_t - 0.5).abs() < 0.01);
+        assert!((corrs.correspondences[2].projection_t - 1.0).abs() < 0.01);
+    }
+
+    // === Additional edge case tests ===
+
+    #[test]
+    fn test_very_close_parallel_lines() {
+        // Two parallel lines very close together - test tie-breaking
+        let lines = vec![
+            Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(5.0, 0.0)),
+            Line2D::new(Point2D::new(0.0, 0.02), Point2D::new(5.0, 0.02)), // 2cm apart
+        ];
+
+        // Point exactly between the two lines
+        let points = vec![Point2D::new(2.5, 0.01)];
+
+        let config = NearestNeighborConfig::default().with_unique_per_point(true);
+        let corrs = find_correspondences(&points, &lines, &config);
+
+        // Should match one line (the closest one)
+        assert_eq!(corrs.len(), 1);
+        // Distance should be ~0.01m to either line
+        assert!(corrs.correspondences[0].distance < 0.015);
+    }
+
+    #[test]
+    fn test_large_line_collection() {
+        // Test with many lines to verify performance doesn't degrade
+        let mut lines = Vec::new();
+
+        // Create a grid of lines (100 lines)
+        for i in 0..10 {
+            // Horizontal lines
+            lines.push(Line2D::new(
+                Point2D::new(0.0, i as f32 * 0.5),
+                Point2D::new(5.0, i as f32 * 0.5),
+            ));
+            // Vertical lines
+            lines.push(Line2D::new(
+                Point2D::new(i as f32 * 0.5, 0.0),
+                Point2D::new(i as f32 * 0.5, 5.0),
+            ));
+        }
+
+        // Many points
+        let points: Vec<Point2D> = (0..50)
+            .map(|i| Point2D::new((i % 10) as f32 * 0.5 + 0.1, (i / 10) as f32 * 0.5 + 0.1))
+            .collect();
+
+        let config = NearestNeighborConfig::default()
+            .with_max_distance(0.5)
+            .with_unique_per_point(true);
+
+        let corrs = find_correspondences(&points, &lines, &config);
+
+        // Should find correspondences for most points
+        assert!(
+            corrs.len() > 30,
+            "Expected >30 matches, got {}",
+            corrs.len()
+        );
+    }
+
+    #[test]
+    fn test_batch_vs_nonbatch_consistency() {
+        // Results from batch and non-batch should be identical
+        let lines_vec = vec![
+            Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(5.0, 0.0)),
+            Line2D::new(Point2D::new(5.0, 0.0), Point2D::new(5.0, 5.0)),
+            Line2D::new(Point2D::new(5.0, 5.0), Point2D::new(0.0, 5.0)),
+            Line2D::new(Point2D::new(0.0, 5.0), Point2D::new(0.0, 0.0)),
+        ];
+
+        let mut collection = LineCollection::new();
+        for line in &lines_vec {
+            collection.push_line(line);
+        }
+
+        let points = vec![
+            Point2D::new(2.5, 0.1),
+            Point2D::new(4.9, 2.5),
+            Point2D::new(2.5, 4.9),
+            Point2D::new(0.1, 2.5),
+        ];
+
+        let config = NearestNeighborConfig::default().with_unique_per_point(true);
+
+        let corrs_regular = find_correspondences(&points, &lines_vec, &config);
+        let corrs_batch = find_correspondences_batch(&points, &collection, &config);
+
+        // Same number of correspondences
+        assert_eq!(corrs_regular.len(), corrs_batch.len());
+
+        // Same line indices matched
+        for i in 0..corrs_regular.len() {
+            assert_eq!(
+                corrs_regular.correspondences[i].line_idx, corrs_batch.correspondences[i].line_idx,
+                "Line index mismatch at position {}",
+                i
+            );
+            assert!(
+                (corrs_regular.correspondences[i].distance
+                    - corrs_batch.correspondences[i].distance)
+                    .abs()
+                    < 0.001,
+                "Distance mismatch at position {}",
+                i
+            );
+        }
+    }
+
+    #[test]
+    fn test_find_correspondences_with_angle_basic() {
+        // Test angle-constrained matching
+        let lines = vec![
+            Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(5.0, 0.0)), // Horizontal
+            Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(0.0, 5.0)), // Vertical
+        ];
+
+        // Sensor at center, point on horizontal line
+        let sensor_pos = Point2D::new(2.5, 2.5);
+        let points = vec![Point2D::new(2.5, 0.1)]; // Point below sensor, near horizontal line
+
+        // Bearing from sensor to point is straight down (perpendicular to horizontal line normal)
+        let config = NearestNeighborConfig::default();
+        let max_angle_deviation = 0.5; // ~29 degrees
+
+        let corrs = find_correspondences_with_angle(
+            &points,
+            &lines,
+            sensor_pos,
+            max_angle_deviation,
+            &config,
+        );
+
+        // Should match horizontal line (bearing is perpendicular to line, parallel to normal)
+        assert_eq!(corrs.len(), 1);
+        assert_eq!(corrs.correspondences[0].line_idx, 0);
+    }
+
+    #[test]
+    fn test_find_correspondences_with_angle_rejection() {
+        // Test that angle constraint rejects bad matches
+        let lines = vec![
+            Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(5.0, 0.0)), // Horizontal
+        ];
+
+        // Sensor to the left, point to the right of sensor
+        let sensor_pos = Point2D::new(0.0, 0.5);
+        let points = vec![Point2D::new(2.5, 0.5)]; // Point to the right, bearing is horizontal
+
+        // Bearing is parallel to line (perpendicular to normal) - should be rejected
+        let config = NearestNeighborConfig::default();
+        let max_angle_deviation = 0.3; // ~17 degrees - strict
+
+        let corrs = find_correspondences_with_angle(
+            &points,
+            &lines,
+            sensor_pos,
+            max_angle_deviation,
+            &config,
+        );
+
+        // Should reject because bearing is parallel to line, not hitting it head-on
+        assert!(corrs.is_empty(), "Should reject grazing angle match");
+    }
+
+    #[test]
+    fn test_config_default_values() {
+        let config = NearestNeighborConfig::default();
+
+        assert_eq!(config.max_distance, 0.5);
+        assert_eq!(config.max_projection_extension, 0.2);
+        assert!(config.unique_per_point);
+    }
+
+    #[test]
+    fn test_multiple_points_same_line() {
+        // Multiple points all matching the same line
+        let lines = vec![Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(10.0, 0.0))];
+
+        let points = vec![
+            Point2D::new(1.0, 0.05),
+            Point2D::new(3.0, 0.08),
+            Point2D::new(5.0, 0.03),
+            Point2D::new(7.0, 0.06),
+            Point2D::new(9.0, 0.04),
+        ];
+
+        let config = NearestNeighborConfig::default();
+        let corrs = find_correspondences(&points, &lines, &config);
+
+        // All points should match the same line
+        assert_eq!(corrs.len(), 5);
+        for corr in corrs.iter() {
+            assert_eq!(corr.line_idx, 0);
+        }
+    }
+
+    #[test]
+    fn test_batch_empty_inputs() {
+        let config = NearestNeighborConfig::default();
+
+        // Empty points
+        let collection = LineCollection::new();
+        let corrs1 = find_correspondences_batch(&[], &collection, &config);
+        assert!(corrs1.is_empty());
+
+        // Empty lines
+        let mut collection2 = LineCollection::new();
+        collection2.push_line(&make_horizontal_line());
+        let corrs2 = find_correspondences_batch(&[], &collection2, &config);
+        assert!(corrs2.is_empty());
+    }
+
+    #[test]
+    fn test_angle_with_empty_inputs() {
+        let config = NearestNeighborConfig::default();
+        let sensor_pos = Point2D::new(0.0, 0.0);
+
+        // Empty points
+        let corrs1 = find_correspondences_with_angle(
+            &[],
+            &[make_horizontal_line()],
+            sensor_pos,
+            0.5,
+            &config,
+        );
+        assert!(corrs1.is_empty());
+
+        // Empty lines
+        let corrs2 = find_correspondences_with_angle(
+            &[Point2D::new(1.0, 0.1)],
+            &[],
+            sensor_pos,
+            0.5,
+            &config,
+        );
+        assert!(corrs2.is_empty());
+    }
+
+    #[test]
+    fn test_non_unique_mode_with_many_lines() {
+        // Test non-unique mode returns all valid matches
+        let lines = vec![
+            Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(5.0, 0.0)),
+            Line2D::new(Point2D::new(0.0, 0.1), Point2D::new(5.0, 0.1)),
+            Line2D::new(Point2D::new(0.0, 0.2), Point2D::new(5.0, 0.2)),
+        ];
+
+        // Point between all three lines
+        let points = vec![Point2D::new(2.5, 0.1)];
+
+        let config = NearestNeighborConfig::default()
+            .with_max_distance(0.2)
+            .with_unique_per_point(false);
+
+        let corrs = find_correspondences(&points, &lines, &config);
+
+        // Should match all three lines (distances: 0.1, 0.0, 0.1)
+        assert_eq!(corrs.len(), 3, "Should match all 3 parallel lines");
+    }
+}

vastu-map/src/matching/point_to_line_icp.rs

@@ -0,0 +1,636 @@
+//! Point-to-Line Iterative Closest Point (ICP) algorithm.
+//!
+//! This module implements the full scan-to-map matching pipeline:
+//!
+//! 1. **Correspondence Search**: Find nearest map lines for each scan point
+//! 2. **Outlier Rejection**: Use RANSAC or distance threshold
+//! 3. **Pose Optimization**: Gauss-Newton solver for point-to-line error
+//! 4. **Iteration**: Repeat until convergence
+//!
+//! # Point-to-Line vs Point-to-Point
+//!
+//! Point-to-Line ICP converges faster than Point-to-Point because:
+//! - Points can slide along lines in the tangent direction
+//! - Only the perpendicular distance matters
+//! - Better handles structured environments (walls, edges)
+
+use crate::core::{Point2D, Pose2D};
+use crate::features::{Line2D, LineCollection};
+
+use super::correspondence::{CorrespondenceSet, MatchResult};
+use super::gauss_newton::{GaussNewtonConfig, optimize_pose, optimize_pose_fast};
+use super::nearest_neighbor::{
+    NearestNeighborConfig, find_correspondences, find_correspondences_batch,
+};
+use super::ransac::{RansacConfig, estimate_pose_ransac};
+use super::scratch::IcpScratchSpace;
+
+/// Configuration for Point-to-Line ICP.
+#[derive(Clone, Debug)]
+pub struct IcpConfig {
+    /// Maximum number of ICP iterations.
+    /// Default: 30
+    pub max_iterations: usize,
+
+    /// Convergence threshold for pose change.
+    /// If ‖Δpose‖ < threshold, ICP stops.
+    /// Default: 1e-4
+    pub convergence_threshold: f32,
+
+    /// Maximum correspondence distance (meters).
+    /// Points farther than this from all lines are rejected.
+    /// Default: 0.5m
+    pub max_correspondence_distance: f32,
+
+    /// Outlier rejection method.
+    /// Default: DistanceThreshold
+    pub outlier_rejection: OutlierRejection,
+
+    /// Minimum number of correspondences required.
+    /// Default: 10
+    pub min_correspondences: usize,
+
+    /// Minimum match confidence to consider successful.
+    /// Default: 0.3
+    pub min_confidence: f32,
+
+    /// Whether to use RANSAC for initial pose estimate.
+    /// Default: false
+    pub use_ransac_init: bool,
+
+    /// Whether to use batch (SIMD) correspondence search.
+    /// Default: true
+    pub use_batch_search: bool,
+}
+
+/// Outlier rejection method.
+#[derive(Clone, Debug)]
+pub enum OutlierRejection {
+    /// Reject by distance threshold (fast).
+    DistanceThreshold(f32),
+    /// Use RANSAC for robust rejection (slower but more robust).
+    Ransac(RansacConfig),
+    /// No outlier rejection.
+    None,
+}
+
+impl Default for IcpConfig {
+    fn default() -> Self {
+        Self {
+            max_iterations: 30,
+            convergence_threshold: 1e-4,
+            max_correspondence_distance: 0.5,
+            outlier_rejection: OutlierRejection::DistanceThreshold(0.15),
+            min_correspondences: 10,
+            min_confidence: 0.3,
+            use_ransac_init: false,
+            use_batch_search: true,
+        }
+    }
+}
+
+impl IcpConfig {
+    /// Create a new configuration with default values.
+    pub fn new() -> Self {
+        Self::default()
+    }
+
+    /// Builder-style setter for maximum iterations.
+    pub fn with_max_iterations(mut self, iterations: usize) -> Self {
+        self.max_iterations = iterations;
+        self
+    }
+
+    /// Builder-style setter for convergence threshold.
+    pub fn with_convergence_threshold(mut self, threshold: f32) -> Self {
+        self.convergence_threshold = threshold;
+        self
+    }
+
+    /// Builder-style setter for maximum correspondence distance.
+    pub fn with_max_correspondence_distance(mut self, meters: f32) -> Self {
+        self.max_correspondence_distance = meters;
+        self
+    }
+
+    /// Builder-style setter for outlier rejection method.
+    pub fn with_outlier_rejection(mut self, method: OutlierRejection) -> Self {
+        self.outlier_rejection = method;
+        self
+    }
+
+    /// Builder-style setter for minimum correspondences.
+    pub fn with_min_correspondences(mut self, count: usize) -> Self {
+        self.min_correspondences = count;
+        self
+    }
+
+    /// Builder-style setter for RANSAC initialization.
+    pub fn with_ransac_init(mut self, enabled: bool) -> Self {
+        self.use_ransac_init = enabled;
+        self
+    }
+}
+
+/// Point-to-Line ICP matcher.
+///
+/// Holds configuration and provides scan matching functionality.
+pub struct PointToLineIcp {
+    /// ICP configuration.
+    config: IcpConfig,
+    /// Nearest neighbor configuration.
+    nn_config: NearestNeighborConfig,
+    /// Gauss-Newton configuration.
+    gn_config: GaussNewtonConfig,
+}
+
+impl PointToLineIcp {
+    /// Create a new ICP matcher with default configuration.
+    pub fn new() -> Self {
+        Self::with_config(IcpConfig::default())
+    }
+
+    /// Create a new ICP matcher with custom configuration.
+    pub fn with_config(config: IcpConfig) -> Self {
+        let nn_config = NearestNeighborConfig::default()
+            .with_max_distance(config.max_correspondence_distance)
+            .with_unique_per_point(true);
+
+        let gn_config = GaussNewtonConfig::default()
+            .with_max_iterations(10)
+            .with_convergence_threshold(config.convergence_threshold / 10.0);
+
+        Self {
+            config,
+            nn_config,
+            gn_config,
+        }
+    }
+
+    /// Match scan points against map lines.
+    ///
+    /// # Arguments
+    /// * `points` - Scan points in world frame (after initial transform)
+    /// * `lines` - Map lines
+    /// * `initial_pose` - Initial pose estimate (typically from odometry)
+    ///
+    /// # Returns
+    /// Match result with optimized pose and confidence.
+    pub fn match_scan(
+        &self,
+        points: &[Point2D],
+        lines: &[Line2D],
+        initial_pose: Pose2D,
+    ) -> MatchResult {
+        if points.is_empty() || lines.is_empty() {
+            return MatchResult::new(initial_pose, CorrespondenceSet::new(), 0, false);
+        }
+
+        // Convert lines to collection for batch operations
+        let line_collection = if self.config.use_batch_search {
+            Some(LineCollection::from_lines(lines))
+        } else {
+            None
+        };
+
+        let mut pose = initial_pose;
+        let mut prev_pose = pose;
+        let mut final_correspondences = CorrespondenceSet::new();
+        let mut converged = false;
+
+        // Optional RANSAC initialization
+        if self.config.use_ransac_init {
+            let initial_corrs =
+                self.find_correspondences_for_pose(points, lines, line_collection.as_ref(), &pose);
+
+            if initial_corrs.len() >= self.config.min_correspondences {
+                let ransac_config = RansacConfig::default()
+                    .with_max_iterations(50)
+                    .with_inlier_threshold(0.15);
+
+                if let Some(ransac_result) =
+                    estimate_pose_ransac(&initial_corrs, lines, &ransac_config)
+                {
+                    pose = pose.compose(ransac_result.pose);
+                }
+            }
+        }
+
+        // Main ICP loop
+        for iteration in 0..self.config.max_iterations {
+            // 1. Find correspondences at current pose
+            let correspondences =
+                self.find_correspondences_for_pose(points, lines, line_collection.as_ref(), &pose);
+
+            if correspondences.len() < self.config.min_correspondences {
+                // Not enough correspondences - return with low confidence
+                return MatchResult::new(pose, correspondences, iteration + 1, false);
+            }
+
+            // 2. Reject outliers
+            let filtered = self.reject_outliers(&correspondences, lines);
+
+            if filtered.len() < self.config.min_correspondences {
+                return MatchResult::new(pose, filtered, iteration + 1, false);
+            }
+
+            // 3. Optimize pose using Gauss-Newton
+            let gn_result = optimize_pose(&filtered, lines, Pose2D::identity(), &self.gn_config);
+
+            // 4. Update pose (compose with delta)
+            pose = pose.compose(gn_result.pose);
+
+            // 5. Check convergence
+            let delta_trans =
+                ((pose.x - prev_pose.x).powi(2) + (pose.y - prev_pose.y).powi(2)).sqrt();
+            let delta_rot = crate::core::math::angle_diff(pose.theta, prev_pose.theta).abs();
+
+            if delta_trans < self.config.convergence_threshold
+                && delta_rot < self.config.convergence_threshold
+            {
+                converged = true;
+                final_correspondences = filtered;
+                break;
+            }
+
+            prev_pose = pose;
+            final_correspondences = filtered;
+        }
+
+        MatchResult::new(
+            pose,
+            final_correspondences,
+            self.config.max_iterations,
+            converged,
+        )
+    }
+
+    /// Match scan against line collection (optimized for large maps).
+    pub fn match_scan_batch(
+        &self,
+        points: &[Point2D],
+        lines: &LineCollection,
+        initial_pose: Pose2D,
+    ) -> MatchResult {
+        let lines_vec = lines.to_lines();
+        self.match_scan(points, &lines_vec, initial_pose)
+    }
+
+    /// Match scan using pre-allocated scratch space (zero-allocation hot path).
+    ///
+    /// This is the most efficient matching method, eliminating per-iteration
+    /// allocations by reusing buffers from the scratch space.
+    ///
+    /// # Arguments
+    /// * `points` - Scan points in robot frame
+    /// * `lines` - Map lines as LineCollection (for SIMD operations)
+    /// * `initial_pose` - Initial pose estimate (typically from odometry)
+    /// * `scratch` - Pre-allocated scratch space for buffer reuse
+    ///
+    /// # Returns
+    /// Match result with optimized pose and confidence.
+    ///
+    /// # Example
+    /// ```rust,ignore
+    /// let mut scratch = IcpScratchSpace::default_capacity();
+    /// let icp = PointToLineIcp::new();
+    ///
+    /// // Reuse scratch across multiple calls
+    /// for (scan, odom_pose) in scans.iter().zip(odometry.iter()) {
+    ///     let result = icp.match_scan_with_scratch(&scan, &map_lines, *odom_pose, &mut scratch);
+    ///     if result.converged {
+    ///         // Use result.pose
+    ///     }
+    /// }
+    /// ```
+    pub fn match_scan_with_scratch(
+        &self,
+        points: &[Point2D],
+        lines: &LineCollection,
+        initial_pose: Pose2D,
+        scratch: &mut IcpScratchSpace,
+    ) -> MatchResult {
+        if points.is_empty() || lines.is_empty() {
+            return MatchResult::new(initial_pose, CorrespondenceSet::new(), 0, false);
+        }
+
+        // Convert to Line2D for outlier rejection (uses Line2D methods)
+        let lines_vec = lines.to_lines();
+        let mut pose = initial_pose;
+        let mut prev_pose = pose;
+        let mut final_correspondences = CorrespondenceSet::new();
+        let mut converged = false;
+
+        // Main ICP loop with scratch space
+        for iteration in 0..self.config.max_iterations {
+            // 1. Transform points and find correspondences using scratch space
+            let (sin, cos) = pose.theta.sin_cos();
+            scratch.transform_points(points, sin, cos, pose.x, pose.y);
+            scratch.find_correspondences(lines, &self.nn_config);
+
+            if scratch.num_correspondences() < self.config.min_correspondences {
+                // Not enough correspondences - return with low confidence
+                return MatchResult::new(
+                    pose,
+                    scratch.take_correspondences(),
+                    iteration + 1,
+                    false,
+                );
+            }
+
+            // 2. Get correspondences and reject outliers
+            let correspondences = scratch.correspondences();
+            let filtered = self.reject_outliers(correspondences, &lines_vec);
+
+            if filtered.len() < self.config.min_correspondences {
+                return MatchResult::new(pose, filtered, iteration + 1, false);
+            }
+
+            // 3. Optimize pose using fast Gauss-Newton (uses pre-computed normals)
+            let gn_result =
+                optimize_pose_fast(&filtered, lines, Pose2D::identity(), &self.gn_config);
+
+            // 4. Update pose (compose with delta)
+            pose = pose.compose(gn_result.pose);
+
+            // 5. Check convergence
+            let delta_trans =
+                ((pose.x - prev_pose.x).powi(2) + (pose.y - prev_pose.y).powi(2)).sqrt();
+            let delta_rot = crate::core::math::angle_diff(pose.theta, prev_pose.theta).abs();
+
+            if delta_trans < self.config.convergence_threshold
+                && delta_rot < self.config.convergence_threshold
+            {
+                converged = true;
+                final_correspondences = filtered;
+                break;
+            }
+
+            prev_pose = pose;
+            final_correspondences = filtered;
+        }
+
+        MatchResult::new(
+            pose,
+            final_correspondences,
+            self.config.max_iterations,
+            converged,
+        )
+    }
+
+    /// Find correspondences for points at a given pose.
+    fn find_correspondences_for_pose(
+        &self,
+        points: &[Point2D],
+        lines: &[Line2D],
+        line_collection: Option<&LineCollection>,
+        pose: &Pose2D,
+    ) -> CorrespondenceSet {
+        // Transform points by pose
+        let (sin, cos) = pose.theta.sin_cos();
+        let transformed: Vec<Point2D> = points
+            .iter()
+            .map(|p| {
+                Point2D::new(
+                    p.x * cos - p.y * sin + pose.x,
+                    p.x * sin + p.y * cos + pose.y,
+                )
+            })
+            .collect();
+
+        // Find correspondences
+        if let Some(collection) = line_collection {
+            find_correspondences_batch(&transformed, collection, &self.nn_config)
+        } else {
+            find_correspondences(&transformed, lines, &self.nn_config)
+        }
+    }
+
+    /// Reject outliers from correspondences.
+    fn reject_outliers(
+        &self,
+        correspondences: &CorrespondenceSet,
+        lines: &[Line2D],
+    ) -> CorrespondenceSet {
+        match &self.config.outlier_rejection {
+            OutlierRejection::None => correspondences.clone(),
+            OutlierRejection::DistanceThreshold(threshold) => {
+                correspondences.filter_by_distance(*threshold)
+            }
+            OutlierRejection::Ransac(config) => {
+                match estimate_pose_ransac(correspondences, lines, config) {
+                    Some(result) => result.inliers,
+                    None => correspondences.filter_by_distance(0.2),
+                }
+            }
+        }
+    }
+}
+
+impl Default for PointToLineIcp {
+    fn default() -> Self {
+        Self::new()
+    }
+}
+
+/// Convenience function for single-shot scan matching.
+///
+/// # Arguments
+/// * `points` - Scan points in robot frame
+/// * `lines` - Map lines in world frame
+/// * `initial_pose` - Initial robot pose in world frame
+///
+/// # Returns
+/// Match result with optimized pose.
+pub fn match_scan(points: &[Point2D], lines: &[Line2D], initial_pose: Pose2D) -> MatchResult {
+    let icp = PointToLineIcp::new();
+    icp.match_scan(points, lines, initial_pose)
+}
+
+/// Match scan with custom configuration.
+pub fn match_scan_with_config(
+    points: &[Point2D],
+    lines: &[Line2D],
+    initial_pose: Pose2D,
+    config: IcpConfig,
+) -> MatchResult {
+    let icp = PointToLineIcp::with_config(config);
+    icp.match_scan(points, lines, initial_pose)
+}
+
+#[cfg(test)]
+mod tests {
+    use super::*;
+    use crate::core::Point2D;
+    use approx::assert_relative_eq;
+
+    fn make_room_lines() -> Vec<Line2D> {
+        // 4m x 4m room centered at origin
+        vec![
+            Line2D::new(Point2D::new(-2.0, -2.0), Point2D::new(2.0, -2.0)), // Bottom
+            Line2D::new(Point2D::new(2.0, -2.0), Point2D::new(2.0, 2.0)),   // Right
+            Line2D::new(Point2D::new(2.0, 2.0), Point2D::new(-2.0, 2.0)),   // Top
+            Line2D::new(Point2D::new(-2.0, 2.0), Point2D::new(-2.0, -2.0)), // Left
+        ]
+    }
+
+    fn make_scan_points_at_origin() -> Vec<Point2D> {
+        // Points as if robot is at origin, looking at walls
+        let mut points = Vec::new();
+
+        // Points seeing the four walls
+        for i in -5..=5 {
+            let x = i as f32 * 0.3;
+            // Bottom wall (y = -2)
+            points.push(Point2D::new(x, -2.0));
+            // Top wall (y = 2)
+            points.push(Point2D::new(x, 2.0));
+        }
+        for i in -5..=5 {
+            let y = i as f32 * 0.3;
+            // Left wall (x = -2)
+            points.push(Point2D::new(-2.0, y));
+            // Right wall (x = 2)
+            points.push(Point2D::new(2.0, y));
+        }
+
+        points
+    }
+
+    #[test]
+    fn test_icp_identity() {
+        let lines = make_room_lines();
+        let points = make_scan_points_at_origin();
+
+        let icp = PointToLineIcp::new();
+        let result = icp.match_scan(&points, &lines, Pose2D::identity());
+
+        assert!(result.converged);
+        assert_relative_eq!(result.pose.x, 0.0, epsilon = 0.1);
+        assert_relative_eq!(result.pose.y, 0.0, epsilon = 0.1);
+        assert_relative_eq!(result.pose.theta.abs(), 0.0, epsilon = 0.1);
+    }
+
+    #[test]
+    fn test_icp_with_offset() {
+        let lines = make_room_lines();
+
+        // Create scan points slightly offset from walls (simulating sensor noise)
+        let mut points = Vec::new();
+        for i in -5..=5 {
+            let x = i as f32 * 0.3;
+            // Points near bottom wall with small offset
+            points.push(Point2D::new(x, -1.95));
+            // Points near top wall with small offset
+            points.push(Point2D::new(x, 1.95));
+        }
+        for i in -5..=5 {
+            let y = i as f32 * 0.3;
+            // Points near left wall with small offset
+            points.push(Point2D::new(-1.95, y));
+            // Points near right wall with small offset
+            points.push(Point2D::new(1.95, y));
+        }
+
+        let config = IcpConfig::default()
+            .with_max_iterations(50)
+            .with_max_correspondence_distance(0.5);
+
+        let icp = PointToLineIcp::with_config(config);
+        let result = icp.match_scan(&points, &lines, Pose2D::identity());
+
+        // Should find correspondences
+        assert!(!result.correspondences.is_empty());
+        // RMS error should be small (points are close to lines)
+        assert!(
+            result.rms_error < 0.2,
+            "RMS error too high: {}",
+            result.rms_error
+        );
+    }
+
+    #[test]
+    fn test_icp_insufficient_points() {
+        let lines = make_room_lines();
+        let points = vec![Point2D::new(0.0, -2.0)]; // Only one point
+
+        let config = IcpConfig::default().with_min_correspondences(5);
+        let icp = PointToLineIcp::with_config(config);
+        let result = icp.match_scan(&points, &lines, Pose2D::identity());
+
+        assert!(!result.converged);
+    }
+
+    #[test]
+    fn test_icp_no_lines() {
+        let lines: Vec<Line2D> = vec![];
+        let points = make_scan_points_at_origin();
+
+        let icp = PointToLineIcp::new();
+        let result = icp.match_scan(&points, &lines, Pose2D::identity());
+
+        assert!(!result.converged);
+        assert!(result.correspondences.is_empty());
+    }
+
+    #[test]
+    fn test_icp_with_ransac() {
+        let lines = make_room_lines();
+        let mut points = make_scan_points_at_origin();
+
+        // Add some outliers
+        points.push(Point2D::new(10.0, 10.0));
+        points.push(Point2D::new(-10.0, -10.0));
+        points.push(Point2D::new(5.0, -5.0));
+
+        let config = IcpConfig::default()
+            .with_outlier_rejection(OutlierRejection::Ransac(RansacConfig::default()));
+
+        let icp = PointToLineIcp::with_config(config);
+        let result = icp.match_scan(&points, &lines, Pose2D::identity());
+
+        // Should still converge despite outliers
+        assert!(result.converged);
+    }
+
+    #[test]
+    fn test_icp_distance_threshold() {
+        let lines = make_room_lines();
+        let mut points = make_scan_points_at_origin();
+
+        // Add outliers far from lines
+        points.push(Point2D::new(0.0, 0.0)); // Center - far from all walls
+
+        let config =
+            IcpConfig::default().with_outlier_rejection(OutlierRejection::DistanceThreshold(0.5));
+
+        let icp = PointToLineIcp::with_config(config);
+        let result = icp.match_scan(&points, &lines, Pose2D::identity());
+
+        assert!(result.converged);
+        // Center point should be filtered out
+        assert!(result.correspondences.len() < points.len());
+    }
+
+    #[test]
+    fn test_match_result_confidence() {
+        let lines = make_room_lines();
+        let points = make_scan_points_at_origin();
+
+        let result = match_scan(&points, &lines, Pose2D::identity());
+
+        assert!(result.converged);
+        assert!(result.confidence > 0.0);
+        assert!(result.is_good_match(0.1));
+    }
+
+    #[test]
+    fn test_convenience_function() {
+        let lines = make_room_lines();
+        let points = make_scan_points_at_origin();
+
+        let result = match_scan(&points, &lines, Pose2D::identity());
+
+        assert!(result.converged);
+    }
+}

vastu-map/src/matching/ransac.rs

@@ -0,0 +1,779 @@
+//! RANSAC-based robust pose estimation.
+//!
+//! Uses Random Sample Consensus to find the best pose transformation
+//! from a set of point-to-line correspondences, robust to outliers.
+
+use rand::Rng;
+
+use crate::core::{Point2D, Pose2D};
+use crate::features::Line2D;
+
+use super::correspondence::{Correspondence, CorrespondenceSet};
+
+/// Configuration for RANSAC pose estimation.
+#[derive(Clone, Debug)]
+pub struct RansacConfig {
+    /// Maximum number of iterations.
+    /// Default: 100
+    pub max_iterations: usize,
+
+    /// Inlier distance threshold (meters).
+    /// Correspondences with distance below this are considered inliers.
+    /// Default: 0.1m
+    pub inlier_threshold: f32,
+
+    /// Minimum inlier ratio required for a valid model.
+    /// If the best model has fewer inliers, the estimation fails.
+    /// Default: 0.3 (30%)
+    pub min_inlier_ratio: f32,
+
+    /// Early termination if inlier ratio exceeds this.
+    /// Default: 0.9 (90%)
+    pub early_termination_ratio: f32,
+
+    /// Minimum number of correspondences for estimation.
+    /// Default: 3
+    pub min_correspondences: usize,
+}
+
+impl Default for RansacConfig {
+    fn default() -> Self {
+        Self {
+            max_iterations: 100,
+            inlier_threshold: 0.1,
+            min_inlier_ratio: 0.3,
+            early_termination_ratio: 0.9,
+            min_correspondences: 3,
+        }
+    }
+}
+
+impl RansacConfig {
+    /// Create a new configuration with default values.
+    pub fn new() -> Self {
+        Self::default()
+    }
+
+    /// Builder-style setter for maximum iterations.
+    pub fn with_max_iterations(mut self, iterations: usize) -> Self {
+        self.max_iterations = iterations;
+        self
+    }
+
+    /// Builder-style setter for inlier threshold.
+    pub fn with_inlier_threshold(mut self, meters: f32) -> Self {
+        self.inlier_threshold = meters;
+        self
+    }
+
+    /// Builder-style setter for minimum inlier ratio.
+    pub fn with_min_inlier_ratio(mut self, ratio: f32) -> Self {
+        self.min_inlier_ratio = ratio;
+        self
+    }
+
+    /// Builder-style setter for minimum correspondences.
+    pub fn with_min_correspondences(mut self, count: usize) -> Self {
+        self.min_correspondences = count;
+        self
+    }
+
+    /// Builder-style setter for early termination ratio.
+    pub fn with_early_termination_ratio(mut self, ratio: f32) -> Self {
+        self.early_termination_ratio = ratio;
+        self
+    }
+}
+
+/// Result of RANSAC pose estimation.
+#[derive(Clone, Debug)]
+pub struct RansacResult {
+    /// Estimated pose transformation.
+    pub pose: Pose2D,
+    /// Inlier correspondences.
+    pub inliers: CorrespondenceSet,
+    /// Inlier ratio (inliers / total correspondences).
+    pub inlier_ratio: f32,
+    /// Number of iterations performed.
+    pub iterations: usize,
+}
+
+/// Estimate pose transformation using RANSAC.
+///
+/// Samples random subsets of correspondences, computes pose hypotheses,
+/// and finds the pose with the most inliers.
+///
+/// # Arguments
+/// * `correspondences` - Point-to-line correspondences
+/// * `lines` - Map lines
+/// * `config` - RANSAC configuration
+///
+/// # Returns
+/// RANSAC result if successful, None if no valid model found.
+pub fn estimate_pose_ransac(
+    correspondences: &CorrespondenceSet,
+    lines: &[Line2D],
+    config: &RansacConfig,
+) -> Option<RansacResult> {
+    let n = correspondences.len();
+    if n < config.min_correspondences {
+        return None;
+    }
+
+    let mut rng = rand::rng();
+    let mut best_inliers = CorrespondenceSet::new();
+    let mut best_pose = Pose2D::identity();
+    let mut best_count = 0;
+
+    for iteration in 0..config.max_iterations {
+        // Sample 2 random correspondences for minimal set
+        let idx1 = rng.random_range(0..n);
+        let mut idx2 = rng.random_range(0..n);
+        while idx2 == idx1 {
+            idx2 = rng.random_range(0..n);
+        }
+
+        let corr1 = &correspondences.correspondences[idx1];
+        let corr2 = &correspondences.correspondences[idx2];
+
+        // Get the corresponding lines
+        let line1 = &lines[corr1.line_idx];
+        let line2 = &lines[corr2.line_idx];
+
+        // Compute pose hypothesis from 2 correspondences
+        let Some(pose) = estimate_pose_from_two_correspondences(corr1, corr2, line1, line2) else {
+            continue;
+        };
+
+        // Count inliers
+        let (inlier_count, inliers) =
+            count_inliers(correspondences, lines, &pose, config.inlier_threshold);
+
+        if inlier_count > best_count {
+            best_count = inlier_count;
+            best_inliers = inliers;
+            best_pose = pose;
+
+            // Early termination check
+            let ratio = inlier_count as f32 / n as f32;
+            if ratio >= config.early_termination_ratio {
+                return Some(RansacResult {
+                    pose: best_pose,
+                    inliers: best_inliers,
+                    inlier_ratio: ratio,
+                    iterations: iteration + 1,
+                });
+            }
+        }
+    }
+
+    // Check minimum inlier ratio
+    let inlier_ratio = best_count as f32 / n as f32;
+    if inlier_ratio < config.min_inlier_ratio {
+        return None;
+    }
+
+    Some(RansacResult {
+        pose: best_pose,
+        inliers: best_inliers,
+        inlier_ratio,
+        iterations: config.max_iterations,
+    })
+}
+
+/// Estimate pose from two point-to-line correspondences.
+///
+/// Uses the constraint that each point must lie on its corresponding line
+/// after transformation.
+fn estimate_pose_from_two_correspondences(
+    corr1: &Correspondence,
+    corr2: &Correspondence,
+    line1: &Line2D,
+    line2: &Line2D,
+) -> Option<Pose2D> {
+    let p1 = corr1.point;
+    let p2 = corr2.point;
+
+    // Get projection points on lines (where points should map to)
+    let target1 = line1.point_at(corr1.projection_t);
+    let target2 = line2.point_at(corr2.projection_t);
+
+    // Compute the transformation that maps p1->target1 and p2->target2
+    // This is a rigid body transformation (rotation + translation)
+
+    // Vector between source points
+    let src_vec = p2 - p1;
+    let src_len = src_vec.length();
+
+    if src_len < 1e-6 {
+        // Points too close together
+        return None;
+    }
+
+    // Vector between target points
+    let dst_vec = target2 - target1;
+    let dst_len = dst_vec.length();
+
+    if dst_len < 1e-6 {
+        // Targets too close together
+        return None;
+    }
+
+    // Check that lengths are approximately equal (rigid transformation)
+    let length_ratio = dst_len / src_len;
+    if (length_ratio - 1.0).abs() > 0.3 {
+        // Too much scale difference - invalid correspondences
+        return None;
+    }
+
+    // Compute rotation angle
+    let src_angle = src_vec.y.atan2(src_vec.x);
+    let dst_angle = dst_vec.y.atan2(dst_vec.x);
+    let theta = dst_angle - src_angle;
+
+    // Normalize angle
+    let theta = crate::core::math::normalize_angle(theta);
+
+    // Compute translation using centroid method
+    let src_centroid = (p1 + p2) * 0.5;
+    let dst_centroid = (target1 + target2) * 0.5;
+
+    // Rotate source centroid
+    let (sin, cos) = theta.sin_cos();
+    let rotated_centroid = Point2D::new(
+        src_centroid.x * cos - src_centroid.y * sin,
+        src_centroid.x * sin + src_centroid.y * cos,
+    );
+
+    // Translation is the difference
+    let tx = dst_centroid.x - rotated_centroid.x;
+    let ty = dst_centroid.y - rotated_centroid.y;
+
+    Some(Pose2D::new(tx, ty, theta))
+}
+
+/// Count inliers for a given pose hypothesis.
+fn count_inliers(
+    correspondences: &CorrespondenceSet,
+    lines: &[Line2D],
+    pose: &Pose2D,
+    threshold: f32,
+) -> (usize, CorrespondenceSet) {
+    let mut inliers = CorrespondenceSet::with_capacity(correspondences.len());
+    let (sin, cos) = pose.theta.sin_cos();
+
+    for corr in correspondences.iter() {
+        // Transform the point
+        let p = corr.point;
+        let transformed = Point2D::new(
+            p.x * cos - p.y * sin + pose.x,
+            p.x * sin + p.y * cos + pose.y,
+        );
+
+        // Compute distance to corresponding line
+        let line = &lines[corr.line_idx];
+        let distance = line.distance_to_point(transformed);
+
+        if distance <= threshold {
+            // Create updated correspondence with new position and distance
+            inliers.push(Correspondence::new(
+                corr.point_idx,
+                corr.line_idx,
+                transformed,
+                distance,
+                line.project_point(transformed),
+            ));
+        }
+    }
+
+    (inliers.len(), inliers)
+}
+
+/// Refine pose estimate using all inliers (least squares).
+///
+/// After RANSAC finds a good initial pose, this function refines it
+/// using all inliers for better accuracy.
+pub fn refine_pose_with_inliers(
+    inliers: &CorrespondenceSet,
+    lines: &[Line2D],
+    initial_pose: Pose2D,
+    iterations: usize,
+) -> Pose2D {
+    if inliers.is_empty() {
+        return initial_pose;
+    }
+
+    let mut pose = initial_pose;
+
+    for _ in 0..iterations {
+        // Accumulate gradients
+        let mut sum_dx = 0.0;
+        let mut sum_dy = 0.0;
+        let mut sum_dtheta = 0.0;
+        let mut count = 0.0;
+
+        let (sin, cos) = pose.theta.sin_cos();
+
+        for corr in inliers.iter() {
+            let p = corr.point;
+
+            // Transform point by current pose
+            let tx = p.x * cos - p.y * sin + pose.x;
+            let ty = p.x * sin + p.y * cos + pose.y;
+            let transformed = Point2D::new(tx, ty);
+
+            let line = &lines[corr.line_idx];
+
+            // Get line normal (perpendicular to line direction)
+            let normal = line.normal();
+
+            // Signed distance (positive = left of line)
+            let signed_dist = line.signed_distance_to_point(transformed);
+
+            // Gradient of distance w.r.t. pose parameters
+            // d(dist)/d(tx) = normal.x
+            // d(dist)/d(ty) = normal.y
+            // d(dist)/d(theta) ≈ cross(p_rotated, normal)
+            let p_rotated = Point2D::new(tx - pose.x, ty - pose.y);
+
+            sum_dx += signed_dist * normal.x;
+            sum_dy += signed_dist * normal.y;
+            sum_dtheta += signed_dist * (p_rotated.x * normal.y - p_rotated.y * normal.x);
+            count += 1.0;
+        }
+
+        if count < 1.0 {
+            break;
+        }
+
+        // Apply gradient descent step
+        let step = 0.5;
+        pose.x -= step * sum_dx / count;
+        pose.y -= step * sum_dy / count;
+        pose.theta -= step * sum_dtheta / count;
+        pose.theta = crate::core::math::normalize_angle(pose.theta);
+    }
+
+    pose
+}
+
+#[cfg(test)]
+mod tests {
+    use super::*;
+    use crate::core::Point2D;
+    use approx::assert_relative_eq;
+
+    fn make_test_lines() -> Vec<Line2D> {
+        vec![
+            // Horizontal line at y=0
+            Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(5.0, 0.0)),
+            // Vertical line at x=5
+            Line2D::new(Point2D::new(5.0, 0.0), Point2D::new(5.0, 5.0)),
+            // Horizontal line at y=5
+            Line2D::new(Point2D::new(5.0, 5.0), Point2D::new(0.0, 5.0)),
+            // Vertical line at x=0
+            Line2D::new(Point2D::new(0.0, 5.0), Point2D::new(0.0, 0.0)),
+        ]
+    }
+
+    fn make_test_correspondences(offset: Point2D) -> CorrespondenceSet {
+        let mut corrs = CorrespondenceSet::new();
+
+        // Points that should match to the square walls
+        // After applying translation, these should land on the lines
+        corrs.push(Correspondence::new(
+            0,
+            0,
+            Point2D::new(1.0, 0.0) - offset,
+            0.0,
+            0.2,
+        ));
+        corrs.push(Correspondence::new(
+            1,
+            0,
+            Point2D::new(2.0, 0.0) - offset,
+            0.0,
+            0.4,
+        ));
+        corrs.push(Correspondence::new(
+            2,
+            1,
+            Point2D::new(5.0, 1.0) - offset,
+            0.0,
+            0.2,
+        ));
+        corrs.push(Correspondence::new(
+            3,
+            1,
+            Point2D::new(5.0, 2.0) - offset,
+            0.0,
+            0.4,
+        ));
+        corrs.push(Correspondence::new(
+            4,
+            2,
+            Point2D::new(4.0, 5.0) - offset,
+            0.0,
+            0.2,
+        ));
+        corrs.push(Correspondence::new(
+            5,
+            3,
+            Point2D::new(0.0, 3.0) - offset,
+            0.0,
+            0.6,
+        ));
+
+        corrs
+    }
+
+    #[test]
+    fn test_ransac_translation() {
+        let lines = make_test_lines();
+        let offset = Point2D::new(0.5, 0.3);
+        let corrs = make_test_correspondences(offset);
+
+        let config = RansacConfig::default()
+            .with_max_iterations(200)
+            .with_inlier_threshold(0.2);
+
+        let result = estimate_pose_ransac(&corrs, &lines, &config);
+
+        assert!(result.is_some(), "RANSAC should find a solution");
+        let result = result.unwrap();
+
+        // The pose should approximately recover the offset
+        assert_relative_eq!(result.pose.x, offset.x, epsilon = 0.3);
+        assert_relative_eq!(result.pose.y, offset.y, epsilon = 0.3);
+        assert!(result.inlier_ratio >= 0.3);
+    }
+
+    #[test]
+    fn test_ransac_insufficient_correspondences() {
+        let lines = make_test_lines();
+        let mut corrs = CorrespondenceSet::new();
+        corrs.push(Correspondence::new(0, 0, Point2D::new(1.0, 0.0), 0.0, 0.2));
+
+        let config = RansacConfig::default().with_min_correspondences(3);
+        let result = estimate_pose_ransac(&corrs, &lines, &config);
+
+        assert!(result.is_none());
+    }
+
+    #[test]
+    fn test_pose_from_two_correspondences() {
+        // Two points on a horizontal line
+        let line1 = Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(10.0, 0.0));
+        let line2 = Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(10.0, 0.0));
+
+        // Points that are offset from where they should be
+        let corr1 = Correspondence::new(0, 0, Point2D::new(1.0, 1.0), 1.0, 0.1);
+        let corr2 = Correspondence::new(1, 0, Point2D::new(5.0, 1.0), 1.0, 0.5);
+
+        let pose = estimate_pose_from_two_correspondences(&corr1, &corr2, &line1, &line2);
+
+        assert!(pose.is_some());
+        let pose = pose.unwrap();
+
+        // Should translate down by approximately 1
+        assert_relative_eq!(pose.y, -1.0, epsilon = 0.1);
+        assert_relative_eq!(pose.theta.abs(), 0.0, epsilon = 0.1);
+    }
+
+    #[test]
+    fn test_count_inliers() {
+        let lines = vec![Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(10.0, 0.0))];
+
+        let mut corrs = CorrespondenceSet::new();
+        // Point at y=0.05 - should be inlier
+        corrs.push(Correspondence::new(
+            0,
+            0,
+            Point2D::new(1.0, 0.05),
+            0.05,
+            0.1,
+        ));
+        // Point at y=0.5 - should be outlier with threshold 0.1
+        corrs.push(Correspondence::new(1, 0, Point2D::new(2.0, 0.5), 0.5, 0.2));
+
+        let pose = Pose2D::identity();
+        let (count, inliers) = count_inliers(&corrs, &lines, &pose, 0.1);
+
+        assert_eq!(count, 1);
+        assert_eq!(inliers.len(), 1);
+    }
+
+    #[test]
+    fn test_refine_pose() {
+        let lines = vec![
+            Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(10.0, 0.0)),
+            Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(0.0, 10.0)),
+        ];
+
+        let mut corrs = CorrespondenceSet::new();
+        // Points slightly off the lines
+        corrs.push(Correspondence::new(0, 0, Point2D::new(1.0, 0.1), 0.1, 0.1));
+        corrs.push(Correspondence::new(1, 0, Point2D::new(5.0, 0.1), 0.1, 0.5));
+        corrs.push(Correspondence::new(2, 1, Point2D::new(0.1, 3.0), 0.1, 0.3));
+
+        let initial = Pose2D::identity();
+        let refined = refine_pose_with_inliers(&corrs, &lines, initial, 10);
+
+        // Pose should shift to better align points with lines
+        // Points have positive y offset, so pose.y should be negative to compensate
+        assert!(refined.y < 0.0 || refined.x < 0.0);
+    }
+
+    // === Additional edge case tests ===
+
+    #[test]
+    fn test_ransac_high_outlier_ratio() {
+        // Test with >50% outliers - RANSAC should still find a valid solution
+        // if the inlier threshold is set appropriately
+        let lines = make_test_lines();
+        let offset = Point2D::new(0.3, 0.2);
+
+        let mut corrs = CorrespondenceSet::new();
+
+        // Add 3 inliers (points that will land on lines after translation)
+        corrs.push(Correspondence::new(
+            0,
+            0,
+            Point2D::new(1.0, 0.0) - offset,
+            0.0,
+            0.2,
+        ));
+        corrs.push(Correspondence::new(
+            1,
+            0,
+            Point2D::new(3.0, 0.0) - offset,
+            0.0,
+            0.6,
+        ));
+        corrs.push(Correspondence::new(
+            2,
+            1,
+            Point2D::new(5.0, 2.0) - offset,
+            0.0,
+            0.4,
+        ));
+
+        // Add 7 outliers (points far from any line)
+        for i in 0..7 {
+            corrs.push(Correspondence::new(
+                3 + i,
+                0,
+                Point2D::new(2.5 + i as f32 * 0.1, 2.5), // Center of square, far from walls
+                2.5,
+                0.5,
+            ));
+        }
+
+        let config = RansacConfig::default()
+            .with_max_iterations(500)
+            .with_inlier_threshold(0.3)
+            .with_min_inlier_ratio(0.2); // Lower threshold since we have many outliers
+
+        let result = estimate_pose_ransac(&corrs, &lines, &config);
+
+        // Should find a solution despite high outlier ratio
+        assert!(
+            result.is_some(),
+            "RANSAC should find solution with 70% outliers"
+        );
+        let result = result.unwrap();
+        assert!(result.inlier_ratio >= 0.2);
+    }
+
+    #[test]
+    fn test_ransac_all_outliers() {
+        // Test with all outliers - RANSAC should return None
+        let lines = make_test_lines();
+
+        let mut corrs = CorrespondenceSet::new();
+
+        // All points are far from any line (center of the square)
+        for i in 0..10 {
+            corrs.push(Correspondence::new(
+                i,
+                0,
+                Point2D::new(2.5 + i as f32 * 0.05, 2.5 + i as f32 * 0.05),
+                2.5,
+                0.5,
+            ));
+        }
+
+        let config = RansacConfig::default()
+            .with_max_iterations(100)
+            .with_inlier_threshold(0.1)
+            .with_min_inlier_ratio(0.5);
+
+        let result = estimate_pose_ransac(&corrs, &lines, &config);
+
+        // Should fail - no valid model with >50% inliers
+        assert!(result.is_none(), "RANSAC should fail with all outliers");
+    }
+
+    #[test]
+    fn test_ransac_early_termination() {
+        // Test that early termination works when we have very good inliers
+        let lines = make_test_lines();
+        let offset = Point2D::new(0.2, 0.1);
+
+        // Create correspondences where all points are inliers
+        let mut corrs = CorrespondenceSet::new();
+        corrs.push(Correspondence::new(
+            0,
+            0,
+            Point2D::new(1.0, 0.0) - offset,
+            0.0,
+            0.2,
+        ));
+        corrs.push(Correspondence::new(
+            1,
+            0,
+            Point2D::new(2.0, 0.0) - offset,
+            0.0,
+            0.4,
+        ));
+        corrs.push(Correspondence::new(
+            2,
+            0,
+            Point2D::new(3.0, 0.0) - offset,
+            0.0,
+            0.6,
+        ));
+        corrs.push(Correspondence::new(
+            3,
+            0,
+            Point2D::new(4.0, 0.0) - offset,
+            0.0,
+            0.8,
+        ));
+
+        let config = RansacConfig::default()
+            .with_max_iterations(1000) // Many iterations
+            .with_inlier_threshold(0.3)
+            .with_early_termination_ratio(0.8); // Should terminate early
+
+        let result = estimate_pose_ransac(&corrs, &lines, &config);
+
+        assert!(result.is_some());
+        let result = result.unwrap();
+
+        // Should have terminated early (before max_iterations)
+        assert!(
+            result.iterations < 1000,
+            "Should terminate early, but used {} iterations",
+            result.iterations
+        );
+        assert!(result.inlier_ratio >= 0.8);
+    }
+
+    #[test]
+    fn test_ransac_degenerate_geometry_collinear() {
+        // Test with degenerate case: all correspondences point to same line
+        // This should still work as long as we can compute a valid transformation
+        let lines = vec![Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(10.0, 0.0))];
+
+        let offset = Point2D::new(0.0, 0.5); // Only Y offset
+
+        let mut corrs = CorrespondenceSet::new();
+        // All points on same line but with Y offset
+        corrs.push(Correspondence::new(
+            0,
+            0,
+            Point2D::new(1.0, 0.0) - offset,
+            0.5,
+            0.1,
+        ));
+        corrs.push(Correspondence::new(
+            1,
+            0,
+            Point2D::new(3.0, 0.0) - offset,
+            0.5,
+            0.3,
+        ));
+        corrs.push(Correspondence::new(
+            2,
+            0,
+            Point2D::new(5.0, 0.0) - offset,
+            0.5,
+            0.5,
+        ));
+        corrs.push(Correspondence::new(
+            3,
+            0,
+            Point2D::new(7.0, 0.0) - offset,
+            0.5,
+            0.7,
+        ));
+
+        let config = RansacConfig::default()
+            .with_max_iterations(200)
+            .with_inlier_threshold(0.3);
+
+        let result = estimate_pose_ransac(&corrs, &lines, &config);
+
+        // Should find a solution (pure translation along Y)
+        assert!(result.is_some());
+        let result = result.unwrap();
+        assert_relative_eq!(result.pose.y, offset.y, epsilon = 0.3);
+    }
+
+    #[test]
+    fn test_ransac_points_too_close() {
+        // Test with points that are too close together
+        let line1 = Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(10.0, 0.0));
+        let line2 = Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(10.0, 0.0));
+
+        // Two points that are extremely close (< 1e-6 apart)
+        let corr1 = Correspondence::new(0, 0, Point2D::new(1.0, 0.5), 0.5, 0.1);
+        let corr2 = Correspondence::new(1, 0, Point2D::new(1.0 + 1e-7, 0.5), 0.5, 0.1);
+
+        let pose = estimate_pose_from_two_correspondences(&corr1, &corr2, &line1, &line2);
+
+        // Should return None because points are too close
+        assert!(pose.is_none(), "Should fail with points too close together");
+    }
+
+    #[test]
+    fn test_ransac_config_builders() {
+        // Test all config builder methods
+        let config = RansacConfig::new()
+            .with_max_iterations(50)
+            .with_inlier_threshold(0.2)
+            .with_min_inlier_ratio(0.4)
+            .with_min_correspondences(5);
+
+        assert_eq!(config.max_iterations, 50);
+        assert_eq!(config.inlier_threshold, 0.2);
+        assert_eq!(config.min_inlier_ratio, 0.4);
+        assert_eq!(config.min_correspondences, 5);
+    }
+
+    #[test]
+    fn test_ransac_empty_correspondences() {
+        let lines = make_test_lines();
+        let corrs = CorrespondenceSet::new();
+
+        let config = RansacConfig::default();
+        let result = estimate_pose_ransac(&corrs, &lines, &config);
+
+        assert!(result.is_none());
+    }
+
+    #[test]
+    fn test_refine_pose_empty_inliers() {
+        let lines = make_test_lines();
+        let corrs = CorrespondenceSet::new();
+        let initial = Pose2D::new(1.0, 2.0, 0.5);
+
+        let refined = refine_pose_with_inliers(&corrs, &lines, initial, 10);
+
+        // Should return initial pose unchanged
+        assert_relative_eq!(refined.x, initial.x);
+        assert_relative_eq!(refined.y, initial.y);
+        assert_relative_eq!(refined.theta, initial.theta);
+    }
+}

vastu-map/src/matching/scratch.rs

@@ -0,0 +1,390 @@
+//! Pre-allocated scratch space for ICP hot paths.
+//!
+//! This module provides buffer reuse to eliminate allocations during the
+//! ICP inner loop, significantly improving performance on embedded systems.
+//!
+//! # Performance Impact
+//!
+//! Without scratch space, each ICP iteration allocates:
+//! - Transformed points buffer (~360 points × 8 bytes = ~3KB)
+//! - Distance buffer (~100 lines × 4 bytes × 360 points = ~140KB)
+//! - Projection buffer (same as distance)
+//! - Correspondence set (~360 × 24 bytes = ~9KB)
+//!
+//! With scratch space, these allocations happen once and are reused.
+
+use crate::core::Point2D;
+use crate::features::LineCollection;
+
+use super::correspondence::{Correspondence, CorrespondenceSet};
+use super::nearest_neighbor::NearestNeighborConfig;
+
+/// Pre-allocated scratch space for ICP operations.
+///
+/// Reuse this struct across multiple `match_scan` calls to avoid
+/// per-iteration allocations. Create once, use many times.
+///
+/// # Example
+/// ```rust,ignore
+/// use vastu_map::matching::{IcpScratchSpace, PointToLineIcp};
+///
+/// // Create scratch space sized for typical scans
+/// let mut scratch = IcpScratchSpace::new(400, 200);
+///
+/// let icp = PointToLineIcp::new();
+/// for scan in scans {
+///     let result = icp.match_scan_with_scratch(&scan, &map, pose, &mut scratch);
+/// }
+/// ```
+#[derive(Clone, Debug)]
+pub struct IcpScratchSpace {
+    /// Buffer for transformed points.
+    pub(crate) transformed_xs: Vec<f32>,
+    pub(crate) transformed_ys: Vec<f32>,
+
+    /// Buffer for distances to all lines (per point).
+    pub(crate) distances: Vec<f32>,
+
+    /// Buffer for projection parameters (per point).
+    pub(crate) projections: Vec<f32>,
+
+    /// Reusable correspondence set.
+    pub(crate) correspondences: CorrespondenceSet,
+}
+
+impl IcpScratchSpace {
+    /// Create new scratch space with capacity hints.
+    ///
+    /// # Arguments
+    /// * `max_points` - Expected maximum scan points (e.g., 360 for a typical lidar)
+    /// * `max_lines` - Expected maximum map lines (e.g., 100-200 for indoor)
+    pub fn new(max_points: usize, max_lines: usize) -> Self {
+        Self {
+            transformed_xs: Vec::with_capacity(max_points),
+            transformed_ys: Vec::with_capacity(max_points),
+            distances: Vec::with_capacity(max_lines),
+            projections: Vec::with_capacity(max_lines),
+            correspondences: CorrespondenceSet::with_capacity(max_points),
+        }
+    }
+
+    /// Create with default capacity (360 points, 200 lines).
+    pub fn default_capacity() -> Self {
+        Self::new(360, 200)
+    }
+
+    /// Clear buffers for next iteration (keeps capacity).
+    #[inline]
+    pub fn clear(&mut self) {
+        self.transformed_xs.clear();
+        self.transformed_ys.clear();
+        self.distances.clear();
+        self.projections.clear();
+        self.correspondences.clear();
+    }
+
+    /// Transform points by pose into internal buffer.
+    ///
+    /// After calling this, `transformed_point()` can be used to access results.
+    #[inline]
+    pub fn transform_points(&mut self, points: &[Point2D], sin: f32, cos: f32, tx: f32, ty: f32) {
+        self.transformed_xs.clear();
+        self.transformed_ys.clear();
+
+        // Pre-extend to avoid reallocations during push
+        self.transformed_xs.reserve(points.len());
+        self.transformed_ys.reserve(points.len());
+
+        for p in points {
+            let new_x = p.x * cos - p.y * sin + tx;
+            let new_y = p.x * sin + p.y * cos + ty;
+            self.transformed_xs.push(new_x);
+            self.transformed_ys.push(new_y);
+        }
+    }
+
+    /// Get a transformed point by index.
+    #[inline]
+    pub fn transformed_point(&self, index: usize) -> Point2D {
+        Point2D::new(self.transformed_xs[index], self.transformed_ys[index])
+    }
+
+    /// Get number of transformed points.
+    #[inline]
+    pub fn num_transformed(&self) -> usize {
+        self.transformed_xs.len()
+    }
+
+    /// Find correspondences using pre-allocated buffers.
+    ///
+    /// This is the zero-allocation hot path for ICP iterations.
+    pub fn find_correspondences(&mut self, lines: &LineCollection, config: &NearestNeighborConfig) {
+        self.correspondences.clear();
+
+        if self.transformed_xs.is_empty() || lines.is_empty() {
+            return;
+        }
+
+        let min_t = -config.max_projection_extension;
+        let max_t = 1.0 + config.max_projection_extension;
+        let n_lines = lines.len();
+        let n_points = self.transformed_xs.len();
+
+        // Ensure buffers are large enough
+        self.distances.resize(n_lines, 0.0);
+        self.projections.resize(n_lines, 0.0);
+
+        for point_idx in 0..n_points {
+            let point = Point2D::new(
+                self.transformed_xs[point_idx],
+                self.transformed_ys[point_idx],
+            );
+
+            // Compute distances and projections into buffers
+            lines.distances_to_point_into(point, &mut self.distances);
+            self.compute_projections_into(point, lines);
+
+            let mut best_distance = config.max_distance;
+            let mut best_corr: Option<Correspondence> = None;
+
+            for line_idx in 0..n_lines {
+                let t = self.projections[line_idx];
+                let distance = self.distances[line_idx];
+
+                // Check projection bounds
+                if t < min_t || t > max_t {
+                    continue;
+                }
+
+                // Check distance threshold
+                if distance > config.max_distance {
+                    continue;
+                }
+
+                if config.unique_per_point {
+                    if distance < best_distance {
+                        best_distance = distance;
+                        best_corr =
+                            Some(Correspondence::new(point_idx, line_idx, point, distance, t));
+                    }
+                } else {
+                    self.correspondences
+                        .push(Correspondence::new(point_idx, line_idx, point, distance, t));
+                }
+            }
+
+            if config.unique_per_point
+                && let Some(corr) = best_corr
+            {
+                self.correspondences.push(corr);
+            }
+        }
+    }
+
+    /// Compute projection parameters into the projections buffer.
+    ///
+    /// Formula: t = dot(point - start, end - start) / |end - start|²
+    fn compute_projections_into(&mut self, point: Point2D, lines: &LineCollection) {
+        let n = lines.len();
+
+        for i in 0..n {
+            let sx = lines.start_xs[i];
+            let sy = lines.start_ys[i];
+            let ex = lines.end_xs[i];
+            let ey = lines.end_ys[i];
+
+            let dx = ex - sx;
+            let dy = ey - sy;
+            let len_sq = dx * dx + dy * dy;
+
+            if len_sq < f32::EPSILON {
+                self.projections[i] = 0.5; // Degenerate line
+            } else {
+                let to_px = point.x - sx;
+                let to_py = point.y - sy;
+                self.projections[i] = (to_px * dx + to_py * dy) / len_sq;
+            }
+        }
+    }
+
+    /// Take the correspondences out of the scratch space.
+    ///
+    /// This moves the correspondence set, leaving an empty one in the scratch space.
+    /// Use `clear()` to reset for the next iteration instead if you don't need ownership.
+    #[inline]
+    pub fn take_correspondences(&mut self) -> CorrespondenceSet {
+        std::mem::take(&mut self.correspondences)
+    }
+
+    /// Get a reference to the current correspondences.
+    #[inline]
+    pub fn correspondences(&self) -> &CorrespondenceSet {
+        &self.correspondences
+    }
+
+    /// Get the number of correspondences found.
+    #[inline]
+    pub fn num_correspondences(&self) -> usize {
+        self.correspondences.len()
+    }
+}
+
+impl Default for IcpScratchSpace {
+    fn default() -> Self {
+        Self::default_capacity()
+    }
+}
+
+#[cfg(test)]
+mod tests {
+    use super::*;
+    use crate::features::Line2D;
+
+    fn make_room_lines() -> LineCollection {
+        let lines = vec![
+            Line2D::new(Point2D::new(-2.0, -2.0), Point2D::new(2.0, -2.0)), // Bottom
+            Line2D::new(Point2D::new(2.0, -2.0), Point2D::new(2.0, 2.0)),   // Right
+            Line2D::new(Point2D::new(2.0, 2.0), Point2D::new(-2.0, 2.0)),   // Top
+            Line2D::new(Point2D::new(-2.0, 2.0), Point2D::new(-2.0, -2.0)), // Left
+        ];
+        LineCollection::from_lines(&lines)
+    }
+
+    #[test]
+    fn test_scratch_space_creation() {
+        let scratch = IcpScratchSpace::new(100, 50);
+        // Verify buffers have the expected capacity
+        assert!(scratch.transformed_xs.capacity() >= 100);
+        assert!(scratch.distances.capacity() >= 50);
+    }
+
+    #[test]
+    fn test_transform_points() {
+        let mut scratch = IcpScratchSpace::new(10, 10);
+        let points = vec![Point2D::new(1.0, 0.0), Point2D::new(0.0, 1.0)];
+
+        // Identity transform
+        scratch.transform_points(&points, 0.0, 1.0, 0.0, 0.0);
+
+        assert_eq!(scratch.num_transformed(), 2);
+        let p0 = scratch.transformed_point(0);
+        let p1 = scratch.transformed_point(1);
+
+        assert!((p0.x - 1.0).abs() < 1e-6);
+        assert!((p0.y - 0.0).abs() < 1e-6);
+        assert!((p1.x - 0.0).abs() < 1e-6);
+        assert!((p1.y - 1.0).abs() < 1e-6);
+    }
+
+    #[test]
+    fn test_transform_with_rotation() {
+        let mut scratch = IcpScratchSpace::new(10, 10);
+        let points = vec![Point2D::new(1.0, 0.0)];
+
+        // 90 degree rotation
+        let (sin, cos) = std::f32::consts::FRAC_PI_2.sin_cos();
+        scratch.transform_points(&points, sin, cos, 0.0, 0.0);
+
+        let p = scratch.transformed_point(0);
+        assert!((p.x - 0.0).abs() < 1e-6);
+        assert!((p.y - 1.0).abs() < 1e-6);
+    }
+
+    #[test]
+    fn test_find_correspondences_basic() {
+        let mut scratch = IcpScratchSpace::new(100, 10);
+        let lines = make_room_lines();
+
+        // Points near the walls
+        let points = vec![
+            Point2D::new(0.0, -1.95), // Near bottom wall
+            Point2D::new(1.95, 0.0),  // Near right wall
+            Point2D::new(0.0, 1.95),  // Near top wall
+            Point2D::new(-1.95, 0.0), // Near left wall
+        ];
+
+        // Identity transform
+        scratch.transform_points(&points, 0.0, 1.0, 0.0, 0.0);
+
+        let config = NearestNeighborConfig::default();
+        scratch.find_correspondences(&lines, &config);
+
+        assert_eq!(scratch.num_correspondences(), 4);
+    }
+
+    #[test]
+    fn test_find_correspondences_with_threshold() {
+        let mut scratch = IcpScratchSpace::new(100, 10);
+        let lines = make_room_lines();
+
+        // One point near wall, one far from walls
+        let points = vec![
+            Point2D::new(0.0, -1.95), // Near bottom wall (0.05m away)
+            Point2D::new(0.0, 0.0),   // Center (2.0m from all walls)
+        ];
+
+        scratch.transform_points(&points, 0.0, 1.0, 0.0, 0.0);
+
+        let config = NearestNeighborConfig::default().with_max_distance(0.5);
+        scratch.find_correspondences(&lines, &config);
+
+        // Only the first point should match
+        assert_eq!(scratch.num_correspondences(), 1);
+    }
+
+    #[test]
+    fn test_clear_reuse() {
+        let mut scratch = IcpScratchSpace::new(100, 10);
+        let lines = make_room_lines();
+        let points = vec![Point2D::new(0.0, -1.95)];
+
+        // First iteration
+        scratch.transform_points(&points, 0.0, 1.0, 0.0, 0.0);
+        let config = NearestNeighborConfig::default();
+        scratch.find_correspondences(&lines, &config);
+        assert_eq!(scratch.num_correspondences(), 1);
+
+        // Clear and reuse
+        scratch.clear();
+        assert_eq!(scratch.num_correspondences(), 0);
+        assert_eq!(scratch.num_transformed(), 0);
+
+        // Second iteration
+        scratch.transform_points(&points, 0.0, 1.0, 0.0, 0.0);
+        scratch.find_correspondences(&lines, &config);
+        assert_eq!(scratch.num_correspondences(), 1);
+    }
+
+    #[test]
+    fn test_take_correspondences() {
+        let mut scratch = IcpScratchSpace::new(100, 10);
+        let lines = make_room_lines();
+        let points = vec![Point2D::new(0.0, -1.95)];
+
+        scratch.transform_points(&points, 0.0, 1.0, 0.0, 0.0);
+        let config = NearestNeighborConfig::default();
+        scratch.find_correspondences(&lines, &config);
+
+        let corrs = scratch.take_correspondences();
+        assert_eq!(corrs.len(), 1);
+        assert_eq!(scratch.num_correspondences(), 0); // Should be empty after take
+    }
+
+    #[test]
+    fn test_empty_inputs() {
+        let mut scratch = IcpScratchSpace::new(100, 10);
+        let lines = LineCollection::new();
+        let config = NearestNeighborConfig::default();
+
+        // Empty points
+        scratch.transform_points(&[], 0.0, 1.0, 0.0, 0.0);
+        scratch.find_correspondences(&lines, &config);
+        assert_eq!(scratch.num_correspondences(), 0);
+
+        // Empty lines
+        let points = vec![Point2D::new(0.0, 0.0)];
+        scratch.transform_points(&points, 0.0, 1.0, 0.0, 0.0);
+        scratch.find_correspondences(&lines, &config);
+        assert_eq!(scratch.num_correspondences(), 0);
+    }
+}

vastu-map/src/query/frontier.rs

@@ -0,0 +1,575 @@
+//! Frontier detection for exploration.
+//!
+//! Frontiers are boundaries between known and unknown space.
+//! In a VectorMap, frontiers are typically unconnected line endpoints
+//! that indicate areas that haven't been fully explored.
+
+use std::collections::HashMap;
+
+use crate::Frontier;
+use crate::core::Point2D;
+use crate::features::{Line2D, LineCollection};
+
+/// Spatial hash grid for endpoint proximity queries.
+/// Maps (cell_x, cell_y) -> Vec<(point, line_index, is_start_endpoint)>
+type EndpointGrid = HashMap<(i32, i32), Vec<(Point2D, usize, bool)>>;
+
+/// Configuration for frontier detection.
+#[derive(Clone, Debug)]
+pub struct FrontierConfig {
+    /// Maximum distance for endpoints to be considered "connected" (meters).
+    /// Endpoints closer than this are not frontiers.
+    /// Default: 0.3m
+    pub connection_distance: f32,
+
+    /// Minimum distance from robot for a valid frontier (meters).
+    /// Frontiers too close are ignored.
+    /// Default: 0.5m
+    pub min_distance_from_robot: f32,
+
+    /// Minimum "openness" score for a valid frontier.
+    /// Higher values require more open space in the frontier direction.
+    /// Default: 0.3
+    pub min_openness: f32,
+}
+
+impl Default for FrontierConfig {
+    fn default() -> Self {
+        Self {
+            connection_distance: 0.3,
+            min_distance_from_robot: 0.5,
+            min_openness: 0.3,
+        }
+    }
+}
+
+impl FrontierConfig {
+    /// Create a new configuration with default values.
+    pub fn new() -> Self {
+        Self::default()
+    }
+
+    /// Builder-style setter for connection distance.
+    pub fn with_connection_distance(mut self, meters: f32) -> Self {
+        self.connection_distance = meters;
+        self
+    }
+
+    /// Builder-style setter for minimum distance from robot.
+    pub fn with_min_distance_from_robot(mut self, meters: f32) -> Self {
+        self.min_distance_from_robot = meters;
+        self
+    }
+}
+
+/// Convert a point to a grid cell coordinate.
+#[inline]
+fn point_to_cell(point: Point2D, cell_size: f32) -> (i32, i32) {
+    (
+        (point.x / cell_size).floor() as i32,
+        (point.y / cell_size).floor() as i32,
+    )
+}
+
+/// Check if a point is connected to any other endpoint in the 3x3 neighborhood.
+#[inline]
+fn is_connected_in_grid(
+    grid: &EndpointGrid,
+    cell: (i32, i32),
+    point: Point2D,
+    line_idx: usize,
+    connection_distance: f32,
+) -> bool {
+    // Check 3x3 neighborhood (covers all points within connection_distance)
+    for dx in -1..=1 {
+        for dy in -1..=1 {
+            let neighbor_cell = (cell.0 + dx, cell.1 + dy);
+            if let Some(neighbors) = grid.get(&neighbor_cell) {
+                for &(other_point, other_idx, _) in neighbors {
+                    // Don't compare with self (same line)
+                    if other_idx == line_idx {
+                        continue;
+                    }
+                    if point.distance(other_point) < connection_distance {
+                        return true;
+                    }
+                }
+            }
+        }
+    }
+    false
+}
+
+/// Detect frontiers from map lines.
+///
+/// Frontiers are line endpoints that don't connect to other lines,
+/// indicating unexplored areas.
+///
+/// Uses spatial hashing for O(n) average complexity instead of O(n²).
+///
+/// # Arguments
+/// * `lines` - Map lines
+/// * `config` - Frontier detection configuration
+///
+/// # Returns
+/// Vector of detected frontiers.
+pub fn detect_frontiers(lines: &[Line2D], config: &FrontierConfig) -> Vec<Frontier> {
+    if lines.is_empty() {
+        return Vec::new();
+    }
+
+    let cell_size = config.connection_distance;
+
+    // Build spatial hash grid - O(n)
+    let mut grid: EndpointGrid = HashMap::with_capacity(lines.len() * 2);
+
+    for (idx, line) in lines.iter().enumerate() {
+        for (point, is_start) in [(line.start, true), (line.end, false)] {
+            let cell = point_to_cell(point, cell_size);
+            grid.entry(cell).or_default().push((point, idx, is_start));
+        }
+    }
+
+    // Find unconnected endpoints - O(n) average case
+    let mut frontiers = Vec::new();
+
+    for (idx, line) in lines.iter().enumerate() {
+        for (point, is_start) in [(line.start, true), (line.end, false)] {
+            let cell = point_to_cell(point, cell_size);
+            if !is_connected_in_grid(&grid, cell, point, idx, config.connection_distance) {
+                frontiers.push(Frontier {
+                    point,
+                    line_idx: idx,
+                    is_start,
+                });
+            }
+        }
+    }
+
+    frontiers
+}
+
+/// Detect frontiers filtered by distance from robot.
+///
+/// Only returns frontiers that are at least `min_distance` from the robot.
+pub fn detect_frontiers_from_robot(
+    lines: &[Line2D],
+    robot_position: Point2D,
+    config: &FrontierConfig,
+) -> Vec<Frontier> {
+    detect_frontiers(lines, config)
+        .into_iter()
+        .filter(|f| f.point.distance(robot_position) >= config.min_distance_from_robot)
+        .collect()
+}
+
+/// Rank frontiers by exploration value.
+///
+/// Returns frontiers sorted by their exploration value (higher is better).
+/// Value considers:
+/// - Distance from robot (closer is slightly better)
+/// - "Openness" (frontiers facing into unknown space are better)
+///
+/// Uses SIMD-optimized LineCollection for efficient batch distance computation.
+///
+/// # Arguments
+/// * `frontiers` - Detected frontiers
+/// * `lines` - Map lines (for openness calculation)
+/// * `robot_position` - Current robot position
+///
+/// # Returns
+/// Frontiers sorted by value (best first).
+pub fn rank_frontiers(
+    frontiers: &[Frontier],
+    lines: &[Line2D],
+    robot_position: Point2D,
+) -> Vec<(Frontier, f32)> {
+    if lines.is_empty() {
+        return frontiers.iter().map(|f| (f.clone(), 0.0)).collect();
+    }
+
+    // Create LineCollection once for SIMD-optimized distance computation
+    let line_collection = LineCollection::from_lines(lines);
+
+    // Reusable buffer for distance computation (avoids allocation per frontier)
+    let mut distance_buffer = Vec::with_capacity(lines.len());
+
+    let mut ranked: Vec<(Frontier, f32)> = frontiers
+        .iter()
+        .map(|f| {
+            let distance = f.point.distance(robot_position);
+            let openness = compute_openness_simd(f, lines, &line_collection, &mut distance_buffer);
+
+            // Value function: prefer nearby, open frontiers
+            // Distance factor: 1.0 at 0m, decaying to ~0.1 at 10m
+            let distance_factor = 1.0 / (1.0 + distance * 0.1);
+
+            // Openness is already 0-1
+            let value = distance_factor * 0.3 + openness * 0.7;
+
+            (f.clone(), value)
+        })
+        .collect();
+
+    // Sort by value (descending)
+    ranked.sort_by(|a, b| b.1.partial_cmp(&a.1).unwrap_or(std::cmp::Ordering::Equal));
+
+    ranked
+}
+
+/// Compute "openness" score for a frontier using SIMD-optimized distance computation.
+///
+/// Openness measures how much unknown space is in the direction
+/// perpendicular to the line at the frontier endpoint.
+fn compute_openness_simd(
+    frontier: &Frontier,
+    lines: &[Line2D],
+    line_collection: &LineCollection,
+    distance_buffer: &mut Vec<f32>,
+) -> f32 {
+    if frontier.line_idx >= lines.len() {
+        return 0.0;
+    }
+
+    let line = &lines[frontier.line_idx];
+
+    // Get direction pointing away from the line
+    let outward_dir = if frontier.is_start {
+        // At start, outward is opposite to line direction
+        -line.unit_direction()
+    } else {
+        // At end, outward is along line direction
+        line.unit_direction()
+    };
+
+    // Also consider the perpendicular direction
+    let perp = Point2D::new(-outward_dir.y, outward_dir.x);
+
+    // Check distances in outward and perpendicular directions
+    let check_distance = 2.0; // meters
+    let probe_outward = frontier.point + outward_dir * check_distance;
+    let probe_left = frontier.point + perp * check_distance;
+    let probe_right = frontier.point - perp * check_distance;
+
+    // Count how many probe points are far from any line
+    // Use SIMD-optimized batch distance computation with buffer reuse
+    let mut open_count = 0;
+
+    for probe in [probe_outward, probe_left, probe_right] {
+        if let Some((_, min_dist)) =
+            line_collection.nearest_line_with_buffer(probe, distance_buffer)
+        {
+            if min_dist > 1.0 {
+                open_count += 1;
+            }
+        } else {
+            // No lines - consider open
+            open_count += 1;
+        }
+    }
+
+    open_count as f32 / 3.0
+}
+
+/// Compute "openness" score for a frontier (non-SIMD version for single queries).
+///
+/// Openness measures how much unknown space is in the direction
+/// perpendicular to the line at the frontier endpoint.
+///
+/// This is the scalar fallback used in tests. For batch operations,
+/// use `compute_openness_simd` through `rank_frontiers()`.
+#[allow(dead_code)]
+fn compute_openness(frontier: &Frontier, lines: &[Line2D]) -> f32 {
+    if frontier.line_idx >= lines.len() {
+        return 0.0;
+    }
+
+    let line = &lines[frontier.line_idx];
+
+    // Get direction pointing away from the line
+    let outward_dir = if frontier.is_start {
+        // At start, outward is opposite to line direction
+        -line.unit_direction()
+    } else {
+        // At end, outward is along line direction
+        line.unit_direction()
+    };
+
+    // Also consider the perpendicular direction
+    let perp = Point2D::new(-outward_dir.y, outward_dir.x);
+
+    // Check distances in outward and perpendicular directions
+    let check_distance = 2.0; // meters
+    let probe_outward = frontier.point + outward_dir * check_distance;
+    let probe_left = frontier.point + perp * check_distance;
+    let probe_right = frontier.point - perp * check_distance;
+
+    // Count how many probe points are far from any line
+    let mut open_count = 0;
+
+    for probe in [probe_outward, probe_left, probe_right] {
+        let min_dist = lines
+            .iter()
+            .map(|l| l.distance_to_point(probe))
+            .fold(f32::MAX, f32::min);
+
+        if min_dist > 1.0 {
+            open_count += 1;
+        }
+    }
+
+    open_count as f32 / 3.0
+}
+
+/// Get the best frontier for exploration.
+///
+/// Convenience function that detects, filters, and ranks frontiers,
+/// returning the best one.
+pub fn get_best_frontier(
+    lines: &[Line2D],
+    robot_position: Point2D,
+    config: &FrontierConfig,
+) -> Option<Frontier> {
+    let frontiers = detect_frontiers_from_robot(lines, robot_position, config);
+
+    if frontiers.is_empty() {
+        return None;
+    }
+
+    let ranked = rank_frontiers(&frontiers, lines, robot_position);
+
+    ranked.into_iter().next().map(|(f, _)| f)
+}
+
+/// Cluster nearby frontiers into frontier regions.
+///
+/// Groups frontiers that are close together, useful for exploration
+/// planning where you want to visit a region rather than individual points.
+pub fn cluster_frontiers(frontiers: &[Frontier], cluster_distance: f32) -> Vec<Vec<Frontier>> {
+    if frontiers.is_empty() {
+        return Vec::new();
+    }
+
+    let mut clusters: Vec<Vec<Frontier>> = Vec::new();
+    let mut assigned = vec![false; frontiers.len()];
+
+    for i in 0..frontiers.len() {
+        if assigned[i] {
+            continue;
+        }
+
+        let mut cluster = vec![frontiers[i].clone()];
+        assigned[i] = true;
+
+        // Find all frontiers close to this one
+        for j in (i + 1)..frontiers.len() {
+            if assigned[j] {
+                continue;
+            }
+
+            // Check if close to any frontier in the cluster
+            let close = cluster
+                .iter()
+                .any(|f| f.point.distance(frontiers[j].point) < cluster_distance);
+
+            if close {
+                cluster.push(frontiers[j].clone());
+                assigned[j] = true;
+            }
+        }
+
+        clusters.push(cluster);
+    }
+
+    clusters
+}
+
+/// Get the centroid of a frontier cluster.
+pub fn cluster_centroid(cluster: &[Frontier]) -> Option<Point2D> {
+    if cluster.is_empty() {
+        return None;
+    }
+
+    let sum_x: f32 = cluster.iter().map(|f| f.point.x).sum();
+    let sum_y: f32 = cluster.iter().map(|f| f.point.y).sum();
+    let n = cluster.len() as f32;
+
+    Some(Point2D::new(sum_x / n, sum_y / n))
+}
+
+#[cfg(test)]
+mod tests {
+    use super::*;
+
+    fn make_partial_room() -> Vec<Line2D> {
+        // Room with one side open (no top wall)
+        vec![
+            Line2D::new(Point2D::new(-5.0, -5.0), Point2D::new(5.0, -5.0)), // Bottom
+            Line2D::new(Point2D::new(5.0, -5.0), Point2D::new(5.0, 5.0)),   // Right
+            Line2D::new(Point2D::new(-5.0, 5.0), Point2D::new(-5.0, -5.0)), // Left
+        ]
+    }
+
+    fn make_complete_room() -> Vec<Line2D> {
+        vec![
+            Line2D::new(Point2D::new(-5.0, -5.0), Point2D::new(5.0, -5.0)), // Bottom
+            Line2D::new(Point2D::new(5.0, -5.0), Point2D::new(5.0, 5.0)),   // Right
+            Line2D::new(Point2D::new(5.0, 5.0), Point2D::new(-5.0, 5.0)),   // Top
+            Line2D::new(Point2D::new(-5.0, 5.0), Point2D::new(-5.0, -5.0)), // Left
+        ]
+    }
+
+    #[test]
+    fn test_detect_frontiers_partial_room() {
+        let lines = make_partial_room();
+        let config = FrontierConfig::default();
+
+        let frontiers = detect_frontiers(&lines, &config);
+
+        // Should have 2 frontiers (the open endpoints of the top)
+        // Right wall top (5, 5) and Left wall top (-5, 5)
+        assert!(frontiers.len() >= 2);
+    }
+
+    #[test]
+    fn test_detect_frontiers_complete_room() {
+        let lines = make_complete_room();
+        let config = FrontierConfig::default();
+
+        let frontiers = detect_frontiers(&lines, &config);
+
+        // Complete room - all corners should be connected
+        // Depending on connection_distance, might have 0 or some frontiers
+        // With default 0.3m, corners at distance 0 should connect
+        assert_eq!(frontiers.len(), 0);
+    }
+
+    #[test]
+    fn test_detect_frontiers_single_line() {
+        let lines = vec![Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(5.0, 0.0))];
+        let config = FrontierConfig::default();
+
+        let frontiers = detect_frontiers(&lines, &config);
+
+        // Both endpoints are unconnected
+        assert_eq!(frontiers.len(), 2);
+    }
+
+    #[test]
+    fn test_detect_frontiers_from_robot() {
+        let lines = vec![Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(5.0, 0.0))];
+        let robot = Point2D::new(0.0, 0.0);
+        let config = FrontierConfig::default().with_min_distance_from_robot(1.0);
+
+        let frontiers = detect_frontiers_from_robot(&lines, robot, &config);
+
+        // Start endpoint is at robot position, should be filtered
+        // End endpoint is 5m away, should be included
+        assert_eq!(frontiers.len(), 1);
+        assert_eq!(frontiers[0].is_start, false); // End endpoint
+    }
+
+    #[test]
+    fn test_rank_frontiers() {
+        let lines = vec![
+            Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(5.0, 0.0)),
+            Line2D::new(Point2D::new(0.0, 10.0), Point2D::new(5.0, 10.0)),
+        ];
+        let robot = Point2D::new(0.0, 0.0);
+        let config = FrontierConfig::default();
+
+        let frontiers = detect_frontiers(&lines, &config);
+        let ranked = rank_frontiers(&frontiers, &lines, robot);
+
+        // Frontiers closer to robot should rank higher
+        assert!(!ranked.is_empty());
+    }
+
+    #[test]
+    fn test_get_best_frontier() {
+        let lines = make_partial_room();
+        let robot = Point2D::zero();
+        let config = FrontierConfig::default();
+
+        let best = get_best_frontier(&lines, robot, &config);
+
+        assert!(best.is_some());
+    }
+
+    #[test]
+    fn test_get_best_frontier_no_frontiers() {
+        let lines = make_complete_room();
+        let robot = Point2D::zero();
+        let config = FrontierConfig::default();
+
+        let best = get_best_frontier(&lines, robot, &config);
+
+        // Complete room has no frontiers
+        assert!(best.is_none());
+    }
+
+    #[test]
+    fn test_cluster_frontiers() {
+        let frontiers = vec![
+            Frontier {
+                point: Point2D::new(0.0, 0.0),
+                line_idx: 0,
+                is_start: true,
+            },
+            Frontier {
+                point: Point2D::new(0.1, 0.0),
+                line_idx: 1,
+                is_start: true,
+            }, // Close to first
+            Frontier {
+                point: Point2D::new(10.0, 10.0),
+                line_idx: 2,
+                is_start: true,
+            }, // Far
+        ];
+
+        let clusters = cluster_frontiers(&frontiers, 0.5);
+
+        // Should have 2 clusters: one with 2 close frontiers, one with the far one
+        assert_eq!(clusters.len(), 2);
+    }
+
+    #[test]
+    fn test_cluster_centroid() {
+        let frontiers = vec![
+            Frontier {
+                point: Point2D::new(0.0, 0.0),
+                line_idx: 0,
+                is_start: true,
+            },
+            Frontier {
+                point: Point2D::new(2.0, 0.0),
+                line_idx: 1,
+                is_start: true,
+            },
+        ];
+
+        let centroid = cluster_centroid(&frontiers);
+
+        assert!(centroid.is_some());
+        let c = centroid.unwrap();
+        assert!((c.x - 1.0).abs() < 0.01);
+        assert!((c.y - 0.0).abs() < 0.01);
+    }
+
+    #[test]
+    fn test_compute_openness() {
+        let lines = vec![Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(5.0, 0.0))];
+
+        let frontier = Frontier {
+            point: Point2D::new(5.0, 0.0),
+            line_idx: 0,
+            is_start: false,
+        };
+
+        let openness = compute_openness(&frontier, &lines);
+
+        // End of single line should have high openness
+        assert!(openness > 0.0);
+    }
+}

vastu-map/src/query/mod.rs

@@ -0,0 +1,42 @@
+//! Map query operations for VectorMap.
+//!
+//! Provides spatial queries against the map:
+//! - **Raycast**: Find distance to obstacles in a direction
+//! - **Occupancy**: Query whether a point is free, occupied, or unknown
+//! - **Frontier**: Detect unexplored boundaries for exploration
+//!
+//! # Example
+//!
+//! ```rust,ignore
+//! use vastu_map::query::{raycast, query_occupancy, detect_frontiers};
+//! use vastu_map::core::Point2D;
+//!
+//! // Raycast to find wall distance
+//! let origin = Point2D::new(0.0, 0.0);
+//! let direction = Point2D::new(1.0, 0.0);
+//! let distance = raycast(origin, direction, 10.0, &map_lines);
+//!
+//! // Check if a point is free
+//! let occupancy = query_occupancy(Point2D::new(1.0, 1.0), &map_lines, None, &config);
+//!
+//! // Find exploration frontiers
+//! let frontiers = detect_frontiers(&map_lines, &frontier_config);
+//! ```
+
+pub mod frontier;
+pub mod occupancy;
+pub mod raycast;
+
+// Re-export main types and functions
+pub use frontier::{
+    FrontierConfig, cluster_centroid, cluster_frontiers, detect_frontiers,
+    detect_frontiers_from_robot, get_best_frontier, rank_frontiers,
+};
+pub use occupancy::{
+    OccupancyConfig, is_path_clear, is_region_clear, query_occupancy, query_occupancy_batch,
+    query_occupancy_indexed,
+};
+pub use raycast::{
+    RaycastResult, raycast, raycast_360, raycast_batch, raycast_detailed, raycast_indexed,
+    raycast_sweep,
+};

vastu-map/src/query/occupancy.rs

@@ -0,0 +1,392 @@
+//! Point occupancy queries for VectorMap.
+//!
+//! Determines whether a point is in free space, occupied, or unknown
+//! based on the map's line features.
+
+use crate::Occupancy;
+use crate::core::{Bounds, Point2D};
+use crate::features::Line2D;
+use crate::integration::SpatialIndex;
+
+/// Configuration for occupancy queries.
+#[derive(Clone, Debug)]
+pub struct OccupancyConfig {
+    /// Distance threshold for considering a point "on" a line (meters).
+    /// Points within this distance of a line are considered occupied.
+    /// Default: 0.1m
+    pub obstacle_distance: f32,
+
+    /// Distance threshold for "near" checks (meters).
+    /// Points must be within this distance of some map feature to be known.
+    /// Default: 2.0m
+    pub known_distance: f32,
+
+    /// Whether to use winding number for inside/outside determination.
+    /// If true, uses contour-based occupancy (like font rendering).
+    /// If false, uses simple distance-based occupancy.
+    /// Default: false (distance-based is simpler and usually sufficient)
+    pub use_winding_number: bool,
+}
+
+impl Default for OccupancyConfig {
+    fn default() -> Self {
+        Self {
+            obstacle_distance: 0.1,
+            known_distance: 2.0,
+            use_winding_number: false,
+        }
+    }
+}
+
+impl OccupancyConfig {
+    /// Create a new configuration with default values.
+    pub fn new() -> Self {
+        Self::default()
+    }
+
+    /// Builder-style setter for obstacle distance.
+    pub fn with_obstacle_distance(mut self, meters: f32) -> Self {
+        self.obstacle_distance = meters;
+        self
+    }
+
+    /// Builder-style setter for known distance.
+    pub fn with_known_distance(mut self, meters: f32) -> Self {
+        self.known_distance = meters;
+        self
+    }
+}
+
+/// Query point occupancy.
+///
+/// # Arguments
+/// * `point` - Point to query
+/// * `lines` - Map lines
+/// * `bounds` - Map bounds (for unknown check)
+/// * `config` - Occupancy configuration
+///
+/// # Returns
+/// Occupancy state of the point.
+pub fn query_occupancy(
+    point: Point2D,
+    lines: &[Line2D],
+    bounds: Option<&Bounds>,
+    config: &OccupancyConfig,
+) -> Occupancy {
+    // First check if point is outside map bounds
+    if let Some(b) = bounds
+        && !b.contains(point)
+    {
+        return Occupancy::Unknown;
+    }
+
+    if lines.is_empty() {
+        return Occupancy::Unknown;
+    }
+
+    // Find minimum distance to any line
+    let mut min_distance = f32::MAX;
+    let mut has_nearby_line = false;
+
+    for line in lines {
+        let dist = line.distance_to_point(point);
+        if dist < min_distance {
+            min_distance = dist;
+        }
+        if dist < config.known_distance {
+            has_nearby_line = true;
+        }
+    }
+
+    // Point is very close to a line - occupied
+    if min_distance < config.obstacle_distance {
+        return Occupancy::Occupied;
+    }
+
+    // Point is within known region - free
+    if has_nearby_line {
+        return Occupancy::Free;
+    }
+
+    // Point is far from all lines - unknown
+    Occupancy::Unknown
+}
+
+/// Query occupancy using spatial index for efficiency.
+pub fn query_occupancy_indexed(
+    point: Point2D,
+    lines: &[Line2D],
+    index: &SpatialIndex,
+    bounds: Option<&Bounds>,
+    config: &OccupancyConfig,
+) -> Occupancy {
+    // First check if point is outside map bounds
+    if let Some(b) = bounds
+        && !b.contains(point)
+    {
+        return Occupancy::Unknown;
+    }
+
+    if lines.is_empty() {
+        return Occupancy::Unknown;
+    }
+
+    // Use spatial index to find nearest line
+    if let Some((_idx, dist)) = index.nearest_line(point) {
+        // Point is very close to a line - occupied
+        if dist < config.obstacle_distance {
+            return Occupancy::Occupied;
+        }
+
+        // Check if any lines are within known distance
+        let nearby = index.lines_within_distance(point, config.known_distance);
+        if !nearby.is_empty() {
+            return Occupancy::Free;
+        }
+    }
+
+    // No nearby lines - unknown
+    Occupancy::Unknown
+}
+
+/// Query occupancy for multiple points.
+///
+/// Returns occupancy for each point.
+pub fn query_occupancy_batch(
+    points: &[Point2D],
+    lines: &[Line2D],
+    bounds: Option<&Bounds>,
+    config: &OccupancyConfig,
+) -> Vec<Occupancy> {
+    points
+        .iter()
+        .map(|&p| query_occupancy(p, lines, bounds, config))
+        .collect()
+}
+
+/// Check if a path is collision-free.
+///
+/// # Arguments
+/// * `start` - Path start point
+/// * `end` - Path end point
+/// * `lines` - Map lines
+/// * `config` - Occupancy configuration
+///
+/// # Returns
+/// True if the path doesn't collide with any obstacles.
+pub fn is_path_clear(
+    start: Point2D,
+    end: Point2D,
+    lines: &[Line2D],
+    config: &OccupancyConfig,
+) -> bool {
+    // Create a line segment representing the path
+    let path = Line2D::new(start, end);
+    let path_len = path.length();
+
+    if path_len < f32::EPSILON {
+        // Zero-length path - just check the point
+        return query_occupancy(start, lines, None, config) != Occupancy::Occupied;
+    }
+
+    // Check for intersections with map lines
+    for line in lines {
+        if path.intersection(line).is_some() {
+            return false;
+        }
+
+        // Also check if path passes too close to any line
+        // Sample points along the path
+        let num_samples = (path_len / config.obstacle_distance).ceil() as usize;
+        let num_samples = num_samples.max(2);
+
+        for i in 0..=num_samples {
+            let t = i as f32 / num_samples as f32;
+            let point = path.point_at(t);
+            let dist = line.distance_to_point(point);
+
+            if dist < config.obstacle_distance {
+                return false;
+            }
+        }
+    }
+
+    true
+}
+
+/// Check if a circular region is collision-free.
+///
+/// # Arguments
+/// * `center` - Circle center
+/// * `radius` - Circle radius
+/// * `lines` - Map lines
+///
+/// # Returns
+/// True if no obstacles within the circle.
+pub fn is_region_clear(center: Point2D, radius: f32, lines: &[Line2D]) -> bool {
+    for line in lines {
+        let dist = line.distance_to_point(center);
+        if dist < radius {
+            return false;
+        }
+    }
+    true
+}
+
+#[cfg(test)]
+mod tests {
+    use super::*;
+
+    fn make_room() -> Vec<Line2D> {
+        vec![
+            Line2D::new(Point2D::new(-5.0, -5.0), Point2D::new(5.0, -5.0)), // Bottom
+            Line2D::new(Point2D::new(5.0, -5.0), Point2D::new(5.0, 5.0)),   // Right
+            Line2D::new(Point2D::new(5.0, 5.0), Point2D::new(-5.0, 5.0)),   // Top
+            Line2D::new(Point2D::new(-5.0, 5.0), Point2D::new(-5.0, -5.0)), // Left
+        ]
+    }
+
+    #[test]
+    fn test_query_center_is_free() {
+        let lines = make_room();
+        // Center is 5m from walls, so need larger known_distance
+        let config = OccupancyConfig::default().with_known_distance(6.0);
+
+        let result = query_occupancy(Point2D::zero(), &lines, None, &config);
+
+        assert_eq!(result, Occupancy::Free);
+    }
+
+    #[test]
+    fn test_query_on_wall_is_occupied() {
+        let lines = make_room();
+        let config = OccupancyConfig::default();
+
+        // Point on the bottom wall
+        let result = query_occupancy(Point2D::new(0.0, -5.0), &lines, None, &config);
+
+        assert_eq!(result, Occupancy::Occupied);
+    }
+
+    #[test]
+    fn test_query_near_wall_is_occupied() {
+        let lines = make_room();
+        let config = OccupancyConfig::default().with_obstacle_distance(0.2);
+
+        // Point very close to wall
+        let result = query_occupancy(Point2D::new(0.0, -4.9), &lines, None, &config);
+
+        assert_eq!(result, Occupancy::Occupied);
+    }
+
+    #[test]
+    fn test_query_outside_bounds_is_unknown() {
+        let lines = make_room();
+        let bounds = Bounds::new(Point2D::new(-6.0, -6.0), Point2D::new(6.0, 6.0));
+        let config = OccupancyConfig::default();
+
+        // Point outside bounds
+        let result = query_occupancy(Point2D::new(10.0, 10.0), &lines, Some(&bounds), &config);
+
+        assert_eq!(result, Occupancy::Unknown);
+    }
+
+    #[test]
+    fn test_query_far_from_lines_is_unknown() {
+        let lines = vec![Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(1.0, 0.0))];
+        let config = OccupancyConfig::default().with_known_distance(1.0);
+
+        // Point far from the only line
+        let result = query_occupancy(Point2D::new(10.0, 10.0), &lines, None, &config);
+
+        assert_eq!(result, Occupancy::Unknown);
+    }
+
+    #[test]
+    fn test_query_empty_map_is_unknown() {
+        let lines: Vec<Line2D> = vec![];
+        let config = OccupancyConfig::default();
+
+        let result = query_occupancy(Point2D::zero(), &lines, None, &config);
+
+        assert_eq!(result, Occupancy::Unknown);
+    }
+
+    #[test]
+    fn test_query_batch() {
+        let lines = make_room();
+        // Center is 5m from walls, so need larger known_distance
+        let config = OccupancyConfig::default().with_known_distance(6.0);
+
+        let points = vec![
+            Point2D::zero(),         // Free (center)
+            Point2D::new(0.0, -5.0), // Occupied (on wall)
+            Point2D::new(2.0, 0.0),  // Free (inside room, near walls)
+        ];
+
+        let results = query_occupancy_batch(&points, &lines, None, &config);
+
+        assert_eq!(results[0], Occupancy::Free);
+        assert_eq!(results[1], Occupancy::Occupied);
+        assert_eq!(results[2], Occupancy::Free);
+    }
+
+    #[test]
+    fn test_is_path_clear_straight() {
+        let lines = make_room();
+        let config = OccupancyConfig::default();
+
+        // Path through center - should be clear
+        let clear = is_path_clear(
+            Point2D::new(-2.0, 0.0),
+            Point2D::new(2.0, 0.0),
+            &lines,
+            &config,
+        );
+
+        assert!(clear);
+    }
+
+    #[test]
+    fn test_is_path_blocked() {
+        let lines = make_room();
+        let config = OccupancyConfig::default();
+
+        // Path through wall - should be blocked
+        let clear = is_path_clear(
+            Point2D::new(0.0, 0.0),
+            Point2D::new(10.0, 0.0), // Goes through right wall
+            &lines,
+            &config,
+        );
+
+        assert!(!clear);
+    }
+
+    #[test]
+    fn test_is_path_too_close() {
+        let lines = vec![Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(10.0, 0.0))];
+        let config = OccupancyConfig::default().with_obstacle_distance(0.2);
+
+        // Path very close to line
+        let clear = is_path_clear(
+            Point2D::new(0.0, 0.1),
+            Point2D::new(10.0, 0.1),
+            &lines,
+            &config,
+        );
+
+        assert!(!clear);
+    }
+
+    #[test]
+    fn test_is_region_clear() {
+        let lines = make_room();
+
+        // Center region should be clear
+        assert!(is_region_clear(Point2D::zero(), 2.0, &lines));
+
+        // Region touching wall should not be clear
+        assert!(!is_region_clear(Point2D::new(4.0, 0.0), 2.0, &lines));
+    }
+}

vastu-map/src/query/raycast.rs

@@ -0,0 +1,328 @@
+//! Ray casting for obstacle detection.
+//!
+//! Computes the distance from a point in a given direction to the
+//! first obstacle (line segment) in the map.
+
+use crate::core::Point2D;
+use crate::features::{Line2D, LineCollection};
+use crate::integration::SpatialIndex;
+
+/// Result of a raycast operation.
+#[derive(Clone, Debug)]
+pub struct RaycastResult {
+    /// Distance to the hit point, or max_range if no hit.
+    pub distance: f32,
+    /// Index of the line that was hit, if any.
+    pub line_idx: Option<usize>,
+    /// The hit point in world coordinates.
+    pub hit_point: Option<Point2D>,
+    /// Whether a hit occurred.
+    pub hit: bool,
+}
+
+impl RaycastResult {
+    /// Create a result indicating no hit.
+    pub fn miss(max_range: f32) -> Self {
+        Self {
+            distance: max_range,
+            line_idx: None,
+            hit_point: None,
+            hit: false,
+        }
+    }
+
+    /// Create a result indicating a hit.
+    pub fn hit(distance: f32, line_idx: usize, hit_point: Point2D) -> Self {
+        Self {
+            distance,
+            line_idx: Some(line_idx),
+            hit_point: Some(hit_point),
+            hit: true,
+        }
+    }
+}
+
+/// Cast a ray and find the first intersection with map lines.
+///
+/// # Arguments
+/// * `origin` - Ray origin in world frame
+/// * `direction` - Ray direction (will be normalized)
+/// * `max_range` - Maximum ray distance
+/// * `lines` - Map line segments
+///
+/// # Returns
+/// Distance to first intersection, or max_range if none.
+pub fn raycast(origin: Point2D, direction: Point2D, max_range: f32, lines: &[Line2D]) -> f32 {
+    raycast_detailed(origin, direction, max_range, lines).distance
+}
+
+/// Cast a ray with detailed hit information.
+///
+/// # Arguments
+/// * `origin` - Ray origin in world frame
+/// * `direction` - Ray direction (will be normalized)
+/// * `max_range` - Maximum ray distance
+/// * `lines` - Map line segments
+///
+/// # Returns
+/// Detailed raycast result.
+pub fn raycast_detailed(
+    origin: Point2D,
+    direction: Point2D,
+    max_range: f32,
+    lines: &[Line2D],
+) -> RaycastResult {
+    if lines.is_empty() {
+        return RaycastResult::miss(max_range);
+    }
+
+    let dir = direction.normalized();
+    let mut closest_dist = max_range;
+    let mut closest_line: Option<usize> = None;
+    let mut closest_point: Option<Point2D> = None;
+
+    for (idx, line) in lines.iter().enumerate() {
+        if let Some(t) = line.ray_intersection(origin, dir)
+            && t > 0.0
+            && t < closest_dist
+        {
+            closest_dist = t;
+            closest_line = Some(idx);
+            closest_point = Some(origin + dir * t);
+        }
+    }
+
+    match closest_line {
+        Some(idx) => RaycastResult::hit(closest_dist, idx, closest_point.unwrap()),
+        None => RaycastResult::miss(max_range),
+    }
+}
+
+/// Cast a ray using spatial index for efficiency.
+///
+/// Uses the spatial index to only test lines in the ray's path.
+pub fn raycast_indexed(
+    origin: Point2D,
+    direction: Point2D,
+    max_range: f32,
+    lines: &[Line2D],
+    _index: &SpatialIndex,
+) -> RaycastResult {
+    // For now, fall back to linear scan
+    // A more efficient implementation would use the R-tree to
+    // find candidate lines along the ray path
+    raycast_detailed(origin, direction, max_range, lines)
+}
+
+/// Cast multiple rays (e.g., for simulating a lidar).
+///
+/// # Arguments
+/// * `origin` - Ray origin
+/// * `start_angle` - Starting angle (radians)
+/// * `end_angle` - Ending angle (radians)
+/// * `num_rays` - Number of rays to cast
+/// * `max_range` - Maximum range
+/// * `lines` - Map lines
+///
+/// # Returns
+/// Vector of distances for each ray.
+pub fn raycast_sweep(
+    origin: Point2D,
+    start_angle: f32,
+    end_angle: f32,
+    num_rays: usize,
+    max_range: f32,
+    lines: &[Line2D],
+) -> Vec<f32> {
+    if num_rays == 0 {
+        return Vec::new();
+    }
+
+    let angle_step = if num_rays > 1 {
+        (end_angle - start_angle) / (num_rays - 1) as f32
+    } else {
+        0.0
+    };
+
+    (0..num_rays)
+        .map(|i| {
+            let angle = start_angle + i as f32 * angle_step;
+            let dir = Point2D::from_polar(1.0, angle);
+            raycast(origin, dir, max_range, lines)
+        })
+        .collect()
+}
+
+/// Cast rays in a full 360° sweep.
+pub fn raycast_360(origin: Point2D, num_rays: usize, max_range: f32, lines: &[Line2D]) -> Vec<f32> {
+    use std::f32::consts::PI;
+    raycast_sweep(origin, -PI, PI, num_rays, max_range, lines)
+}
+
+/// Batch raycast using LineCollection (optimized for SIMD).
+///
+/// Note: Currently falls back to per-line iteration.
+/// Future optimization could use SIMD for parallel intersection tests.
+pub fn raycast_batch(
+    origin: Point2D,
+    direction: Point2D,
+    max_range: f32,
+    lines: &LineCollection,
+) -> f32 {
+    let lines_vec = lines.to_lines();
+    raycast(origin, direction, max_range, &lines_vec)
+}
+
+#[cfg(test)]
+mod tests {
+    use super::*;
+    use crate::core::Point2D;
+    use approx::assert_relative_eq;
+    use std::f32::consts::PI;
+
+    fn make_room() -> Vec<Line2D> {
+        vec![
+            Line2D::new(Point2D::new(-5.0, -5.0), Point2D::new(5.0, -5.0)), // Bottom
+            Line2D::new(Point2D::new(5.0, -5.0), Point2D::new(5.0, 5.0)),   // Right
+            Line2D::new(Point2D::new(5.0, 5.0), Point2D::new(-5.0, 5.0)),   // Top
+            Line2D::new(Point2D::new(-5.0, 5.0), Point2D::new(-5.0, -5.0)), // Left
+        ]
+    }
+
+    #[test]
+    fn test_raycast_hit() {
+        let lines = make_room();
+        let origin = Point2D::zero();
+        let direction = Point2D::new(1.0, 0.0); // East
+
+        let dist = raycast(origin, direction, 100.0, &lines);
+
+        // Should hit right wall at x=5
+        assert_relative_eq!(dist, 5.0, epsilon = 0.01);
+    }
+
+    #[test]
+    fn test_raycast_miss() {
+        let lines = vec![Line2D::new(
+            Point2D::new(10.0, 0.0),
+            Point2D::new(10.0, 10.0),
+        )];
+        let origin = Point2D::zero();
+        let direction = Point2D::new(-1.0, 0.0); // West (away from line)
+
+        let dist = raycast(origin, direction, 100.0, &lines);
+
+        assert_eq!(dist, 100.0); // Max range
+    }
+
+    #[test]
+    fn test_raycast_detailed() {
+        let lines = make_room();
+        let origin = Point2D::zero();
+        let direction = Point2D::new(0.0, -1.0); // South
+
+        let result = raycast_detailed(origin, direction, 100.0, &lines);
+
+        assert!(result.hit);
+        assert_relative_eq!(result.distance, 5.0, epsilon = 0.01);
+        assert_eq!(result.line_idx, Some(0)); // Bottom wall
+        assert!(result.hit_point.is_some());
+        assert_relative_eq!(result.hit_point.unwrap().y, -5.0, epsilon = 0.01);
+    }
+
+    #[test]
+    fn test_raycast_diagonal() {
+        let lines = make_room();
+        let origin = Point2D::zero();
+        let direction = Point2D::new(1.0, 1.0).normalized(); // Northeast
+
+        let dist = raycast(origin, direction, 100.0, &lines);
+
+        // Should hit corner at (5, 5), distance = sqrt(50) ≈ 7.07
+        assert_relative_eq!(dist, 5.0 * 2.0_f32.sqrt(), epsilon = 0.1);
+    }
+
+    #[test]
+    fn test_raycast_empty_lines() {
+        let lines: Vec<Line2D> = vec![];
+        let origin = Point2D::zero();
+        let direction = Point2D::new(1.0, 0.0);
+
+        let dist = raycast(origin, direction, 10.0, &lines);
+
+        assert_eq!(dist, 10.0);
+    }
+
+    #[test]
+    fn test_raycast_sweep() {
+        let lines = make_room();
+        let origin = Point2D::zero();
+
+        // Cast 4 rays: N, E, S, W
+        let distances = raycast_sweep(origin, -PI, PI, 4, 100.0, &lines);
+
+        // Note: with 4 rays from -π to π:
+        // Ray 0: angle = -π (West) → should hit left wall at distance 5
+        // Ray 1: angle = -π/3 ≈ -60° (roughly SW)
+        // etc.
+        assert_eq!(distances.len(), 4);
+        // All should be around 5m (hitting walls)
+        for dist in &distances {
+            assert!(*dist <= 10.0); // Should hit some wall
+        }
+    }
+
+    #[test]
+    fn test_raycast_360() {
+        let lines = make_room();
+        let origin = Point2D::zero();
+
+        let distances = raycast_360(origin, 8, 100.0, &lines);
+
+        assert_eq!(distances.len(), 8);
+        // All rays from center should hit a wall
+        for dist in &distances {
+            assert!(*dist < 100.0);
+        }
+    }
+
+    #[test]
+    fn test_raycast_from_edge() {
+        let lines = make_room();
+        // Origin near left wall
+        let origin = Point2D::new(-4.9, 0.0);
+        let direction = Point2D::new(-1.0, 0.0); // Toward wall
+
+        let dist = raycast(origin, direction, 100.0, &lines);
+
+        // Should hit wall very close
+        assert_relative_eq!(dist, 0.1, epsilon = 0.01);
+    }
+
+    #[test]
+    fn test_raycast_parallel_to_wall() {
+        let lines = vec![Line2D::new(Point2D::new(0.0, 5.0), Point2D::new(10.0, 5.0))];
+        let origin = Point2D::zero();
+        let direction = Point2D::new(1.0, 0.0); // Parallel to line
+
+        let dist = raycast(origin, direction, 100.0, &lines);
+
+        // Should not hit (parallel)
+        assert_eq!(dist, 100.0);
+    }
+
+    #[test]
+    fn test_raycast_behind_origin() {
+        let lines = vec![Line2D::new(
+            Point2D::new(-5.0, -1.0),
+            Point2D::new(-5.0, 1.0),
+        )];
+        let origin = Point2D::zero();
+        let direction = Point2D::new(1.0, 0.0); // Away from line
+
+        let dist = raycast(origin, direction, 100.0, &lines);
+
+        // Line is behind, should not hit
+        assert_eq!(dist, 100.0);
+    }
+}

vastu-map/src/simd/f32x4.rs

@@ -0,0 +1,427 @@
+//! 4-wide f32 SIMD vector type.
+//!
+//! This implementation is designed for LLVM auto-vectorization on ARMv7 NEON.
+//! The struct uses 16-byte alignment to match NEON's 128-bit registers.
+//!
+//! # Target Platform
+//!
+//! Baseline: Allwinner A33 (Cortex-A7)
+//! - ARMv7-A architecture
+//! - NEON 128-bit SIMD (16 Q-registers)
+//! - VFPv4 with FMA support
+//!
+//! # NEON Instruction Mapping
+//!
+//! | Rust Operation | NEON Intrinsic | Cycles |
+//! |----------------|----------------|--------|
+//! | `Float4 + Float4` | `vaddq_f32` | 1 |
+//! | `Float4 - Float4` | `vsubq_f32` | 1 |
+//! | `Float4 * Float4` | `vmulq_f32` | 1 |
+//! | `Float4::min()` | `vminq_f32` | 1 |
+//! | `Float4::max()` | `vmaxq_f32` | 1 |
+//! | `Float4::splat()` | `vdupq_n_f32` | 1 |
+//! | `mul_add()` | `vfmaq_f32` | 1 |
+
+use std::ops::{Add, AddAssign, Mul, Sub};
+
+/// A 4-wide f32 vector optimized for SIMD auto-vectorization.
+///
+/// # Memory Layout
+///
+/// Uses `#[repr(C, align(16))]` to ensure:
+/// - C-compatible memory layout for predictable access patterns
+/// - 16-byte alignment matching NEON's 128-bit (16-byte) Q-registers
+///
+/// # Auto-vectorization
+///
+/// All methods use `#[inline(always)]` to encourage LLVM to inline
+/// and recognize vectorizable patterns. When compiled with
+/// `-C target-feature=+neon -C target-cpu=cortex-a7`, LLVM will
+/// typically emit NEON instructions for these operations.
+#[derive(Clone, Copy, Debug, PartialEq)]
+#[repr(C, align(16))]
+pub struct Float4(pub [f32; 4]);
+
+impl Float4 {
+    /// Creates a new `Float4` from an array of 4 f32 values.
+    ///
+    /// # Example
+    /// ```
+    /// use vastu_map::simd::Float4;
+    /// let v = Float4::new([1.0, 2.0, 3.0, 4.0]);
+    /// ```
+    #[inline(always)]
+    pub const fn new(arr: [f32; 4]) -> Self {
+        Self(arr)
+    }
+
+    /// Creates a `Float4` with all elements set to the same value.
+    ///
+    /// This is equivalent to NEON's `vdupq_n_f32` intrinsic.
+    ///
+    /// # Example
+    /// ```
+    /// use vastu_map::simd::Float4;
+    /// let v = Float4::splat(5.0);
+    /// assert_eq!(v.to_array(), [5.0, 5.0, 5.0, 5.0]);
+    /// ```
+    #[inline(always)]
+    pub const fn splat(v: f32) -> Self {
+        Self([v, v, v, v])
+    }
+
+    /// Creates a `Float4` with all elements set to zero.
+    #[inline(always)]
+    pub const fn zero() -> Self {
+        Self([0.0, 0.0, 0.0, 0.0])
+    }
+
+    /// Extracts the elements as an array.
+    ///
+    /// # Example
+    /// ```
+    /// use vastu_map::simd::Float4;
+    /// let v = Float4::new([1.0, 2.0, 3.0, 4.0]);
+    /// let arr = v.to_array();
+    /// assert_eq!(arr, [1.0, 2.0, 3.0, 4.0]);
+    /// ```
+    #[inline(always)]
+    pub const fn to_array(self) -> [f32; 4] {
+        self.0
+    }
+
+    /// Element-wise minimum of two vectors.
+    ///
+    /// Returns a new vector where each element is the minimum of the
+    /// corresponding elements in `self` and `other`.
+    ///
+    /// This is equivalent to NEON's `vminq_f32` intrinsic.
+    ///
+    /// # Example
+    /// ```
+    /// use vastu_map::simd::Float4;
+    /// let a = Float4::new([1.0, 5.0, 3.0, 8.0]);
+    /// let b = Float4::new([2.0, 3.0, 4.0, 1.0]);
+    /// let min = a.min(b);
+    /// assert_eq!(min.to_array(), [1.0, 3.0, 3.0, 1.0]);
+    /// ```
+    #[inline(always)]
+    pub fn min(self, other: Self) -> Self {
+        Self([
+            self.0[0].min(other.0[0]),
+            self.0[1].min(other.0[1]),
+            self.0[2].min(other.0[2]),
+            self.0[3].min(other.0[3]),
+        ])
+    }
+
+    /// Element-wise maximum of two vectors.
+    ///
+    /// Returns a new vector where each element is the maximum of the
+    /// corresponding elements in `self` and `other`.
+    ///
+    /// This is equivalent to NEON's `vmaxq_f32` intrinsic.
+    ///
+    /// # Example
+    /// ```
+    /// use vastu_map::simd::Float4;
+    /// let a = Float4::new([1.0, 5.0, 3.0, 8.0]);
+    /// let b = Float4::new([2.0, 3.0, 4.0, 1.0]);
+    /// let max = a.max(b);
+    /// assert_eq!(max.to_array(), [2.0, 5.0, 4.0, 8.0]);
+    /// ```
+    #[inline(always)]
+    pub fn max(self, other: Self) -> Self {
+        Self([
+            self.0[0].max(other.0[0]),
+            self.0[1].max(other.0[1]),
+            self.0[2].max(other.0[2]),
+            self.0[3].max(other.0[3]),
+        ])
+    }
+
+    /// Fused multiply-add: `(self * a) + b`.
+    ///
+    /// Computes `self * a + b` with better precision and performance than
+    /// separate multiply and add operations. On ARM NEON, this maps to
+    /// `vfmaq_f32` (VFPv4).
+    ///
+    /// # Example
+    /// ```
+    /// use vastu_map::simd::Float4;
+    /// let a = Float4::new([1.0, 2.0, 3.0, 4.0]);
+    /// let b = Float4::new([2.0, 2.0, 2.0, 2.0]);
+    /// let c = Float4::new([10.0, 10.0, 10.0, 10.0]);
+    /// let result = a.mul_add(b, c);
+    /// // (1*2+10, 2*2+10, 3*2+10, 4*2+10) = (12, 14, 16, 18)
+    /// assert_eq!(result.to_array(), [12.0, 14.0, 16.0, 18.0]);
+    /// ```
+    #[inline(always)]
+    pub fn mul_add(self, a: Self, b: Self) -> Self {
+        Self([
+            self.0[0].mul_add(a.0[0], b.0[0]),
+            self.0[1].mul_add(a.0[1], b.0[1]),
+            self.0[2].mul_add(a.0[2], b.0[2]),
+            self.0[3].mul_add(a.0[3], b.0[3]),
+        ])
+    }
+
+    /// Fused negative multiply-add: `-(self * a) + b` = `b - (self * a)`.
+    ///
+    /// Useful for computing expressions like `tx - xs * sin_theta`.
+    ///
+    /// # Example
+    /// ```
+    /// use vastu_map::simd::Float4;
+    /// let a = Float4::new([1.0, 2.0, 3.0, 4.0]);
+    /// let b = Float4::new([2.0, 2.0, 2.0, 2.0]);
+    /// let c = Float4::new([10.0, 10.0, 10.0, 10.0]);
+    /// let result = a.neg_mul_add(b, c);
+    /// // (10-1*2, 10-2*2, 10-3*2, 10-4*2) = (8, 6, 4, 2)
+    /// assert_eq!(result.to_array(), [8.0, 6.0, 4.0, 2.0]);
+    /// ```
+    #[inline(always)]
+    pub fn neg_mul_add(self, a: Self, b: Self) -> Self {
+        Self([
+            (-self.0[0]).mul_add(a.0[0], b.0[0]),
+            (-self.0[1]).mul_add(a.0[1], b.0[1]),
+            (-self.0[2]).mul_add(a.0[2], b.0[2]),
+            (-self.0[3]).mul_add(a.0[3], b.0[3]),
+        ])
+    }
+
+    /// Horizontal sum of all elements.
+    ///
+    /// Returns the sum of all 4 elements in the vector.
+    #[inline(always)]
+    pub fn sum(self) -> f32 {
+        self.0[0] + self.0[1] + self.0[2] + self.0[3]
+    }
+
+    /// Horizontal minimum of all elements.
+    ///
+    /// Returns the minimum of all 4 elements in the vector.
+    #[inline(always)]
+    pub fn horizontal_min(self) -> f32 {
+        self.0[0].min(self.0[1]).min(self.0[2]).min(self.0[3])
+    }
+
+    /// Horizontal maximum of all elements.
+    ///
+    /// Returns the maximum of all 4 elements in the vector.
+    #[inline(always)]
+    pub fn horizontal_max(self) -> f32 {
+        self.0[0].max(self.0[1]).max(self.0[2]).max(self.0[3])
+    }
+}
+
+impl Add for Float4 {
+    type Output = Self;
+
+    /// Element-wise addition.
+    ///
+    /// This is equivalent to NEON's `vaddq_f32` intrinsic.
+    #[inline(always)]
+    fn add(self, rhs: Self) -> Self {
+        Self([
+            self.0[0] + rhs.0[0],
+            self.0[1] + rhs.0[1],
+            self.0[2] + rhs.0[2],
+            self.0[3] + rhs.0[3],
+        ])
+    }
+}
+
+impl Sub for Float4 {
+    type Output = Self;
+
+    /// Element-wise subtraction.
+    ///
+    /// This is equivalent to NEON's `vsubq_f32` intrinsic.
+    #[inline(always)]
+    fn sub(self, rhs: Self) -> Self {
+        Self([
+            self.0[0] - rhs.0[0],
+            self.0[1] - rhs.0[1],
+            self.0[2] - rhs.0[2],
+            self.0[3] - rhs.0[3],
+        ])
+    }
+}
+
+impl Mul for Float4 {
+    type Output = Self;
+
+    /// Element-wise multiplication.
+    ///
+    /// This is equivalent to NEON's `vmulq_f32` intrinsic.
+    #[inline(always)]
+    fn mul(self, rhs: Self) -> Self {
+        Self([
+            self.0[0] * rhs.0[0],
+            self.0[1] * rhs.0[1],
+            self.0[2] * rhs.0[2],
+            self.0[3] * rhs.0[3],
+        ])
+    }
+}
+
+impl AddAssign for Float4 {
+    /// In-place element-wise addition.
+    #[inline(always)]
+    fn add_assign(&mut self, rhs: Self) {
+        self.0[0] += rhs.0[0];
+        self.0[1] += rhs.0[1];
+        self.0[2] += rhs.0[2];
+        self.0[3] += rhs.0[3];
+    }
+}
+
+impl Default for Float4 {
+    fn default() -> Self {
+        Self::zero()
+    }
+}
+
+#[cfg(test)]
+mod tests {
+    use super::*;
+
+    #[test]
+    fn test_new_and_to_array() {
+        let v = Float4::new([1.0, 2.0, 3.0, 4.0]);
+        assert_eq!(v.to_array(), [1.0, 2.0, 3.0, 4.0]);
+    }
+
+    #[test]
+    fn test_splat() {
+        let v = Float4::splat(5.0);
+        assert_eq!(v.to_array(), [5.0, 5.0, 5.0, 5.0]);
+    }
+
+    #[test]
+    fn test_zero() {
+        let v = Float4::zero();
+        assert_eq!(v.to_array(), [0.0, 0.0, 0.0, 0.0]);
+    }
+
+    #[test]
+    fn test_add() {
+        let a = Float4::new([1.0, 2.0, 3.0, 4.0]);
+        let b = Float4::new([5.0, 6.0, 7.0, 8.0]);
+        let sum = a + b;
+        assert_eq!(sum.to_array(), [6.0, 8.0, 10.0, 12.0]);
+    }
+
+    #[test]
+    fn test_sub() {
+        let a = Float4::new([5.0, 6.0, 7.0, 8.0]);
+        let b = Float4::new([1.0, 2.0, 3.0, 4.0]);
+        let diff = a - b;
+        assert_eq!(diff.to_array(), [4.0, 4.0, 4.0, 4.0]);
+    }
+
+    #[test]
+    fn test_mul() {
+        let a = Float4::new([1.0, 2.0, 3.0, 4.0]);
+        let b = Float4::new([2.0, 3.0, 4.0, 5.0]);
+        let prod = a * b;
+        assert_eq!(prod.to_array(), [2.0, 6.0, 12.0, 20.0]);
+    }
+
+    #[test]
+    fn test_add_assign() {
+        let mut a = Float4::new([1.0, 2.0, 3.0, 4.0]);
+        let b = Float4::new([5.0, 6.0, 7.0, 8.0]);
+        a += b;
+        assert_eq!(a.to_array(), [6.0, 8.0, 10.0, 12.0]);
+    }
+
+    #[test]
+    fn test_min() {
+        let a = Float4::new([1.0, 5.0, 3.0, 8.0]);
+        let b = Float4::new([2.0, 3.0, 4.0, 1.0]);
+        let min = a.min(b);
+        assert_eq!(min.to_array(), [1.0, 3.0, 3.0, 1.0]);
+    }
+
+    #[test]
+    fn test_max() {
+        let a = Float4::new([1.0, 5.0, 3.0, 8.0]);
+        let b = Float4::new([2.0, 3.0, 4.0, 1.0]);
+        let max = a.max(b);
+        assert_eq!(max.to_array(), [2.0, 5.0, 4.0, 8.0]);
+    }
+
+    #[test]
+    fn test_mul_add() {
+        let a = Float4::new([1.0, 2.0, 3.0, 4.0]);
+        let b = Float4::new([2.0, 2.0, 2.0, 2.0]);
+        let c = Float4::new([10.0, 10.0, 10.0, 10.0]);
+        let result = a.mul_add(b, c);
+        assert_eq!(result.to_array(), [12.0, 14.0, 16.0, 18.0]);
+    }
+
+    #[test]
+    fn test_neg_mul_add() {
+        let a = Float4::new([1.0, 2.0, 3.0, 4.0]);
+        let b = Float4::new([2.0, 2.0, 2.0, 2.0]);
+        let c = Float4::new([10.0, 10.0, 10.0, 10.0]);
+        let result = a.neg_mul_add(b, c);
+        assert_eq!(result.to_array(), [8.0, 6.0, 4.0, 2.0]);
+    }
+
+    #[test]
+    fn test_sum() {
+        let v = Float4::new([1.0, 2.0, 3.0, 4.0]);
+        assert_eq!(v.sum(), 10.0);
+    }
+
+    #[test]
+    fn test_horizontal_min() {
+        let v = Float4::new([3.0, 1.0, 4.0, 2.0]);
+        assert_eq!(v.horizontal_min(), 1.0);
+    }
+
+    #[test]
+    fn test_horizontal_max() {
+        let v = Float4::new([3.0, 1.0, 4.0, 2.0]);
+        assert_eq!(v.horizontal_max(), 4.0);
+    }
+
+    #[test]
+    fn test_alignment() {
+        let v = Float4::new([1.0, 2.0, 3.0, 4.0]);
+        let ptr = &v as *const Float4 as usize;
+        assert_eq!(ptr % 16, 0, "Float4 should be 16-byte aligned");
+    }
+
+    #[test]
+    fn test_splat_special_values() {
+        let inf = Float4::splat(f32::INFINITY);
+        assert!(inf.to_array().iter().all(|&x| x == f32::INFINITY));
+
+        let neg_inf = Float4::splat(f32::NEG_INFINITY);
+        assert!(neg_inf.to_array().iter().all(|&x| x == f32::NEG_INFINITY));
+
+        let zero = Float4::splat(0.0);
+        assert_eq!(zero.to_array(), [0.0, 0.0, 0.0, 0.0]);
+    }
+
+    #[test]
+    fn test_fma_rotation_transform() {
+        // Test FMA for 2D rotation: x' = x*cos - y*sin, y' = x*sin + y*cos
+        let xs = Float4::new([1.0, 2.0, 3.0, 4.0]);
+        let ys = Float4::new([0.0, 0.0, 0.0, 0.0]);
+        let cos_t = Float4::splat(0.0); // cos(90°) = 0
+        let sin_t = Float4::splat(1.0); // sin(90°) = 1
+
+        // x' = x*cos - y*sin
+        let new_xs = ys.neg_mul_add(sin_t, xs * cos_t);
+        // y' = x*sin + y*cos
+        let new_ys = xs.mul_add(sin_t, ys * cos_t);
+
+        assert_eq!(new_xs.to_array(), [0.0, 0.0, 0.0, 0.0]);
+        assert_eq!(new_ys.to_array(), [1.0, 2.0, 3.0, 4.0]);
+    }
+}

vastu-map/src/simd/mod.rs

@@ -0,0 +1,8 @@
+//! SIMD types for auto-vectorization.
+//!
+//! This module provides SIMD-friendly types that encourage LLVM to
+//! emit vectorized instructions on ARM NEON and x86 SSE/AVX.
+
+mod f32x4;
+
+pub use f32x4::Float4;

vastu-map/src/vector_map.rs

@@ -0,0 +1,931 @@
+//! VectorMap: The main SLAM map implementation.
+//!
+//! VectorMap uses line and corner features as the primary map representation,
+//! providing efficient storage and queries for indoor environments.
+
+use crate::core::{Bounds, Point2D, PointCloud2D, Pose2D};
+use crate::extraction::{CornerConfig, SplitMergeConfig, detect_corners, extract_lines};
+use crate::features::{Corner2D, FeatureSet, Line2D, LineCollection};
+use crate::integration::{
+    AssociationConfig, MergerConfig, SpatialIndex, batch_merge, create_new_line, find_associations,
+    find_unmatched_scan_lines,
+};
+use crate::matching::{IcpConfig, match_scan};
+use crate::query::{FrontierConfig, OccupancyConfig, detect_frontiers, query_occupancy, raycast};
+use crate::{Frontier, Map, ObserveResult, Occupancy, Path};
+
+/// Configuration for VectorMap.
+#[derive(Clone, Debug)]
+pub struct VectorMapConfig {
+    /// Configuration for line extraction.
+    pub extraction: SplitMergeConfig,
+
+    /// Configuration for corner detection.
+    pub corner: CornerConfig,
+
+    /// Configuration for scan matching (ICP).
+    pub matching: IcpConfig,
+
+    /// Configuration for line association.
+    pub association: AssociationConfig,
+
+    /// Configuration for line merging.
+    pub merger: MergerConfig,
+
+    /// Configuration for frontier detection.
+    pub frontier: FrontierConfig,
+
+    /// Configuration for occupancy queries.
+    pub occupancy: OccupancyConfig,
+
+    /// Minimum match confidence to use scan matching result.
+    /// Below this, odometry is used instead.
+    /// Default: 0.3
+    pub min_match_confidence: f32,
+
+    /// Whether to update the map with new observations.
+    /// Set to false for localization-only mode.
+    /// Default: true
+    pub mapping_enabled: bool,
+}
+
+impl Default for VectorMapConfig {
+    fn default() -> Self {
+        Self {
+            extraction: SplitMergeConfig::default(),
+            corner: CornerConfig::default(),
+            matching: IcpConfig::default(),
+            association: AssociationConfig::default(),
+            merger: MergerConfig::default(),
+            frontier: FrontierConfig::default(),
+            occupancy: OccupancyConfig::default(),
+            min_match_confidence: 0.3,
+            mapping_enabled: true,
+        }
+    }
+}
+
+impl VectorMapConfig {
+    /// Create a new configuration with default values.
+    pub fn new() -> Self {
+        Self::default()
+    }
+
+    /// Builder-style setter for minimum match confidence.
+    pub fn with_min_match_confidence(mut self, confidence: f32) -> Self {
+        self.min_match_confidence = confidence;
+        self
+    }
+
+    /// Builder-style setter for mapping enabled.
+    pub fn with_mapping_enabled(mut self, enabled: bool) -> Self {
+        self.mapping_enabled = enabled;
+        self
+    }
+}
+
+/// VectorMap: Feature-based 2D SLAM map.
+///
+/// Stores the environment as a collection of line and corner features.
+/// Provides localization via scan matching and mapping via feature integration.
+///
+/// # Example
+///
+/// ```rust,ignore
+/// use vastu_map::{VectorMap, VectorMapConfig, Map};
+/// use vastu_map::core::{Pose2D, PointCloud2D, Point2D};
+///
+/// let config = VectorMapConfig::default();
+/// let mut map = VectorMap::new(config);
+///
+/// // Process scans
+/// let scan = PointCloud2D::from_points(&[
+///     Point2D::new(1.0, 0.0),
+///     Point2D::new(1.0, 0.1),
+///     Point2D::new(1.0, 0.2),
+/// ]);
+/// let odometry = Pose2D::identity();
+///
+/// let result = map.observe(&scan, odometry);
+/// println!("Pose: ({:.2}, {:.2}, {:.2})", result.pose.x, result.pose.y, result.pose.theta);
+/// ```
+pub struct VectorMap {
+    /// Configuration.
+    config: VectorMapConfig,
+
+    /// Line features.
+    lines: Vec<Line2D>,
+
+    /// Corner features.
+    corners: Vec<Corner2D>,
+
+    /// Spatial index for efficient queries.
+    index: SpatialIndex,
+
+    /// Line collection (SoA format) for SIMD operations.
+    line_collection: LineCollection,
+
+    /// Current map bounds.
+    map_bounds: Option<Bounds>,
+
+    /// Current robot pose estimate.
+    current_pose: Pose2D,
+
+    /// Number of observations processed.
+    observation_count: usize,
+}
+
+impl VectorMap {
+    /// Create a new empty VectorMap.
+    pub fn new(config: VectorMapConfig) -> Self {
+        Self {
+            config,
+            lines: Vec::new(),
+            corners: Vec::new(),
+            index: SpatialIndex::empty(),
+            line_collection: LineCollection::new(),
+            map_bounds: None,
+            current_pose: Pose2D::identity(),
+            observation_count: 0,
+        }
+    }
+
+    /// Get the current configuration.
+    pub fn config(&self) -> &VectorMapConfig {
+        &self.config
+    }
+
+    /// Get all lines in the map.
+    pub fn lines(&self) -> &[Line2D] {
+        &self.lines
+    }
+
+    /// Get all corners in the map.
+    pub fn corners(&self) -> &[Corner2D] {
+        &self.corners
+    }
+
+    /// Get the current robot pose.
+    pub fn current_pose(&self) -> Pose2D {
+        self.current_pose
+    }
+
+    /// Get the number of observations processed.
+    pub fn observation_count(&self) -> usize {
+        self.observation_count
+    }
+
+    /// Get the feature set (lines and corners).
+    pub fn features(&self) -> FeatureSet {
+        FeatureSet::from_features(&self.lines, &self.corners)
+    }
+
+    /// Add a line to the map.
+    pub fn add_line(&mut self, line: Line2D) {
+        let idx = self.lines.len();
+        self.lines.push(line);
+        self.line_collection.push_line(&line);
+        self.update_bounds(&line);
+
+        // Incremental index update (O(log n) vs O(n) rebuild)
+        self.index.insert(line, idx);
+    }
+
+    /// Add multiple lines to the map.
+    pub fn add_lines(&mut self, lines: &[Line2D]) {
+        // Threshold for incremental vs full rebuild
+        // For small additions, incremental is faster
+        // For large additions, bulk rebuild is more efficient
+        const INCREMENTAL_THRESHOLD: usize = 20;
+
+        let start_idx = self.lines.len();
+
+        for line in lines {
+            self.lines.push(*line);
+            self.line_collection.push_line(line);
+            self.update_bounds(line);
+        }
+
+        if lines.len() <= INCREMENTAL_THRESHOLD && self.lines.len() > lines.len() * 2 {
+            // Incremental insert for small additions to large maps
+            for (i, line) in lines.iter().enumerate() {
+                self.index.insert(*line, start_idx + i);
+            }
+        } else {
+            // Full rebuild for large additions or small maps
+            self.rebuild_index();
+        }
+    }
+
+    /// Set the current pose directly.
+    pub fn set_pose(&mut self, pose: Pose2D) {
+        self.current_pose = pose;
+    }
+
+    /// Process a scan observation.
+    ///
+    /// This is the main SLAM function. It:
+    /// 1. Extracts features from the scan
+    /// 2. Matches against the map (localization)
+    /// 3. Updates the map with new features (mapping)
+    fn process_scan(&mut self, scan: &PointCloud2D, odometry: Pose2D) -> ObserveResult {
+        self.observation_count += 1;
+
+        // If map is empty, use odometry and add all features
+        if self.lines.is_empty() {
+            return self.initialize_from_scan(scan, odometry);
+        }
+
+        // Apply odometry to get predicted pose
+        let predicted_pose = self.current_pose.compose(odometry);
+
+        // Transform scan to world frame using predicted pose
+        let world_scan = scan.transform(&predicted_pose);
+        let scan_points: Vec<Point2D> = world_scan.iter().collect();
+
+        // Try scan matching
+        let match_result = match_scan(&scan_points, &self.lines, Pose2D::identity());
+
+        // Determine final pose
+        let (final_pose, confidence) = if match_result.converged
+            && match_result.confidence >= self.config.min_match_confidence
+        {
+            // Apply match correction to predicted pose
+            let corrected = predicted_pose.compose(match_result.pose);
+            (corrected, match_result.confidence)
+        } else {
+            // Fall back to odometry
+            (predicted_pose, 0.0)
+        };
+
+        self.current_pose = final_pose;
+
+        // Extract features from scan
+        let features = self.extract_features(&world_scan);
+
+        // Update map if mapping is enabled
+        let (features_added, features_merged) = if self.config.mapping_enabled {
+            self.integrate_features(&features)
+        } else {
+            (0, 0)
+        };
+
+        ObserveResult {
+            pose: final_pose,
+            confidence,
+            features_extracted: features.lines().len(),
+            features_added,
+            features_merged,
+        }
+    }
+
+    /// Initialize map from first scan.
+    fn initialize_from_scan(&mut self, scan: &PointCloud2D, odometry: Pose2D) -> ObserveResult {
+        self.current_pose = odometry;
+
+        // Transform scan to world frame
+        let world_scan = scan.transform(&odometry);
+
+        // Extract features
+        let features = self.extract_features(&world_scan);
+
+        // Add all features to map
+        let num_lines = features.lines().len();
+        let feature_lines: Vec<Line2D> = features.lines().to_vec();
+        self.add_lines(&feature_lines);
+
+        for corner in features.corners() {
+            self.corners.push(corner);
+        }
+
+        ObserveResult {
+            pose: odometry,
+            confidence: 0.0, // No match on first scan
+            features_extracted: num_lines,
+            features_added: num_lines,
+            features_merged: 0,
+        }
+    }
+
+    /// Extract features from a scan.
+    fn extract_features(&self, scan: &PointCloud2D) -> FeatureSet {
+        let points: Vec<Point2D> = scan.iter().collect();
+
+        // Extract lines
+        let lines = extract_lines(&points, &self.config.extraction);
+
+        // Extract corners from lines
+        let corners = detect_corners(&lines, &self.config.corner);
+
+        FeatureSet::from_features(&lines, &corners)
+    }
+
+    /// Integrate new features into the map.
+    fn integrate_features(&mut self, features: &FeatureSet) -> (usize, usize) {
+        let scan_lines: Vec<Line2D> = features.lines().to_vec();
+
+        if scan_lines.is_empty() {
+            return (0, 0);
+        }
+
+        // Find associations
+        let associations = find_associations(
+            &scan_lines,
+            &self.lines,
+            Some(&self.index),
+            &self.config.association,
+        );
+
+        // Merge matched lines
+        let merged = batch_merge(
+            &scan_lines,
+            &mut self.lines,
+            &associations,
+            &self.config.merger,
+        );
+        let features_merged = merged.len();
+
+        // Find and add unmatched lines
+        let unmatched = find_unmatched_scan_lines(
+            &scan_lines,
+            &self.lines,
+            Some(&self.index),
+            &self.config.association,
+        );
+
+        let mut features_added = 0;
+        for idx in unmatched {
+            let new_line = create_new_line(&scan_lines[idx]);
+            self.lines.push(new_line);
+            features_added += 1;
+        }
+
+        // Rebuild auxiliary structures
+        self.rebuild_line_collection();
+        self.rebuild_index();
+        self.recompute_bounds();
+
+        // Update corners
+        self.update_corners();
+
+        (features_added, features_merged)
+    }
+
+    /// Update bounds to include a line.
+    fn update_bounds(&mut self, line: &Line2D) {
+        match &mut self.map_bounds {
+            Some(b) => {
+                b.expand_to_include(line.start);
+                b.expand_to_include(line.end);
+            }
+            None => {
+                let mut bounds = Bounds::from_point(line.start);
+                bounds.expand_to_include(line.end);
+                self.map_bounds = Some(bounds);
+            }
+        }
+    }
+
+    /// Recompute bounds from all lines.
+    fn recompute_bounds(&mut self) {
+        if self.lines.is_empty() {
+            self.map_bounds = None;
+            return;
+        }
+
+        let mut bounds = Bounds::from_point(self.lines[0].start);
+        for line in &self.lines {
+            bounds.expand_to_include(line.start);
+            bounds.expand_to_include(line.end);
+        }
+        self.map_bounds = Some(bounds);
+    }
+
+    /// Rebuild the spatial index.
+    fn rebuild_index(&mut self) {
+        self.index.rebuild(&self.lines);
+    }
+
+    /// Rebuild the line collection from lines.
+    fn rebuild_line_collection(&mut self) {
+        self.line_collection = LineCollection::from_lines(&self.lines);
+    }
+
+    /// Update corners based on current lines.
+    fn update_corners(&mut self) {
+        self.corners = detect_corners(&self.lines, &self.config.corner);
+    }
+}
+
+impl Map for VectorMap {
+    fn observe(&mut self, scan: &PointCloud2D, odometry: Pose2D) -> ObserveResult {
+        self.process_scan(scan, odometry)
+    }
+
+    fn raycast(&self, from: Point2D, direction: Point2D, max_range: f32) -> f32 {
+        raycast(from, direction, max_range, &self.lines)
+    }
+
+    fn query(&self, point: Point2D) -> Occupancy {
+        query_occupancy(
+            point,
+            &self.lines,
+            self.map_bounds.as_ref(),
+            &self.config.occupancy,
+        )
+    }
+
+    fn frontiers(&self) -> Vec<Frontier> {
+        detect_frontiers(&self.lines, &self.config.frontier)
+    }
+
+    fn get_path(&self, _from: Point2D, _to: Point2D) -> Option<Path> {
+        // Path planning not yet implemented
+        None
+    }
+
+    fn bounds(&self) -> Bounds {
+        self.map_bounds.unwrap_or_else(Bounds::empty)
+    }
+
+    fn clear(&mut self) {
+        self.lines.clear();
+        self.corners.clear();
+        self.index.clear();
+        self.line_collection.clear();
+        self.map_bounds = None;
+        self.current_pose = Pose2D::identity();
+        self.observation_count = 0;
+    }
+}
+
+impl Default for VectorMap {
+    fn default() -> Self {
+        Self::new(VectorMapConfig::default())
+    }
+}
+
+#[cfg(test)]
+mod tests {
+    use super::*;
+
+    fn make_simple_scan() -> PointCloud2D {
+        // Simulate a scan seeing a wall in front
+        let mut xs = Vec::new();
+        let mut ys = Vec::new();
+
+        for i in -10..=10 {
+            xs.push(2.0); // Wall at x=2
+            ys.push(i as f32 * 0.1);
+        }
+
+        PointCloud2D { xs, ys }
+    }
+
+    #[allow(dead_code)]
+    fn make_room_scan() -> PointCloud2D {
+        // Simulate a scan from center of a 4x4 room
+        let mut xs = Vec::new();
+        let mut ys = Vec::new();
+
+        // See walls at ±2 in each direction
+        for i in -10..=10 {
+            let y = i as f32 * 0.2;
+            // Left wall
+            xs.push(-2.0);
+            ys.push(y);
+            // Right wall
+            xs.push(2.0);
+            ys.push(y);
+        }
+        for i in -10..=10 {
+            let x = i as f32 * 0.2;
+            // Bottom wall
+            xs.push(x);
+            ys.push(-2.0);
+            // Top wall
+            xs.push(x);
+            ys.push(2.0);
+        }
+
+        PointCloud2D { xs, ys }
+    }
+
+    #[test]
+    fn test_new_map() {
+        let config = VectorMapConfig::default();
+        let map = VectorMap::new(config);
+
+        assert!(map.lines.is_empty());
+        assert!(map.corners.is_empty());
+        assert_eq!(map.observation_count, 0);
+    }
+
+    #[test]
+    fn test_add_line() {
+        let mut map = VectorMap::default();
+        let line = Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(5.0, 0.0));
+
+        map.add_line(line);
+
+        assert_eq!(map.lines.len(), 1);
+        assert!(map.map_bounds.is_some());
+    }
+
+    #[test]
+    fn test_first_observation() {
+        let mut map = VectorMap::default();
+        let scan = make_simple_scan();
+
+        let result = map.observe(&scan, Pose2D::identity());
+
+        assert_eq!(result.pose, Pose2D::identity());
+        assert!(map.lines.len() > 0 || result.features_extracted > 0);
+        assert_eq!(map.observation_count, 1);
+    }
+
+    #[test]
+    fn test_raycast_through_map() {
+        let mut map = VectorMap::default();
+        map.add_line(Line2D::new(Point2D::new(5.0, -5.0), Point2D::new(5.0, 5.0)));
+
+        let dist = map.raycast(Point2D::zero(), Point2D::new(1.0, 0.0), 100.0);
+
+        assert!((dist - 5.0).abs() < 0.1);
+    }
+
+    #[test]
+    fn test_query_occupancy() {
+        let mut map = VectorMap::default();
+        map.add_line(Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(10.0, 0.0)));
+
+        // On the line
+        let on_line = map.query(Point2D::new(5.0, 0.0));
+        assert_eq!(on_line, Occupancy::Occupied);
+    }
+
+    #[test]
+    fn test_bounds() {
+        let mut map = VectorMap::default();
+
+        let empty_bounds = map.bounds();
+        assert!(empty_bounds.is_empty());
+
+        map.add_line(Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(10.0, 5.0)));
+
+        let bounds = map.bounds();
+        assert!(!bounds.is_empty());
+        assert!(bounds.contains(Point2D::new(5.0, 2.0)));
+    }
+
+    #[test]
+    fn test_clear() {
+        let mut map = VectorMap::default();
+        map.add_line(Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(5.0, 0.0)));
+
+        map.clear();
+
+        assert!(map.lines.is_empty());
+        assert!(map.corners.is_empty());
+        assert!(map.map_bounds.is_none());
+        assert_eq!(map.current_pose, Pose2D::identity());
+    }
+
+    #[test]
+    fn test_frontiers_empty_map() {
+        let map = VectorMap::default();
+        let frontiers = map.frontiers();
+
+        assert!(frontiers.is_empty());
+    }
+
+    #[test]
+    fn test_frontiers_partial_room() {
+        let mut map = VectorMap::default();
+
+        // Add three walls of a room (missing one side)
+        map.add_lines(&[
+            Line2D::new(Point2D::new(-2.0, -2.0), Point2D::new(2.0, -2.0)), // Bottom
+            Line2D::new(Point2D::new(2.0, -2.0), Point2D::new(2.0, 2.0)),   // Right
+            Line2D::new(Point2D::new(-2.0, 2.0), Point2D::new(-2.0, -2.0)), // Left
+        ]);
+
+        let frontiers = map.frontiers();
+
+        // Should have frontiers at the open top
+        assert!(!frontiers.is_empty());
+    }
+
+    #[test]
+    fn test_localization_only_mode() {
+        let config = VectorMapConfig::default().with_mapping_enabled(false);
+        let mut map = VectorMap::new(config);
+
+        // Add initial map
+        map.add_line(Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(10.0, 0.0)));
+        let initial_count = map.lines.len();
+
+        // Process scan - should not add features
+        let scan = make_simple_scan();
+        map.observe(&scan, Pose2D::identity());
+
+        // Line count should not change
+        assert_eq!(map.lines.len(), initial_count);
+    }
+
+    #[test]
+    fn test_features() {
+        let mut map = VectorMap::default();
+        map.add_lines(&[
+            Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(5.0, 0.0)),
+            Line2D::new(Point2D::new(5.0, 0.0), Point2D::new(5.0, 5.0)),
+        ]);
+
+        let features = map.features();
+
+        assert_eq!(features.lines().len(), 2);
+    }
+
+    // === Additional full-cycle integration tests ===
+
+    /// Create a scan simulating walls of a square room from a given position.
+    #[allow(dead_code)]
+    fn make_scan_from_position(_pos: Point2D, room_size: f32) -> PointCloud2D {
+        let half = room_size / 2.0;
+        let mut xs = Vec::new();
+        let mut ys = Vec::new();
+
+        // Generate points for each wall relative to position
+        let walls = [
+            (Point2D::new(-half, -half), Point2D::new(half, -half)), // Bottom
+            (Point2D::new(half, -half), Point2D::new(half, half)),   // Right
+            (Point2D::new(half, half), Point2D::new(-half, half)),   // Top
+            (Point2D::new(-half, half), Point2D::new(-half, -half)), // Left
+        ];
+
+        for (start, end) in walls {
+            // Generate 15 points along each wall
+            for i in 0..15 {
+                let t = i as f32 / 14.0;
+                let point = Point2D::new(
+                    start.x + (end.x - start.x) * t,
+                    start.y + (end.y - start.y) * t,
+                );
+                // Transform to world coordinates (assuming pos is robot position)
+                xs.push(point.x);
+                ys.push(point.y);
+            }
+        }
+
+        PointCloud2D { xs, ys }
+    }
+
+    #[test]
+    fn test_multi_scan_mapping_grows_map() {
+        // Test that processing multiple scans grows the map appropriately
+        let mut map = VectorMap::default();
+
+        // Create a better scan with dense points along walls
+        let mut scan1 = PointCloud2D {
+            xs: Vec::new(),
+            ys: Vec::new(),
+        };
+        // Left wall - many closely spaced points
+        for i in 0..20 {
+            scan1.xs.push(-2.0);
+            scan1.ys.push(-2.0 + i as f32 * 0.2);
+        }
+        // Right wall
+        for i in 0..20 {
+            scan1.xs.push(2.0);
+            scan1.ys.push(-2.0 + i as f32 * 0.2);
+        }
+
+        // First scan - should initialize the map
+        let _ = map.observe(&scan1, Pose2D::identity());
+
+        let _lines_after_first = map.lines().len();
+        // Even if no features extracted (depends on config), map should handle it
+        assert_eq!(map.observation_count(), 1);
+
+        // Second scan from slightly different position
+        let odometry = Pose2D::new(0.3, 0.2, 0.05);
+        let _ = map.observe(&scan1, odometry);
+
+        assert_eq!(map.observation_count(), 2);
+        // Map processes observations successfully if we reach here
+    }
+
+    #[test]
+    fn test_slam_cycle_extract_match_integrate() {
+        // Test complete SLAM cycle: extract → match → associate → merge
+        let mut map = VectorMap::default();
+
+        // Initialize with known walls
+        map.add_lines(&[
+            Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(5.0, 0.0)), // Bottom wall
+            Line2D::new(Point2D::new(5.0, 0.0), Point2D::new(5.0, 5.0)), // Right wall
+            Line2D::new(Point2D::new(5.0, 5.0), Point2D::new(0.0, 5.0)), // Top wall
+            Line2D::new(Point2D::new(0.0, 5.0), Point2D::new(0.0, 0.0)), // Left wall
+        ]);
+
+        let initial_lines = map.lines().len();
+
+        // Simulate scan from center of room seeing all walls
+        let mut scan = PointCloud2D {
+            xs: Vec::new(),
+            ys: Vec::new(),
+        };
+        // Bottom wall points
+        for i in 0..10 {
+            scan.xs.push(0.5 + i as f32 * 0.4);
+            scan.ys.push(0.05);
+        }
+        // Right wall points
+        for i in 0..10 {
+            scan.xs.push(4.95);
+            scan.ys.push(0.5 + i as f32 * 0.4);
+        }
+
+        // Set robot pose at center
+        map.set_pose(Pose2D::new(2.5, 2.5, 0.0));
+
+        // Process scan with zero odometry (robot hasn't moved)
+        let _ = map.observe(&scan, Pose2D::identity());
+
+        // Verify SLAM components worked
+        // Map should not explode in size
+        assert!(
+            map.lines().len() <= initial_lines + 5,
+            "Map should not grow excessively: had {}, now {}",
+            initial_lines,
+            map.lines().len()
+        );
+    }
+
+    #[test]
+    fn test_localization_maintains_pose() {
+        // Test that consecutive scans maintain reasonable pose
+        let config = VectorMapConfig::default().with_min_match_confidence(0.1);
+        let mut map = VectorMap::new(config);
+
+        // Add a simple room
+        map.add_lines(&[
+            Line2D::new(Point2D::new(-2.0, -2.0), Point2D::new(2.0, -2.0)),
+            Line2D::new(Point2D::new(2.0, -2.0), Point2D::new(2.0, 2.0)),
+            Line2D::new(Point2D::new(2.0, 2.0), Point2D::new(-2.0, 2.0)),
+            Line2D::new(Point2D::new(-2.0, 2.0), Point2D::new(-2.0, -2.0)),
+        ]);
+
+        // Start at origin
+        map.set_pose(Pose2D::identity());
+
+        // Process several scans with small movements
+        let scan = make_room_scan();
+        let mut total_movement = 0.0;
+
+        for _ in 0..5 {
+            let odom = Pose2D::new(0.1, 0.05, 0.02);
+            map.observe(&scan, odom);
+            total_movement += (0.1f32.powi(2) + 0.05f32.powi(2)).sqrt();
+        }
+
+        // Final pose should be roughly accumulated odometry
+        let pose = map.current_pose();
+        let pose_dist = (pose.x.powi(2) + pose.y.powi(2)).sqrt();
+
+        // Allow for some drift, but should be in the ballpark
+        assert!(
+            pose_dist < total_movement * 2.0,
+            "Pose drift too large: {} vs max {}",
+            pose_dist,
+            total_movement * 2.0
+        );
+    }
+
+    #[test]
+    fn test_map_bounds_grow_with_exploration() {
+        let mut map = VectorMap::default();
+
+        // Initial small map
+        map.add_line(Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(1.0, 0.0)));
+        let initial_bounds = map.bounds();
+
+        // Add more lines extending the map
+        map.add_line(Line2D::new(Point2D::new(1.0, 0.0), Point2D::new(5.0, 0.0)));
+        map.add_line(Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(0.0, 3.0)));
+
+        let expanded_bounds = map.bounds();
+
+        // Bounds should have grown
+        assert!(
+            expanded_bounds.width() > initial_bounds.width(),
+            "Bounds width should grow"
+        );
+        assert!(
+            expanded_bounds.height() > initial_bounds.height(),
+            "Bounds height should grow"
+        );
+    }
+
+    #[test]
+    fn test_feature_count_stability() {
+        // Test that feature count stabilizes when observing same scene repeatedly
+        let mut map = VectorMap::default();
+        let scan = make_room_scan();
+
+        // Process first scan
+        map.observe(&scan, Pose2D::identity());
+        let count_after_1 = map.lines().len();
+
+        // Process same scan 5 more times with tiny movements
+        for _ in 0..5 {
+            let small_move = Pose2D::new(0.01, 0.01, 0.005);
+            map.observe(&scan, small_move);
+        }
+
+        let count_after_6 = map.lines().len();
+
+        // Feature count should stabilize (not grow unboundedly)
+        assert!(
+            count_after_6 <= count_after_1 * 3,
+            "Feature count should stabilize: started with {}, ended with {}",
+            count_after_1,
+            count_after_6
+        );
+    }
+
+    #[test]
+    fn test_config_propagation() {
+        // Test that config values are properly propagated
+        let config = VectorMapConfig::default()
+            .with_min_match_confidence(0.5)
+            .with_mapping_enabled(false);
+
+        let map = VectorMap::new(config);
+
+        assert_eq!(map.config().min_match_confidence, 0.5);
+        assert!(!map.config().mapping_enabled);
+    }
+
+    #[test]
+    fn test_set_pose() {
+        let mut map = VectorMap::default();
+
+        let target_pose = Pose2D::new(1.5, 2.5, 0.7);
+        map.set_pose(target_pose);
+
+        assert_eq!(map.current_pose().x, target_pose.x);
+        assert_eq!(map.current_pose().y, target_pose.y);
+        assert_eq!(map.current_pose().theta, target_pose.theta);
+    }
+
+    #[test]
+    fn test_observation_count_increments() {
+        let mut map = VectorMap::default();
+        let scan = make_simple_scan();
+
+        assert_eq!(map.observation_count(), 0);
+
+        map.observe(&scan, Pose2D::identity());
+        assert_eq!(map.observation_count(), 1);
+
+        map.observe(&scan, Pose2D::identity());
+        assert_eq!(map.observation_count(), 2);
+
+        map.observe(&scan, Pose2D::identity());
+        assert_eq!(map.observation_count(), 3);
+    }
+
+    #[test]
+    fn test_add_lines_batch() {
+        let mut map = VectorMap::default();
+
+        let lines = vec![
+            Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(1.0, 0.0)),
+            Line2D::new(Point2D::new(1.0, 0.0), Point2D::new(1.0, 1.0)),
+            Line2D::new(Point2D::new(1.0, 1.0), Point2D::new(0.0, 1.0)),
+        ];
+
+        map.add_lines(&lines);
+
+        assert_eq!(map.lines().len(), 3);
+        assert!(map.bounds().contains(Point2D::new(0.5, 0.5)));
+    }
+
+    #[test]
+    fn test_empty_scan_handling() {
+        let mut map = VectorMap::default();
+        map.add_line(Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(5.0, 0.0)));
+
+        let empty_scan = PointCloud2D {
+            xs: vec![],
+            ys: vec![],
+        };
+        let result = map.observe(&empty_scan, Pose2D::identity());
+
+        // Should handle gracefully
+        assert_eq!(result.features_extracted, 0);
+        assert_eq!(map.observation_count(), 1);
+    }
+}

vastu-map/tests/integration/adapters.rs

@@ -0,0 +1,120 @@
+//! Type conversions between sangam-io and vastu-map
+//!
+//! sangam-io's lidar simulator produces `Vec<(f32, f32, u8)>` which maps
+//! directly to vastu-map's `PolarScan.points` format.
+//!
+//! Note: vastu-map's extract_lines algorithm can hang on large continuous
+//! point clouds (>100 points). We subsample lidar data to work around this.
+
+use vastu_map::core::{PointCloud2D, PolarScan, Pose2D};
+
+/// Convert lidar scan data from sangam-io format to vastu-map PointCloud2D.
+///
+/// This function subsamples the data to avoid performance issues with
+/// vastu-map's line extraction algorithm on large continuous point clouds.
+///
+/// # Arguments
+/// * `lidar_data` - Raw lidar data from LidarSimulator::generate_scan()
+/// * `min_quality` - Minimum quality value to include (0-255)
+/// * `min_range` - Minimum valid range in meters
+/// * `max_range` - Maximum valid range in meters
+///
+/// # Returns
+/// Cartesian point cloud in robot frame, filtered by quality and range.
+pub fn lidar_to_point_cloud(
+    lidar_data: Vec<(f32, f32, u8)>,
+    min_quality: u8,
+    min_range: f32,
+    max_range: f32,
+) -> PointCloud2D {
+    // Split lidar data into 4 quadrants to avoid continuous loop issue
+    // vastu-map's algorithm hangs on continuous point clouds
+    // By taking only front-facing points, we get a simpler pattern
+
+    // Filter to front 180 degrees only (angle between -PI/2 and PI/2)
+    let front_points: Vec<_> = lidar_data
+        .into_iter()
+        .filter(|(angle, _, _)| {
+            // Keep only front-facing points
+            angle.abs() < std::f32::consts::FRAC_PI_2
+        })
+        .step_by(5) // Subsample to ~18 points
+        .collect();
+
+    let polar = PolarScan {
+        points: front_points,
+    };
+    polar.to_cartesian(min_quality, min_range, max_range)
+}
+
+/// Convert lidar scan data without subsampling (for testing with known small datasets).
+///
+/// # Warning
+/// Only use this with small point clouds (<100 points) or datasets that
+/// don't form continuous loops.
+pub fn lidar_to_point_cloud_full(
+    lidar_data: Vec<(f32, f32, u8)>,
+    min_quality: u8,
+    min_range: f32,
+    max_range: f32,
+) -> PointCloud2D {
+    let polar = PolarScan { points: lidar_data };
+    polar.to_cartesian(min_quality, min_range, max_range)
+}
+
+/// Create a vastu-map Pose2D from physics state.
+pub fn physics_to_pose(x: f32, y: f32, theta: f32) -> Pose2D {
+    Pose2D::new(x, y, theta)
+}
+
+#[cfg(test)]
+mod tests {
+    use super::*;
+    use std::f32::consts::FRAC_PI_2;
+
+    #[test]
+    fn test_lidar_to_point_cloud_full() {
+        // Test the full (unfiltered) conversion
+        let lidar_data = vec![
+            (0.0, 1.0, 100),                 // Forward, 1m
+            (FRAC_PI_2, 2.0, 100),           // Left, 2m
+            (std::f32::consts::PI, 1.5, 50), // Behind, low quality
+        ];
+
+        let cloud = lidar_to_point_cloud_full(lidar_data, 60, 0.1, 10.0);
+
+        // Third point filtered by quality
+        assert_eq!(cloud.len(), 2);
+
+        // First point should be ~(1, 0)
+        assert!((cloud.xs[0] - 1.0).abs() < 0.001);
+        assert!(cloud.ys[0].abs() < 0.001);
+
+        // Second point should be ~(0, 2)
+        assert!(cloud.xs[1].abs() < 0.001);
+        assert!((cloud.ys[1] - 2.0).abs() < 0.001);
+    }
+
+    #[test]
+    fn test_lidar_to_point_cloud_filters_back() {
+        // Test that the subsampled version only keeps front-facing points
+        let lidar_data: Vec<_> = (0..360)
+            .map(|i| {
+                let angle = (i as f32).to_radians();
+                (angle, 1.0, 100u8)
+            })
+            .collect();
+
+        let cloud = lidar_to_point_cloud(lidar_data, 50, 0.1, 10.0);
+
+        // Should only keep front 180 degrees, subsampled by 5
+        // ~90 points in front, subsampled to ~18
+        assert!(cloud.len() > 0, "Should have some points");
+        assert!(cloud.len() < 30, "Should be subsampled");
+
+        // All points should be in front (x > 0 for angle near 0)
+        // But actually the cloud is in robot frame, so we check y range
+        // Points from -90 to +90 degrees will have x values near cos(angle)
+        // which is positive for angles near 0
+    }
+}

vastu-map/tests/integration/harness.rs

@@ -0,0 +1,324 @@
+//! Core test harness for integration tests
+//!
+//! Provides synchronous simulation using sangam-io's mock device components
+//! directly (without spawning daemon threads).
+
+use sangam_io::devices::mock::config::{LidarConfig, RobotConfig};
+use sangam_io::devices::mock::lidar_sim::LidarSimulator;
+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::core::Pose2D;
+use vastu_map::{Map, VectorMap, VectorMapConfig};
+
+use crate::adapters::lidar_to_point_cloud;
+use crate::metrics::TestMetrics;
+use crate::path_generator::PathSegment;
+use crate::visualization::Visualizer;
+
+/// Test harness configuration.
+#[derive(Clone)]
+pub struct HarnessConfig {
+    /// Path to map YAML file
+    pub map_file: String,
+    /// Starting X position in meters
+    pub start_x: f32,
+    /// Starting Y position in meters
+    pub start_y: f32,
+    /// Starting orientation in radians
+    pub start_theta: f32,
+    /// Robot configuration
+    pub robot: RobotConfig,
+    /// Lidar configuration
+    pub lidar: LidarConfig,
+    /// SLAM configuration
+    pub slam: VectorMapConfig,
+    /// Random seed (0 = random)
+    pub random_seed: u64,
+    /// Minimum lidar quality to include
+    pub min_quality: u8,
+    /// Minimum lidar range in meters
+    pub min_range: f32,
+    /// Maximum lidar range in meters
+    pub max_range: f32,
+    /// Simulation timestep in seconds
+    pub dt: f32,
+    /// Lidar scan interval (in simulation steps)
+    pub lidar_interval: usize,
+}
+
+impl Default for HarnessConfig {
+    fn default() -> Self {
+        Self {
+            map_file: String::new(),
+            start_x: 2.5,
+            start_y: 2.5,
+            start_theta: 0.0,
+            robot: RobotConfig::default(),
+            lidar: LidarConfig::default(),
+            slam: VectorMapConfig::default(),
+            random_seed: 42, // Fixed seed for reproducibility
+            min_quality: 50,
+            min_range: 0.15,
+            max_range: 8.0,
+            dt: 1.0 / 110.0,    // 110Hz simulation
+            lidar_interval: 22, // ~5Hz lidar (110/22 = 5)
+        }
+    }
+}
+
+impl HarnessConfig {
+    /// Create config for the simple_room map.
+    pub fn simple_room() -> Self {
+        // Use env!() to get project root at compile time
+        let map_file = concat!(
+            env!("CARGO_MANIFEST_DIR"),
+            "/../sangam-io/maps/simple_room.yaml"
+        );
+        Self {
+            map_file: map_file.to_string(),
+            start_x: 2.5,
+            start_y: 2.5,
+            ..Default::default()
+        }
+    }
+
+    /// Create config for the large_room_obstacles map.
+    pub fn large_room_obstacles() -> Self {
+        let map_file = concat!(
+            env!("CARGO_MANIFEST_DIR"),
+            "/../sangam-io/maps/large_room_obstacles.yaml"
+        );
+        Self {
+            map_file: map_file.to_string(),
+            start_x: 5.0,
+            start_y: 5.0,
+            ..Default::default()
+        }
+    }
+}
+
+/// Result of running a test scenario.
+#[derive(Debug)]
+pub struct TestResult {
+    /// Computed metrics
+    pub metrics: TestMetrics,
+    /// Final SLAM pose estimate
+    pub final_pose: Pose2D,
+    /// Ground truth pose from physics
+    pub ground_truth_pose: Pose2D,
+    /// Number of observations processed
+    pub observations: usize,
+    /// Total simulation time in seconds
+    pub sim_time: f32,
+}
+
+/// Main test harness for running integration tests.
+pub struct TestHarness {
+    config: HarnessConfig,
+    map: SimulationMap,
+    physics: PhysicsState,
+    lidar_sim: LidarSimulator,
+    slam: VectorMap,
+    visualizer: Option<Visualizer>,
+    sim_time: f32,
+    tick_count: usize,
+    observations: usize,
+    last_slam_pose: Pose2D,
+}
+
+impl TestHarness {
+    /// Create a new test harness.
+    pub fn new(config: HarnessConfig) -> Result<Self, Box<dyn std::error::Error>> {
+        // Load map
+        let map = SimulationMap::load(&config.map_file)?;
+
+        // Initialize physics at start pose
+        let physics = PhysicsState::new(
+            config.start_x,
+            config.start_y,
+            config.start_theta,
+            &config.robot,
+        );
+
+        // Create noise generator with fixed seed
+        let noise = NoiseGenerator::new(config.random_seed);
+
+        // Create lidar simulator
+        let lidar_sim = LidarSimulator::new(&config.lidar, noise);
+
+        // Create SLAM map
+        let slam = VectorMap::new(config.slam.clone());
+
+        let start_pose = Pose2D::new(config.start_x, config.start_y, config.start_theta);
+
+        Ok(Self {
+            config,
+            map,
+            physics,
+            lidar_sim,
+            slam,
+            visualizer: None,
+            sim_time: 0.0,
+            tick_count: 0,
+            observations: 0,
+            last_slam_pose: start_pose,
+        })
+    }
+
+    /// Enable visualization output.
+    pub fn with_visualization(mut self, output_path: &str) -> Self {
+        self.visualizer = Some(Visualizer::new(output_path));
+        self
+    }
+
+    /// Run simulation along a path.
+    pub fn run_path(&mut self, segments: &[PathSegment]) -> TestResult {
+        for segment in segments {
+            self.run_segment(segment);
+        }
+
+        self.finalize()
+    }
+
+    /// Run a single path segment.
+    fn run_segment(&mut self, segment: &PathSegment) {
+        let steps = (segment.duration / self.config.dt) as usize;
+
+        for _ in 0..steps {
+            // Update physics
+            self.physics.update(
+                self.config.dt,
+                segment.linear_vel,
+                segment.angular_vel,
+                &self.map,
+                &self.config.robot,
+            );
+
+            self.tick_count += 1;
+            self.sim_time += self.config.dt;
+
+            // Generate lidar scan at configured interval
+            if self.tick_count >= self.config.lidar_interval {
+                self.tick_count = 0;
+                self.process_lidar_scan();
+            }
+        }
+    }
+
+    /// Process a lidar scan and feed to SLAM.
+    fn process_lidar_scan(&mut self) {
+        // Generate lidar scan
+        let scan_data = self.lidar_sim.generate_scan(
+            &self.map,
+            self.physics.x(),
+            self.physics.y(),
+            self.physics.theta(),
+        );
+
+        // Convert to point cloud
+        let cloud = lidar_to_point_cloud(
+            scan_data,
+            self.config.min_quality,
+            self.config.min_range,
+            self.config.max_range,
+        );
+
+        // Skip if scan is too sparse
+        if cloud.len() < 10 {
+            return;
+        }
+
+        // Compute relative odometry from ground truth (simplified)
+        // In real tests, this would come from encoder simulation
+        let current_truth = Pose2D::new(self.physics.x(), self.physics.y(), self.physics.theta());
+
+        let odometry = if self.observations == 0 {
+            // First observation - use absolute pose as initial guess
+            current_truth
+        } else {
+            // Subsequent observations - compute relative pose
+            self.compute_relative_odometry(current_truth)
+        };
+
+        // Feed to SLAM
+        let result = self.slam.observe(&cloud, odometry);
+        self.last_slam_pose = result.pose;
+        self.observations += 1;
+
+        log::debug!(
+            "Observation {}: truth=({:.2}, {:.2}, {:.1}°), slam=({:.2}, {:.2}, {:.1}°), conf={:.2}",
+            self.observations,
+            current_truth.x,
+            current_truth.y,
+            current_truth.theta.to_degrees(),
+            result.pose.x,
+            result.pose.y,
+            result.pose.theta.to_degrees(),
+            result.confidence
+        );
+    }
+
+    /// Compute relative odometry between last SLAM pose and current truth.
+    fn compute_relative_odometry(&self, current_truth: Pose2D) -> Pose2D {
+        // For simplicity, use a small delta pose based on expected motion
+        // This simulates imperfect odometry
+        let dx = current_truth.x - self.last_slam_pose.x;
+        let dy = current_truth.y - self.last_slam_pose.y;
+        let dtheta = normalize_angle(current_truth.theta - self.last_slam_pose.theta);
+
+        // Transform to robot frame
+        let cos_th = self.last_slam_pose.theta.cos();
+        let sin_th = self.last_slam_pose.theta.sin();
+        let dx_robot = dx * cos_th + dy * sin_th;
+        let dy_robot = -dx * sin_th + dy * cos_th;
+
+        Pose2D::new(dx_robot, dy_robot, dtheta)
+    }
+
+    /// Finalize the test and compute metrics.
+    fn finalize(&mut self) -> TestResult {
+        let final_pose = self.slam.current_pose();
+        let ground_truth_pose =
+            Pose2D::new(self.physics.x(), self.physics.y(), self.physics.theta());
+
+        // Compute metrics using the simulation map
+        let metrics = TestMetrics::compute(&self.slam, &self.map, final_pose, ground_truth_pose);
+
+        // Generate visualization if enabled
+        if let Some(ref viz) = self.visualizer {
+            viz.render(&self.slam, &self.map, final_pose, ground_truth_pose);
+        }
+
+        TestResult {
+            metrics,
+            final_pose,
+            ground_truth_pose,
+            observations: self.observations,
+            sim_time: self.sim_time,
+        }
+    }
+
+    /// Get current SLAM map reference.
+    pub fn slam(&self) -> &VectorMap {
+        &self.slam
+    }
+
+    /// Get current physics state.
+    pub fn physics(&self) -> &PhysicsState {
+        &self.physics
+    }
+}
+
+/// Normalize angle to [-PI, PI].
+fn normalize_angle(angle: f32) -> f32 {
+    let mut a = angle;
+    while a > std::f32::consts::PI {
+        a -= 2.0 * std::f32::consts::PI;
+    }
+    while a < -std::f32::consts::PI {
+        a += 2.0 * std::f32::consts::PI;
+    }
+    a
+}

vastu-map/tests/integration/metrics.rs

@@ -0,0 +1,237 @@
+//! Quantitative validation metrics
+//!
+//! Computes error metrics comparing SLAM output against ground truth:
+//! - Position and orientation error
+//! - Wall accuracy (how close detected lines are to actual walls in map)
+
+use sangam_io::devices::mock::map_loader::SimulationMap;
+use vastu_map::VectorMap;
+use vastu_map::core::Pose2D;
+use vastu_map::features::Line2D;
+
+/// Test metrics results.
+#[derive(Debug, Clone)]
+pub struct TestMetrics {
+    /// Position error in meters (Euclidean distance)
+    pub position_error: f32,
+    /// Orientation error in radians
+    pub orientation_error: f32,
+    /// Average distance from detected lines to actual walls in meters
+    pub wall_distance: f32,
+    /// Percentage of detected line points that are near actual walls (0.0-1.0)
+    pub wall_accuracy: f32,
+    /// Number of detected walls/features
+    pub detected_walls: usize,
+}
+
+impl TestMetrics {
+    /// Compute all metrics comparing SLAM output to ground truth map.
+    pub fn compute(
+        slam: &VectorMap,
+        map: &SimulationMap,
+        final_pose: Pose2D,
+        truth_pose: Pose2D,
+    ) -> Self {
+        // Position error
+        let position_error =
+            ((final_pose.x - truth_pose.x).powi(2) + (final_pose.y - truth_pose.y).powi(2)).sqrt();
+
+        // Orientation error (normalized to [-PI, PI])
+        let orientation_error = angle_diff(final_pose.theta, truth_pose.theta).abs();
+
+        // Get detected lines
+        let detected_lines = slam.lines();
+        let detected_walls = detected_lines.len();
+
+        // Compute wall accuracy using the simulation map
+        let (wall_distance, wall_accuracy) = compute_wall_accuracy(&detected_lines, map);
+
+        Self {
+            position_error,
+            orientation_error,
+            wall_distance,
+            wall_accuracy,
+            detected_walls,
+        }
+    }
+
+    /// Check if metrics pass acceptance criteria.
+    pub fn passes(&self, criteria: &AcceptanceCriteria) -> bool {
+        self.position_error <= criteria.max_position_error
+            && self.orientation_error <= criteria.max_orientation_error
+            && self.wall_accuracy >= criteria.min_wall_accuracy
+    }
+
+    /// Format metrics as a human-readable string.
+    pub fn summary(&self) -> String {
+        format!(
+            "Position error: {:.3}m, Orientation error: {:.1}°, \
+             Wall distance: {:.3}m, Wall accuracy: {:.0}%, Lines: {}",
+            self.position_error,
+            self.orientation_error.to_degrees(),
+            self.wall_distance,
+            self.wall_accuracy * 100.0,
+            self.detected_walls
+        )
+    }
+}
+
+/// Acceptance criteria for test pass/fail determination.
+#[derive(Clone, Debug)]
+pub struct AcceptanceCriteria {
+    /// Maximum position error in meters
+    pub max_position_error: f32,
+    /// Maximum orientation error in radians
+    pub max_orientation_error: f32,
+    /// Minimum wall accuracy (0.0-1.0)
+    pub min_wall_accuracy: f32,
+}
+
+impl Default for AcceptanceCriteria {
+    fn default() -> Self {
+        Self {
+            max_position_error: 0.1,      // 10cm
+            max_orientation_error: 0.087, // ~5 degrees
+            min_wall_accuracy: 0.7,       // 70% of line points near walls
+        }
+    }
+}
+
+impl AcceptanceCriteria {
+    /// Strict criteria for high-precision tests.
+    pub fn strict() -> Self {
+        Self {
+            max_position_error: 0.05,     // 5cm
+            max_orientation_error: 0.035, // ~2 degrees
+            min_wall_accuracy: 0.85,      // 85%
+        }
+    }
+
+    /// Lenient criteria for noisy/challenging tests.
+    pub fn lenient() -> Self {
+        Self {
+            max_position_error: 0.2,      // 20cm
+            max_orientation_error: 0.175, // ~10 degrees
+            min_wall_accuracy: 0.5,       // 50%
+        }
+    }
+}
+
+/// Compute angle difference normalized to [-PI, PI].
+fn angle_diff(a: f32, b: f32) -> f32 {
+    let diff = a - b;
+    if diff > std::f32::consts::PI {
+        diff - 2.0 * std::f32::consts::PI
+    } else if diff < -std::f32::consts::PI {
+        diff + 2.0 * std::f32::consts::PI
+    } else {
+        diff
+    }
+}
+
+/// Maximum distance (meters) from line point to wall to count as valid.
+const WALL_DISTANCE_THRESHOLD: f32 = 0.15; // 15cm
+
+/// Number of sample points along each line.
+const SAMPLES_PER_LINE: usize = 20;
+
+/// Compute wall accuracy by checking if detected lines lie on occupied pixels.
+///
+/// Returns (average_distance, accuracy) where:
+/// - average_distance: mean distance from line sample points to nearest wall
+/// - accuracy: fraction of sample points that are within threshold of a wall
+fn compute_wall_accuracy(detected: &[Line2D], map: &SimulationMap) -> (f32, f32) {
+    if detected.is_empty() {
+        return (0.0, 0.0);
+    }
+
+    let mut total_distance = 0.0;
+    let mut near_wall_count = 0;
+    let mut total_samples = 0;
+
+    let resolution = map.resolution();
+
+    for line in detected {
+        // Sample points along the line
+        for i in 0..SAMPLES_PER_LINE {
+            let t = i as f32 / (SAMPLES_PER_LINE - 1) as f32;
+            let x = line.start.x + t * (line.end.x - line.start.x);
+            let y = line.start.y + t * (line.end.y - line.start.y);
+
+            // Find distance to nearest occupied cell
+            let dist = distance_to_nearest_wall(x, y, map, resolution);
+            total_distance += dist;
+            total_samples += 1;
+
+            if dist <= WALL_DISTANCE_THRESHOLD {
+                near_wall_count += 1;
+            }
+        }
+    }
+
+    let avg_distance = total_distance / total_samples as f32;
+    let accuracy = near_wall_count as f32 / total_samples as f32;
+
+    (avg_distance, accuracy)
+}
+
+/// Find distance from point to nearest occupied cell in map.
+///
+/// Searches in a spiral pattern outward from the point.
+fn distance_to_nearest_wall(x: f32, y: f32, map: &SimulationMap, resolution: f32) -> f32 {
+    // Check if point itself is on a wall
+    if map.is_occupied(x, y) {
+        return 0.0;
+    }
+
+    // Search outward in a grid pattern
+    let max_search = (WALL_DISTANCE_THRESHOLD * 2.0 / resolution) as i32;
+
+    for radius in 1..=max_search {
+        for dx in -radius..=radius {
+            for dy in -radius..=radius {
+                // Only check cells at this radius (perimeter)
+                if dx.abs() != radius && dy.abs() != radius {
+                    continue;
+                }
+
+                let check_x = x + dx as f32 * resolution;
+                let check_y = y + dy as f32 * resolution;
+
+                if map.is_occupied(check_x, check_y) {
+                    let dist = ((check_x - x).powi(2) + (check_y - y).powi(2)).sqrt();
+                    return dist;
+                }
+            }
+        }
+    }
+
+    // No wall found within search radius
+    WALL_DISTANCE_THRESHOLD * 2.0
+}
+
+#[cfg(test)]
+mod tests {
+    use super::*;
+
+    #[test]
+    fn test_angle_diff() {
+        assert!((angle_diff(0.0, 0.0)).abs() < 0.001);
+        assert!((angle_diff(0.1, 0.0) - 0.1).abs() < 0.001);
+        assert!((angle_diff(std::f32::consts::PI, -std::f32::consts::PI)).abs() < 0.001);
+    }
+
+    #[test]
+    fn test_acceptance_criteria() {
+        let metrics = TestMetrics {
+            position_error: 0.05,
+            orientation_error: 0.05,
+            wall_distance: 0.05,
+            wall_accuracy: 0.9,
+            detected_walls: 4,
+        };
+
+        assert!(metrics.passes(&AcceptanceCriteria::default()));
+        assert!(metrics.passes(&AcceptanceCriteria::lenient()));
+    }
+}

vastu-map/tests/integration/mod.rs

@@ -0,0 +1,32 @@
+//! Integration tests for vastu-map using sangam-io mock device
+//!
+//! These tests verify the SLAM pipeline using simulated sensor data from
+//! sangam-io's mock device, which provides ray-cast lidar in simulated environments.
+//!
+//! # Running Tests
+//!
+//! ```bash
+//! # Run all integration tests
+//! cargo test --features integration-tests -- --nocapture
+//!
+//! # Run specific scenario
+//! cargo test --features integration-tests square_room
+//!
+//! # View generated visualizations
+//! open test_output/square_room.svg
+//! ```
+
+mod adapters;
+mod harness;
+mod metrics;
+mod odometry;
+mod path_generator;
+mod scenarios;
+mod visualization;
+mod yaml_config;
+mod yaml_runner;
+
+// Re-export for test convenience
+pub use harness::{HarnessConfig, TestHarness, TestResult};
+pub use metrics::{AcceptanceCriteria, TestMetrics};
+pub use path_generator::{PathGenerator, PathSegment};

vastu-map/tests/integration/odometry.rs

@@ -0,0 +1,240 @@
+//! Odometry simulation from encoder ticks
+//!
+//! Simulates odometry estimation using differential drive kinematics,
+//! with configurable drift and noise for testing SLAM robustness.
+
+use vastu_map::core::Pose2D;
+
+/// Odometry noise/drift configuration.
+#[derive(Clone, Debug)]
+pub struct OdometryConfig {
+    /// Systematic drift per meter traveled (e.g., 0.02 = 2% drift)
+    pub drift_per_meter: f32,
+    /// Angular drift per radian rotated
+    pub angular_drift_per_rad: f32,
+    /// Random position noise standard deviation (meters)
+    pub position_noise_stddev: f32,
+    /// Random angular noise standard deviation (radians)
+    pub angular_noise_stddev: f32,
+}
+
+impl Default for OdometryConfig {
+    fn default() -> Self {
+        Self {
+            drift_per_meter: 0.0,
+            angular_drift_per_rad: 0.0,
+            position_noise_stddev: 0.0,
+            angular_noise_stddev: 0.0,
+        }
+    }
+}
+
+impl OdometryConfig {
+    /// Create a clean odometry config with no drift or noise.
+    pub fn clean() -> Self {
+        Self::default()
+    }
+
+    /// Create a noisy odometry config for testing robustness.
+    pub fn noisy() -> Self {
+        Self {
+            drift_per_meter: 0.02,        // 2% systematic drift
+            angular_drift_per_rad: 0.01,  // 1% angular drift
+            position_noise_stddev: 0.005, // 5mm random noise
+            angular_noise_stddev: 0.002,  // ~0.1 degree noise
+        }
+    }
+}
+
+/// Odometry estimator that tracks robot pose from encoder ticks.
+///
+/// Uses differential drive kinematics to compute pose changes from
+/// left/right wheel encoder tick differences.
+pub struct OdometryEstimator {
+    config: OdometryConfig,
+    accumulated_pose: Pose2D,
+    last_left_ticks: u16,
+    last_right_ticks: u16,
+    wheel_base: f32,
+    ticks_per_meter: f32,
+    initialized: bool,
+}
+
+impl OdometryEstimator {
+    /// Create a new odometry estimator.
+    ///
+    /// # Arguments
+    /// * `wheel_base` - Distance between wheels in meters
+    /// * `ticks_per_meter` - Encoder ticks per meter of travel
+    /// * `config` - Drift/noise configuration
+    pub fn new(wheel_base: f32, ticks_per_meter: f32, config: OdometryConfig) -> Self {
+        Self {
+            config,
+            accumulated_pose: Pose2D::identity(),
+            last_left_ticks: 0,
+            last_right_ticks: 0,
+            wheel_base,
+            ticks_per_meter,
+            initialized: false,
+        }
+    }
+
+    /// Initialize with starting pose and encoder values.
+    pub fn initialize(&mut self, start_pose: Pose2D, left_ticks: u16, right_ticks: u16) {
+        self.accumulated_pose = start_pose;
+        self.last_left_ticks = left_ticks;
+        self.last_right_ticks = right_ticks;
+        self.initialized = true;
+    }
+
+    /// Update with new encoder ticks, returns the accumulated pose.
+    ///
+    /// Handles u16 wrapping correctly.
+    pub fn update(&mut self, left_ticks: u16, right_ticks: u16) -> Pose2D {
+        if !self.initialized {
+            self.last_left_ticks = left_ticks;
+            self.last_right_ticks = right_ticks;
+            self.initialized = true;
+            return self.accumulated_pose;
+        }
+
+        // Handle u16 wrapping
+        let delta_left = left_ticks.wrapping_sub(self.last_left_ticks) as i16;
+        let delta_right = right_ticks.wrapping_sub(self.last_right_ticks) as i16;
+
+        self.last_left_ticks = left_ticks;
+        self.last_right_ticks = right_ticks;
+
+        // Convert ticks to meters
+        let left_dist = delta_left as f32 / self.ticks_per_meter;
+        let right_dist = delta_right as f32 / self.ticks_per_meter;
+
+        // Apply drift
+        let left_dist = left_dist * (1.0 + self.config.drift_per_meter);
+        let right_dist = right_dist * (1.0 + self.config.drift_per_meter);
+
+        // Differential drive kinematics
+        let linear = (left_dist + right_dist) / 2.0;
+        let angular = (right_dist - left_dist) / self.wheel_base;
+
+        // Apply angular drift
+        let angular = angular * (1.0 + self.config.angular_drift_per_rad);
+
+        // Compute pose delta in current robot frame
+        let (dx, dy, dtheta) = if angular.abs() < 1e-6 {
+            // Straight line motion
+            (linear, 0.0, 0.0)
+        } else {
+            // Arc motion
+            let radius = linear / angular;
+            (
+                radius * angular.sin(),
+                radius * (1.0 - angular.cos()),
+                angular,
+            )
+        };
+
+        // Transform to world frame and accumulate
+        let cos_theta = self.accumulated_pose.theta.cos();
+        let sin_theta = self.accumulated_pose.theta.sin();
+
+        self.accumulated_pose.x += dx * cos_theta - dy * sin_theta;
+        self.accumulated_pose.y += dx * sin_theta + dy * cos_theta;
+        self.accumulated_pose.theta += dtheta;
+
+        // Normalize theta to [-PI, PI]
+        self.accumulated_pose.theta = normalize_angle(self.accumulated_pose.theta);
+
+        self.accumulated_pose
+    }
+
+    /// Get the relative pose change since the last call to `relative_update`.
+    ///
+    /// This is useful for feeding to VectorMap::observe() which expects
+    /// relative odometry.
+    pub fn relative_update(&mut self, left_ticks: u16, right_ticks: u16) -> Pose2D {
+        let prev_pose = self.accumulated_pose;
+        let new_pose = self.update(left_ticks, right_ticks);
+
+        // Compute relative transformation
+        let dx = new_pose.x - prev_pose.x;
+        let dy = new_pose.y - prev_pose.y;
+        let dtheta = normalize_angle(new_pose.theta - prev_pose.theta);
+
+        // Transform back to robot frame (inverse rotation)
+        let cos_theta = prev_pose.theta.cos();
+        let sin_theta = prev_pose.theta.sin();
+        let dx_robot = dx * cos_theta + dy * sin_theta;
+        let dy_robot = -dx * sin_theta + dy * cos_theta;
+
+        Pose2D::new(dx_robot, dy_robot, dtheta)
+    }
+
+    /// Get the current accumulated pose estimate.
+    pub fn pose(&self) -> Pose2D {
+        self.accumulated_pose
+    }
+
+    /// Reset the odometry to a given pose.
+    pub fn reset(&mut self, pose: Pose2D) {
+        self.accumulated_pose = pose;
+    }
+}
+
+/// Normalize angle to [-PI, PI] range.
+fn normalize_angle(angle: f32) -> f32 {
+    let mut a = angle;
+    while a > std::f32::consts::PI {
+        a -= 2.0 * std::f32::consts::PI;
+    }
+    while a < -std::f32::consts::PI {
+        a += 2.0 * std::f32::consts::PI;
+    }
+    a
+}
+
+#[cfg(test)]
+mod tests {
+    use super::*;
+    use std::f32::consts::PI;
+
+    #[test]
+    fn test_straight_motion() {
+        let mut odom = OdometryEstimator::new(0.233, 4464.0, OdometryConfig::clean());
+        odom.initialize(Pose2D::identity(), 0, 0);
+
+        // Move forward 1 meter (4464 ticks on each wheel)
+        let pose = odom.update(4464, 4464);
+
+        assert!((pose.x - 1.0).abs() < 0.01);
+        assert!(pose.y.abs() < 0.01);
+        assert!(pose.theta.abs() < 0.01);
+    }
+
+    #[test]
+    fn test_rotation() {
+        let mut odom = OdometryEstimator::new(0.233, 4464.0, OdometryConfig::clean());
+        odom.initialize(Pose2D::identity(), 0, 0);
+
+        // Rotate in place (opposite wheel motion)
+        // For 90 degree turn: arc_length = wheel_base/2 * PI/2
+        let arc_ticks = (0.233 / 2.0 * PI / 2.0 * 4464.0) as u16;
+        let pose = odom.update(65535 - arc_ticks + 1, arc_ticks); // Left backward, right forward
+
+        // Should have rotated approximately PI/2
+        assert!((pose.theta - PI / 2.0).abs() < 0.1);
+    }
+
+    #[test]
+    fn test_wrapping() {
+        let mut odom = OdometryEstimator::new(0.233, 4464.0, OdometryConfig::clean());
+        odom.initialize(Pose2D::identity(), 65530, 65530);
+
+        // Wrap around
+        let pose = odom.update(10, 10);
+
+        // Should have moved forward ~16 ticks worth
+        let expected_dist = 16.0 / 4464.0;
+        assert!((pose.x - expected_dist).abs() < 0.001);
+    }
+}

vastu-map/tests/integration/path_generator.rs

@@ -0,0 +1,236 @@
+//! Robot path generation utilities
+//!
+//! Provides predefined path patterns for integration tests:
+//! - Rectangle (wall following)
+//! - Spiral outward (coverage)
+//! - Random walk (exploration)
+//! - Loop closure (return to start)
+
+use rand::prelude::*;
+use std::f32::consts::PI;
+
+/// A path segment with velocity commands and duration.
+#[derive(Clone, Debug)]
+pub struct PathSegment {
+    /// Linear velocity in m/s (positive = forward)
+    pub linear_vel: f32,
+    /// Angular velocity in rad/s (positive = CCW)
+    pub angular_vel: f32,
+    /// Duration in seconds
+    pub duration: f32,
+}
+
+impl PathSegment {
+    /// Create a forward motion segment.
+    pub fn forward(speed: f32, duration: f32) -> Self {
+        Self {
+            linear_vel: speed,
+            angular_vel: 0.0,
+            duration,
+        }
+    }
+
+    /// Create a left turn segment (CCW).
+    pub fn turn_left(angular_speed: f32, angle: f32) -> Self {
+        Self {
+            linear_vel: 0.0,
+            angular_vel: angular_speed,
+            duration: angle / angular_speed,
+        }
+    }
+
+    /// Create a right turn segment (CW).
+    pub fn turn_right(angular_speed: f32, angle: f32) -> Self {
+        Self {
+            linear_vel: 0.0,
+            angular_vel: -angular_speed,
+            duration: angle / angular_speed,
+        }
+    }
+
+    /// Create an arc motion segment.
+    pub fn arc(linear: f32, angular: f32, duration: f32) -> Self {
+        Self {
+            linear_vel: linear,
+            angular_vel: angular,
+            duration,
+        }
+    }
+
+    /// Create a stop segment (no motion).
+    pub fn stop(duration: f32) -> Self {
+        Self {
+            linear_vel: 0.0,
+            angular_vel: 0.0,
+            duration,
+        }
+    }
+}
+
+/// Path generator utilities.
+pub struct PathGenerator;
+
+impl PathGenerator {
+    /// Generate a spiral outward coverage pattern.
+    ///
+    /// # Arguments
+    /// * `initial_radius` - Starting radius in meters
+    /// * `expansion_rate` - Radius increase per loop in meters
+    /// * `num_loops` - Number of complete loops
+    /// * `linear_speed` - Linear velocity in m/s
+    pub fn spiral_outward(
+        initial_radius: f32,
+        expansion_rate: f32,
+        num_loops: usize,
+        linear_speed: f32,
+    ) -> Vec<PathSegment> {
+        let mut segments = Vec::new();
+        let mut radius = initial_radius;
+
+        for _ in 0..num_loops {
+            // Arc for one loop
+            let circumference = 2.0 * PI * radius;
+            let duration = circumference / linear_speed;
+            let angular_vel = linear_speed / radius;
+
+            segments.push(PathSegment::arc(linear_speed, angular_vel, duration));
+
+            // Expand radius
+            radius += expansion_rate;
+        }
+
+        segments
+    }
+
+    /// Generate a rectangle pattern (wall following).
+    ///
+    /// # Arguments
+    /// * `width` - Rectangle width in meters
+    /// * `height` - Rectangle height in meters
+    /// * `linear_speed` - Linear velocity in m/s
+    /// * `turn_speed` - Angular velocity during turns in rad/s
+    pub fn rectangle(
+        width: f32,
+        height: f32,
+        linear_speed: f32,
+        turn_speed: f32,
+    ) -> Vec<PathSegment> {
+        let mut segments = Vec::new();
+
+        for i in 0..4 {
+            let length = if i % 2 == 0 { width } else { height };
+
+            // Forward segment
+            segments.push(PathSegment::forward(linear_speed, length / linear_speed));
+
+            // Turn 90 degrees left
+            segments.push(PathSegment::turn_left(turn_speed, PI / 2.0));
+        }
+
+        segments
+    }
+
+    /// Generate a random exploration pattern.
+    ///
+    /// # Arguments
+    /// * `num_segments` - Number of forward-turn pairs
+    /// * `linear_speed` - Linear velocity in m/s
+    /// * `segment_length` - Base forward distance in meters
+    /// * `turn_speed` - Angular velocity during turns in rad/s
+    /// * `seed` - Random seed for reproducibility
+    pub fn random_walk(
+        num_segments: usize,
+        linear_speed: f32,
+        segment_length: f32,
+        turn_speed: f32,
+        seed: u64,
+    ) -> Vec<PathSegment> {
+        let mut rng = StdRng::seed_from_u64(seed);
+        let mut segments = Vec::new();
+
+        for _ in 0..num_segments {
+            // Random forward distance (0.5x to 1.5x base length)
+            let distance = segment_length * (0.5 + rng.random::<f32>());
+            segments.push(PathSegment::forward(linear_speed, distance / linear_speed));
+
+            // Random turn (-90 to +90 degrees)
+            let angle = (rng.random::<f32>() - 0.5) * PI;
+            if angle > 0.0 {
+                segments.push(PathSegment::turn_left(turn_speed, angle));
+            } else {
+                segments.push(PathSegment::turn_right(turn_speed, -angle));
+            }
+        }
+
+        segments
+    }
+
+    /// Generate a loop closure path (exploration + return to start).
+    ///
+    /// Reverses the given exploration path to return to the starting position.
+    pub fn loop_closure(exploration_path: Vec<PathSegment>) -> Vec<PathSegment> {
+        let mut full_path = exploration_path.clone();
+
+        // Reverse the path to return to start
+        for segment in exploration_path.into_iter().rev() {
+            full_path.push(PathSegment {
+                linear_vel: -segment.linear_vel,
+                angular_vel: -segment.angular_vel,
+                duration: segment.duration,
+            });
+        }
+
+        full_path
+    }
+
+    /// Generate a full rotation path for observing surrounding walls.
+    ///
+    /// # Arguments
+    /// * `turn_speed` - Angular velocity in rad/s
+    /// * `pause_duration` - Pause at start and end for observations
+    pub fn full_rotation(turn_speed: f32, pause_duration: f32) -> Vec<PathSegment> {
+        vec![
+            PathSegment::stop(pause_duration),
+            PathSegment::turn_left(turn_speed, 2.0 * PI),
+            PathSegment::stop(pause_duration),
+        ]
+    }
+}
+
+#[cfg(test)]
+mod tests {
+    use super::*;
+
+    #[test]
+    fn test_rectangle_path() {
+        let path = PathGenerator::rectangle(2.0, 1.0, 0.5, 1.0);
+        assert_eq!(path.len(), 8); // 4 forward + 4 turn segments
+    }
+
+    #[test]
+    fn test_spiral_path() {
+        let path = PathGenerator::spiral_outward(0.5, 0.2, 3, 0.2);
+        assert_eq!(path.len(), 3); // 3 loops
+    }
+
+    #[test]
+    fn test_random_walk_deterministic() {
+        let path1 = PathGenerator::random_walk(5, 0.2, 0.5, 0.5, 42);
+        let path2 = PathGenerator::random_walk(5, 0.2, 0.5, 0.5, 42);
+        assert_eq!(path1.len(), path2.len());
+        // Same seed should produce identical paths
+        for (s1, s2) in path1.iter().zip(path2.iter()) {
+            assert_eq!(s1.duration, s2.duration);
+        }
+    }
+
+    #[test]
+    fn test_loop_closure() {
+        let exploration = vec![
+            PathSegment::forward(0.2, 1.0),
+            PathSegment::turn_left(0.5, PI / 2.0),
+        ];
+        let full = PathGenerator::loop_closure(exploration.clone());
+        assert_eq!(full.len(), exploration.len() * 2);
+    }
+}

vastu-map/tests/integration/scenarios/maps/medium_room.pgm

@@ -0,0 +1,165 @@
+P2
+# Medium room with obstacles for SLAM testing (8m x 8m)
+# Obstacles: table, pillar, L-shape, small box
+160 160
+255
+0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
+0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0 0 0 0 0 0 0 0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0 0 0 0 0 0 0 0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0 0 0 0 0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0 0 0 0 0 0 0 0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0 0 0 0 0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0 0 0 0 0 0 0 0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0 0 0 0 0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0 0 0 0 0 0 0 0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0 0 0 0 0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0 0 0 0 0 0 0 0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0 0 0 0 0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0 0 0 0 0 0 0 0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0 0 0 0 0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0 0 0 0 0 0 0 0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0 0 0 0 0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0 0 0 0 0 0 0 0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0 0 0 0 0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0 0 0 0 0 0 0 0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0 0 0 0 0 0 0 0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0 0 0 0 0 0 0 0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0 0 0 0 0 0 0 0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0 0 0 0 0 0 0 0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0 0 0 0 0 0 0 0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0 0 0 0 0 0 0 0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0 0 0 0 0 0 0 0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0 0 0 0 0 0 0 0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0 0 0 0 0 0 0 0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0 0 0 0 0 0 0 0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0 0 0 0 0 0 0 0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0 0 0 0 0 0 0 0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0 0 0 0 0 0 0 0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0 0 0 0 0 0 0 0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0 0 0 0 0 0 0 0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0 0 0 0 0 0 0 0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0 0 0 0 0 0 0 0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0 0 0 0 0 0 0 0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0 0 0 0 0 0 0 0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0 0 0 0 0 0 0 0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
+0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0

vastu-map/tests/integration/scenarios/maps/medium_room.yaml

@@ -0,0 +1,8 @@
+# Medium room with obstacles for SLAM testing (8m x 8m)
+# Obstacles: table (2,2), pillar (6,2), L-shape (5,5), small box (2,6)
+image: medium_room.pgm
+resolution: 0.05
+origin: [0.0, 0.0, 0.0]
+negate: 0
+occupied_thresh: 0.65
+free_thresh: 0.196

vastu-map/tests/integration/scenarios/medium_room_exploration.yaml

@@ -0,0 +1,42 @@
+# Medium Room Exploration Test
+#
+# Starting from center (4m, 4m), explore the room with rotation
+# and forward motion to discover all walls and obstacles.
+#
+# Map: 8m x 8m room with 4 obstacles
+# Wheel base: 0.233m
+
+name: "medium_room_exploration"
+map: "maps/medium_room.yaml"
+
+start_pose:
+  x: 4.0
+  y: 4.0
+  theta: 0.0
+
+path:
+  # Initial 360° rotation to scan surroundings
+  # 360° = 2π * 0.233 / 2 = 0.732m per wheel
+  - {left: -0.732, right: 0.732}
+
+  # Move forward 1m
+  - {left: 1.0, right: 1.0}
+
+  # Turn right 90° and move
+  # 90° = π/2 * 0.233 / 2 = 0.183m
+  - {left: 0.183, right: -0.183}
+  - {left: 1.5, right: 1.5}
+
+  # Turn right 90° and move back
+  - {left: 0.183, right: -0.183}
+  - {left: 1.0, right: 1.0}
+
+  # Another rotation to scan
+  - {left: -0.732, right: 0.732}
+
+  # Explore other direction
+  - {left: 0.183, right: -0.183}
+  - {left: 2.0, right: 2.0}
+
+  # Final rotation
+  - {left: -0.732, right: 0.732}

vastu-map/tests/integration/scenarios/medium_room_obstacles.yaml

@@ -0,0 +1,52 @@
+# Medium Room Obstacle Navigation Test
+#
+# Navigate around obstacles to test obstacle detection.
+# Visit areas near each obstacle.
+#
+# Obstacles:
+# - Table at (2m, 2m)
+# - Pillar at (6m, 2m)
+# - L-shape at (5m, 5m)
+# - Small box at (2m, 6m)
+
+name: "medium_room_obstacles"
+map: "maps/medium_room.yaml"
+
+start_pose:
+  x: 4.0
+  y: 4.0
+  theta: 0.0
+
+path:
+  # Scan from center
+  - {left: -0.732, right: 0.732}
+
+  # Move toward table area
+  - {left: -0.366, right: 0.366}  # Turn 180°
+  - {left: 1.5, right: 1.5}       # Move toward (2.5, 4)
+  - {left: -0.183, right: 0.183}  # Turn left 90°
+  - {left: 1.5, right: 1.5}       # Move toward table
+
+  # Scan near table
+  - {left: -0.732, right: 0.732}
+
+  # Navigate to pillar area
+  - {left: 0.183, right: -0.183}  # Turn right 90°
+  - {left: 4.0, right: 4.0}       # Move across to pillar side
+
+  # Scan near pillar
+  - {left: -0.732, right: 0.732}
+
+  # Move up toward L-shape
+  - {left: -0.183, right: 0.183}  # Turn left 90°
+  - {left: 3.0, right: 3.0}       # Move up
+
+  # Scan near L-shape
+  - {left: -0.732, right: 0.732}
+
+  # Navigate to small box area
+  - {left: 0.366, right: -0.366}  # Turn right 180°
+  - {left: 3.0, right: 3.0}       # Move toward small box
+
+  # Final scan
+  - {left: -0.732, right: 0.732}

vastu-map/tests/integration/scenarios/medium_room_perimeter.yaml

@@ -0,0 +1,46 @@
+# Medium Room Perimeter Test
+#
+# Starting near bottom-left, trace along the perimeter of the room
+# to detect all outer walls.
+#
+# Map: 8m x 8m room
+# Path: Rectangle around the room (keeping 1m from walls)
+
+name: "medium_room_perimeter"
+map: "maps/medium_room.yaml"
+
+start_pose:
+  x: 1.0
+  y: 1.0
+  theta: 0.0  # Facing right (+X)
+
+path:
+  # Initial scan
+  - {left: -0.732, right: 0.732}
+
+  # Side 1: Move right along bottom (6m)
+  - {left: 6.0, right: 6.0}
+
+  # Turn left 90° (CCW)
+  - {left: -0.183, right: 0.183}
+
+  # Side 2: Move up along right side (6m)
+  - {left: 6.0, right: 6.0}
+
+  # Turn left 90°
+  - {left: -0.183, right: 0.183}
+
+  # Side 3: Move left along top (6m)
+  - {left: 6.0, right: 6.0}
+
+  # Turn left 90°
+  - {left: -0.183, right: 0.183}
+
+  # Side 4: Move down along left side (6m)
+  - {left: 6.0, right: 6.0}
+
+  # Turn left 90° (back to original heading)
+  - {left: -0.183, right: 0.183}
+
+  # Final scan
+  - {left: -0.732, right: 0.732}

vastu-map/tests/integration/scenarios/mod.rs

@@ -0,0 +1,14 @@
+//! YAML-based test scenarios for integration tests
+//!
+//! Each scenario is defined in a YAML file with:
+//! - Map reference (PGM + YAML)
+//! - Start pose
+//! - Path as wheel distance commands
+//!
+//! # Running Tests
+//!
+//! ```bash
+//! cargo test --features integration-tests -- --nocapture
+//! ```
+
+mod yaml_tests;

vastu-map/tests/integration/scenarios/yaml_tests.rs

@@ -0,0 +1,88 @@
+//! YAML-based scenario tests
+//!
+//! Runs all test scenarios defined in YAML files in the scenarios directory.
+//!
+//! # Usage
+//!
+//! ```bash
+//! cargo test --features integration-tests -- --nocapture
+//! ```
+//!
+//! # Adding New Scenarios
+//!
+//! Create a new `.yaml` file in `tests/integration/scenarios/` with:
+//!
+//! ```yaml
+//! name: "my_scenario"
+//! map: "maps/medium_room.yaml"
+//! start_pose:
+//!   x: 4.0
+//!   y: 4.0
+//!   theta: 0.0
+//! path:
+//!   - {left: 0.5, right: 0.5}   # forward 0.5m
+//!   - {left: -0.183, right: 0.183}  # turn left 90°
+//! ```
+
+use crate::yaml_runner;
+
+#[test]
+fn test_yaml_scenarios() {
+    env_logger::try_init().ok();
+
+    let scenarios_dir = concat!(env!("CARGO_MANIFEST_DIR"), "/tests/integration/scenarios");
+
+    println!("\n========================================");
+    println!("  YAML Integration Test Scenarios");
+    println!("========================================");
+
+    let results = yaml_runner::run_all_scenarios(scenarios_dir);
+
+    if results.is_empty() {
+        println!("\nNo YAML scenarios found in: {}", scenarios_dir);
+        println!("Create .yaml files in tests/integration/scenarios/");
+        return;
+    }
+
+    println!("\n========================================");
+    println!("  Results Summary");
+    println!("========================================\n");
+
+    let mut passed = 0;
+    let mut failed = 0;
+    let mut failures = Vec::new();
+
+    for (name, result) in &results {
+        match result {
+            Ok(r) => {
+                passed += 1;
+                println!("PASS: {}", name);
+                println!("      Position error: {:.3}m", r.metrics.position_error);
+                println!(
+                    "      Wall accuracy:  {:.0}%",
+                    r.metrics.wall_accuracy * 100.0
+                );
+                println!("      Lines detected: {}", r.metrics.detected_walls);
+                println!(
+                    "      Final pose:     ({:.2}, {:.2}, {:.1}°)",
+                    r.final_pose.x,
+                    r.final_pose.y,
+                    r.final_pose.theta.to_degrees()
+                );
+                println!();
+            }
+            Err(e) => {
+                failed += 1;
+                println!("FAIL: {}", name);
+                println!("      Error: {}\n", e);
+                failures.push(name.clone());
+            }
+        }
+    }
+
+    println!("========================================");
+    println!("  Total: {} passed, {} failed", passed, failed);
+    println!("========================================\n");
+
+    assert!(failures.is_empty(), "Failed scenarios: {:?}", failures);
+}

vastu-map/tests/integration/visualization.rs

@@ -0,0 +1,260 @@
+//! SVG visualization for manual inspection
+//!
+//! Generates SVG files showing:
+//! - Map walls from PGM (gray background)
+//! - Detected walls (blue solid lines)
+//! - Ground truth pose (green marker)
+//! - Estimated pose (red marker)
+
+use sangam_io::devices::mock::map_loader::SimulationMap;
+use svg::Document;
+use svg::node::element::{Circle, Group, Line, Rectangle, Text};
+
+use vastu_map::VectorMap;
+use vastu_map::core::{Point2D, Pose2D};
+
+/// Visualization generator.
+pub struct Visualizer {
+    output_path: String,
+    scale: f32,
+    margin: f32,
+}
+
+impl Visualizer {
+    /// Create a new visualizer.
+    ///
+    /// # Arguments
+    /// * `output_path` - Path for the output SVG file
+    pub fn new(output_path: &str) -> Self {
+        Self {
+            output_path: output_path.to_string(),
+            scale: 100.0, // 100 pixels per meter
+            margin: 50.0,
+        }
+    }
+
+    /// Render the SLAM result to an SVG file.
+    pub fn render(
+        &self,
+        slam: &VectorMap,
+        map: &SimulationMap,
+        final_pose: Pose2D,
+        truth_pose: Pose2D,
+    ) {
+        // Compute bounds from map
+        let map_width_m = map.width() as f32 * map.resolution();
+        let map_height_m = map.height() as f32 * map.resolution();
+
+        let width = (map_width_m * self.scale + 2.0 * self.margin) as i32;
+        let height = (map_height_m * self.scale + 2.0 * self.margin) as i32;
+
+        // Create document
+        let mut doc = Document::new()
+            .set("width", width)
+            .set("height", height)
+            .set("viewBox", (0, 0, width, height));
+
+        // Background
+        doc = doc.add(
+            Rectangle::new()
+                .set("x", 0)
+                .set("y", 0)
+                .set("width", width)
+                .set("height", height)
+                .set("fill", "white"),
+        );
+
+        // Map walls (gray)
+        let map_group = self.render_map_walls(map, height as f32);
+        doc = doc.add(map_group);
+
+        // Detected walls (blue solid)
+        let detected_group = self.render_detected(slam, height as f32);
+        doc = doc.add(detected_group);
+
+        // Poses
+        let pose_group = self.render_poses(final_pose, truth_pose, height as f32);
+        doc = doc.add(pose_group);
+
+        // Legend
+        let legend = self.render_legend();
+        doc = doc.add(legend);
+
+        // Ensure output directory exists
+        if let Some(parent) = std::path::Path::new(&self.output_path).parent() {
+            std::fs::create_dir_all(parent).ok();
+        }
+
+        // Save
+        svg::save(&self.output_path, &doc).expect("Failed to save SVG");
+    }
+
+    /// Render map walls by sampling the occupancy grid.
+    fn render_map_walls(&self, map: &SimulationMap, svg_height: f32) -> Group {
+        let mut group = Group::new().set("id", "map_walls");
+
+        let resolution = map.resolution();
+        let sample_step = resolution * 2.0; // Sample every 2 pixels for efficiency
+
+        let map_width_m = map.width() as f32 * resolution;
+        let map_height_m = map.height() as f32 * resolution;
+
+        // Sample the map and draw small rectangles for occupied cells
+        let mut x = 0.0;
+        while x < map_width_m {
+            let mut y = 0.0;
+            while y < map_height_m {
+                if map.is_occupied(x, y) {
+                    let (sx, sy) = self.transform_point(Point2D::new(x, y), 0.0, 0.0, svg_height);
+                    let rect_size = sample_step * self.scale;
+
+                    let rect = Rectangle::new()
+                        .set("x", sx - rect_size / 2.0)
+                        .set("y", sy - rect_size / 2.0)
+                        .set("width", rect_size)
+                        .set("height", rect_size)
+                        .set("fill", "#CCCCCC")
+                        .set("stroke", "none");
+
+                    group = group.add(rect);
+                }
+                y += sample_step;
+            }
+            x += sample_step;
+        }
+
+        group
+    }
+
+    fn render_detected(&self, slam: &VectorMap, svg_height: f32) -> Group {
+        let mut group = Group::new().set("id", "detected");
+
+        for line in slam.lines() {
+            let svg_line = self.create_line(
+                line.start, line.end, 0.0, 0.0, svg_height, "blue", 2.0, None,
+            );
+            group = group.add(svg_line);
+        }
+
+        group
+    }
+
+    fn render_poses(&self, estimated: Pose2D, truth: Pose2D, svg_height: f32) -> Group {
+        let mut group = Group::new().set("id", "poses");
+
+        // Ground truth pose (green)
+        group = group.add(self.create_robot_marker(truth, 0.0, 0.0, svg_height, "green"));
+
+        // Estimated pose (red)
+        group = group.add(self.create_robot_marker(estimated, 0.0, 0.0, svg_height, "red"));
+
+        group
+    }
+
+    fn render_legend(&self) -> Group {
+        Group::new().set("id", "legend").add(
+            Text::new("Blue=Detected, Gray=MapWalls, Green=TruePose, Red=EstimatedPose")
+                .set("x", 10)
+                .set("y", 20)
+                .set("font-size", 12)
+                .set("font-family", "monospace")
+                .set("fill", "black"),
+        )
+    }
+
+    fn transform_point(
+        &self,
+        p: Point2D,
+        origin_x: f32,
+        origin_y: f32,
+        svg_height: f32,
+    ) -> (f32, f32) {
+        // Transform world coordinates to SVG coordinates
+        // SVG has Y increasing downward, world has Y increasing upward
+        let x = (p.x - origin_x) * self.scale + self.margin;
+        let y = svg_height - ((p.y - origin_y) * self.scale + self.margin);
+        (x, y)
+    }
+
+    fn create_line(
+        &self,
+        start: Point2D,
+        end: Point2D,
+        origin_x: f32,
+        origin_y: f32,
+        svg_height: f32,
+        color: &str,
+        width: f32,
+        dash: Option<&str>,
+    ) -> Line {
+        let (x1, y1) = self.transform_point(start, origin_x, origin_y, svg_height);
+        let (x2, y2) = self.transform_point(end, origin_x, origin_y, svg_height);
+
+        let mut line = Line::new()
+            .set("x1", x1)
+            .set("y1", y1)
+            .set("x2", x2)
+            .set("y2", y2)
+            .set("stroke", color)
+            .set("stroke-width", width);
+
+        if let Some(pattern) = dash {
+            line = line.set("stroke-dasharray", pattern);
+        }
+
+        line
+    }
+
+    fn create_robot_marker(
+        &self,
+        pose: Pose2D,
+        origin_x: f32,
+        origin_y: f32,
+        svg_height: f32,
+        color: &str,
+    ) -> Group {
+        let (cx, cy) = self.transform_point(pose.position(), origin_x, origin_y, svg_height);
+
+        // Robot body (circle)
+        let radius = 10.0;
+        let body = Circle::new()
+            .set("cx", cx)
+            .set("cy", cy)
+            .set("r", radius)
+            .set("fill", color)
+            .set("opacity", 0.7);
+
+        // Direction indicator (line)
+        let dir_length = 15.0;
+        // Note: In SVG, Y increases downward, so we negate the sin component
+        let dir_x = cx + dir_length * pose.theta.cos();
+        let dir_y = cy - dir_length * pose.theta.sin();
+
+        let direction = Line::new()
+            .set("x1", cx)
+            .set("y1", cy)
+            .set("x2", dir_x)
+            .set("y2", dir_y)
+            .set("stroke", color)
+            .set("stroke-width", 3);
+
+        Group::new().add(body).add(direction)
+    }
+}
+
+#[cfg(test)]
+mod tests {
+    use super::*;
+
+    #[test]
+    fn test_transform_point() {
+        let viz = Visualizer::new("test.svg");
+        let (x, y) = viz.transform_point(Point2D::new(0.0, 0.0), 0.0, 0.0, 600.0);
+
+        // At origin (0,0) with svg_height=600, margin=50, scale=100
+        // x = 0 * 100 + 50 = 50
+        // y = 600 - (0 * 100 + 50) = 550
+        assert!((x - 50.0).abs() < 0.001);
+        assert!((y - 550.0).abs() < 0.001);
+    }
+}

vastu-map/tests/integration/yaml_config.rs

@@ -0,0 +1,106 @@
+//! YAML configuration structures for scenario-based tests
+//!
+//! Allows defining test scenarios in YAML files with wheel distance commands
+//! for precise, encoder-based robot movement.
+
+use serde::Deserialize;
+
+/// Top-level scenario configuration loaded from YAML.
+#[derive(Debug, Deserialize)]
+pub struct ScenarioConfig {
+    /// Scenario name (used for output files)
+    pub name: String,
+    /// Path to map YAML file (relative to scenario file)
+    pub map: String,
+    /// Starting pose
+    #[serde(default)]
+    pub start_pose: PoseConfig,
+    /// Path as wheel distance commands
+    pub path: Vec<WheelCommand>,
+}
+
+/// Robot pose configuration.
+#[derive(Debug, Deserialize)]
+pub struct PoseConfig {
+    /// X position in meters
+    #[serde(default = "default_2_5")]
+    pub x: f32,
+    /// Y position in meters
+    #[serde(default = "default_2_5")]
+    pub y: f32,
+    /// Orientation in radians
+    #[serde(default)]
+    pub theta: f32,
+}
+
+impl Default for PoseConfig {
+    fn default() -> Self {
+        Self {
+            x: 2.5,
+            y: 2.5,
+            theta: 0.0,
+        }
+    }
+}
+
+/// Wheel distance command - precise encoder-based movement.
+///
+/// For differential drive robot (wheel_base = 0.233m):
+/// - Forward/Back: left = right = distance
+/// - Rotate in place: left = -right = θ * wheel_base / 2 (θ in radians)
+///   - 90° = π/2 * 0.233 / 2 = 0.183m per wheel
+///   - 180° = π * 0.233 / 2 = 0.366m per wheel
+///   - 360° = 2π * 0.233 / 2 = 0.732m per wheel
+/// - Arc: Different distances create curves
+#[derive(Debug, Deserialize)]
+pub struct WheelCommand {
+    /// Left wheel distance in meters (positive = forward)
+    pub left: f32,
+    /// Right wheel distance in meters (positive = forward)
+    pub right: f32,
+}
+
+fn default_2_5() -> f32 {
+    2.5
+}
+
+#[cfg(test)]
+mod tests {
+    use super::*;
+
+    #[test]
+    fn test_deserialize_scenario() {
+        let yaml = r#"
+name: "test_scenario"
+map: "../maps/simple_room.yaml"
+start_pose:
+  x: 2.5
+  y: 2.5
+  theta: 0.0
+path:
+  - {left: 0.5, right: 0.5}
+  - {left: -0.183, right: 0.183}
+"#;
+
+        let config: ScenarioConfig = serde_yaml::from_str(yaml).expect("Failed to parse YAML");
+        assert_eq!(config.name, "test_scenario");
+        assert_eq!(config.path.len(), 2);
+        assert_eq!(config.path[0].left, 0.5);
+        assert_eq!(config.path[0].right, 0.5);
+    }
+
+    #[test]
+    fn test_default_pose() {
+        let yaml = r#"
+name: "minimal"
+map: "test.yaml"
+path:
+  - {left: 1.0, right: 1.0}
+"#;
+
+        let config: ScenarioConfig = serde_yaml::from_str(yaml).expect("Failed to parse YAML");
+        assert_eq!(config.start_pose.x, 2.5);
+        assert_eq!(config.start_pose.y, 2.5);
+        assert_eq!(config.start_pose.theta, 0.0);
+    }
+}

vastu-map/tests/integration/yaml_runner.rs

@@ -0,0 +1,213 @@
+//! YAML scenario runner
+//!
+//! Loads and executes test scenarios defined in YAML files.
+
+use crate::harness::{HarnessConfig, TestHarness, TestResult};
+use crate::path_generator::PathSegment;
+use crate::yaml_config::{ScenarioConfig, WheelCommand};
+use std::path::Path;
+
+/// Wheel base of the robot in meters (from RobotConfig default).
+const WHEEL_BASE: f32 = 0.233;
+
+/// Default wheel speed for simulation in m/s.
+const DEFAULT_SPEED: f32 = 0.2;
+
+/// Load a scenario configuration from a YAML file.
+pub fn load_scenario<P: AsRef<Path>>(
+    path: P,
+) -> Result<ScenarioConfig, Box<dyn std::error::Error>> {
+    let content = std::fs::read_to_string(path)?;
+    Ok(serde_yaml::from_str(&content)?)
+}
+
+/// Run a scenario from a YAML file.
+///
+/// Returns the test result or an error if the scenario failed to load/run.
+pub fn run_scenario<P: AsRef<Path>>(
+    yaml_path: P,
+) -> Result<TestResult, Box<dyn std::error::Error>> {
+    let yaml_path = yaml_path.as_ref();
+    let config = load_scenario(yaml_path)?;
+
+    // Resolve map path relative to YAML file
+    let yaml_dir = yaml_path.parent().unwrap_or(Path::new("."));
+    let map_file = yaml_dir
+        .join(&config.map)
+        .canonicalize()
+        .map(|p| p.to_string_lossy().to_string())
+        .unwrap_or_else(|_| yaml_dir.join(&config.map).to_string_lossy().to_string());
+
+    let harness_config = HarnessConfig {
+        map_file,
+        start_x: config.start_pose.x,
+        start_y: config.start_pose.y,
+        start_theta: config.start_pose.theta,
+        ..Default::default()
+    };
+
+    let path = convert_wheel_commands(&config.path);
+
+    // Ensure output directory exists
+    std::fs::create_dir_all("results").ok();
+
+    let svg_path = format!("results/{}.svg", config.name);
+    let mut harness = TestHarness::new(harness_config)?.with_visualization(&svg_path);
+
+    Ok(harness.run_path(&path))
+}
+
+/// Convert wheel distance commands to PathSegments.
+///
+/// Uses differential drive kinematics:
+/// - linear_dist = (left + right) / 2
+/// - angular_dist = (right - left) / wheel_base
+fn convert_wheel_commands(commands: &[WheelCommand]) -> Vec<PathSegment> {
+    commands
+        .iter()
+        .filter_map(|cmd| {
+            // Calculate linear and angular distances
+            let linear_dist = (cmd.left + cmd.right) / 2.0;
+            let angular_dist = (cmd.right - cmd.left) / WHEEL_BASE; // radians
+
+            // Calculate duration based on max wheel distance
+            let max_wheel_dist = cmd.left.abs().max(cmd.right.abs());
+            if max_wheel_dist < 0.001 {
+                return None; // Skip zero-distance commands
+            }
+
+            let duration = max_wheel_dist / DEFAULT_SPEED;
+
+            // Calculate velocities
+            let linear_vel = linear_dist / duration;
+            let angular_vel = angular_dist / duration;
+
+            Some(PathSegment {
+                linear_vel,
+                angular_vel,
+                duration,
+            })
+        })
+        .collect()
+}
+
+/// Run all YAML scenarios in a directory.
+///
+/// Returns a vector of (scenario_name, result) tuples.
+pub fn run_all_scenarios<P: AsRef<Path>>(dir: P) -> Vec<(String, Result<TestResult, String>)> {
+    let mut results = Vec::new();
+
+    let entries = match std::fs::read_dir(&dir) {
+        Ok(entries) => entries,
+        Err(e) => {
+            log::warn!("Failed to read scenario directory: {}", e);
+            return results;
+        }
+    };
+
+    // Collect and sort entries for consistent ordering
+    let mut yaml_files: Vec<_> = entries
+        .filter_map(|e| e.ok())
+        .map(|e| e.path())
+        .filter(|p| p.extension().map_or(false, |e| e == "yaml" || e == "yml"))
+        .filter(|p| !p.to_string_lossy().contains("/maps/")) // Skip map yaml files
+        .collect();
+
+    yaml_files.sort();
+
+    for path in yaml_files {
+        let name = path
+            .file_stem()
+            .unwrap_or_default()
+            .to_string_lossy()
+            .to_string();
+
+        println!("\n--- Running scenario: {} ---", name);
+        let result = run_scenario(&path).map_err(|e| e.to_string());
+
+        match &result {
+            Ok(r) => println!("  {}", r.metrics.summary()),
+            Err(e) => println!("  FAILED: {}", e),
+        }
+
+        results.push((name, result));
+    }
+
+    results
+}
+
+#[cfg(test)]
+mod tests {
+    use super::*;
+
+    #[test]
+    fn test_wheel_command_forward() {
+        let commands = vec![WheelCommand {
+            left: 1.0,
+            right: 1.0,
+        }];
+        let segments = convert_wheel_commands(&commands);
+
+        assert_eq!(segments.len(), 1);
+        let seg = &segments[0];
+
+        // Forward: linear = 1.0, angular = 0
+        assert!((seg.linear_vel - DEFAULT_SPEED).abs() < 0.01);
+        assert!(seg.angular_vel.abs() < 0.01);
+        assert!((seg.duration - 5.0).abs() < 0.01); // 1.0m / 0.2m/s = 5s
+    }
+
+    #[test]
+    fn test_wheel_command_rotate() {
+        // 90 degree turn: left = -0.183, right = 0.183
+        let turn_dist = std::f32::consts::FRAC_PI_2 * WHEEL_BASE / 2.0;
+        let commands = vec![WheelCommand {
+            left: -turn_dist,
+            right: turn_dist,
+        }];
+        let segments = convert_wheel_commands(&commands);
+
+        assert_eq!(segments.len(), 1);
+        let seg = &segments[0];
+
+        // Rotation in place: linear = 0
+        assert!(seg.linear_vel.abs() < 0.01);
+        // Angular should be positive (CCW)
+        assert!(seg.angular_vel > 0.0);
+    }
+
+    #[test]
+    fn test_wheel_command_arc() {
+        // Arc motion: different wheel distances
+        let commands = vec![WheelCommand {
+            left: 0.5,
+            right: 0.7,
+        }];
+        let segments = convert_wheel_commands(&commands);
+
+        assert_eq!(segments.len(), 1);
+        let seg = &segments[0];
+
+        // Both linear and angular should be non-zero
+        assert!(seg.linear_vel > 0.0);
+        assert!(seg.angular_vel > 0.0); // Turning left (outer wheel faster)
+    }
+
+    #[test]
+    fn test_skip_zero_commands() {
+        let commands = vec![
+            WheelCommand {
+                left: 0.0,
+                right: 0.0,
+            },
+            WheelCommand {
+                left: 1.0,
+                right: 1.0,
+            },
+        ];
+        let segments = convert_wheel_commands(&commands);
+
+        // Zero command should be skipped
+        assert_eq!(segments.len(), 1);
+    }
+}