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
diff --git a/point_cloud_registration/icp.py b/point_cloud_registration/icp.py
index c26c57e..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.T
+        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)
@@ -65,7 +68,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]
@@ -75,10 +80,10 @@ 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(source[i])
+            J[:, 3:] = -R @ skew(src_mask[i])
             # residual
             r = src_trans[i] - qs[i]
             # Hessian
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/point_cloud_registration/ndt.py b/point_cloud_registration/ndt.py
index 181aa86..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
@@ -75,23 +77,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 translation (body-frame increment: t += R @ dt)
+            J[:, :3] = R
             # 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..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)
@@ -83,18 +86,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] = R.T @ n
+            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/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/point_cloud_registration/voxelized_plane_icp.py b/point_cloud_registration/voxelized_plane_icp.py
index 36c380a..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)
@@ -83,18 +86,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] = R.T @ n
+            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..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,16 +17,24 @@ def generate_test_data():
     return target, source
 
 
-def test_calc_H_g_e2(generate_test_data):
+@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, cur_T, 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)
 
-    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_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)
diff --git a/tests/test_ndt.py b/tests/test_ndt.py
index f21c757..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,17 +20,24 @@ def generate_test_data():
     return target, normals, source
 
 
-def test_calc_H_g_e2(generate_test_data):
+@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, cur_T, 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)
 
     # 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 7d90cf9..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,16 +17,24 @@ def generate_test_data():
     return target, source
 
 
-def test_calc_H_g_e2(generate_test_data):
+@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, cur_T, 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)
 
-    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_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)
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)
diff --git a/tests/test_vpicp.py b/tests/test_vpicp.py
index 8ee86ef..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,17 +20,24 @@ def generate_test_data():
     return target, normals, source
 
 
-def test_calc_H_g_e2(generate_test_data):
+@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, cur_T, 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)
 
     # Compute results using both methods
     H1, g1, e2_1 = plane_icp.calc_H_g_e2(cur_T, source)