From cb29b59bdf288566225f8d439408ce8f9055d274 Mon Sep 17 00:00:00 2001
From: Akshay Hinduja <ahinduja@mbari.org>
Date: Tue, 11 Aug 2026 10:30:15 -0700
Subject: [PATCH 1/6] ci: test the checked-out package, not the PyPI release

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.
---
 .github/workflows/python-app.yml | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/.github/workflows/python-app.yml b/.github/workflows/python-app.yml
index 4bf2ff8..72e811a 100644
--- a/.github/workflows/python-app.yml
+++ b/.github/workflows/python-app.yml
@@ -27,7 +27,7 @@ jobs:
       run: |
         sudo apt-get install freeglut3-dev
         python -m pip install --upgrade pip
-        pip install point-cloud-registration
+        pip install -e . pykdtree
         pip install q3dviewer==1.1.6
         pip install pytest
     - name: Test with pytest

From 9fa5f34944a70e861019f47bf5598281809116ec Mon Sep 17 00:00:00 2001
From: Akshay Hinduja <ahinduja@mbari.org>
Date: Tue, 11 Aug 2026 10:35:46 -0700
Subject: [PATCH 2/6] fix: repair the no_parallel reference implementations
 (masking, gating)

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.
---
 point_cloud_registration/icp.py                 |  4 +++-
 point_cloud_registration/ndt.py                 |  8 +++-----
 point_cloud_registration/plane_icp.py           |  7 +++----
 point_cloud_registration/voxelized_plane_icp.py |  7 +++----
 tests/test_icp.py                               |  6 +++++-
 tests/test_ndt.py                               | 11 +++++++----
 tests/test_picp.py                              |  6 +++++-
 tests/test_vpicp.py                             | 11 +++++++----
 8 files changed, 36 insertions(+), 24 deletions(-)

diff --git a/point_cloud_registration/icp.py b/point_cloud_registration/icp.py
index c26c57e..d4a9a6b 100644
--- a/point_cloud_registration/icp.py
+++ b/point_cloud_registration/icp.py
@@ -65,7 +65,9 @@ def calc_H_g_e2_no_parallel_ver(self, cur_T, source):
         src_trans = transform_points(cur_T, source)
         dist, idx = self.kdtree.query(src_trans.astype(np.float32))
         mask = dist < self.max_dist
+        idx = idx[mask]
         src_trans = src_trans[mask]
+        src_mask = source[mask]
         num = src_trans.shape[0]
         # Find corresponding target points
         qs = self.target[idx]
@@ -78,7 +80,7 @@ def calc_H_g_e2_no_parallel_ver(self, cur_T, source):
             # Jacobian of the transformation
             J[:, :3] = np.eye(3)
             # Jacobian of the rotation
-            J[:, 3:] = -R @ skew(source[i])
+            J[:, 3:] = -R @ skew(src_mask[i])
             # residual
             r = src_trans[i] - qs[i]
             # Hessian
diff --git a/point_cloud_registration/ndt.py b/point_cloud_registration/ndt.py
index 181aa86..2b21a23 100644
--- a/point_cloud_registration/ndt.py
+++ b/point_cloud_registration/ndt.py
@@ -75,23 +75,21 @@ def calc_H_g_e2_no_parallel_ver(self, cur_T, source):
         mask = dist < self.max_dist
         means = means[mask]
         icov = icov[mask]
-        #src_mask = source[mask]
+        src_mask = source[mask]
         src_trans = src_trans[mask]
         H = np.zeros((6, 6))
         g = np.zeros(6)
         e2 = 0
 
-        for i in range(source.shape[0]):
+        for i in range(src_mask.shape[0]):
             J = np.zeros((3, 6))
             # Jacobian of the transformation
             J[:, :3] = np.eye(3)
             # Jacobian of the rotation
-            J[:, 3:] = -R @ skew(source[i])
+            J[:, 3:] = -R @ skew(src_mask[i])
             # residual
             r = src_trans[i] - means[i]
 
-            if dist[i] > self.max_dist:
-                continue
             H += J.T @ icov[i] @  J
             g += J.T @ icov[i] @ r
             e2 += r @ icov[i] @ r
diff --git a/point_cloud_registration/plane_icp.py b/point_cloud_registration/plane_icp.py
index 52c1b63..ecde47d 100644
--- a/point_cloud_registration/plane_icp.py
+++ b/point_cloud_registration/plane_icp.py
@@ -83,18 +83,17 @@ def calc_H_g_e2_no_parallel_ver(self, cur_T, source):
         means = self.target[idx]
         norms = self.normal[idx]
         src_trans = src_trans[mask]
+        src_mask = source[mask]
 
         H = np.zeros((6, 6))
         g = np.zeros(6)
         e2 = 0
-        for i in range(source.shape[0]):
+        for i in range(src_mask.shape[0]):
             n = norms[i]
             r = n @ (src_trans[i] - means[i])
             J = np.zeros((1, 6))
             J[0, :3] = n
-            J[0, 3:] = skew(source[i]) @ (R.T @ n.T)
-            if np.abs(r) > self.max_dist:
-                continue
+            J[0, 3:] = skew(src_mask[i]) @ (R.T @ n.T)
             H += J.T @ J
             g += J[0] * r
             e2 += r * r
diff --git a/point_cloud_registration/voxelized_plane_icp.py b/point_cloud_registration/voxelized_plane_icp.py
index 36c380a..bac474d 100644
--- a/point_cloud_registration/voxelized_plane_icp.py
+++ b/point_cloud_registration/voxelized_plane_icp.py
@@ -83,18 +83,17 @@ def calc_H_g_e2_no_parallel_ver(self, cur_T, source):
         means = query_data['mean'][mask]
         norms = query_data['norm'][mask]
         src_trans = src_trans[mask]
