Skip to content

docs(roadmap): add the localization specs (E1–E5) #31

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
gabrielfrasantos merged 2 commits into from
Oct 24, 2026
Merged

docs(roadmap): add the localization specs (E1–E5) #31

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
9e3365b
docs(roadmap): add the localization specs (E1–E5)
claude Oct 24, 2026
1e25a4e
docs: wrap the interface-destructor rule
claude Oct 24, 2026
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
7 changes: 6 additions & 1 deletion .github/instructions/robotics-cpp.instructions.md
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -56,7 +56,12 @@ Use `math::IsFinite` for finiteness checks: a consumer may still build with `-ff
- SOLID principles — constructor injection, depend on abstractions
- DRY — extract shared logic into helpers or templates
- RAII for resource management
- **Protected, non-virtual interface destructors**: declare `~Interface() = default` in a `protected:` section, never `virtual ~Interface()` (defaulted or `= 0`). A virtual destructor puts a deleting destructor in every implementation's vtable, which links `operator delete` (and with it `malloc`/`free`) into heap-free firmware, and under multiple inheritance adds a thunk per base. The protected destructor makes deletion through an interface pointer a compile error. Concrete classes declare no destructor unless they need one, and callers hold the concrete type rather than a `std::unique_ptr` to the interface.
- **Protected, non-virtual interface destructors**: declare `~Interface() = default` in a `protected:` section,
never `virtual ~Interface()` (defaulted or `= 0`). A virtual destructor puts a deleting destructor in every
implementation's vtable, which links `operator delete` (and with it `malloc`/`free`) into heap-free firmware, and
under multiple inheritance adds a thunk per base. The protected destructor makes deletion through an interface
pointer a compile error. Concrete classes declare no destructor unless they need one, and callers hold the
concrete type rather than a `std::unique_ptr` to the interface.
- No virtual calls in ISR-callable or real-time critical paths

## Documentation — MANDATORY
Expand Down
72 changes: 38 additions & 34 deletions ROADMAP.md
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,8 +1,8 @@
# Robotics Toolbox — Roadmap

Prioritized backlog by robot family. Manipulators (**M**) and wheeled robots (**W**, with **F1**) have specs
(`roadmap/<domain>/<Name>/`); the other families are entries only so far, and their specs come family by
family:
Prioritized backlog by robot family. Manipulators (**M**), wheeled robots (**W**, with **F1**) and
localization (**E**) have specs (`roadmap/<domain>/<Name>/`); the other families are entries only so far, and
their specs come family by family:

| Prefix | Family | Section |
|--------|---------------------------------------------------------------|---------------------------------------------------------------------------|
Expand Down Expand Up @@ -43,29 +43,30 @@ master-list item number (NT53 is upstream item 53). A robotics item that depends
numerical-toolbox pin that ships it. NT53–NT65 have shipped upstream, and their links point to the upstream
docs; the current pin (`63c9b91`) includes them.

| # | Component | Upstream module | Spec / doc | Needed by |
|------|-------------------------------------------------------|----------------------|-----------------------------------|----------------------------------------------------------------------------|
| NT53 | SE(3) transform, twists, wrenches, exp/log, PoseError | `math` | [SE3Transform][nt53] | M7, M8, M10, M13, M15, M21, M23, M25, M26, M30, F3, A1, A6, L3, S2, G1, C1 |
| NT54 | Angle utilities (wrap, shortest difference, unwrap) | `math` | [AngleUtilities][nt54] | M21, W6, W9, W10, S1 |
| NT55 | Polynomial point-to-point profile (+ profile types) | `motion_profiles` | [PolynomialProfile][nt55] | M10, F4, L2 |
| NT56 | Trapezoidal (LSPB) profile + duration sync | `motion_profiles` | [TrapezoidalProfile][nt56] | M10 (time law, optional), F4 |
| NT57 | Friction compensation (Coulomb + viscous + Stribeck) | `controllers` | [FrictionCompensation][nt57] | M5, M12, M29 (optional feedforward) |
| NT58 | Symplectic integrators (semi-implicit Euler, Verlet) | `solvers` | [SymplecticIntegrators][nt58] | M34 |
| NT59 | Tridiagonal (Thomas) solver | `math` | [TridiagonalSolver][nt59] | NT61 |
| NT60 | S-curve (jerk-limited) profile | `motion_profiles` | [SCurveProfile][nt60] | M10 (time law, optional) |
| NT61 | Cubic spline interpolation (vector-valued) | `math` | [CubicSpline][nt61] | M33 |
| NT62 | Damped pseudo-inverse + null-space projector | `solvers` | [DampedPseudoInverse][nt62] | M14, H5 |
| NT63 | Incremental QR least squares (rank-revealing) | `estimators/offline` | [IncrementalQrLeastSquares][nt63] | M22 |
| NT64 | Levenberg–Marquardt (retraction, pluggable damping) | `optimization` | [LevenbergMarquardt][nt64] | M13, M23, E5, C2 |
| NT65 | Dense convex QP (Goldfarb–Idnani) | `solvers` | [ActiveSetQp][nt65] | M25 (exact path), L6, G2, G3 |
| NT66 | SE(2) transform, twists, exp/log, pose error | `math` | [SE2Transform][nt66] (spec) | F1, W1–W10, M24, E1, E2, E3, E5 |
| NT67 | Kabsch / Umeyama point-set alignment | `solvers` | [master list][ntrm], no spec yet | E5, C1 |
| NT68 | Lie-group Jacobians and ⊕/⊖ (SO(3), SE(3), SE(2)) | `math` | [master list][ntrm], no spec yet | C1 (and NT71) |
| NT69 | Control allocation (redistributed pseudo-inverse, QP) | `controllers` | [master list][ntrm], no spec yet | A2, S3 |
| NT70 | Particle filter (bounded, systematic resampling) | `filters/active` | [master list][ntrm], no spec yet | E4 |
| NT71 | Error-state EKF on Lie groups | `filters/active` | [master list][ntrm], no spec yet | L5 |
| NT72 | Minimum-derivative (min-snap) piecewise polynomials | `motion_profiles` | [master list][ntrm], no spec yet | A7 |
| NT73 | Online jerk-limited trajectory from arbitrary state | `motion_profiles` | [master list][ntrm], no spec yet | F2 |
| # | Component | Upstream module | Spec / doc | Needed by |
|------|-------------------------------------------------------|----------------------|-------------------------------------|----------------------------------------------------------------------------|
| NT53 | SE(3) transform, twists, wrenches, exp/log, PoseError | `math` | [SE3Transform][nt53] | M7, M8, M10, M13, M15, M21, M23, M25, M26, M30, F3, A1, A6, L3, S2, G1, C1 |
| NT54 | Angle utilities (wrap, shortest difference, unwrap) | `math` | [AngleUtilities][nt54] | M21, W6, W9, W10, E1, E3, S1 |
| NT55 | Polynomial point-to-point profile (+ profile types) | `motion_profiles` | [PolynomialProfile][nt55] | M10, F4, L2 |
| NT56 | Trapezoidal (LSPB) profile + duration sync | `motion_profiles` | [TrapezoidalProfile][nt56] | M10 (time law, optional), F4 |
| NT57 | Friction compensation (Coulomb + viscous + Stribeck) | `controllers` | [FrictionCompensation][nt57] | M5, M12, M29 (optional feedforward) |
| NT58 | Symplectic integrators (semi-implicit Euler, Verlet) | `solvers` | [SymplecticIntegrators][nt58] | M34 |
| NT59 | Tridiagonal (Thomas) solver | `math` | [TridiagonalSolver][nt59] | NT61 |
| NT60 | S-curve (jerk-limited) profile | `motion_profiles` | [SCurveProfile][nt60] | M10 (time law, optional) |
| NT61 | Cubic spline interpolation (vector-valued) | `math` | [CubicSpline][nt61] | M33 |
| NT62 | Damped pseudo-inverse + null-space projector | `solvers` | [DampedPseudoInverse][nt62] | M14, H5 |
| NT63 | Incremental QR least squares (rank-revealing) | `estimators/offline` | [IncrementalQrLeastSquares][nt63] | M22 |
| NT64 | Levenberg–Marquardt (retraction, pluggable damping) | `optimization` | [LevenbergMarquardt][nt64] | M13, M23, E5, C2 |
| NT65 | Dense convex QP (Goldfarb–Idnani) | `solvers` | [ActiveSetQp][nt65] | M25 (exact path), L6, G2, G3 |
| NT66 | SE(2) transform, twists, exp/log, pose error | `math` | [SE2Transform][nt66] (spec) | F1, W1–W10, M24, E1, E2, E3, E5 |
| NT67 | Kabsch / Umeyama point-set alignment | `solvers` | [PointSetAlignment][nt67] (spec) | E5, C1 |
| NT68 | Lie-group Jacobians and ⊕/⊖ (SO(3), SE(3), SE(2)) | `math` | [master list][ntrm], no spec yet | C1 (and NT71) |
| NT69 | Control allocation (redistributed pseudo-inverse, QP) | `controllers` | [master list][ntrm], no spec yet | A2, S3 |
| NT70 | Particle filter (bounded, systematic resampling) | `filters/active` | [ParticleFilter][nt70] (spec) | E4 |
| NT71 | Error-state EKF on Lie groups | `filters/active` | [master list][ntrm], no spec yet | L5 |
| NT72 | Minimum-derivative (min-snap) piecewise polynomials | `motion_profiles` | [master list][ntrm], no spec yet | A7 |
| NT73 | Online jerk-limited trajectory from arbitrary state | `motion_profiles` | [master list][ntrm], no spec yet | F2 |
| NT74 | Residual-form Kalman update with χ² gating | `filters/active` | [ResidualKalmanUpdate][nt74] (spec) | E1, E3 |

[ntrm]: https://github.com/embedded-pro/numerical-toolbox-cpp/blob/main/ROADMAP.md#master-list-by-priority
[nt53]: https://github.com/embedded-pro/numerical-toolbox-cpp/blob/main/doc/math/SE3Transform.md
Expand All @@ -82,6 +83,9 @@ docs; the current pin (`63c9b91`) includes them.
[nt64]: https://github.com/embedded-pro/numerical-toolbox-cpp/blob/main/doc/optimization/LevenbergMarquardt.md
[nt65]: https://github.com/embedded-pro/numerical-toolbox-cpp/blob/main/doc/solvers/ActiveSetQp.md
[nt66]: https://github.com/embedded-pro/numerical-toolbox-cpp/blob/main/roadmap/math/SE2Transform/implementation.md
[nt67]: https://github.com/embedded-pro/numerical-toolbox-cpp/blob/main/roadmap/solvers/PointSetAlignment/implementation.md
[nt70]: https://github.com/embedded-pro/numerical-toolbox-cpp/blob/main/roadmap/filters/active/ParticleFilter/implementation.md
[nt74]: https://github.com/embedded-pro/numerical-toolbox-cpp/blob/main/roadmap/filters/active/ResidualKalmanUpdate/implementation.md

## Robot manipulators & other manipulator types

Expand Down Expand Up @@ -490,29 +494,29 @@ Cross-family building blocks that several of the families below depend on.

| # | Component | Target module | Depends on | Difficulty |
|----|--------------------------------------------------------|----------------|------------------|------------|
| E1 | Wheel + IMU odometry fusion (EKF on SE(2)) | `localization` | F1, NT66 | ★★★☆☆ |
| E1 | Wheel + gyro odometry fusion (EKF on SE(2)) | `localization` | F1, NT66, NT74 | ★★★☆☆ |
| E2 | Occupancy-grid mapping (log-odds, ray casting) | `localization` | NT66 | ★★★☆☆ |
| E3 | EKF landmark localization (known map, range–bearing) | `localization` | NT66 | ★★★☆☆ |
| E3 | EKF landmark localization (known map, range–bearing) | `localization` | F1, NT66, NT74 | ★★★☆☆ |
| E4 | Monte Carlo localization | `localization` | NT70, E2, F1 | ★★★★☆ |
| E5 | 2-D scan matching (point-to-point / point-to-line ICP) | `localization` | NT67, NT64, NT66 | ★★★★☆ |

**E1. Odometry fusion.** EKF combining wheel odometry with gyro (and optionally accelerometer) on SE(2), with slip-aware process noise.
- *Algorithm / paper:* S. Thrun, W. Burgard, D. Fox, *Probabilistic Robotics* (2005), Ch. 5 and 7.
- *Reuses / builds on:* F1, NT66; upstream `ExtendedKalmanFilter`.
**E1. Odometry fusion.** EKF on `(x, y, θ, gyro bias)`: wheel speed and gyro rate drive the prediction, the wheel yaw rate is a χ²-gated bias measurement (a rejected update flags slip), and optional heading and position fixes share the same update.
- *Algorithm / paper:* S. Thrun, W. Burgard, D. Fox, *Probabilistic Robotics* (2005), Ch. 5 and 7; J. Borenstein, L. Feng, "Gyrodometry," *ICRA* 1996.
- *Reuses / builds on:* F1, NT66, NT74 (upstream `ExtendedKalmanFilter` cannot wrap residuals or gate).

**E2. Occupancy-grid mapping.** Log-odds cell updates from range scans along Bresenham rays on a bounded grid.
- *Algorithm / paper:* A. Elfes, *Computer*, 22(6), 1989; Thrun et al. (2005), Ch. 9.
- *Reuses / builds on:* NT66.

**E3. EKF landmark localization.** Pose tracking against a known landmark map with range–bearing measurements and gated data association.
- *Algorithm / paper:* Thrun et al. (2005), Ch. 7.
- *Reuses / builds on:* NT66; upstream `ExtendedKalmanFilter`.
- *Reuses / builds on:* F1 (odometry noise), NT66, NT74.

**E4. Monte Carlo localization.** Particle-filter localization in an occupancy grid with the odometry motion model and a likelihood-field sensor model.
**E4. Monte Carlo localization.** Particle-filter localization in an occupancy grid with the odometry motion model and a likelihood-field sensor model (two-pass chamfer distance map); samplers injected by the caller.
- *Algorithm / paper:* F. Dellaert, D. Fox, W. Burgard, S. Thrun, *ICRA* 1999.
- *Reuses / builds on:* NT70, E2, F1.

**E5. Scan matching.** Iterative closest point between consecutive laser scans or against a map, with point-to-line error and outlier rejection.
**E5. Scan matching.** Iterative closest point between consecutive laser scans or against a map: point-to-point (Kabsch) or point-to-line (Levenberg–Marquardt), outlier rejection and degeneracy detection.
- *Algorithm / paper:* P. Besl, N. McKay, *IEEE T-PAMI*, 14(2), 1992; A. Censi, *ICRA* 2008.
- *Reuses / builds on:* NT67, NT64, NT66.

Expand Down
2 changes: 1 addition & 1 deletion roadmap/DEPLOYMENT.md
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -21,7 +21,7 @@ Read the spec's three files first (`implementation.md`, `tests.md`, `explanation
4. **CMake**
- Add `.hpp` to `target_sources(...)`, `.cpp` to `robotics_add_coverage_sources(...)`,
`Test<Name>.cpp` to the `_test` target's `target_sources`.
- New module (`trajectory`, `controllers/manipulator`, `mobile`, `planning`):
- New module (`trajectory`, `controllers/manipulator`, `mobile`, `planning`, `localization`):
create `robotics/<module>/CMakeLists.txt` via `robotics_add_header_library(...)`, add a
`test/` subdir, register it in the parent `CMakeLists.txt`, and add a `doc/<module>/` folder.

