Merge d05c19b into 3061891

graphslam/edge/edge_landmark.py

@@ -0,0 +1,167 @@
+# Copyright (c) 2020 Jeff Irion and contributors
+
+r"""A class for landmark edges.
+
+"""
+
+
+import numpy as np
+
+try:
+    import matplotlib.pyplot as plt
+except ImportError:  # pragma: no cover
+    plt = None
+
+from .base_edge import BaseEdge
+
+from ..pose.r2 import PoseR2
+from ..pose.se2 import PoseSE2
+from ..pose.r3 import PoseR3
+from ..pose.se3 import PoseSE3
+from ..util import upper_triangular_matrix_to_full_matrix
+
+
+class EdgeLandmark(BaseEdge):
+    r"""A class for representing landmark edges in Graph SLAM.
+
+    Parameters
+    ----------
+    vertices : list[graphslam.vertex.Vertex]
+        A list of the vertices constrained by the edge
+    information : np.ndarray
+        The information matrix :math:`\Omega_j` associated with the edge
+    estimate : BasePose
+        The expected measurement :math:`\mathbf{z}_j`
+    offset : BasePose
+        The offset
+
+    Attributes
+    ----------
+    vertices : list[graphslam.vertex.Vertex]
+        A list of the vertices constrained by the edge
+    information : np.ndarray
+        The information matrix :math:`\Omega_j` associated with the edge
+    estimate : BasePose
+        The expected measurement :math:`\mathbf{z}_j`
+    offset : BasePose
+        The offset
+
+    """
+
+    def __init__(self, vertex_ids, information, estimate, vertices=None, offset=None):
+        super().__init__(vertex_ids, information, estimate, vertices)
+        self.offset = offset
+
+    def calc_error(self):
+        r"""Calculate the error for the edge: :math:`\mathbf{e}_j \in \mathbb{R}^\bullet`.
+
+        .. math::
+
+           \mathbf{e}_j = \mathbf{z}_j - ((p_1 \oplus p_{\text{offset}})^{-1} \oplus p_2)
+
+
+        Returns
+        -------
+        np.ndarray
+            The error for the edge
+
+        """
+        return (self.estimate - ((self.vertices[0].pose + self.offset).inverse + self.vertices[1].pose)).to_compact()
+
+    def calc_jacobians(self):
+        r"""Calculate the Jacobian of the edge's error with respect to each constrained pose.
+
+        .. math::
+
+           \frac{\partial}{\partial \Delta \mathbf{x}^k} \left[ \mathbf{e}_j(\mathbf{x}^k \boxplus \Delta \mathbf{x}^k) \right]
+
+
+        Returns
+        -------
+        list[np.ndarray]
+            The Jacobian matrices for the edge with respect to each constrained pose
+
+        """
+        pose_oplus_offset = self.vertices[0].pose + self.offset
+        # fmt: off
+        return [-np.dot(np.dot(np.dot(pose_oplus_offset.inverse.jacobian_self_oplus_point_wrt_self(self.vertices[1].pose), pose_oplus_offset.jacobian_inverse()), self.vertices[0].pose.jacobian_self_oplus_other_wrt_self(self.offset)), self.vertices[0].pose.jacobian_boxplus()),
+                -np.dot(pose_oplus_offset.inverse.jacobian_self_oplus_point_wrt_point(self.vertices[1].pose), self.vertices[1].pose.jacobian_boxplus())]
+        # fmt: on
+
+    def to_g2o(self):
+        """Export the edge to the .g2o format.
+
+        Returns
+        -------
+        str
+            The edge in .g2o format
+
+        """
+        # https://docs.ros.org/en/kinetic/api/rtabmap/html/OptimizerG2O_8cpp_source.html
+        # fmt: off
+        if isinstance(self.vertices[0].pose, PoseSE2):
+            return "EDGE_SE2_XY {} {} {} {} ".format(self.vertex_ids[0], self.vertex_ids[1], self.estimate[0], self.estimate[1]) + " ".join([str(x) for x in self.information[np.triu_indices(2, 0)]]) + "\n"
+
+        if isinstance(self.vertices[0].pose, PoseSE3):
+            offset_parameter = 0
+            return "EDGE_SE3_TRACKXYZ {} {} {} {} {} {} ".format(self.vertex_ids[0], self.vertex_ids[1], offset_parameter, self.estimate[0], self.estimate[1], self.estimate[2]) + " ".join([str(x) for x in self.information[np.triu_indices(3, 0)]]) + "\n"
+        # fmt: on
+
+        raise NotImplementedError
+
+    @classmethod
+    def from_g2o(cls, line):
+        """Load an edge from a line in a .g2o file.
+
+        Parameters
+        ----------
+        line : str
+            The line from the .g2o file
+
+        Returns
+        -------
+        EdgeLandmark, None
+            The instantiated edge object, or ``None`` if ``line`` does not correspond to a landmark edge
+
+        """
+        if line.startswith("EDGE_SE2_XY "):
+            numbers = line[len("EDGE_SE2_XY "):].split()  # fmt: skip
+            arr = np.array([float(number) for number in numbers[2:]], dtype=np.float64)
+            vertex_ids = [int(numbers[0]), int(numbers[1])]
+            estimate = PoseSE2(arr[:2], 0.0)
+            information = upper_triangular_matrix_to_full_matrix(arr[2:], 2)
+            return EdgeLandmark(vertex_ids, information, estimate)
+
+        if line.startswith("EDGE_SE3_TRACKXYZ "):
+            # TODO: handle the offset
+            numbers = line[len("EDGE_SE3_TRACKXYZ "):].split()  # fmt: skip
+            arr = np.array([float(number) for number in numbers[3:]], dtype=np.float64)
+            vertex_ids = [int(numbers[0]), int(numbers[1])]
+            estimate = PoseSE3(arr[:3], [0.0, 0.0, 0.0, 1.0])
+            information = upper_triangular_matrix_to_full_matrix(arr[3:], 3)
+            return EdgeLandmark(vertex_ids, information, estimate)
+
+        return None
+
+    def plot(self, color="b"):
+        """Plot the edge.
+
+        Parameters
+        ----------
+        color : str
+            The color that will be used to plot the edge
+
+        """
+        if plt is None:  # pragma: no cover
+            raise NotImplementedError
+
+        if isinstance(self.vertices[0].pose, (PoseR2, PoseSE2)):
+            xy = np.array([v.pose.position for v in self.vertices])
+            plt.plot(xy[:, 0], xy[:, 1], color=color)
+
+        elif isinstance(self.vertices[0].pose, (PoseR3, PoseSE3)):
+            xyz = np.array([v.pose.position for v in self.vertices])
+            plt.plot(xyz[:, 0], xyz[:, 1], xyz[:, 2], color=color)
+
+        else:
+            raise NotImplementedError