+        src_mask = source[mask]
 
         H = np.zeros((6, 6))
         g = np.zeros(6)
         e2 = 0
-        for i in range(source.shape[0]):
+        for i in range(src_mask.shape[0]):
             n = norms[i]
             r = n @ (src_trans[i] - means[i])
             J = np.zeros((1, 6))
             J[0, :3] = n
-            J[0, 3:] = skew(source[i]) @ (R.T @ n.T)
-            if np.abs(r) > self.max_dist:
-                continue
+            J[0, 3:] = skew(src_mask[i]) @ (R.T @ n.T)
             H += J.T @ J
             g += J[0] * r
             e2 += r * r
diff --git a/tests/test_icp.py b/tests/test_icp.py
index 2b12983..5c94206 100644
--- a/tests/test_icp.py
+++ b/tests/test_icp.py
@@ -17,11 +17,15 @@ def generate_test_data():
     return target, source
 
 
-def test_calc_H_g_e2(generate_test_data):
+@pytest.mark.parametrize("with_outliers", [False, True], ids=["all_inliers", "with_outliers"])
+def test_calc_H_g_e2(generate_test_data, with_outliers):
     """
     Test that calc_H_g_e2 and calc_H_g_e2_no_parallel_ver produce the same results.
     """
     target, source = generate_test_data
+    if with_outliers:
+        # Points far outside the target's reach: rejected by the max_dist gate.
+        source = np.vstack([source[:10] + 50.0, source])
     source = source.astype(np.float32)
     icp = ICP(max_iter=10, max_dist=2.0, tol=1e-3)
     icp.set_target(target)
diff --git a/tests/test_ndt.py b/tests/test_ndt.py
index f21c757..c0ce68a 100644
--- a/tests/test_ndt.py
+++ b/tests/test_ndt.py
@@ -19,15 +19,18 @@ def generate_test_data():
     return target, normals, source
 
 
-def test_calc_H_g_e2(generate_test_data):
+@pytest.mark.parametrize("with_outliers", [False, True], ids=["all_inliers", "with_outliers"])
+def test_calc_H_g_e2(generate_test_data, with_outliers):
     """
-    Test that calc_H_g_e2 and calc_H_g_e2x produce the same results.
+    Test that calc_H_g_e2 and calc_H_g_e2_no_parallel_ver produce the same results.
     """
-    target, normals, source = generate_test_data
+    target, _, source = generate_test_data
+    if with_outliers:
+        # Points far outside the target's reach: rejected by the max_dist gate.
+        source = np.vstack([source[:10] + 50.0, source])
     source = source.astype(np.float32)
     plane_icp = NDT(voxel_size=1.0, max_iter=10, max_dist=2.0, tol=1e-3)
     plane_icp.set_target(target)
-    plane_icp.normal = normals
 
     cur_T = np.eye(4)  # Initial transformation (identity matrix)
 
diff --git a/tests/test_picp.py b/tests/test_picp.py
index 7d90cf9..1cb357c 100644
--- a/tests/test_picp.py
+++ b/tests/test_picp.py
@@ -17,11 +17,15 @@ def generate_test_data():
     return target, source
 
 
-def test_calc_H_g_e2(generate_test_data):
+@pytest.mark.parametrize("with_outliers", [False, True], ids=["all_inliers", "with_outliers"])
+def test_calc_H_g_e2(generate_test_data, with_outliers):
     """
     Test that calc_H_g_e2 and calc_H_g_e2_no_parallel_ver produce the same results.
     """
     target, source = generate_test_data
+    if with_outliers:
+        # Points far outside the target's reach: rejected by the max_dist gate.
+        source = np.vstack([source[:10] + 50.0, source])
     source = source.astype(np.float32)
     vpicp = PlaneICP(max_iter=10, max_dist=2.0, tol=1e-3)
     vpicp.set_target(target)
diff --git a/tests/test_vpicp.py b/tests/test_vpicp.py
index 8ee86ef..82c59f6 100644
--- a/tests/test_vpicp.py
+++ b/tests/test_vpicp.py
@@ -19,15 +19,18 @@ def generate_test_data():
     return target, normals, source
 
 
-def test_calc_H_g_e2(generate_test_data):
+@pytest.mark.parametrize("with_outliers", [False, True], ids=["all_inliers", "with_outliers"])
+def test_calc_H_g_e2(generate_test_data, with_outliers):
     """
-    Test that calc_H_g_e2 and calc_H_g_e2x produce the same results.
+    Test that calc_H_g_e2 and calc_H_g_e2_no_parallel_ver produce the same results.
     """
-    target, normals, source = generate_test_data
+    target, _, source = generate_test_data
+    if with_outliers:
+        # Points far outside the target's reach: rejected by the max_dist gate.
+        source = np.vstack([source[:10] + 50.0, source])
     source = source.astype(np.float32)
     plane_icp = VPlaneICP(voxel_size=1.0, max_iter=10, max_dist=2.0, tol=1e-3)
     plane_icp.set_target(target)
-    plane_icp.normal = normals
 
     cur_T = np.eye(4)  # Initial transformation (identity matrix)
 

