Implement Jacobians for inverse and pose (+) point composition (#49)

* Implement Jacobians for inverse and pose (+) point composition

* WIP

* WIP

* WIP

* Fix

* Cleanup

* WIP

* WIP

* WIP

* WIP

* WIP

* WIP

* WIP

* WIP

* WIP

* WIP

* WIP

graphslam/edge/base_edge.py

@@ -120,10 +120,7 @@ def calc_jacobians(self):
         """
         err = self.calc_error()
 
-        # The dimensionality of the compact pose representation
-        dim = len(self.vertices[0].pose.to_compact())
-
-        return [self._calc_jacobian(err, dim, i) for i in range(len(self.vertices))]
+        return [self._calc_jacobian(err, v.pose.COMPACT_DIMENSIONALITY, i) for i, v in enumerate(self.vertices)]
 
     def _calc_jacobian(self, err, dim, vertex_index):
         r"""Calculate the Jacobian of the edge with respect to the specified vertex's pose.

graphslam/pose/base_pose.py

@@ -311,3 +311,46 @@ def jacobian_boxplus(self):
 
         """
         raise NotImplementedError
+
+    def jacobian_self_oplus_point_wrt_self(self, point):
+        r"""Compute the Jacobian of :math:`p_1 \oplus p_2` w.r.t. :math:`p_1`, where `:math:p_2` is a point.
+
+        Parameters
+        ----------
+        point : BasePose
+            The point that is being added to ``self``
+
+        Returns
+        -------
+        np.ndarray
+            The Jacobian of :math:`p_1 \oplus p_2` w.r.t. :math:`p_1`.
+
+        """
+        raise NotImplementedError
+
+    def jacobian_self_oplus_point_wrt_point(self, point):
+        r"""Compute the Jacobian of :math:`p_1 \oplus p_2` w.r.t. :math:`p_2`, where `:math:p_2` is a point.
+
+        Parameters
+        ----------
+        point : BasePose
+            The point that is being added to ``self``
+
+        Returns
+        -------
+        np.ndarray
+            The Jacobian of :math:`p_1 \oplus p_2` w.r.t. :math:`p_2`.
+
+        """
+        raise NotImplementedError
+
+    def jacobian_inverse(self):
+        r"""Compute the Jacobian of :math:`p^{-1}`.
+
+        Returns
+        -------
+        np.ndarray
+            The Jacobian of :math:`p^{-1}
+
+        """
+        raise NotImplementedError

graphslam/pose/r2.py

@@ -292,3 +292,46 @@ def jacobian_boxplus(self):
 
         """
         return np.eye(2)
+
+    def jacobian_self_oplus_point_wrt_self(self, point):
+        r"""Compute the Jacobian of :math:`p_1 \oplus p_2` w.r.t. :math:`p_1`, where `:math:p_2` is a point.
+
+        Parameters
+        ----------
+        point : PoseR2
+            The point that is being added to ``self``
+
+        Returns
+        -------
+        np.ndarray
+            The Jacobian of :math:`p_1 \oplus p_2` w.r.t. :math:`p_1`.
+
+        """
+        return np.eye(2)
+
+    def jacobian_self_oplus_point_wrt_point(self, point):
+        r"""Compute the Jacobian of :math:`p_1 \oplus p_2` w.r.t. :math:`p_2`, where `:math:p_2` is a point.
+
+        Parameters
+        ----------
+        point : PoseR2
+            The point that is being added to ``self``
+
+        Returns
+        -------
+        np.ndarray
+            The Jacobian of :math:`p_1 \oplus p_2` w.r.t. :math:`p_2`.
+
+        """
+        return np.eye(2)
+
+    def jacobian_inverse(self):
+        r"""Compute the Jacobian of :math:`p^{-1}`.
+
+        Returns
+        -------
+        np.ndarray
+            The Jacobian of :math:`p^{-1}
+
+        """
+        return -np.eye(2)

graphslam/pose/r3.py

@@ -292,3 +292,46 @@ def jacobian_boxplus(self):
 
         """
         return np.eye(3)
+
+    def jacobian_self_oplus_point_wrt_self(self, point):
+        r"""Compute the Jacobian of :math:`p_1 \oplus p_2` w.r.t. :math:`p_1`, where `:math:p_2` is a point.
+
+        Parameters
+        ----------
+        point : PoseR3
+            The point that is being added to ``self``
+
+        Returns
+        -------
+        np.ndarray
+            The Jacobian of :math:`p_1 \oplus p_2` w.r.t. :math:`p_1`.
+
+        """
+        return np.eye(3)
+
+    def jacobian_self_oplus_point_wrt_point(self, point):
+        r"""Compute the Jacobian of :math:`p_1 \oplus p_2` w.r.t. :math:`p_2`, where `:math:p_2` is a point.
+
+        Parameters
+        ----------
+        point : PoseR3
+            The point that is being added to ``self``
+
+        Returns
+        -------
+        np.ndarray
+            The Jacobian of :math:`p_1 \oplus p_2` w.r.t. :math:`p_2`.
+
+        """
+        return np.eye(3)
+
+    def jacobian_inverse(self):
+        r"""Compute the Jacobian of :math:`p^{-1}`.
+
+        Returns
+        -------
+        np.ndarray
+            The Jacobian of :math:`p^{-1}
+
+        """
+        return -np.eye(3)

graphslam/pose/se2.py

@@ -390,3 +390,56 @@ def jacobian_boxplus(self):
                          [np.sin(self[2]), np.cos(self[2]), 0.],
                          [0., 0., 1.]])
         # fmt: on
