test_Triangulation.py
Go to the documentation of this file.
1 """
2 GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
3 Atlanta, Georgia 30332-0415
4 All Rights Reserved
5 
6 See LICENSE for the license information
7 
8 Test Triangulation
9 Authors: Frank Dellaert & Fan Jiang (Python) & Sushmita Warrier & John Lambert
10 """
11 # pylint: disable=no-name-in-module, invalid-name, no-member
12 import unittest
13 from typing import Iterable, List, Optional, Tuple, Union
14 
15 import numpy as np
16 from gtsam.utils.test_case import GtsamTestCase
17 
18 import gtsam
19 from gtsam import (Cal3_S2, Cal3Bundler, CameraSetCal3_S2,
20  CameraSetCal3Bundler, PinholeCameraCal3_S2,
21  PinholeCameraCal3Bundler, Point2, Point3, Pose3, Rot3,
22  TriangulationParameters, TriangulationResult)
23 
24 UPRIGHT = Rot3.Ypr(-np.pi / 2, 0.0, -np.pi / 2)
25 
26 
28  """Tests for triangulation with shared and individual calibrations"""
29 
30  def setUp(self):
31  """Set up two camera poses"""
32  # Looking along X-axis, 1 meter above ground plane (x-y)
33  pose1 = Pose3(UPRIGHT, Point3(0, 0, 1))
34 
35  # create second camera 1 meter to the right of first camera
36  pose2 = pose1.compose(Pose3(Rot3(), Point3(1, 0, 0)))
37  # twoPoses
38  self.poses = [pose1, pose2]
39 
40  # landmark ~5 meters infront of camera
41  self.landmark = Point3(5, 0.5, 1.2)
42 
44  self,
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]]]:
52  """
53  Generate vector of measurements for given calibration and camera model.
54 
55  Args:
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)
60 
61  Returns:
62  list of measurements and list/CameraSet object for cameras
63  """
64  if camera_set is not None:
65  cameras = camera_set()
66  else:
67  cameras = []
68  measurements = []
69 
70  for k, pose in zip(cal_params, self.poses):
71  K = calibration(*k)
72  camera = camera_model(pose, K)
73  cameras.append(camera)
74  z = camera.project(self.landmark)
75  measurements.append(z)
76 
77  return measurements, cameras
78 
79  def test_TriangulationExample(self) -> None:
80  """Tests triangulation with shared Cal3_S2 calibration"""
81  # Some common constants
82  sharedCal = (1500, 1200, 0, 640, 480)
83 
84  measurements, _ = self.generate_measurements(
85  calibration=Cal3_S2,
86  camera_model=PinholeCameraCal3_S2,
87  cal_params=(sharedCal, sharedCal))
88 
89  triangulated_landmark = gtsam.triangulatePoint3(self.poses,
90  Cal3_S2(sharedCal),
91  measurements,
92  rank_tol=1e-9,
93  optimize=True)
94  self.gtsamAssertEquals(self.landmark, triangulated_landmark, 1e-9)
95 
96  # Add some noise and try again: result should be ~ (4.995, 0.499167, 1.19814)
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]))
100 
101  triangulated_landmark = gtsam.triangulatePoint3(self.poses,
102  Cal3_S2(sharedCal),
103  measurements_noisy,
104  rank_tol=1e-9,
105  optimize=True)
106 
107  self.gtsamAssertEquals(self.landmark, triangulated_landmark, 1e-2)
108 
109  def test_distinct_Ks(self) -> None:
110  """Tests triangulation with individual Cal3_S2 calibrations"""
111  # two camera parameters
112  K1 = (1500, 1200, 0, 640, 480)
113  K2 = (1600, 1300, 0, 650, 440)
114 
115  measurements, cameras = self.generate_measurements(
116  calibration=Cal3_S2,
117  camera_model=PinholeCameraCal3_S2,
118  cal_params=(K1, K2),
119  camera_set=CameraSetCal3_S2)
120 
121  triangulated_landmark = gtsam.triangulatePoint3(cameras,
122  measurements,
123  rank_tol=1e-9,
124  optimize=True)
125  self.gtsamAssertEquals(self.landmark, triangulated_landmark, 1e-9)
126 
127  def test_distinct_Ks_Bundler(self) -> None:
128  """Tests triangulation with individual Cal3Bundler calibrations"""
129  # two camera parameters
130  K1 = (1500, 0, 0, 640, 480)
131  K2 = (1600, 0, 0, 650, 440)
132 
133  measurements, cameras = self.generate_measurements(
134  calibration=Cal3Bundler,
135  camera_model=PinholeCameraCal3Bundler,
136  cal_params=(K1, K2),
137  camera_set=CameraSetCal3Bundler)
138 
139  triangulated_landmark = gtsam.triangulatePoint3(cameras,
140  measurements,
141  rank_tol=1e-9,
142  optimize=True)
143  self.gtsamAssertEquals(self.landmark, triangulated_landmark, 1e-9)
144 
146  """Ensure triangulation with a robust model works."""
147  sharedCal = Cal3_S2(1500, 1200, 0, 640, 480)
148 
149  # landmark ~5 meters infront of camera
150  landmark = Point3(5, 0.5, 1.2)
151 
152  pose1 = Pose3(UPRIGHT, Point3(0, 0, 1))
153  pose2 = pose1 * Pose3(Rot3(), Point3(1, 0, 0))
154  pose3 = pose1 * Pose3(Rot3.Ypr(0.1, 0.2, 0.1), Point3(0.1, -2, -0.1))
155 
156  camera1 = PinholeCameraCal3_S2(pose1, sharedCal)
157  camera2 = PinholeCameraCal3_S2(pose2, sharedCal)
158  camera3 = PinholeCameraCal3_S2(pose3, sharedCal)
159 
160  z1: Point2 = camera1.project(landmark)
161  z2: Point2 = camera2.project(landmark)
162  z3: Point2 = camera3.project(landmark)
163 
164  poses = [pose1, pose2, pose3]
165  measurements = [z1, z2, z3]
166 
167  # noise free, so should give exactly the landmark
168  actual = gtsam.triangulatePoint3(poses,
169  sharedCal,
170  measurements,
171  rank_tol=1e-9,
172  optimize=False)
173  self.assertTrue(np.allclose(landmark, actual, atol=1e-2))
174 
175  # Add outlier
176  measurements[0] += Point2(100, 120) # very large pixel noise!
177 
178  # now estimate does not match landmark
179  actual2 = gtsam.triangulatePoint3(poses,
180  sharedCal,
181  measurements,
182  rank_tol=1e-9,
183  optimize=False)
184  # DLT is surprisingly robust, but still off (actual error is around 0.26m)
185  self.assertTrue(np.linalg.norm(landmark - actual2) >= 0.2)
186  self.assertTrue(np.linalg.norm(landmark - actual2) <= 0.5)
187 
188  # Again with nonlinear optimization
189  actual3 = gtsam.triangulatePoint3(poses,
190  sharedCal,
191  measurements,
192  rank_tol=1e-9,
193  optimize=True)
194  # result from nonlinear (but non-robust optimization) is close to DLT and still off
195  self.assertTrue(np.allclose(actual2, actual3, atol=0.1))
196 
197  # Again with nonlinear optimization, this time with robust loss
201  actual4 = gtsam.triangulatePoint3(poses,
202  sharedCal,
203  measurements,
204  rank_tol=1e-9,
205  optimize=True,
206  model=model)
207  # using the Huber loss we now have a quite small error!! nice!
208  self.assertTrue(np.allclose(landmark, actual4, atol=0.05))
209 
211  """Check safe triangulation function."""
212  pose1, pose2 = self.poses
213 
214  K1 = Cal3_S2(1500, 1200, 0, 640, 480)
215  # create first camera. Looking along X-axis, 1 meter above ground plane (x-y)
216  camera1 = PinholeCameraCal3_S2(pose1, K1)
217 
218  # create second camera 1 meter to the right of first camera
219  K2 = Cal3_S2(1600, 1300, 0, 650, 440)
220  camera2 = PinholeCameraCal3_S2(pose2, K2)
221 
222  # 1. Project two landmarks into two cameras and triangulate
223  z1 = camera1.project(self.landmark)
224  z2 = camera2.project(self.landmark)
225 
226  cameras = CameraSetCal3_S2()
227  cameras.append(camera1)
228  cameras.append(camera2)
229 
230  measurements = []
231  measurements.append(z1)
232  measurements.append(z2)
233 
234  landmarkDistanceThreshold = 10 # landmark is closer than that
235  # all default except landmarkDistanceThreshold:
236  params = TriangulationParameters(1.0, False, landmarkDistanceThreshold)
237  actual: TriangulationResult = gtsam.triangulateSafe(
238  cameras, measurements, params)
239  self.gtsamAssertEquals(actual.get(), self.landmark, 1e-2)
240  self.assertTrue(actual.valid())
241 
242  landmarkDistanceThreshold = 4 # landmark is farther than that
243  params2 = TriangulationParameters(1.0, False,
244  landmarkDistanceThreshold)
245  actual = gtsam.triangulateSafe(cameras, measurements, params2)
246  self.assertTrue(actual.farPoint())
247 
248  # 3. Add a slightly rotated third camera above with a wrong measurement
249  # (OUTLIER)
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)
252  camera3 = PinholeCameraCal3_S2(pose3, K3)
253  z3 = camera3.project(self.landmark)
254 
255  cameras.append(camera3)
256  measurements.append(z3 + Point2(10, -10))
257 
258  landmarkDistanceThreshold = 10 # landmark is closer than that
259  outlierThreshold = 100 # loose, the outlier is going to pass
260  params3 = TriangulationParameters(1.0, False,
261  landmarkDistanceThreshold,
262  outlierThreshold)
263  actual = gtsam.triangulateSafe(cameras, measurements, params3)
264  self.assertTrue(actual.valid())
265 
266  # now set stricter threshold for outlier rejection
267  outlierThreshold = 5 # tighter, the outlier is not going to pass
268  params4 = TriangulationParameters(1.0, False,
269  landmarkDistanceThreshold,
270  outlierThreshold)
271  actual = gtsam.triangulateSafe(cameras, measurements, params4)
272  self.assertTrue(actual.outlier())
273 
274 
275 if __name__ == "__main__":
276  unittest.main()
gtsam::CameraSetCal3_S2
CameraSet< PinholeCamera< Cal3_S2 > > CameraSetCal3_S2
Definition: triangulation.h:758
gtsam::noiseModel::mEstimator::Huber::Create
static shared_ptr Create(double k, const ReweightScheme reweight=Block)
Definition: LossFunctions.cpp:203
test_Triangulation.TestTriangulationExample
Definition: test_Triangulation.py:27
gtsam::noiseModel::Robust::Create
static shared_ptr Create(const RobustModel::shared_ptr &robust, const NoiseModel::shared_ptr noise)
Definition: NoiseModel.cpp:740
gtsam::utils.test_case.GtsamTestCase.gtsamAssertEquals
def gtsamAssertEquals(self, actual, expected, tol=1e-9)
Definition: test_case.py:18
test_Triangulation.TestTriangulationExample.poses
poses
Definition: test_Triangulation.py:38
gtsam::TriangulationParameters
Definition: triangulation.h:558
test_Triangulation.TestTriangulationExample.test_triangulation_robust_three_poses
None test_triangulation_robust_three_poses(self)
Definition: test_Triangulation.py:145
gtsam::Rot3
Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
Definition: Rot3.h:58
gtsam::Pose3
Definition: Pose3.h:37
test_Triangulation.TestTriangulationExample.test_distinct_Ks_Bundler
None test_distinct_Ks_Bundler(self)
Definition: test_Triangulation.py:127
gtsam::noiseModel::Unit::Create
static shared_ptr Create(size_t dim)
Definition: NoiseModel.h:631
gtsam::PinholeCameraCal3_S2
gtsam::PinholeCamera< gtsam::Cal3_S2 > PinholeCameraCal3_S2
Definition: SimpleCamera.h:34
test_Triangulation.TestTriangulationExample.test_outliers_and_far_landmarks
None test_outliers_and_far_landmarks(self)
Definition: test_Triangulation.py:210
gtsam::triangulateSafe
TriangulationResult triangulateSafe(const CameraSet< CAMERA > &cameras, const typename CAMERA::MeasurementVector &measured, const TriangulationParameters &params)
triangulateSafe: extensive checking of the outcome
Definition: triangulation.h:698
gtsam::utils.test_case
Definition: test_case.py:1
gtsam::Point2
Vector2 Point2
Definition: Point2.h:32
gtsam::triangulatePoint3
Point3 triangulatePoint3(const CameraSet< PinholeCamera< CALIBRATION >> &cameras, const Point2Vector &measurements, double rank_tol=1e-9, bool optimize=false, const SharedNoiseModel &model=nullptr, const bool useLOST=false)
Pinhole-specific version.
Definition: triangulation.h:549
test_Triangulation.TestTriangulationExample.test_distinct_Ks
None test_distinct_Ks(self)
Definition: test_Triangulation.py:109
test_Triangulation.TestTriangulationExample.test_TriangulationExample
None test_TriangulationExample(self)
Definition: test_Triangulation.py:79
gtsam::utils.test_case.GtsamTestCase
Definition: test_case.py:15
gtsam::Cal3_S2
The most common 5DOF 3D->2D calibration.
Definition: Cal3_S2.h:34
gtsam::Point3
Vector3 Point3
Definition: Point3.h:38
test_Triangulation.TestTriangulationExample.landmark
landmark
Definition: test_Triangulation.py:41
test_Triangulation.TestTriangulationExample.setUp
def setUp(self)
Definition: test_Triangulation.py:30
test_Triangulation.TestTriangulationExample.generate_measurements
Tuple[List[Point2], Union[CameraSetCal3Bundler, CameraSetCal3_S2, List[Cal3Bundler], List[Cal3_S2]]] generate_measurements(self, Union[Cal3Bundler, Cal3_S2] calibration, Union[PinholeCameraCal3Bundler, PinholeCameraCal3_S2] camera_model, Iterable[Iterable[Union[int, float]]] cal_params, Optional[Union[CameraSetCal3Bundler, CameraSetCal3_S2]] camera_set=None)
Definition: test_Triangulation.py:43


gtsam
Author(s):
autogenerated on Sat Nov 16 2024 04:07:09