diff --git a/python/gtsam/tests/test_Cal3Unified.py b/python/gtsam/tests/test_Cal3Unified.py index 0b09fc3ae7..ca0959e44c 100644 --- a/python/gtsam/tests/test_Cal3Unified.py +++ b/python/gtsam/tests/test_Cal3Unified.py @@ -40,6 +40,19 @@ def setUpClass(cls): xi = 1 cls.stereographic = gtsam.Cal3Unified(fx, fy, s, u0, v0, k1, k2, p1, p2, xi) + p1 = [-1.0, 0.0, -1.0] + p2 = [ 1.0, 0.0, -1.0] + q1 = gtsam.Rot3(1.0, 0.0, 0.0, 0.0) + q2 = gtsam.Rot3(1.0, 0.0, 0.0, 0.0) + pose1 = gtsam.Pose3(q1, p1) + pose2 = gtsam.Pose3(q2, p2) + camera1 = gtsam.PinholeCameraCal3Unified(pose1, cls.stereographic) + camera2 = gtsam.PinholeCameraCal3Unified(pose2, cls.stereographic) + cls.origin = np.array([0.0, 0.0, 0.0]) + cls.poses = gtsam.Pose3Vector([pose1, pose2]) + cls.cameras = gtsam.CameraSetCal3Unified([camera1, camera2]) + cls.measurements = gtsam.Point2Vector([k.project(cls.origin) for k in cls.cameras]) + def test_Cal3Unified(self): K = gtsam.Cal3Unified() self.assertEqual(K.fx(), 1.) @@ -108,40 +121,17 @@ def test_sfm_factor2(self): @unittest.skip("triangulatePoint3 currently seems to require perspective projections.") def test_triangulation(self): """Estimate spatial point from image measurements""" - p1 = [-1.0, 0.0, -1.0] - p2 = [ 1.0, 0.0, -1.0] - q1 = gtsam.Rot3(1.0, 0.0, 0.0, 0.0) - q2 = gtsam.Rot3(1.0, 0.0, 0.0, 0.0) - obj_point = np.array([0.0, 0.0, 0.0]) - pose1 = gtsam.Pose3(q1, p1) - pose2 = gtsam.Pose3(q2, p2) - camera1 = gtsam.PinholeCameraCal3Unified(pose1, self.stereographic) - camera2 = gtsam.PinholeCameraCal3Unified(pose2, self.stereographic) - cameras = gtsam.CameraSetCal3Unified([camera1, camera2]) - measurements = gtsam.Point2Vector([k.project(obj_point) for k in cameras]) - triangulated = gtsam.triangulatePoint3(cameras, measurements, rank_tol=1e-9, optimize=True) + triangulated = gtsam.triangulatePoint3(self.cameras, self.measurements, rank_tol=1e-9, optimize=True) self.gtsamAssertEquals(measurements[0], self.img_point) - self.gtsamAssertEquals(triangulated, obj_point) + self.gtsamAssertEquals(triangulated, self.origin) def test_triangulation_rectify(self): """Estimate spatial point from image measurements using rectification""" - p1 = [-1.0, 0.0, -1.0] - p2 = [ 1.0, 0.0, -1.0] - q1 = gtsam.Rot3(1.0, 0.0, 0.0, 0.0) - q2 = gtsam.Rot3(1.0, 0.0, 0.0, 0.0) - obj_point = np.array([0.0, 0.0, 0.0]) - pose1 = gtsam.Pose3(q1, p1) - pose2 = gtsam.Pose3(q2, p2) - camera1 = gtsam.PinholeCameraCal3Unified(pose1, self.stereographic) - camera2 = gtsam.PinholeCameraCal3Unified(pose2, self.stereographic) - cameras = gtsam.CameraSetCal3Unified([camera1, camera2]) - measurements = gtsam.Point2Vector([k.project(obj_point) for k in cameras]) - rectified = gtsam.Point2Vector([k.calibration().calibrate(pt) for k, pt in zip(cameras, measurements)]) + rectified = gtsam.Point2Vector([k.calibration().calibrate(pt) for k, pt in zip(self.cameras, self.measurements)]) shared_cal = gtsam.Cal3_S2() - poses = gtsam.Pose3Vector([pose1, pose2]) - triangulated = gtsam.triangulatePoint3(poses, shared_cal, rectified, rank_tol=1e-9, optimize=False) + triangulated = gtsam.triangulatePoint3(self.poses, shared_cal, rectified, rank_tol=1e-9, optimize=False) self.gtsamAssertEquals(measurements[0], self.img_point) - self.gtsamAssertEquals(triangulated, obj_point) + self.gtsamAssertEquals(triangulated, self.origin) def test_retract(self): expected = gtsam.Cal3Unified(100 + 2, 105 + 3, 0.0 + 4, 320 + 5, 240 + 6,