From 33f33afce0d7c5dc56e0c5056ca0a57182abc649 Mon Sep 17 00:00:00 2001 From: Claude Date: Fri, 6 Nov 2026 08:38:26 +0000 Subject: [PATCH 1/3] build: bump numerical-toolbox for ParticleFilter (NT70) Co-Authored-By: Claude Opus 5.5 Claude-Session: https://claude.ai/code/session_014WUWh44uw7k4JjMd6YkAwN --- CMakeLists.txt | 2 +- ROADMAP.md | 8 ++++---- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 5621b55..dd7b031 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -47,7 +47,7 @@ if (NOT TARGET numerical.math) FetchContent_Declare( numerical_toolbox GIT_REPOSITORY https://github.com/embedded-pro/numerical-toolbox-cpp.git - GIT_TAG 182e068ef3239a585eb4beb47175945a724788a6 # main + GIT_TAG 6080be4e519bb199be7447055491d27c7d96e25d # main ) set(NUMERICAL_TOOLBOX_ENABLE_OPTIMIZATIONS ${ROBOTICS_TOOLBOX_ENABLE_OPTIMIZATIONS} CACHE BOOL "" FORCE) diff --git a/ROADMAP.md b/ROADMAP.md index 9f09bea..e112fd9 100644 --- a/ROADMAP.md +++ b/ROADMAP.md @@ -40,8 +40,8 @@ this roadmap needs that upstream does not ship yet are specified and built upstr Generic algorithms live in numerical-toolbox-cpp and are referenced here as **NT** plus its master-list item number (NT53 is upstream item 53). A robotics item that depends on one needs a -numerical-toolbox pin that ships it. NT53–NT67 and NT74 have shipped upstream, and their links point to the -upstream docs; the current pin (`182e068`) includes them. +numerical-toolbox pin that ships it. NT53–NT67, NT70 and NT74 have shipped upstream, and their links point to the +upstream docs; the current pin (`6080be4`) includes them. | # | Component | Upstream module | Spec / doc | Needed by | |------|-------------------------------------------------------|----------------------|-----------------------------------|----------------------------------------------------------------------------| @@ -62,7 +62,7 @@ upstream docs; the current pin (`182e068`) includes them. | NT67 | Kabsch / Umeyama point-set alignment | `solvers` | [PointSetAlignment][nt67] | 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 | +| NT70 | Particle filter (bounded, systematic resampling) | `filters/active` | [ParticleFilter][nt70] | 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 | @@ -84,7 +84,7 @@ upstream docs; the current pin (`182e068`) includes them. [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/doc/math/SE2Transform.md [nt67]: https://github.com/embedded-pro/numerical-toolbox-cpp/blob/main/doc/solvers/PointSetAlignment.md -[nt70]: https://github.com/embedded-pro/numerical-toolbox-cpp/blob/main/roadmap/filters/active/ParticleFilter/implementation.md +[nt70]: https://github.com/embedded-pro/numerical-toolbox-cpp/blob/main/doc/filters/active/ParticleFilter.md [nt74]: https://github.com/embedded-pro/numerical-toolbox-cpp/blob/main/doc/filters/active/ResidualKalmanUpdate.md ## Robot manipulators & other manipulator types From c36baa4d9a9bfa180b566e6625bdb04ec286b189 Mon Sep 17 00:00:00 2001 From: Claude Date: Fri, 6 Nov 2026 08:43:19 +0000 Subject: [PATCH 2/3] feat(localization): add MonteCarloLocalization (E4) Particle-filter localization in an occupancy grid: a two-pass 8-neighbour chamfer likelihood field, a noisy odometry motion model from IncrementDeviation, a likelihood-field scan score with a random-measurement floor and beam stride, ESS-triggered systematic resampling and a circular-mean pose estimate, on top of upstream filters::ParticleFilter (NT70). Co-Authored-By: Claude Opus 5.5 Claude-Session: https://claude.ai/code/session_014WUWh44uw7k4JjMd6YkAwN --- README.md | 2 +- ROADMAP.md | 8 +- doc/localization/MonteCarloLocalization.md | 97 +++++++ doc/localization/README.md | 13 +- roadmap/README.md | 5 +- .../MonteCarloLocalization/explanation.md | 33 --- .../MonteCarloLocalization/implementation.md | 117 -------- .../MonteCarloLocalization/tests.md | 60 ---- robotics/localization/CMakeLists.txt | 2 + .../localization/MonteCarloLocalization.cpp | 14 + .../localization/MonteCarloLocalization.hpp | 273 ++++++++++++++++++ robotics/localization/test/CMakeLists.txt | 1 + .../test/TestMonteCarloLocalization.cpp | 190 ++++++++++++ 13 files changed, 587 insertions(+), 228 deletions(-) create mode 100644 doc/localization/MonteCarloLocalization.md delete mode 100644 roadmap/localization/MonteCarloLocalization/explanation.md delete mode 100644 roadmap/localization/MonteCarloLocalization/implementation.md delete mode 100644 roadmap/localization/MonteCarloLocalization/tests.md create mode 100644 robotics/localization/MonteCarloLocalization.cpp create mode 100644 robotics/localization/MonteCarloLocalization.hpp create mode 100644 robotics/localization/test/TestMonteCarloLocalization.cpp diff --git a/README.md b/README.md index afed608..6bb5cf2 100644 --- a/README.md +++ b/README.md @@ -42,7 +42,7 @@ project already defines those targets. Includes are namespaced by domain, e.g. | [Planning](doc/planning/README.md) | Primitive Distance Queries, Artificial Potential Fields, Self-Collision Checking, Convex Collision (GJK and EPA), Grid Search (A* and Dijkstra), RRT-Connect, Dubins Shortest Paths, Dynamic Window Approach, Reeds–Shepp Shortest Paths | | [Trajectory](doc/trajectory/README.md) | Cubic Spline Trajectory, Cartesian Path with SLERP Orientation | | [Mobile Robots](doc/mobile/README.md) | Planar Odometry, Differential-Drive Kinematics, Car-Like Kinematics, Omnidirectional Kinematics, Skid-Steer Kinematics, Pure-Pursuit Path Tracking, Kanayama Trajectory Tracking, Stanley Path Tracking | -| [Localization & Mapping](doc/localization/README.md) | Occupancy-Grid Mapping, Odometry Fusion, EKF Landmark Localization, 2-D Scan Matching (ICP) | +| [Localization & Mapping](doc/localization/README.md) | Occupancy-Grid Mapping, Odometry Fusion, EKF Landmark Localization, 2-D Scan Matching (ICP), Monte Carlo Localization | | [Manipulator Control](doc/controllers/manipulator/README.md) | PD Control with Gravity Compensation, Computed-Torque Control, Impedance Control, Operational-Space Control, Hybrid Position/Force Control, Slotine–Li Adaptive Control, Cable Tension Distribution | Each category page lists its algorithms with a brief description and links to the detailed diff --git a/ROADMAP.md b/ROADMAP.md index e112fd9..3c05bbd 100644 --- a/ROADMAP.md +++ b/ROADMAP.md @@ -288,13 +288,7 @@ All planning items have shipped: see [Planning](doc/planning/README.md). ## Localization and mapping -| # | Component | Target module | Depends on | Difficulty | -|----|--------------------------|----------------|--------------|------------| -| E4 | Monte Carlo localization | `localization` | NT70, E2, F1 | ★★★★☆ | - -**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. +All localization items have shipped: see [Localization & Mapping](doc/localization/README.md). ## Calibration diff --git a/doc/localization/MonteCarloLocalization.md b/doc/localization/MonteCarloLocalization.md new file mode 100644 index 0000000..a0cb9a5 --- /dev/null +++ b/doc/localization/MonteCarloLocalization.md @@ -0,0 +1,97 @@ +# Monte Carlo Localization (Likelihood Field) + +## Overview & Motivation + +A robot with a map still has to find out where it is on that map. At power-up it may be anywhere. After a collision or being carried, its estimate may be wrong. In a building of repeated corridors, several places may look the same until the robot drives far enough to tell them apart. A Gaussian filter, with its single mode, cannot represent any of these situations. + +**Monte Carlo localization** (MCL) represents the belief over poses with a set of particles, each a candidate pose with a weight. Odometry moves every particle with its own noise sample. Each laser scan reweights the particles by how well the scan fits the map from that pose. Resampling then concentrates the particles on the poses that explain the data. +Started from particles spread over the whole map, MCL localizes globally; started near a known pose, it tracks. + +The **likelihood field** sensor model makes the scan evaluation cheap. Instead of ray-casting each beam through the map, it looks up how far each beam's end point lies from the nearest obstacle. These distances are precomputed once per map. + +## Mathematical Theory + +### Likelihood Field + +From the occupancy grid, every cell gets the distance to its nearest occupied cell, computed by a two-pass 8-neighbour chamfer transform. The forward pass runs over rows and columns in ascending order and relaxes each cell from its west, south and two southern diagonal neighbours. The backward pass runs in descending order and relaxes from the east, north and two northern diagonal neighbours: + +$$d(c) \leftarrow \min\big(d(c),\; d(c') + w(c, c')\big), \qquad w = 1 \text{ (axial)}, \;\; \sqrt{2} \text{ (diagonal)}$$ + +Distances are scaled by the cell size and capped at $d_{\max}$. Points outside the map score $d_{\max}$. The chamfer metric overestimates the Euclidean distance by at most about 8 %, well inside the sensor model's spread. + +### Motion Model + +For an odometry increment $\Delta = (d_x, d_y, d_\theta)$ in the body frame, the increment noise model gives the deviations $(\sigma_{xy}, \sigma_{xy}, \sigma_\theta)$. These grow with the distance and rotation travelled. Each particle draws its own noisy increment from a caller-supplied standard normal sampler and composes it: + +$$x^{(i)} \leftarrow x^{(i)} \oplus \big(d_x + \sigma_{xy}\,n_1,\;\; d_y + \sigma_{xy}\,n_2,\;\; d_\theta + \sigma_\theta\,n_3\big)$$ + +### Sensor Model + +For a particle, the sensor pose is the particle pose composed with the sensor mount. Each used beam's end point $e_k$ is scored against the likelihood field: + +$$\ell = \sum_k \log\left(z_{\text{hit}}\, e^{-d(e_k)^2 / 2\sigma_{\text{hit}}^2} + \frac{z_{\text{rand}}}{r_{\max}}\right)$$ + +The random-measurement floor $z_{\text{rand}}/r_{\max}$ keeps one unexplained beam, such as a person or glass, from eliminating a particle. Beams at or beyond the maximum range carry no end point and are skipped. A beam stride uses every $k$-th beam to save computation. + +### Filtering and Estimate + +The log-likelihoods update the particle weights in the log domain. The particles are resampled systematically, from a caller-supplied uniform number, when the effective sample size falls below a fraction of the particle count. The pose estimate is the weighted mean, with the heading averaged as a unit vector so it stays correct across $\pm\pi$: + +$$\bar\theta = \operatorname{atan2}\Big(\sum_i w_i \sin\theta_i,\; \sum_i w_i \cos\theta_i\Big)$$ + +## Complexity Analysis + +| Operation | Time | Space | Notes | +|------------------|--------------|----------|-----------------------------------------------------| +| Likelihood field | $O(W H)$ | $O(W H)$ | Two raster passes, once per map | +| Predict | $O(N)$ | $O(1)$ | Three normal draws and one composition per particle | +| Update | $O(N B / s)$ | $O(N)$ | One lookup, exponential and logarithm per used beam | +| Particle store | — | $O(N)$ | Two particle buffers, weights and scratch | + +For $N$ particles, $B$ beams per scan, stride $s$ and a $W \times H$ grid. 500 particles with 60 beams cost about 30 000 field lookups per scan. + +## Step-by-Step Walkthrough + +A $4 \times 4$ m room at $10$ cm resolution has walls on its border, an inner wall and a shelf. The robot stands at $(1.0, 1.5)$ heading $0.2$ rad and takes a 12-beam scan every $30°$: + +1. **Likelihood field**: around a single occupied cell, cells 5 apart along an axis are $0.5$ m away, and 4 apart diagonally $0.566$ m. The cell at $(3, 4)$ cells is $0.524$ m away by the chamfer metric, against $0.5$ m Euclidean. +2. **Noise-free prediction**: particles at $(1, 2, 0.3)$ moved by $0.5$ m and $0.1$ rad all reach $(1.478, 2.148, 0.4)$. +3. **Noisy prediction**: with every normal draw at $+1$, the increment grows by $\sigma_{xy} = 0.112$ m and $\sigma_\theta = 0.055$ rad, and the particles reach $(1.552, 2.288, 0.455)$. +4. **Scan update**: over 27 candidate poses offset by $\pm 0.2$ m and $\pm 0.1$ rad around the truth, the true pose takes $99.5$ % of the weight. The estimate is $(1.0, 1.5, 0.2004)$. +5. **Resampling**: with the effective sample size at about $1$, far below half of 27, resampling places every particle on the true pose. +6. **Maximum-range scan**: a scan where every beam hits nothing within range leaves the weights uniform. + +## Pitfalls & Edge Cases + +- **Particle deprivation**: with too few particles, or too little motion noise, the filter can lose the true pose. Size the particle set for the area, and keep the noise realistic. +- **Kidnapping**: once every particle is wrong, MCL cannot recover by itself. Inject uniformly drawn particles when the average likelihood drops (augmented MCL). +- **Overconfident sensor model**: a small $\sigma_{\text{hit}}$ or no random floor collapses the weights in one scan. Keep the floor and use a beam stride for correlated beams. +- **Map quality**: unmapped furniture and people violate the static-world assumption. The random floor absorbs some of it. +- **Determinism**: all randomness comes from the caller's samplers, so a seeded generator makes runs reproducible. + +## Variants & Generalizations + +- **KLD-sampling MCL**: adapts the particle count to the spread of the belief. +- **Augmented MCL**: recovers from kidnapping by injecting random particles when likelihoods drop. +- **Beam and ray-casting models**: model occlusions explicitly at a higher cost per beam. +- **Scan-matching refinement**: polishes the best particle with ICP for precise tracking. + +## Applications + +- Indoor service and warehouse robots localizing in a prebuilt map. +- Global localization at start-up, and recovery after a robot is moved. +- Localization in symmetric environments where several hypotheses must be kept. + +## Connections to Other Algorithms + +- [Occupancy-Grid Mapping](OccupancyGridMap.md): builds the map the likelihood field is computed from. +- [Planar Odometry](../mobile/PlanarOdometry.md): the increment convention and the motion noise model. +- [EKF Landmark Localization](LandmarkLocalization.md): the Gaussian alternative for tracking with known landmarks. +- [2-D Scan Matching (ICP)](ScanMatcher.md): refines a tracked pose against a reference scan. +- The particle filter and SE(2) transform come from [numerical-toolbox-cpp](https://github.com/embedded-pro/numerical-toolbox-cpp). + +## References & Further Reading + +- Dellaert, F., Fox, D., Burgard, W. and Thrun, S. (1999). "Monte Carlo localization for mobile robots." *IEEE International Conference on Robotics and Automation*, 1322–1328. +- Thrun, S., Burgard, W. and Fox, D. (2005). *Probabilistic Robotics*. MIT Press. Chapters 6 and 8. +- Borgefors, G. (1986). "Distance transformations in digital images." *Computer Vision, Graphics, and Image Processing*, 34(3), 344–371. diff --git a/doc/localization/README.md b/doc/localization/README.md index 52b60b2..0bc9e25 100644 --- a/doc/localization/README.md +++ b/doc/localization/README.md @@ -4,9 +4,10 @@ Maps, state estimation and localization for mobile robots moving in the plane. ## Algorithms -| Algorithm | Description | -|------------------------------------------------------|--------------------------------------------------------------------------------------------------------------------------------------------------------------------------| -| [Occupancy-Grid Mapping](OccupancyGridMap.md) | Bounded grid of clamped log-odds cells updated from range scans along integer Bresenham rays, with max-range clearing — $O(\sum \rho / r)$ | -| [Odometry Fusion](OdometryFusion.md) | Wheel and gyroscope EKF on SE(2) with gyro-bias estimation, χ²-gated slip detection and optional heading and position updates — $O(1)$ | -| [EKF Landmark Localization](LandmarkLocalization.md) | Range–bearing EKF against a known landmark map, with body-frame odometry noise, Mahalanobis association, χ² gating and wrapped bearings — $O(L)$ | -| [2-D Scan Matching (ICP)](ScanMatcher.md) | Point-to-point and point-to-line ICP between laser scans with gated nearest-neighbour association, gap-aware segment normals and corridor degeneracy detection — $O(NM)$ | +| Algorithm | Description | +|-------------------------------------------------------|-----------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------| +| [Occupancy-Grid Mapping](OccupancyGridMap.md) | Bounded grid of clamped log-odds cells updated from range scans along integer Bresenham rays, with max-range clearing — $O(\sum \rho / r)$ | +| [Odometry Fusion](OdometryFusion.md) | Wheel and gyroscope EKF on SE(2) with gyro-bias estimation, χ²-gated slip detection and optional heading and position updates — $O(1)$ | +| [EKF Landmark Localization](LandmarkLocalization.md) | Range–bearing EKF against a known landmark map, with body-frame odometry noise, Mahalanobis association, χ² gating and wrapped bearings — $O(L)$ | +| [2-D Scan Matching (ICP)](ScanMatcher.md) | Point-to-point and point-to-line ICP between laser scans with gated nearest-neighbour association, gap-aware segment normals and corridor degeneracy detection — $O(NM)$ | +| [Monte Carlo Localization](MonteCarloLocalization.md) | Particle-filter localization in an occupancy grid with a noisy odometry motion model, a chamfer likelihood-field scan model with a random-measurement floor, ESS-triggered systematic resampling and a circular-mean estimate — $O(NB)$ | diff --git a/roadmap/README.md b/roadmap/README.md index 6ce97d9..17d63d2 100644 --- a/roadmap/README.md +++ b/roadmap/README.md @@ -23,7 +23,7 @@ All pseudocode respects the library constraints: **no heap**, bounded containers (no `Q15`/`Q31`). The generic signature keeps a future fixed-point specialisation cheap to add. The canonical worked example is -[localization/MonteCarloLocalization](localization/MonteCarloLocalization/implementation.md). +[trajectory/TimeOptimalPathParameterization](trajectory/TimeOptimalPathParameterization/implementation.md). Conventions shared by every spec: 6-vectors are ordered linear part first — twists `(v; ω)`, wrenches `(f; n)` — as fixed by upstream [math/SE3Transform](https://github.com/embedded-pro/numerical-toolbox-cpp/blob/main/doc/math/SE3Transform.md) (NT53), and planar @@ -90,6 +90,3 @@ prerequisites. ### `trajectory` `TimeOptimalPathParameterization` (M27) - -### `localization` -`MonteCarloLocalization` (E4) diff --git a/roadmap/localization/MonteCarloLocalization/explanation.md b/roadmap/localization/MonteCarloLocalization/explanation.md deleted file mode 100644 index bfdbeee..0000000 --- a/roadmap/localization/MonteCarloLocalization/explanation.md +++ /dev/null @@ -1,33 +0,0 @@ -# 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 deleted file mode 100644 index 6073104..0000000 --- a/roadmap/localization/MonteCarloLocalization/implementation.md +++ /dev/null @@ -1,117 +0,0 @@ -# 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 deleted file mode 100644 index 974f1e1..0000000 --- a/roadmap/localization/MonteCarloLocalization/tests.md +++ /dev/null @@ -1,60 +0,0 @@ -# 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/robotics/localization/CMakeLists.txt b/robotics/localization/CMakeLists.txt index 0396313..35a45ae 100644 --- a/robotics/localization/CMakeLists.txt +++ b/robotics/localization/CMakeLists.txt @@ -16,6 +16,7 @@ target_link_libraries(robotics.localization ${ROBOTICS_VISIBILITY} target_sources(robotics.localization PRIVATE LandmarkLocalization.hpp + MonteCarloLocalization.hpp OccupancyGridMap.hpp OdometryFusion.hpp ScanMatcher.hpp @@ -23,6 +24,7 @@ target_sources(robotics.localization PRIVATE robotics_add_coverage_sources(robotics.localization LandmarkLocalization.cpp + MonteCarloLocalization.cpp OccupancyGridMap.cpp OdometryFusion.cpp ScanMatcher.cpp diff --git a/robotics/localization/MonteCarloLocalization.cpp b/robotics/localization/MonteCarloLocalization.cpp new file mode 100644 index 0000000..d0295f1 --- /dev/null +++ b/robotics/localization/MonteCarloLocalization.cpp @@ -0,0 +1,14 @@ +#include "robotics/localization/MonteCarloLocalization.hpp" + +namespace localization +{ + template class LikelihoodField; + template class LikelihoodField; + template class MonteCarloLocalization; + template class MonteCarloLocalization; + template class MonteCarloLocalization; + template void MonteCarloLocalization::InitializeAround(const math::SE2Transform&, const math::Vector3&, float (&)()); + template void MonteCarloLocalization::Predict(const math::SE2Transform&, float (&)()); + template void MonteCarloLocalization::Initialize (&)(std::size_t)>(math::SE2Transform (&)(std::size_t)); + template bool MonteCarloLocalization::Update(std::span>, float (&)()); +} diff --git a/robotics/localization/MonteCarloLocalization.hpp b/robotics/localization/MonteCarloLocalization.hpp new file mode 100644 index 0000000..4776c12 --- /dev/null +++ b/robotics/localization/MonteCarloLocalization.hpp @@ -0,0 +1,273 @@ +#pragma once + +#include "infra/util/ReallyAssert.hpp" +#include "numerical/filters/active/ParticleFilter.hpp" +#include "numerical/math/CompilerOptimizations.hpp" +#include "numerical/math/Math.hpp" +#include "numerical/math/SE2Transform.hpp" +#include "robotics/localization/OccupancyGridMap.hpp" +#include "robotics/mobile/PlanarOdometry.hpp" +#include +#include +#include +#include +#include +#include +#include + +namespace localization +{ + template + struct MclParameters + { + static_assert(std::is_floating_point_v, "MclParameters supports floating-point types"); + + mobile::OdometryNoise motion{}; + T hitSigma{}; + T hitWeight{}; + T randomWeight{}; + T maxRange{}; + T resampleFraction{}; + std::size_t beamStride{ 1 }; + }; + + template + class LikelihoodField + { + static_assert(std::is_floating_point_v, "LikelihoodField supports floating-point types"); + + public: + LikelihoodField(const OccupancyGridMap& map, T occupiedThreshold, T maxDistance); + + T Distance(const math::Vector2& world) const; + + private: + using Cells = std::array; + + static std::size_t Index(std::size_t x, std::size_t y); + static void Relax(Cells& cells, std::size_t x, std::size_t y, int dx, int dy, T weight); + static void ForwardPass(Cells& cells); + static void BackwardPass(Cells& cells); + + Cells distance{}; + OccupancyGridParameters grid; + T maxDistance; + }; + + template + class MonteCarloLocalization + { + static_assert(std::is_floating_point_v, "MonteCarloLocalization supports floating-point types"); + + public: + using Filter = filters::ParticleFilter; + + MonteCarloLocalization(const LikelihoodField& field, const math::SE2Transform& sensorMount, const MclParameters& parameters); + + template + void Initialize(Draw&& draw); + template + void InitializeAround(const math::SE2Transform& pose, const math::Vector3& sigma, Normal&& normal); + template + OPTIMIZE_FOR_SPEED void Predict(const math::SE2Transform& increment, Normal&& normal); + template + OPTIMIZE_FOR_SPEED bool Update(std::span> scan, Uniform&& uniform); + + math::SE2Transform Estimate() const; + const Filter& Particles() const; + + private: + T LogLikelihood(const math::Vector3& pose, std::span> scan) const; + + Filter filter; + const LikelihoodField& field; + math::SE2Transform sensorMount; + MclParameters parameters; + }; + + template + LikelihoodField::LikelihoodField(const OccupancyGridMap& map, T occupiedThreshold, T maxDistance) + : grid{ map.Parameters() } + , maxDistance{ maxDistance } + { + for (std::size_t y = 0; y < Height; ++y) + for (std::size_t x = 0; x < Width; ++x) + distance[Index(x, y)] = map.IsOccupied(GridCell{ static_cast(x), static_cast(y) }, occupiedThreshold) ? T{} : std::numeric_limits::infinity(); + + ForwardPass(distance); + BackwardPass(distance); + + for (auto& cell : distance) + cell = std::min(cell * grid.resolution, maxDistance); + } + + template + T LikelihoodField::Distance(const math::Vector2& world) const + { + const T column{ math::Floor((world[0] - grid.origin[0]) / grid.resolution) }; + const T row{ math::Floor((world[1] - grid.origin[1]) / grid.resolution) }; + const bool inside{ column >= T{} && row >= T{} && column < static_cast(Width) && row < static_cast(Height) }; + return inside ? distance[Index(static_cast(column), static_cast(row))] : maxDistance; + } + + template + std::size_t LikelihoodField::Index(std::size_t x, std::size_t y) + { + return y * Width + x; + } + + template + void LikelihoodField::Relax(Cells& cells, std::size_t x, std::size_t y, int dx, int dy, T weight) + { + const auto nx{ static_cast(x) + dx }; + const auto ny{ static_cast(y) + dy }; + const bool inside{ nx >= 0 && ny >= 0 && nx < static_cast(Width) && ny < static_cast(Height) }; + const T candidate{ inside ? cells[Index(static_cast(nx), static_cast(ny))] + weight : std::numeric_limits::infinity() }; + cells[Index(x, y)] = std::min(cells[Index(x, y)], candidate); + } + + template + void LikelihoodField::ForwardPass(Cells& cells) + { + const T diagonal{ std::numbers::sqrt2_v }; + for (std::size_t y = 0; y < Height; ++y) + for (std::size_t x = 0; x < Width; ++x) + { + Relax(cells, x, y, -1, 0, T{ 1 }); + Relax(cells, x, y, 0, -1, T{ 1 }); + Relax(cells, x, y, -1, -1, diagonal); + Relax(cells, x, y, 1, -1, diagonal); + } + } + + template + void LikelihoodField::BackwardPass(Cells& cells) + { + const T diagonal{ std::numbers::sqrt2_v }; + for (std::size_t row = Height; row > 0; --row) + for (std::size_t column = Width; column > 0; --column) + { + const std::size_t x{ column - 1 }; + const std::size_t y{ row - 1 }; + Relax(cells, x, y, 1, 0, T{ 1 }); + Relax(cells, x, y, 0, 1, T{ 1 }); + Relax(cells, x, y, 1, 1, diagonal); + Relax(cells, x, y, -1, 1, diagonal); + } + } + + template + MonteCarloLocalization::MonteCarloLocalization(const LikelihoodField& field, const math::SE2Transform& sensorMount, + const MclParameters& parameters) + : field{ field } + , sensorMount{ sensorMount } + , parameters{ parameters } + { + really_assert(parameters.beamStride > 0 && parameters.maxRange > T{} && parameters.hitSigma > T{}); + } + + template + template + void MonteCarloLocalization::Initialize(Draw&& draw) + { + filter.Initialize([&draw](std::size_t i) + { + return draw(i).Pose(); + }); + } + + template + template + void MonteCarloLocalization::InitializeAround(const math::SE2Transform& pose, const math::Vector3& sigma, Normal&& normal) + { + filter.Initialize([&pose, &sigma, &normal](std::size_t) + { + const T dx{ sigma[0] * normal() }; + const T dy{ sigma[1] * normal() }; + const T dtheta{ sigma[2] * normal() }; + return (pose * math::SE2Transform::FromPose(dx, dy, dtheta)).Pose(); + }); + } + + template + template + OPTIMIZE_FOR_SPEED void MonteCarloLocalization::Predict(const math::SE2Transform& increment, Normal&& normal) + { + const auto delta{ increment.Pose() }; + const auto deviation{ mobile::IncrementDeviation(increment, parameters.motion) }; + filter.Predict([&delta, &deviation, &normal](math::Vector3& particle, std::size_t) + { + const T dx{ delta[0] + deviation[0] * normal() }; + const T dy{ delta[1] + deviation[1] * normal() }; + const T dtheta{ delta[2] + deviation[2] * normal() }; + particle = (math::SE2Transform::FromPose(particle[0], particle[1], particle[2]) * math::SE2Transform::FromPose(dx, dy, dtheta)).Pose(); + }); + } + + template + template + OPTIMIZE_FOR_SPEED bool MonteCarloLocalization::Update(std::span> scan, Uniform&& uniform) + { + filter.Update([this, scan](const math::Vector3& particle) + { + return LogLikelihood(particle, scan); + }); + return filter.ResampleIfNeeded(parameters.resampleFraction, uniform()); + } + + template + math::SE2Transform MonteCarloLocalization::Estimate() const + { + const auto particles{ filter.Particles() }; + const auto weights{ filter.Weights() }; + T x{}; + T y{}; + T sine{}; + T cosine{}; + for (std::size_t i = 0; i < NumParticles; ++i) + { + x += weights[i] * particles[i][0]; + y += weights[i] * particles[i][1]; + sine += weights[i] * math::Sin(particles[i][2]); + cosine += weights[i] * math::Cos(particles[i][2]); + } + return math::SE2Transform::FromPose(x, y, math::Atan2(sine, cosine)); + } + + template + auto MonteCarloLocalization::Particles() const -> const Filter& + { + return filter; + } + + template + T MonteCarloLocalization::LogLikelihood(const math::Vector3& pose, std::span> scan) const + { + const auto sensor{ math::SE2Transform::FromPose(pose[0], pose[1], pose[2]) * sensorMount }; + const T floor{ parameters.randomWeight / parameters.maxRange }; + const T spread{ T{ 2 } * parameters.hitSigma * parameters.hitSigma }; + T logLikelihood{}; + for (std::size_t k = 0; k < scan.size(); k += parameters.beamStride) + { + const auto& beam{ scan[k] }; + if (beam.range >= parameters.maxRange) + continue; + + const T d{ field.Distance(sensor.Apply(math::Vector2{ beam.range * math::Cos(beam.bearing), beam.range * math::Sin(beam.bearing) })) }; + logLikelihood += math::Log(parameters.hitWeight * math::Exp(-d * d / spread) + floor); + } + return logLikelihood; + } + +#ifdef ROBOTICS_TOOLBOX_COVERAGE_BUILD + extern template class LikelihoodField; + extern template class LikelihoodField; + extern template class MonteCarloLocalization; + extern template class MonteCarloLocalization; + extern template class MonteCarloLocalization; + extern template void MonteCarloLocalization::InitializeAround(const math::SE2Transform&, const math::Vector3&, float (&)()); + extern template void MonteCarloLocalization::Predict(const math::SE2Transform&, float (&)()); + extern template void MonteCarloLocalization::Initialize (&)(std::size_t)>(math::SE2Transform (&)(std::size_t)); + extern template bool MonteCarloLocalization::Update(std::span>, float (&)()); +#endif +} diff --git a/robotics/localization/test/CMakeLists.txt b/robotics/localization/test/CMakeLists.txt index efec557..2258111 100644 --- a/robotics/localization/test/CMakeLists.txt +++ b/robotics/localization/test/CMakeLists.txt @@ -9,6 +9,7 @@ target_link_libraries(robotics.localization_test PUBLIC target_sources(robotics.localization_test PRIVATE TestLandmarkLocalization.cpp + TestMonteCarloLocalization.cpp TestOccupancyGridMap.cpp TestOdometryFusion.cpp TestScanMatcher.cpp diff --git a/robotics/localization/test/TestMonteCarloLocalization.cpp b/robotics/localization/test/TestMonteCarloLocalization.cpp new file mode 100644 index 0000000..6e8bb00 --- /dev/null +++ b/robotics/localization/test/TestMonteCarloLocalization.cpp @@ -0,0 +1,190 @@ +#include "numerical/math/SE2Transform.hpp" +#include "robotics/localization/MonteCarloLocalization.hpp" +#include "robotics/localization/OccupancyGridMap.hpp" +#include +#include +#include +#include +#include +#include + +namespace +{ + using Transform = math::SE2Transform; + using Point = math::Vector2; + using Beam = localization::RangeBeam; + using Cell = localization::GridCell; + using Room = localization::OccupancyGridMap; + using Field = localization::LikelihoodField; + using Tracker = localization::MonteCarloLocalization; + using Lattice = localization::MonteCarloLocalization; + + constexpr float pi{ std::numbers::pi_v }; + const Transform truth{ Transform::FromPose(1.0f, 1.5f, 0.2f) }; + const localization::OccupancyGridParameters gridParameters{ 0.1f, Point{ 0.0f, 0.0f }, 0.85f, -0.4f, -2.0f, 3.5f, 4.0f }; + + float Zero() + { + return 0.0f; + } + + float One() + { + return 1.0f; + } + + float Half() + { + return 0.5f; + } + + Transform LatticePose(std::size_t i) + { + const float dx{ 0.2f * (static_cast(i % 3) - 1.0f) }; + const float dy{ 0.2f * (static_cast((i / 3) % 3) - 1.0f) }; + const float dtheta{ 0.1f * (static_cast(i / 9) - 1.0f) }; + return truth * Transform::FromPose(dx, dy, dtheta); + } + + class TestMonteCarloLocalization + : public ::testing::Test + { + protected: + TestMonteCarloLocalization() + { + for (int k = 0; k < 40; ++k) + { + Occupy(k, 0); + Occupy(k, 39); + Occupy(0, k); + Occupy(39, k); + } + for (int y = 0; y <= 14; ++y) + Occupy(20, y); + for (int x = 25; x <= 39; ++x) + Occupy(x, 28); + } + + void Occupy(int x, int y) + { + room.SetLogOdds(Cell{ x, y }, 3.5f); + } + + std::array Scan() const + { + std::array scan{}; + for (std::size_t k = 0; k < scan.size(); ++k) + { + const float bearing{ static_cast(k) * pi / 6.0f }; + const float heading{ truth.Heading() + bearing }; + float range{ 0.0f }; + while (range < 4.0f) + { + const Point end{ truth.translation[0] + range * std::cos(heading), truth.translation[1] + range * std::sin(heading) }; + const Cell cell{ room.WorldToCell(end) }; + if (room.Contains(cell) && room.IsOccupied(cell, 0.0f)) + break; + range += 0.005f; + } + scan[k] = Beam{ std::min(range, 4.0f), bearing }; + } + return scan; + } + + static void ExpectPose(const Transform& actual, float x, float y, float heading, float tolerance) + { + const auto pose{ actual.Pose() }; + EXPECT_NEAR(pose[0], x, tolerance); + EXPECT_NEAR(pose[1], y, tolerance); + EXPECT_NEAR(pose[2], heading, tolerance); + } + + Room room{ gridParameters }; + localization::MclParameters parameters{ { 0.05f, 0.01f, 0.05f, 0.01f }, 0.1f, 0.9f, 0.1f, 4.0f, 0.5f, 1 }; + }; +} + +TEST_F(TestMonteCarloLocalization, likelihood_field_follows_the_chamfer_metric) +{ + Room single{ gridParameters }; + single.SetLogOdds(Cell{ 20, 20 }, 3.5f); + const Field field{ single, 0.0f, 2.0f }; + + EXPECT_NEAR(field.Distance(single.CellCenter(Cell{ 20, 20 })), 0.0f, 1e-6f); + EXPECT_NEAR(field.Distance(single.CellCenter(Cell{ 25, 20 })), 0.5f, 1e-6f); + EXPECT_NEAR(field.Distance(single.CellCenter(Cell{ 24, 24 })), 0.565685f, 1e-6f); + EXPECT_NEAR(field.Distance(single.CellCenter(Cell{ 23, 24 })), 0.524264f, 1e-6f); + EXPECT_EQ(field.Distance(Point{ -1.0f, 2.0f }), 2.0f); +} + +TEST_F(TestMonteCarloLocalization, predict_without_noise_applies_the_increment) +{ + const Field field{ room, 0.0f, 2.0f }; + Tracker tracker{ field, Transform::Identity(), parameters }; + tracker.InitializeAround(Transform::FromPose(1.0f, 2.0f, 0.3f), math::Vector3{ 0.1f, 0.1f, 0.1f }, Zero); + + tracker.Predict(Transform::FromPose(0.5f, 0.0f, 0.1f), Zero); + + for (const auto& particle : tracker.Particles().Particles()) + ExpectPose(Transform::FromPose(particle[0], particle[1], particle[2]), 1.477668f, 2.147760f, 0.4f, 1e-5f); +} + +TEST_F(TestMonteCarloLocalization, predict_noise_scales_with_the_motion) +{ + const Field field{ room, 0.0f, 2.0f }; + Tracker tracker{ field, Transform::Identity(), parameters }; + tracker.InitializeAround(Transform::FromPose(1.0f, 2.0f, 0.3f), math::Vector3{ 0.1f, 0.1f, 0.1f }, Zero); + + tracker.Predict(Transform::FromPose(0.5f, 0.0f, 0.1f), One); + + for (const auto& particle : tracker.Particles().Particles()) + ExpectPose(Transform::FromPose(particle[0], particle[1], particle[2]), 1.551732f, 2.288168f, 0.454772f, 1e-5f); +} + +TEST_F(TestMonteCarloLocalization, scan_update_favours_the_true_pose) +{ + const Field field{ room, 0.0f, 2.0f }; + parameters.resampleFraction = 0.0f; + Lattice lattice{ field, Transform::Identity(), parameters }; + lattice.Initialize(LatticePose); + const auto scan{ Scan() }; + + EXPECT_FALSE(lattice.Update(scan, Half)); + + const auto& best{ lattice.Particles().MaximumWeight() }; + ExpectPose(Transform::FromPose(best[0], best[1], best[2]), 1.0f, 1.5f, 0.2f, 1e-6f); + float bestWeight{ 0.0f }; + for (const float weight : lattice.Particles().Weights()) + bestWeight = std::max(bestWeight, weight); + EXPECT_NEAR(bestWeight, 0.995028f, 1e-4f); + ExpectPose(lattice.Estimate(), 1.0f, 1.5f, 0.200381f, 1e-4f); +} + +TEST_F(TestMonteCarloLocalization, resampling_concentrates_on_the_true_pose) +{ + const Field field{ room, 0.0f, 2.0f }; + Lattice lattice{ field, Transform::Identity(), parameters }; + lattice.Initialize(LatticePose); + const auto scan{ Scan() }; + + EXPECT_TRUE(lattice.Update(scan, Half)); + + for (const auto& particle : lattice.Particles().Particles()) + ExpectPose(Transform::FromPose(particle[0], particle[1], particle[2]), 1.0f, 1.5f, 0.2f, 1e-6f); + ExpectPose(lattice.Estimate(), 1.0f, 1.5f, 0.2f, 1e-5f); +} + +TEST_F(TestMonteCarloLocalization, max_range_beams_are_ignored) +{ + const Field field{ room, 0.0f, 2.0f }; + Lattice lattice{ field, Transform::Identity(), parameters }; + lattice.Initialize(LatticePose); + std::array empty{}; + for (std::size_t k = 0; k < empty.size(); ++k) + empty[k] = Beam{ 4.0f, static_cast(k) * pi / 6.0f }; + + lattice.Update(empty, Half); + + for (const float weight : lattice.Particles().Weights()) + EXPECT_NEAR(weight, 1.0f / 27.0f, 1e-6f); +} From b61ac1e0d0372f1b7d41c0b52d18b5b2299a767b Mon Sep 17 00:00:00 2001 From: Claude Date: Fri, 6 Nov 2026 09:46:29 +0000 Subject: [PATCH 3/3] build: pin numerical-toolbox to the NT70 merge commit Co-Authored-By: Claude Opus 5.5 Claude-Session: https://claude.ai/code/session_014WUWh44uw7k4JjMd6YkAwN --- CMakeLists.txt | 2 +- ROADMAP.md | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index dd7b031..b0ba6de 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -47,7 +47,7 @@ if (NOT TARGET numerical.math) FetchContent_Declare( numerical_toolbox GIT_REPOSITORY https://github.com/embedded-pro/numerical-toolbox-cpp.git - GIT_TAG 6080be4e519bb199be7447055491d27c7d96e25d # main + GIT_TAG bf5cc90370575866dbddf7271fb44d46ae0954d0 # main ) set(NUMERICAL_TOOLBOX_ENABLE_OPTIMIZATIONS ${ROBOTICS_TOOLBOX_ENABLE_OPTIMIZATIONS} CACHE BOOL "" FORCE) diff --git a/ROADMAP.md b/ROADMAP.md index 3c05bbd..a45ed2a 100644 --- a/ROADMAP.md +++ b/ROADMAP.md @@ -41,7 +41,7 @@ this roadmap needs that upstream does not ship yet are specified and built upstr Generic algorithms live in numerical-toolbox-cpp and are referenced here as **NT** plus its master-list item number (NT53 is upstream item 53). A robotics item that depends on one needs a numerical-toolbox pin that ships it. NT53–NT67, NT70 and NT74 have shipped upstream, and their links point to the -upstream docs; the current pin (`6080be4`) includes them. +upstream docs; the current pin (`bf5cc90`) includes them. | # | Component | Upstream module | Spec / doc | Needed by | |------|-------------------------------------------------------|----------------------|-----------------------------------|----------------------------------------------------------------------------|