Fix body frame jacobians

.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

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

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)

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

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

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

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

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)

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)

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)

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)