diff --git a/cython/gtsam/tests/test_Cal3Unified.py b/cython/gtsam/tests/test_Cal3Unified.py index b7097a24e..3225d2ff9 100644 --- a/cython/gtsam/tests/test_Cal3Unified.py +++ b/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() diff --git a/cython/gtsam/tests/test_SimpleCamera.py b/cython/gtsam/tests/test_SimpleCamera.py new file mode 100644 index 000000000..d6a095d23 --- /dev/null +++ b/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()