From cb29002aef00795bd4adbdb94df0fda3f47a79fe Mon Sep 17 00:00:00 2001
From: Akshay Hinduja <ahinduja@mbari.org>
Date: Tue, 11 Aug 2026 10:38:25 -0700
Subject: [PATCH 3/6] fix: ICP fast-path rotational gradient

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.
---
 point_cloud_registration/icp.py |  2 +-
 tests/test_icp.py               | 10 +++++++---
 tests/test_ndt.py               |  9 +++++++--
 tests/test_picp.py              | 10 +++++++---
 tests/test_vpicp.py             |  9 +++++++--
 5 files changed, 29 insertions(+), 11 deletions(-)

diff --git a/point_cloud_registration/icp.py b/point_cloud_registration/icp.py
index d4a9a6b..4336ff6 100644
--- a/point_cloud_registration/icp.py
+++ b/point_cloud_registration/icp.py
@@ -50,7 +50,7 @@ def calc_H_g_e2(self, cur_T, source):
         H[3:, :3] = H_lr.T
         H[3:, 3:] = H_rr
         g0 = rs.sum(axis=0)
-        Rt_r = rs @ R.T
+        Rt_r = rs @ R  # row i is R.T @ rs[i]: the reference J is [I | -R@skew(p)]
         g1 = np.einsum('nij,ni->j', S, -Rt_r)
         g = np.hstack([g0, g1])
         e2 = np.sum(rs * rs)
diff --git a/tests/test_icp.py b/tests/test_icp.py
index 5c94206..dd74eb5 100644
--- a/tests/test_icp.py
+++ b/tests/test_icp.py
@@ -1,7 +1,7 @@
 import numpy as np
 import pytest
 from point_cloud_registration.icp import ICP
-from point_cloud_registration.math_tools import expSO3
+from point_cloud_registration.math_tools import expSO3, makeT
 
 
 @pytest.fixture
@@ -17,8 +17,13 @@ def generate_test_data():
     return target, source
 
 
+@pytest.mark.parametrize(
+    "cur_T",
+    [np.eye(4), makeT(expSO3(np.array([0.3, -0.2, 0.4])), np.array([0.1, 0.2, -0.1]))],
+    ids=["identity_pose", "rotated_pose"],
+)
 @pytest.mark.parametrize("with_outliers", [False, True], ids=["all_inliers", "with_outliers"])
-def test_calc_H_g_e2(generate_test_data, with_outliers):
+def test_calc_H_g_e2(generate_test_data, cur_T, with_outliers):
     """
     Test that calc_H_g_e2 and calc_H_g_e2_no_parallel_ver produce the same results.
     """
@@ -30,7 +35,6 @@ def test_calc_H_g_e2(generate_test_data, with_outliers):
     icp = ICP(max_iter=10, max_dist=2.0, tol=1e-3)
     icp.set_target(target)
 
-    cur_T = np.eye(4)  # Initial transformation (identity matrix)
 
     # Compute results using both methods
     H1, g1, e2_1 = icp.calc_H_g_e2(cur_T, source)
diff --git a/tests/test_ndt.py b/tests/test_ndt.py
index c0ce68a..a834b16 100644
--- a/tests/test_ndt.py
+++ b/tests/test_ndt.py
@@ -2,6 +2,7 @@
 import pytest
 from point_cloud_registration import NDT
 from point_cloud_registration import expSO3
+from point_cloud_registration.math_tools import makeT
 
 
 @pytest.fixture
@@ -19,8 +20,13 @@ def generate_test_data():
     return target, normals, source
 
 
+@pytest.mark.parametrize(
+    "cur_T",
+    [np.eye(4), makeT(expSO3(np.array([0.3, -0.2, 0.4])), np.array([0.1, 0.2, -0.1]))],
+    ids=["identity_pose", "rotated_pose"],
+)
 @pytest.mark.parametrize("with_outliers", [False, True], ids=["all_inliers", "with_outliers"])
-def test_calc_H_g_e2(generate_test_data, with_outliers):
+def test_calc_H_g_e2(generate_test_data, cur_T, with_outliers):
     """
     Test that calc_H_g_e2 and calc_H_g_e2_no_parallel_ver produce the same results.
     """
@@ -32,7 +38,6 @@ def test_calc_H_g_e2(generate_test_data, with_outliers):
     plane_icp = NDT(voxel_size=1.0, max_iter=10, max_dist=2.0, tol=1e-3)
     plane_icp.set_target(target)
 
-    cur_T = np.eye(4)  # Initial transformation (identity matrix)
 
     # Compute results using both methods
     H1, g1, e2_1 = plane_icp.calc_H_g_e2(cur_T, source)
diff --git a/tests/test_picp.py b/tests/test_picp.py
index 1cb357c..1bc3987 100644
--- a/tests/test_picp.py
+++ b/tests/test_picp.py
@@ -1,7 +1,7 @@
 import numpy as np
 import pytest
 from point_cloud_registration.plane_icp import PlaneICP
-from point_cloud_registration.math_tools import expSO3
+from point_cloud_registration.math_tools import expSO3, makeT
 
 
 @pytest.fixture
@@ -17,8 +17,13 @@ def generate_test_data():
     return target, source
 
 
+@pytest.mark.parametrize(
+    "cur_T",
+    [np.eye(4), makeT(expSO3(np.array([0.3, -0.2, 0.4])), np.array([0.1, 0.2, -0.1]))],
+    ids=["identity_pose", "rotated_pose"],
+)
 @pytest.mark.parametrize("with_outliers", [False, True], ids=["all_inliers", "with_outliers"])
-def test_calc_H_g_e2(generate_test_data, with_outliers):
+def test_calc_H_g_e2(generate_test_data, cur_T, with_outliers):
     """
     Test that calc_H_g_e2 and calc_H_g_e2_no_parallel_ver produce the same results.
     """
