diff --git a/Cargo.toml b/Cargo.toml index 260af9d..92f7199 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -22,25 +22,3 @@ tracing = { version = "0.1.41", default-features = false, features = ["attribute mapping-algorithms = { path = "crates/mapping-algorithms" } mapping-suites = { path = "crates/mapping-suites" } - -[profile.mindebug-dev] -inherits = "dev" -debug = "line-tables-only" - -[profile.release] -lto = "thin" -debug = "line-tables-only" - -[profile.nodebug-release] -inherits = "release" -debug = false - -[profile.debug-release] -inherits = "release" -debug = true - -[profile.dist-release] -inherits = "release" -codegen-units = 1 -debug = false -lto = "fat" \ No newline at end of file diff --git a/crates/mapping-suites/Cargo.toml b/crates/mapping-suites/Cargo.toml index 832334d..36752f3 100644 --- a/crates/mapping-suites/Cargo.toml +++ b/crates/mapping-suites/Cargo.toml @@ -16,12 +16,13 @@ all-features = true [features] default = ["std"] -std = ["mapping-algorithms/std", "nalgebra/std", "thiserror", "tracing?/std"] +std = ["mapping-algorithms/std", "nalgebra/std", "num-traits/std", "thiserror", "tracing?/std"] -tracing = ["dep:tracing"] +tracing = ["dep:tracing", "mapping-algorithms/tracing"] [dependencies] nalgebra = { workspace = true } +num-traits = { workspace = true } thiserror = { workspace = true, optional = true } tracing = { workspace = true, optional = true } diff --git a/crates/mapping-suites/src/hector_mapper/grid_map/mod.rs b/crates/mapping-suites/src/hector_mapper/grid_map/mod.rs new file mode 100644 index 0000000..2779faf --- /dev/null +++ b/crates/mapping-suites/src/hector_mapper/grid_map/mod.rs @@ -0,0 +1,1531 @@ +// SPDX-License-Identifier: MIT +/* + * Copyright (c) [2023 - Present] Emily Matheys + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +pub(crate) use scan::ScanUpdater; +pub(crate) use types::{GridMapConfig, MapSample, RayTermination, ScanBeam}; +pub use types::{GridMapError, GridMapResult}; + +use nalgebra::{ComplexField, Point, RealField, SVector}; +use num_traits::{AsPrimitive, ConstOne, ConstZero}; + +use crate::{Box, Vec, array, fmt, ops::RangeInclusive}; + +mod scan; +mod types; + +/// A dense, fixed-extent occupancy grid storing per-cell log-odds. +/// +/// Index-space only: positions are in fractional cell coordinates, where `1.0` is one cell. +/// Converting metres to cells is the caller's responsibility. Axis `0` varies fastest. +/// +/// # Generics +/// * `T`: Either an [`prim@f32`] or [`prim@f64`]. +/// * `N`: a usize, representing the number of dimensions. +#[derive(Clone, PartialEq)] +pub(crate) struct GridMap { + /// Flat log-odds array - the interpolation hot path reads only this. + odds: Box<[T]>, + /// Encoded as `frame << 1 | marked_occupied`. + last_frame_to_update: Box<[u32]>, + dimensions: [usize; N], + strides: [usize; N], + interp_limits: [T; N], + occupied_delta: T, + free_delta: T, + min_log_odds: T, + max_log_odds: T, + frame: u32, +} + +impl fmt::Debug for GridMap { + fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result { + f.debug_struct("GridMap") + .field("dimensions", &self.dimensions) + .field("cell_count", &self.odds.len()) + .field("occupied_delta", &self.occupied_delta) + .field("free_delta", &self.free_delta) + .field("min_log_odds", &self.min_log_odds) + .field("max_log_odds", &self.max_log_odds) + .field("frame", &self.frame) + .finish_non_exhaustive() + } +} + +impl GridMap +where + T: AsPrimitive + AsPrimitive + ConstOne + ConstZero + Copy + RealField, + usize: AsPrimitive, +{ + /// Allocates a new, fully unknown occupancy grid. + /// + /// # Arguments + /// * `dimensions`: the extent of each axis in cells; every entry must be at least `2`. + /// * `config`: the inverse sensor model and saturation confidence. + /// + /// # Returns + /// A [`GridMap`] with every cell at a probability of one half. + /// + /// # Errors + /// * [`GridMapError::DimensionTooSmall`] or [`GridMapError::CapacityOverflow`]: if `dimensions` + /// is unusable. + /// * [`GridMapError::InvalidOccupiedProbability`], [`GridMapError::InvalidFreeProbability`] or + /// [`GridMapError::InvalidMaxConfidence`]: if `config` is inconsistent. + /// * [`GridMapError::AllocationFailed`]: if the allocator cannot satisfy the request. + /// + /// # Warnings + /// Storage is `product(dimensions)` cells twice over, so a three-dimensional map grows cubically. + #[cfg_attr(feature = "tracing", tracing::instrument("Create Grid Map", skip_all))] + pub(crate) fn new(dimensions: [usize; N], config: &GridMapConfig) -> GridMapResult { + for (axis, &extent) in dimensions.iter().enumerate() { + if extent < 2 { + return Err(GridMapError::DimensionTooSmall { axis, extent }); + } + } + + let cells = dimensions + .iter() + .try_fold(1usize, |acc, &extent| acc.checked_mul(extent)) + .ok_or(GridMapError::CapacityOverflow)?; + + let (occupied_delta, free_delta, max_log_odds) = config.resolve()?; + + let mut odds = Vec::new(); + odds.try_reserve_exact(cells) + .map_err(|_| GridMapError::AllocationFailed { cells })?; + odds.resize(cells, T::ZERO); + + let mut stamps = Vec::new(); + stamps + .try_reserve_exact(cells) + .map_err(|_| GridMapError::AllocationFailed { cells })?; + stamps.resize(cells, 0u32); + + Ok(Self { + odds: odds.into_boxed_slice(), + last_frame_to_update: stamps.into_boxed_slice(), + dimensions, + strides: array::from_fn(|idx| dimensions.iter().take(idx).product()), + interp_limits: array::from_fn(|idx| (dimensions[idx] - 1).as_()), + occupied_delta, + free_delta, + min_log_odds: -max_log_odds, + max_log_odds, + // last_frame_to_update is zeroized, so we set this to 1 to immediately start updating. + frame: 1, + }) + } + + /// The log-odds range at which cells saturate, symmetric about zero. + /// + /// # Returns + /// A [`RangeInclusive`] of `T`, the interval every cell value lies within. + fn log_odds_bounds(&self) -> RangeInclusive { + self.min_log_odds..=self.max_log_odds + } +} + +// Addressing. Every read and write in the crate funnels through these functions. +impl GridMap +where + T: AsPrimitive + AsPrimitive + ConstOne + ConstZero + Copy + RealField, + usize: AsPrimitive, +{ + /// Converts a cell index into a flat index into the log-odds array. + /// + /// Each axis is range-checked individually *before* it contributes to the sum; checking only the + /// final sum is unsound, since an overshoot on one axis lands on a valid cell in the next row. + /// + /// # Arguments + /// * `index`: the cell to resolve, which may be negative or out of range. + /// + /// # Returns + /// A [`prim@usize`], the flat index. + /// + /// # Errors + /// * [`GridMapError::OutOfBounds`]: naming the first failing axis in ascending order. + #[inline] + fn translate_to_index(&self, index: &Point) -> GridMapResult { + let mut linear = 0usize; + for axis in 0..N { + // Reinterpreting as unsigned folds the negative test into the upper-bound test: any + // negative value becomes enormous and fails the very same comparison. + let coordinate = index[axis].cast_unsigned(); + if coordinate >= self.dimensions[axis] { + return Err(GridMapError::OutOfBounds { + axis, + index: index[axis], + extent: self.dimensions[axis], + }); + } + linear += coordinate * self.strides[axis]; + } + + Ok(linear) + } + + /// Locates the `2^N` interpolation stencil containing a point, and where in it the point lies. + /// + /// Verifying the stencil once here is what lets the sampler address all `2^N` corners directly. + /// + /// # Arguments + /// * `point`: a position in fractional cell coordinates. + /// + /// # Returns + /// An [`Option`] of the flat index of the stencil's low corner, and the point's fractional + /// offset along each axis - the per-axis interpolation weights. [`None`] if `point` is + /// non-finite or outside the map. A point on an axis's upper limit belongs to the last + /// stencil on that axis, at full weight. + #[inline] + fn stencil_containing(&self, point: &Point) -> Option<(usize, [T; N])> { + let mut base = 0usize; + let mut frac = [T::ZERO; N]; + for axis in 0..N { + let coordinate = point[axis]; + // Every comparison against NaN is false, so a NaN coordinate fails this test and no + // separate check is needed. + if !(T::ZERO..=self.interp_limits[axis]).contains(&coordinate) { + return None; + } + // Non-negativity is proven above, so this truncating cast *is* a floor. Going through + // `ComplexField::floor` instead would emit a libm call under `--no-default-features`. + let mut cell: usize = coordinate.as_(); + let mut offset = coordinate - cell.as_(); + + // The top of the domain has no cell above it. Folding it into the last stencil at + // full weight yields the same value, and stops the corner gather running off the end + // of the row into the *next* one, whose cells are nowhere near the query. + if cell + 1 >= self.dimensions[axis] { + cell = self.dimensions[axis] - 2; + offset = T::ONE; + } + + frac[axis] = offset; + base += cell * self.strides[axis]; + } + Some((base, frac)) + } + + /// Resolves a fractional coordinate to the cell containing it. + /// + /// Explicitly fallible, because a float-to-integer cast saturates and maps NaN to zero. + /// + /// # Arguments + /// * `point`: a position in fractional cell coordinates. + /// + /// # Returns + /// The [`Point`] whose cell contains `point`. + /// + /// # Errors + /// * [`GridMapError::NonFiniteCoordinate`]: if any coordinate is NaN or infinite. + /// * [`GridMapError::OutOfBounds`]: naming the first axis outside the map. + pub(crate) fn cell_containing(&self, point: &Point) -> GridMapResult> { + let mut index = Point::::origin(); + for axis in 0..N { + let coordinate = point[axis]; + if !ComplexField::is_finite(&coordinate) { + return Err(GridMapError::NonFiniteCoordinate { axis }); + } + + let extent = self.dimensions[axis]; + // A coordinate of exactly `extent` is already past the last cell, so the valid + // fractional range is half-open. + if !(T::ZERO..extent.as_()).contains(&coordinate) { + return Err(GridMapError::OutOfBounds { + axis, + // Truncating and saturating; for the just-out-of-range coordinates a + // caller is likely debugging, this is the cell they meant. + index: coordinate.as_(), + extent, + }); + } + + // Non-negativity is proven above, so truncation is a floor. + index[axis] = coordinate.as_(); + } + Ok(index) + } +} + +// Reads, single-cell writes, and the scan entry point. +impl GridMap +where + T: AsPrimitive + AsPrimitive + ConstOne + ConstZero + Copy + RealField, + usize: AsPrimitive, +{ + /// The occupancy probability corresponding to a log-odds value. + /// + /// # Arguments + /// * `log_odds`: the value to convert. + /// + /// # Returns + /// A `T` in the range `0.0..=1.0`. + #[inline] + fn log_odds_to_probability(log_odds: T) -> T { + T::ONE / (T::ONE + ComplexField::exp(-log_odds)) + } + + /// Maps a log-odds sample through the logistic function, carrying its gradient by the chain rule. + /// + /// # Arguments + /// * `sample`: a sample whose value is log-odds. + /// + /// # Returns + /// A sample whose value is a probability, for the cost of one exponential. + #[inline] + fn sample_to_probability(sample: MapSample) -> MapSample { + let probability = Self::log_odds_to_probability(sample.value); + MapSample { + value: probability, + gradient: sample.gradient * (probability * (T::ONE - probability)), + } + } + + /// Multilinearly interpolates the log-odds field and its analytic gradient. + /// + /// The value is a weighted sum over the `2^N` corners of the stencil containing `point`, and + /// each gradient component is the same sum over the differences across that axis, with the + /// axis's own weight left out. Performs no transcendental operations. + /// + /// # Arguments + /// * `point`: a position in fractional cell coordinates. + /// + /// # Returns + /// An [`Option`] of [`MapSample`], or [`None`] if `point` is non-finite or outside the map. + /// + /// # Warnings + /// This interpolates log-odds and sigmoids afterwards, whereas Hector SLAM interpolates + /// already-sigmoided probabilities. The fields agree only at cell corners, so gains and + /// thresholds taken from such an implementation do not carry over unscaled. + pub(crate) fn sample_log_odds(&self, point: &Point) -> Option> { + let (base, frac) = self.stencil_containing(point)?; + + // A corner is a bit pattern: bit `axis` set means the high side of that axis. The stencil + // has been proven in range, so every index built this way addresses a real cell. + let corner_odds = |corner: usize| -> T { + self.odds[(0..N).fold(base, |acc, axis| { + acc + if corner >> axis & 1 == 1 { + self.strides[axis] + } else { + 0 + } + })] + }; + + // The product of the per-axis weights, optionally leaving one axis out; omitting axis `k` + // is what turns the value's weighted sum into its derivative along `k`. + let weight = |corner: usize, skip: Option| -> T { + (0..N) + .filter(|axis| Some(*axis) != skip) + .fold(T::ONE, |acc, axis| { + acc * if corner >> axis & 1 == 1 { + frac[axis] + } else { + T::ONE - frac[axis] + } + }) + }; + + let corners = 1usize << N; + let value = (0..corners).fold(T::ZERO, |acc, corner| { + acc + weight(corner, None) * corner_odds(corner) + }); + + let mut gradient = SVector::::zeros(); + for axis in 0..N { + // Pair every low corner with its neighbour across `axis`; the difference between the + // two is that edge's derivative. + gradient[axis] = (0..corners).filter(|corner| corner >> axis & 1 == 0).fold( + T::ZERO, + |acc, corner| { + acc + weight(corner, Some(axis)) + * (corner_odds(corner | (1 << axis)) - corner_odds(corner)) + }, + ); + } + + Some(MapSample { value, gradient }) + } + + /// Multilinearly interpolates the log-odds field, then maps it through the logistic function. + /// + /// Costs one exponential per query rather than one per corner. See the warning on + /// [`sample_log_odds`](Self::sample_log_odds). + /// + /// # Arguments + /// * `point`: a position in fractional cell coordinates. + /// + /// # Returns + /// An [`Option`] of [`MapSample`] whose value is a probability in the range `0.0..=1.0`, or + /// [`None`] if `point` is non-finite or outside the map. + pub(crate) fn sample_probability(&self, point: &Point) -> Option> { + self.sample_log_odds(point).map(Self::sample_to_probability) + } + + /// Reads the log-odds of a single cell. + /// + /// # Arguments + /// * `index`: the cell to read, which may be negative or out of range. + /// + /// # Returns + /// An [`Option`] of `T`, or [`None`] if `index` lies outside the map. + #[inline] + pub(crate) fn log_odds_at(&self, index: &Point) -> Option { + self.translate_to_index(index) + .ok() + .map(|linear| self.odds[linear]) + } + + /// Reads the occupancy probability of a single cell. + /// + /// # Arguments + /// * `index`: the cell to read, which may be negative or out of range. + /// + /// # Returns + /// An [`Option`] of `T` in the range `0.0..=1.0`, or [`None`] if `index` lies outside the map. + /// An unwritten cell reads as exactly one half. + #[inline] + pub(crate) fn probability_at(&self, index: &Point) -> Option { + self.log_odds_at(index).map(Self::log_odds_to_probability) + } + + /// Overwrites a cell's log-odds, bypassing the sensor model and per-scan deduplication. + /// + /// # Arguments + /// * `index`: the cell to overwrite. + /// * `log_odds`: the value to store, clamped into [`log_odds_bounds`](Self::log_odds_bounds). + /// + /// # Errors + /// * [`GridMapError::OutOfBounds`]: naming the first offending axis. + pub(crate) fn set_log_odds( + &mut self, + index: &Point, + log_odds: T, + ) -> GridMapResult<()> { + let linear = self.translate_to_index(index)?; + self.odds[linear] = log_odds.clamp(self.min_log_odds, self.max_log_odds); + Ok(()) + } + + /// Overwrites a cell's occupancy probability. See [`set_log_odds`](Self::set_log_odds). + /// + /// # Arguments + /// * `index`: the cell to overwrite. + /// * `probability`: strictly between `0.0` and `1.0`. + /// + /// # Errors + /// * [`GridMapError::OutOfBounds`]: naming the first offending axis. + /// * [`GridMapError::InvalidOccupiedProbability`]: if `probability` is not strictly between zero + /// and one, since either endpoint is infinite in log-odds. + pub(crate) fn set_probability( + &mut self, + index: &Point, + probability: T, + ) -> GridMapResult<()> { + if !(probability > T::ZERO && probability < T::ONE) { + return Err(GridMapError::InvalidOccupiedProbability); + } + self.set_log_odds(index, types::logit(probability)) + } + + /// Returns every cell to unknown, preserving the dimensions, configuration and allocation. + #[cfg_attr(feature = "tracing", tracing::instrument("Reset Grid Map", skip_all))] + pub(crate) fn reset(&mut self) { + self.odds.fill(T::ZERO); + self.last_frame_to_update.fill(0); + self.frame = 1; + } + + /// Opens a scan-scoped update session. + /// + /// Each cell absorbs at most one update per scan, however many beams cross it. The returned guard + /// borrows the map mutably, so no second scan can begin while it lives. + /// + /// # Returns + /// A [`ScanUpdater`] borrowing this map. + #[must_use = "a ScanUpdater performs no work until beams are integrated into it"] + #[cfg_attr( + feature = "tracing", + tracing::instrument("Begin Scan", skip_all, level = "debug") + )] + pub(crate) fn begin_scan(&mut self) -> ScanUpdater<'_, T, N> { + // Bit 0 of a stamp records "marked occupied during this scan", so the generation takes + // bits 1 upwards. A stale stamp would eventually alias the reused counter and silently stop + // that cell updating; at 100Hz that is about 248 days away, so this is a backstop. + if self.frame >= u32::MAX >> 1 { + self.last_frame_to_update.fill(0); + self.frame = 1; + } else { + self.frame += 1; + } + + ScanUpdater::new(self) + } + + /// Forces the scan generation counter, so the wraparound sweep can be tested directly. + /// + /// # Arguments + /// * `frame`: the generation to set. + #[cfg(test)] + pub(super) fn force_frame(&mut self, frame: u32) { + self.frame = frame; + } +} + +// The per-scan update machinery. Public access is through [`ScanUpdater`], which owns the +// invariant that a generation has been advanced first. +impl GridMap +where + T: AsPrimitive + AsPrimitive + ConstOne + ConstZero + Copy + RealField, + usize: AsPrimitive, +{ + /// Clamps a point so that every coordinate addresses a cell of the map. + /// + /// # Arguments + /// * `point`: a position in fractional cell coordinates, possibly resting on a bounding face. + /// + /// # Returns + /// A [`Point`] inside the addressable domain, in the cell an interior `point` already lay in. + fn clamp_to_addressable_cell(&self, point: Point) -> Point { + // Clipping lands endpoints *on* the bounding faces, where the coordinate `extent` + // addresses cell `extent`, off the map. The plotter substitutes the exact endpoint for its + // final step, so leaving it there would drop the last cell inside the map too. + let half = T::ONE / (T::ONE + T::ONE); + + let mut point = point; + for axis in 0..N { + let limit: T = self.dimensions[axis].as_(); + point[axis] = point[axis].clamp(T::ZERO, limit - half); + } + point + } + + /// Clips a segment to the map's bounding box using the slab method. + /// + /// This is what bounds a ray cast: a step count follows from a beam's length, so an absurd but + /// finite endpoint would otherwise be walked cell by cell only to be discarded. + /// + /// # Arguments + /// * `origin`: the segment's start, in fractional cell coordinates. + /// * `endpoint`: the segment's end, in fractional cell coordinates. + /// + /// # Returns + /// An [`Option`] of the clipped start, the clipped end, and whether `endpoint` itself lay inside + /// the map; [`None`] if the segment never enters it. + fn clip_segment( + &self, + origin: &Point, + endpoint: &Point, + ) -> Option<(Point, Point, bool)> { + let direction = endpoint - origin; + let mut entry_fraction = T::ZERO; + let mut exit_fraction = T::ONE; + + for axis in 0..N { + let extent: T = self.dimensions[axis].as_(); + let delta = direction[axis]; + + if delta.is_zero() { + // Parallel to this slab: either wholly inside it, or the segment misses entirely. + // The upper bound is exclusive, matching `cell_containing`: a coordinate of + // exactly `extent` already lies in cell `extent`, which is off the map. + if origin[axis] < T::ZERO || origin[axis] >= extent { + return None; + } + continue; + } + + let near = (T::ZERO - origin[axis]) / delta; + let far = (extent - origin[axis]) / delta; + let (near, far) = if near > far { (far, near) } else { (near, far) }; + + entry_fraction = entry_fraction.max(near); + exit_fraction = exit_fraction.min(far); + + if entry_fraction > exit_fraction { + return None; + } + } + + let entry = origin + direction * entry_fraction; + let exit = origin + direction * exit_fraction; + + // The slab test intersects the *closed* box, but the addressable domain is half-open, so + // a segment can survive it lying wholly within an upper face - which the projection below + // would slide onto a row of real cells it never crossed. Both ends on the face is that + // case. + for axis in 0..N { + let extent: T = self.dimensions[axis].as_(); + if entry[axis] >= extent && exit[axis] >= extent { + return None; + } + } + + Some(( + self.clamp_to_addressable_cell(entry), + self.clamp_to_addressable_cell(exit), + // `exit_fraction` only ever shrinks from one, so it is still one precisely when the + // endpoint was never clipped. An endpoint on an upper face escapes clipping too, but + // addresses a cell off the map, so the range check rejects it as well. + exit_fraction >= T::ONE + && (0..N).all(|axis| { + let extent: T = self.dimensions[axis].as_(); + (T::ZERO..extent).contains(&exit[axis]) + }), + )) + } + + /// Records that a beam passed through a cell, applying the free increment. + /// + /// A cell already touched by this scan, in either state, is left alone. + /// + /// # Arguments + /// * `index`: the cell observed as free. + /// + /// # Returns + /// A [`prim@bool`], whether the cell was updated. + /// + /// # Errors + /// * [`GridMapError::OutOfBounds`]: naming the first offending axis. + #[inline] + fn mark_free_at(&mut self, index: &Point) -> GridMapResult { + let linear = self.translate_to_index(index)?; + if self.last_frame_to_update[linear] >> 1 == self.frame { + return Ok(false); + } + + self.last_frame_to_update[linear] = self.frame << 1; + self.odds[linear] = + (self.odds[linear] + self.free_delta).clamp(self.min_log_odds, self.max_log_odds); + Ok(true) + } + + /// Records that a beam terminated in a cell, applying the occupied increment. + /// + /// Occupancy wins within a scan: an earlier free update on the same cell is retracted first, + /// exactly, unless the cell has saturated free - there what that update contributed is no + /// longer recoverable, and nothing is retracted. + /// + /// # Arguments + /// * `index`: the cell observed as occupied. + /// + /// # Returns + /// A [`prim@bool`], whether the cell was updated. + /// + /// # Errors + /// * [`GridMapError::OutOfBounds`]: naming the first offending axis. + #[inline] + fn mark_occupied_at(&mut self, index: &Point) -> GridMapResult { + let linear = self.translate_to_index(index)?; + + let current = self.frame << 1; + let delta = if self.last_frame_to_update[linear] == current | 1 { + return Ok(false); + } else if self.last_frame_to_update[linear] == current { + // Marked free earlier in this scan, so retract that first: the free increment is + // negative, and subtracting it adds its magnitude back. On the floor the clamp ate an + // unknown part of that update, so crediting it back would put the cell above where a + // lone return does, making the scan's outcome depend on beam order. Retract nothing + // there: exact for a cell that was already on the floor, never optimistic just above. + if self.odds[linear] <= self.min_log_odds { + self.occupied_delta + } else { + self.occupied_delta - self.free_delta + } + } else { + self.occupied_delta + }; + + self.last_frame_to_update[linear] = current | 1; + self.odds[linear] = (self.odds[linear] + delta).clamp(self.min_log_odds, self.max_log_odds); + Ok(true) + } +} + +#[cfg(test)] +mod tests { + use super::*; + use nalgebra::{Point2, Point3}; + + /// A map with the default sensor model, which every test in this module shares. + fn map(dimensions: [usize; N]) -> GridMap { + GridMap::new(dimensions, &GridMapConfig::default()).unwrap() + } + + #[test] + fn test_new_rejects_dimension_too_small() { + // Both a zero extent and a single-cell extent are unusable: neither admits a stencil. + for extent in [0, 1] { + for axis in 0..3 { + let mut dimensions = [4usize; 3]; + dimensions[axis] = extent; + + assert_eq!( + GridMap::::new(dimensions, &GridMapConfig::default()).unwrap_err(), + GridMapError::DimensionTooSmall { axis, extent } + ); + } + } + } + + #[test] + fn test_new_rejects_capacity_overflow() { + assert_eq!( + GridMap::::new([usize::MAX, usize::MAX, 2], &GridMapConfig::default()) + .unwrap_err(), + GridMapError::CapacityOverflow + ); + } + + #[test] + fn test_new_propagates_config_errors() { + let config = GridMapConfig::::builder() + .with_free_probability(0.9) + .build(); + + assert_eq!( + GridMap::::new([4, 4], &config).unwrap_err(), + GridMapError::InvalidFreeProbability + ); + } + + #[test] + fn test_new_reports_expected_shape() { + let grid = map([3usize, 5, 7]); + + assert_eq!(grid.dimensions, [3, 5, 7]); + assert_eq!(grid.odds.len(), 105); + } + + #[test] + fn test_new_initialises_every_cell_to_unknown() { + let grid = map([3usize, 5]); + + assert!(grid.odds.iter().all(|&odds| odds == 0.0)); + assert_eq!( + grid.probability_at(&Point2::new(1, 2)), + Some(0.5), + "a never-written cell must read as exactly one half" + ); + } + + #[test] + fn test_log_odds_bounds_are_symmetric() { + let grid = map([4usize, 4]); + let bounds = grid.log_odds_bounds(); + + assert_eq!(*bounds.start(), -*bounds.end()); + assert!((*bounds.end() - 3.476_098_7).abs() < 1e-5); + } + + #[test] + fn test_strides_2d_non_square() { + // Axis 0 is fastest-varying, so its stride is always one. + assert_eq!(map([3usize, 5]).strides, [1, 3]); + } + + #[test] + fn test_strides_3d_non_cubic() { + assert_eq!(map([3usize, 5, 7]).strides, [1, 3, 15]); + } + + #[test] + fn test_linearize_is_bijective_2d() { + let grid = map([3usize, 5]); + let mut seen = Vec::new(); + + for y in 0..5 { + for x in 0..3 { + seen.push(grid.translate_to_index(&Point2::new(x, y)).unwrap()); + } + } + + seen.sort_unstable(); + seen.dedup(); + assert_eq!(seen.len(), grid.odds.len()); + assert_eq!(*seen.last().unwrap(), grid.odds.len() - 1); + } + + #[test] + fn test_linearize_is_bijective_3d() { + let grid = map([3usize, 5, 7]); + let mut seen = Vec::new(); + + for z in 0..7 { + for y in 0..5 { + for x in 0..3 { + seen.push(grid.translate_to_index(&Point3::new(x, y, z)).unwrap()); + } + } + } + + seen.sort_unstable(); + seen.dedup(); + assert_eq!(seen.len(), grid.odds.len()); + assert_eq!(*seen.last().unwrap(), grid.odds.len() - 1); + } + + /// The load-bearing test of the whole addressing scheme. Summing `coord * stride` and then + /// bounds-checking the sum is unsound: in a `[4, 4]` map the cell `[4, 0]` sums to `4`, which + /// is the perfectly valid cell `[0, 1]`. It is not enough to assert that the call fails - + /// the aliased cell must be shown to be untouched. + #[test] + fn test_row_wrap_is_rejected() { + let mut grid = map([4usize, 4]); + let aliased = Point2::new(0, 1); + assert_eq!(grid.translate_to_index(&aliased), Ok(4)); + + assert_eq!( + grid.translate_to_index(&Point2::new(4, 0)).unwrap_err(), + GridMapError::OutOfBounds { + axis: 0, + index: 4, + extent: 4 + } + ); + assert!(grid.set_log_odds(&Point2::new(4, 0), 1.0).is_err()); + assert_eq!( + grid.log_odds_at(&aliased), + Some(0.0), + "the write must not have landed on the cell the bad index aliases" + ); + } + + #[test] + fn test_column_wrap_is_rejected_3d() { + let mut grid = map([4usize, 4, 4]); + let aliased = Point3::new(0, 0, 1); + assert_eq!(grid.translate_to_index(&aliased), Ok(16)); + + // [0, 4, 0] sums to 16, which is the valid cell one slab along. + assert_eq!( + grid.translate_to_index(&Point3::new(0, 4, 0)).unwrap_err(), + GridMapError::OutOfBounds { + axis: 1, + index: 4, + extent: 4 + } + ); + assert!(grid.set_log_odds(&Point3::new(0, 4, 0), 1.0).is_err()); + assert_eq!(grid.log_odds_at(&aliased), Some(0.0)); + } + + #[test] + fn test_negative_index_is_rejected_not_wrapped() { + let grid = map([4usize, 4]); + + for (index, axis) in [ + (Point2::new(-1, 0), 0), + (Point2::new(0, -1), 1), + (Point2::new(-1, -1), 0), + ] { + assert_eq!( + grid.translate_to_index(&index).unwrap_err(), + GridMapError::OutOfBounds { + axis, + index: index[axis], + extent: 4 + }, + "a negative index must fail on its own axis, not wrap onto another" + ); + assert_eq!(grid.log_odds_at(&index), None); + } + } + + #[test] + fn test_out_of_bounds_reports_first_failing_axis() { + let grid = map([4usize, 5, 6]); + + assert_eq!( + grid.translate_to_index(&Point3::new(9, 9, 9)).unwrap_err(), + GridMapError::OutOfBounds { + axis: 0, + index: 9, + extent: 4 + }, + "axes are checked in ascending order" + ); + assert_eq!( + grid.translate_to_index(&Point3::new(0, 9, 9)).unwrap_err(), + GridMapError::OutOfBounds { + axis: 1, + index: 9, + extent: 5 + } + ); + assert_eq!( + grid.translate_to_index(&Point3::new(0, 0, -3)).unwrap_err(), + GridMapError::OutOfBounds { + axis: 2, + index: -3, + extent: 6 + } + ); + } + + #[test] + fn test_cell_containing_resolves_interior() { + let grid = map([10usize, 10]); + + assert_eq!( + grid.cell_containing(&Point2::new(3.0, 4.0)).unwrap(), + Point2::new(3, 4) + ); + assert_eq!( + grid.cell_containing(&Point2::new(3.9, 4.1)).unwrap(), + Point2::new(3, 4), + "a fractional coordinate resolves to the cell containing it" + ); + assert_eq!( + grid.cell_containing(&Point2::new(0.0, 0.0)).unwrap(), + Point2::new(0, 0) + ); + } + + /// A float-to-integer cast in Rust saturates and maps NaN to zero, so an unchecked conversion + /// would silently redirect a NaN coordinate to the map origin rather than rejecting it. + #[test] + fn test_cell_containing_rejects_non_finite() { + let grid = map([10usize, 10]); + + for (point, axis) in [ + (Point2::new(f32::NAN, 1.0), 0), + (Point2::new(1.0, f32::NAN), 1), + (Point2::new(f32::INFINITY, 1.0), 0), + (Point2::new(1.0, f32::NEG_INFINITY), 1), + ] { + assert_eq!( + grid.cell_containing(&point).unwrap_err(), + GridMapError::NonFiniteCoordinate { axis } + ); + } + } + + #[test] + fn test_cell_containing_rejects_out_of_range() { + let grid = map([10usize, 10]); + + // A coordinate of exactly the extent is already past the final cell. + assert!(grid.cell_containing(&Point2::new(10.0, 1.0)).is_err()); + assert!(grid.cell_containing(&Point2::new(9.999, 1.0)).is_ok()); + // Truncation toward zero would fold -0.5 into cell zero; it must be rejected instead. + assert!(grid.cell_containing(&Point2::new(-0.5, 1.0)).is_err()); + } + + #[test] + fn test_set_log_odds_clamps_to_bounds() { + let mut grid = map([4usize, 4]); + let index = Point2::new(1, 1); + let bounds = grid.log_odds_bounds(); + + grid.set_log_odds(&index, 1000.0).unwrap(); + assert_eq!(grid.log_odds_at(&index), Some(*bounds.end())); + + grid.set_log_odds(&index, -1000.0).unwrap(); + assert_eq!(grid.log_odds_at(&index), Some(*bounds.start())); + } + + #[test] + fn test_set_probability_round_trips_through_log_odds() { + let mut grid = map([4usize, 4]); + let index = Point2::new(2, 3); + + grid.set_probability(&index, 0.8).unwrap(); + assert!((grid.probability_at(&index).unwrap() - 0.8).abs() < 1e-6); + } + + #[test] + fn test_set_probability_rejects_zero_and_one() { + let mut grid = map([4usize, 4]); + let index = Point2::new(0, 0); + + // Either endpoint is infinite in log-odds, so both are refused. + for probability in [0.0, 1.0, -0.5, 1.5] { + assert_eq!( + grid.set_probability(&index, probability).unwrap_err(), + GridMapError::InvalidOccupiedProbability + ); + } + assert_eq!(grid.log_odds_at(&index), Some(0.0)); + } + + #[test] + fn test_log_odds_and_probability_agree() { + let mut grid = map([4usize, 4]); + let index = Point2::new(1, 2); + grid.set_log_odds(&index, 1.5).unwrap(); + + let expected = 1.0 / (1.0 + (-1.5f32).exp()); + assert!((grid.probability_at(&index).unwrap() - expected).abs() < 1e-6); + } + + #[test] + fn test_reset_returns_every_cell_to_unknown() { + let mut grid = map([4usize, 4]); + grid.set_log_odds(&Point2::new(1, 1), 2.0).unwrap(); + grid.set_log_odds(&Point2::new(2, 2), -2.0).unwrap(); + + grid.reset(); + + assert!(grid.odds.iter().all(|&odds| odds == 0.0)); + assert_eq!(grid.dimensions, [4, 4], "the shape must survive a reset"); + } + + #[test] + fn test_cell_layout_is_axis_zero_fastest() { + let mut grid = map([3usize, 5]); + grid.set_log_odds(&Point2::new(1, 0), 1.0).unwrap(); + grid.set_log_odds(&Point2::new(0, 1), 2.0).unwrap(); + + let odds = grid.odds.to_vec(); + assert_eq!(odds.len(), 15); + assert_eq!(odds[1], 1.0, "a step along axis 0 moves one element"); + assert_eq!( + odds[3], 2.0, + "a step along axis 1 moves dimensions[0] elements" + ); + } + + #[test] + fn test_reads_take_shared_reference() { + let grid = map([4usize, 4]); + + // Two live shared borrows, both querying: a scan matcher needs exactly this. + let first = &grid; + let second = &grid; + assert_eq!( + first.log_odds_at(&Point2::new(1, 1)), + second.log_odds_at(&Point2::new(1, 1)) + ); + } + + #[cfg(feature = "std")] + #[test] + fn test_debug_omits_cell_contents() { + let rendered = format!("{:?}", map([64usize, 64])); + + assert!(rendered.contains("dimensions")); + assert!(rendered.contains("cell_count")); + assert!( + rendered.len() < 512, + "Debug must not render four thousand cells: {rendered}" + ); + } + + #[test] + fn test_clip_segment_entirely_inside_is_identity() { + let grid = map([10usize, 10]); + let (entry, exit, inside) = grid + .clip_segment(&Point2::new(1.0, 1.0), &Point2::new(5.0, 5.0)) + .unwrap(); + + assert_eq!(entry, Point2::new(1.0, 1.0)); + assert_eq!(exit, Point2::new(5.0, 5.0)); + assert!(inside); + } + + #[test] + fn test_clip_segment_entirely_outside_is_none() { + let grid = map([10usize, 10]); + + assert!( + grid.clip_segment(&Point2::new(-5.0, -5.0), &Point2::new(-1.0, -1.0)) + .is_none() + ); + assert!( + grid.clip_segment(&Point2::new(20.0, 1.0), &Point2::new(30.0, 1.0)) + .is_none() + ); + // Parallel to the slab and outside it: the axis-aligned early exit. + assert!( + grid.clip_segment(&Point2::new(1.0, -3.0), &Point2::new(8.0, -3.0)) + .is_none() + ); + } + + #[test] + fn test_clip_segment_trims_the_far_end() { + let grid = map([10usize, 10]); + let (entry, exit, inside) = grid + .clip_segment(&Point2::new(5.0, 5.0), &Point2::new(25.0, 5.0)) + .unwrap(); + + assert_eq!(entry, Point2::new(5.0, 5.0)); + assert_eq!( + exit, + Point2::new(9.5, 5.0), + "the exit is pulled back to the centre of the last cell, since a coordinate of exactly 10.0 addresses cell 10, which is off the map" + ); + assert!(!inside, "the caller's endpoint was clipped away"); + } + + #[test] + fn test_clip_segment_trims_the_near_end() { + let grid = map([10usize, 10]); + let (entry, exit, inside) = grid + .clip_segment(&Point2::new(-10.0, 5.0), &Point2::new(5.0, 5.0)) + .unwrap(); + + assert_eq!(entry, Point2::new(0.0, 5.0)); + assert_eq!(exit, Point2::new(5.0, 5.0)); + assert!(inside, "the caller's endpoint was inside all along"); + } + + #[test] + fn test_clip_segment_passing_clean_through() { + let grid = map([10usize, 10, 10]); + let (entry, exit, inside) = grid + .clip_segment(&Point3::new(-8.0, 5.0, 5.0), &Point3::new(18.0, 5.0, 5.0)) + .unwrap(); + + assert_eq!(entry, Point3::new(0.0, 5.0, 5.0)); + assert_eq!(exit, Point3::new(9.5, 5.0, 5.0)); + assert!(!inside); + } + + /// The addressable domain is half-open, so a beam lying along `y == extent` is in row 8 of an + /// eight-row map: off it. Accepting the segment and then projecting it into the last cell + /// would write free space across a row the beam never crossed, and the acceptance turned on + /// an exact float equality - one ulp further out the same beam was rejected outright. + #[test] + fn test_clip_segment_rejects_a_segment_lying_on_the_upper_face() { + let grid = map([8usize, 8]); + + assert!( + grid.clip_segment(&Point2::new(2.0, 8.0), &Point2::new(5.0, 8.0)) + .is_none() + ); + // The same beam a hair inside is still a row-7 beam and must survive. + let (entry, exit, _) = grid + .clip_segment(&Point2::new(2.0, 7.9999), &Point2::new(5.0, 7.9999)) + .unwrap(); + assert_eq!(entry, Point2::new(2.0, 7.5)); + assert_eq!(exit, Point2::new(5.0, 7.5)); + } + + /// The non-parallel counterpart, which the slab test lets through: `entry_fraction` and + /// `exit_fraction` both collapse to zero, leaving a degenerate segment sitting on the face. + #[test] + fn test_clip_segment_rejects_a_segment_leaving_from_the_upper_face() { + let grid = map([8usize, 8]); + + assert!( + grid.clip_segment(&Point2::new(8.0, 3.0), &Point2::new(9.0, 3.0)) + .is_none() + ); + // Starting on the same face but aiming *inward* genuinely enters the map. + let (entry, exit, _) = grid + .clip_segment(&Point2::new(8.0, 3.0), &Point2::new(7.0, 3.0)) + .unwrap(); + assert_eq!(entry, Point2::new(7.5, 3.0)); + assert_eq!(exit, Point2::new(7.0, 3.0)); + } + + /// An endpoint on an upper face escapes clipping, so `exit_fraction` is still exactly one - + /// but the cell it addresses is off the map, and the caller reads "inside" as "the beam really + /// stopped here". It must read as clipped, exactly as the endpoint one ulp beyond it does. + #[test] + fn test_clip_segment_endpoint_on_the_upper_face_is_not_inside() { + let grid = map([8usize, 8]); + + let (_, on_face, inside) = grid + .clip_segment(&Point2::new(4.5, 4.5), &Point2::new(8.0, 4.5)) + .unwrap(); + assert!(!inside); + + let (_, past_face, still_outside) = grid + .clip_segment(&Point2::new(4.5, 4.5), &Point2::new(8.0001, 4.5)) + .unwrap(); + assert!(!still_outside); + assert_eq!( + on_face, past_face, + "the cells covered must not turn on an exact float equality either" + ); + } + + /// A small deterministic generator, so the differential tests are reproducible without + /// pulling `rand` into this crate. + struct Lcg(u64); + + impl Lcg { + fn next_unit(&mut self) -> f64 { + self.0 = self + .0 + .wrapping_mul(6_364_136_223_846_793_005) + .wrapping_add(1_442_695_040_888_963_407); + ((self.0 >> 11) as f64) / ((1u64 << 53) as f64) + } + } + + /// Fills a map with pseudo-random log-odds spanning most of the clamp band. + fn noisy_map(dimensions: [usize; N], seed: u64) -> GridMap { + let mut grid = GridMap::::new(dimensions, &GridMapConfig::default()).unwrap(); + let mut rng = Lcg(seed); + for cell in grid.odds.iter_mut() { + *cell = rng.next_unit() * 6.0 - 3.0; + } + grid + } + + #[test] + fn test_bilinear_matches_hand_computed() { + let mut grid = GridMap::::new([2, 2], &GridMapConfig::default()).unwrap(); + grid.set_log_odds(&Point2::new(0, 0), 0.1).unwrap(); + grid.set_log_odds(&Point2::new(1, 0), 0.2).unwrap(); + grid.set_log_odds(&Point2::new(0, 1), 0.3).unwrap(); + grid.set_log_odds(&Point2::new(1, 1), 0.4).unwrap(); + + // lower = 0.1 + 0.1*0.25 = 0.125; upper = 0.3 + 0.1*0.25 = 0.325 + // value = 0.125 + (0.325 - 0.125)*0.75 = 0.275 + let sample = grid.sample_log_odds(&Point2::new(0.25, 0.75)).unwrap(); + assert!((sample.value - 0.275).abs() < 1e-12); + assert!((sample.gradient[0] - 0.1).abs() < 1e-12); + assert!((sample.gradient[1] - 0.2).abs() < 1e-12); + } + + #[test] + fn test_trilinear_matches_hand_computed() { + let mut grid = GridMap::::new([2, 2, 2], &GridMapConfig::default()).unwrap(); + for z in 0..2 { + for y in 0..2 { + for x in 0..2 { + let value = 0.1 * f64::from(1 + x + 2 * y + 4 * z); + grid.set_log_odds(&Point3::new(x as isize, y as isize, z as isize), value) + .unwrap(); + } + } + } + + // At the stencil centre the value is the mean of all eight corners. + let sample = grid.sample_log_odds(&Point3::new(0.5, 0.5, 0.5)).unwrap(); + assert!((sample.value - 0.45).abs() < 1e-12); + assert!((sample.gradient[0] - 0.1).abs() < 1e-12); + assert!((sample.gradient[1] - 0.2).abs() < 1e-12); + assert!((sample.gradient[2] - 0.4).abs() < 1e-12); + } + + /// On a field that is exactly linear, interpolation must be exact and the gradient constant. + #[test] + fn test_linear_ramp_is_exact_with_constant_gradient() { + let mut grid = GridMap::::new([8, 8], &GridMapConfig::default()).unwrap(); + let ramp = |x: f64, y: f64| 0.25 * x - 0.125 * y + 0.5; + + for y in 0..8 { + for x in 0..8 { + grid.set_log_odds( + &Point2::new(x as isize, y as isize), + ramp(f64::from(x), f64::from(y)), + ) + .unwrap(); + } + } + + let mut rng = Lcg(0x1234); + for _ in 0..500 { + let (x, y) = (rng.next_unit() * 7.0, rng.next_unit() * 7.0); + let sample = grid.sample_log_odds(&Point2::new(x, y)).unwrap(); + + assert!((sample.value - ramp(x, y)).abs() < 1e-12); + assert!((sample.gradient[0] - 0.25).abs() < 1e-12); + assert!((sample.gradient[1] - -0.125).abs() < 1e-12); + } + } + + #[test] + fn test_value_is_exact_at_integer_coordinates() { + let grid = noisy_map([6usize, 6], 0xABCD); + + for y in 0..5 { + for x in 0..5 { + let sample = grid + .sample_log_odds(&Point2::new(f64::from(x), f64::from(y))) + .unwrap(); + let cell = grid + .log_odds_at(&Point2::new(x as isize, y as isize)) + .unwrap(); + assert!((sample.value - cell).abs() < 1e-12); + } + } + } + + #[test] + fn test_gradient_matches_central_differences_2d() { + let grid = noisy_map([9usize, 9], 0x2222); + let step = 1e-5; + let mut rng = Lcg(0x3333); + + for _ in 0..500 { + let point = Point2::new(1.0 + rng.next_unit() * 6.0, 1.0 + rng.next_unit() * 6.0); + let sample = grid.sample_log_odds(&point).unwrap(); + + for axis in 0..2 { + let (mut low, mut high) = (point, point); + low[axis] -= step; + high[axis] += step; + let numeric = (grid.sample_log_odds(&high).unwrap().value + - grid.sample_log_odds(&low).unwrap().value) + / (2.0 * step); + + assert!( + (sample.gradient[axis] - numeric).abs() < 1e-6, + "axis {axis} at {point:?}: {} vs {numeric}", + sample.gradient[axis] + ); + } + } + } + + #[test] + fn test_gradient_matches_central_differences_3d() { + let grid = noisy_map([7usize, 7, 7], 0x4444); + let step = 1e-5; + let mut rng = Lcg(0x5555); + + for _ in 0..500 { + let point = Point3::new( + 1.0 + rng.next_unit() * 4.0, + 1.0 + rng.next_unit() * 4.0, + 1.0 + rng.next_unit() * 4.0, + ); + let sample = grid.sample_log_odds(&point).unwrap(); + + for axis in 0..3 { + let (mut low, mut high) = (point, point); + low[axis] -= step; + high[axis] += step; + let numeric = (grid.sample_log_odds(&high).unwrap().value + - grid.sample_log_odds(&low).unwrap().value) + / (2.0 * step); + + assert!( + (sample.gradient[axis] - numeric).abs() < 1e-6, + "axis {axis}" + ); + } + } + } + + /// A non-square map whose field varies along x alone. A transposed stride would show up here + /// as a gradient pointing the wrong way, which a square map would hide. + #[test] + fn test_gradient_axis_order_2d() { + let mut grid = GridMap::::new([3, 9], &GridMapConfig::default()).unwrap(); + for y in 0..9 { + for x in 0..3 { + grid.set_log_odds(&Point2::new(x, y as isize), 0.5 * x as f64) + .unwrap(); + } + } + + let sample = grid.sample_log_odds(&Point2::new(1.5, 4.5)).unwrap(); + assert!((sample.gradient[0] - 0.5).abs() < 1e-12); + assert!(sample.gradient[1].abs() < 1e-12); + } + + /// Interpolation needs a neighbour on the far side, so the usable domain stops at + /// `dimensions[axis] - 1`, inclusive. The final row contributes only its low corner. + #[test] + fn test_sample_at_upper_domain_edge_is_inclusive() { + let grid = noisy_map([4usize, 4], 0x6666); + + assert!(grid.sample_log_odds(&Point2::new(3.0, 3.0)).is_some()); + assert!(grid.sample_log_odds(&Point2::new(3.0001, 3.0)).is_none()); + assert!(grid.sample_log_odds(&Point2::new(3.0, 3.0001)).is_none()); + assert!(grid.sample_log_odds(&Point2::new(4.0, 1.0)).is_none()); + } + + #[test] + fn test_sample_at_upper_domain_edge_is_inclusive_3d() { + let grid = noisy_map([4usize, 5, 6], 0x7777); + + assert!(grid.sample_log_odds(&Point3::new(3.0, 4.0, 5.0)).is_some()); + assert!( + grid.sample_log_odds(&Point3::new(3.0, 4.0, 5.0001)) + .is_none() + ); + } + + /// `f64::NAN as usize` is zero, so an unchecked cast would sample the map origin and return a + /// plausible number for a meaningless query. + #[test] + fn test_sample_rejects_non_finite_coordinate() { + let grid = noisy_map([8usize, 8], 0x8888); + + for point in [ + Point2::new(f64::NAN, 2.0), + Point2::new(2.0, f64::NAN), + Point2::new(f64::INFINITY, 2.0), + Point2::new(2.0, f64::NEG_INFINITY), + ] { + assert!(grid.sample_log_odds(&point).is_none(), "{point:?}"); + assert!(grid.sample_probability(&point).is_none(), "{point:?}"); + } + } + + #[test] + fn test_sample_rejects_non_finite_coordinate_3d() { + let grid = noisy_map([6usize, 6, 6], 0x9999); + assert!( + grid.sample_log_odds(&Point3::new(1.0, f64::NAN, 1.0)) + .is_none() + ); + } + + /// Truncation toward zero would fold any coordinate in `(-1, 0)` into cell zero, so a point + /// just off the low corner must be rejected rather than silently clamped inward. + #[test] + fn test_sample_rejects_negative_coordinate() { + let grid = noisy_map([8usize, 8], 0xAAAA); + + assert!(grid.sample_log_odds(&Point2::new(-0.5, 2.0)).is_none()); + assert!(grid.sample_log_odds(&Point2::new(2.0, -0.5)).is_none()); + assert!(grid.sample_log_odds(&Point2::new(-1e-9, 2.0)).is_none()); + assert!(grid.sample_log_odds(&Point2::new(0.0, 2.0)).is_some()); + } + + #[test] + fn test_sample_probability_applies_the_chain_rule() { + let grid = noisy_map([8usize, 8], 0xBBBB); + let mut rng = Lcg(0xCCCC); + + for _ in 0..500 { + let point = Point2::new(rng.next_unit() * 7.0, rng.next_unit() * 7.0); + let log_odds = grid.sample_log_odds(&point).unwrap(); + let probability = grid.sample_probability(&point).unwrap(); + + let expected = 1.0 / (1.0 + (-log_odds.value).exp()); + assert!((probability.value - expected).abs() < 1e-12); + + // dP/dx = P * (1 - P) * dl/dx + let scale = expected * (1.0 - expected); + assert!((probability.gradient - log_odds.gradient * scale).norm() < 1e-12); + } + } + + #[test] + fn test_sample_probability_gradient_matches_central_differences() { + let grid = noisy_map([9usize, 9], 0xDDDD); + let step = 1e-5; + let mut rng = Lcg(0xEEEE); + + for _ in 0..300 { + let point = Point2::new(1.0 + rng.next_unit() * 6.0, 1.0 + rng.next_unit() * 6.0); + let sample = grid.sample_probability(&point).unwrap(); + + for axis in 0..2 { + let (mut low, mut high) = (point, point); + low[axis] -= step; + high[axis] += step; + let numeric = (grid.sample_probability(&high).unwrap().value + - grid.sample_probability(&low).unwrap().value) + / (2.0 * step); + + assert!( + (sample.gradient[axis] - numeric).abs() < 1e-6, + "axis {axis}" + ); + } + } + } + + #[test] + fn test_sample_probability_3d_matches_logistic() { + let grid = noisy_map([5usize, 5, 5], 0xF0F0); + let point = Point3::new(1.25, 2.5, 3.75); + + let log_odds = grid.sample_log_odds(&point).unwrap(); + let probability = grid.sample_probability(&point).unwrap(); + + let expected = 1.0 / (1.0 + (-log_odds.value).exp()); + assert!((probability.value - expected).abs() < 1e-12); + assert!(probability.value > 0.0 && probability.value < 1.0); + } + + /// An unwritten map is uniform, so every gradient must vanish exactly. + #[test] + fn test_gradient_is_zero_in_a_uniform_field() { + let grid = GridMap::::new([8, 8], &GridMapConfig::default()).unwrap(); + let sample = grid.sample_log_odds(&Point2::new(3.25, 4.75)).unwrap(); + + assert_eq!(sample.value, 0.0); + assert_eq!(sample.gradient, SVector::::zeros()); + assert_eq!( + grid.sample_probability(&Point2::new(3.25, 4.75)) + .unwrap() + .value, + 0.5 + ); + } + + /// A sample sitting exactly on the last column of an *interior* row is the dangerous case: + /// the neighbour gather would still succeed, having quietly walked into the next row, and + /// return a plausible value with a gradient built from a cell that is nowhere near the query. + /// The far corner merely fails; this one lies. + #[test] + fn test_upper_edge_does_not_borrow_the_next_row() { + let mut grid = GridMap::::new([4, 4], &GridMapConfig::default()).unwrap(); + + // A field varying along x alone, so any leakage across a row boundary is unmistakable. + for y in 0..4 { + for x in 0..4 { + grid.set_log_odds(&Point2::new(x, y), f64::from(x as i32)) + .unwrap(); + } + } + + let sample = grid.sample_log_odds(&Point2::new(3.0, 1.0)).unwrap(); + + assert_eq!( + sample.value, 3.0, + "the value must be the cell itself, not a blend with the next row" + ); + assert!( + (sample.gradient[0] - 1.0).abs() < 1e-12, + "the x gradient must come from cells (2,1) and (3,1); borrowing (0,2) would give -3" + ); + assert!(sample.gradient[1].abs() < 1e-12); + + // The same must hold along the other axis, and at the far corner of the map. + let corner = grid.sample_log_odds(&Point2::new(3.0, 3.0)).unwrap(); + assert_eq!(corner.value, 3.0); + assert!((corner.gradient[0] - 1.0).abs() < 1e-12); + } + + #[test] + fn test_upper_edge_does_not_borrow_the_next_slab_3d() { + let mut grid = GridMap::::new([3, 3, 3], &GridMapConfig::default()).unwrap(); + for z in 0..3 { + for y in 0..3 { + for x in 0..3 { + grid.set_log_odds(&Point3::new(x, y, z), f64::from(y as i32)) + .unwrap(); + } + } + } + + let sample = grid.sample_log_odds(&Point3::new(1.0, 2.0, 1.0)).unwrap(); + assert_eq!(sample.value, 2.0); + assert!(sample.gradient[0].abs() < 1e-12); + assert!((sample.gradient[1] - 1.0).abs() < 1e-12); + assert!(sample.gradient[2].abs() < 1e-12); + } +} diff --git a/crates/mapping-suites/src/hector_mapper/grid_map/scan.rs b/crates/mapping-suites/src/hector_mapper/grid_map/scan.rs new file mode 100644 index 0000000..9523556 --- /dev/null +++ b/crates/mapping-suites/src/hector_mapper/grid_map/scan.rs @@ -0,0 +1,873 @@ +// SPDX-License-Identifier: MIT +/* + * Copyright (c) [2023 - Present] Emily Matheys + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +use mapping_algorithms::lines::{BresenhamError, BresenhamLine}; +use nalgebra::{ComplexField, Point, RealField}; +use num_traits::{AsPrimitive, ConstOne, ConstZero}; + +use super::{GridMap, GridMapError, GridMapResult, RayTermination, ScanBeam}; +use crate::Borrow; + +/// A scan-scoped update session over a [`GridMap`], from [`GridMap::begin_scan`]. +/// +/// Each cell absorbs one update per scan at most, and an occupied observation beats a free one. +/// +/// # Generics +/// * `T`: Either an [`prim@f32`] or [`prim@f64`]. +/// * `N`: a usize, representing the number of dimensions. +#[derive(Debug)] +pub(crate) struct ScanUpdater<'a, T, const N: usize> { + map: &'a mut GridMap, +} + +impl<'a, T, const N: usize> ScanUpdater<'a, T, N> +where + T: AsPrimitive + AsPrimitive + ConstOne + ConstZero + Copy + RealField, + usize: AsPrimitive, +{ + /// Wraps a map whose generation counter has already been advanced. + /// + /// # Arguments + /// * `map`: the map to borrow for the duration of the scan. + /// + /// # Returns + /// A [`ScanUpdater`] borrowing `map`. + pub(super) fn new(map: &'a mut GridMap) -> Self { + Self { map } + } + + /// Records that a beam passed through a cell. + /// + /// # Arguments + /// * `index`: the cell observed as free. + /// + /// # Errors + /// * [`GridMapError::OutOfBounds`]: naming the first offending axis. Use + /// [`update_ray`](Self::update_ray) for geometry expected to leave the map, which clips instead. + pub(crate) fn mark_free(&mut self, index: &Point) -> GridMapResult<()> { + self.map.mark_free_at(index)?; + Ok(()) + } + + /// Records that a beam terminated in a cell, retracting any free update from this same scan. + /// + /// # Arguments + /// * `index`: the cell observed as occupied. + /// + /// # Errors + /// * [`GridMapError::OutOfBounds`]: naming the first offending axis. + pub(crate) fn mark_occupied(&mut self, index: &Point) -> GridMapResult<()> { + self.map.mark_occupied_at(index)?; + Ok(()) + } + + /// Integrates one range measurement, marking the cells it crosses free and applying the beam's + /// termination to the cell it stops in. + /// + /// The segment is clipped to the map first, so a beam aimed far outside costs no more than one + /// crossing it. Allocates nothing. + /// + /// # Arguments + /// * `beam`: the measurement to integrate, by value or by reference. + /// + /// # Generics + /// * `B`: any [`Borrow>`](Borrow). + /// + /// # Returns + /// A [`prim@usize`], the number of cells updated; zero if the beam missed the map. + /// + /// # Errors + /// * [`GridMapError::Ray`]: if either point carries a NaN or infinite coordinate. + #[cfg_attr( + feature = "tracing", + tracing::instrument("Update Ray", skip_all, level = "trace") + )] + pub(crate) fn update_ray>>( + &mut self, + beam: B, + ) -> GridMapResult { + let ScanBeam { + origin, + endpoint, + termination, + } = beam.borrow(); + + // Checked up front rather than left to the plotter: clipping an infinite endpoint would + // silently collapse the beam to zero length instead of reporting the bad input. + for axis in 0..N { + if !ComplexField::is_finite(&origin[axis]) || !ComplexField::is_finite(&endpoint[axis]) + { + return Err(GridMapError::Ray(BresenhamError::NonFiniteCoordinate)); + } + } + + let Some((entry, exit, endpoint_is_inside)) = self.map.clip_segment(origin, endpoint) + else { + return Ok(0); + }; + + let mut line = BresenhamLine::::plotter(entry, exit)?; + // Hold one cell behind, so the last cell falls out of the loop without needing the line's + // length or a second pass over it. + let Some(mut previous) = line.next() else { + return Ok(0); + }; + + let mut updated = 0; + for cell in line { + updated += usize::from(self.map.mark_free_at(&previous).unwrap_or(false)); + previous = cell; + } + + // A clipped endpoint is not where the beam really stopped, so it is evidence of free space + // only; treating it as a return would plant a phantom obstacle on the map boundary. + updated += usize::from( + match termination { + RayTermination::Hit if endpoint_is_inside => self.map.mark_occupied_at(&previous), + _ => self.map.mark_free_at(&previous), + } + .unwrap_or(false), + ); + + Ok(updated) + } + + /// Integrates a whole scan, as [`update_ray`](Self::update_ray) does for each beam. + /// + /// The beams need not share an origin. Taking each by [`Borrow`] lets a stored scan be + /// integrated without being consumed. + /// + /// # Arguments + /// * `beams`: the measurements to integrate. + /// + /// # Generics + /// * `B`: any [`Borrow>`](Borrow). + /// * `I`: any [`IntoIterator`] over those beams. + /// + /// # Returns + /// A [`prim@usize`], the total number of cells updated. + /// + /// # Errors + /// * [`GridMapError::Ray`]: for the first beam carrying a non-finite coordinate. Beams before it + /// have already been applied. + #[cfg_attr( + feature = "tracing", + tracing::instrument("Update Scan", skip_all, level = "debug") + )] + pub(crate) fn update_scan(&mut self, beams: I) -> GridMapResult + where + B: Borrow>, + I: IntoIterator, + { + beams + .into_iter() + .try_fold(0usize, |updated, beam| Ok(updated + self.update_ray(beam)?)) + } +} + +#[cfg(test)] +mod tests { + use super::super::GridMapConfig; + use super::*; + use crate::Vec; + use nalgebra::Point2; + + fn map(dimensions: [usize; 2]) -> GridMap { + GridMap::new(dimensions, &GridMapConfig::default()).unwrap() + } + + /// The log-odds increments the default sensor model produces. + fn increments(grid: &GridMap) -> (f32, f32) { + (grid.occupied_delta, grid.free_delta) + } + + fn beam( + origin: Point2, + endpoint: Point2, + termination: RayTermination, + ) -> ScanBeam { + ScanBeam { + origin, + endpoint, + termination, + } + } + + #[test] + fn test_begin_scan_advances_the_generation() { + let mut grid = map([8, 8]); + let first = grid.frame; + + drop(grid.begin_scan()); + assert_eq!(grid.frame, first + 1); + + drop(grid.begin_scan()); + assert_eq!(grid.frame, first + 2); + } + + /// A scan crosses the same cell from many beams. Counting each crossing would make the map + /// confident in proportion to beam density rather than to evidence. + #[test] + fn test_repeated_mark_free_in_one_scan_applies_once() { + let mut grid = map([8, 8]); + let (_, free) = increments(&grid); + let cell = Point2::new(3, 3); + + let mut scan = grid.begin_scan(); + for _ in 0..10 { + scan.mark_free(&cell).unwrap(); + } + + assert!((grid.log_odds_at(&cell).unwrap() - free).abs() < 1e-6); + } + + #[test] + fn test_repeated_mark_occupied_in_one_scan_applies_once() { + let mut grid = map([8, 8]); + let (occupied, _) = increments(&grid); + let cell = Point2::new(3, 3); + + let mut scan = grid.begin_scan(); + for _ in 0..10 { + scan.mark_occupied(&cell).unwrap(); + } + + assert!((grid.log_odds_at(&cell).unwrap() - occupied).abs() < 1e-6); + } + + #[test] + fn test_mark_free_after_occupied_in_same_scan_is_ignored() { + let mut grid = map([8, 8]); + let (occupied, _) = increments(&grid); + let cell = Point2::new(2, 2); + + let mut scan = grid.begin_scan(); + scan.mark_occupied(&cell).unwrap(); + scan.mark_free(&cell).unwrap(); + + assert!((grid.log_odds_at(&cell).unwrap() - occupied).abs() < 1e-6); + } + + /// A beam passing through a cell, then another terminating in it, must leave the cell exactly + /// as though only the return had been seen. + #[test] + fn test_mark_occupied_after_free_in_same_scan_nets_to_occupied_only() { + let mut grid = map([8, 8]); + let (occupied, _) = increments(&grid); + let cell = Point2::new(2, 2); + + let mut scan = grid.begin_scan(); + scan.mark_free(&cell).unwrap(); + scan.mark_occupied(&cell).unwrap(); + + assert!((grid.log_odds_at(&cell).unwrap() - occupied).abs() < 1e-6); + } + + /// The stamps exist so that a scan's outcome does not depend on the order its beams happened + /// to arrive in. A saturated cell is where that guarantee used to fail: the free update is + /// clamped away entirely, and retracting the full increment afterwards credited back evidence + /// the cell never absorbed, leaving `free`-then-`occupied` a whole `free_delta` above + /// `occupied`-then-`free`. + #[test] + fn test_occupied_after_free_at_the_floor_is_order_independent() { + let cell = Point2::new(1, 1); + let floor = *map([4, 4]).log_odds_bounds().start(); + + let saturated = || { + let mut grid = map([4, 4]); + grid.set_log_odds(&cell, floor).unwrap(); + grid + }; + + let mut free_then_occupied = saturated(); + { + let mut scan = free_then_occupied.begin_scan(); + scan.mark_free(&cell).unwrap(); + scan.mark_occupied(&cell).unwrap(); + } + + let mut occupied_then_free = saturated(); + { + let mut scan = occupied_then_free.begin_scan(); + scan.mark_occupied(&cell).unwrap(); + scan.mark_free(&cell).unwrap(); + } + + let mut occupied_alone = saturated(); + occupied_alone.begin_scan().mark_occupied(&cell).unwrap(); + + let (first, second, alone) = ( + free_then_occupied.log_odds_at(&cell).unwrap(), + occupied_then_free.log_odds_at(&cell).unwrap(), + occupied_alone.log_odds_at(&cell).unwrap(), + ); + + assert!( + (first - second).abs() < 1e-6, + "beam order changed the result: {first} against {second}" + ); + assert!( + (first - alone).abs() < 1e-6, + "a cell passed through and then terminated in must read as though only the return \ + had been seen: {first} against {alone}" + ); + } + + /// What is recoverable off the floor. Within one `free_delta` of it the clamp has eaten an + /// unknown part of the free update, so the retraction cannot be exact; it must still never + /// credit back more than the cell absorbed, and never lose more than the increment itself. + #[test] + fn test_retraction_never_over_credits_a_clamped_free_update() { + let cell = Point2::new(1, 1); + let grid = map([4, 4]); + let (_, free) = increments(&grid); + let floor = *grid.log_odds_bounds().start(); + + for step in 0..=40u8 { + let pre = floor + (f32::from(step) / 40.0) * -floor; + + let mut free_then_occupied = map([4, 4]); + free_then_occupied.set_log_odds(&cell, pre).unwrap(); + { + let mut scan = free_then_occupied.begin_scan(); + scan.mark_free(&cell).unwrap(); + scan.mark_occupied(&cell).unwrap(); + } + + let mut occupied_alone = map([4, 4]); + occupied_alone.set_log_odds(&cell, pre).unwrap(); + occupied_alone.begin_scan().mark_occupied(&cell).unwrap(); + + let (both, alone) = ( + free_then_occupied.log_odds_at(&cell).unwrap(), + occupied_alone.log_odds_at(&cell).unwrap(), + ); + + assert!( + both <= alone + 1e-6, + "pre {pre}: a retracted free update must never leave the cell more occupied \ + than a lone return would: {both} against {alone}" + ); + assert!( + both >= alone + free - 1e-6, + "pre {pre}: nor lose more than the free increment itself: {both} against {alone}" + ); + // Clear of the floor by a full increment, nothing was clamped and it is exact. + if pre >= floor - free { + assert!( + (both - alone).abs() < 1e-6, + "pre {pre}: nothing was clamped here, so the retraction must be exact" + ); + } + } + } + + #[test] + fn test_same_cell_in_two_scans_applies_twice() { + let mut grid = map([8, 8]); + let (_, free) = increments(&grid); + let cell = Point2::new(1, 1); + + for _ in 0..2 { + grid.begin_scan().mark_free(&cell).unwrap(); + } + + assert!((grid.log_odds_at(&cell).unwrap() - free * 2.0).abs() < 1e-6); + } + + /// Without the sweep, a stamp left by an old generation would compare equal to the reused + /// counter and that cell would silently stop updating. + #[test] + fn test_generation_wraparound_sweeps_stamps() { + let mut grid = map([8, 8]); + let (_, free) = increments(&grid); + let cell = Point2::new(4, 4); + + // Stamp the cell during generation 1. + grid.force_frame(1); + grid.begin_scan().mark_free(&cell).unwrap(); + let after_first = grid.log_odds_at(&cell).unwrap(); + assert!(grid.last_frame_to_update.iter().any(|&stamp| stamp != 0)); + + // Wrap: the counter returns to 1, so every stale stamp must be cleared. + grid.force_frame(u32::MAX >> 1); + let mut scan = grid.begin_scan(); + scan.mark_free(&cell).unwrap(); + + assert_eq!(grid.frame, 1); + assert!( + (grid.log_odds_at(&cell).unwrap() - (after_first + free)).abs() < 1e-6, + "the cell must still update after the counter wraps" + ); + } + + #[test] + fn test_mark_out_of_bounds_reports_the_axis() { + let mut grid = map([4, 4]); + let mut scan = grid.begin_scan(); + + assert_eq!( + scan.mark_free(&Point2::new(4, 0)).unwrap_err(), + GridMapError::OutOfBounds { + axis: 0, + index: 4, + extent: 4 + } + ); + assert_eq!( + scan.mark_occupied(&Point2::new(0, -1)).unwrap_err(), + GridMapError::OutOfBounds { + axis: 1, + index: -1, + extent: 4 + } + ); + } + + #[test] + fn test_repeated_updates_saturate_at_the_bounds() { + let mut grid = map([4, 4]); + let bounds = grid.log_odds_bounds(); + let (occupied, free) = (Point2::new(1, 1), Point2::new(2, 2)); + + for _ in 0..1000 { + let mut scan = grid.begin_scan(); + scan.mark_occupied(&occupied).unwrap(); + scan.mark_free(&free).unwrap(); + } + + assert_eq!(grid.log_odds_at(&occupied), Some(*bounds.end())); + assert_eq!(grid.log_odds_at(&free), Some(*bounds.start())); + } + + /// The regression test for an unbounded free cell. Without a lower clamp, ten thousand free + /// observations leave a cell at around -4000, needing thousands of contrary hits to recover - + /// a permanently blind cell, which defeats the point of a probabilistic grid. + #[test] + fn test_saturated_cell_recovers_under_opposite_updates() { + let mut grid = map([4, 4]); + let cell = Point2::new(1, 1); + + for _ in 0..10_000 { + grid.begin_scan().mark_free(&cell).unwrap(); + } + assert_eq!( + grid.log_odds_at(&cell), + Some(*grid.log_odds_bounds().start()) + ); + + let mut scans = 0; + while grid.probability_at(&cell).unwrap() <= 0.5 { + grid.begin_scan().mark_occupied(&cell).unwrap(); + scans += 1; + assert!(scans < 20, "a saturated cell must recover promptly"); + } + + assert_eq!( + scans, 5, + "ceil(3.476 / 0.847) observations to cross one half" + ); + } + + #[test] + fn test_updater_map_reflects_in_progress_updates() { + let mut grid = map([8, 8]); + let cell = Point2::new(3, 3); + + let mut scan = grid.begin_scan(); + scan.mark_occupied(&cell).unwrap(); + + assert!( + scan.map.log_odds_at(&cell).unwrap() > 0.0, + "a read through the guard must see the scan so far" + ); + } + + #[test] + fn test_update_ray_marks_free_and_endpoint_occupied() { + let mut grid = map([16, 16]); + + let updated = grid + .begin_scan() + .update_ray(beam( + Point2::new(0.5, 0.5), + Point2::new(5.5, 0.5), + RayTermination::Hit, + )) + .unwrap(); + + assert_eq!(updated, 6); + for x in 0..5 { + assert!( + grid.log_odds_at(&Point2::new(x, 0)).unwrap() < 0.0, + "cell {x} along the beam should read as free" + ); + } + assert!( + grid.log_odds_at(&Point2::new(5, 0)).unwrap() > 0.0, + "the cell the beam terminated in should read as occupied" + ); + } + + /// A beam that ran to its maximum range without a return is evidence of free space at its far + /// end. Marking it occupied would plant a phantom obstacle at the sensor's range limit. + #[test] + fn test_max_range_beam_marks_endpoint_free() { + let mut grid = map([16, 16]); + + grid.begin_scan() + .update_ray(beam( + Point2::new(0.5, 0.5), + Point2::new(5.5, 0.5), + RayTermination::MaxRange, + )) + .unwrap(); + + assert!( + grid.log_odds_at(&Point2::new(5, 0)).unwrap() < 0.0, + "no obstacle may be recorded where the beam simply ran out" + ); + } + + #[test] + fn test_update_ray_zero_length_marks_only_the_endpoint() { + let mut grid = map([16, 16]); + let point = Point2::new(4.5, 4.5); + + let updated = grid + .begin_scan() + .update_ray(beam(point, point, RayTermination::Hit)) + .unwrap(); + + assert_eq!(updated, 1); + assert!(grid.log_odds_at(&Point2::new(4, 4)).unwrap() > 0.0); + } + + #[test] + fn test_update_ray_clips_at_the_map_edge() { + let mut grid = map([8, 8]); + + let updated = grid + .begin_scan() + .update_ray(beam( + Point2::new(0.5, 0.5), + Point2::new(40.5, 0.5), + RayTermination::Hit, + )) + .unwrap(); + + assert!( + updated <= 9, + "a clipped beam must not update more cells than the map holds along that axis" + ); + assert!(grid.log_odds_at(&Point2::new(7, 0)).unwrap() != 0.0); + } + + /// The endpoint of a clipped beam is not where the beam really stopped, so it carries no + /// evidence of occupancy - the same phantom-obstacle failure as a max-range return. + #[test] + fn test_clipped_hit_endpoint_is_marked_free_not_occupied() { + let mut grid = map([8, 8]); + + grid.begin_scan() + .update_ray(beam( + Point2::new(0.5, 4.5), + Point2::new(40.5, 4.5), + RayTermination::Hit, + )) + .unwrap(); + + for x in 0..8 { + assert!( + grid.log_odds_at(&Point2::new(x, 4)).unwrap() < 0.0, + "cell {x} is on the clipped run and must read free, obstacle-free" + ); + } + } + + /// A beam lying along the upper face is off the map, and the projection that keeps clipped + /// beams addressable must not be allowed to drag it back on. + #[test] + fn test_beam_on_the_upper_face_writes_nothing() { + let mut grid = map([8, 8]); + + let updated = grid + .begin_scan() + .update_ray(beam( + Point2::new(2.0, 8.0), + Point2::new(5.0, 8.0), + RayTermination::MaxRange, + )) + .unwrap(); + + assert_eq!(updated, 0); + assert!( + grid.odds.iter().all(|&odds| odds == 0.0), + "row 7 must not absorb a beam that travelled along row 8" + ); + } + + /// The phantom obstacle again, reached by the one route the clipped-endpoint rule missed: an + /// endpoint of exactly `extent` is never clipped, so it used to be reported as a genuine + /// return and marked the boundary cell occupied. + #[test] + fn test_hit_endpoint_on_the_upper_face_is_marked_free_not_occupied() { + let mut grid = map([8, 8]); + + grid.begin_scan() + .update_ray(beam( + Point2::new(4.5, 4.5), + Point2::new(8.0, 4.5), + RayTermination::Hit, + )) + .unwrap(); + + assert!( + grid.log_odds_at(&Point2::new(7, 4)).unwrap() < 0.0, + "the beam stopped in cell 8, off the map; cell 7 saw it pass through and nothing more" + ); + } + + #[test] + fn test_update_ray_entirely_outside_returns_ok_zero() { + let mut grid = map([8, 8]); + + let updated = grid + .begin_scan() + .update_ray(beam( + Point2::new(-50.0, -50.0), + Point2::new(-40.0, -40.0), + RayTermination::Hit, + )) + .unwrap(); + + assert_eq!(updated, 0); + assert!(grid.odds.iter().all(|&odds| odds == 0.0)); + } + + #[test] + fn test_update_ray_rejects_non_finite_coordinates() { + let mut grid = map([8, 8]); + let mut scan = grid.begin_scan(); + let good = Point2::new(1.0, 1.0); + + for bad in [ + Point2::new(f32::NAN, 1.0), + Point2::new(1.0, f32::NAN), + Point2::new(f32::INFINITY, 1.0), + Point2::new(1.0, f32::NEG_INFINITY), + ] { + assert_eq!( + scan.update_ray(beam(good, bad, RayTermination::Hit)) + .unwrap_err(), + GridMapError::Ray(BresenhamError::NonFiniteCoordinate), + "endpoint {bad:?}" + ); + assert_eq!( + scan.update_ray(beam(bad, good, RayTermination::Hit)) + .unwrap_err(), + GridMapError::Ray(BresenhamError::NonFiniteCoordinate), + "origin {bad:?}" + ); + } + + assert!( + grid.odds.iter().all(|&odds| odds == 0.0), + "a rejected beam must not have written anything, least of all to the origin cell" + ); + } + + /// A step count is derived from a beam's length, and a merely absurd endpoint is never + /// rejected. Without clipping the segment first, this beam would walk a billion cells to throw + /// every one of them away. + #[test] + fn test_far_endpoint_does_not_walk_unbounded() { + let mut grid = map([16, 16]); + + let updated = grid + .begin_scan() + .update_ray(beam( + Point2::new(8.5, 8.5), + Point2::new(1.0e9, 8.5), + RayTermination::Hit, + )) + .unwrap(); + + assert!( + updated <= 32, + "the walk must be bounded by the map, not by the caller's endpoint: {updated}" + ); + } + + /// Both entry points take the beam by [`Borrow`], so a stored scan is integrated without + /// being consumed. `update_scan(&beams)` also drives `update_ray` with a `&ScanBeam`. + #[test] + fn test_beams_are_accepted_by_value_and_by_reference() { + let mut grid = map([16, 16]); + let beams = Vec::from([beam( + Point2::new(0.5, 0.5), + Point2::new(5.5, 0.5), + RayTermination::Hit, + )]); + + assert_eq!(grid.begin_scan().update_scan(&beams).unwrap(), 6); + assert_eq!(grid.begin_scan().update_ray(beams[0]).unwrap(), 6); + // Consuming the scan is still allowed; each fresh scan rewrites the same six cells. + assert_eq!(grid.begin_scan().update_scan(beams).unwrap(), 6); + } + + #[test] + fn test_update_scan_sums_updated_cells() { + let mut grid = map([16, 16]); + let origin = Point2::new(0.5, 0.5); + // One origin borrowed across every beam; only the endpoints are owned. + let beams = Vec::from([ + beam(origin, Point2::new(5.5, 0.5), RayTermination::Hit), + beam(origin, Point2::new(0.5, 5.5), RayTermination::Hit), + ]); + + let updated = grid.begin_scan().update_scan(beams).unwrap(); + + // Six cells each, less the shared origin cell, which is deduplicated across the scan. + assert_eq!(updated, 11); + } + + #[test] + fn test_update_scan_dedups_overlapping_beams() { + let mut grid = map([16, 16]); + let (_, free) = increments(&grid); + let origin = Point2::new(0.5, 0.5); + + // Two collinear beams of different lengths, so the shorter one's cells are all revisited. + let beams = Vec::from([ + beam(origin, Point2::new(4.5, 0.5), RayTermination::MaxRange), + beam(origin, Point2::new(8.5, 0.5), RayTermination::MaxRange), + ]); + grid.begin_scan().update_scan(beams).unwrap(); + + for x in 0..4 { + assert!( + (grid.log_odds_at(&Point2::new(x, 0)).unwrap() - free).abs() < 1e-6, + "cell {x} was crossed twice but must be counted once" + ); + } + } + + #[test] + fn test_update_scan_reports_the_first_non_finite_beam() { + let mut grid = map([16, 16]); + let origin = Point2::new(0.5, 0.5); + let beams = Vec::from([ + beam(origin, Point2::new(5.5, 0.5), RayTermination::Hit), + beam(origin, Point2::new(f32::NAN, 0.5), RayTermination::Hit), + beam(origin, Point2::new(0.5, 5.5), RayTermination::Hit), + ]); + + assert_eq!( + grid.begin_scan().update_scan(beams).unwrap_err(), + GridMapError::Ray(BresenhamError::NonFiniteCoordinate) + ); + assert!( + grid.log_odds_at(&Point2::new(5, 0)).unwrap() > 0.0, + "beams preceding the failure have already been applied" + ); + } + + #[test] + fn test_clipped_beam_still_covers_the_last_cell_in_the_map() { + let mut grid = map([8, 8]); + + grid.begin_scan() + .update_ray(beam( + Point2::new(0.5, 2.5), + Point2::new(99.5, 2.5), + RayTermination::MaxRange, + )) + .unwrap(); + + // Every cell of the row, including the last, must have been visited: pulling the clipped + // exit back into cell 7 is what stops the plotter's endpoint substitution from skipping it. + for x in 0..8 { + assert!( + grid.log_odds_at(&Point2::new(x, 2)).unwrap() < 0.0, + "cell {x} of the clipped row was never visited" + ); + } + } + + /// An end-to-end pass: sweep a synthetic wall, then confirm the map reads the way a scan + /// matcher would need it to. + #[test] + fn test_synthetic_scan_builds_a_readable_wall() { + let mut grid = GridMap::::new([256, 256], &GridMapConfig::default()).unwrap(); + let origin = Point2::new(128.5, 128.5); + let wall_x = 200.5; + + let beams = (118..=138) + .map(|y| { + beam( + origin, + Point2::new(wall_x, y as f32 + 0.5), + RayTermination::Hit, + ) + }) + .collect::>(); + + // Ten sweeps of a flat wall, so the cells there saturate well clear of one half. The scan + // is integrated by reference, so it is built once rather than per sweep. + for _ in 0..10 { + grid.begin_scan().update_scan(&beams).unwrap(); + } + + // The wall reads as occupied. + assert!( + grid.probability_at(&Point2::new(200, 128)).unwrap() > 0.9, + "the wall should be confidently occupied" + ); + // The space the beams crossed reads as free. + assert!( + grid.probability_at(&Point2::new(160, 128)).unwrap() < 0.1, + "the swept space should be confidently free" + ); + // Everything the sensor never saw is still unknown, including behind the wall. + assert_eq!(grid.probability_at(&Point2::new(230, 128)), Some(0.5)); + assert_eq!(grid.probability_at(&Point2::new(60, 60)), Some(0.5)); + + // Approaching the wall from the free side, the log-odds field must rise toward it, which + // is the signal a Gauss-Newton scan matcher follows. + let sample = grid.sample_log_odds(&Point2::new(199.5, 128.5)).unwrap(); + assert!( + sample.gradient[0] > 0.0, + "the gradient must point toward the wall, not away from it" + ); + assert!( + sample.gradient[1].abs() < sample.gradient[0], + "a wall parallel to y should produce almost no gradient along it" + ); + + // The probability field agrees in direction, being a monotone map of the log-odds one. + let probability = grid.sample_probability(&Point2::new(199.5, 128.5)).unwrap(); + assert!(probability.gradient[0] > 0.0); + assert!(probability.value > 0.0 && probability.value < 1.0); + } +} diff --git a/crates/mapping-suites/src/hector_mapper/grid_map/types.rs b/crates/mapping-suites/src/hector_mapper/grid_map/types.rs new file mode 100644 index 0000000..0cb379a --- /dev/null +++ b/crates/mapping-suites/src/hector_mapper/grid_map/types.rs @@ -0,0 +1,565 @@ +// SPDX-License-Identifier: MIT +/* + * Copyright (c) [2023 - Present] Emily Matheys + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +use mapping_algorithms::lines::BresenhamError; +use nalgebra::{Point, RealField, SVector, Scalar}; +use num_traits::{AsPrimitive, ConstOne, ConstZero}; + +use crate::fmt; + +/// The errors that can arise while building or updating an occupancy grid. +/// +/// Also derives [`thiserror::Error`] under the `std` feature. +#[derive(Copy, Clone, Debug, Eq, PartialEq)] +#[cfg_attr(feature = "std", derive(thiserror::Error))] +pub enum GridMapError { + /// An axis was shorter than the two cells an interpolation stencil requires. + DimensionTooSmall { + /// The offending axis. + axis: usize, + /// The requested extent of that axis. + extent: usize, + }, + /// The product of the requested dimensions does not fit in a [`prim@usize`]. + CapacityOverflow, + /// The allocator could not provide storage for the requested number of cells. + AllocationFailed { + /// The number of cells whose allocation failed. + cells: usize, + }, + /// A cell index fell outside the map. Axes are checked in ascending order, and only the first + /// failing one is reported. + OutOfBounds { + /// The offending axis. + axis: usize, + /// The offending index. + index: isize, + /// The exclusive upper bound of that axis. + extent: usize, + }, + /// A coordinate was NaN or infinite. + NonFiniteCoordinate { + /// The offending axis. + axis: usize, + }, + /// The occupied probability was not strictly between `0.5` and `1.0`. + InvalidOccupiedProbability, + /// The free probability was not strictly between `0.0` and `0.5`. + InvalidFreeProbability, + /// The maximum confidence was not strictly between `0.5` and `1.0`. + InvalidMaxConfidence, + /// A beam could not be plotted. + Ray(BresenhamError), +} + +impl fmt::Display for GridMapError { + fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result { + match self { + Self::DimensionTooSmall { axis, extent } => write!( + f, + "axis {axis} has an extent of {extent}, but at least 2 cells are required" + ), + Self::CapacityOverflow => { + write!(f, "the requested dimensions overflow a usize") + } + Self::AllocationFailed { cells } => { + write!(f, "could not allocate storage for {cells} cells") + } + Self::OutOfBounds { + axis, + index, + extent, + } => write!( + f, + "index {index} on axis {axis} is outside the extent 0..{extent}" + ), + Self::NonFiniteCoordinate { axis } => { + write!(f, "the coordinate on axis {axis} is not finite") + } + Self::InvalidOccupiedProbability => { + write!(f, "the occupied probability must be between 0.5 and 1.0") + } + Self::InvalidFreeProbability => { + write!(f, "the free probability must be between 0.0 and 0.5") + } + Self::InvalidMaxConfidence => { + write!(f, "the maximum confidence must be between 0.5 and 1.0") + } + Self::Ray(err) => write!(f, "the beam could not be plotted: {err}"), + } + } +} + +impl From for GridMapError { + fn from(value: BresenhamError) -> Self { + Self::Ray(value) + } +} + +/// The result of a fallible grid map operation, carrying a [`GridMapError`] on failure. +pub type GridMapResult = Result; + +/// Why a beam stopped where it did. +#[derive(Copy, Clone, Debug, Eq, PartialEq)] +pub(crate) enum RayTermination { + /// The sensor received a return, so the final cell is evidence of occupancy. + Hit, + /// The beam reached its maximum range, so the final cell is evidence of free space. + MaxRange, +} + +/// One range measurement: where the beam started, where it stopped, and why. +/// +/// # Generics +/// * `T`: Either an [`prim@f32`] or [`prim@f64`]. Bounded by [`Scalar`], as [`Point`] itself is. +/// * `N`: a usize, representing the number of dimensions. +#[derive(Copy, Clone, Debug, PartialEq)] +pub(crate) struct ScanBeam { + /// The sensor position, in fractional cell coordinates. + pub origin: Point, + /// Where the beam stopped, in fractional cell coordinates. + pub endpoint: Point, + /// Whether the beam stopped on a return or at its maximum range. + pub termination: RayTermination, +} + +/// An interpolated value and its analytic spatial gradient, both from one `2^N`-corner gather. +/// +/// # Generics +/// * `T`: Either an [`prim@f32`] or [`prim@f64`]. +/// * `N`: a usize, representing the number of dimensions. +#[derive(Copy, Clone, Debug, PartialEq)] +pub(crate) struct MapSample { + /// The interpolated value; either log-odds, or a probability in the range `0.0..=1.0`. + pub value: T, + /// The gradient of [`value`](Self::value), in value per *cell*, not per metre. + pub gradient: SVector, +} + +impl MapSample { + /// The log-odds sample of an unobserved region. + /// + /// # Returns + /// A [`MapSample`] whose value and gradient are both zero. + pub(crate) fn unknown_log_odds() -> Self { + Self { + value: T::ZERO, + gradient: SVector::zeros(), + } + } + + /// The probability sample of an unobserved region. + /// + /// # Returns + /// A [`MapSample`] carrying a value of `0.5` and a zero gradient. + pub(crate) fn unknown_probability() -> Self { + Self { + value: T::ONE / (T::ONE + T::ONE), + gradient: SVector::zeros(), + } + } +} + +/// The inverse sensor model and saturation behaviour of a grid map. +/// +/// The values are validated when the map is built, not by the builder. +/// +/// # Generics +/// * `T`: Either an [`prim@f32`] or [`prim@f64`]. +#[derive(Clone, Copy, Debug, PartialEq)] +pub(crate) struct GridMapConfig { + /// `p(occupied | beam terminated here)`, strictly between `0.5` and `1.0`. + pub(crate) occupied_probability: T, + /// `p(occupied | beam passed through here)`, strictly between `0.0` and `0.5`. + pub(crate) free_probability: T, + /// The confidence at which a cell saturates, strictly between `0.5` and `1.0`. Saturating + /// keeps the map able to revise a long-held belief. + pub(crate) max_confidence: T, +} + +impl Default for GridMapConfig +where + f32: AsPrimitive, +{ + /// An occupied probability of `0.7`, a free probability of `0.4`, and saturation at `0.97`. + fn default() -> Self { + Self { + occupied_probability: 0.7.as_(), + free_probability: 0.4.as_(), + max_confidence: 0.97.as_(), + } + } +} + +impl GridMapConfig +where + f32: AsPrimitive, +{ + /// Returns a builder for the configuration struct. + /// + /// # Returns + /// A [`GridMapConfigBuilder`], pre-populated with the defaults. + pub(crate) fn builder() -> GridMapConfigBuilder { + GridMapConfigBuilder { + _internal: Self::default(), + } + } +} + +impl GridMapConfig { + /// Converts the sensor model into the log-odds increments a map stores. + /// + /// Taking an uninformative prior, each Bayesian update reduces to adding the inverse sensor + /// model's logit, so both increments are additions and the free one is negative. + /// + /// # Returns + /// The occupied increment, the free increment, and the saturation bound. + /// + /// # Errors + /// * [`GridMapError::InvalidOccupiedProbability`], [`GridMapError::InvalidFreeProbability`] or + /// [`GridMapError::InvalidMaxConfidence`]: if that value lies outside its open interval. + /// The intervals are open because zero and one are infinite in log-odds. + pub(crate) fn resolve(&self) -> GridMapResult<(T, T, T)> { + let half = T::ONE / (T::ONE + T::ONE); + + if !(self.occupied_probability > half && self.occupied_probability < T::ONE) { + return Err(GridMapError::InvalidOccupiedProbability); + } + + if !(self.free_probability > T::ZERO && self.free_probability < half) { + return Err(GridMapError::InvalidFreeProbability); + } + + if !(self.max_confidence > half && self.max_confidence < T::ONE) { + return Err(GridMapError::InvalidMaxConfidence); + } + + Ok(( + logit(self.occupied_probability), + logit(self.free_probability), + logit(self.max_confidence), + )) + } +} + +/// The log-odds of a probability, `ln(p / (1 - p))`. +/// +/// # Arguments +/// * `probability`: a value strictly between zero and one; either endpoint gives an infinity. +/// +/// # Generics +/// * `T`: Either an [`prim@f32`] or [`prim@f64`]. +/// +/// # Returns +/// The log-odds, positive above one half and negative below it. +#[inline] +pub(crate) fn logit(probability: T) -> T { + nalgebra::ComplexField::ln(probability / (T::ONE - probability)) +} + +/// A builder for [`GridMapConfig`]. +/// +/// # Generics +/// * `T`: Either an [`prim@f32`] or [`prim@f64`]. +#[derive(Clone, Copy, Debug, PartialEq)] +pub(crate) struct GridMapConfigBuilder { + _internal: GridMapConfig, +} + +impl GridMapConfigBuilder { + /// The probability that a cell is occupied, given that a beam terminated inside it. + /// + /// # Arguments + /// * `occupied_probability`: strictly between `0.5` and `1.0`. + /// + /// # Returns + /// A copy of self, with that value replaced. + pub(crate) fn with_occupied_probability(&self, occupied_probability: T) -> Self { + Self { + _internal: GridMapConfig { + occupied_probability, + ..self._internal + }, + } + } + + /// The probability that a cell is occupied, given that a beam passed through it. + /// + /// # Arguments + /// * `free_probability`: strictly between `0.0` and `0.5`. + /// + /// # Returns + /// A copy of self, with that value replaced. + pub(crate) fn with_free_probability(&self, free_probability: T) -> Self { + Self { + _internal: GridMapConfig { + free_probability, + ..self._internal + }, + } + } + + /// The confidence at which a cell saturates. + /// + /// # Arguments + /// * `max_confidence`: strictly between `0.5` and `1.0`. + /// + /// # Returns + /// A copy of self, with that value replaced. + pub(crate) fn with_max_confidence(&self, max_confidence: T) -> Self { + Self { + _internal: GridMapConfig { + max_confidence, + ..self._internal + }, + } + } + + /// Generates a [`GridMapConfig`] from the builder's current contents. + /// + /// # Returns + /// A [`GridMapConfig`]. Takes `&self`, so the builder stays intact for another use. + pub(crate) fn build(&self) -> GridMapConfig { + self._internal + } +} + +#[cfg(test)] +mod tests { + use super::*; + + #[test] + fn test_config_default_matches_builder_defaults() { + assert_eq!( + GridMapConfig::::builder().build(), + GridMapConfig::::default() + ); + } + + #[test] + fn test_config_builder_setters() { + let config = GridMapConfig::::builder() + .with_occupied_probability(0.8) + .with_free_probability(0.3) + .with_max_confidence(0.99) + .build(); + + assert_eq!(config.occupied_probability, 0.8); + assert_eq!(config.free_probability, 0.3); + assert_eq!(config.max_confidence, 0.99); + } + + #[test] + fn test_config_builder_does_not_consume_self() { + let builder = GridMapConfig::::builder().with_occupied_probability(0.8); + + // `build` takes &self, so a builder may be reused; both results must agree. + assert_eq!(builder.build(), builder.build()); + assert_eq!(builder.build().occupied_probability, 0.8); + } + + #[test] + fn test_logit_derivation() { + // ln(0.7 / 0.3), the increment a single occupied observation contributes. + assert!((logit(0.7f64) - 0.8472978603872034_f64).abs() < 1e-12); + // ln(0.4 / 0.6); negative, so the free update is an addition like the occupied one. + assert!((logit(0.4f64) - -0.4054651081081643_f64).abs() < 1e-12); + // A probability of one half carries no information at all. + assert_eq!(logit(0.5f64), 0.0); + } + + #[test] + fn test_resolve_produces_expected_increments() { + let (occupied, free, max) = GridMapConfig::::builder() + .with_occupied_probability(0.7) + .with_free_probability(0.4) + .with_max_confidence(0.97) + .build() + .resolve() + .unwrap(); + + assert!( + occupied > 0.0, + "an occupied observation must raise the odds" + ); + assert!(free < 0.0, "a free observation must lower the odds"); + assert!((occupied - 0.8472978603872034).abs() < 1e-12); + assert!((free - -0.4054651081081643).abs() < 1e-12); + assert!((max - 3.4760986898352724).abs() < 1e-12); + } + + /// The defaults are written as [`prim@f32`] literals and converted, matching + /// [`ICPConfiguration`](mapping_algorithms::point_clouds::ICPConfiguration), so a + /// [`prim@f64`] map's defaults carry single-precision rounding. That is a deliberate + /// consequence of the convention rather than a bug, but it should not drift unnoticed. + #[test] + fn test_defaults_carry_single_precision_rounding() { + let (occupied, _, _) = GridMapConfig::::default().resolve().unwrap(); + + assert!((occupied - 0.8472978603872034).abs() < 1e-6); + assert!((occupied - 0.8472978603872034).abs() > 1e-12); + assert_eq!( + GridMapConfig::::default().occupied_probability, + 0.7f32, + "a single-precision map should see its defaults exactly" + ); + } + + /// The prior implementation derived its increments as `(p - 1/p).ln()`, which is `ln` of a + /// negative number for any sensible free probability, and so silently NaN. Sweep the whole + /// valid domain and insist every derived quantity is finite. + #[test] + fn test_no_config_in_valid_range_produces_nan() { + for occupied_step in 1..100 { + let occupied_probability = 0.5 + f64::from(occupied_step) * 0.005; + for free_step in 1..100 { + let free_probability = f64::from(free_step) * 0.005; + + let config = GridMapConfig::::builder() + .with_occupied_probability(occupied_probability) + .with_free_probability(free_probability) + .build(); + + let (occupied, free, max) = config.resolve().unwrap(); + assert!( + occupied.is_finite() && free.is_finite() && max.is_finite(), + "non-finite increment for p_occ={occupied_probability}, p_free={free_probability}" + ); + assert!(occupied > 0.0); + assert!(free < 0.0); + } + } + } + + #[test] + fn test_resolve_rejects_occupied_probability_out_of_range() { + for probability in [0.5, 0.4, 1.0, 0.0, 1.5, -0.1, f64::NAN] { + let config = GridMapConfig::::builder() + .with_occupied_probability(probability) + .build(); + assert_eq!( + config.resolve().unwrap_err(), + GridMapError::InvalidOccupiedProbability, + "accepted an occupied probability of {probability}" + ); + } + } + + #[test] + fn test_resolve_rejects_free_probability_out_of_range() { + for probability in [0.5, 0.6, 0.0, 1.0, -0.1, f64::NAN] { + let config = GridMapConfig::::builder() + .with_free_probability(probability) + .build(); + assert_eq!( + config.resolve().unwrap_err(), + GridMapError::InvalidFreeProbability, + "accepted a free probability of {probability}" + ); + } + } + + #[test] + fn test_resolve_rejects_max_confidence_out_of_range() { + for confidence in [0.5, 0.4, 1.0, 0.0, 1.5, f64::NAN] { + let config = GridMapConfig::::builder() + .with_max_confidence(confidence) + .build(); + assert_eq!( + config.resolve().unwrap_err(), + GridMapError::InvalidMaxConfidence, + "accepted a maximum confidence of {confidence}" + ); + } + } + + #[test] + fn test_unknown_samples() { + let log_odds = MapSample::::unknown_log_odds(); + assert_eq!(log_odds.value, 0.0); + assert_eq!(log_odds.gradient, SVector::::zeros()); + + let probability = MapSample::::unknown_probability(); + assert_eq!(probability.value, 0.5); + assert_eq!(probability.gradient, SVector::::zeros()); + } + + #[test] + fn test_bresenham_error_converts() { + let converted: GridMapError = BresenhamError::NonFiniteCoordinate.into(); + assert_eq!( + converted, + GridMapError::Ray(BresenhamError::NonFiniteCoordinate) + ); + } + + #[cfg(feature = "std")] + #[test] + fn test_error_display_for_each_variant() { + fn assert_error(error: E) -> String { + error.to_string() + } + + // Every variant must render; these are the arms coverage would otherwise never reach. + assert_eq!( + assert_error(GridMapError::DimensionTooSmall { axis: 1, extent: 1 }), + "axis 1 has an extent of 1, but at least 2 cells are required" + ); + assert_eq!( + assert_error(GridMapError::CapacityOverflow), + "the requested dimensions overflow a usize" + ); + assert_eq!( + assert_error(GridMapError::AllocationFailed { cells: 7 }), + "could not allocate storage for 7 cells" + ); + assert_eq!( + assert_error(GridMapError::OutOfBounds { + axis: 0, + index: -1, + extent: 512 + }), + "index -1 on axis 0 is outside the extent 0..512" + ); + assert_eq!( + assert_error(GridMapError::NonFiniteCoordinate { axis: 2 }), + "the coordinate on axis 2 is not finite" + ); + assert_eq!( + assert_error(GridMapError::InvalidOccupiedProbability), + "the occupied probability must be between 0.5 and 1.0" + ); + assert_eq!( + assert_error(GridMapError::InvalidFreeProbability), + "the free probability must be between 0.0 and 0.5" + ); + assert_eq!( + assert_error(GridMapError::InvalidMaxConfidence), + "the maximum confidence must be between 0.5 and 1.0" + ); + assert!( + assert_error(GridMapError::Ray(BresenhamError::ZeroDimensions)) + .starts_with("the beam could not be plotted:") + ); + } +} diff --git a/crates/mapping-suites/src/hector_mapper/mod.rs b/crates/mapping-suites/src/hector_mapper/mod.rs new file mode 100644 index 0000000..800132e --- /dev/null +++ b/crates/mapping-suites/src/hector_mapper/mod.rs @@ -0,0 +1,27 @@ +// SPDX-License-Identifier: MIT +/* + * Copyright (c) [2023 - Present] Emily Matheys + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +pub use grid_map::{GridMapError, GridMapResult}; + +#[allow(dead_code)] +mod grid_map; diff --git a/crates/mapping-suites/src/lib.rs b/crates/mapping-suites/src/lib.rs index a7c4bb1..946379d 100644 --- a/crates/mapping-suites/src/lib.rs +++ b/crates/mapping-suites/src/lib.rs @@ -32,3 +32,15 @@ extern crate alloc; #[cfg(not(feature = "std"))] extern crate core; + +#[cfg(feature = "std")] +use std::{array, borrow::Borrow, boxed::Box, fmt, ops, vec::Vec}; + +#[cfg(not(feature = "std"))] +use { + alloc::{boxed::Box, vec::Vec}, + core::{array, borrow::Borrow, fmt, ops}, +}; + +/// A suite implementing the Hector SLAM mapping algorithm and its occupancy grid. +pub mod hector_mapper;