Merge 21f4ba9 into f74e24a

graphslam/edge/edge_landmark.py

@@ -7,8 +7,19 @@
 
 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.
@@ -108,7 +119,18 @@ def to_g2o(self):
             The edge in .g2o format
 
         """
-        # Not yet implemented
+        # 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):
+            # TODO: handle the offset
+            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):
@@ -125,7 +147,24 @@ def from_g2o(cls, line):
             The instantiated edge object, or ``None`` if ``line`` does not correspond to a landmark edge
 
         """
-        # Not yet implemented
+        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 = PoseR2(arr[:2])
+            information = upper_triangular_matrix_to_full_matrix(arr[2:], 2)
+            return EdgeLandmark(vertex_ids, information, estimate, offset=PoseSE2.identity())
+
+        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 = PoseR3(arr[:3])
+            information = upper_triangular_matrix_to_full_matrix(arr[3:], 3)
+            return EdgeLandmark(vertex_ids, information, estimate, offset=PoseSE3.identity())
+
+        return None
 
     def plot(self, color="b"):
         """Plot the edge.
@@ -136,7 +175,19 @@ def plot(self, color="b"):
             The color that will be used to plot the edge
 
         """
-        # Not yet implemented
+        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
 
     def equals(self, other, tol=1e-6):
         """Check whether two edges are equal.

tests/test_edge_landmark.py

@@ -71,6 +71,72 @@ def test_calc_jacobians2d(self):
             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 = PoseSE2(np.random.random_sample(2), 0.0)
+            information = np.eye(2)
+            estimate = PoseR2(np.random.random_sample(2))
+
+            v1 = Vertex(1, p1)
+            v2 = Vertex(2, p2)
+
+            e = EdgeLandmark([1, 2], information, estimate, [v1, v2], offset)
+            e2 = EdgeLandmark.from_g2o(e.to_g2o())
+
+            # Set the `offset`, since that's currently not written to / read from .g2o
+            self.assertFalse(e.equals(e2))
+            e2.offset = None
+            self.assertFalse(e.equals(e2))
+            e2.offset = offset
+
+            self.assertTrue(e.equals(e2))
+
+    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 = PoseSE3(np.random.random_sample(3), [0.0, 0.0, 0.0, 1.0])
+            information = np.eye(3)
+            estimate = PoseR3(np.random.random_sample(3))
+
+            p1.normalize()
+
+            v1 = Vertex(1, p1)
+            v2 = Vertex(2, p2)
+
+            e = EdgeLandmark([1, 2], information, estimate, [v1, v2], offset)
+            e2 = EdgeLandmark.from_g2o(e.to_g2o())
+
+            # Set the `offset`, since that's currently not written to / read from .g2o
+            self.assertFalse(e.equals(e2))
+            e2.offset = None
+            self.assertFalse(e.equals(e2))
+            e2.offset = offset
+            self.assertTrue(e.equals(e2))
+
+    def test_to_g2o_and_from_g2o_edge_cases(self):
+        """Test edge cases for the `to_g2o` and `from_g2o` methods."""
+        offset = PoseSE3.identity()
+        information = np.eye(3)
+        estimate = PoseR3.identity()
+
+        v = Vertex(0, None)
+        e = EdgeLandmark([1, 2], information, estimate, [v, v], offset)
+
+        with self.assertRaises(NotImplementedError):
+            e.to_g2o()
+
+        edge_or_none = EdgeLandmark.from_g2o("bologna")
+        self.assertIsNone(edge_or_none)
+
     def test_equals(self):
         """Test that the `equals` method works correctly."""
         np.random.seed(0)