@@ -30,7 +35,6 @@ def test_calc_H_g_e2(generate_test_data, with_outliers):
     vpicp = PlaneICP(max_iter=10, max_dist=2.0, tol=1e-3)
     vpicp.set_target(target)
 
-    cur_T = np.eye(4)  # Initial transformation (identity matrix)
 
     # Compute results using both methods
     H1, g1, e2_1 = vpicp.calc_H_g_e2(cur_T, source)
diff --git a/tests/test_vpicp.py b/tests/test_vpicp.py
index 82c59f6..6d9d644 100644
--- a/tests/test_vpicp.py
+++ b/tests/test_vpicp.py
@@ -2,6 +2,7 @@
 import pytest
 from point_cloud_registration import VPlaneICP
 from point_cloud_registration import expSO3
+from point_cloud_registration.math_tools import makeT
 
 
 @pytest.fixture
@@ -19,8 +20,13 @@ def generate_test_data():
     return target, normals, source
 
 
+@pytest.mark.parametrize(
+    "cur_T",
+    [np.eye(4), makeT(expSO3(np.array([0.3, -0.2, 0.4])), np.array([0.1, 0.2, -0.1]))],
+    ids=["identity_pose", "rotated_pose"],
+)
 @pytest.mark.parametrize("with_outliers", [False, True], ids=["all_inliers", "with_outliers"])
-def test_calc_H_g_e2(generate_test_data, with_outliers):
+def test_calc_H_g_e2(generate_test_data, cur_T, with_outliers):
     """
     Test that calc_H_g_e2 and calc_H_g_e2_no_parallel_ver produce the same results.
     """
@@ -32,7 +38,6 @@ def test_calc_H_g_e2(generate_test_data, with_outliers):
     plane_icp = VPlaneICP(voxel_size=1.0, max_iter=10, max_dist=2.0, tol=1e-3)
     plane_icp.set_target(target)
 
-    cur_T = np.eye(4)  # Initial transformation (identity matrix)
 
     # Compute results using both methods
     H1, g1, e2_1 = plane_icp.calc_H_g_e2(cur_T, source)

From 0f7c7b5c9108bed432f9b43975b4261b57bc5c3f Mon Sep 17 00:00:00 2001
From: Akshay Hinduja <ahinduja@mbari.org>
Date: Tue, 11 Aug 2026 10:47:46 -0700
Subject: [PATCH 4/6] fix: retraction-consistent (body-frame) Jacobians in all
 solvers
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

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.
---
 point_cloud_registration/icp.py               |  13 +-
 point_cloud_registration/ndt.py               |  12 +-
 point_cloud_registration/plane_icp.py         |   9 +-
 .../voxelized_plane_icp.py                    |   9 +-
 tests/test_retraction_consistency.py          | 221 ++++++++++++++++++
 5 files changed, 248 insertions(+), 16 deletions(-)
 create mode 100644 tests/test_retraction_consistency.py

diff --git a/point_cloud_registration/icp.py b/point_cloud_registration/icp.py
index 4336ff6..cde97ac 100644
--- a/point_cloud_registration/icp.py
+++ b/point_cloud_registration/icp.py
@@ -41,16 +41,19 @@ def calc_H_g_e2(self, cur_T, source):
         R = cur_T[:3, :3]
         S = skews(src_mask)
         S_sum = skew(np.sum(src_mask, axis=0))
+        # J = [R | -R@skew(p)]: the tangent-space increment dx is applied by
+        # plus() as T @ [expSO3(dx[3:]) | dx[:3]], so both blocks carry R.
+        # In H the R factors cancel (R.T@R = I), leaving these closed forms.
         H_ll = num * np.eye(3)
-        H_lr = - R @ S_sum
+        H_lr = - S_sum
         H_rr = skew2(src_mask)
         H = np.zeros((6, 6))
         H[:3, :3] = H_ll
         H[:3, 3:] = H_lr
         H[3:, :3] = H_lr.T
         H[3:, 3:] = H_rr
-        g0 = rs.sum(axis=0)
-        Rt_r = rs @ R  # row i is R.T @ rs[i]: the reference J is [I | -R@skew(p)]
+        Rt_r = rs @ R  # row i is R.T @ rs[i]
+        g0 = Rt_r.sum(axis=0)
         g1 = np.einsum('nij,ni->j', S, -Rt_r)
         g = np.hstack([g0, g1])
         e2 = np.sum(rs * rs)
@@ -77,8 +80,8 @@ def calc_H_g_e2_no_parallel_ver(self, cur_T, source):
         e2 = 0
         for i in range(num):
             J = np.zeros((3, 6))
-            # Jacobian of the transformation
-            J[:, :3] = np.eye(3)
+            # Jacobian of the translation (body-frame increment: t += R @ dt)
+            J[:, :3] = R
             # Jacobian of the rotation
             J[:, 3:] = -R @ skew(src_mask[i])
             # residual
diff --git a/point_cloud_registration/ndt.py b/point_cloud_registration/ndt.py
index 2b21a23..c5af15d 100644
--- a/point_cloud_registration/ndt.py
+++ b/point_cloud_registration/ndt.py
@@ -37,10 +37,12 @@ def calc_H_g_e2(self, cur_T, source):
         src_trans = src_trans[mask]
 
         diff = src_trans - means  # shape: (N, 3)
+        # The tangent-space increment dx is applied by plus() as
+        # T @ [expSO3(dx[3:]) | dx[:3]], so J0 = R (not I) and J1 = -R@skew(p).
         J1 = -R @ skews(src_mask)
         icov_J1 = np.einsum('nij,njk->nik', icov, J1)
