diff --git a/.github/instructions/robotics-cpp.instructions.md b/.github/instructions/robotics-cpp.instructions.md index be802f9..4682032 100644 --- a/.github/instructions/robotics-cpp.instructions.md +++ b/.github/instructions/robotics-cpp.instructions.md @@ -56,7 +56,12 @@ Use `math::IsFinite` for finiteness checks: a consumer may still build with `-ff - SOLID principles — constructor injection, depend on abstractions - DRY — extract shared logic into helpers or templates - RAII for resource management -- **Protected, non-virtual interface destructors**: declare `~Interface() = default` in a `protected:` section, never `virtual ~Interface()` (defaulted or `= 0`). A virtual destructor puts a deleting destructor in every implementation's vtable, which links `operator delete` (and with it `malloc`/`free`) into heap-free firmware, and under multiple inheritance adds a thunk per base. The protected destructor makes deletion through an interface pointer a compile error. Concrete classes declare no destructor unless they need one, and callers hold the concrete type rather than a `std::unique_ptr` to the interface. +- **Protected, non-virtual interface destructors**: declare `~Interface() = default` in a `protected:` section, + never `virtual ~Interface()` (defaulted or `= 0`). A virtual destructor puts a deleting destructor in every + implementation's vtable, which links `operator delete` (and with it `malloc`/`free`) into heap-free firmware, and + under multiple inheritance adds a thunk per base. The protected destructor makes deletion through an interface + pointer a compile error. Concrete classes declare no destructor unless they need one, and callers hold the + concrete type rather than a `std::unique_ptr` to the interface. - No virtual calls in ISR-callable or real-time critical paths ## Documentation — MANDATORY diff --git a/ROADMAP.md b/ROADMAP.md index ee6fb26..3458ea5 100644 --- a/ROADMAP.md +++ b/ROADMAP.md @@ -1,8 +1,8 @@ # Robotics Toolbox — Roadmap -Prioritized backlog by robot family. Manipulators (**M**) and wheeled robots (**W**, with **F1**) have specs -(`roadmap///`); the other families are entries only so far, and their specs come family by -family: +Prioritized backlog by robot family. Manipulators (**M**), wheeled robots (**W**, with **F1**) and +localization (**E**) have specs (`roadmap///`); the other families are entries only so far, and +their specs come family by family: | Prefix | Family | Section | |--------|---------------------------------------------------------------|---------------------------------------------------------------------------| @@ -43,29 +43,30 @@ master-list item number (NT53 is upstream item 53). A robotics item that depends numerical-toolbox pin that ships it. NT53–NT65 have shipped upstream, and their links point to the upstream docs; the current pin (`63c9b91`) includes them. -| # | Component | Upstream module | Spec / doc | Needed by | -|------|-------------------------------------------------------|----------------------|-----------------------------------|----------------------------------------------------------------------------| -| NT53 | SE(3) transform, twists, wrenches, exp/log, PoseError | `math` | [SE3Transform][nt53] | M7, M8, M10, M13, M15, M21, M23, M25, M26, M30, F3, A1, A6, L3, S2, G1, C1 | -| NT54 | Angle utilities (wrap, shortest difference, unwrap) | `math` | [AngleUtilities][nt54] | M21, W6, W9, W10, S1 | -| NT55 | Polynomial point-to-point profile (+ profile types) | `motion_profiles` | [PolynomialProfile][nt55] | M10, F4, L2 | -| NT56 | Trapezoidal (LSPB) profile + duration sync | `motion_profiles` | [TrapezoidalProfile][nt56] | M10 (time law, optional), F4 | -| NT57 | Friction compensation (Coulomb + viscous + Stribeck) | `controllers` | [FrictionCompensation][nt57] | M5, M12, M29 (optional feedforward) | -| NT58 | Symplectic integrators (semi-implicit Euler, Verlet) | `solvers` | [SymplecticIntegrators][nt58] | M34 | -| NT59 | Tridiagonal (Thomas) solver | `math` | [TridiagonalSolver][nt59] | NT61 | -| NT60 | S-curve (jerk-limited) profile | `motion_profiles` | [SCurveProfile][nt60] | M10 (time law, optional) | -| NT61 | Cubic spline interpolation (vector-valued) | `math` | [CubicSpline][nt61] | M33 | -| NT62 | Damped pseudo-inverse + null-space projector | `solvers` | [DampedPseudoInverse][nt62] | M14, H5 | -| NT63 | Incremental QR least squares (rank-revealing) | `estimators/offline` | [IncrementalQrLeastSquares][nt63] | M22 | -| NT64 | Levenberg–Marquardt (retraction, pluggable damping) | `optimization` | [LevenbergMarquardt][nt64] | M13, M23, E5, C2 | -| NT65 | Dense convex QP (Goldfarb–Idnani) | `solvers` | [ActiveSetQp][nt65] | M25 (exact path), L6, G2, G3 | -| NT66 | SE(2) transform, twists, exp/log, pose error | `math` | [SE2Transform][nt66] (spec) | F1, W1–W10, M24, E1, E2, E3, E5 | -| NT67 | Kabsch / Umeyama point-set alignment | `solvers` | [master list][ntrm], no spec yet | E5, C1 | -| NT68 | Lie-group Jacobians and ⊕/⊖ (SO(3), SE(3), SE(2)) | `math` | [master list][ntrm], no spec yet | C1 (and NT71) | -| NT69 | Control allocation (redistributed pseudo-inverse, QP) | `controllers` | [master list][ntrm], no spec yet | A2, S3 | -| NT70 | Particle filter (bounded, systematic resampling) | `filters/active` | [master list][ntrm], no spec yet | E4 | -| NT71 | Error-state EKF on Lie groups | `filters/active` | [master list][ntrm], no spec yet | L5 | -| NT72 | Minimum-derivative (min-snap) piecewise polynomials | `motion_profiles` | [master list][ntrm], no spec yet | A7 | -| NT73 | Online jerk-limited trajectory from arbitrary state | `motion_profiles` | [master list][ntrm], no spec yet | F2 | +| # | Component | Upstream module | Spec / doc | Needed by | +|------|-------------------------------------------------------|----------------------|-------------------------------------|----------------------------------------------------------------------------| +| NT53 | SE(3) transform, twists, wrenches, exp/log, PoseError | `math` | [SE3Transform][nt53] | M7, M8, M10, M13, M15, M21, M23, M25, M26, M30, F3, A1, A6, L3, S2, G1, C1 | +| NT54 | Angle utilities (wrap, shortest difference, unwrap) | `math` | [AngleUtilities][nt54] | M21, W6, W9, W10, E1, E3, S1 | +| NT55 | Polynomial point-to-point profile (+ profile types) | `motion_profiles` | [PolynomialProfile][nt55] | M10, F4, L2 | +| NT56 | Trapezoidal (LSPB) profile + duration sync | `motion_profiles` | [TrapezoidalProfile][nt56] | M10 (time law, optional), F4 | +| NT57 | Friction compensation (Coulomb + viscous + Stribeck) | `controllers` | [FrictionCompensation][nt57] | M5, M12, M29 (optional feedforward) | +| NT58 | Symplectic integrators (semi-implicit Euler, Verlet) | `solvers` | [SymplecticIntegrators][nt58] | M34 | +| NT59 | Tridiagonal (Thomas) solver | `math` | [TridiagonalSolver][nt59] | NT61 | +| NT60 | S-curve (jerk-limited) profile | `motion_profiles` | [SCurveProfile][nt60] | M10 (time law, optional) | +| NT61 | Cubic spline interpolation (vector-valued) | `math` | [CubicSpline][nt61] | M33 | +| NT62 | Damped pseudo-inverse + null-space projector | `solvers` | [DampedPseudoInverse][nt62] | M14, H5 | +| NT63 | Incremental QR least squares (rank-revealing) | `estimators/offline` | [IncrementalQrLeastSquares][nt63] | M22 | +| NT64 | Levenberg–Marquardt (retraction, pluggable damping) | `optimization` | [LevenbergMarquardt][nt64] | M13, M23, E5, C2 | +| NT65 | Dense convex QP (Goldfarb–Idnani) | `solvers` | [ActiveSetQp][nt65] | M25 (exact path), L6, G2, G3 | +| NT66 | SE(2) transform, twists, exp/log, pose error | `math` | [SE2Transform][nt66] (spec) | F1, W1–W10, M24, E1, E2, E3, E5 | +| NT67 | Kabsch / Umeyama point-set alignment | `solvers` | [PointSetAlignment][nt67] (spec) | E5, C1 | +| NT68 | Lie-group Jacobians and ⊕/⊖ (SO(3), SE(3), SE(2)) | `math` | [master list][ntrm], no spec yet | C1 (and NT71) | +| NT69 | Control allocation (redistributed pseudo-inverse, QP) | `controllers` | [master list][ntrm], no spec yet | A2, S3 | +| NT70 | Particle filter (bounded, systematic resampling) | `filters/active` | [ParticleFilter][nt70] (spec) | E4 | +| NT71 | Error-state EKF on Lie groups | `filters/active` | [master list][ntrm], no spec yet | L5 | +| NT72 | Minimum-derivative (min-snap) piecewise polynomials | `motion_profiles` | [master list][ntrm], no spec yet | A7 | +| NT73 | Online jerk-limited trajectory from arbitrary state | `motion_profiles` | [master list][ntrm], no spec yet | F2 | +| NT74 | Residual-form Kalman update with χ² gating | `filters/active` | [ResidualKalmanUpdate][nt74] (spec) | E1, E3 | [ntrm]: https://github.com/embedded-pro/numerical-toolbox-cpp/blob/main/ROADMAP.md#master-list-by-priority [nt53]: https://github.com/embedded-pro/numerical-toolbox-cpp/blob/main/doc/math/SE3Transform.md @@ -82,6 +83,9 @@ docs; the current pin (`63c9b91`) includes them. [nt64]: https://github.com/embedded-pro/numerical-toolbox-cpp/blob/main/doc/optimization/LevenbergMarquardt.md [nt65]: https://github.com/embedded-pro/numerical-toolbox-cpp/blob/main/doc/solvers/ActiveSetQp.md [nt66]: https://github.com/embedded-pro/numerical-toolbox-cpp/blob/main/roadmap/math/SE2Transform/implementation.md +[nt67]: https://github.com/embedded-pro/numerical-toolbox-cpp/blob/main/roadmap/solvers/PointSetAlignment/implementation.md +[nt70]: https://github.com/embedded-pro/numerical-toolbox-cpp/blob/main/roadmap/filters/active/ParticleFilter/implementation.md +[nt74]: https://github.com/embedded-pro/numerical-toolbox-cpp/blob/main/roadmap/filters/active/ResidualKalmanUpdate/implementation.md ## Robot manipulators & other manipulator types @@ -490,15 +494,15 @@ Cross-family building blocks that several of the families below depend on. | # | Component | Target module | Depends on | Difficulty | |----|--------------------------------------------------------|----------------|------------------|------------| -| E1 | Wheel + IMU odometry fusion (EKF on SE(2)) | `localization` | F1, NT66 | ★★★☆☆ | +| E1 | Wheel + gyro odometry fusion (EKF on SE(2)) | `localization` | F1, NT66, NT74 | ★★★☆☆ | | E2 | Occupancy-grid mapping (log-odds, ray casting) | `localization` | NT66 | ★★★☆☆ | -| E3 | EKF landmark localization (known map, range–bearing) | `localization` | NT66 | ★★★☆☆ | +| E3 | EKF landmark localization (known map, range–bearing) | `localization` | F1, NT66, NT74 | ★★★☆☆ | | E4 | Monte Carlo localization | `localization` | NT70, E2, F1 | ★★★★☆ | | E5 | 2-D scan matching (point-to-point / point-to-line ICP) | `localization` | NT67, NT64, NT66 | ★★★★☆ | -**E1. Odometry fusion.** EKF combining wheel odometry with gyro (and optionally accelerometer) on SE(2), with slip-aware process noise. -- *Algorithm / paper:* S. Thrun, W. Burgard, D. Fox, *Probabilistic Robotics* (2005), Ch. 5 and 7. -- *Reuses / builds on:* F1, NT66; upstream `ExtendedKalmanFilter`. +**E1. Odometry fusion.** EKF on `(x, y, θ, gyro bias)`: wheel speed and gyro rate drive the prediction, the wheel yaw rate is a χ²-gated bias measurement (a rejected update flags slip), and optional heading and position fixes share the same update. +- *Algorithm / paper:* S. Thrun, W. Burgard, D. Fox, *Probabilistic Robotics* (2005), Ch. 5 and 7; J. Borenstein, L. Feng, "Gyrodometry," *ICRA* 1996. +- *Reuses / builds on:* F1, NT66, NT74 (upstream `ExtendedKalmanFilter` cannot wrap residuals or gate). **E2. Occupancy-grid mapping.** Log-odds cell updates from range scans along Bresenham rays on a bounded grid. - *Algorithm / paper:* A. Elfes, *Computer*, 22(6), 1989; Thrun et al. (2005), Ch. 9. @@ -506,13 +510,13 @@ Cross-family building blocks that several of the families below depend on. **E3. EKF landmark localization.** Pose tracking against a known landmark map with range–bearing measurements and gated data association. - *Algorithm / paper:* Thrun et al. (2005), Ch. 7. -- *Reuses / builds on:* NT66; upstream `ExtendedKalmanFilter`. +- *Reuses / builds on:* F1 (odometry noise), NT66, NT74. -**E4. Monte Carlo localization.** Particle-filter localization in an occupancy grid with the odometry motion model and a likelihood-field sensor model. +**E4. Monte Carlo localization.** Particle-filter localization in an occupancy grid with the odometry motion model and a likelihood-field sensor model (two-pass chamfer distance map); samplers injected by the caller. - *Algorithm / paper:* F. Dellaert, D. Fox, W. Burgard, S. Thrun, *ICRA* 1999. - *Reuses / builds on:* NT70, E2, F1. -**E5. Scan matching.** Iterative closest point between consecutive laser scans or against a map, with point-to-line error and outlier rejection. +**E5. Scan matching.** Iterative closest point between consecutive laser scans or against a map: point-to-point (Kabsch) or point-to-line (Levenberg–Marquardt), outlier rejection and degeneracy detection. - *Algorithm / paper:* P. Besl, N. McKay, *IEEE T-PAMI*, 14(2), 1992; A. Censi, *ICRA* 2008. - *Reuses / builds on:* NT67, NT64, NT66. diff --git a/roadmap/DEPLOYMENT.md b/roadmap/DEPLOYMENT.md index 876b62c..bd32ef5 100644 --- a/roadmap/DEPLOYMENT.md +++ b/roadmap/DEPLOYMENT.md @@ -21,7 +21,7 @@ Read the spec's three files first (`implementation.md`, `tests.md`, `explanation 4. **CMake** - Add `.hpp` to `target_sources(...)`, `.cpp` to `robotics_add_coverage_sources(...)`, `Test.cpp` to the `_test` target's `target_sources`. - - New module (`trajectory`, `controllers/manipulator`, `mobile`, `planning`): + - New module (`trajectory`, `controllers/manipulator`, `mobile`, `planning`, `localization`): create `robotics//CMakeLists.txt` via `robotics_add_header_library(...)`, add a `test/` subdir, register it in the parent `CMakeLists.txt`, and add a `doc//` folder. diff --git a/roadmap/README.md b/roadmap/README.md index 93fd4b9..6495b9e 100644 --- a/roadmap/README.md +++ b/roadmap/README.md @@ -3,8 +3,8 @@ Design-level pseudocode for proposed components in [../ROADMAP.md](../ROADMAP.md). These are **specifications, not compilable code** — they describe *what* to build and *how* the algorithm works, so an implementer can produce the real templated C++ afterwards. Every manipulator -(M) and wheeled-robot (W) item has a spec, as does F1; the other families (F, A, L, S, G, P, E, C, H) are -ROADMAP entries only, and their specs are added here family by family. +(M), wheeled-robot (W) and localization (E) item has a spec, as does F1; the other families (F, A, L, S, G, +P, C, H) are ROADMAP entries only, and their specs are added here family by family. ## Convention @@ -103,5 +103,8 @@ prerequisites. ### `planning` `DubinsPath` (W8) · `DynamicWindowApproach` (W9) · `ReedsSheppPath` (W10) +### `localization` +`OdometryFusion` (E1) · `OccupancyGridMap` (E2) · `LandmarkLocalization` (E3) · `MonteCarloLocalization` (E4) · `ScanMatcher` (E5) + ### `controllers/manipulator` `PdGravityCompensation` (M5) · `ComputedTorqueControl` (M12) · `ImpedanceControl` (M17) · `OperationalSpaceControl` (M18) · `HybridPositionForceControl` (M19) · `SlotineLiAdaptiveControl` (M20) · `CableTensionDistribution` (M25) diff --git a/roadmap/localization/LandmarkLocalization/explanation.md b/roadmap/localization/LandmarkLocalization/explanation.md new file mode 100644 index 0000000..03780db --- /dev/null +++ b/roadmap/localization/LandmarkLocalization/explanation.md @@ -0,0 +1,33 @@ +# EKF Landmark Localization — Overview + +## What it is +A Kalman-filter localizer for a robot that knows where certain landmarks are — reflective posts, +fiducial markers, poles — and measures their distance and direction. It keeps a pose estimate and its +uncertainty, moves it with odometry, and corrects it with every landmark sighting. + +## Why it matters (embedded) +Many industrial AGVs, lawn robots and warehouse vehicles navigate this way: a laser picks out +reflectors, or a camera picks out markers, at surveyed positions. With a 3-element state the filter is +tiny, so it runs at sensor rate on a microcontroller and gives a consistent uncertainty for safety checks +and planners. + +## How it works (intuition) +Between sightings the pose moves with the odometry increment, and the uncertainty grows in proportion to +how far and how much the robot turned. When a landmark is seen, the filter predicts where it should +appear given the current estimate. The difference between seen and predicted range and bearing corrects +the pose, weighted by how uncertain each is, and the bearing difference is taken the short way round the +circle. If a sighting carries no identity, the landmark it most plausibly belongs to is chosen; sightings +too far from every landmark are rejected as clutter. + +## Key parameters +- **motion noise** — growth of position and heading uncertainty with distance and rotation. +- **range and bearing noise** of the sensor. +- **gate** — how implausible a sighting may be before it is rejected. + +## Reference +S. Thrun, W. Burgard, D. Fox, *Probabilistic Robotics* (2005), Ch. 7; J. J. Leonard, H. F. Durrant-Whyte, +"Mobile robot localization by tracking geometric beacons," *IEEE T-RA*, 7(3), 1991. + +## See also +`PlanarOdometry` (F1, the increment), `OdometryFusion` (E1); upstream `ResidualKalmanUpdate` (NT74), +`SE2Transform` (NT66). diff --git a/roadmap/localization/LandmarkLocalization/implementation.md b/roadmap/localization/LandmarkLocalization/implementation.md new file mode 100644 index 0000000..3152db5 --- /dev/null +++ b/roadmap/localization/LandmarkLocalization/implementation.md @@ -0,0 +1,113 @@ +# EKF Landmark Localization (Known Map, Range–Bearing) — Implementation Pseudocode + +> Roadmap ref: #E3 (★★★☆☆) · Target: `robotics/localization` · Namespace `localization` · Type: `float` (templated on `T`, instantiated for `float` only) + +Pose tracking against a known map of point landmarks (reflectors, fiducials, poles) seen as range and +bearing. The motion step composes the odometry increment (F1) with noise proportional to translation +and rotation. Each observation is either labelled with a landmark id or associated by minimum +Mahalanobis distance. It is χ²-gated and applied with upstream `ResidualKalmanUpdate` (NT74), with the +bearing residual wrapped. + +## Data structures + +```cpp +template # static_assert(std::is_floating_point_v); instantiated for float +struct Landmark: + math::Vector2 position # world frame + std::uint16_t id + +template +struct RangeBearing: + T range, bearing # sensor = robot frame + std::optional id # known identity (fiducial) or nullopt (association needed) + +template +struct LandmarkLocalizationNoise: + mobile::OdometryNoise motion # F1: α₁…α₄ + T rangeNoise, bearingNoise # σ_r (m), σ_b (rad) + T gate # χ²(2), e.g. 9.21 (99 %) + +template +struct LandmarkUpdate: + filters::KalmanUpdateResult result + std::optional landmark # index used (nullopt if none in the map) + +template +class LandmarkLocalization: + std::array, MaxLandmarks> map; std::size_t count + math::Vector3 state # (x, y, θ) + math::SquareMatrix covariance + LandmarkLocalizationNoise noise +``` + +## Interface + +```cpp +LandmarkLocalization(const math::SE2Transform& pose, const math::SquareMatrix& covariance, const LandmarkLocalizationNoise& noise) +bool SetMap(std::span> landmarks) # false if more than MaxLandmarks +void Predict(const math::SE2Transform& increment) # body-frame motion since the last call; hot path +LandmarkUpdate Update(const RangeBearing& observation) # hot path +std::optional Nis(const RangeBearing& observation, std::size_t landmark) const +math::SE2Transform Pose() const; const math::SquareMatrix& Covariance() const +``` + +`increment` is `previousOdometry.Inverse() * currentOdometry`, i.e. `Exp(F1::Increment(...), 1)`. + +## Algorithm (pseudocode) + +```text +function Predict(Δ): # OPTIMIZE_FOR_SPEED + (dx, dy, dθ) = Δ.Pose(); c, s = cos θ, sin θ; Δs² = dx² + dy² + F = [[1, 0, −s·dx − c·dy], [0, 1, c·dx − s·dy], [0, 0, 1]] # ∂(pose ⊕ Δ)/∂pose + J = blockdiag(R(θ), 1) # ∂(pose ⊕ Δ)/∂Δ + σ = mobile::IncrementDeviation(Δ, motion) # F1: (σ_xy, σ_xy, σ_θ) + P = CongruenceTransform(F, P) + CongruenceTransform(J, diag(σ²)) + state = (Pose() * Δ).Pose() + +function Expected(m): # predicted measurement and Jacobian for landmark m + d = m.position − (x, y); q = d·d; ρ = √q + ẑ = (ρ, AngleDifference(atan2(d.y, d.x), θ)) + H = [[−d.x/ρ, −d.y/ρ, 0], [d.y/q, −d.x/q, −1]] + return ẑ, H + +function Residual(z, m): (ẑ, H) = Expected(m); return ((z.range − ẑ.range, AngleDifference(z.bearing, ẑ.bearing)), H) + +function Update(z): + k = z.id ? index of the landmark with that id : argmin over map of Nis(z, k) # nearest neighbour in Mahalanobis distance + if no such landmark: return { singular, nullopt } + (r, H) = Residual(z, map[k]); R = diag(σ_r², σ_b²) + result = ResidualKalmanUpdate::Apply(state, P, r, H, R, gate); θ = WrapToPi(θ) + return { result, k } +``` + +## Complexity & memory + +- `Predict`: two 3×3 congruences. `Update`: `O(MaxLandmarks)` NIS evaluations (2×2 Cholesky each) for an + unlabelled observation, then one NT74 update. +- Memory: `3·MaxLandmarks` scalars of map plus 12 of state. No heap. + +## Numerical / embedded notes + +- **Increment noise in the body frame.** F1's `IncrementDeviation` adds the noise to the body-frame + increment, which `J` rotates into the world. Forward and backward motion are treated alike (test: a + reverse step gives the same covariance as a forward one), unlike the rotate–translate–rotate split. +- **Bearing wrap.** A landmark straight behind the robot sits on the ±π seam. The wrapped residual is + 0.026665 where the raw difference is −6.256520; the test checks the update stays small. +- **Association.** Nearest neighbour by NIS with a gate is the classic low-cost choice. Where two + landmarks are inside the gate together, prefer labelled observations, or space landmarks wider than + the 3σ position uncertainty. +- Observations taken at one time are applied one after another; each update refines `P` for the next. +- Float-only: `static_assert(std::is_floating_point_v)`. + +## Deployment + +- Header: `robotics/localization/LandmarkLocalization.hpp` — `#pragma once`, `OPTIMIZE_FOR_SPEED` on + `Predict`/`Update`, and `extern template class LandmarkLocalization;` under + `#ifdef ROBOTICS_TOOLBOX_COVERAGE_BUILD`. +- Coverage: `robotics/localization/LandmarkLocalization.cpp` → `template class LandmarkLocalization;` +- Test: `robotics/localization/test/TestLandmarkLocalization.cpp` +- Doc: `doc/localization/LandmarkLocalization.md` (per `doc/TEMPLATE.md`) +- CMake: `.hpp` → `target_sources(robotics.localization ...)`; `.cpp` → `robotics_add_coverage_sources`; + test → `robotics.localization_test`. +- Depends on: F1 (`OdometryNoise`, `IncrementDeviation`, increment convention), NT66, NT54, NT74 (`ResidualKalmanUpdate`). +- Generic pattern: see `roadmap/DEPLOYMENT.md`. diff --git a/roadmap/localization/LandmarkLocalization/tests.md b/roadmap/localization/LandmarkLocalization/tests.md new file mode 100644 index 0000000..74fd688 --- /dev/null +++ b/roadmap/localization/LandmarkLocalization/tests.md @@ -0,0 +1,53 @@ +# EKF Landmark Localization — Unit Test Plan (Pseudocode) + +> GoogleTest · `TEST_F` (`float`) · `StrictMock` only · no heap. + +## Fixture + +```cpp +class TestLandmarkLocalization : public ::testing::Test: + std::array, 4> landmarks{ { (5, 0), 0 }, { (0, 5), 1 }, { (5, 5), 2 }, { (−3, 2), 3 } } + LandmarkLocalizationNoise noise{ { 0.05, 0.01, 0.05, 0.01 }, 0.1, 0.02, 9.21 } + LandmarkLocalization filter{ FromPose(1, 1, 0.3), diag(0.05, 0.05, 0.01), noise } # SetMap(landmarks) + Vector3 truth{ 1.1, 0.9, 0.32 } + # Measure(truth, k) = exact range and bearing of landmark k from the true pose +# each case below is a TEST_F(TestLandmarkLocalization, ) +``` + +## Test cases (Arrange / Act / Assert) + +```cpp +predict_composes_the_increment_and_grows_the_covariance: + Act: Predict(FromPose(0.5, 0, 0.1)) + Assert: Pose().Pose() ≈ (1.477668, 1.147760, 0.4); diag(P) ≈ (0.062818, 0.064882, 0.013) + P(0,1) ≈ −0.000706; P(1,2) ≈ 0.004777 +reverse_motion_has_the_same_noise_as_forward: + Arrange: filter at Identity with P = 0 + Act: Predict(FromPose(−0.5, 0, 0)) + Assert: Pose().Pose() ≈ (−0.5, 0, 0); diag(P) ≈ (0.0125, 0.0125, 0.0025) +unlabelled_observation_is_associated_by_mahalanobis_distance: + Act: Update({ Measure(truth, 2), nullopt }) + Assert: landmark == 2; status applied, nis ≈ 0.002138; Pose().Pose() ≈ (1.001568, 0.996349, 0.295825) + # NIS against landmarks 0, 1, 3: 119.6, 118.1, 371.8 +labelled_observations_converge_towards_the_truth: + Act: Update({ Measure(truth, 2), 2 }), then { 0 }, { 1 }, { 3 } + Assert: Pose().Pose() ≈ (1.092522, 0.904252, 0.318948); diag(P) ≈ (0.002722, 0.002077, 0.000122) +outlier_is_gated: + Act: Update({ (0.5, 2.5), nullopt }) + Assert: status gated (best nis ≈ 219.48 against landmark 3); pose and covariance unchanged +bearing_residual_is_wrapped_across_pi: + Arrange: filter at FromPose(6, 4.9, 0), P = diag(0.05, 0.05, 0.01); landmark 1 at bearing +3.124928 + Act: Update({ (6.0, −π + 0.01), 1 }) + Assert: status applied, nis ≈ 0.060327; Pose().Pose() ≈ (5.999620, 4.918856, −0.022620) +``` + +## Reference vectors + +- From `(1, 1, 0.3)`: landmark 2 predicted at `(5.656854, 0.485398)`; measured from the truth at + `(5.658622, 0.490393)`. The Jacobian is `[[−0.707107, −0.707107, 0], [0.125, −0.125, −1]]`. +- The wrapped bearing residual is `0.026665`; unwrapped it would be `−6.256520`. + +## Edge cases + +- Empty map ⇒ `Update` returns `{ singular, nullopt }`. +- An `id` that is not in the map ⇒ `{ singular, nullopt }`, state unchanged. diff --git a/roadmap/localization/MonteCarloLocalization/explanation.md b/roadmap/localization/MonteCarloLocalization/explanation.md new file mode 100644 index 0000000..bfdbeee --- /dev/null +++ b/roadmap/localization/MonteCarloLocalization/explanation.md @@ -0,0 +1,33 @@ +# Monte Carlo Localization — Overview + +## What it is +A way for a robot to find and track its position on a known map using a cloud of guesses. Each guess +("particle") is a possible pose. The cloud moves with the odometry and is re-weighted by how well the +laser scan fits the map from that pose, so wrong guesses fade and right ones multiply. + +## Why it matters (embedded) +Monte Carlo localization is the standard indoor localizer (ROS AMCL is built on it). It copes with +ambiguous places, starting with no idea of the position, and occasional clutter. With a fixed particle +count and a precomputed distance map, each scan costs a predictable amount of work. Because the random +numbers come from the caller, test and field runs are reproducible. + +## How it works (intuition) +When the robot moves, every particle moves by the same odometry step plus a little random noise, more +for longer or more twisting steps. When a scan arrives, each particle projects the beam end points into +the map from its own pose. Points that land near walls score well, using a precomputed map of the +distance from every cell to the nearest wall. Scores become weights, and when only a few particles carry +most of the weight the cloud is resampled around them. The reported pose is the weighted average, with +headings averaged as directions so that +179° and −179° average to 180°, not 0°. + +## Key parameters +- **particle count** (compile-time) and **resampling threshold**. +- **motion noise** — how much the cloud spreads per metre and per radian. +- **hit sigma**, **hit / random weights**, **beam stride** — sensor model and its cost. + +## Reference +F. Dellaert, D. Fox, W. Burgard, S. Thrun, "Monte Carlo localization for mobile robots," *ICRA* 1999; +S. Thrun, W. Burgard, D. Fox, *Probabilistic Robotics* (2005), Ch. 6.4 (likelihood field) and Ch. 8.3. + +## See also +`OccupancyGridMap` (E2), `LandmarkLocalization` (E3, the Gaussian alternative), `PlanarOdometry` (F1); +upstream `ParticleFilter` (NT70). diff --git a/roadmap/localization/MonteCarloLocalization/implementation.md b/roadmap/localization/MonteCarloLocalization/implementation.md new file mode 100644 index 0000000..6073104 --- /dev/null +++ b/roadmap/localization/MonteCarloLocalization/implementation.md @@ -0,0 +1,117 @@ +# Monte Carlo Localization (Likelihood Field) — Implementation Pseudocode + +> Roadmap ref: #E4 (★★★★☆) · Target: `robotics/localization` · Namespace `localization` · Type: `float` (templated on `T`, instantiated for `float` only) + +Particle-filter localization in an occupancy grid (E2). Upstream `ParticleFilter` (NT70) carries the +particles. The odometry motion model samples the same body-frame increment noise as E3. The +likelihood-field sensor model scores each beam end point by its distance to the nearest occupied cell, +precomputed once per map with a two-pass chamfer transform. Because the filter is multimodal, it can +localize globally (from a spread prior) as well as track. All randomness comes from caller-injected +samplers, so runs are deterministic under a fixed seed. + +## Data structures + +```cpp +template # static_assert(std::is_floating_point_v); instantiated for float +struct MclParameters: + mobile::OdometryNoise motion # F1: α₁…α₄ + T hitSigma # σ_hit, m + T hitWeight, randomWeight # z_hit, z_rand (z_hit + z_rand = 1) + T maxRange # beams ≥ maxRange are skipped + T resampleFraction # ESS threshold as a fraction of N, e.g. 0.5 + std::size_t beamStride # use every k-th beam + +template +class LikelihoodField: + std::array distance # metres to the nearest occupied cell, capped at maxDistance + OccupancyGridParameters grid # resolution and origin of the map it was built from + T maxDistance + +template +class MonteCarloLocalization: + filters::ParticleFilter filter # state (x, y, θ); NT70 + const LikelihoodField& field + math::SE2Transform sensorMount # sensor in the body frame + MclParameters parameters +``` + +## Interface + +```cpp +LikelihoodField(const OccupancyGridMap& map, T occupiedThreshold, T maxDistance) +T LikelihoodField::Distance(const math::Vector2& world) const # maxDistance outside the map + +MonteCarloLocalization(const LikelihoodField& field, const math::SE2Transform& sensorMount, const MclParameters& parameters) +template void Initialize(Draw&& draw) # draw(i) ⇒ SE2Transform: global or local prior +template void InitializeAround(const math::SE2Transform& pose, const math::Vector3& sigma, Normal&& normal) +template void Predict(const math::SE2Transform& increment, Normal&& normal) # hot path +template bool Update(std::span> scan, Uniform&& uniform) # true if it resampled +math::SE2Transform Estimate() const # weighted mean, circular in θ +const filters::ParticleFilter& Particles() const +``` + +`normal()` returns a standard normal draw and `uniform()` a draw in `[0, 1)`, both of type `T`. + +## Algorithm (pseudocode) + +```text +function LikelihoodField(map, threshold, cap): # 8-neighbour chamfer with weights (1, √2), two raster passes + d[c] = map.LogOdds(c) > threshold ? 0 : ∞ + forward pass (rows ↑, columns ↑): d[c] = min(d[c], d[W] + 1, d[S] + 1, d[SW] + √2, d[SE] + √2) + backward pass (rows ↓, columns ↓): d[c] = min(d[c], d[E] + 1, d[N] + 1, d[NE] + √2, d[NW] + √2) + distance[c] = min(d[c]·resolution, cap) + +function Predict(Δ, normal): # OPTIMIZE_FOR_SPEED + (dx, dy, dθ) = Δ.Pose(); σ = mobile::IncrementDeviation(Δ, motion) # F1 + filter.Predict([&](x, i) { + noisy = FromPose(dx + σ₀·normal(), dy + σ₁·normal(), dθ + σ₂·normal()) + x = (FromPose(x) * noisy).Pose() }) # heading stays wrapped + +function Update(scan, uniform): # OPTIMIZE_FOR_SPEED + filter.Update([&](x) { + sensor = FromPose(x) * sensorMount; ℓ = 0 + for every beamStride-th beam (r, b) with r < maxRange: + d = field.Distance(sensor.Apply((r cos b, r sin b))) + ℓ += log(z_hit·exp(−d²/(2σ_hit²)) + z_rand/maxRange) + return ℓ }) + return filter.ResampleIfNeeded(resampleFraction, uniform()) + +function Estimate(): + (x̄, ȳ) = Σ wᵢ (xᵢ, yᵢ); θ̄ = atan2(Σ wᵢ sin θᵢ, Σ wᵢ cos θᵢ) + return FromPose(x̄, ȳ, θ̄) +``` + +## Complexity & memory + +- `Update`: `NumParticles · (beams / stride)` field lookups and `exp`/`log` pairs; 500 particles × 60 beams = + 30 k per scan. `Predict`: `3·NumParticles` normal draws. +- Memory: the field has `Width·Height` scalars (the same as the map); the filter has `8·NumParticles` + scalars (NT70). No heap. + +## Numerical / embedded notes + +- **Likelihood field, not ray casting.** Scoring end points against a precomputed distance map costs one + lookup per beam, against tens of cell steps for a ray-cast beam model. It is also smooth in the pose, + which keeps particle weights from collapsing. The two-pass chamfer overestimates the Euclidean distance + by at most 8 % (test: 0.524264 against 0.5), well inside `σ_hit`. +- **`z_rand` floor.** The random-measurement term keeps one unexplained beam (a person, glass) from zeroing + a particle; with the log-domain filter (NT70) the remaining beams decide. +- **Circular mean.** Averaging headings component-wise fails across ±π, so `Estimate` averages unit + vectors. +- **Kidnapping.** For recovery from a wrong estimate, inject a few uniformly drawn particles through + `Particles()` when the average likelihood drops (augmented MCL, Thrun et al. Ch. 8.3). +- Float-only: `static_assert(std::is_floating_point_v)`. + +## Deployment + +- Header: `robotics/localization/MonteCarloLocalization.hpp` (with `LikelihoodField`) — `#pragma once`, + `OPTIMIZE_FOR_SPEED` on `Predict`/`Update`, and `extern template class LikelihoodField;` / + `extern template class MonteCarloLocalization;` under `#ifdef ROBOTICS_TOOLBOX_COVERAGE_BUILD`. +- Coverage: `robotics/localization/MonteCarloLocalization.cpp` → the same instantiations. +- Test: `robotics/localization/test/TestMonteCarloLocalization.cpp` +- Doc: `doc/localization/MonteCarloLocalization.md` (per `doc/TEMPLATE.md`) +- CMake: `.hpp` → `target_sources(robotics.localization ...)`; `.cpp` → `robotics_add_coverage_sources`; + test → `robotics.localization_test`. +- Depends on: E2 (`OccupancyGridMap`, `RangeBeam`), F1 (`OdometryNoise`, `IncrementDeviation`, increment + convention), NT70 (`ParticleFilter`), NT66. +- Generic pattern: see `roadmap/DEPLOYMENT.md`. diff --git a/roadmap/localization/MonteCarloLocalization/tests.md b/roadmap/localization/MonteCarloLocalization/tests.md new file mode 100644 index 0000000..974f1e1 --- /dev/null +++ b/roadmap/localization/MonteCarloLocalization/tests.md @@ -0,0 +1,60 @@ +# Monte Carlo Localization — Unit Test Plan (Pseudocode) + +> GoogleTest · `TEST_F` (`float`) · `StrictMock` only · no heap. + +## Fixture + +```cpp +class TestMonteCarloLocalization : public ::testing::Test: + # 4 m × 4 m room, 40 × 40 cells of 0.1 m, origin (0, 0); occupied cells set with SetLogOdds(c, 3.5): + # the border (x or y index 0 or 39), an inner wall x = 20 for y = 0..14, a shelf y = 28 for x = 25..39 + OccupancyGridMap room + LikelihoodField field{ room, 0, 2.0 } + MclParameters parameters{ { 0.05, 0.01, 0.05, 0.01 }, 0.1, 0.9, 0.1, 4.0, 0.5, 1 } + SE2Transform truth{ FromPose(1.0, 1.5, 0.2) } + # Scan(): 12 beams at bearings 0°, 30°, …, 330°, ray-cast in the test from the truth against `room` (5 mm steps, 4 m cap) + # Lattice(i): the 27 poses truth ⊕ offsets dx, dy ∈ {−0.2, 0, 0.2}, dθ ∈ {−0.1, 0, 0.1} +# each case below is a TEST_F(TestMonteCarloLocalization, ) +``` + +## Test cases (Arrange / Act / Assert) + +```cpp +likelihood_field_follows_the_chamfer_metric: + Arrange: a 40 × 40 map with only cell (20, 20) occupied + Assert: distance at cells (20, 20) = 0, (25, 20) = 0.5, (24, 24) ≈ 0.565685, (23, 24) ≈ 0.524264 (Euclidean 0.5) + Distance(world point outside the map) == 2.0 +predict_without_noise_applies_the_increment: + Arrange: MonteCarloLocalization with every particle at FromPose(1, 2, 0.3); normal() ⇒ 0 + Act: Predict(FromPose(0.5, 0, 0.1), normal) + Assert: every particle ≈ (1.477668, 2.147760, 0.4) +predict_noise_scales_with_the_motion: + Arrange: same, normal() ⇒ +1 + Act: Predict(FromPose(0.5, 0, 0.1), normal) + Assert: every particle ≈ FromPose(1, 2, 0.3) * FromPose(0.5 + 0.112250, 0.112250, 0.1 + 0.054772) + = (1.551732, 2.288168, 0.454772) +scan_update_favours_the_true_pose: + Arrange: MonteCarloLocalization, Initialize(Lattice), resampleFraction = 0 (never resample) + Act: Update(Scan(), uniform ⇒ 0.5) + Assert: returns false; MaximumWeight() == truth with weight ≈ 0.995028; Estimate() ≈ (1.0, 1.5, 0.200381) +resampling_concentrates_on_the_true_pose: + Arrange: as above with resampleFraction = 0.5 + Act: Update(Scan(), uniform ⇒ 0.5) + Assert: returns true (ESS ≈ 1.01 < 13.5); every particle == truth; Estimate() ≈ (1.0, 1.5, 0.2) +max_range_beams_are_ignored: + Arrange: Initialize(Lattice) + Act: Update(12 beams of range 4.0, uniform ⇒ 0.5) + Assert: all weights still 1/27 +``` + +## Reference vectors + +- `Scan()` ranges from `(1.0, 1.5, 0.2)`: `2.960, 2.005, 2.535, 2.450, 1.360, 0.950, 0.920, 1.205, 1.480, 1.430, + 1.515, 1.055` m at 0°…330°. +- Log-likelihood at the truth `−0.935538`; 0.2 m off in x, `−15.590720`. +- Noise for `Δ = (0.5, 0, 0.1)`: `σ_xy = 0.112250`, `σ_θ = 0.054772`. + +## Edge cases + +- A beam ending outside the map scores with `maxDistance` (does not crash, does not favour leaving the map). +- `beamStride = 3` uses beams 0, 3, 6, 9 only. diff --git a/roadmap/localization/OccupancyGridMap/explanation.md b/roadmap/localization/OccupancyGridMap/explanation.md new file mode 100644 index 0000000..10ddbf5 --- /dev/null +++ b/roadmap/localization/OccupancyGridMap/explanation.md @@ -0,0 +1,33 @@ +# Occupancy-Grid Mapping — Overview + +## What it is +A map of the robot's surroundings as a grid of small square cells, each holding how likely it is to be +occupied. Every laser or sonar beam updates the cells it passes through (probably free) and the cell where +it hit something (probably occupied). + +## Why it matters (embedded) +Occupancy grids are the standard map for indoor robots: they are what localization matches scans +against, what planners search, and what a robot builds when it explores. The update is pure integer +stepping and additions, so mapping keeps up with the sensor on a small processor. The fixed grid size +makes memory use known in advance. + +## How it works (intuition) +Each cell stores the log of the odds that it is occupied, so combining evidence is just adding. A beam is +traced from the sensor's cell to the cell where it ended with Bresenham's line algorithm. Every cell on +the way gets a small negative amount (the beam passed through, so it is likely free), and the end cell +gets a positive amount (something reflected it). Beams that reached the sensor's maximum range found +nothing, so they only clear. Values are clamped, so a cell that has been seen as free many times can +still become occupied when a door closes. + +## Key parameters +- **resolution** and **origin** — cell size and where the grid sits in the world. +- **occupied / free increments** — how much one beam changes a cell. +- **clamp limits** and **maximum range**. + +## Reference +A. Elfes, "Using occupancy grids for mobile robot perception and navigation," *Computer*, 22(6), 1989; +S. Thrun, W. Burgard, D. Fox, *Probabilistic Robotics* (2005), Ch. 9; J. E. Bresenham, "Algorithm for +computer control of a digital plotter," *IBM Systems Journal*, 4(1), 1965. + +## See also +`MonteCarloLocalization` (E4, uses the map), `math::SE2Transform` (NT66). diff --git a/roadmap/localization/OccupancyGridMap/implementation.md b/roadmap/localization/OccupancyGridMap/implementation.md new file mode 100644 index 0000000..11e68ed --- /dev/null +++ b/roadmap/localization/OccupancyGridMap/implementation.md @@ -0,0 +1,98 @@ +# Occupancy-Grid Mapping (Log-Odds, Ray Casting) — Implementation Pseudocode + +> Roadmap ref: #E2 (★★★☆☆) · Target: `robotics/localization` · Namespace `localization` · Type: `float` (templated on `T`, instantiated for `float` only) + +Bounded 2-D occupancy grid updated from range scans with known sensor poses. Each beam marks the cells +it crosses as freer and its end cell as more occupied, adding clamped log-odds along a Bresenham ray. +Readings at maximum range only clear space. The grid also serves as the map for Monte Carlo +localization (E4) and as the obstacle source for grid planners (P3). + +## Data structures + +```cpp +template # static_assert(std::is_floating_point_v); instantiated for float +struct RangeBeam: + T range # metres + T bearing # radians, sensor frame + +template +struct OccupancyGridParameters: + T resolution # metres per cell + math::Vector2 origin # world position of the corner of cell (0, 0) + T logOddsOccupied # added at a hit, e.g. 0.85 (p = 0.70) + T logOddsFree # added on a pass-through, e.g. −0.4 (p = 0.40) + T logOddsMin, logOddsMax # clamp, e.g. [−2, 3.5]: keeps the map able to change + T maxRange # readings ≥ maxRange mark free space only + +struct GridCell { int x; int y; } + +template +class OccupancyGridMap: + std::array logOdds{} # row-major, 0 = unknown (p = 0.5) + OccupancyGridParameters parameters +``` + +## Interface + +```cpp +explicit OccupancyGridMap(const OccupancyGridParameters& parameters) +void Integrate(const math::SE2Transform& sensorPose, std::span> beams) # hot path +GridCell WorldToCell(const math::Vector2& point) const # floor((p − origin)/resolution) +math::Vector2 CellCenter(GridCell cell) const +bool Contains(GridCell cell) const +T LogOdds(GridCell cell) const; void SetLogOdds(GridCell cell, T value) # clamped; maps loaded from storage +T Probability(GridCell cell) const # 1 − 1/(1 + e^l) +bool IsOccupied(GridCell cell, T threshold) const # l > threshold (0 ⇔ p > 0.5) +const OccupancyGridParameters& Parameters() const +``` + +## Algorithm (pseudocode) + +```text +function Integrate(pose, beams): # OPTIMIZE_FOR_SPEED + start = WorldToCell(pose.translation) + for each (r, b) in beams: + hit = r < maxRange; ρ = min(r, maxRange) + end = WorldToCell(pose.Apply((ρ cos b, ρ sin b))) + for each cell c on Bresenham(start, end), in order: + if not Contains(c): break # leaves the map: stop the ray + Δ = (c == end and hit) ? logOddsOccupied : logOddsFree + logOdds[c] = clamp(logOdds[c] + Δ, logOddsMin, logOddsMax) + +function Bresenham(a, b): # all octants, integer only + dx = |b.x − a.x|; dy = −|b.y − a.y|; sx = sign(b.x − a.x); sy = sign(b.y − a.y); e = dx + dy + loop: emit a; if a == b: stop + e2 = 2e; if e2 ≥ dy: e += dy, a.x += sx; if e2 ≤ dx: e += dx, a.y += sy +``` + +## Complexity & memory + +- `Integrate`: `O(Σ beam length / resolution)` cell updates, integer stepping only. A 360-beam scan at + 4 m and 5 cm resolution is about 29 k additions. +- Memory: `Width·Height` scalars. 200 × 200 cells (10 m at 5 cm) take 160 KB in `float`; size the grid to + the target. No heap. + +## Numerical / embedded notes + +- **Log odds** turn the Bayesian cell update into an addition. The clamp keeps a cell that has been + observed many times able to change when the world changes (doors, people). +- **Max-range readings** have no reliable hit. Marking their end cell occupied would paint phantom walls, + so they only clear. +- The start cell is the sensor's own cell, which is also cleared: a sensor cannot sit inside an obstacle. +- Bresenham visits one cell per major-axis step; diagonal rays skip corner-touching cells. That is the + standard trade-off; an exact traversal (Amanatides–Woo) is a drop-in replacement for the cell walker. +- Rays that leave the grid stop at its border, and their outside end point is never written. +- Float-only: `static_assert(std::is_floating_point_v)`. + +## Deployment + +- Header: `robotics/localization/OccupancyGridMap.hpp` — `#pragma once`, `OPTIMIZE_FOR_SPEED` on + `Integrate`, and `extern template class OccupancyGridMap;` under + `#ifdef ROBOTICS_TOOLBOX_COVERAGE_BUILD`. +- Coverage: `robotics/localization/OccupancyGridMap.cpp` → `template class OccupancyGridMap;` +- Test: `robotics/localization/test/TestOccupancyGridMap.cpp` +- Doc: `doc/localization/OccupancyGridMap.md` (per `doc/TEMPLATE.md`) +- CMake: `.hpp` → `target_sources(robotics.localization ...)`; `.cpp` → `robotics_add_coverage_sources`; + test → `robotics.localization_test`. +- Depends on: NT66 (`SE2Transform`, `Vector2`). +- Generic pattern: see `roadmap/DEPLOYMENT.md`. diff --git a/roadmap/localization/OccupancyGridMap/tests.md b/roadmap/localization/OccupancyGridMap/tests.md new file mode 100644 index 0000000..b7212fc --- /dev/null +++ b/roadmap/localization/OccupancyGridMap/tests.md @@ -0,0 +1,52 @@ +# Occupancy-Grid Mapping — Unit Test Plan (Pseudocode) + +> GoogleTest · `TEST_F` (`float`) · `StrictMock` only · no heap. + +## Fixture + +```cpp +class TestOccupancyGridMap : public ::testing::Test: + OccupancyGridParameters parameters{ 0.1, (0, 0), 0.85, −0.4, −2.0, 3.5, 4.0 } + OccupancyGridMap map{ parameters } # 1 m × 1 m + SE2Transform sensor{ FromPose(0.05, 0.05, 0) } # centre of cell (0, 0) + # NonZeroCells() lists (x, y, logOdds) of every touched cell +# each case below is a TEST_F(TestOccupancyGridMap, ) +``` + +## Test cases (Arrange / Act / Assert) + +```cpp +axis_beam_clears_the_path_and_marks_the_hit: + Act: Integrate(sensor, { (0.5, 0) }) + Assert: cells (0..4, 0) = −0.4; cell (5, 0) = 0.85; nothing else touched +diagonal_and_shallow_beams_follow_bresenham: + Act: Integrate(sensor, { (0.5√2, π/4) }) on one map, Integrate(sensor, { (hypot(0.7, 0.3), atan2(0.3, 0.7)) }) on another + Assert: diagonal: (0,0), (1,1), (2,2), (3,3), (4,4) free, (5,5) occupied + shallow: (0,0), (1,0), (2,1), (3,1), (4,2), (5,2), (6,3) free, (7,3) occupied +max_range_reading_only_clears: + Arrange: parameters.maxRange = 0.5 + Act: Integrate(sensor, { (0.5, 0) }) + Assert: cells (0..5, 0) = −0.4; no occupied cell +ray_leaving_the_map_stops_at_the_border: + Act: Integrate(sensor, { (2.0, 0) }) + Assert: cells (0..9, 0) = −0.4; no occupied cell, no write outside the grid +repeated_hits_clamp_the_log_odds: + Act: 10 × Integrate(sensor, { (0.5, 0) }) + Assert: LogOdds((5, 0)) == 3.5; LogOdds((0, 0)) == −2.0 + Probability((5, 0)) ≈ 0.970688; Probability((0, 0)) ≈ 0.119203 +world_to_cell_respects_the_origin: + Arrange: parameters.origin = (−0.5, −0.5) + Assert: WorldToCell((0, 0)) == (5, 5); WorldToCell((−0.5, −0.5)) == (0, 0); WorldToCell((0.449, −0.051)) == (9, 4) + CellCenter((5, 5)) ≈ (0.05, 0.05) +``` + +## Reference vectors + +- `l = 0.85` ⇔ `p = 0.700567`; `l = −0.4` ⇔ `p = 0.401312`; clamp `[−2, 3.5]` ⇔ `p ∈ [0.119203, 0.970688]`. +- Bresenham `(0,0) → (7,3)`: `(0,0) (1,0) (2,1) (3,1) (4,2) (5,2) (6,3) (7,3)`, the same cells in reverse. + +## Edge cases + +- A beam of range 0 marks only the sensor cell (occupied if `0 < maxRange`, which the caller should + filter as an invalid reading). +- Negative-direction beams (bearing π) walk with `sx = −1` and stop at column 0. diff --git a/roadmap/localization/OdometryFusion/explanation.md b/roadmap/localization/OdometryFusion/explanation.md new file mode 100644 index 0000000..de1f807 --- /dev/null +++ b/roadmap/localization/OdometryFusion/explanation.md @@ -0,0 +1,32 @@ +# Wheel + Gyro Odometry Fusion — Overview + +## What it is +A small Kalman filter that combines a wheeled robot's wheel encoders with a yaw-rate gyro. It gives a +better pose estimate than either alone, learns the gyro's bias as it goes, and notices when the wheels +are slipping. + +## Why it matters (embedded) +Wheel odometry is good at distance but bad at heading: slip and uneven floors twist the estimated path. +A MEMS gyro is good at short-term heading but drifts with its bias. Fusing them on the robot's +microcontroller, at the control rate and with a 4-element state, gives a dead-reckoning estimate that +holds its heading far longer. It also feeds localization (E3, E4) a better motion estimate. + +## How it works (intuition) +Every cycle the robot moves forward by the wheel-measured speed and turns by the gyro rate minus the +current bias estimate, along an exact arc. The uncertainty grows with distance and with the gyro noise. +The wheels also report a turn rate; when it agrees with the bias-corrected gyro, the difference refines +the bias, which is easiest at standstill. When the two disagree beyond what noise can explain, the wheels +are slipping, and the comparison is skipped and reported. Absolute heading or position fixes, when +available, correct the pose through the same update. + +## Key parameters +- **speed noise** (floor plus a fraction of speed), **gyro noise**, **bias random walk**. +- **wheel yaw-rate noise** and the **slip gate** (χ² threshold). + +## Reference +S. Thrun, W. Burgard, D. Fox, *Probabilistic Robotics* (2005), Ch. 3 and 5; J. Borenstein, L. Feng, +"Gyrodometry: a new method for combining data from gyros and odometry in mobile robots," *ICRA* 1996. + +## See also +`PlanarOdometry` (F1), `DifferentialDrive` (W1), `SkidSteerKinematics` (W4); upstream `ResidualKalmanUpdate` +(NT74) and `SE2Transform` (NT66). diff --git a/roadmap/localization/OdometryFusion/implementation.md b/roadmap/localization/OdometryFusion/implementation.md new file mode 100644 index 0000000..9255453 --- /dev/null +++ b/roadmap/localization/OdometryFusion/implementation.md @@ -0,0 +1,100 @@ +# Wheel + Gyro Odometry Fusion (EKF on SE(2)) — Implementation Pseudocode + +> Roadmap ref: #E1 (★★★☆☆) · Target: `robotics/localization` · Namespace `localization` · Type: `float` (templated on `T`, instantiated for `float` only) + +Dead reckoning that trusts the gyro for rotation and the wheels for distance. It estimates the gyro +bias from the agreement between wheel and gyro yaw rates, and refuses that comparison when the wheels +slip. State `(x, y, θ, b_g)`: planar pose plus gyro bias. The mean is propagated with the exact SE(2) +arc (NT66, as in F1). Every measurement update goes through upstream `ResidualKalmanUpdate` (NT74), so +heading residuals are wrapped and slip shows up as a χ²-gated wheel update. Optional absolute heading +(compass, dual-antenna GNSS) and position (GNSS, scan matching E5) updates share the same path. + +## Data structures + +```cpp +template # static_assert(std::is_floating_point_v); instantiated for float +struct OdometryFusionNoise: + T speedNoiseFloor # σ_v at standstill, m/s + T speedNoiseGain # σ_v grows by this fraction of |v| + T gyroNoise # σ_g, rad/s per sample + T biasRandomWalk # σ_b, rad/s/√s + T wheelYawRateNoise # σ_w, rad/s (wheel-derived yaw rate) + T slipGate # χ²(1) gate for the wheel update, e.g. 6.63 (99 %) + +template +class OdometryFusion: + math::Vector state # (x, y, θ, b_g), θ wrapped to (−π, π] + math::SquareMatrix covariance + OdometryFusionNoise noise +``` + +## Interface + +```cpp +OdometryFusion(const math::SE2Transform& pose, const math::SquareMatrix& covariance, const OdometryFusionNoise& noise) +void Predict(T speed, T gyroRate, T dt) # hot path +filters::KalmanUpdateResult UpdateWheelYawRate(T wheelYawRate, T gyroRate) # gated: status gated ⇒ slip +filters::KalmanUpdateResult UpdateHeading(T heading, T sigma) +filters::KalmanUpdateResult UpdatePosition(const math::Vector2& position, const math::SquareMatrix& covariance) +math::SE2Transform Pose() const; T GyroBias() const +const math::Vector& State() const; const math::SquareMatrix& Covariance() const +``` + +`speed` is the forward speed from a wheel model (W1 `Forward`, W4), `gyroRate` the raw z-gyro, and +`wheelYawRate` the wheel model's `ω`. + +## Algorithm (pseudocode) + +```text +function Predict(v, ω_g, dt): # OPTIMIZE_FOR_SPEED + ω = ω_g − b; θm = θ + ω·dt/2 # midpoint heading for the Jacobians + pose = (Pose() * SE2Transform::Exp((v, 0, ω), dt)) # exact arc (NT66) + F = I₄ with F[0,2] = −v·dt·sin θm, F[0,3] = v·dt²/2·sin θm, + F[1,2] = v·dt·cos θm, F[1,3] = −v·dt²/2·cos θm, F[2,3] = −dt + G = [[dt·cos θm, −v·dt²/2·sin θm], [dt·sin θm, v·dt²/2·cos θm], [0, dt], [0, 0]] # ∂f/∂(v, ω_g) + σ_v = speedNoiseFloor + speedNoiseGain·|v| + Q = G·diag(σ_v², σ_g²)·Gᵀ + diag(0, 0, 0, σ_b²·dt) + P = CongruenceTransform(F, P) + Q; state = (pose.x, pose.y, pose.Heading(), b) + +function UpdateWheelYawRate(ω_w, ω_g): # both rates measure the true ω; the difference observes b + r = (ω_w − (ω_g − b)); H = [0, 0, 0, −1]; R = σ_w² + σ_g² + return Apply(state, P, r, H, R, slipGate); wrap θ # NT74; gated ⇒ wheels slipping, nothing applied + +function UpdateHeading(θ_m, σ): + r = AngleDifference(θ_m, θ); H = [0, 0, 1, 0]; return Apply(state, P, r, H, σ², ∞); wrap θ + +function UpdatePosition(p, Σ): + r = p − (x, y); H = [[1, 0, 0, 0], [0, 1, 0, 0]]; return Apply(state, P, r, H, Σ, ∞); wrap θ +``` + +## Complexity & memory + +- `Predict`: one `Exp`, one 4×4 congruence and a 4×2 noise map. Updates: NT74 with `M ≤ 2`. +- Memory: 20 scalars of state plus the noise block. No heap. + +## Numerical / embedded notes + +- **Why a bias state.** An uncorrected 0.02 rad/s gyro bias turns 5 s at standstill into a 0.1 rad heading + error. With the wheel update the estimated bias reaches 0.019992 and the heading error stays below + `4e-5` (test). Standstill, where the wheel yaw rate is exactly 0, is the strongest bias observation. +- **Slip awareness.** During slip (spinning on carpet, a stuck caster) the wheel yaw rate disagrees with + the gyro far beyond `σ_w`. The gate rejects the update, and the returned status lets the caller flag + slip or raise `speedNoiseGain` for the following predicts. +- **Linearisation.** The mean uses the exact arc; the Jacobians use the midpoint heading, whose difference + from the exact arc's Jacobian is `O(v·ω²·dt³)`. +- `R = σ_w² + σ_g²` in the wheel update because the gyro sample enters the predicted rate. +- An accelerometer can later add a lateral-slip test (`a_y ≈ v·ω`). It is left out to keep the state at 4. +- Float-only: `static_assert(std::is_floating_point_v)`. + +## Deployment + +- Header: `robotics/localization/OdometryFusion.hpp` — `#pragma once`, `OPTIMIZE_FOR_SPEED` on `Predict`, + and `extern template class OdometryFusion;` under `#ifdef ROBOTICS_TOOLBOX_COVERAGE_BUILD`. +- Coverage: `robotics/localization/OdometryFusion.cpp` → `template class OdometryFusion;` +- Test: `robotics/localization/test/TestOdometryFusion.cpp` +- Doc: `doc/localization/OdometryFusion.md` (per `doc/TEMPLATE.md`) +- CMake: new module `robotics/localization` (`robotics.localization`, test target + `robotics.localization_test`), created by the first `localization` item deployed. +- Depends on: F1 (motion integration convention), NT66 (`SE2Transform`), NT54 (`AngleDifference`, `WrapToPi`), + NT74 (`ResidualKalmanUpdate`); upstream `MatrixOperations::CongruenceTransform`. +- Generic pattern: see `roadmap/DEPLOYMENT.md`. diff --git a/roadmap/localization/OdometryFusion/tests.md b/roadmap/localization/OdometryFusion/tests.md new file mode 100644 index 0000000..d4cda47 --- /dev/null +++ b/roadmap/localization/OdometryFusion/tests.md @@ -0,0 +1,50 @@ +# Odometry Fusion — Unit Test Plan (Pseudocode) + +> GoogleTest · `TEST_F` (`float`) · `StrictMock` only · no heap. + +## Fixture + +```cpp +class TestOdometryFusion : public ::testing::Test: + OdometryFusionNoise noise{ 0.01, 0.05, 0.01, 1e-4, 0.02, 6.63 } + SquareMatrix P0{ diag(0.01, 0.01, 0.01, 0.0025) } + OdometryFusion fusion{ Identity, P0, noise } +# each case below is a TEST_F(TestOdometryFusion, ) +``` + +## Test cases (Arrange / Act / Assert) + +```cpp +straight_drive_grows_the_covariance_along_and_across: + Act: 10 × Predict(1, 0, 0.1) + Assert: State() ≈ (1, 0, 0, 0); diag(P) ≈ (0.010360, 0.020628, 0.012510, 0.0025); P(1,2) ≈ 0.011255 +arc_uses_the_exact_se2_exponential: + Act: 20 × Predict(1, 0.5, 0.1) + Assert: Pose().Pose() ≈ (1.682942, 0.919395, 1.0) == Exp((1, 0, 0.5), 2) +standstill_estimates_the_gyro_bias: + Act: 500 × { Predict(0, 0.02, 0.01); UpdateWheelYawRate(0, 0.02) } + Assert: GyroBias() ≈ 0.019992 (tol 1e-4); |θ| < 1e-4; P(3,3) ≈ 1.02e-6 + # after the first update the bias is already 0.016667 +slip_is_gated_and_leaves_the_state_unchanged: + Arrange: Predict(1, 0, 0.1) + Act: result = UpdateWheelYawRate(1.0, 0.0) + Assert: result.status == gated, nis ≈ 333.33; State() and Covariance() unchanged +heading_update_wraps_the_residual: + Arrange: fusion at FromPose(0, 0, 3.1) + Act: UpdateHeading(−3.1, 0.05) + Assert: status applied; θ ≈ −3.116637 # K = 0.8 on the wrapped residual 0.083185 +position_update_pulls_the_pose_and_shrinks_the_covariance: + Act: UpdatePosition((0.1, −0.05), diag(0.01, 0.01)) + Assert: State() ≈ (0.05, −0.025, 0, 0); P(0,0) ≈ P(1,1) ≈ 0.005; nis ≈ 0.625 +``` + +## Reference vectors + +- Gyro bias 0.02 rad/s for 5 s without wheel updates ⇒ heading 0.1 rad; with them ⇒ `4e-5` rad. +- Straight drive (v = 1, dt = 0.1, 10 steps): the cross-track variance doubles (`0.01 → 0.020628`) through + the heading uncertainty. + +## Edge cases + +- `v = 0` and `ω_g − b = 0` ⇒ `F = I` except the bias column; covariance grows only by `Q`. +- A singular update (all-zero covariance and noise) returns `singular` and changes nothing. diff --git a/roadmap/localization/ScanMatcher/explanation.md b/roadmap/localization/ScanMatcher/explanation.md new file mode 100644 index 0000000..384a2e9 --- /dev/null +++ b/roadmap/localization/ScanMatcher/explanation.md @@ -0,0 +1,34 @@ +# 2-D Scan Matching (ICP) — Overview + +## What it is +A method to work out how a robot moved between two laser scans, or where a scan sits on a map, by +overlaying the scans. It repeatedly pairs each new point with the nearest old point and solves for the +shift and rotation that line the pairs up, until the overlay stops improving. + +## Why it matters (embedded) +Scan matching turns a 2-D lidar into an odometry sensor that does not slip, corrects wheel-odometry +drift, and is the front end of most 2-D SLAM systems. On a small processor it has to run with bounded +memory and a bounded number of iterations, and it must notice when the geometry, such as a featureless +corridor, cannot pin the motion down. + +## How it works (intuition) +Starting from a guess (usually odometry), the current scan is moved onto the reference and each point is +matched to its closest reference point. Pairs that are too far apart are treated as clutter. In the +simplest variant the best rigid motion for these pairs is computed directly, and the process repeats +with new pairs. Because two scans rarely hit a wall at exactly the same spots, a better variant measures +only how far each point is from the wall (the line through its two nearest reference points). It then +solves for the motion that removes those across-wall distances, which converges faster and lands exactly +on the wall. + +## Key parameters +- **metric** — point-to-point or point-to-line. +- **maximum correspondence distance** — the outlier bound. +- **iteration limit** and **convergence tolerances**; **refinement steps** per iteration. + +## Reference +P. J. Besl, N. D. McKay, "A method for registration of 3-D shapes," *IEEE T-PAMI*, 14(2), 1992; +A. Censi, "An ICP variant using a point-to-line metric," *ICRA* 2008. + +## See also +`OdometryFusion` (E1, consumes the result as a position update), `MonteCarloLocalization` (E4); upstream +`PointSetAlignment` (NT67), `LevenbergMarquardt` (NT64), `SE2Transform` (NT66). diff --git a/roadmap/localization/ScanMatcher/implementation.md b/roadmap/localization/ScanMatcher/implementation.md new file mode 100644 index 0000000..c63f5c6 --- /dev/null +++ b/roadmap/localization/ScanMatcher/implementation.md @@ -0,0 +1,117 @@ +# 2-D Scan Matching (Point-to-Point / Point-to-Line ICP) — Implementation Pseudocode + +> Roadmap ref: #E5 (★★★★☆) · Target: `robotics/localization` · Namespace `localization` · Type: `float` (templated on `T`, instantiated for `float` only) + +Iterative closest point between a reference scan (the previous scan, or points from a map) and the +current scan. It finds the transform from the current sensor frame to the reference frame. Each iteration +associates every current point with its nearest reference point within a distance bound, then solves for +the transform: +- **point-to-point:** weighted Kabsch in 2-D (NT67); +- **point-to-line:** a few Levenberg–Marquardt steps on the normal distances to the local reference + segments (NT64), with SE(2) retraction (NT66). + +Point-to-line converges in fewer iterations and has no sampling bias when the two scans sample the walls +at different spots; the tests show both. + +## Data structures + +```cpp +enum class IcpMetric : std::uint8_t { pointToPoint, pointToLine } + +template # static_assert(std::is_floating_point_v); instantiated for float +struct ScanMatcherParameters: + IcpMetric metric + T maxCorrespondenceDistance # pairs farther apart are outliers + std::size_t maxIterations + T translationTolerance, rotationTolerance # convergence on the per-iteration increment + std::size_t minCorrespondences # fail below this (≥ 3) + std::size_t refinementIterations # LM steps per ICP iteration (point-to-line), e.g. 3 + +template +struct ScanMatchResult: + math::SE2Transform transform # current sensor frame → reference frame + std::size_t iterations, correspondences + T rms # over accepted pairs, in the chosen metric + bool converged + +template +class ScanMatcher: + ScanMatcherParameters parameters + # per-call work arrays on the stack: transformed points, matched reference points, normals, weights (MaxPoints each) +``` + +## Interface + +```cpp +explicit ScanMatcher(const ScanMatcherParameters& parameters) +std::optional> Match(std::span> reference, # ordered by bearing (scan order) + std::span> current, + const math::SE2Transform& initialGuess) const +``` + +`nullopt` when either span exceeds `MaxPoints`, when an iteration has fewer than `minCorrespondences` +pairs, or when an LM step fails (degenerate geometry). + +## Algorithm (pseudocode) + +```text +function Match(ref, cur, T): + for it in [1, maxIterations]: + yᵢ = T.Apply(curᵢ) + for each i: # brute-force nearest neighbour, O(N·M) + j = argmin_j ‖ref_j − yᵢ‖; accept if ‖ref_j − yᵢ‖ ≤ maxCorrespondenceDistance (weight 1, else 0) + if pointToLine: # local segment: j and its closer scan-order neighbour + k = closer of j − 1, j + 1 to yᵢ; t = ref_k − ref_j; nᵢ = (−t.y, t.x)/‖t‖ + if accepted < minCorrespondences: return nullopt + if pointToPoint: + a = PointSetAlignment::Align(y, matched ref, false, weights) # NT67 + if not a: return nullopt + ΔT = SE2 from a.rotation, a.translation + else: + N = JᵀJ of PointToLineProblem at Identity # 3×3 + if λ_min(N) ≤ 1e-6·λ_max(N): return nullopt # JacobiEigenSolver: unobservable direction + ΔT = LevenbergMarquardt over PointToLineProblem (Identity start, refinementIterations steps) # NT64 + T = ΔT * T + if ‖ΔT.translation‖ < translationTolerance and |ΔT.Heading()| < rotationTolerance: + return { T, it, accepted, rms(T), true } + return { T, maxIterations, accepted, rms(T), false } + +class PointToLineProblem : optimization::LeastSquaresProblem, MaxPoints, 3>: + Residual(D): eᵢ = wᵢ · nᵢ·(D.Apply(yᵢ) − qᵢ) # unused rows are 0 + Jacobian(D): rowᵢ = wᵢ · [nᵢ.x, nᵢ.y, nᵢ.y·y′ᵢ.x − nᵢ.x·y′ᵢ.y] with y′ᵢ = D.Apply(yᵢ) + Retract(D, δ) = FromPose(δ) * D # left perturbation +``` + +## Complexity & memory + +- Association: `O(N·M)` per iteration (66 × 68 points ≈ 4.5 k distances). For full 360-beam scans, use + scan-order (projective) association or a bounded k-d tree; the matcher's interface does not change. +- Point-to-line: an LM step forms a `MaxPoints × 3` Jacobian and a 3×3 normal system. +- Stack: about `8·MaxPoints` scalars of work arrays plus the LM Jacobian (`3·MaxPoints`). No heap. + +## Numerical / embedded notes + +- **Sampling bias.** Two scans rarely hit the same wall spots. Point-to-point then pulls the points + towards the reference samples and settles 3 cm off (test). Point-to-line measures only the distance + across the wall, so it recovers the transform exactly. +- **Initial guess.** ICP converges locally. Seed it with odometry (F1, E1); with a 0.15 m / 0.1 rad guess + error the test still converges with point-to-line. +- **Degeneracy.** A corridor (parallel walls only) leaves the motion along the walls unobservable: the + point-to-line normal matrix `JᵀJ` has a zero eigenvalue. The eigenvalue-ratio check makes `Match` return + `nullopt` rather than a drifted answer, and the caller falls back to odometry along that axis. +- Outlier points (people, glass) beyond `maxCorrespondenceDistance` get weight 0 and do not move the + solution (test). +- Float-only: `static_assert(std::is_floating_point_v)`. + +## Deployment + +- Header: `robotics/localization/ScanMatcher.hpp` — `#pragma once`, `OPTIMIZE_FOR_SPEED` on `Match`, and + `extern template class ScanMatcher;` under `#ifdef ROBOTICS_TOOLBOX_COVERAGE_BUILD`. +- Coverage: `robotics/localization/ScanMatcher.cpp` → `template class ScanMatcher;` +- Test: `robotics/localization/test/TestScanMatcher.cpp` +- Doc: `doc/localization/ScanMatcher.md` (per `doc/TEMPLATE.md`) +- CMake: `.hpp` → `target_sources(robotics.localization ...)`; `.cpp` → `robotics_add_coverage_sources`; + test → `robotics.localization_test`. +- Depends on: NT66 (`SE2Transform`), NT67 (`PointSetAlignment`), NT64 (`LevenbergMarquardt`, `ConstantDamping`); + upstream `JacobiEigenSolver`. +- Generic pattern: see `roadmap/DEPLOYMENT.md`. diff --git a/roadmap/localization/ScanMatcher/tests.md b/roadmap/localization/ScanMatcher/tests.md new file mode 100644 index 0000000..5321e5e --- /dev/null +++ b/roadmap/localization/ScanMatcher/tests.md @@ -0,0 +1,56 @@ +# Scan Matching — Unit Test Plan (Pseudocode) + +> GoogleTest · `TEST_F` (`float`) · `StrictMock` only · no heap. + +## Fixture + +```cpp +class TestScanMatcher : public ::testing::Test: + # reference: 68 points on three walls, in scan order: + # x = 2 for y = −1.0 … 1.5 (26 points), y = 1.5 for x = −0.5 … 2.0 (26), y = −1 for x = −0.5 … 1.0 (16) + std::array, 68> reference + SE2Transform truth{ FromPose(0.1, −0.05, 0.05) } # current → reference + # sameSpots: truth.Inverse() applied to every reference point (exact correspondences exist) + # halfSpots: wall points at half-spacing offsets (y = −0.95 … 1.45, x = −0.45 … 1.95, x = −0.45 … 0.95; + # 65 points) mapped by truth.Inverse() + ScanMatcherParameters parameters{ pointToLine, 0.5, 50, 1e-6, 1e-6, 3, 3 } +# each case below is a TEST_F(TestScanMatcher, ) +``` + +## Test cases (Arrange / Act / Assert) + +```cpp +both_metrics_recover_the_transform_with_exact_correspondences: + Act: Match(reference, sameSpots, Identity) with each metric + Assert: transform ≈ truth (1e-4); correspondences == 68; converged; rms ≈ 0 + iterations: point-to-point 3, point-to-line 2 +point_to_line_removes_the_sampling_bias: + Act: Match(reference, halfSpots, Identity) with each metric + Assert: point-to-line: transform ≈ truth (1e-4), 65 correspondences + point-to-point: max |error| of (x, y, θ) > 0.01 # double reference: 0.034 +point_to_line_converges_from_an_offset_guess: + Act: Match(reference, sameSpots, FromPose(−0.05, 0.05, −0.05)) + Assert: transform ≈ truth (1e-4) +far_outlier_is_rejected: + Arrange: halfSpots plus the point (0.6, 0.2) # 1.2 m from every wall + Act: Match(reference, that, Identity) + Assert: correspondences == 65 (of 66); transform ≈ truth (1e-4) +too_few_correspondences_fail: + Arrange: maxCorrespondenceDistance = 0.01 + Assert: Match(reference, halfSpots, Identity) == nullopt +corridor_is_reported_as_degenerate: + Arrange: reference and current both on the two walls y = ±1 (x = −1 … 1, 21 points each), current shifted by (0.1, 0) + Assert: point-to-line Match(...) == nullopt # n_x = 0 for every pair: JᵀJ has a zero eigenvalue +``` + +## Reference vectors + +- `truth = (0.1, −0.05, 0.05)`. +- Half-spacing point-to-point fixed point: `(0.065830, −0.062154, 0.043042)`, an error of 0.034. +- From the offset guess `(−0.05, 0.05, −0.05)` with `sameSpots`, point-to-point stalls at + `(0.062305, −0.054129, 0.016504)` while point-to-line reaches `truth` in 2 iterations. + +## Edge cases + +- `current` larger than `MaxPoints` ⇒ `nullopt`. +- `maxIterations` reached without meeting the tolerances ⇒ `converged == false` with the last transform. diff --git a/roadmap/mobile/PlanarOdometry/implementation.md b/roadmap/mobile/PlanarOdometry/implementation.md index d3989dd..c91d046 100644 --- a/roadmap/mobile/PlanarOdometry/implementation.md +++ b/roadmap/mobile/PlanarOdometry/implementation.md @@ -13,6 +13,11 @@ position, as they do with Euler or Runge-Kutta integration of `(ẋ, ẏ, θ̇)` template # static_assert(std::is_floating_point_v); instantiated for float class PlanarOdometry: math::SE2Transform pose # body frame in the odometry frame (NT66) + +template +struct OdometryNoise: # increment noise shared by the localization filters (E3, E4) + T rotationFromRotation, rotationFromTranslation # α₁, α₂: σ_θ² = α₁Δθ² + α₂Δs² + T translationFromTranslation, translationFromRotation # α₃, α₄: σ_xy² = α₃Δs² + α₄Δθ² ``` ## Interface @@ -24,6 +29,10 @@ void Integrate(const math::Vector3& twist, T dt) # body twist (vx, void Reset(const math::SE2Transform& pose) const math::SE2Transform& Pose() const static math::Vector3 Increment(const math::SE2Transform& from, const math::SE2Transform& to) + +template +math::Vector3 IncrementDeviation(const math::SE2Transform& increment, const OdometryNoise& noise) + # (σ_xy, σ_xy, σ_θ) of the body-frame increment ``` ## Algorithm (pseudocode) @@ -34,6 +43,11 @@ function Integrate(ξ, dt): # OPTIMIZE_FOR_SPEED function Increment(from, to): # body-frame motion between two odometry readings return (from.Inverse() * to).Log() # (Δx, Δy, Δθ) as a twist held for unit time + +function IncrementDeviation(Δ, α): # noise grows with distance and rotation; same for reverse + (dx, dy, dθ) = Δ.Pose(); Δs² = dx² + dy² + σ_xy = √(α₃Δs² + α₄dθ²); σ_θ = √(α₁dθ² + α₂Δs²) + return (σ_xy, σ_xy, σ_θ) ``` ## Complexity & memory @@ -53,13 +67,16 @@ function Increment(from, to): # body-frame motion between two heading change between the two readings to stay below `π`; sample faster than half a turn. - Odometry error grows without bound. Wheel-radius and track-width errors dominate it, which is the wheel models' job to calibrate (UMBmark), not this integrator's. +- `IncrementDeviation` is Thrun's odometry noise model applied to the body-frame increment instead of + the rotate–translate–rotate split, whose first rotation jumps by π when the robot reverses. - Float-only: `static_assert(std::is_floating_point_v)`. ## Deployment - Header: `robotics/mobile/PlanarOdometry.hpp` — `#pragma once`, `OPTIMIZE_FOR_SPEED` on `Integrate`, and `extern template class PlanarOdometry;` under `#ifdef ROBOTICS_TOOLBOX_COVERAGE_BUILD`. -- Coverage: `robotics/mobile/PlanarOdometry.cpp` → `template class PlanarOdometry;` +- Coverage: `robotics/mobile/PlanarOdometry.cpp` → `template class PlanarOdometry;` and + `template math::Vector3 IncrementDeviation(...);` - Test: `robotics/mobile/test/TestPlanarOdometry.cpp` - Doc: `doc/mobile/PlanarOdometry.md` (per `doc/TEMPLATE.md`) - CMake: new module `robotics/mobile` (`robotics.mobile`, test target `robotics.mobile_test`), created by diff --git a/roadmap/mobile/PlanarOdometry/tests.md b/roadmap/mobile/PlanarOdometry/tests.md index 4629f4b..ba78049 100644 --- a/roadmap/mobile/PlanarOdometry/tests.md +++ b/roadmap/mobile/PlanarOdometry/tests.md @@ -30,6 +30,10 @@ increment_recovers_the_integrated_twist: reset_replaces_the_pose: Act: Integrate(arc, 1); Reset(FromPose(−1, 3, 2)) Assert: Pose().Pose() ≈ (−1, 3, 2) +increment_deviation_grows_with_distance_and_rotation: + Arrange: α = (0.05, 0.01, 0.05, 0.01) + Assert: IncrementDeviation(FromPose(0.5, 0, 0.1), α) ≈ (0.112250, 0.112250, 0.054772) + IncrementDeviation(FromPose(−0.5, 0, 0), α) == IncrementDeviation(FromPose(0.5, 0, 0), α) long_runs_keep_a_unit_rotation: Act: 10⁵ × Integrate(arc, 1e-3) Assert: cosine² + sine² ≈ 1 (tol 1e-6)