+
+    def jacobian_self_oplus_point_wrt_self(self, point):
+        r"""Compute the Jacobian of :math:`p_1 \oplus p_2` w.r.t. :math:`p_1`, where `:math:p_2` is a point.
+
+        Parameters
+        ----------
+        point : PoseR2
+            The point that is being added to ``self``
+
+        Returns
+        -------
+        np.ndarray
+            The Jacobian of :math:`p_1 \oplus p_2` w.r.t. :math:`p_1`.
+
+        """
+        # fmt: off
+        return np.array([[1., 0., -point[0] * np.sin(self[2]) - point[1] * np.cos(self[2])],
+                         [0., 1., point[0] * np.cos(self[2]) - point[1] * np.sin(self[2])]])
+        # fmt: on
+
+    def jacobian_self_oplus_point_wrt_point(self, point):
+        r"""Compute the Jacobian of :math:`p_1 \oplus p_2` w.r.t. :math:`p_2`, where `:math:p_2` is a point.
+
+        Parameters
+        ----------
+        point : PoseR2
+            The point that is being added to ``self``
+
+        Returns
+        -------
+        np.ndarray
+            The Jacobian of :math:`p_1 \oplus p_2` w.r.t. :math:`p_2`.
+
+        """
+        # fmt: off
+        return np.array([[np.cos(self[2]), -np.sin(self[2])],
+                         [np.sin(self[2]), np.cos(self[2])]])
+        # fmt: on
+
+    def jacobian_inverse(self):
+        r"""Compute the Jacobian of :math:`p^{-1}`.
+
+        Returns
+        -------
+        np.ndarray
+            The Jacobian of :math:`p^{-1}
+
+        """
+        # fmt: off
+        return np.array([[-np.cos(self[2]), -np.sin(self[2]), self[0] * np.sin(self[2]) - self[1] * np.cos(self[2])],
+                         [np.sin(self[2]), -np.cos(self[2]), self[0] * np.cos(self[2]) + self[1] * np.sin(self[2])],
+                         [0., 0., -1.]])
+        # fmt: on

graphslam/pose/se3.py

@@ -198,8 +198,8 @@ def __add__(self, other):
         if isinstance(other, PoseR3) or (isinstance(other, np.ndarray) and len(other) == 3):
             # pose (+) point
             # fmt: off