-        H_ll = np.sum(icov, axis=0) # sum (J0.T * icov * J0)
-        H_lr = np.sum(icov_J1, axis=0)
+        H_ll = R.T @ np.sum(icov, axis=0) @ R # sum (J0.T * icov * J0)
+        H_lr = R.T @ np.sum(icov_J1, axis=0)
         H_rr = np.einsum('nji,njk->ik', J1, icov_J1)
 
         H = np.zeros((6, 6))
@@ -50,7 +52,7 @@ def calc_H_g_e2(self, cur_T, source):
         H[3:, 3:] = H_rr
 
         icov_r = np.einsum('nij,nj->ni', icov, diff)
-        g0 = np.sum(icov_r, axis=0)  # J0.T * icov * diff
+        g0 = R.T @ np.sum(icov_r, axis=0)  # J0.T * icov * diff
         g1 = np.einsum('nji,nj->i', J1, icov_r) # J1.T * icov * diff
         g = np.hstack([g0, g1])  # shape: (6,)
         e2 = np.einsum('ni,ni->', diff, icov_r)  # r.T * icov * r
@@ -83,8 +85,8 @@ def calc_H_g_e2_no_parallel_ver(self, cur_T, source):
 
         for i in range(src_mask.shape[0]):
             J = np.zeros((3, 6))
-            # Jacobian of the transformation
-            J[:, :3] = np.eye(3)
+            # Jacobian of the translation (body-frame increment: t += R @ dt)
+            J[:, :3] = R
             # Jacobian of the rotation
             J[:, 3:] = -R @ skew(src_mask[i])
             # residual
diff --git a/point_cloud_registration/plane_icp.py b/point_cloud_registration/plane_icp.py
index ecde47d..62bbb46 100644
--- a/point_cloud_registration/plane_icp.py
+++ b/point_cloud_registration/plane_icp.py
@@ -47,10 +47,13 @@ def calc_H_g_e2(self, cur_T, source):
         diff = src_trans - means
         src_mask = source[mask]
         rs = np.einsum('ij,ij->i', norms, diff)
-        Jt = norms
         Rt_norms = R.T @ norms.T
+        # The tangent-space increment dx is applied by plus() as
+        # T @ [expSO3(dx[3:]) | dx[:3]], so the residual row is
+        # n.T @ [R | -R@skew(p)] = [(R.T@n).T | (skew(p)@R.T@n).T].
+        Jt = Rt_norms.T
         # # equal to skew_time_vector
-        # Jr = np.einsum('ijk,ki->ij', skews(src_mask), Rt_norms) 
+        # Jr = np.einsum('ijk,ki->ij', skews(src_mask), Rt_norms)
         Jr = skew_time_vector(src_mask, Rt_norms.T)
         H_ll = np.einsum('ij,ik->jk', Jt, Jt)
         H_lr = np.einsum('ij,ik->jk', Jt, Jr)
@@ -92,7 +95,7 @@ def calc_H_g_e2_no_parallel_ver(self, cur_T, source):
             n = norms[i]
             r = n @ (src_trans[i] - means[i])
             J = np.zeros((1, 6))
-            J[0, :3] = n
+            J[0, :3] = R.T @ n
             J[0, 3:] = skew(src_mask[i]) @ (R.T @ n.T)
             H += J.T @ J
             g += J[0] * r
diff --git a/point_cloud_registration/voxelized_plane_icp.py b/point_cloud_registration/voxelized_plane_icp.py
index bac474d..e4b7b4f 100644
--- a/point_cloud_registration/voxelized_plane_icp.py
+++ b/point_cloud_registration/voxelized_plane_icp.py
@@ -42,10 +42,13 @@ def calc_H_g_e2(self, cur_T, source):
         diff = src_trans - means
         src_mask = source[mask]
         rs = np.einsum('ij,ij->i', norms, diff)
-        Jt = norms
         Rt_norms = R.T @ norms.T
+        # The tangent-space increment dx is applied by plus() as
+        # T @ [expSO3(dx[3:]) | dx[:3]], so the residual row is
+        # n.T @ [R | -R@skew(p)] = [(R.T@n).T | (skew(p)@R.T@n).T].
+        Jt = Rt_norms.T
         # # equal to skew_time_vector
-        # Jr = np.einsum('ijk,ki->ij', skews(src_mask), Rt_norms) 
+        # Jr = np.einsum('ijk,ki->ij', skews(src_mask), Rt_norms)
         Jr = skew_time_vector(src_mask, Rt_norms.T)
         H_ll = np.einsum('ij,ik->jk', Jt, Jt)
         H_lr = np.einsum('ij,ik->jk', Jt, Jr)
@@ -92,7 +95,7 @@ def calc_H_g_e2_no_parallel_ver(self, cur_T, source):
             n = norms[i]
             r = n @ (src_trans[i] - means[i])
             J = np.zeros((1, 6))
-            J[0, :3] = n
+            J[0, :3] = R.T @ n
             J[0, 3:] = skew(src_mask[i]) @ (R.T @ n.T)
             H += J.T @ J
             g += J[0] * r
