Add Vertex.equals() method (#77)

* Add Vertex.equals() method

* Linting

* Linting

graphslam/graph.py

@@ -647,5 +647,5 @@ def equals(self, other, tol=1e-6):
             return False
 
         # fmt: off
-        return all(e1.equals(e2, tol) for e1, e2 in zip(self._edges, other._edges)) and all(v1.pose.equals(v2.pose, tol) for v1, v2 in zip(self._vertices, other._vertices))
+        return all(e1.equals(e2, tol) for e1, e2 in zip(self._edges, other._edges)) and all(v1.equals(v2, tol) for v1, v2 in zip(self._vertices, other._vertices))
         # fmt: on

graphslam/vertex.py

@@ -50,6 +50,26 @@ def __init__(self, vertex_id, pose, fixed=False):
         self.fixed = fixed
         self.gradient_index = None
 
+    def equals(self, other, tol=1e-6):
+        """Check whether two vertices are equal.
+
+        Parameters
+        ----------
+        other : Vertex
+            The vertex to which we are comparing
+        tol : float
+            The tolerance
+
+        Returns
+        -------
+        bool
+            Whether the two vertices are equal
+
+        """
+        # fmt: off
+        return self.id == other.id and (type(self.pose) is type(other.pose)) and self.pose.equals(other.pose, tol)  # noqa
+        # fmt: on
+
     def to_g2o(self):
         """Export the vertex to the .g2o format.
 

tests/test_vertex.py

@@ -45,6 +45,19 @@ def test_plot(self):
                 fig.add_subplot(111, projection="3d")
             v.plot()
 
+    def test_equals(self):
+        """Test that the ``equals`` method works correctly."""
+        v0 = Vertex(1, PoseR2([1, 2]))
+
+        v1 = Vertex(1, PoseR2([1, 2]))
+        self.assertTrue(v0.equals(v1))
+
+        v1.id = 2
+        self.assertFalse(v0.equals(v1))
+
+        v2 = Vertex(1, PoseR3([1, 2, 3]))
+        self.assertFalse(v0.equals(v2))
+
     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]))
@@ -53,8 +66,7 @@ def test_to_g2o_from_g2o(self):
         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))
+            self.assertTrue(v.equals(Vertex.from_g2o(v.to_g2o())))
 
     def test_to_g2o_unsupported_type(self):
         """Test that an unsupported pose type cannot be written to a .g2o file."""