Add a test for Vertex.to_g2o() and Vertex.from_g2o() (#75)

tests/test_vertex.py

@@ -19,7 +19,7 @@
 
 
 class TestVertex(unittest.TestCase):
-    """Tests for the ``Vetex`` class."""
+    """Tests for the ``Vertex`` class."""
 
     def test_constructor(self):
         """Test that a ``Vertex`` object can be created."""
@@ -45,7 +45,18 @@ def test_plot(self):
                 fig.add_subplot(111, projection="3d")
             v.plot()
 
-    def test_to_g2o(self):
+    def test_to_g2o_from_g2o(self):
+        """Test that the ``to_g2o`` and ``from_g2o`` methods work correctly."""
+        v_r2 = Vertex(1, PoseR2([1, 2]))
+        v_se2 = Vertex(2, PoseSE2([1, 2], 3))
+        v_r3 = Vertex(3, PoseR3([1, 2, 3]))
+        v_se3 = Vertex(4, PoseSE3([1, 2, 3], [0.5, 0.5, 0.5, 0.5]))
+
+        for v in [v_r2, v_se2, v_r3, v_se3]:
+            self.assertEqual(v.id, Vertex.from_g2o(v.to_g2o()).id)
+            self.assertTrue(v.pose.equals(Vertex.from_g2o(v.to_g2o()).pose))
+
+    def test_to_g2o_unsupported_type(self):
         """Test that an unsupported pose type cannot be written to a .g2o file."""
         # Use `None` in lieue of an actual unsupported pose
         v = Vertex(0, None)