diff --git a/tests/test_retraction_consistency.py b/tests/test_retraction_consistency.py
new file mode 100644
index 0000000..7dd6643
--- /dev/null
+++ b/tests/test_retraction_consistency.py
@@ -0,0 +1,221 @@
+"""
+Retraction-consistency oracle for every solver's calc_H_g_e2.
+
+align() applies its Gauss-Newton step through the right-multiplicative
+retraction plus(T, dx) = T @ [expSO3(dx[3:]) | dx[:3]], so the (H, g)
+returned by calc_H_g_e2 must be the Gauss-Newton pair of the solver's
+own cost with respect to THAT increment (a body-frame right-tangent
+6-vector, DOF order [tx, ty, tz, wx, wy, wz]).
+
+The oracle freezes the correspondences the solver selected at cur_T
+(Gauss-Newton linearizes with correspondences held fixed), rebuilds the
+residual model in float64, differentiates each residual through plus()
+by central differences, and checks
+
+    g == sum_i J_i.T @ W_i @ r_i        H == sum_i J_i.T @ W_i @ J_i
+
+independently.  This catches frame errors that fast-vs-reference parity
+tests cannot see, because those compare two implementations that could
+share the same wrong convention.
+"""
+import numpy as np
+import pytest
+
+from point_cloud_registration.icp import ICP
+from point_cloud_registration.plane_icp import PlaneICP
+from point_cloud_registration.voxelized_plane_icp import VPlaneICP
+from point_cloud_registration.ndt import NDT
+from point_cloud_registration.math_tools import expSO3, makeT, plus, transform_points
+
+H_STEP = 3e-3
+CUR_T = makeT(expSO3(np.array([0.3, -0.2, 0.4])), np.array([0.1, 0.2, -0.1]))
+
+
+def _numeric_J(residual_fn, dim):
+    """Central-difference Jacobian of residual_fn: R^6 -> R^(N,dim)."""
+    r0 = residual_fn(np.zeros(6))
+    J = np.zeros((r0.shape[0], dim, 6))
+    for k in range(6):
+        dx = np.zeros(6)
+        dx[k] = H_STEP
+        rp = residual_fn(dx)
+        rm = residual_fn(-dx)
+        J[:, :, k] = (rp - rm) / (2.0 * H_STEP)
+    return r0, J
+
+
+def _assert_H_g_match(H, g, r0, J, W=None):
+    """Compare solver (H, g) with the numeric Gauss-Newton pair."""
+    if W is None:
+        Wr = r0
+        WJ = J
+    else:
+        Wr = np.einsum('nij,nj->ni', W, r0)
+        WJ = np.einsum('nij,njk->nik', W, J)
+    g_num = np.einsum('nik,ni->k', J, Wr)
+    H_num = np.einsum('nik,nil->kl', J, WJ)
+    g_scale = max(np.max(np.abs(g_num)), 1.0)
+    H_scale = max(np.max(np.abs(H_num)), 1.0)
+    assert np.allclose(g, g_num, atol=2e-3 * g_scale), (
+        f"g mismatch:\nsolver {g}\nnumeric {g_num}")
+    assert np.allclose(H, H_num, atol=2e-3 * H_scale), (
+        f"H mismatch: max |dH| = {np.max(np.abs(H - H_num))}")
+
+
+@pytest.fixture
+def cloud_pair():
+    np.random.seed(42)
+    target = np.random.rand(500, 3) * 4.0
+    R = expSO3(np.array([0.1, 0.2, 0.3]))
+    t = np.array([0.5, -0.3, 0.2])
+    source = ((R @ target.T).T + t).astype(np.float32)
+    return target, source
+
+
+def test_icp_matches_numeric_gauss_newton(cloud_pair):
+    target, source = cloud_pair
+    icp = ICP(max_iter=10, max_dist=2.0)
+    icp.set_target(target)
+    H, g, _ = icp.calc_H_g_e2(CUR_T, source)
+
+    # Freeze the correspondences exactly as the solver selected them.
+    src_trans = transform_points(CUR_T.astype(np.float32), source)
+    dist, idx = icp.kdtree.query(src_trans)
+    mask = dist < icp.max_dist
+    p = source[mask].astype(np.float64)
+    q = target[idx[mask]].astype(np.float64)
+
+    def residual(dx):
+        T = plus(CUR_T, dx)
+        return transform_points(T, p) - q
+
+    r0, J = _numeric_J(residual, 3)
+    _assert_H_g_match(H, g, r0, J)
+
+
+def test_plane_icp_matches_numeric_gauss_newton(cloud_pair):
+    target, source = cloud_pair
+    picp = PlaneICP(max_iter=10, max_dist=2.0, k=10)
+    picp.set_target(target)
+    H, g, _ = picp.calc_H_g_e2(CUR_T, source)
+
+    src_trans = transform_points(CUR_T.astype(np.float32), source)
+    dist, idx = picp.kdtree.query(src_trans)
+    mask = dist < picp.max_dist
+    p = source[mask].astype(np.float64)
+    m = picp.target[idx[mask]].astype(np.float64)
+    n = picp.normal[idx[mask]].astype(np.float64)
+
+    def residual(dx):
+        T = plus(CUR_T, dx)
+        return np.einsum('ij,ij->i', n, transform_points(T, p) - m)[:, None]
+
+    r0, J = _numeric_J(residual, 1)
+    _assert_H_g_match(H, g, r0, J)
+
+
+def test_vplane_icp_matches_numeric_gauss_newton(cloud_pair):
+    target, source = cloud_pair
+    vpicp = VPlaneICP(voxel_size=1.0, max_iter=10, max_dist=2.0)
+    vpicp.set_target(target)
+    H, g, _ = vpicp.calc_H_g_e2(CUR_T, source)
+
+    src_trans = transform_points(CUR_T.astype(np.float32), source)
+    query = vpicp.voxels.query(src_trans, ['mean', 'norm'])
+    mask = query['dist'] < vpicp.max_dist
+    p = source[mask].astype(np.float64)
+    m = query['mean'][mask].astype(np.float64)
+    n = query['norm'][mask].astype(np.float64)
+
+    def residual(dx):
+        T = plus(CUR_T, dx)
+        return np.einsum('ij,ij->i', n, transform_points(T, p) - m)[:, None]
+
+    r0, J = _numeric_J(residual, 1)
+    _assert_H_g_match(H, g, r0, J)
+
+
+def test_ndt_matches_numeric_gauss_newton(cloud_pair):
+    target, source = cloud_pair
+    ndt = NDT(voxel_size=1.0, max_iter=10, max_dist=2.0)
+    ndt.set_target(target)
+    H, g, _ = ndt.calc_H_g_e2(CUR_T, source)
+
+    src_trans = transform_points(CUR_T.astype(np.float32), source)
+    query = ndt.voxels.query(src_trans, ['icov', 'mean'])
+    mask = query['dist'] < ndt.max_dist
+    p = source[mask].astype(np.float64)
+    m = query['mean'][mask].astype(np.float64)
+    W = query['icov'][mask].astype(np.float64)
+
+    def residual(dx):
+        T = plus(CUR_T, dx)
+        return transform_points(T, p) - m
+
+    r0, J = _numeric_J(residual, 3)
+    _assert_H_g_match(H, g, r0, J, W=W)
+
+
+class TestAlignRecovery:
+    """End-to-end: align() must reach the true pose, not just a stationary one."""
+
+    def test_icp_recovers_large_rotation(self):
+        np.random.seed(7)
+        target = np.random.rand(200, 3) * 2.0
+        T_true = makeT(expSO3(np.array([0.5, 0.0, 0.0])),
+                       np.array([0.3, -0.2, 0.1]))
+        source = transform_points(np.linalg.inv(T_true), target)
+        icp = ICP(max_iter=100, max_dist=5.0, tol=1e-9)
+        icp.set_target(target)
+        T = icp.align(source.astype(np.float32))
+        assert np.allclose(T, T_true, atol=1e-3), (
+            f"pose error {np.max(np.abs(T - T_true))}")
+
+    def test_plane_icp_recovers_from_rotated_init(self):
+        rng = np.random.default_rng(0)
+        pts = []
+        for _ in range(3):
+            u = rng.uniform(0.0, 4.0, (400, 2))
+            pts.append(np.column_stack([u[:, 0], u[:, 1], np.zeros(400)]))
+        corner = np.vstack([
+            pts[0],
+            pts[1][:, [0, 2, 1]],
+            pts[2][:, [2, 0, 1]],
+        ])
+        T_true = makeT(expSO3(np.array([0.02, -0.03, 0.05])),
+                       np.array([0.15, -0.1, 0.08]))
+        source = transform_points(np.linalg.inv(T_true), corner)
+        picp = PlaneICP(max_iter=60, max_dist=1.0, tol=1e-8, k=10)
+        picp.set_target(corner)
+        init_T = makeT(expSO3(np.array([0.1, -0.05, 0.08])), np.zeros(3))
+        T = picp.align(source.astype(np.float32), init_T=init_T)
+        assert np.allclose(T, T_true, atol=1e-3), (
+            f"pose error {np.max(np.abs(T - T_true))}")
+
+    @pytest.mark.parametrize("engine_cls", [VPlaneICP, NDT])
+    def test_voxel_solvers_reduce_pose_error(self, engine_cls):
+        # Three gently textured faces of a corner, offset by +0.5 so the
+        # surfaces sit mid-voxel: faces lying exactly on voxel boundaries
+        # give voxels that straddle two faces and blend their normals,
+        # which biases the voxelized cost minimum away from the true pose.
+        rng = np.random.default_rng(3)
+        faces = []
+        for axes in ((0, 1, 2), (0, 2, 1), (2, 0, 1)):
+            u = rng.uniform(0.0, 4.0, (3000, 2))
+            face = np.zeros((3000, 3))
+            face[:, axes[0]] = u[:, 0]
+            face[:, axes[1]] = u[:, 1]
+            face[:, axes[2]] = 0.05 * np.sin(u[:, 0]) * np.cos(u[:, 1])
+            faces.append(face)
+        target = np.vstack(faces) + 0.5
+        T_true = makeT(expSO3(np.array([0.03, -0.02, 0.04])),
+                       np.array([0.2, -0.15, 0.1]))
+        source = transform_points(np.linalg.inv(T_true), target)
+        engine = engine_cls(voxel_size=1.0, max_iter=60, max_dist=2.0, tol=1e-8)
+        engine.set_target(target)
+        T = engine.align(source.astype(np.float32))
+        err_before = np.linalg.norm(np.eye(4) - T_true)
+        err_after = np.linalg.norm(T - T_true)
+        assert err_after < err_before / 10.0, (
+            f"before {err_before}, after {err_after}")
+        assert np.allclose(T[:3, 3], T_true[:3, 3], atol=3e-2)

