Fix bug in SE(3) pose composition Jacobians (#51)

* Fix bug in SE(3) pose composition Jacobians * Another fix
JeffLIrion · Nov 6, 2023 · c7b39cf · c7b39cf
1 parent ffde16f
commit c7b39cf
Show file tree

Hide file tree

Showing 2 changed files with 4 additions and 4 deletions.
diff --git a/graphslam/pose/se3.py b/graphslam/pose/se3.py
@@ -250,7 +250,7 @@ def jacobian_self_oplus_other_wrt_self(self, other):
         """
         # fmt: off
         return np.array([[1., 0., 0., 2. * self[4] * other[1] + 2. * self[5] * other[2], -4. * self[4] * other[0] + 2. * self[3] * other[1] + 2. * self[6] * other[2], -4. * self[5] * other[0] - 2. * self[6] * other[1] + 2. * self[3] * other[2], -2. * self[5] * other[1] + 2. * self[4] * other[2]],
-                         [0., 1., 0., 2. * self[4] * other[0] - 4. * self[3] * other[1] - 2. * self[6] * other[2], 2. * self[3] * other[0] + 2. * self[5] * other[2], -2. * self[6] * other[0] - 4. * self[5] * other[1] + self[4] * other[2], 2. * self[5] * other[0] - 2. * self[3] * other[2]],
+                         [0., 1., 0., 2. * self[4] * other[0] - 4. * self[3] * other[1] - 2. * self[6] * other[2], 2. * self[3] * other[0] + 2. * self[5] * other[2], 2. * self[6] * other[0] - 4. * self[5] * other[1] + 2. * self[4] * other[2], 2. * self[5] * other[0] - 2. * self[3] * other[2]],
                          [0., 0., 1., 2. * self[5] * other[0] + 2. * self[6] * other[1] - 4. * self[3] * other[2], -2. * self[6] * other[0] + 2. * self[5] * other[1] - 4. * self[4] * other[2], 2. * self[3] * other[0] + 2. * self[4] * other[1], -2. * self[4] * other[0] + 2. * self[3] * other[1]],
                          [0., 0., 0., other[6], other[5], -other[4], other[3]],
                          [0., 0., 0., -other[5], other[6], other[3], other[4]],
@@ -275,7 +275,7 @@ def jacobian_self_oplus_other_wrt_self_compact(self, other):
         """
         # fmt: off
         return np.array([[1., 0., 0., 2. * self[4] * other[1] + 2. * self[5] * other[2], -4. * self[4] * other[0] + 2. * self[3] * other[1] + 2. * self[6] * other[2], -4. * self[5] * other[0] - 2. * self[6] * other[1] + 2. * self[3] * other[2], -2. * self[5] * other[1] + 2. * self[4] * other[2]],
-                         [0., 1., 0., 2. * self[4] * other[0] - 4. * self[3] * other[1] - 2. * self[6] * other[2], 2. * self[3] * other[0] + 2. * self[5] * other[2], -2. * self[6] * other[0] - 4. * self[5] * other[1] + self[4] * other[2], 2. * self[5] * other[0] - 2. * self[3] * other[2]],
+                         [0., 1., 0., 2. * self[4] * other[0] - 4. * self[3] * other[1] - 2. * self[6] * other[2], 2. * self[3] * other[0] + 2. * self[5] * other[2], 2. * self[6] * other[0] - 4. * self[5] * other[1] + 2. * self[4] * other[2], 2. * self[5] * other[0] - 2. * self[3] * other[2]],
                          [0., 0., 1., 2. * self[5] * other[0] + 2. * self[6] * other[1] - 4. * self[3] * other[2], -2. * self[6] * other[0] + 2. * self[5] * other[1] - 4. * self[4] * other[2], 2. * self[3] * other[0] + 2. * self[4] * other[1], -2. * self[4] * other[0] + 2. * self[3] * other[1]],
                          [0., 0., 0., other[6], other[5], -other[4], other[3]],
                          [0., 0., 0., -other[5], other[6], other[3], other[4]],

diff --git a/tests/test_pose_se3.py b/tests/test_pose_se3.py
@@ -196,7 +196,7 @@ def test_jacobian_self_oplus_other(self):
 
             self.assertEqual(len(numerical_jacobians), len(analytical_jacobians))
             for n, a in zip(numerical_jacobians, analytical_jacobians):
-                self.assertAlmostEqual(np.linalg.norm(n[:, :3] - a[:, :3]), 0.0)
+                self.assertAlmostEqual(np.linalg.norm(n - a), 0.0, places=5)
 
     def test_jacobian_self_ominus_other(self):
         """Test that the ``jacobian_self_ominus_other_wrt_self`` and ``jacobian_self_ominus_other_wrt_other`` methods are correctly implemented."""
@@ -244,7 +244,7 @@ def test_jacobian_self_oplus_other_compact(self):
 
             self.assertEqual(len(numerical_jacobians), len(analytical_jacobians))
             for n, a in zip(numerical_jacobians, analytical_jacobians):
-                self.assertAlmostEqual(np.linalg.norm(n[:, :3] - a[:, :3]), 0.0)
+                self.assertAlmostEqual(np.linalg.norm(n - a), 0.0, places=5)
 
     def test_jacobian_self_ominus_other_compact(self):
         """Test that the ``jacobian_self_ominus_other_wrt_self_compact`` and ``jacobian_self_ominus_other_wrt_other_compact`` methods are correctly implemented."""