tests/test_edge_landmark.py

@@ -0,0 +1,108 @@
+# Copyright (c) 2020 Jeff Irion and contributors
+
+"""Unit tests for the edge_landmark.py module.
+
+"""
+
+
+import unittest
+
+import numpy as np
+
+from graphslam.vertex import Vertex
+from graphslam.edge.base_edge import BaseEdge
+from graphslam.edge.edge_landmark import EdgeLandmark
+from graphslam.pose.r2 import PoseR2
+from graphslam.pose.r3 import PoseR3
+from graphslam.pose.se2 import PoseSE2
+from graphslam.pose.se3 import PoseSE3
+
+
+class TestEdgeLandmark(unittest.TestCase):
+    """Tests for the ``EdgeLandmark`` class."""
+
+    def test_calc_jacobians3d(self):
+        """Test that the ``calc_jacobians`` method is correctly implemented."""
+        np.random.seed(0)
+
+        for a in range(10):
+            p1 = PoseSE3(np.random.random_sample(3), np.random.random_sample(4))
+            p2 = PoseR3(np.random.random_sample(3))
+            offset = PoseSE3(np.random.random_sample(3), [0.0, 0.0, 0.0, 1.0])
+
+            p1.normalize()
+
+            v1 = Vertex(1, p1)
+            v2 = Vertex(2, p2)
+
+            e = EdgeLandmark([1, 2], np.eye(3), np.zeros(3), [v1, v2], offset)
+
+            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, places=5)
+
+    def test_calc_jacobians2d(self):
+        """Test that the ``calc_jacobians`` method is correctly implemented."""
+        np.random.seed(0)
+
+        for a in range(10):
+            p1 = PoseSE2(np.random.random_sample(2), np.random.random_sample())
+            p2 = PoseR2(np.random.random_sample(2))
+            offset = PoseSE2(np.random.random_sample(2), 0.0)
+
+            v1 = Vertex(1, p1)
+            v2 = Vertex(2, p2)
+
+            e = EdgeLandmark([1, 2], np.eye(2), np.zeros(2), [v1, v2], offset)
+
+            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, places=5)
+
+    def test_to_g2o_and_from_g2o_2d(self):
+        """Test that the `to_g2o` and `from_g2o` methods work correctly for 2D landmark edges."""
+        np.random.seed(0)
+
+        for _ in range(10):
+            p1 = PoseSE2(np.random.random_sample(2), np.random.random_sample())
+            p2 = PoseR2(np.random.random_sample(2))
+            offset = PoseR2(np.random.random_sample(2))
+            estimate = PoseSE2(np.random.random_sample(2), 0.0)
+
+            v1 = Vertex(1, p1)
+            v2 = Vertex(2, p2)
+
+            e = EdgeLandmark([1, 2], np.eye(2), estimate, [v1, v2], offset)
+
+            self.assertTrue(e.equals(EdgeLandmark.from_g2o(e.to_g2o())))
+
+    def test_to_g2o_and_from_g2o_3d(self):
+        """Test that the `to_g2o` and `from_g2o` methods work correctly for 3D landmark edges."""
+        np.random.seed(0)
+
+        for _ in range(10):
+            p1 = PoseSE3(np.random.random_sample(3), np.random.random_sample(4))
+            p2 = PoseR3(np.random.random_sample(3))
+            offset = PoseR3(np.random.random_sample(3))
+            estimate = PoseSE3(np.random.random_sample(3), [0.0, 0.0, 0.0, 1.0])
+
+            p1.normalize()
+
+            v1 = Vertex(1, p1)
+            v2 = Vertex(2, p2)
+
+            e = EdgeLandmark([1, 2], np.eye(3), estimate, [v1, v2], offset)
+
+            self.assertTrue(e.equals(EdgeLandmark.from_g2o(e.to_g2o())))
+
+
+if __name__ == "__main__":
+    unittest.main()