From fed2f13106e5a0b248b68cceca3df9b8f156ea2f Mon Sep 17 00:00:00 2001
From: Akshay Hinduja <ahinduja@mbari.org>
Date: Tue, 11 Aug 2026 10:49:39 -0700
Subject: [PATCH 5/6] fix: second-order small-angle branch in expSO3

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.
---
 point_cloud_registration/math_tools.py |  8 ++++-
 tests/test_math_tools.py               | 43 ++++++++++++++++++++++++++
 2 files changed, 50 insertions(+), 1 deletion(-)
 create mode 100644 tests/test_math_tools.py

diff --git a/point_cloud_registration/math_tools.py b/point_cloud_registration/math_tools.py
index a1bba26..59e08e5 100644
--- a/point_cloud_registration/math_tools.py
+++ b/point_cloud_registration/math_tools.py
@@ -88,7 +88,13 @@ def expSO3(omega):
     nearZero = theta2 <= epsilon
     W = skew(omega)
     if (nearZero):
-        return np.eye(3) + W
+        # Second-order Taylor of sin(theta)/theta and (1-cos(theta))/theta^2:
+        # the first-order I + W is not in SO(3) (det = 1 + O(theta^2)), and
+        # repeated small steps applied through plus() accumulate that
+        # scale/shear error into the pose.
+        A = 1.0 - theta2 / 6.0
+        B = 0.5 - theta2 / 24.0
+        return np.eye(3) + A * W + B * W.dot(W)
     else:
         K = W/theta
         KK = K.dot(K)
