Algorithms & Mathematical Foundations
This document provides rigorous mathematical descriptions of the core solvers in Locus. For the pipeline data flow, see Pipeline. For memory layout, see Memory Model.
1. Homography Estimation (DLT)
Module: decoder.rs (Homography, HomographyDda)
A homography \(\mathbf{H} \in \mathbb{R}^{3 \times 3}\) is the projective transformation mapping points from canonical tag space to image pixels:
where \(\tilde{\mathbf{p}} = [x, y, 1]^T\) are homogeneous coordinates.
1.1 General DLT (from_pairs)
Given 4 point correspondences \((\mathbf{s}_i, \mathbf{d}_i)\), the Direct Linear Transform constructs an \(8 \times 9\) matrix \(\mathbf{A}\) where each correspondence contributes two rows:
Setting \(h_{33} = 1\) (dehomogenization), the remaining 8 parameters are solved via LU decomposition of the reduced \(8 \times 8\) system:
A post-hoc reprojection check validates \(\|\mathbf{H} \tilde{\mathbf{s}}_i - \tilde{\mathbf{d}}_i\|^2 < 10^{-4}\) for all 4 points, rejecting degenerate configurations.
1.2 Optimized Square-to-Quad (square_to_quad)
When the source points are the canonical unit square \(\{(-1,-1), (1,-1), (1,1), (-1,1)\}\), the coefficient matrix \(\mathbf{M}\) has a fixed sparsity pattern. The implementation hardcodes these coefficients, eliminating the generic source-point loop while producing an identical \(8 \times 8\) LU solve.
1.3 Digital Differential Analyzer (DDA)
For grid-based sampling (decoding), recomputing \(\mathbf{H} \tilde{\mathbf{p}}\) per pixel is wasteful. The DDA exploits the linearity of the homogeneous numerators:
Stepping by \((\Delta u, \Delta v)\) in tag space requires only 3 additions per pixel:
The perspective divide \(\mathbf{p}_{\text{img}} = (n_x/d, \; n_y/d)\) is computed using a hardware reciprocal approximation (rcp_nr) with one Newton-Raphson refinement step for ~23-bit accuracy.
2. Sub-pixel Corner Refinement
Two corner estimators run in sequence on the default path. The ERF edge fit (§2.1–2.4) refines quad corners during extraction or, under decode-first ordering, on the candidates that decode. On every decoded marker, decoder.corner_subpix (standard, grid) then re-estimates the corners from junctions and whole-edge lines (§2.5) and removes the photometric inset the marker's own bit edges measure (§2.6).
Module (ERF): edge_refinement.rs
2.1 Intensity Model
The observed intensity along the normal to a tag edge is modeled as a PSF-blurred step function:
where:
- \(A, B\) are the dark and light intensities on either side of the edge.
- \(d\) is the signed perpendicular distance from the sample point to the edge line.
- \(\sigma\) is the Gaussian blur parameter (PSF width).
- \(\operatorname{erf}(x) = \frac{2}{\sqrt{\pi}} \int_0^x e^{-t^2} dt\) is the error function.
2.2 Edge Parameterization
The edge line is parameterized by its perpendicular offset \(\rho\) from an initial estimate:
where \(\mathbf{n}\) is the unit normal to the edge and \(\mathbf{p}_0\) is a point on the initial edge estimate.
2.3 Gauss-Newton Optimization
The scalar parameter \(\rho\) is refined by minimizing the sum of squared residuals:
The Jacobian for each sample is:
The Gauss-Newton update is:
with convergence when \(|\Delta\rho| < 10^{-4}\) or after 15 iterations.
2.4 Configuration Variants
| Variant | A/B Estimation | Min Contrast | Use Case |
|---|---|---|---|
| Quad-style | Once before GN loop | None | Quad corner refinement |
| Decoder-style | Per iteration | Early exit if $ | B-A |
The erf_approx function is shared across both variants and has a SIMD-vectorized 4-wide version (erf_approx_v4) for batch evaluation.
2.5 Gradient-orthogonality corners and whole-edge fusion
Module: refinement.rs (subpix_marker_corners)
Junction estimate. At a corner \(\mathbf{c}\), every image gradient in the neighbourhood is orthogonal to \(\mathbf{p} - \mathbf{c}\): zero on flat patches, normal to the edge on an edge through \(\mathbf{c}\). The corner therefore solves the cv::cornerSubPix normal equations over a Gaussian-weighted window,
re-centred until the step is below 0.005 px. A sandwich covariance from the residual gives each solution an uncertainty. The window half-width is tried at 0.3, 0.5 and 0.75 of the marker cell (clamped to 2–4 px: past the blur, inside the border cell). The largest window whose uncertainty is within a factor of 2 of the best is kept. The estimator converges on any junction in its window, so a solution is kept only if the image around it still shows the corner of this marker's black border: the inward diagonal is darker than the two quiet-zone side probes. Markers with cells under ~3.3 px keep their seed corners.
Gross-corner repair. A corner the junction model rejects while both neighbours pass is usually not near the marker corner at all: quad extraction cut across a blurred apex or a touching square. Its two edges still run straight from the good neighbours. Each is refitted on its half next to the good neighbour, the corner is re-placed at their crossing (iterated, at most a quarter side), and the result must pass the junction model. A repaired corner can make its neighbour repairable, so the sweep repeats until nothing changes.
Whole-edge fusion (undistorted images). The junction sees only the corner's neighbourhood; each edge carries information along its whole length. Each side is fitted as a straight line through gradient-weighted edge stations over its middle 70 %, with \(\operatorname{Var}(\text{offset}) = s^2/N\) and \(\operatorname{Var}(\text{angle}) = s^2/\sum t^2\) from the stations' own scatter, so bit edges or clutter in the band inflate the line's uncertainty. Two adjacent lines give a line corner and its covariance. At an L-corner the junction and line corners are fused by inverse covariance when they agree (Mahalanobis \(d^2 \le 9.21\), the \(\chi^2_2\) 99 % point). Fusion is all-or-nothing per marker: mixing estimators with different apex biases would turn the square into rotation error. At an X-junction (an AprilGrid connector touching the corner) the edge lines carry the photometric edge offset and the junction point does not, so the junction estimate stands alone.
2.6 Marker photometric inset calibration
Module: marker_inset.rs
Why. A camera blurs in linear light and then applies its tone curve. On the recorded intensities every blurred edge is therefore shifted by some \(\delta\) toward its dark side — 0.3–0.5 px on sRGB images, growing with blur. Every gradient corner estimator (Locus's, OpenCV's, aruco_nano's) reports a marker's corners 0.4–0.7 px inside the printed ones. On a single tag that inset reads as depth (centimetres of translation), and under perspective it is not even the image of a smaller square, so it becomes rotation error too. Since \(\delta\) depends on blur, which varies with depth along an oblique edge, it cannot be a detector constant.
Fit per marker. The decoded marker measures \(\delta\) itself. Its bit boundaries come in both polarities; each shifts toward its dark side by the same \(\delta\), and the decoded layout says where each boundary should be. For each boundary station, the measured edge offset from where the corners' homography puts it is modelled as
with \(\sigma = \pm 1\) the side that is dark, \(s\) a scale of the interior layout about the marker centre, \(\varepsilon\) per side how far the detected corners sit inside the true outline, and an optional bow term (added only when an F-test supports it) for lens distortion the pinhole model does not describe. Polarity separates \(\delta\); the outer boundaries, which carry no \(s\) term, pin \(\varepsilon\). The ten-parameter robust (Huber IRLS) fit needs no tone curve, blur model or camera knowledge, and the corners move outward by \(\varepsilon\) per side. Guards (enough boundaries found, small residual scale, \(|s - 1| \le 0.05\), inset \(\le 1.5\) px, cells ≥ ~3.3 px) leave a marker whose layout does not match — e.g. a 2-bit Kalibr border decoded as a 1-bit tag — with its corners unchanged. See the rotation-tail lessons.
2.7 Board-level inset (BoardEstimator)
Module: board.rs
On a board the pose is shared, so the inset is estimated jointly instead: the board LM alternates with a per-frame Huber estimate of the marker-corner inset along each corner's edge-offset direction. Each corner estimator gets its own \(\delta\) — corners placed by the junction pass (corner_refined bit set) and corners from the ERF edge fit carry different photometric offsets (equal on L-corners, not on AprilGrid's X-junctions). An inset is modelled only when it exceeds three standard errors.
3. IPPE-Square Pose Estimation
Module: pose.rs
3.1 Homography Normalization
The pixel-space homography \(\mathbf{H}_\text{pixel}\) from square_to_quad is normalized by the inverse intrinsics:
The metric homography accounting for tag size \(s\) and the modern OpenCV top-left origin convention:
3.2 Jacobian SVD Decomposition
The Jacobian \(\mathbf{J} = [\mathbf{h}_1, \mathbf{h}_2]\) encodes the image-plane stretch. Its \(2 \times 2\) Gram matrix:
has eigenvalues \(\sigma_1^2, \sigma_2^2\) (singular values squared) computed analytically:
3.3 Dual Pose Solutions
IPPE-Square produces two candidate poses from the SVD, corresponding to the two minima of the planar PnP error surface (the "Necker reversal" ambiguity). The candidate with the lower reprojection error is selected.
Frontal degeneracy: When \(|\sigma_1 - \sigma_2| < 10^{-4} \sigma_1\), the two solutions collapse. In this case, Gram-Schmidt orthonormalization of \([\mathbf{h}_1, \mathbf{h}_2, \mathbf{h}_1 \times \mathbf{h}_2]\) produces the rotation, and the translation scale \(\gamma = (\sigma_1 + \sigma_2)/2\).
3.4 Orthogonalization
The raw rotation estimate from the SVD is projected onto \(SO(3)\) via polar decomposition:
ensuring \(\det(\mathbf{R}) = +1\).
4. Levenberg-Marquardt Pose Refinement
The LM stage selects its cost surface from the availability of per-corner
covariances — not a user mode flag. When covariances are present (image
view supplied to the detector, or external covariances passed by a direct
caller of pose::estimate_tag_pose_with_config),
the weighted-Mahalanobis path (§4.2) runs and returns a 6×6 pose
covariance. When the caller skips both (e.g. a pure pose-only refit from
pre-extracted corners), the LM falls back to the unweighted Huber path
(§4.1) and reports no covariance. Both paths share the same IPPE-Square
seed and the same Marquardt / Nielsen trust-region machinery.
4.1 Unweighted Huber LM (fallback when covariances unavailable)
Module: pose.rs | Function: refine_pose_lm
Objective
Minimize Huber-robust geometric reprojection error:
where \(\mathbf{r}_i = \mathbf{p}_i^\text{obs} - \pi(\mathbf{R} \mathbf{P}_i + \mathbf{t})\) is the 2D reprojection residual and \(\pi\) is the pinhole projection.
Huber Loss
Applied per corner as an IRLS weight: \(w_i = \min(1, \; \delta / \|\mathbf{r}_i\|)\).
Jacobian Structure
The \(2 \times 6\) Jacobian for corner \(i\) with respect to the \(\mathfrak{se}(3)\) twist \(\boldsymbol{\xi} = [\mathbf{t}, \boldsymbol{\omega}]^T\):
where:
Normal Equations with Marquardt Scaling
Marquardt diagonal scaling damps each DOF proportionally to its own curvature, correcting the scale mismatch between translational and rotational gradient magnitudes. Solved via Cholesky decomposition.
Trust-Region (Nielsen Schedule)
- Accept: Gain ratio \(\rho = \text{actual}/\text{predicted} > 0\) triggers \(\lambda \leftarrow \lambda \cdot \max(1/3, \; 1 - (2\rho-1)^3)\), \(\nu \leftarrow 2\).
- Reject: \(\lambda \leftarrow \lambda \cdot \nu\), \(\nu \leftarrow 2\nu\).
Manifold Update
The \(\mathfrak{se}(3)\) increment is applied via the exponential map:
The rotation update uses UnitQuaternion::from_scaled_axis for numerically stable \(SO(3)\) integration.
Convergence
- Gradient convergence (primary): \(\|\mathbf{J}^T \mathbf{W} \mathbf{r}\|_\infty < 10^{-8}\)
- Function convergence (secondary): relative cost reduction \((c_\text{prev} - c_\text{new}) / c_\text{prev} < 10^{-6}\) — a unit-free, scale-invariant gate (replacing a mixed-unit \(\|\boldsymbol{\delta}\|\) step gate that summed rad² and m²).
- Maximum 20 iterations (typically exits in 3-6).
4.2 Weighted LM (Mahalanobis Distance)
Module: pose_weighted.rs | Function: refine_pose_lm_weighted
Objective
Minimize Huber-robust Mahalanobis distance using per-corner information matrices:
where:
is the Mahalanobis distance and \(\mathbf{W}_i = \boldsymbol{\Sigma}_i^{-1}\) is the information matrix (inverse of the \(2 \times 2\) corner covariance from the image Structure Tensor, or the caller-supplied external covariances).
Huber-on-Mahalanobis IRLS
The IRLS weight in the Mahalanobis metric:
The augmented information matrix: \(\tilde{\mathbf{W}}_i = w(s_i) \cdot \mathbf{W}_i\)
The threshold \(k = 1.345\) provides 95% asymptotic efficiency under Gaussian noise.
Normal Equations
The Jacobian \(\mathbf{J}_i\) has the same structure as in Fast mode.
Corner Uncertainty Sources
| Source | Method | Module |
|---|---|---|
| Structure Tensor | \(\boldsymbol{\Sigma}_c \approx \sigma_n^2 \mathbf{S}^{-1}\) where \(\mathbf{S}\) is the Sobel-based structure tensor | pose_weighted.rs |
| External | Caller-supplied \(2 \times 2\) covariances (direct pose::estimate_tag_pose_with_config callers only; the detector pipeline always uses the Structure Tensor) |
pose.rs |
Gain-Scheduled Tikhonov Regularization
For severely foreshortened tags (grazing angles), the structure tensor becomes ill-conditioned. A gain-scheduled regularizer prevents the information matrix from exploding:
where the anisotropy ratio \(R = \lambda_{\min} / \lambda_{\max}\) and:
The quadratic transfer function keeps \(\alpha \approx 0\) for well-conditioned frontal tags (\(R \to 1\)) and smoothly ramps to \(\alpha_{\max}\) for degenerate configurations (\(R \to 0\)).
Covariance Output
At convergence, the \(6 \times 6\) pose covariance (Cramer-Rao lower bound) is extracted from the final normal equations:
This encodes the full translational and rotational uncertainty and is returned alongside the refined pose.
5. Decoding Strategies
Module: decoder.rs, strategy.rs, dictionaries.rs
Each bit cell is sampled at its grid center via the homography DDA. The sampled intensity is compared against a per-cell adaptive threshold to produce a binary code (strategy::bits_from_intensities).
Nearest codeword. The code is matched against all four rotations of every codeword in one branch-free pass: each entry yields the key \((\operatorname{popcount}(b \oplus c_i) \ll 16) \,|\, i\), and the minimum key is the nearest codeword, lowest index on ties. The pass is a vectorizable popcount-and-min, dispatched at runtime to popcnt/AVX2 or NEON; it is \(O(\text{codes})\) per candidate, not a table lookup. (A multi-index-hash path exists for payloads above 36 bits; no shipped family uses it.)
Acceptance. A match stands when its distance is within the family budget (decoder.max_hamming_error, else the family default) and the marker's dark border ring passes its error budget (see Pipeline §4d). Near misses within the family's recovery window — the largest distance random bits reach with probability ≤ 0.1 — get a refinement retry when the seed shows a dark ring.
6. Board-Level Pose Estimation
Module: board.rs, charuco.rs
Board pose estimation aggregates evidence from multiple detected tags into a single, more precise 6-DOF board pose. The solver backend (RobustPoseSolver) is shared by both board types; the two paths differ only in how they assemble the point correspondences.
6.1 AprilGrid: Tag-Corner Correspondences
Struct: BoardEstimator
Each visible tag contributes 4 point correspondences: its refined image corners paired with pre-computed 3D board-frame coordinates from AprilGridTopology::obj_points.
The board LM also estimates a per-frame photometric inset for each corner estimator (junction-pass corners vs. ERF corners), alternating with the pose; see §2.7.
The group_size=4 parameter tells RobustPoseSolver that these 4 points belong to a rigid group — RANSAC hypotheses are drawn from whole tags, not individual corners, preventing degenerate single-tag hypotheses.
Seed poses are derived from each tag's individual Stage-5 pose by subtracting the tag's board-frame origin:
where \(\mathbf{o}_{\text{tag}}\) is the tag's top-left corner in board coordinates.
6.2 ChAruco: Saddle-Point Correspondences
Struct: CharucoRefiner
ChAruco boards embed ArUco markers in alternating cells of a checkerboard. The interior checkerboard corners — saddle points — are sharper, more photometrically stable features than tag corners and are used as the primary measurement for board pose.
Board Geometry (Two-Layer Model)
| Layer | Feature | Coordinate Source |
|---|---|---|
| A — Tags | ArUco markers occupying cells where \((r+c)\) is even. Each tag fills marker_length × marker_length within its square_length × square_length cell. |
CharucoTopology::obj_points |
| B — Saddles | Interior checkerboard corners at the outer corners of each square. Indexed as sr*(cols-1)+sc for grid position (sr, sc). |
CharucoTopology::saddle_points |
The white padding margin between a tag edge and its enclosing square corner is:
This means saddle points are outside the tag's physical boundary — they cannot be obtained by interpolating the tag's own corners.
Saddle Prediction via Homography Extrapolation
Each tag stores a \(3 \times 3\) homography mapping canonical coordinates \([-1, 1]^2\) to the tag's image corners. The canonical coordinates of a saddle at board position \((s_x, s_y)\) relative to a tag with top-left corner \((t_x, t_y)\) are:
Because the saddle is at the outer corner of the enclosing square (not the marker), \(|u| > 1\) or \(|v| > 1\) — the homography is evaluated outside its training domain. This is a valid linear extrapolation for the projective model and correctly maps to the saddle's image location.
The predicted image coordinates are:
Structure-Tensor Gate (saddle iteration deliberately omitted)
Sub-pixel refinement of the homography-predicted saddle was tried (Gauss-Newton on the structure-tensor / gradient pair, Förstner direct-solve) and empirically falsified on this corpus — the Newton step is quenched by structure-tensor scaling (|step| ~ |∇I|/det(S) ~ 10⁻⁵ px, below f32 precision after the downstream cast), and applying Förstner regresses pose by ~4.5× on the regression_board_hub::board_charuco_v1_golden_forstner snapshot. The principled finding (see memory/project_refine_saddle_noop.md and memory/project_aprilgrid_saddle_refinement_negative.md) is that the homography projection of step (1) is already as good as it gets on this dataset.
What survives is a determinant-based rejection of degenerate windows. The structure tensor \(\mathbf{S}\) is accumulated over a \((2r+1) \times (2r+1)\) window using 3×3 Sobel kernels:
The Tikhonov-regularised determinant \(\det(\mathbf{S} + \mathbf{I})\) catches flat or low-conditioning neighborhoods (corners obscured by occlusion, motion blur, or grazing-angle foreshortening) before they reach the LO-RANSAC stage.
LO-RANSAC + AW-LM
Accepted saddles feed RobustPoseSolver with group_size=1 (each saddle is independent). The solver runs LO-RANSAC followed by anisotropically-weighted Levenberg-Marquardt using the inverse structure tensor as the per-point information matrix. Returns None if fewer than 4 saddles survive.
Memory model: all scratch buffers (img, obj, info, seeds, seen, touched) are pre-allocated in CharucoRefiner::new(). estimate() performs zero heap allocations.