-            return PoseR3([self[0] + other[0] + 2. * (-(self[4]**2 + self[5]**2) * other[0] + (self[3] * self[4] - self[5] * self[6]) * other[1] + (self[4] * self[6] + self[3] * self[5]) * other[2]),
-                           self[1] + other[1] + 2. * ((self[5] * self[6] + self[3] * self[4]) * other[0] - (self[3]**2 + self[5]**2) * other[1] + (self[4] * self[5] - self[3] * self[6]) * other[2]),
+            return PoseR3([self[0] + other[0] + 2. * (-(self[4]**2 + self[5]**2) * other[0] + (self[3] * self[4] - self[5] * self[6]) * other[1] + (self[3] * self[5] + self[4] * self[6]) * other[2]),
+                           self[1] + other[1] + 2. * ((self[3] * self[4] + self[5] * self[6]) * other[0] - (self[3]**2 + self[5]**2) * other[1] + (self[4] * self[5] - self[3] * self[6]) * other[2]),
                            self[2] + other[2] + 2. * ((self[3] * self[5] - self[4] * self[6]) * other[0] + (self[3] * self[6] + self[4] * self[5]) * other[1] - (self[3]**2 + self[4]**2) * other[2])])
             # fmt: on
 
@@ -449,3 +449,65 @@ def jacobian_boxplus(self):
                          [0., 0., 0., -self[3], -self[4], -self[5]]],
                         dtype=np.float64)
         # fmt: on
+
+    def jacobian_self_oplus_point_wrt_self(self, point):
+        r"""Compute the Jacobian of :math:`p_1 \oplus p_2` w.r.t. :math:`p_1`, where `:math:p_2` is a point.
+
+        Parameters
+        ----------
+        point : PoseR3
+            The point that is being added to ``self``
+
+        Returns
+        -------
+        np.ndarray
+            The Jacobian of :math:`p_1 \oplus p_2` w.r.t. :math:`p_1`.
+
+        """
+        # fmt: off
+        return np.array([[1., 0., 0., 2. * self[4] * point[1] + 2. * self[5] * point[2], -4. * self[4] * point[0] + 2. * self[3] * point[1] + 2. * self[6] * point[2], -4. * self[5] * point[0] - 2. * self[6] * point[1] + 2. * self[3] * point[2], -2. * self[5] * point[1] + 2. * self[4] * point[2]],
+                         [0., 1., 0., 2. * self[4] * point[0] - 4. * self[3] * point[1] - 2. * self[6] * point[2], 2. * self[3] * point[0] + 2. * self[5] * point[2], 2. * self[6] * point[0] - 4. * self[5] * point[1] + 2. * self[4] * point[2], 2. * self[5] * point[0] - 2. * self[3] * point[2]],
+                         [0., 0., 1., 2. * self[5] * point[0] + 2. * self[6] * point[1] - 4. * self[3] * point[2], -2. * self[6] * point[0] + 2. * self[5] * point[1] - 4. * self[4] * point[2], 2. * self[3] * point[0] + 2. * self[4] * point[1], -2. * self[4] * point[0] + 2. * self[3] * point[1]]],
+                        dtype=np.float64)
+        # fmt: on
+
+    def jacobian_self_oplus_point_wrt_point(self, point):
+        r"""Compute the Jacobian of :math:`p_1 \oplus p_2` w.r.t. :math:`p_2`, where `:math:p_2` is a point.
+
+        Parameters
+        ----------
+        point : PoseR3
+            The point that is being added to ``self``
+
+        Returns
+        -------
+        np.ndarray
+            The Jacobian of :math:`p_1 \oplus p_2` w.r.t. :math:`p_2`.
+
+        """
+        # fmt: off
+        return np.array([[1. - 2. * (self[4]**2 + self[5]**2), 2. * (self[3] * self[4] - self[5] * self[6]), 2. * (self[3] * self[5] + self[4] * self[6])],
+                         [2. * (self[3] * self[4] + self[5] * self[6]), 1. - 2. * (self[3]**2 + self[5]**2), 2. * (self[4] * self[5] - self[3] * self[6])],
+                         [2. * (self[3] * self[5] - self[4] * self[6]), 2. * (self[3] * self[6] + self[4] * self[5]), 1. - 2. * (self[3]**2 + self[4]**2)]],
+                        dtype=np.float64)
+        # fmt: on
+
+    def jacobian_inverse(self):
+        r"""Compute the Jacobian of :math:`p^{-1}`.
+
+        Returns
+        -------
+        np.ndarray
+            The Jacobian of :math:`p^{-1}
+
+        """
+        # fmt: off
+        return np.array([[-1. + 2. * (self[4]**2 + self[5]**2), -2. * (self[3] * self[4] + self[5] * self[6]), -2. * (self[3] * self[5] - self[4] * self[6]), -2. * (self[4] * self[1] + self[5] * self[2]), 2. * (2. * self[4] * self[0] - self[3] * self[1] + self[6] * self[2]), 2. * (2. * self[5] * self[0] - self[6] * self[1] - self[3] * self[2]), 2. * (-self[5] * self[1] + self[4] * self[2])],
+                         [-2. * (self[3] * self[4] - self[5] * self[6]), -1. + 2. * (self[3]**2 + self[5]**2), -2. * (self[4] * self[5] + self[3] * self[6]), 2. * (-self[4] * self[0] + 2. * self[3] * self[1] - self[6] * self[2]), -2. * (self[3] * self[0] + self[5] * self[2]), 2. * (self[6] * self[0] + 2. * self[5] * self[1] - self[4] * self[2]), 2. * (self[5] * self[0] - self[3] * self[2])],
+                         [-2. * (self[3] * self[5] + self[4] * self[6]), -2. * (self[4] * self[5] - self[3] * self[6]), -1. + 2. * (self[3]**2 + self[4]**2), 2. * (-self[5] * self[0] + self[6] * self[1] + 2. * self[3] * self[2]), -2. * (self[6] * self[0] + self[5] * self[1] - 2. * self[4] * self[2]), -2. * (self[3] * self[0] + self[4] * self[1]), 2. * (-self[4] * self[0] + self[3] * self[1])],
+                         [0., 0., 0., -1., 0., 0., 0.],
+                         [0., 0., 0., 0., -1., 0., 0.],
+                         [0., 0., 0., 0., 0., -1., 0.],
+                         [0., 0., 0., 0., 0., 0., 1.]],
+                        dtype=np.float64)
+        # fmt: on