diff --git a/tests/test_math_tools.py b/tests/test_math_tools.py
new file mode 100644
index 0000000..d3f7d38
--- /dev/null
+++ b/tests/test_math_tools.py
@@ -0,0 +1,43 @@
+"""
+expSO3 must return a member of SO(3) in both of its branches.
+
+The small-angle branch used to return the first-order I + W, which is
+not orthonormal (det = 1 + O(theta^2)): at |omega| = 3e-3 the
+determinant error is ~9e-6, and every Gauss-Newton step taken through
+plus() bakes that scale/shear into the pose estimate.
+"""
+import numpy as np
+
+from point_cloud_registration.math_tools import expSO3, skew
+
+
+def _rodrigues(omega):
+    """Exact Rodrigues formula, valid for any nonzero angle (float64)."""
+    theta = np.linalg.norm(omega)
+    K = skew(omega) / theta
+    return np.eye(3) + np.sin(theta) * K + (1.0 - np.cos(theta)) * (K @ K)
+
+
+def _assert_in_SO3(R, atol):
+    np.testing.assert_allclose(R.T @ R, np.eye(3), atol=atol)
+    assert abs(np.linalg.det(R) - 1.0) < atol
+
+
+def test_small_angle_branch_is_orthonormal():
+    # theta^2 = 9e-6 <= epsilon = 1e-5: exercises the near-zero branch.
+    R = expSO3(np.array([3e-3, 0.0, 0.0]))
+    _assert_in_SO3(R, atol=1e-12)
+
+
+def test_small_angle_branch_matches_rodrigues():
+    omega = np.array([1.5e-3, -2e-3, 1e-3])
+    np.testing.assert_allclose(expSO3(omega), _rodrigues(omega), atol=1e-12)
+
+
+def test_large_angle_branch_is_orthonormal():
+    R = expSO3(np.array([0.5, -0.3, 0.2]))
+    _assert_in_SO3(R, atol=1e-12)
+
+
+def test_zero_rotation_is_identity():
+    np.testing.assert_allclose(expSO3(np.zeros(3)), np.eye(3), atol=1e-15)

From ffd208802138e5760c0ad4b5de408be0728349b0 Mon Sep 17 00:00:00 2001
From: Akshay Hinduja <ahinduja@mbari.org>
Date: Tue, 11 Aug 2026 10:51:21 -0700
Subject: [PATCH 6/6] fix: align() must not alias init_T

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.
---
 point_cloud_registration/registration.py |  4 ++-
 tests/test_registration_align.py         | 33 ++++++++++++++++++++++++
 2 files changed, 36 insertions(+), 1 deletion(-)
 create mode 100644 tests/test_registration_align.py

diff --git a/point_cloud_registration/registration.py b/point_cloud_registration/registration.py
index f46dde0..4183b73 100644
--- a/point_cloud_registration/registration.py
+++ b/point_cloud_registration/registration.py
@@ -81,7 +81,9 @@ def align(self, source, init_T=np.eye(4), verbose=False):
             raise ValueError("Target is not set.")
 
         source = source.astype(np.float32)
-        cur_T = init_T
+        # Copy: align() must not return the caller's array (or the shared
+        # mutable np.eye(4) default) when it converges before the first step.
+        cur_T = init_T.copy()
         # dx_norm = np.inf
         # best_T = cur_T
         # best_error = np.inf
diff --git a/tests/test_registration_align.py b/tests/test_registration_align.py
new file mode 100644
index 0000000..6d3001c
--- /dev/null
+++ b/tests/test_registration_align.py
@@ -0,0 +1,33 @@
+"""
+Behavioral tests for Registration.align().
+"""
+import numpy as np
+
+from point_cloud_registration.icp import ICP
+
+
+def test_align_does_not_alias_init_T():
+    """
+    align() must return a transform the caller owns.
+
+    With source == target the very first step is ~zero, so align()
+    converges before ever calling plus(): without the defensive copy it
+    returns the init_T object itself — and with the mutable np.eye(4)
+    default, a caller mutating the result silently corrupts the default
+    for every subsequent align() call in the process.
+    """
+    np.random.seed(1)
+    target = np.random.rand(100, 3)
+    icp = ICP(max_iter=10, max_dist=2.0, tol=1e-3)
+    icp.set_target(target)
+    source = target.astype(np.float32)
+
+    init_T = np.eye(4)
+    T = icp.align(source, init_T=init_T)
+    assert T is not init_T
+
+    # Mutating the result must not corrupt the shared default argument.
+    T_default = icp.align(source)
+    T_default[0, 3] = 123.0
+    T_again = icp.align(source)
+    np.testing.assert_allclose(T_again, np.eye(4), atol=1e-6)