Add tests for EdgeOdometry.calc_jacobians (#60)

tests/test_base_edge.py

@@ -1,6 +1,6 @@
 # Copyright (c) 2020 Jeff Irion and contributors
 
-"""Unit tests for the graph.py module.
+"""Unit tests for the ``BaseEdge`` class.
 
 """
 

tests/test_edge_odometry.py

@@ -1,13 +1,16 @@
 # Copyright (c) 2020 Jeff Irion and contributors
 
-"""Unit tests for the graph.py module.
+"""Unit tests for the ``EdgeOdometry`` class.
 
 """
 
 
 import unittest
 
+import numpy as np
+
 from graphslam.vertex import Vertex
+from graphslam.edge.base_edge import BaseEdge
 from graphslam.edge.edge_odometry import EdgeOdometry
 from graphslam.pose.r2 import PoseR2
 from graphslam.pose.r3 import PoseR3
@@ -34,6 +37,98 @@ def test_plot(self):
             e = EdgeOdometry(0, 1, 0, [v, v])
             e.plot()
 
+    def test_calc_jacobians_r2(self):
+        """Test that the ``calc_jacobians`` method gives the correct results."""
+        np.random.seed(0)
+
+        for _ in range(10):
+            p1 = PoseR2(np.random.random_sample(2))
+            p2 = PoseR2(np.random.random_sample(2))
+            estimate = PoseR2(np.random.random_sample(2))
+
+            v1 = Vertex(1, p1)
+            v2 = Vertex(2, p2)
+
+            e = EdgeOdometry([1, 2], np.eye(2), estimate, [v1, v2])
+
+            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_jacobians_r3(self):
+        """Test that the ``calc_jacobians`` method gives the correct results."""
+        np.random.seed(0)
+
+        for _ in range(10):
+            p1 = PoseR3(np.random.random_sample(3))
+            p2 = PoseR3(np.random.random_sample(3))
+            estimate = PoseR3(np.random.random_sample(3))
+
+            v1 = Vertex(1, p1)
+            v2 = Vertex(2, p2)
+
+            e = EdgeOdometry([1, 2], np.eye(3), estimate, [v1, v2])
+
+            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_jacobians_se2(self):
+        """Test that the ``calc_jacobians`` method gives the correct results."""
+        np.random.seed(0)
+
+        for _ in range(10):
+            p1 = PoseSE2(np.random.random_sample(2), np.random.random_sample())
+            p2 = PoseSE2(np.random.random_sample(2), np.random.random_sample())
+            estimate = PoseSE2(np.random.random_sample(2), np.random.random_sample())
+
+            v1 = Vertex(1, p1)
+            v2 = Vertex(2, p2)
+
+            e = EdgeOdometry([1, 2], np.eye(3), estimate, [v1, v2])
+
+            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_jacobians_se3(self):
+        """Test that the ``calc_jacobians`` method gives the correct results."""
+        np.random.seed(0)
+
+        for _ in range(10):
+            p1 = PoseSE3(np.random.random_sample(3), np.random.random_sample(4))
+            p2 = PoseSE3(np.random.random_sample(3), np.random.random_sample(4))
+            estimate = PoseSE3(np.random.random_sample(3), np.random.random_sample(4))
+
+            p1.normalize()
+            p2.normalize()
+            estimate.normalize()
+
+            v1 = Vertex(1, p1)
+            v2 = Vertex(2, p2)
+
+            e = EdgeOdometry([1, 2], np.eye(6), estimate, [v1, v2])
+
+            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)
+
 
 if __name__ == "__main__":
     unittest.main()

tests/test_vertex.py

@@ -1,6 +1,6 @@
 # Copyright (c) 2020 Jeff Irion and contributors
 
-"""Unit tests for the graph.py module.
+"""Unit tests for the ``Vertex`` class.
 
 """