Expand Down
7 changes: 5 additions & 2 deletions roadmap/README.md
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -3,8 +3,8 @@
Design-level pseudocode for proposed components in [../ROADMAP.md](../ROADMAP.md).
These are **specifications, not compilable code** — they describe *what* to build and *how*
the algorithm works, so an implementer can produce the real templated C++ afterwards. Every manipulator
(M) and wheeled-robot (W) item has a spec, as does F1; the other families (F, A, L, S, G, P, E, C, H) are
ROADMAP entries only, and their specs are added here family by family.
(M), wheeled-robot (W) and localization (E) item has a spec, as does F1; the other families (F, A, L, S, G,
P, C, H) are ROADMAP entries only, and their specs are added here family by family.

## Convention

Expand Down Expand Up @@ -103,5 +103,8 @@ prerequisites.
### `planning`
`DubinsPath` (W8) · `DynamicWindowApproach` (W9) · `ReedsSheppPath` (W10)

### `localization`
`OdometryFusion` (E1) · `OccupancyGridMap` (E2) · `LandmarkLocalization` (E3) · `MonteCarloLocalization` (E4) · `ScanMatcher` (E5)

### `controllers/manipulator`
`PdGravityCompensation` (M5) · `ComputedTorqueControl` (M12) · `ImpedanceControl` (M17) · `OperationalSpaceControl` (M18) · `HybridPositionForceControl` (M19) · `SlotineLiAdaptiveControl` (M20) · `CableTensionDistribution` (M25)
Loading
Loading