diff --git a/python/gtsam/tests/test_Cal3Unified.py b/python/gtsam/tests/test_Cal3Unified.py index ff9d659607..f2c1ada488 100644 --- a/python/gtsam/tests/test_Cal3Unified.py +++ b/python/gtsam/tests/test_Cal3Unified.py @@ -14,94 +14,93 @@ import gtsam from gtsam.utils.test_case import GtsamTestCase +from gtsam.symbol_shorthand import K, L, P class TestCal3Unified(GtsamTestCase): + @classmethod + def setUpClass(cls): + """ + Stereographic fisheye projection + + An equidistant fisheye projection with focal length f is defined + as the relation r/f = 2*tan(theta/2), with r being the radius in the + image plane and theta the incident angle of the object point. + """ + x, y, z = 1.0, 0.0, 1.0 + r = np.linalg.norm([x, y, z]) + u, v = 2*x/(z+r), 0.0 + #u, v = 2*np.tan(np.arctan2(x, z)/2), 0.0 + cls.obj_point = np.array([x, y, z]) + cls.img_point = np.array([u, v]) + + fx, fy, s, u0, v0 = 2, 2, 0, 0, 0 + k1, k2, p1, p2 = 0, 0, 0, 0 + xi = 1 + cls.stereographic = gtsam.Cal3Unified(fx, fy, s, u0, v0, k1, k2, p1, p2, xi) + def test_Cal3Unified(self): K = gtsam.Cal3Unified() self.assertEqual(K.fx(), 1.) self.assertEqual(K.fx(), 1.) def test_distortion(self): - "Stereographic fisheye model of focal length f, defined as r/f = 2*tan(theta/2)" - fx, fy, s, u0, v0 = 2, 2, 0, 0, 0 - k1, k2, p1, p2 = 0, 0, 0, 0 - xi = 1 - stereographic = gtsam.Cal3Unified(fx, fy, s, u0, v0, k1, k2, p1, p2, xi) - x, y, z = 1, 0, 1 - u, v = stereographic.uncalibrate([x, y]) + """Stereographic fisheye model of focal length f, defined as r/f = 2*tan(theta/2)""" + x, y, z = self.obj_point r = np.linalg.norm([x, y, z]) - # Note: 2*tan(atan2(x, z)/2) = 2/(1+sqrt(x^2+z^2)) - self.assertAlmostEqual(2*np.tan(np.arctan2(x, z)/2), 2/(1+r)) - self.assertAlmostEqual(u, 2/(1+r)) - x2, y2 = stereographic.calibrate([u, v]) - self.assertAlmostEqual(x2, x) + # Note: 2*tan(atan2(x, z)/2) = 2*x/(z+sqrt(x^2+z^2)) + self.assertAlmostEqual(2*np.tan(np.arctan2(x, z)/2), 2*x/(z+r)) + perspective_pt = self.obj_point[0:2]/self.obj_point[2] + distorted_pt = self.stereographic.uncalibrate(perspective_pt) + rectified_pt = self.stereographic.calibrate(distorted_pt) + self.gtsamAssertEquals(distorted_pt, self.img_point) + self.gtsamAssertEquals(rectified_pt, perspective_pt) def test_pinhole(self): - "Spatial stereographic camera projection" - x, y, z = 1.0, 0.0, 1.0 - r = np.linalg.norm([x, y, z]) - u, v = 2/(1+r), 0.0 - objPoint = np.array([x, y, z]) - imgPoint = np.array([u, v]) - fx, fy, s, u0, v0 = 2, 2, 0, 0, 0 - k1, k2, p1, p2 = 0, 0, 0, 0 - xi = 1 - stereographic = gtsam.Cal3Unified(fx, fy, s, u0, v0, k1, k2, p1, p2, xi) + """Spatial stereographic camera projection""" + x, y, z = self.obj_point + u, v = self.img_point + r = np.linalg.norm(self.obj_point) pose = gtsam.Pose3() - camera = gtsam.PinholeCameraCal3Unified(pose, stereographic) - pt1 = camera.Project(objPoint) + camera = gtsam.PinholeCameraCal3Unified(pose, self.stereographic) + pt1 = camera.Project(self.obj_point) self.gtsamAssertEquals(pt1, np.array([x/z, y/z])) - pt2 = camera.project(objPoint) - self.gtsamAssertEquals(pt2, np.array([u, v])) - obj1 = camera.backproject([u, v], z) - self.gtsamAssertEquals(obj1, np.array([x, y, z])) - r1 = camera.range(np.array([x, y, z])) + pt2 = camera.project(self.obj_point) + self.gtsamAssertEquals(pt2, self.img_point) + obj1 = camera.backproject(self.img_point, z) + self.gtsamAssertEquals(obj1, self.obj_point) + r1 = camera.range(self.obj_point) self.assertEqual(r1, r) def test_generic_factor(self): - "Evaluate residual using pose and point as state variables" - fx, fy, s, u0, v0 = 2, 2, 0, 0, 0 - k1, k2, p1, p2 = 0, 0, 0, 0 - xi = 1 - objPoint = np.array([1, 0, 1]) - r = np.linalg.norm(objPoint) - imgPoint = np.array([2/(1+r), 0]) + """Evaluate residual using pose and point as state variables""" graph = gtsam.NonlinearFactorGraph() state = gtsam.Values() - measured = imgPoint - noiseModel = gtsam.noiseModel.Isotropic.Sigma(2, 1) - poseKey = gtsam.symbol_shorthand.P(0) - pointKey = gtsam.symbol_shorthand.L(0) - k = gtsam.Cal3Unified(fx, fy, s, u0, v0, k1, k2, p1, p2, xi) - state.insert_pose3(poseKey, gtsam.Pose3()) - state.insert_point3(pointKey, gtsam.Point3(objPoint)) - factor = gtsam.GenericProjectionFactorCal3Unified(measured, noiseModel, poseKey, pointKey, k) + measured = self.img_point + noise_model = gtsam.noiseModel.Isotropic.Sigma(2, 1) + pose_key, point_key = P(0), L(0) + k = self.stereographic + state.insert_pose3(pose_key, gtsam.Pose3()) + state.insert_point3(point_key, self.obj_point) + factor = gtsam.GenericProjectionFactorCal3Unified(measured, noise_model, pose_key, point_key, k) graph.add(factor) score = graph.error(state) self.assertAlmostEqual(score, 0) def test_sfm_factor2(self): - "Evaluate residual with camera, pose and point as state variables" - fx, fy, s, u0, v0 = 2, 2, 0, 0, 0 - k1, k2, p1, p2 = 0, 0, 0, 0 - xi = 1 - objPoint = np.array([1, 0, 1]) - r = np.linalg.norm(objPoint) - imgPoint = np.array([2/(1+r), 0]) + """Evaluate residual with camera, pose and point as state variables""" + r = np.linalg.norm(self.obj_point) graph = gtsam.NonlinearFactorGraph() state = gtsam.Values() - measured = imgPoint - noiseModel = gtsam.noiseModel.Isotropic.Sigma(2, 1) - cameraKey = gtsam.symbol_shorthand.K(0) - poseKey = gtsam.symbol_shorthand.P(0) - landmarkKey = gtsam.symbol_shorthand.L(0) - k = gtsam.Cal3Unified(fx, fy, s, u0, v0, k1, k2, p1, p2, xi) - state.insert_cal3unified(cameraKey, k) - state.insert_pose3(poseKey, gtsam.Pose3()) - state.insert_point3(landmarkKey, gtsam.Point3(objPoint)) - factor = gtsam.GeneralSFMFactor2Cal3Unified(measured, noiseModel, poseKey, landmarkKey, cameraKey) + measured = self.img_point + noise_model = gtsam.noiseModel.Isotropic.Sigma(2, 1) + camera_key, pose_key, landmark_key = K(0), P(0), L(0) + k = self.stereographic + state.insert_cal3unified(camera_key, k) + state.insert_pose3(pose_key, gtsam.Pose3()) + state.insert_point3(landmark_key, self.obj_point) + factor = gtsam.GeneralSFMFactor2Cal3Unified(measured, noise_model, pose_key, landmark_key, camera_key) graph.add(factor) score = graph.error(state) self.assertAlmostEqual(score, 0)