tests/edge_types.py

@@ -78,3 +78,30 @@ def calc_jacobians(self):
         return [np.dot(self.vertices[0].pose.jacobian_self_ominus_other_wrt_self_compact(self.vertices[1].pose), self.vertices[0].pose.jacobian_boxplus()),
                 np.dot(self.vertices[0].pose.jacobian_self_ominus_other_wrt_other_compact(self.vertices[1].pose), self.vertices[1].pose.jacobian_boxplus())]
         # fmt: on
+
+
+class EdgeOPlusPoint(BaseEdgeForTests):
+    """A simple edge class for testing."""
+
+    def calc_error(self):
+        """A simple "error" method."""
+        return (self.vertices[0].pose + self.vertices[1].pose).to_array()
+
+    def calc_jacobians(self):
+        """Calculate the Jacobians."""
+        # fmt: off
+        return [np.dot(self.vertices[0].pose.jacobian_self_oplus_point_wrt_self(self.vertices[1].pose), self.vertices[0].pose.jacobian_boxplus()),
+                np.dot(self.vertices[0].pose.jacobian_self_oplus_point_wrt_point(self.vertices[1].pose), self.vertices[1].pose.jacobian_boxplus())]
+        # fmt: on
+
+
+class EdgeInverse(BaseEdgeForTests):
+    """A simple edge class for testing."""
+
+    def calc_error(self):
+        """A simple "error" method."""
+        return self.vertices[0].pose.inverse.to_array()
+
+    def calc_jacobians(self):
+        """Calculate the Jacobians."""
+        return [np.dot(self.vertices[0].pose.jacobian_inverse(), self.vertices[0].pose.jacobian_boxplus())]  # fmt: skip

