2 GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
3 Atlanta, Georgia 30332-0415
6 See LICENSE for the license information
9 Authors: Frank Dellaert & Fan Jiang (Python) & Sushmita Warrier & John Lambert
13 from typing
import Iterable, List, Optional, Tuple, Union
19 from gtsam
import (Cal3_S2, Cal3Bundler, CameraSetCal3_S2,
20 CameraSetCal3Bundler, PinholeCameraCal3_S2,
21 PinholeCameraCal3Bundler, Point2, Point3, Pose3, Rot3,
22 TriangulationParameters, TriangulationResult)
24 UPRIGHT = Rot3.Ypr(-np.pi / 2, 0.0, -np.pi / 2)
28 """Tests for triangulation with shared and individual calibrations"""
31 """Set up two camera poses"""
45 calibration: Union[Cal3Bundler, Cal3_S2],
46 camera_model: Union[PinholeCameraCal3Bundler, PinholeCameraCal3_S2],
47 cal_params: Iterable[Iterable[Union[int, float]]],
48 camera_set: Optional[Union[CameraSetCal3Bundler,
49 CameraSetCal3_S2]] =
None,
50 ) -> Tuple[List[Point2], Union[CameraSetCal3Bundler, CameraSetCal3_S2,
51 List[Cal3Bundler], List[Cal3_S2]]]:
53 Generate vector of measurements for given calibration and camera model.
56 calibration: Camera calibration e.g. Cal3_S2
57 camera_model: Camera model e.g. PinholeCameraCal3_S2
58 cal_params: Iterable of camera parameters for `calibration` e.g. [K1, K2]
59 camera_set: Cameraset object (for individual calibrations)
62 list of measurements and list/CameraSet object for cameras
64 if camera_set
is not None:
65 cameras = camera_set()
70 for k, pose
in zip(cal_params, self.
poses):
72 camera = camera_model(pose, K)
73 cameras.append(camera)
75 measurements.append(z)
77 return measurements, cameras
80 """Tests triangulation with shared Cal3_S2 calibration"""
82 sharedCal = (1500, 1200, 0, 640, 480)
86 camera_model=PinholeCameraCal3_S2,
87 cal_params=(sharedCal, sharedCal))
97 measurements_noisy = []
98 measurements_noisy.append(measurements[0] - np.array([0.1, 0.5]))
99 measurements_noisy.append(measurements[1] - np.array([-0.2, 0.3]))
110 """Tests triangulation with individual Cal3_S2 calibrations"""
112 K1 = (1500, 1200, 0, 640, 480)
113 K2 = (1600, 1300, 0, 650, 440)
117 camera_model=PinholeCameraCal3_S2,
119 camera_set=CameraSetCal3_S2)
128 """Tests triangulation with individual Cal3Bundler calibrations"""
130 K1 = (1500, 0, 0, 640, 480)
131 K2 = (1600, 0, 0, 650, 440)
134 calibration=Cal3Bundler,
135 camera_model=PinholeCameraCal3Bundler,
137 camera_set=CameraSetCal3Bundler)
146 """Ensure triangulation with a robust model works."""
147 sharedCal =
Cal3_S2(1500, 1200, 0, 640, 480)
150 landmark =
Point3(5, 0.5, 1.2)
154 pose3 = pose1 *
Pose3(Rot3.Ypr(0.1, 0.2, 0.1),
Point3(0.1, -2, -0.1))
160 z1: Point2 = camera1.project(landmark)
161 z2: Point2 = camera2.project(landmark)
162 z3: Point2 = camera3.project(landmark)
164 poses = [pose1, pose2, pose3]
165 measurements = [z1, z2, z3]
173 self.assertTrue(np.allclose(landmark, actual, atol=1e-2))
176 measurements[0] +=
Point2(100, 120)
185 self.assertTrue(np.linalg.norm(landmark - actual2) >= 0.2)
186 self.assertTrue(np.linalg.norm(landmark - actual2) <= 0.5)
195 self.assertTrue(np.allclose(actual2, actual3, atol=0.1))
208 self.assertTrue(np.allclose(landmark, actual4, atol=0.05))
211 """Check safe triangulation function."""
212 pose1, pose2 = self.
poses
214 K1 =
Cal3_S2(1500, 1200, 0, 640, 480)
219 K2 =
Cal3_S2(1600, 1300, 0, 650, 440)
227 cameras.append(camera1)
228 cameras.append(camera2)
231 measurements.append(z1)
232 measurements.append(z2)
234 landmarkDistanceThreshold = 10
238 cameras, measurements, params)
240 self.assertTrue(actual.valid())
242 landmarkDistanceThreshold = 4
244 landmarkDistanceThreshold)
246 self.assertTrue(actual.farPoint())
250 pose3 = pose1 *
Pose3(Rot3.Ypr(0.1, 0.2, 0.1),
Point3(0.1, -2, -.1))
251 K3 =
Cal3_S2(700, 500, 0, 640, 480)
255 cameras.append(camera3)
256 measurements.append(z3 +
Point2(10, -10))
258 landmarkDistanceThreshold = 10
259 outlierThreshold = 100
261 landmarkDistanceThreshold,
264 self.assertTrue(actual.valid())
269 landmarkDistanceThreshold,
272 self.assertTrue(actual.outlier())
275 if __name__ ==
"__main__":