diff --git a/python/gtsam/tests/test_Triangulation.py b/python/gtsam/tests/test_Triangulation.py index 574452afa..b54c05ce1 100644 --- a/python/gtsam/tests/test_Triangulation.py +++ b/python/gtsam/tests/test_Triangulation.py @@ -10,16 +10,16 @@ Author: Frank Dellaert & Fan Jiang (Python) """ import unittest -import numpy as np - import gtsam as g +import numpy as np +from gtsam import (Cal3_S2, Cal3Bundler, CameraSetCal3_S2, + CameraSetCal3Bundler, PinholeCameraCal3_S2, + PinholeCameraCal3Bundler, Point2Vector, Point3, Pose3, + Pose3Vector, Rot3, triangulatePoint3) from gtsam.utils.test_case import GtsamTestCase -from gtsam import Cal3_S2, Cal3Bundler, Rot3, Pose3, \ - PinholeCameraCal3_S2, PinholeCameraCal3Bundler, Point3, \ - Point2Vector, Pose3Vector, triangulatePoint3, \ - CameraSetCal3_S2, CameraSetCal3Bundler from numpy.core.records import array + class TestVisualISAMExample(GtsamTestCase): """ Tests for triangulation with shared and individual calibrations """ @@ -38,7 +38,7 @@ class TestVisualISAMExample(GtsamTestCase): # landmark ~5 meters infront of camera self.landmark = Point3(5, 0.5, 1.2) - + def generate_measurements(self, calibration, camera_model, camera_set, *cal_params): """ Generate vector of measurements for given calibration and camera model Args: @@ -53,26 +53,27 @@ class TestVisualISAMExample(GtsamTestCase): cameras = camera_set() else: cameras = [] - measurements = Point2Vector() + measurements = Point2Vector() for k, pose in zip(cal_params, self.poses): K = calibration(*k) camera = camera_model(pose, K) cameras.append(camera) z = camera.project(self.landmark) - measurements.append(z) + measurements.append(z) return measurements, cameras - def test_TriangulationExample(self): """ Tests triangulation with shared Cal3_S2 calibration""" # Some common constants - sharedCal = (1500, 1200, 0, 640, 480) + sharedCal = (1500, 1200, 0, 640, 480) - measurements, _ = self.generate_measurements(Cal3_S2, PinholeCameraCal3_S2, None, sharedCal, sharedCal) + measurements, _ = self.generate_measurements( + Cal3_S2, PinholeCameraCal3_S2, None, sharedCal, sharedCal) - triangulated_landmark = triangulatePoint3(self.poses, Cal3_S2(sharedCal), measurements, rank_tol=1e-9, optimize=True) + triangulated_landmark = triangulatePoint3(self.poses, Cal3_S2( + sharedCal), measurements, rank_tol=1e-9, optimize=True) self.gtsamAssertEquals(self.landmark, triangulated_landmark, 1e-9) # Add some noise and try again: result should be ~ (4.995, 0.499167, 1.19814) @@ -80,7 +81,8 @@ class TestVisualISAMExample(GtsamTestCase): measurements_noisy.append(measurements[0] - np.array([0.1, 0.5])) measurements_noisy.append(measurements[1] - np.array([-0.2, 0.3])) - triangulated_landmark = triangulatePoint3(self.poses, Cal3_S2(sharedCal), measurements_noisy, rank_tol=1e-9, optimize=True) + triangulated_landmark = triangulatePoint3(self.poses, Cal3_S2( + sharedCal), measurements_noisy, rank_tol=1e-9, optimize=True) self.gtsamAssertEquals(self.landmark, triangulated_landmark, 1e-2) def test_distinct_Ks(self): @@ -89,9 +91,11 @@ class TestVisualISAMExample(GtsamTestCase): K1 = (1500, 1200, 0, 640, 480) K2 = (1600, 1300, 0, 650, 440) - measurements, cameras = self.generate_measurements(Cal3_S2, PinholeCameraCal3_S2, CameraSetCal3_S2, K1, K2) + measurements, cameras = self.generate_measurements( + Cal3_S2, PinholeCameraCal3_S2, CameraSetCal3_S2, K1, K2) - triangulated_landmark = triangulatePoint3(cameras, measurements, rank_tol=1e-9, optimize=True) + triangulated_landmark = triangulatePoint3( + cameras, measurements, rank_tol=1e-9, optimize=True) self.gtsamAssertEquals(self.landmark, triangulated_landmark, 1e-9) def test_distinct_Ks_Bundler(self): @@ -100,10 +104,13 @@ class TestVisualISAMExample(GtsamTestCase): K1 = (1500, 0, 0, 640, 480) K2 = (1600, 0, 0, 650, 440) - measurements, cameras = self.generate_measurements(Cal3Bundler, PinholeCameraCal3Bundler, CameraSetCal3Bundler, K1, K2) + measurements, cameras = self.generate_measurements( + Cal3Bundler, PinholeCameraCal3Bundler, CameraSetCal3Bundler, K1, K2) + + triangulated_landmark = triangulatePoint3( + cameras, measurements, rank_tol=1e-9, optimize=True) + self.gtsamAssertEquals(self.landmark, triangulated_landmark, 1e-9) - triangulated_landmark = triangulatePoint3(cameras, measurements, rank_tol=1e-9, optimize=True) - self.gtsamAssertEquals(self.landmark, triangulated_landmark, 1e-9) if __name__ == "__main__": unittest.main()