From 3402e46ad19cc6e2bef1c71cd777b167e09fe136 Mon Sep 17 00:00:00 2001
From: roderick-koehle <50633232+roderick-koehle@users.noreply.github.com>
Date: Tue, 13 Jul 2021 22:00:21 +0200
Subject: [PATCH] Shared data for triangulation unit tests

---
 python/gtsam/tests/test_Cal3Fisheye.py | 46 ++++++++++----------------
 1 file changed, 18 insertions(+), 28 deletions(-)

diff --git a/python/gtsam/tests/test_Cal3Fisheye.py b/python/gtsam/tests/test_Cal3Fisheye.py
index d731204ef..756113b93 100644
--- a/python/gtsam/tests/test_Cal3Fisheye.py
+++ b/python/gtsam/tests/test_Cal3Fisheye.py
@@ -33,6 +33,19 @@ class TestCal3Fisheye(GtsamTestCase):
         u, v = np.arctan2(x, z), 0.0
         cls.obj_point = np.array([x, y, z])
         cls.img_point = np.array([u, v])
+
+        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.PinholeCameraCal3Fisheye(pose1)
+        camera2 = gtsam.PinholeCameraCal3Fisheye(pose2)
+        cls.origin = np.array([0.0, 0.0, 0.0])
+        cls.poses = gtsam.Pose3Vector([pose1, pose2])
+        cls.cameras = gtsam.CameraSetCal3Fisheye([camera1, camera2])
+        cls.measurements = gtsam.Point2Vector([k.project(cls.origin) for k in cls.cameras])
         
     def test_Cal3Fisheye(self):
         K = gtsam.Cal3Fisheye()
@@ -96,40 +109,17 @@ class TestCal3Fisheye(GtsamTestCase):
     @unittest.skip("triangulatePoint3 currently seems to require perspective projections.")
     def test_triangulation_skipped(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.PinholeCameraCal3Fisheye(pose1)
-        camera2 = gtsam.PinholeCameraCal3Fisheye(pose2)
-        cameras = gtsam.CameraSetCal3Fisheye([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.PinholeCameraCal3Fisheye(pose1)
-        camera2 = gtsam.PinholeCameraCal3Fisheye(pose2)
-        cameras = gtsam.CameraSetCal3Fisheye([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.Cal3Fisheye(100 + 2, 105 + 3, 0.0 + 4, 320 + 5, 240 + 6,