tests/test_base_pose.py

@@ -162,6 +162,24 @@ def test_jacobian_boxplus(self):
         with self.assertRaises(NotImplementedError):
             _ = p1.jacobian_boxplus()
 
+    def test_jacobian_self_oplus_point(self):
+        """Test that the ``jacobian_self_oplus_point_wrt_self`` and ``jacobian_self_oplus_point_wrt_point`` methods are not implemented."""
+        p1 = BasePose([])
+        p2 = BasePose([])
+
+        with self.assertRaises(NotImplementedError):
+            _ = p1.jacobian_self_oplus_point_wrt_self(p2)
+
+        with self.assertRaises(NotImplementedError):
+            _ = p1.jacobian_self_oplus_point_wrt_point(p2)
+
+    def test_jacobian_inverse(self):
+        """Test that the ``jacobian_inverse`` method is not implemented."""
+        p1 = BasePose([])
+
+        with self.assertRaises(NotImplementedError):
+            _ = p1.jacobian_inverse()
+
 
 if __name__ == "__main__":
     unittest.main()

tests/test_pose_r2.py

@@ -12,7 +12,7 @@
 from graphslam.vertex import Vertex
 from graphslam.pose.r2 import PoseR2
 from graphslam.edge.base_edge import BaseEdge
-from .edge_types import EdgeOMinus, EdgeOMinusCompact, EdgeOPlus, EdgeOPlusCompact
+from .edge_types import EdgeInverse, EdgeOMinus, EdgeOMinusCompact, EdgeOPlus, EdgeOPlusCompact, EdgeOPlusPoint
 
 
 class TestPoseR2(unittest.TestCase):
@@ -199,6 +199,46 @@ def test_jacobian_self_ominus_other_compact(self):
             for n, a in zip(numerical_jacobians, analytical_jacobians):
                 self.assertAlmostEqual(np.linalg.norm(n - a), 0.0)
 
+    def test_jacobian_self_oplus_point(self):
+        """Test that the ``jacobian_self_oplus_point_wrt_self`` and ``jacobian_self_oplus_point_wrt_point`` methods are correctly implemented."""
+        np.random.seed(0)
+
+        for _ in range(10):
+            p1 = PoseR2(np.random.random_sample(2))
+            p2 = PoseR2(np.random.random_sample(2))
+
+            v1 = Vertex(1, p1)
+            v2 = Vertex(2, p2)
+
+            e = EdgeOPlusPoint([1, 2], np.eye(2), np.zeros(2), [v1, v2])
+
+            numerical_jacobians = BaseEdge.calc_jacobians(e)
+
+            analytical_jacobians = e.calc_jacobians()
+
+            self.assertEqual(len(numerical_jacobians), len(analytical_jacobians))
+            for n, a in zip(numerical_jacobians, analytical_jacobians):
+                self.assertAlmostEqual(np.linalg.norm(n - a), 0.0)
+
+    def test_jacobian_inverse(self):
+        """Test that the ``jacobian_inverse`` method is correctly implemented."""
+        np.random.seed(0)
+
+        for _ in range(10):
+            p1 = PoseR2(np.random.random_sample(2))
+
+            v1 = Vertex(1, p1)
+
+            e = EdgeInverse([1], np.eye(2), np.zeros(2), [v1])
+
+            numerical_jacobians = BaseEdge.calc_jacobians(e)
+
+            analytical_jacobians = e.calc_jacobians()
+
+            self.assertEqual(len(numerical_jacobians), len(analytical_jacobians))
+            for n, a in zip(numerical_jacobians, analytical_jacobians):
+                self.assertAlmostEqual(np.linalg.norm(n - a), 0.0)
+
 
 if __name__ == "__main__":
     unittest.main()