More unit tests, specifically one that exercises overloaded static method

cython/gtsam/tests/test_Cal3Unified.py

@@ -1,6 +1,5 @@
 import unittest
 import gtsam
-from math import *
 import numpy as np
 
 
@@ -11,5 +10,13 @@ class TestCal3Unified(unittest.TestCase):
         self.assertEqual(K.fx(), 1.)
         self.assertEqual(K.fx(), 1.)
 
+    def test_retract(self):
+        expected  = gtsam.Cal3Unified(100 + 2, 105 + 3, 0.0 + 4, 320 + 5, 240 + 6, 1e-3 + 7, 2.0*1e-3 + 8, 3.0*1e-3 + 9, 4.0*1e-3 + 10, 0.1 + 1)
+        K = gtsam.Cal3Unified(100, 105, 0.0, 320, 240, 1e-3, 2.0*1e-3, 3.0*1e-3, 4.0*1e-3, 0.1)
+        d = np.array([2, 3, 4, 5, 6, 7, 8, 9, 10, 1], order='F')
+        actual = K.retract(d)
+        self.assertTrue(actual.equals(expected, 1e-9))
+        np.testing.assert_allclose(d, K.localCoordinates(actual))
+
 if __name__ == "__main__":
     unittest.main()

cython/gtsam/tests/test_SimpleCamera.py

@@ -0,0 +1,32 @@
+import math
+import numpy as np
+import unittest
+
+from gtsam import Pose2, Point3, Rot3, Pose3, Cal3_S2, SimpleCamera
+
+K = Cal3_S2(625, 625, 0, 0, 0)
+
+class TestCal3Unified(unittest.TestCase):
+
+    def test_constructor(self):
+        pose1 = Pose3(Rot3(np.diag([1, -1, -1])), Point3(0, 0, 0.5))
+        camera = SimpleCamera(pose1, K)
+        self.assertTrue(camera.calibration().equals(K, 1e-9))
+        self.assertTrue(camera.pose().equals(pose1, 1e-9))
+
+    def test_level2(self):
+        # Create a level camera, looking in Y-direction
+        pose2 = Pose2(0.4,0.3,math.pi/2.0)
+        camera = SimpleCamera.Level(K, pose2, 0.1)
+
+        # expected
+        x = Point3(1,0,0)
+        y = Point3(0,0,-1)
+        z = Point3(0,1,0)
+        wRc = Rot3(x,y,z)
+        expected = Pose3(wRc,Point3(0.4,0.3,0.1))
+        self.assertTrue(camera.pose().equals(expected, 1e-9))
+
+
+if __name__ == "__main__":
+    unittest.main()