Add degeneracy detection and solution remapping in align() - #6
Open
AkshayHinduja wants to merge 13 commits into
Open
Add degeneracy detection and solution remapping in align()#6AkshayHinduja wants to merge 13 commits into
AkshayHinduja wants to merge 13 commits into
Conversation
The workflow installed point-cloud-registration from PyPI, so every CI run exercised the released wheel rather than the code under review. Install the checkout in editable mode instead. pykdtree is listed explicitly because the editable install resolves through the pyproject.toml poetry-core backend, whose dependency list does not include the KDTree backend the library uses at runtime.
The reference (educational) versions of calc_H_g_e2 mishandled rejected correspondences: - icp.py never masked idx, silently pairing masked source points with the wrong targets whenever a rejection occurred before them. - plane_icp.py, voxelized_plane_icp.py and ndt.py iterated over the unmasked source length while norms/means/src_trans had already been reduced, raising IndexError on any rejection. - The plane variants re-gated on the residual magnitude (a signed point-to-plane distance, not the correspondence distance the mask already applies) and ndt.py re-checked unmasked dist[i] against masked loop indices. Mask idx and source consistently, iterate the masked length, and drop the bogus secondary gates. The parity tests now cover rejection: each is parametrized with a with_outliers case that prepends far-away points, which previously crashed three references and silently corrupted the fourth.
The vectorized point-to-point path computed Rt_r = rs @ R.T, which is R @ r per row; the reference Jacobian J = [I | -R@skew(p)] needs R.T @ r, i.e. rs @ R. H was unaffected, but g[3:] was wrong at any non-identity pose, shifting the Gauss-Newton stationary point: on noise-free data with |w_true| = 0.5, align() settled ~0.3 rad from the true rotation. The parity tests are now parametrized over a rotated evaluation pose as well as identity, which catches this: before the fix only the ICP rotated-pose cases failed, confirming the other three solvers were already consistent with their references.
align() applies its Gauss-Newton step through the right-multiplicative retraction plus(T, dx) = T @ [expSO3(dx[3:]) | dx[:3]], so translation increments act in the body frame (t += R @ dt). Every solver built its translation Jacobian in the world frame instead (J_t = I for point-to-point/NDT, J_t = n rows for point-to-plane); the exact Jacobian for this retraction is J = [R | -R@skew(p)]. The stationary points were unchanged (the two gradients differ by an invertible map), but H mixed world-frame translation coordinates with body-frame rotation coordinates. Anything that interprets H's eigenstructure — degeneracy analysis, covariance extraction, per-direction step control — reads the wrong translation directions as soon as R differs from identity. Per solver: ICP g0 = R.T @ sum(r) and H_lr collapses to -skew(sum p) (the R factors cancel in H); PlaneICP/VPlaneICP translation rows become (R.T @ n); NDT H_ll = R.T @ (sum icov) @ R, H_lr and g0 gain the same R.T factor; every H_rr/g1/e2 is untouched. Reference implementations updated in lockstep. New tests/test_retraction_consistency.py holds every solver to a numerical oracle: with the solver's own correspondences frozen, a float64 central-difference Jacobian of the residual through plus() must reproduce both g = sum J'Wr and H = sum J'WJ — a check that fast-vs-reference parity can never provide, since both paths could share the same wrong convention. End-to-end recovery tests cover ICP at |w| = 0.5, PlaneICP from a rotated initial pose, and the voxel solvers on a mid-voxel textured corner.
For theta^2 <= 1e-5 the exponential map returned the first-order I + W, which is not a rotation: det(I + W) = 1 + O(theta^2), about 1.000009 at |omega| = 3e-3. Every small Gauss-Newton step applied through plus() bakes that scale/shear into the pose, and near convergence all steps are small. Use the second-order Taylor expansion of the Rodrigues coefficients instead (A = 1 - theta^2/6, B = 1/2 - theta^2/24), which agrees with the exact formula to ~3e-15 at the branch boundary and is orthonormal to machine precision.
align() bound cur_T = init_T, and when it converges before the first plus() call (source already aligned: the first solved step is below tol) it returned the caller's own array. With the mutable np.eye(4) default argument, a caller mutating the returned pose silently corrupts the default for every subsequent align() call in the process. Copy on entry instead.
Consumers of a registration result often need the observability of the solution — covariance extraction, degeneracy analysis, factor-graph weighting. Store the Gauss-Newton Hessian of the final align() iteration and expose it as a read-only last_hessian property. If the loop exhausts max_iter, cur_T has advanced past the last linearization, so the Hessian is recomputed at the returned pose rather than stored stale. last_hessian is expressed in the right-tangent (body) frame of the returned pose, DOF order [tx, ty, tz, wx, wy, wz] — the increment coordinates plus() consumes; the docstring spells out the congruence transform world-frame consumers need.
Add point_cloud_registration/degeneracy.py: eigendecompose a 6x6 registration Hessian, classify each eigen-direction as constrained or degenerate, and project the Gauss-Newton step so its component along every degenerate direction is zero (solution remapping). Classification is two-phase: eigenvalues below 1e-10 * lambda_max are structural zeros and always degenerate; the remaining directions are tested against the adaptive sqrt(lambda_max / lambda_min) threshold (with the scale-dependence of that published criterion documented, and an explicit lambda_threshold override for callers with a calibrated curvature floor). apply_sr_solve implements the V_f^-1 V_u projection and documents the raw-analysis/damped-solve pairing contract for exactly singular Hessians. All quantities are body-frame right-tangent, DOF order [tx, ty, tz, wx, wy, wz], matching math_tools.plus(). References: A. Hinduja, B.-J. Ho, M. Kaess, IROS 2019 (Algorithm 1, solution remapping inside the ICP iteration, sqrt threshold); J. Zhang, M. Kaess, S. Singh, ICRA 2016 (origin of the solution remapping update). NumPy only.
align() gains three opt-in kwargs, all defaulting to the previous behavior: - lm_damping: solve (H + lambda*I) dx = -g with lambda scaled to trace(H) (Levenberg-Marquardt), keeping the solve well posed on geometry that leaves H singular or near-singular; lstsq fallback if the solve still fails. On well-conditioned data the damping is small enough to leave the solution unchanged (pinned by test). - use_solution_remapping: at each iteration classify the Hessian's eigen-directions with analyse_hessian and zero the step along the degenerate ones with apply_sr_solve, holding the pose where the data says nothing (Hinduja, Ho & Kaess IROS 2019; Zhang, Kaess & Singh ICRA 2016). - sr_lambda_threshold: fixed eigenvalue floor overriding the adaptive SR threshold. The two compose deliberately: analysis runs on the raw H so the classification stays honest, while the solve uses the damped H — an isotropic shift leaves the eigenvectors identical. Because H, g and the step all live in the body frame of the retraction, SR holds the unobservable directions even from a rotated initial guess: the new regression aligns a flat plane from a 0.25 rad rolled init and asserts no world x/y/yaw motion (this leaked ~0.13 m before the body-frame Jacobian fix).
…s/cov_reg through VPlaneICP/NDT VoxelGrid gains cov_reg (default 0.0): an isotropic +cov_reg*I shift added to every per-voxel covariance before the min_points mask. Coplanar or collinear voxels otherwise produce singular covariances; NDT's calc_icov can only clamp the zero determinant, which silently collapses those voxels' information toward zero. The shift keeps them invertible without changing plane normals (eigenvectors are invariant under isotropic shifts). set_points now raises ValueError explicitly when the min_points mask empties the voxel set — previously that surfaced only incidentally from the pykdtree backend; the scipy backend built an empty tree silently. VPlaneICP and NDT constructors gain min_points=10, cov_reg=0.0 and thread them into the VoxelGrid built by set_target, so callers no longer need to rebuild .voxels by hand to tune voxel admission. Defaults preserve the previous behavior exactly. 12 new tests (regularization, defaults parity, raise-on-empty, threading, NDT icov contrast, end-to-end VPlaneICP convergence). Note: an upstream PR from this fork should be prepared as a squashed branch off 5cedcb3 (v1.0.5); main history stays additive.
Add data/generate_synthetic.py, a NumPy-only, fully deterministic
generator (fixed seeds, byte-identical regeneration) plus a minimal
ASCII PCD v0.7 writer/reader, and commit the four small clouds it
produces (~60-75 KB each):
- synthetic_staircase_{target,source}.pcd — treads and risers constrain
every DOF except cross-step translation: exactly one degenerate
direction (ty). The faces keep a 0.10 m margin at the concave folds;
without it k-NN PCA normals blend the two faces and the blended
y-components stop ty from being flagged.
- synthetic_plane_{target,source}.pcd — a featureless flat seafloor:
tx, ty and yaw are unobservable (three degenerate directions).
Target/source pairs are independent samplings of the same surface
(deterministic centering, never the empirical mean), so registration
sees realistic correspondence noise rather than a permutation of
identical points.
tests/test_synthetic_data.py pins the story with margin assertions
(flagged eigenvalue < 0.8x threshold, next eigenvalue > 5x) for both
the generators and the committed files as-loaded.
Original synthetic data, MIT-licensed with the repository.
Add demo_degeneracy.py: loads the synthetic staircase and flat-plane pairs from data/ (regenerating in memory when the files are absent), displaces the source scan by a known ground truth whose components along the degenerate directions are zero, and aligns it back with PlaneICP twice — plain (lm_damping) vs use_solution_remapping=True. Prints the DegeneracyResult and a per-DOF error table (works without matplotlib), and renders a 2x2 figure per case: the scans before alignment, a top-down view after alignment with a zoom on the drift, the Hessian eigenvalue spectrum against the SR threshold, and per-DOF |error| bars. Colors are Okabe-Ito colorblind-safe. Measured on the committed clouds: the staircase drifts 3.3 cm along the unobservable cross-step direction without SR vs 2.1 mm with it; the flat plane drifts 14 cm / 1.1 deg in tx/ty/yaw vs sub-millimetre. python3 demo_degeneracy.py --save writes imgs/degeneracy_*.png (committed, embedded in the README's new Degeneracy Detection & Solution Remapping section). matplotlib stays a demo-only dependency; the library remains NumPy-only.
The last_hessian docstring described blockdiag(R, I) @ H @ blockdiag(R, I).T as a world-frame Hessian; that expression only rotates the translation block while rotational increments stay body-frame, and a genuine left-tangent (world) Hessian also carries the translation-rotation coupling. State the per-direction mapping (R @ dt, R @ w) and point to the SE(3) adjoint congruence instead. Also drop the 'library is NumPy-only' phrasing from the demo and its README section: the runtime uses a KD-tree backend (pykdtree by default), so the honest claim is that the degeneracy feature and demo add no dependencies to the library.
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
1 participant
Add this suggestion to a batch that can be applied as a single commit.This suggestion is invalid because no changes were made to the code.Suggestions cannot be applied while the pull request is closed.Suggestions cannot be applied while viewing a subset of changes.Only one suggestion per line can be applied in a batch.Add this suggestion to a batch that can be applied as a single commit.Applying suggestions on deleted lines is not supported.You must change the existing code in this line in order to create a valid suggestion.Outdated suggestions cannot be applied.This suggestion has been applied or marked resolved.Suggestions cannot be applied from pending reviews.Suggestions cannot be applied on multi-line comments.Suggestions cannot be applied while the pull request is queued to merge.Suggestion cannot be applied right now. Please check back later.
Degeneracy detection and solution remapping in align()
Motivation
Common scenes—including featureless seafloors, straight corridors, and staircases—do not fully constrain registration. An unconstrained Gauss-Newton solve can then drift along unobservable directions in response to sampling noise.
This PR adds opt-in degeneracy detection and solution remapping to
align(), along with a reproducible demo and synthetic degenerate data. It relies on PR1's body-frame Jacobian correction: becauseH,g, and the update are expressed in the same retraction frame, the projection also holds from a rotated initial pose. Before that correction, it leaked approximately 0.13 m.What's added
point_cloud_registration/degeneracy.py(NumPy only):analyse_hessianeigendecomposes the 6×6 Gauss-Newton Hessian and labels each eigen-direction constrained or degenerate. Structural zero modes are always degenerate; remaining modes use a condition-number test, with a documentedlambda_thresholdoverride.apply_sr_solvethen projects the step so degenerate directions receive no update.align(use_solution_remapping=, sr_lambda_threshold=, lm_damping=): all parameters are opt-in and defaults preserve existing behavior exactly.lm_dampingkeeps rank-deficient solves well posed. Degeneracy is analysed on the rawH, while the solve uses the damped Hessian; isotropic damping preserves eigenvectors.Registration.last_hessian: exposes the Hessian at the returned pose, recomputing it ifmax_iteris exhausted. This supports downstream covariance and observability analysis. The documentation identifies it as a right-tangent (body-frame) quantity and gives the mapping required by world-frame consumers.VoxelGrid(min_points=, cov_reg=), threaded throughVPlaneICPandNDT: isotropic covariance regularization keeps coplanar voxels invertible without changing their normals.set_pointsnow raises explicitly whenmin_pointsremoves every voxel.Demo + data
demo_degeneracy.py(matplotlib is demo-only) compares unmodified alignment and solution remapping on a staircase (one degenerate DOF: cross-step translation) and flat plane (three: tx, ty, yaw). It includes eigenvalue-spectrum and per-DOF error panels. For headless use:python3 demo_degeneracy.py --save.data/generate_synthetic.pyand four committed.pcdclouds (~60–75 KB each): original MIT-licensed synthetic data, regenerated byte-for-byte from fixed seeds. Source and target are independently sampled from the same surface, introducing realistic correspondence noise.On the committed clouds, solution remapping reduces staircase cross-step drift from 3.3 cm to 2.1 mm, and plane error from 14 cm / 1.1° to sub-millimetre.
tests/test_synthetic_data.pylocks in the expected classification, DOF identities, and margins for both the generators and the committed files as loaded.Scope guarantees
align()behavior is opt-in; defaults unchanged (pinned by tests).References