Add approx_equal methods to the BaseEdge, BasePose, and Graph classes

graphslam/edge/base_edge.py

@@ -7,6 +7,8 @@
 
 import numpy as np
 
+from graphslam.pose.base_pose import BasePose
+
 
 #: The difference that will be used for numerical differentiation
 EPSILON = 1e-6
@@ -171,3 +173,33 @@ def plot(self, color=''):
 
         """
         raise NotImplementedError
+
+    def approx_equal(self, other, tol=1e-6):
+        """Check whether two edges are approximately equal.
+
+        Parameters
+        ----------
+        other : BaseEdge
+            The edge to which we are comparing
+        tol : float
+            The tolerance
+
+        Returns
+        -------
+        bool
+            Whether the two edges are approximately equal
+
+        """
+        if len(self.vertex_ids) != len(other.vertex_ids):
+            return False
+
+        if any(v_id1 != v_id2 for v_id1, v_id2 in zip(self.vertex_ids, other.vertex_ids)):
+            return False
+
+        if self.information.shape != other.information.shape or np.linalg.norm(self.information - other.information) / max(np.linalg.norm(self.information), tol) >= tol:
+            return False
+
+        if isinstance(self.estimate, BasePose):
+            return isinstance(other.estimate, BasePose) and self.estimate.approx_equal(other.estimate, tol)
+
+        return not isinstance(other.estimate, BasePose) and np.linalg.norm(self.estimate - other.estimate) / max(np.linalg.norm(self.estimate), tol) < tol

graphslam/graph.py

@@ -348,3 +348,25 @@ def plot(self, vertex_color='r', vertex_marker='o', vertex_markersize=3, edge_co
             plt.title(title)
 
         plt.show()
+
+    def approx_equal(self, other, tol=1e-6):
+        """Check whether two graphs are approximately equal.
+
+        Parameters
+        ----------
+        other : Graph
+            The graph to which we are comparing
+        tol : float
+            The tolerance
+
+        Returns
+        -------
+        bool
+            Whether the two graphs are approximately equal
+
+        """
+        # pylint: disable=protected-access
+        if len(self._edges) != len(other._edges) or len(self._vertices) != len(other._vertices):
+            return False
+
+        return all(e1.approx_equal(e2, tol) for e1, e2 in zip(self._edges, other._edges)) and all(v1.pose.approx_equal(v2.pose, tol) for v1, v2 in zip(self._vertices, other._vertices))

graphslam/pose/base_pose.py

@@ -46,6 +46,24 @@ def to_compact(self):
         """
         raise NotImplementedError
 
+    def approx_equal(self, other, tol=1e-6):
+        """Check whether two poses are approximately equal.
+
+        Parameters
+        ----------
+        other : BasePose
+            The pose to which we are comparing
+        tol : float
+            The tolerance
+
+        Returns
+        -------
+        bool
+            Whether the two poses are approximately equal
+
+        """
+        return np.linalg.norm(self.to_array() - other.to_array()) / max(np.linalg.norm(self.to_array()), tol) < tol
+
     # ======================================================================= #
     #                                                                         #
     #                                Properties                               #

tests/test_base_edge.py

@@ -127,6 +127,32 @@ def test_plot(self):
         with self.assertRaises(NotImplementedError):
             e.plot()
 
+    def test_approx_equal(self):
+        """Test that the ``approx_equal`` method works as expected."""
+        p1 = PoseR2([1, 2])
+        p2 = PoseR2([3, 4])
+        estimate = PoseR2([0, 0])
+
+        v1 = Vertex(1, p1)
+        v2 = Vertex(2, p2)
+
+        e1 = EdgeOdometry([1, 2], np.eye(2), estimate, [v1, v2])
+        e2 = EdgeOdometry([1, 2], np.eye(2), estimate, [v1, v2])
+
+        self.assertTrue(e1.approx_equal(e2))
+
+        e2.estimate = 123
+        self.assertFalse(e2.approx_equal(e1))
+
+        e2.information = np.eye(1)
+        self.assertFalse(e1.approx_equal(e2))
+
+        e2.vertex_ids = [3, 4]
+        self.assertFalse(e1.approx_equal(e2))
+
+        e2.vertex_ids = [5]
+        self.assertFalse(e1.approx_equal(e2))
+
 
 if __name__ == '__main__':
     unittest.main()

tests/test_graph.py

@@ -148,6 +148,11 @@ def test_plot(self):
         with patch("graphslam.graph.plt.show"):
             self.g.plot(title="Title")
 
+    def test_approx_equal(self):
+        """Test that the ``approx_equal`` method returns False when comparing to an empty graph."""
+        g = Graph([], [])
+        self.assertFalse(g.approx_equal(self.g))
+
 
 class TestGraphR3(TestGraphR2):
     r"""Tests for the ``Graph`` class with :math:`\mathbb{R}^3` poses.
@@ -243,12 +248,12 @@ def test_intel(self):
 
         optimized = os.path.join(os.path.dirname(__file__), "input_INTEL_optimized.g2o")
 
-        # An assertion so that linting doesn't complain about `optimized` being unused
-        self.assertEqual(os.path.dirname(__file__), os.path.dirname(optimized))
-
         # Uncomment this line to write the output file
         # g.to_g2o(optimized)
 
+        g2 = load_g2o_se2(optimized)
+        self.assertTrue(g.approx_equal(g2))
+
     def test_parking_garage(self):
         """Test for optimizing the parking garage dataset."""
         parking_garage = os.path.join(os.path.dirname(__file__), "..", "data", "parking-garage.g2o")
@@ -258,12 +263,12 @@ def test_parking_garage(self):
 
         optimized = os.path.join(os.path.dirname(__file__), "parking-garage_optimized.g2o")
 
-        # An assertion so that linting doesn't complain about `optimized` being unused
-        self.assertEqual(os.path.dirname(__file__), os.path.dirname(optimized))
-
         # Uncomment this line to write the output file
         # g.to_g2o(optimized)
 
+        g2 = load_g2o_se3(optimized)
+        self.assertTrue(g.approx_equal(g2))
+
 
 if __name__ == '__main__':
     unittest.main()