Fix body frame jacobians - #5
Open
AkshayHinduja wants to merge 6 commits into
Open
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.
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.
Correctness fixes for Jacobians, ICP, expSO3, and reference implementations
Summary
Numerical validation of the registration math uncovered several correctness issues. This PR fixes them and adds regression coverage; it introduces no new features. A follow-up PR will build degeneracy handling on this corrected foundation.
Fixes
ICP fast-path rotational gradient (
icp.py): the vectorized path usedrs @ R.T(R @ rper row), while its Jacobian,J = [I | -R@skew(p)], requiresR.T @ r(rs @ R). The Hessian was unaffected, butg[3:]was wrong away from identity and moved the Gauss-Newton stationary point. On noise-free data at |ω| = 0.5,align()converged about 0.3 rad from the true rotation. Parity tests now include rotated poses.Retraction-consistent Jacobians in all four solvers:
plus()applies a right-multiplicative update (T @ [expSO3(w)|dt]), so translation increments are expressed in the body frame. The solvers instead used world-frame translation Jacobians (J_t = Ifor point-to-point/NDT andJ_t = nfor point-to-plane). The correct Jacobian isJ = [R | -R@skew(p)].This did not change stationary points, but it mixed world-frame translation and body-frame rotation in
H. That invalidates interpretations of its eigenstructure—such as covariance, observability/degeneracy, and direction-dependent step control—once the pose is rotated. The fast paths remain closed-form because theRfactors partially cancel (for example, ICPH_lr = -skew(Σp)).expSO3small-angle branch: for θ² ≤ 1e-5, it returned the first-order approximationI + W, which is not in SO(3) (det ≈ 1.000009at |ω| = 3 mrad). Near convergence, repeated small updates therefore introduced scale/shear. The branch now uses second-order Taylor expansions of the Rodrigues coefficients; it is orthonormal to machine precision and agrees with the exact formula to ~3e-15 at the branch boundary.Reference (
*_no_parallel_ver) implementations: rejected correspondences were handled incorrectly.icp.pyleftidxunmasked, silently misaligning pairs; the other three implementations looped over unmasked bounds and could raiseIndexError; and the plane variants gated twice on residual instead of correspondence distance. Since these implementations underpin parity tests, those tests were only trustworthy for all-inlier inputs. They now also cover outliers.align()aliasedinit_T: if convergence occurred before the first step,align()returned the caller's array. With the mutablenp.eye(4)default, mutating that result also corrupted the default for later calls.align()now copies the input on entry.New test infrastructure
tests/test_retraction_consistency.pyvalidates each solver against a numerical oracle. With correspondences frozen, a float64 central-difference Jacobian of the residual throughplus()must independently reproduceg = ΣJᵀWrandH = ΣJᵀWJ. This catches convention errors that fast-vs-reference parity cannot, because both implementations could share the same mistake.The suite also adds end-to-end recovery tests: ICP at |ω| = 0.5, PlaneICP from a rotated initialization, and voxel solvers on a textured corner.
CI
CI now runs
pip install -e . pykdtreerather than installing the released PyPI package, ensuring tests exercise this checkout.pykdtreeis installed explicitly because the poetry-core backend's dependency list omits it. Without this change, the new tests would run against the PyPI wheel.Test suite: 6 → 31 tests, all green.
Retraction convention
The solvers use the body-frame, right-tangent parametrization consumed by
math_tools.plus(), with DOF order[tx, ty, tz, ωx, ωy, ωz]. Solver docstrings now state this explicitly.