12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

moved camera_set to generate_measurements

release/4.3a0
Sushmita 2020-12-05 13:16:13 -05:00
parent 6a4a40de44
commit 858884f1e7
1 changed files with 11 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

28
python/gtsam/tests/test_Triangulation.py
View File

@ -38,18 +38,22 @@ class TestVisualISAMExample(GtsamTestCase):
# landmark ~5 meters infront of camera # landmark ~5 meters infront of camera
self.landmark = Point3(5, 0.5, 1.2) self.landmark = Point3(5, 0.5, 1.2)
def generate_measurements(self, calibration, camera_model, *cal_params): def generate_measurements(self, calibration, camera_model, camera_set, *cal_params):
""" Generate vector of measurements for given calibration and camera model """ Generate vector of measurements for given calibration and camera model
Args: Args:
calibration: Camera calibration e.g. Cal3_S2 calibration: Camera calibration e.g. Cal3_S2
camera_model: Camera model e.g. PinholeCameraCal3_S2 camera_model: Camera model e.g. PinholeCameraCal3_S2
cal_params: (list of) camera parameters e.g. K1, K2 cal_params: (list of) camera parameters e.g. K1, K2
camera_set: Cameraset object (for individual calibrations)
Returns: Returns:
list of measurements and cameras list of measurements and list/CameraSet object for cameras
""" """
cameras = [] if camera_set is not None:
cameras = camera_set()
else:
cameras = []
measurements = Point2Vector() measurements = Point2Vector()
for k, pose in zip(cal_params, self.poses): for k, pose in zip(cal_params, self.poses):
K = calibration(*k) K = calibration(*k)
camera = camera_model(pose, K) camera = camera_model(pose, K)
@ -65,7 +69,7 @@ class TestVisualISAMExample(GtsamTestCase):
# Some common constants # Some common constants
sharedCal = (1500, 1200, 0, 640, 480) sharedCal = (1500, 1200, 0, 640, 480)
measurements, _ = self.generate_measurements(Cal3_S2, PinholeCameraCal3_S2, 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) self.gtsamAssertEquals(self.landmark, triangulated_landmark, 1e-9)
@ -84,12 +88,7 @@ class TestVisualISAMExample(GtsamTestCase):
K1 = (1500, 1200, 0, 640, 480) K1 = (1500, 1200, 0, 640, 480)
K2 = (1600, 1300, 0, 650, 440) K2 = (1600, 1300, 0, 650, 440)
measurements, camera_list = self.generate_measurements(Cal3_S2, PinholeCameraCal3_S2, K1, K2) measurements, cameras = self.generate_measurements(Cal3_S2, PinholeCameraCal3_S2, CameraSetCal3_S2, K1, K2)
# convert list to CameraSet object
cameras = CameraSetCal3_S2()
for camera in camera_list:
cameras.append(camera)
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) self.gtsamAssertEquals(self.landmark, triangulated_landmark, 1e-9)
@ -100,12 +99,7 @@ class TestVisualISAMExample(GtsamTestCase):
K1 = (1500, 0, 0, 640, 480) K1 = (1500, 0, 0, 640, 480)
K2 = (1600, 0, 0, 650, 440) K2 = (1600, 0, 0, 650, 440)
measurements, camera_list = self.generate_measurements(Cal3Bundler, PinholeCameraCal3Bundler, K1, K2) measurements, cameras = self.generate_measurements(Cal3Bundler, PinholeCameraCal3Bundler, CameraSetCal3Bundler, K1, K2)
# convert list to CameraSet object
cameras = CameraSetCal3Bundler()
for camera in camera_list:
cameras.append(camera)
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) self.gtsamAssertEquals(self.landmark, triangulated_landmark, 1e-9)