diff --git a/CMakeLists.txt b/CMakeLists.txt index 4ef8b73..5621b55 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 b93d6e074d6aeee4f9395feb54429d55954ebd45 # main + GIT_TAG 182e068ef3239a585eb4beb47175945a724788a6 # main ) set(NUMERICAL_TOOLBOX_ENABLE_OPTIMIZATIONS ${ROBOTICS_TOOLBOX_ENABLE_OPTIMIZATIONS} CACHE BOOL "" FORCE) diff --git a/README.md b/README.md index 5a63f33..afed608 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 | +| [Localization & Mapping](doc/localization/README.md) | Occupancy-Grid Mapping, Odometry Fusion, EKF Landmark Localization, 2-D Scan Matching (ICP) | | [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 0b543bf..9f09bea 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–NT66 and NT74 have shipped upstream, and their links point to the -upstream docs; the current pin (`b93d6e0`) includes them. +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. | # | Component | Upstream module | Spec / doc | Needed by | |------|-------------------------------------------------------|----------------------|-----------------------------------|----------------------------------------------------------------------------| @@ -59,7 +59,7 @@ upstream docs; the current pin (`b93d6e0`) includes them. | 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] | F1, W1–W10, M24, E1, E2, E3, E5 | -| NT67 | Kabsch / Umeyama point-set alignment | `solvers` | [PointSetAlignment][nt67] (spec) | E5, C1 | +| 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 | @@ -83,7 +83,7 @@ upstream docs; the current pin (`b93d6e0`) 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/doc/math/SE2Transform.md -[nt67]: https://github.com/embedded-pro/numerical-toolbox-cpp/blob/main/roadmap/solvers/PointSetAlignment/implementation.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 [nt74]: https://github.com/embedded-pro/numerical-toolbox-cpp/blob/main/doc/filters/active/ResidualKalmanUpdate.md @@ -288,19 +288,14 @@ 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 | ★★★★☆ | -| E5 | 2-D scan matching (point-to-point / point-to-line ICP) | `localization` | NT67, NT64, NT66 | ★★★★☆ | +| # | 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. -**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. - ## Calibration | # | Component | Target module | Depends on | Difficulty | diff --git a/doc/localization/README.md b/doc/localization/README.md index 2d73569..52b60b2 100644 --- a/doc/localization/README.md +++ b/doc/localization/README.md @@ -4,8 +4,9 @@ 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)$ | +| 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)$ | diff --git a/doc/localization/ScanMatcher.md b/doc/localization/ScanMatcher.md new file mode 100644 index 0000000..89639c1 --- /dev/null +++ b/doc/localization/ScanMatcher.md @@ -0,0 +1,98 @@ +# 2-D Scan Matching (Point-to-Point and Point-to-Line ICP) + +## Overview & Motivation + +A laser scanner sees the same walls from slightly different poses as the robot moves. Aligning two consecutive scans therefore measures the motion between them, often far more accurately than wheel odometry, and aligning a scan with a map localises the robot. The standard tool is **iterative closest point** (ICP). + +ICP needs no features. Each iteration pairs every point of the current scan with the nearest point of the reference scan, solves for the transform that best aligns the pairs, applies it, and repeats until the correction becomes negligible. Two error metrics are common: +- **Point-to-point** minimises the distances between paired points, with a closed-form solution. +- **Point-to-line** minimises only the distance across the local reference wall. It converges in fewer iterations and has no bias when the two scans sample the walls at different spots. + +## Mathematical Theory + +### Association + +With the current estimate $T$, each current point is moved to $y_i = T\,c_i$ and paired with its nearest reference point $r_j$. Pairs farther apart than a gate $d_{\max}$ are outliers and get zero weight. + +For point-to-line, each moved point also takes the normal $n_i$ of the nearest reference segment between consecutive scan points. Segments longer than the gate are gaps between walls, not walls, and are skipped. Without this, the junction between two walls in scan order would produce a bogus diagonal segment. + +### Point-to-Point Step + +The increment is the weighted rigid alignment of the moved points onto their pairs: + +$$\Delta T = \arg\min_{R,\,t} \sum_i w_i \left\| r_{j(i)} - (R\,y_i + t) \right\|^2$$ + +This is solved in closed form by the 2-D Kabsch algorithm. + +### Point-to-Line Step + +The increment minimises the distances along the normals: + +$$\Delta T = \arg\min_{\Delta} \sum_i w_i \left( n_i^T (\Delta\,y_i - q_i) \right)^2$$ + +Here $q_i$ is a point on the paired segment. A few Levenberg–Marquardt steps solve it with left perturbations $\Delta \leftarrow \exp(\delta)\,\Delta$, using the Jacobian row + +$$J_i = w_i \left( n_{i,x},\;\; n_{i,y},\;\; n_{i,y}\,y'_{i,x} - n_{i,x}\,y'_{i,y} \right)$$ + +### Degeneracy + +In a corridor whose walls are all parallel, motion along the walls changes no normal distance. $J^T J$ then has a zero eigenvalue. The matcher computes its eigenvalues and refuses the match when the smallest is below $10^{-6}$ of the largest. The caller then falls back to odometry along that direction. + +### Convergence + +The estimate is updated as $T \leftarrow \Delta T\,T$ until the increment's translation and rotation both fall below their tolerances. + +## Complexity Analysis + +| Operation | Time | Space | Notes | +|---------------------|----------|--------|----------------------------------------------------------| +| Association | $O(N M)$ | $O(N)$ | Brute-force nearest point and nearest segment | +| Point-to-point step | $O(N)$ | $O(1)$ | One weighted 2-D alignment | +| Point-to-line step | $O(N)$ | $O(N)$ | A few Levenberg–Marquardt steps on a $3 \times 3$ system | +| Work arrays | — | $O(N)$ | Moved points, pairs, normals and weights on the stack | + +For $N$ current and $M$ reference points. For full 360-beam scans, projective (scan-order) association or a bounded k-d tree replaces the brute-force search without changing the interface. + +## Step-by-Step Walkthrough + +The reference scan has 68 points on three walls of a room: $x = 2$, $y = 1.5$ and $y = -1$, spaced $0.1$ m apart. The current scan is taken after the robot moved by $(0.1, -0.05, 0.05)$: + +1. **Exact correspondences**: when the current scan samples the same wall spots, both metrics recover the motion to $10^{-4}$. Point-to-point needs 3 iterations, point-to-line 2. +2. **Sampling bias**: when the current scan samples the walls halfway between the reference points, point-to-point settles about $3$ cm off, pulled towards the reference samples. Point-to-line still recovers the motion exactly. +3. **Offset guess**: starting from $(-0.05, 0.05, -0.05)$ instead of the identity, point-to-line still converges to the true motion. +4. **Outlier**: an extra point $1.2$ m from every wall, such as a person in the room, is rejected by the $0.5$ m gate. 65 of the 66 points are paired, and the result is unchanged. +5. **Tight gate**: with a $1$ cm gate, no pair survives, and the match fails. +6. **Corridor**: two parallel walls leave the motion along them unobservable, and the match is refused. + +## Pitfalls & Edge Cases + +- **Initial guess**: ICP converges only locally. Seed it with odometry, especially when turning. +- **Corners and scan ends**: duplicate corner points and the jumps between walls in scan order can produce wrong normals. Gating long segments avoids them. +- **Degenerate geometry**: corridors and open spaces with one wall leave directions unobservable. Check the refusal and fall back to odometry. +- **Dynamic objects**: people and doors produce outliers. Keep the gate tight once the estimate is close. +- **Point density**: brute-force association scales with the product of the scan sizes. Downsample or use projective association for large scans. + +## Variants & Generalizations + +- **Point-to-plane ICP**: the 3-D counterpart for depth cameras and 3-D lidar. +- **Generalised ICP**: plane-to-plane covariances on both scans. +- **Normal distributions transform (NDT)**: matching against a grid of local Gaussians. +- **Correlative scan matching**: exhaustive search over a pose grid for robustness to large initial errors. + +## Applications + +- Laser odometry for indoor robots and warehouse vehicles. +- Map-based localization by matching scans against a known map. +- Loop closure and pose-graph SLAM front ends. + +## Connections to Other Algorithms + +- [Odometry Fusion](OdometryFusion.md): seeds the initial guess and can absorb the match as a position update. +- [EKF Landmark Localization](LandmarkLocalization.md): the feature-based alternative when the environment has known landmarks. +- The point-set alignment, Levenberg–Marquardt solver, eigenvalue solver and SE(2) transform come from [numerical-toolbox-cpp](https://github.com/embedded-pro/numerical-toolbox-cpp). + +## References & Further Reading + +- Besl, P.J. and McKay, N.D. (1992). "A method for registration of 3-D shapes." *IEEE Transactions on Pattern Analysis and Machine Intelligence*, 14(2), 239–256. +- Censi, A. (2008). "An ICP variant using a point-to-line metric." *IEEE International Conference on Robotics and Automation*, 19–25. +- Lu, F. and Milios, E. (1997). "Robot pose estimation in unknown environments by matching 2D range scans." *Journal of Intelligent and Robotic Systems*, 18(3), 249–275. diff --git a/roadmap/README.md b/roadmap/README.md index 937cd16..6ce97d9 100644 --- a/roadmap/README.md +++ b/roadmap/README.md @@ -92,4 +92,4 @@ prerequisites. `TimeOptimalPathParameterization` (M27) ### `localization` -`MonteCarloLocalization` (E4) · `ScanMatcher` (E5) +`MonteCarloLocalization` (E4) diff --git a/roadmap/localization/ScanMatcher/explanation.md b/roadmap/localization/ScanMatcher/explanation.md deleted file mode 100644 index 384a2e9..0000000 --- a/roadmap/localization/ScanMatcher/explanation.md +++ /dev/null @@ -1,34 +0,0 @@ -# 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 deleted file mode 100644 index c63f5c6..0000000 --- a/roadmap/localization/ScanMatcher/implementation.md +++ /dev/null @@ -1,117 +0,0 @@ -# 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 deleted file mode 100644 index 5321e5e..0000000 --- a/roadmap/localization/ScanMatcher/tests.md +++ /dev/null @@ -1,56 +0,0 @@ -# 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/robotics/localization/CMakeLists.txt b/robotics/localization/CMakeLists.txt index 3873cdf..0396313 100644 --- a/robotics/localization/CMakeLists.txt +++ b/robotics/localization/CMakeLists.txt @@ -9,6 +9,8 @@ target_link_libraries(robotics.localization ${ROBOTICS_VISIBILITY} infra.util numerical.filters.active numerical.math + numerical.optimization + numerical.solver robotics.mobile ) @@ -16,12 +18,14 @@ target_sources(robotics.localization PRIVATE LandmarkLocalization.hpp OccupancyGridMap.hpp OdometryFusion.hpp + ScanMatcher.hpp ) robotics_add_coverage_sources(robotics.localization LandmarkLocalization.cpp OccupancyGridMap.cpp OdometryFusion.cpp + ScanMatcher.cpp ) add_subdirectory(test) diff --git a/robotics/localization/ScanMatcher.cpp b/robotics/localization/ScanMatcher.cpp new file mode 100644 index 0000000..1a66dc5 --- /dev/null +++ b/robotics/localization/ScanMatcher.cpp @@ -0,0 +1,6 @@ +#include "robotics/localization/ScanMatcher.hpp" + +namespace localization +{ + template class ScanMatcher; +} diff --git a/robotics/localization/ScanMatcher.hpp b/robotics/localization/ScanMatcher.hpp new file mode 100644 index 0000000..63047c1 --- /dev/null +++ b/robotics/localization/ScanMatcher.hpp @@ -0,0 +1,305 @@ +#pragma once + +#include "numerical/math/CompilerOptimizations.hpp" +#include "numerical/math/Math.hpp" +#include "numerical/math/Matrix.hpp" +#include "numerical/math/SE2Transform.hpp" +#include "numerical/optimization/DampingPolicy.hpp" +#include "numerical/optimization/LeastSquaresProblem.hpp" +#include "numerical/optimization/LevenbergMarquardt.hpp" +#include "numerical/solvers/JacobiEigenSolver.hpp" +#include "numerical/solvers/PointSetAlignment.hpp" +#include +#include +#include +#include +#include +#include +#include +#include + +namespace localization +{ + enum class IcpMetric : std::uint8_t + { + PointToPoint, + PointToLine + }; + + template + struct ScanMatcherParameters + { + static_assert(std::is_floating_point_v, "ScanMatcherParameters supports floating-point types"); + + IcpMetric metric{ IcpMetric::PointToLine }; + T maxCorrespondenceDistance{}; + std::size_t maxIterations{}; + T translationTolerance{}; + T rotationTolerance{}; + std::size_t minCorrespondences{ 3 }; + std::size_t refinementIterations{ 3 }; + }; + + template + struct ScanMatchResult + { + static_assert(std::is_floating_point_v, "ScanMatchResult supports floating-point types"); + + math::SE2Transform transform{}; + std::size_t iterations{}; + std::size_t correspondences{}; + T rms{}; + bool converged{ false }; + }; + + template + class ScanMatcher + { + static_assert(std::is_floating_point_v, "ScanMatcher supports floating-point types"); + static_assert(MaxPoints >= 3, "ScanMatcher needs room for at least three points"); + + public: + using Point = math::Vector2; + + explicit ScanMatcher(const ScanMatcherParameters& parameters); + + OPTIMIZE_FOR_SPEED std::optional> Match(std::span reference, std::span current, const math::SE2Transform& initialGuess) const; + + private: + struct Correspondences + { + std::array moved{}; + std::array matched{}; + std::array normals{}; + std::array weights{}; + std::size_t size{ 0 }; + std::size_t accepted{ 0 }; + }; + + class PointToLineProblem + : public optimization::LeastSquaresProblem, MaxPoints, 3> + { + public: + explicit PointToLineProblem(const Correspondences& correspondences); + + math::Vector Residual(const math::SE2Transform& delta) const override; + math::Matrix Jacobian(const math::SE2Transform& delta) const override; + math::SE2Transform Retract(const math::SE2Transform& delta, const math::Vector& step) const override; + + private: + const Correspondences& correspondences; + }; + + static constexpr T degeneracyRatio{ T{ 1e-6f } }; + static constexpr T dampingSquared{ T{ 1e-9f } }; + + void Associate(std::span reference, std::span current, const math::SE2Transform& transform, Correspondences& correspondences) const; + std::size_t NearestSegment(std::span reference, const Point& point) const; + static T SegmentDistance(const Point& from, const Point& to, const Point& point); + static Point SegmentNormal(const Point& from, const Point& to); + std::optional> Step(const Correspondences& correspondences) const; + std::optional> PointToPointStep(const Correspondences& correspondences) const; + std::optional> PointToLineStep(const Correspondences& correspondences) const; + T Rms(const Correspondences& correspondences) const; + ScanMatchResult Result(std::span reference, std::span current, const math::SE2Transform& transform, std::size_t iterations, bool converged) const; + + ScanMatcherParameters parameters; + }; + + template + ScanMatcher::ScanMatcher(const ScanMatcherParameters& parameters) + : parameters{ parameters } + {} + + template + OPTIMIZE_FOR_SPEED std::optional> ScanMatcher::Match(std::span reference, std::span current, + const math::SE2Transform& initialGuess) const + { + if (reference.size() > MaxPoints || current.size() > MaxPoints || reference.size() < 2) + return std::nullopt; + + auto transform{ initialGuess }; + Correspondences correspondences{}; + for (std::size_t iteration = 1; iteration <= parameters.maxIterations; ++iteration) + { + Associate(reference, current, transform, correspondences); + if (correspondences.accepted < parameters.minCorrespondences) + return std::nullopt; + + const auto delta{ Step(correspondences) }; + if (!delta) + return std::nullopt; + + transform = (*delta * transform).Normalized(); + const bool small{ math::Hypot(delta->translation[0], delta->translation[1]) < parameters.translationTolerance && math::Abs(delta->Heading()) < parameters.rotationTolerance }; + if (small) + return Result(reference, current, transform, iteration, true); + } + + return Result(reference, current, transform, parameters.maxIterations, false); + } + + template + void ScanMatcher::Associate(std::span reference, std::span current, const math::SE2Transform& transform, Correspondences& correspondences) const + { + correspondences.size = current.size(); + correspondences.accepted = 0; + for (std::size_t i = 0; i < current.size(); ++i) + { + const auto moved{ transform.Apply(current[i]) }; + std::size_t nearest{ 0 }; + T nearestDistance{ std::numeric_limits::infinity() }; + for (std::size_t j = 0; j < reference.size(); ++j) + { + const T distance{ math::Hypot(reference[j][0] - moved[0], reference[j][1] - moved[1]) }; + if (distance < nearestDistance) + { + nearest = j; + nearestDistance = distance; + } + } + + const bool accepted{ nearestDistance <= parameters.maxCorrespondenceDistance }; + const std::size_t segment{ NearestSegment(reference, moved) }; + correspondences.moved[i] = moved; + correspondences.matched[i] = parameters.metric == IcpMetric::PointToPoint ? reference[nearest] : reference[segment]; + correspondences.normals[i] = SegmentNormal(reference[segment], reference[segment + 1]); + correspondences.weights[i] = accepted ? T{ 1 } : T{}; + correspondences.accepted += accepted ? 1 : 0; + } + } + + template + std::size_t ScanMatcher::NearestSegment(std::span reference, const Point& point) const + { + std::size_t best{ 0 }; + T bestDistance{ std::numeric_limits::infinity() }; + for (std::size_t k = 0; k + 1 < reference.size(); ++k) + { + const bool gap{ math::Hypot(reference[k + 1][0] - reference[k][0], reference[k + 1][1] - reference[k][1]) > parameters.maxCorrespondenceDistance }; + const T distance{ gap ? std::numeric_limits::infinity() : SegmentDistance(reference[k], reference[k + 1], point) }; + if (distance < bestDistance) + { + best = k; + bestDistance = distance; + } + } + return best; + } + + template + T ScanMatcher::SegmentDistance(const Point& from, const Point& to, const Point& point) + { + const T dx{ to[0] - from[0] }; + const T dy{ to[1] - from[1] }; + const T along{ std::clamp(((point[0] - from[0]) * dx + (point[1] - from[1]) * dy) / (dx * dx + dy * dy), T{}, T{ 1 }) }; + return math::Hypot(point[0] - (from[0] + along * dx), point[1] - (from[1] + along * dy)); + } + + template + auto ScanMatcher::SegmentNormal(const Point& from, const Point& to) -> Point + { + const T dx{ to[0] - from[0] }; + const T dy{ to[1] - from[1] }; + const T length{ math::Hypot(dx, dy) }; + return Point{ -dy / length, dx / length }; + } + + template + std::optional> ScanMatcher::Step(const Correspondences& correspondences) const + { + return parameters.metric == IcpMetric::PointToPoint ? PointToPointStep(correspondences) : PointToLineStep(correspondences); + } + + template + std::optional> ScanMatcher::PointToPointStep(const Correspondences& correspondences) const + { + const std::span moved{ correspondences.moved.data(), correspondences.size }; + const std::span matched{ correspondences.matched.data(), correspondences.size }; + const std::span weights{ correspondences.weights.data(), correspondences.size }; + const auto alignment{ solvers::PointSetAlignment::Align(moved, matched, false, weights) }; + return alignment ? std::optional{ math::SE2Transform::FromPose(alignment->translation[0], alignment->translation[1], math::Atan2(alignment->rotation.at(1, 0), alignment->rotation.at(0, 0))) } : std::nullopt; + } + + template + std::optional> ScanMatcher::PointToLineStep(const Correspondences& correspondences) const + { + const PointToLineProblem problem{ correspondences }; + const auto jacobian{ problem.Jacobian(math::SE2Transform::Identity()) }; + solvers::JacobiEigenSolver eigen{}; + const bool solved{ eigen.Solve(jacobian.Transpose() * jacobian) }; + if (!solved || eigen.Eigenvalues()[0] <= degeneracyRatio * eigen.Eigenvalues()[2]) + return std::nullopt; + + const optimization::ConstantDamping damping{ dampingSquared }; + const optimization::LevenbergMarquardt, MaxPoints, 3> solver{ problem, damping, { T{}, std::numeric_limits::infinity(), parameters.refinementIterations } }; + return solver.Solve(math::SE2Transform::Identity()).x; + } + + template + T ScanMatcher::Rms(const Correspondences& correspondences) const + { + T sum{}; + for (std::size_t i = 0; i < correspondences.size; ++i) + { + const T dx{ correspondences.moved[i][0] - correspondences.matched[i][0] }; + const T dy{ correspondences.moved[i][1] - correspondences.matched[i][1] }; + const T distance{ parameters.metric == IcpMetric::PointToPoint ? math::Hypot(dx, dy) : correspondences.normals[i][0] * dx + correspondences.normals[i][1] * dy }; + sum += correspondences.weights[i] * distance * distance; + } + return math::Sqrt(sum / static_cast(correspondences.accepted)); + } + + template + ScanMatchResult ScanMatcher::Result(std::span reference, std::span current, const math::SE2Transform& transform, std::size_t iterations, + bool converged) const + { + Correspondences correspondences{}; + Associate(reference, current, transform, correspondences); + return ScanMatchResult{ transform, iterations, correspondences.accepted, correspondences.accepted > 0 ? Rms(correspondences) : T{}, converged }; + } + + template + ScanMatcher::PointToLineProblem::PointToLineProblem(const Correspondences& correspondences) + : correspondences{ correspondences } + {} + + template + math::Vector ScanMatcher::PointToLineProblem::Residual(const math::SE2Transform& delta) const + { + math::Vector residual{}; + for (std::size_t i = 0; i < correspondences.size; ++i) + { + const auto moved{ delta.Apply(correspondences.moved[i]) }; + const auto& normal{ correspondences.normals[i] }; + residual[i] = correspondences.weights[i] * (normal[0] * (moved[0] - correspondences.matched[i][0]) + normal[1] * (moved[1] - correspondences.matched[i][1])); + } + return residual; + } + + template + math::Matrix ScanMatcher::PointToLineProblem::Jacobian(const math::SE2Transform& delta) const + { + math::Matrix jacobian{}; + for (std::size_t i = 0; i < correspondences.size; ++i) + { + const auto moved{ delta.Apply(correspondences.moved[i]) }; + const auto& normal{ correspondences.normals[i] }; + const T weight{ correspondences.weights[i] }; + jacobian.at(i, 0) = weight * normal[0]; + jacobian.at(i, 1) = weight * normal[1]; + jacobian.at(i, 2) = weight * (normal[1] * moved[0] - normal[0] * moved[1]); + } + return jacobian; + } + + template + math::SE2Transform ScanMatcher::PointToLineProblem::Retract(const math::SE2Transform& delta, const math::Vector& step) const + { + return math::SE2Transform::FromPose(step[0], step[1], step[2]) * delta; + } + +#ifdef ROBOTICS_TOOLBOX_COVERAGE_BUILD + extern template class ScanMatcher; +#endif +} diff --git a/robotics/localization/test/CMakeLists.txt b/robotics/localization/test/CMakeLists.txt index 295b3fe..efec557 100644 --- a/robotics/localization/test/CMakeLists.txt +++ b/robotics/localization/test/CMakeLists.txt @@ -11,4 +11,5 @@ target_sources(robotics.localization_test PRIVATE TestLandmarkLocalization.cpp TestOccupancyGridMap.cpp TestOdometryFusion.cpp + TestScanMatcher.cpp ) diff --git a/robotics/localization/test/TestScanMatcher.cpp b/robotics/localization/test/TestScanMatcher.cpp new file mode 100644 index 0000000..c0aa307 --- /dev/null +++ b/robotics/localization/test/TestScanMatcher.cpp @@ -0,0 +1,152 @@ +#include "numerical/math/SE2Transform.hpp" +#include "robotics/localization/ScanMatcher.hpp" +#include +#include +#include +#include +#include + +namespace +{ + using Transform = math::SE2Transform; + using Point = math::Vector2; + using Matcher = localization::ScanMatcher; + using localization::IcpMetric; + + template + std::array Walls(float rightStart, std::size_t rightCount, float topStart, std::size_t topCount, float bottomStart, std::size_t bottomCount) + { + std::array points{}; + std::size_t k{ 0 }; + for (std::size_t i = 0; i < rightCount; ++i) + points[k++] = Point{ 2.0f, rightStart + 0.1f * static_cast(i) }; + for (std::size_t i = 0; i < topCount; ++i) + points[k++] = Point{ topStart + 0.1f * static_cast(i), 1.5f }; + for (std::size_t i = 0; i < bottomCount; ++i) + points[k++] = Point{ bottomStart + 0.1f * static_cast(i), -1.0f }; + return points; + } + + template + std::array Map(const Transform& transform, const std::array& points) + { + std::array mapped{}; + for (std::size_t i = 0; i < N; ++i) + mapped[i] = transform.Apply(points[i]); + return mapped; + } + + class TestScanMatcher + : public ::testing::Test + { + protected: + static void ExpectNear(const Transform& actual, const Transform& expected) + { + const auto a{ actual.Pose() }; + const auto e{ expected.Pose() }; + for (std::size_t i = 0; i < 3; ++i) + EXPECT_NEAR(a[i], e[i], 1e-4f); + } + + std::array reference{ Walls<68>(-1.0f, 26, -0.5f, 26, -0.5f, 16) }; + Transform truth{ Transform::FromPose(0.1f, -0.05f, 0.05f) }; + std::array sameSpots{ Map(truth.Inverse(), reference) }; + std::array halfSpots{ Map(truth.Inverse(), Walls<65>(-0.95f, 25, -0.45f, 25, -0.45f, 15)) }; + localization::ScanMatcherParameters parameters{ IcpMetric::PointToLine, 0.5f, 50, 1e-6f, 1e-6f, 3, 3 }; + }; +} + +TEST_F(TestScanMatcher, both_metrics_recover_the_transform_with_exact_correspondences) +{ + auto pointToPoint{ parameters }; + pointToPoint.metric = IcpMetric::PointToPoint; + + const auto line{ Matcher{ parameters }.Match(reference, sameSpots, Transform::Identity()) }; + const auto point{ Matcher{ pointToPoint }.Match(reference, sameSpots, Transform::Identity()) }; + + ASSERT_TRUE(line.has_value()); + ASSERT_TRUE(point.has_value()); + ExpectNear(line->transform, truth); + ExpectNear(point->transform, truth); + EXPECT_EQ(line->correspondences, 68u); + EXPECT_EQ(point->correspondences, 68u); + EXPECT_TRUE(line->converged); + EXPECT_TRUE(point->converged); + EXPECT_NEAR(line->rms, 0.0f, 1e-4f); + EXPECT_NEAR(point->rms, 0.0f, 1e-4f); + EXPECT_EQ(point->iterations, 3u); + EXPECT_EQ(line->iterations, 2u); +} + +TEST_F(TestScanMatcher, point_to_line_removes_the_sampling_bias) +{ + auto pointToPoint{ parameters }; + pointToPoint.metric = IcpMetric::PointToPoint; + + const auto line{ Matcher{ parameters }.Match(reference, halfSpots, Transform::Identity()) }; + const auto point{ Matcher{ pointToPoint }.Match(reference, halfSpots, Transform::Identity()) }; + + ASSERT_TRUE(line.has_value()); + ExpectNear(line->transform, truth); + EXPECT_EQ(line->correspondences, 65u); + ASSERT_TRUE(point.has_value()); + const auto error{ (truth.Inverse() * point->transform).Pose() }; + EXPECT_GT(std::max({ std::abs(error[0]), std::abs(error[1]), std::abs(error[2]) }), 0.01f); +} + +TEST_F(TestScanMatcher, point_to_line_converges_from_an_offset_guess) +{ + const auto guess{ Transform::FromPose(-0.05f, 0.05f, -0.05f) }; + + const auto result{ Matcher{ parameters }.Match(reference, sameSpots, guess) }; + + ASSERT_TRUE(result.has_value()); + ExpectNear(result->transform, truth); + + auto single{ parameters }; + single.maxIterations = 1; + const auto truncated{ Matcher{ single }.Match(reference, sameSpots, guess) }; + ASSERT_TRUE(truncated.has_value()); + EXPECT_FALSE(truncated->converged); +} + +TEST_F(TestScanMatcher, far_outlier_is_rejected) +{ + std::array withOutlier{}; + for (std::size_t i = 0; i < halfSpots.size(); ++i) + withOutlier[i] = halfSpots[i]; + withOutlier[65] = Point{ 0.6f, 0.2f }; + + const auto result{ Matcher{ parameters }.Match(reference, withOutlier, Transform::Identity()) }; + + ASSERT_TRUE(result.has_value()); + EXPECT_EQ(result->correspondences, 65u); + ExpectNear(result->transform, truth); +} + +TEST_F(TestScanMatcher, too_few_correspondences_fail) +{ + auto tight{ parameters }; + tight.maxCorrespondenceDistance = 0.01f; + + EXPECT_FALSE(Matcher{ tight }.Match(reference, halfSpots, Transform::Identity()).has_value()); + + const std::array oversized{}; + EXPECT_FALSE(Matcher{ parameters }.Match(reference, oversized, Transform::Identity()).has_value()); +} + +TEST_F(TestScanMatcher, corridor_is_reported_as_degenerate) +{ + std::array corridor{}; + std::array shifted{}; + for (std::size_t i = 0; i < 21; ++i) + { + const float x{ -1.0f + 0.1f * static_cast(i) }; + corridor[i] = Point{ x, -1.0f }; + corridor[21 + i] = Point{ x, 1.0f }; + shifted[i] = Point{ x + 0.1f, -1.0f }; + shifted[21 + i] = Point{ x + 0.1f, 1.0f }; + } + + EXPECT_FALSE(Matcher{ parameters }.Match(corridor, shifted, Transform::Identity()).has_value()); +}