test_backwards_compatibility.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 Unit tests to ensure backwards compatibility of the Python wrapper.
9 Author: Varun Agrawal
10 """
11 import unittest
12 from typing import Iterable, List, Optional, Tuple, Union
13 
14 import numpy as np
15 from gtsam.gtsfm import Keypoints
16 from gtsam.symbol_shorthand import X
17 from gtsam.utils.test_case import GtsamTestCase
18 
19 import gtsam
20 from gtsam import (BetweenFactorPose2, Cal3_S2, Cal3Bundler, CameraSetCal3_S2,
21  CameraSetCal3Bundler, IndexPair, LevenbergMarquardtParams,
22  PinholeCameraCal3_S2, PinholeCameraCal3Bundler, Point2,
23  Point2Pairs, Point3, Pose2, Pose2Pairs, Pose3, Rot2, Rot3,
24  SfmTrack2d, ShonanAveraging2, ShonanAveragingParameters2,
25  Similarity2, Similarity3, TriangulationParameters,
26  TriangulationResult)
27 
28 UPRIGHT = Rot3.Ypr(-np.pi / 2, 0.0, -np.pi / 2)
29 
30 
32  """Tests for backwards compatibility of the Python wrapper."""
33 
34  def setUp(self):
35  """Setup test fixtures"""
36  p1 = [-1.0, 0.0, -1.0]
37  p2 = [1.0, 0.0, -1.0]
38  q1 = Rot3(1.0, 0.0, 0.0, 0.0)
39  q2 = Rot3(1.0, 0.0, 0.0, 0.0)
40  pose1 = Pose3(q1, p1)
41  pose2 = Pose3(q2, p2)
42  camera1 = gtsam.PinholeCameraCal3Fisheye(pose1)
43  camera2 = gtsam.PinholeCameraCal3Fisheye(pose2)
44  self.origin = np.array([0.0, 0.0, 0.0])
45  self.poses = gtsam.Pose3Vector([pose1, pose2])
46 
47  self.fisheye_cameras = gtsam.CameraSetCal3Fisheye([camera1, camera2])
49  [k.project(self.origin) for k in self.fisheye_cameras])
50 
51  fx, fy, s, u0, v0 = 2, 2, 0, 0, 0
52  k1, k2, p1, p2 = 0, 0, 0, 0
53  xi = 1
54  self.stereographic = gtsam.Cal3Unified(fx, fy, s, u0, v0, k1, k2, p1,
55  p2, xi)
56  camera1 = gtsam.PinholeCameraCal3Unified(pose1, self.stereographic)
57  camera2 = gtsam.PinholeCameraCal3Unified(pose2, self.stereographic)
58  self.unified_cameras = gtsam.CameraSetCal3Unified([camera1, camera2])
60  [k.project(self.origin) for k in self.unified_cameras])
61 
62 
64  pose1 = Pose3(UPRIGHT, Point3(0, 0, 1))
65 
66  # create second camera 1 meter to the right of first camera
67  pose2 = pose1.compose(Pose3(Rot3(), Point3(1, 0, 0)))
68  # twoPoses
70  self.triangulation_poses.append(pose1)
71  self.triangulation_poses.append(pose2)
72 
73  # landmark ~5 meters infront of camera
74  self.landmark = Point3(5, 0.5, 1.2)
75 
77  """
78  Estimate spatial point from image measurements using
79  rectification from a Cal3Fisheye camera model.
80  """
81  rectified = gtsam.Point2Vector([
82  k.calibration().calibrate(pt)
83  for k, pt in zip(self.fisheye_cameras, self.fisheye_measurements)
84  ])
85  shared_cal = gtsam.Cal3_S2()
86  triangulated = gtsam.triangulatePoint3(self.poses,
87  shared_cal,
88  rectified,
89  rank_tol=1e-9,
90  optimize=False)
91  self.gtsamAssertEquals(triangulated, self.origin)
92 
94  """
95  Estimate spatial point from image measurements using
96  rectification from a Cal3Unified camera model.
97  """
98  rectified = gtsam.Point2Vector([
99  k.calibration().calibrate(pt)
100  for k, pt in zip(self.unified_cameras, self.unified_measurements)
101  ])
102  shared_cal = gtsam.Cal3_S2()
103  triangulated = gtsam.triangulatePoint3(self.poses,
104  shared_cal,
105  rectified,
106  rank_tol=1e-9,
107  optimize=False)
108  self.gtsamAssertEquals(triangulated, self.origin)
109 
110  def test_track_generation(self) -> None:
111  """Ensures that DSF generates three tracks from measurements
112  in 3 images (H=200,W=400)."""
113  kps_i0 = Keypoints(np.array([[10.0, 20], [30, 40]]))
114  kps_i1 = Keypoints(np.array([[50.0, 60], [70, 80], [90, 100]]))
115  kps_i2 = Keypoints(np.array([[110.0, 120], [130, 140]]))
116 
117  keypoints_list = gtsam.KeypointsVector()
118  keypoints_list.append(kps_i0)
119  keypoints_list.append(kps_i1)
120  keypoints_list.append(kps_i2)
121 
122  # For each image pair (i1,i2), we provide a (K,2) matrix
123  # of corresponding image indices (k1,k2).
124  matches_dict = gtsam.MatchIndicesMap()
125  matches_dict[IndexPair(0, 1)] = np.array([[0, 0], [1, 1]])
126  matches_dict[IndexPair(1, 2)] = np.array([[2, 0], [1, 1]])
127 
129  matches_dict,
130  keypoints_list,
131  verbose=False,
132  )
133  assert len(tracks) == 3
134 
135  # Verify track 0.
136  track0 = tracks[0]
137  assert track0.numberMeasurements() == 2
138  np.testing.assert_allclose(track0.measurements[0][1], Point2(10, 20))
139  np.testing.assert_allclose(track0.measurements[1][1], Point2(50, 60))
140  assert track0.measurements[0][0] == 0
141  assert track0.measurements[1][0] == 1
142  np.testing.assert_allclose(
143  track0.measurementMatrix(),
144  [
145  [10, 20],
146  [50, 60],
147  ],
148  )
149  np.testing.assert_allclose(track0.indexVector(), [0, 1])
150 
151  # Verify track 1.
152  track1 = tracks[1]
153  np.testing.assert_allclose(
154  track1.measurementMatrix(),
155  [
156  [30, 40],
157  [70, 80],
158  [130, 140],
159  ],
160  )
161  np.testing.assert_allclose(track1.indexVector(), [0, 1, 2])
162 
163  # Verify track 2.
164  track2 = tracks[2]
165  np.testing.assert_allclose(
166  track2.measurementMatrix(),
167  [
168  [90, 100],
169  [110, 120],
170  ],
171  )
172  np.testing.assert_allclose(track2.indexVector(), [1, 2])
173 
174  def test_sfm_track_2d_constructor(self) -> None:
175  """Test construction of 2D SfM track."""
176  measurements = gtsam.SfmMeasurementVector()
177  measurements.append((0, Point2(10, 20)))
178  track = SfmTrack2d(measurements=measurements)
179  track.measurement(0)
180  assert track.numberMeasurements() == 1
181 
183  '''
184  Simple test that checks for equality between C++ example
185  file and the Python implementation. See
186  gtsam_unstable/examples/FixedLagSmootherExample.cpp
187  '''
188  # Define a batch fixed lag smoother, which uses
189  # Levenberg-Marquardt to perform the nonlinear optimization
190  lag = 2.0
191  smoother_batch = gtsam.BatchFixedLagSmoother(lag)
192 
193  # Create containers to store the factors and linearization points
194  # that will be sent to the smoothers
195  new_factors = gtsam.NonlinearFactorGraph()
196  new_values = gtsam.Values()
197  new_timestamps = gtsam.FixedLagSmootherKeyTimestampMap()
198 
199  # Create a prior on the first pose, placing it at the origin
200  prior_mean = Pose2(0, 0, 0)
201  prior_noise = gtsam.noiseModel.Diagonal.Sigmas(
202  np.array([0.3, 0.3, 0.1]))
203  X1 = 0
204  new_factors.push_back(
205  gtsam.PriorFactorPose2(X1, prior_mean, prior_noise))
206  new_values.insert(X1, prior_mean)
207  new_timestamps.insert((X1, 0.0))
208 
209  delta_time = 0.25
210  time = 0.25
211 
212  i = 0
213 
214  ground_truth = [
215  Pose2(0.995821, 0.0231012, 0.0300001),
216  Pose2(1.49284, 0.0457247, 0.045),
217  Pose2(1.98981, 0.0758879, 0.06),
218  Pose2(2.48627, 0.113502, 0.075),
219  Pose2(2.98211, 0.158558, 0.09),
220  Pose2(3.47722, 0.211047, 0.105),
221  Pose2(3.97149, 0.270956, 0.12),
222  Pose2(4.4648, 0.338272, 0.135),
223  Pose2(4.95705, 0.41298, 0.15),
224  Pose2(5.44812, 0.495063, 0.165),
225  Pose2(5.9379, 0.584503, 0.18),
226  ]
227 
228  # Iterates from 0.25s to 3.0s, adding 0.25s each loop
229  # In each iteration, the agent moves at a constant speed
230  # and its two odometers measure the change. The smoothed
231  # result is then compared to the ground truth
232  while time <= 3.0:
233  previous_key = int(1000 * (time - delta_time))
234  current_key = int(1000 * time)
235 
236  # assign current key to the current timestamp
237  new_timestamps.insert((current_key, time))
238 
239  # Add a guess for this pose to the new values
240  # Assume that the robot moves at 2 m/s. Position is time[s] *
241  # 2[m/s]
242  current_pose = Pose2(time * 2, 0, 0)
243  new_values.insert(current_key, current_pose)
244 
245  # Add odometry factors from two different sources with different
246  # error stats
247  odometry_measurement_1 = Pose2(0.61, -0.08, 0.02)
248  odometry_noise_1 = gtsam.noiseModel.Diagonal.Sigmas(
249  np.array([0.1, 0.1, 0.05]))
250  new_factors.push_back(
251  gtsam.BetweenFactorPose2(previous_key, current_key,
252  odometry_measurement_1,
253  odometry_noise_1))
254 
255  odometry_measurement_2 = Pose2(0.47, 0.03, 0.01)
256  odometry_noise_2 = gtsam.noiseModel.Diagonal.Sigmas(
257  np.array([0.05, 0.05, 0.05]))
258  new_factors.push_back(
259  gtsam.BetweenFactorPose2(previous_key, current_key,
260  odometry_measurement_2,
261  odometry_noise_2))
262 
263  # Update the smoothers with the new factors. In this case,
264  # one iteration must pass for Levenberg-Marquardt to accurately
265  # estimate
266  if time >= 0.50:
267  smoother_batch.update(new_factors, new_values, new_timestamps)
268 
269  estimate = smoother_batch.calculateEstimatePose2(current_key)
270  self.assertTrue(estimate.equals(ground_truth[i], 1e-4))
271  i += 1
272 
273  new_timestamps.clear()
274  new_values.clear()
275  new_factors.resize(0)
276 
277  time += delta_time
278 
279  def test_ordering(self):
280  """Test ordering"""
282 
283  x0 = X(0)
284  x1 = X(1)
285  x2 = X(2)
286 
287  BETWEEN_NOISE = gtsam.noiseModel.Diagonal.Sigmas(np.ones(1))
288  PRIOR_NOISE = gtsam.noiseModel.Diagonal.Sigmas(np.ones(1))
289 
290  gfg.add(x1, np.eye(1), x0, -np.eye(1), np.ones(1), BETWEEN_NOISE)
291  gfg.add(x2, np.eye(1), x1, -np.eye(1), 2 * np.ones(1), BETWEEN_NOISE)
292  gfg.add(x0, np.eye(1), np.zeros(1), PRIOR_NOISE)
293 
294  keys = (x0, x1, x2)
295  ordering = gtsam.Ordering()
296  for key in keys[::-1]:
297  ordering.push_back(key)
298 
299  bn = gfg.eliminateSequential(ordering)
300  self.assertEqual(bn.size(), 3)
301 
302  keyVector = gtsam.KeyVector()
303  keyVector.append(keys[2])
304  ordering = gtsam.Ordering.ColamdConstrainedLastGaussianFactorGraph(
305  gfg, keyVector)
306  bn = gfg.eliminateSequential(ordering)
307  self.assertEqual(bn.size(), 3)
308 
309  def test_find(self):
310  """
311  Check that optimizing for Karcher mean (which minimizes Between distance)
312  gets correct result.
313  """
314  R = Rot3.Expmap(np.array([0.1, 0, 0]))
315 
316  rotations = gtsam.Rot3Vector([R, R.inverse()])
317  expected = Rot3()
318  actual = gtsam.FindKarcherMean(rotations)
319  self.gtsamAssertEquals(expected, actual)
320 
322  """Averaging 3 identity rotations should yield the identity."""
323  a1Rb1 = Rot3()
324  a2Rb2 = Rot3()
325  a3Rb3 = Rot3()
326 
327  aRb_list = gtsam.Rot3Vector([a1Rb1, a2Rb2, a3Rb3])
328  aRb_expected = Rot3()
329 
330  aRb = gtsam.FindKarcherMean(aRb_list)
331  self.gtsamAssertEquals(aRb, aRb_expected)
332 
333  def test_factor(self):
334  """Check that the InnerConstraint factor leaves the mean unchanged."""
335  # Make a graph with two variables, one between, and one InnerConstraint
336  # The optimal result should satisfy the between, while moving the other
337  # variable to make the mean the same as before.
338  # Mean of R and R' is identity. Let's make a BetweenFactor making R21 =
339  # R*R*R, i.e. geodesic length is 3 rather than 2.
340  R = Rot3.Expmap(np.array([0.1, 0, 0]))
342 
344  R12 = R.compose(R.compose(R))
345  graph.add(gtsam.BetweenFactorRot3(1, 2, R12, MODEL))
346  keys = gtsam.KeyVector()
347  keys.append(1)
348  keys.append(2)
349  graph.add(gtsam.KarcherMeanFactorRot3(keys))
350 
351  initial = gtsam.Values()
352  initial.insert(1, R.inverse())
353  initial.insert(2, R)
354  expected = Rot3()
355 
356  result = gtsam.GaussNewtonOptimizer(graph, initial).optimize()
357  actual = gtsam.FindKarcherMean(
358  gtsam.Rot3Vector([result.atRot3(1),
359  result.atRot3(2)]))
360  self.gtsamAssertEquals(expected, actual)
361  self.gtsamAssertEquals(R12, result.atRot3(1).between(result.atRot3(2)))
362 
363  def test_align(self) -> None:
364  """Ensure estimation of the Pose2 element to align two 2d point clouds succeeds.
365 
366  Two point clouds represent horseshoe-shapes of the same size, just rotated and translated:
367 
368  | X---X
369  | |
370  | X---X
371  ------------------
372  |
373  |
374  O | O
375  | | |
376  O---O
377  """
378  pts_a = [
379  Point2(1, -3),
380  Point2(1, -5),
381  Point2(-1, -5),
382  Point2(-1, -3),
383  ]
384  pts_b = [
385  Point2(3, 1),
386  Point2(1, 1),
387  Point2(1, 3),
388  Point2(3, 3),
389  ]
390 
391  ab_pairs = Point2Pairs(list(zip(pts_a, pts_b)))
392  aTb = Pose2.Align(ab_pairs)
393  self.assertIsNotNone(aTb)
394 
395  for pt_a, pt_b in zip(pts_a, pts_b):
396  pt_a_ = aTb.transformFrom(pt_b)
397  np.testing.assert_allclose(pt_a, pt_a_)
398 
399  # Matrix version
400  A = np.array(pts_a).T
401  B = np.array(pts_b).T
402  aTb = Pose2.Align(A, B)
403  self.assertIsNotNone(aTb)
404 
405  for pt_a, pt_b in zip(pts_a, pts_b):
406  pt_a_ = aTb.transformFrom(pt_b)
407  np.testing.assert_allclose(pt_a, pt_a_)
408 
410  """Test if Align method can align 2 squares."""
411  square = np.array([[0, 0, 0], [0, 1, 0], [1, 1, 0], [1, 0, 0]],
412  float).T
413  sTt = Pose3(Rot3.Rodrigues(0, 0, -np.pi), gtsam.Point3(2, 4, 0))
414  transformed = sTt.transformTo(square)
415 
416  st_pairs = gtsam.Point3Pairs()
417  for j in range(4):
418  st_pairs.append((square[:, j], transformed[:, j]))
419 
420  # Recover the transformation sTt
421  estimated_sTt = Pose3.Align(st_pairs)
422  self.gtsamAssertEquals(estimated_sTt, sTt, 1e-10)
423 
424  # Matrix version
425  estimated_sTt = Pose3.Align(square, transformed)
426  self.gtsamAssertEquals(estimated_sTt, sTt, 1e-10)
427 
429  """Check if ShonanAveraging2 constructor works when not initialized from g2o file.
430 
431  GT pose graph:
432 
433  | cam 1 = (0,4)
434  --o
435  | .
436  . .
437  . .
438  | |
439  o-- ... o--
440  cam 0 cam 2 = (4,0)
441  (0,0)
442  """
443  num_images = 3
444 
445  wTi_list = [
446  Pose2(Rot2.fromDegrees(0), np.array([0, 0])),
447  Pose2(Rot2.fromDegrees(90), np.array([0, 4])),
448  Pose2(Rot2.fromDegrees(0), np.array([4, 0])),
449  ]
450 
451  edges = [(0, 1), (1, 2), (0, 2)]
452  i2Ri1_dict = {(i1, i2):
453  wTi_list[i2].inverse().compose(wTi_list[i1]).rotation()
454  for (i1, i2) in edges}
455 
456  lm_params = LevenbergMarquardtParams.CeresDefaults()
457  shonan_params = ShonanAveragingParameters2(lm_params)
458  shonan_params.setUseHuber(False)
459  shonan_params.setCertifyOptimality(True)
460 
461  noise_model = gtsam.noiseModel.Unit.Create(3)
462  between_factors = gtsam.BetweenFactorPose2s()
463  for (i1, i2), i2Ri1 in i2Ri1_dict.items():
464  i2Ti1 = Pose2(i2Ri1, np.zeros(2))
465  between_factors.append(
466  BetweenFactorPose2(i2, i1, i2Ti1, noise_model))
467 
468  obj = ShonanAveraging2(between_factors, shonan_params)
469  initial = obj.initializeRandomly()
470  result_values, _ = obj.run(initial, min_p=2, max_p=100)
471 
472  wRi_list = [result_values.atRot2(i) for i in range(num_images)]
473  thetas_deg = np.array([wRi.degrees() for wRi in wRi_list])
474 
475  # map all angles to [0,360)
476  thetas_deg = thetas_deg % 360
477  thetas_deg -= thetas_deg[0]
478 
479  expected_thetas_deg = np.array([0.0, 90.0, 0.0])
480  np.testing.assert_allclose(thetas_deg, expected_thetas_deg, atol=0.1)
481 
483  """Test Align of list of Pose2Pair.
484 
485  Scenario:
486  3 object poses
487  same scale (no gauge ambiguity)
488  world frame has poses rotated about 180 degrees.
489  world and egovehicle frame translated by 15 meters w.r.t. each other
490  """
491  R180 = Rot2.fromDegrees(180)
492 
493  # Create source poses (three objects o1, o2, o3 living in the egovehicle "e" frame)
494  # Suppose they are 3d cuboids detected by an onboard sensor in the egovehicle frame
495  eTo0 = Pose2(Rot2(), np.array([5, 0]))
496  eTo1 = Pose2(Rot2(), np.array([10, 0]))
497  eTo2 = Pose2(Rot2(), np.array([15, 0]))
498 
499  eToi_list = [eTo0, eTo1, eTo2]
500 
501  # Create destination poses
502  # (same three objects, but instead living in the world "w" frame)
503  wTo0 = Pose2(R180, np.array([-10, 0]))
504  wTo1 = Pose2(R180, np.array([-5, 0]))
505  wTo2 = Pose2(R180, np.array([0, 0]))
506 
507  wToi_list = [wTo0, wTo1, wTo2]
508 
509  we_pairs = Pose2Pairs(list(zip(wToi_list, eToi_list)))
510 
511  # Recover the transformation wSe (i.e. world_S_egovehicle)
512  wSe = Similarity2.Align(we_pairs)
513 
514  for wToi, eToi in zip(wToi_list, eToi_list):
515  self.gtsamAssertEquals(wToi, wSe.transformFrom(eToi))
516 
518  """Test if Align Pose2Pairs method can account for gauge ambiguity.
519 
520  Scenario:
521  3 object poses
522  with gauge ambiguity (2x scale)
523  world frame has poses rotated by 90 degrees.
524  world and egovehicle frame translated by 11 meters w.r.t. each other
525  """
526  R90 = Rot2.fromDegrees(90)
527 
528  # Create source poses (three objects o1, o2, o3 living in the egovehicle "e" frame)
529  # Suppose they are 3d cuboids detected by an onboard sensor in the egovehicle frame
530  eTo0 = Pose2(Rot2(), np.array([1, 0]))
531  eTo1 = Pose2(Rot2(), np.array([2, 0]))
532  eTo2 = Pose2(Rot2(), np.array([4, 0]))
533 
534  eToi_list = [eTo0, eTo1, eTo2]
535 
536  # Create destination poses
537  # (same three objects, but instead living in the world/city "w" frame)
538  wTo0 = Pose2(R90, np.array([0, 12]))
539  wTo1 = Pose2(R90, np.array([0, 14]))
540  wTo2 = Pose2(R90, np.array([0, 18]))
541 
542  wToi_list = [wTo0, wTo1, wTo2]
543 
544  we_pairs = Pose2Pairs(list(zip(wToi_list, eToi_list)))
545 
546  # Recover the transformation wSe (i.e. world_S_egovehicle)
547  wSe = Similarity2.Align(we_pairs)
548 
549  for wToi, eToi in zip(wToi_list, eToi_list):
550  self.gtsamAssertEquals(wToi, wSe.transformFrom(eToi))
551 
553  """Test if Align Pose2Pairs method can account for gauge ambiguity.
554 
555  Make sure a big and small square can be aligned.
556  The u's represent a big square (10x10), and v's represents a small square (4x4).
557 
558  Scenario:
559  4 object poses
560  with gauge ambiguity (2.5x scale)
561  """
562  # 0, 90, 180, and 270 degrees yaw
563  R0 = Rot2.fromDegrees(0)
564  R90 = Rot2.fromDegrees(90)
565  R180 = Rot2.fromDegrees(180)
566  R270 = Rot2.fromDegrees(270)
567 
568  aTi0 = Pose2(R0, np.array([2, 3]))
569  aTi1 = Pose2(R90, np.array([12, 3]))
570  aTi2 = Pose2(R180, np.array([12, 13]))
571  aTi3 = Pose2(R270, np.array([2, 13]))
572 
573  aTi_list = [aTi0, aTi1, aTi2, aTi3]
574 
575  bTi0 = Pose2(R0, np.array([4, 3]))
576  bTi1 = Pose2(R90, np.array([8, 3]))
577  bTi2 = Pose2(R180, np.array([8, 7]))
578  bTi3 = Pose2(R270, np.array([4, 7]))
579 
580  bTi_list = [bTi0, bTi1, bTi2, bTi3]
581 
582  ab_pairs = Pose2Pairs(list(zip(aTi_list, bTi_list)))
583 
584  # Recover the transformation wSe (i.e. world_S_egovehicle)
585  aSb = Similarity2.Align(ab_pairs)
586 
587  for aTi, bTi in zip(aTi_list, bTi_list):
588  self.gtsamAssertEquals(aTi, aSb.transformFrom(bTi))
589 
591  """Test Align Pose3Pairs method.
592 
593  Scenario:
594  3 object poses
595  same scale (no gauge ambiguity)
596  world frame has poses rotated about x-axis (90 degree roll)
597  world and egovehicle frame translated by 15 meters w.r.t. each other
598  """
599  Rx90 = Rot3.Rx(np.deg2rad(90))
600 
601  # Create source poses (three objects o1, o2, o3 living in the egovehicle "e" frame)
602  # Suppose they are 3d cuboids detected by an onboard sensor in the egovehicle frame
603  eTo0 = Pose3(Rot3(), np.array([5, 0, 0]))
604  eTo1 = Pose3(Rot3(), np.array([10, 0, 0]))
605  eTo2 = Pose3(Rot3(), np.array([15, 0, 0]))
606 
607  eToi_list = [eTo0, eTo1, eTo2]
608 
609  # Create destination poses
610  # (same three objects, but instead living in the world/city "w" frame)
611  wTo0 = Pose3(Rx90, np.array([-10, 0, 0]))
612  wTo1 = Pose3(Rx90, np.array([-5, 0, 0]))
613  wTo2 = Pose3(Rx90, np.array([0, 0, 0]))
614 
615  wToi_list = [wTo0, wTo1, wTo2]
616 
617  we_pairs = gtsam.Pose3Pairs(list(zip(wToi_list, eToi_list)))
618 
619  # Recover the transformation wSe (i.e. world_S_egovehicle)
620  wSe = Similarity3.Align(we_pairs)
621 
622  for wToi, eToi in zip(wToi_list, eToi_list):
623  self.gtsamAssertEquals(wToi, wSe.transformFrom(eToi))
624 
626  """Test if Align Pose3Pairs method can account for gauge ambiguity.
627 
628  Scenario:
629  3 object poses
630  with gauge ambiguity (2x scale)
631  world frame has poses rotated about z-axis (90 degree yaw)
632  world and egovehicle frame translated by 11 meters w.r.t. each other
633  """
634  Rz90 = Rot3.Rz(np.deg2rad(90))
635 
636  # Create source poses (three objects o1, o2, o3 living in the egovehicle "e" frame)
637  # Suppose they are 3d cuboids detected by an onboard sensor in the egovehicle frame
638  eTo0 = Pose3(Rot3(), np.array([1, 0, 0]))
639  eTo1 = Pose3(Rot3(), np.array([2, 0, 0]))
640  eTo2 = Pose3(Rot3(), np.array([4, 0, 0]))
641 
642  eToi_list = [eTo0, eTo1, eTo2]
643 
644  # Create destination poses
645  # (same three objects, but instead living in the world/city "w" frame)
646  wTo0 = Pose3(Rz90, np.array([0, 12, 0]))
647  wTo1 = Pose3(Rz90, np.array([0, 14, 0]))
648  wTo2 = Pose3(Rz90, np.array([0, 18, 0]))
649 
650  wToi_list = [wTo0, wTo1, wTo2]
651 
652  we_pairs = gtsam.Pose3Pairs(list(zip(wToi_list, eToi_list)))
653 
654  # Recover the transformation wSe (i.e. world_S_egovehicle)
655  wSe = Similarity3.Align(we_pairs)
656 
657  for wToi, eToi in zip(wToi_list, eToi_list):
658  self.gtsamAssertEquals(wToi, wSe.transformFrom(eToi))
659 
661  """Test if Align Pose3Pairs method can account for gauge ambiguity.
662 
663  Make sure a big and small square can be aligned.
664  The u's represent a big square (10x10), and v's represents a small square (4x4).
665 
666  Scenario:
667  4 object poses
668  with gauge ambiguity (2.5x scale)
669  """
670  # 0, 90, 180, and 270 degrees yaw
671  R0 = Rot3.Rz(np.deg2rad(0))
672  R90 = Rot3.Rz(np.deg2rad(90))
673  R180 = Rot3.Rz(np.deg2rad(180))
674  R270 = Rot3.Rz(np.deg2rad(270))
675 
676  aTi0 = Pose3(R0, np.array([2, 3, 0]))
677  aTi1 = Pose3(R90, np.array([12, 3, 0]))
678  aTi2 = Pose3(R180, np.array([12, 13, 0]))
679  aTi3 = Pose3(R270, np.array([2, 13, 0]))
680 
681  aTi_list = [aTi0, aTi1, aTi2, aTi3]
682 
683  bTi0 = Pose3(R0, np.array([4, 3, 0]))
684  bTi1 = Pose3(R90, np.array([8, 3, 0]))
685  bTi2 = Pose3(R180, np.array([8, 7, 0]))
686  bTi3 = Pose3(R270, np.array([4, 7, 0]))
687 
688  bTi_list = [bTi0, bTi1, bTi2, bTi3]
689 
690  ab_pairs = gtsam.Pose3Pairs(list(zip(aTi_list, bTi_list)))
691 
692  # Recover the transformation wSe (i.e. world_S_egovehicle)
693  aSb = Similarity3.Align(ab_pairs)
694 
695  for aTi, bTi in zip(aTi_list, bTi_list):
696  self.gtsamAssertEquals(aTi, aSb.transformFrom(bTi))
697 
699  self,
700  calibration: Union[Cal3Bundler, Cal3_S2],
701  camera_model: Union[PinholeCameraCal3Bundler, PinholeCameraCal3_S2],
702  cal_params: Iterable[Iterable[Union[int, float]]],
703  camera_set: Optional[Union[CameraSetCal3Bundler,
704  CameraSetCal3_S2]] = None,
705  ) -> Tuple[List[Point2], Union[CameraSetCal3Bundler, CameraSetCal3_S2,
706  List[Cal3Bundler], List[Cal3_S2]]]:
707  """
708  Generate vector of measurements for given calibration and camera model.
709 
710  Args:
711  calibration: Camera calibration e.g. Cal3_S2
712  camera_model: Camera model e.g. PinholeCameraCal3_S2
713  cal_params: Iterable of camera parameters for `calibration` e.g. [K1, K2]
714  camera_set: Cameraset object (for individual calibrations)
715 
716  Returns:
717  list of measurements and list/CameraSet object for cameras
718  """
719  if camera_set is not None:
720  cameras = camera_set()
721  else:
722  cameras = []
723  measurements = gtsam.Point2Vector()
724 
725  for k, pose in zip(cal_params, self.triangulation_poses):
726  K = calibration(*k)
727  camera = camera_model(pose, K)
728  cameras.append(camera)
729  z = camera.project(self.landmark)
730  measurements.append(z)
731 
732  return measurements, cameras
733 
734  def test_TriangulationExample(self) -> None:
735  """Tests triangulation with shared Cal3_S2 calibration"""
736  # Some common constants
737  sharedCal = (1500, 1200, 0, 640, 480)
738 
739  measurements, _ = self.generate_measurements(
740  calibration=Cal3_S2,
741  camera_model=PinholeCameraCal3_S2,
742  cal_params=(sharedCal, sharedCal))
743 
744  triangulated_landmark = gtsam.triangulatePoint3(
745  self.triangulation_poses,
746  Cal3_S2(sharedCal),
747  measurements,
748  rank_tol=1e-9,
749  optimize=True)
750  self.gtsamAssertEquals(self.landmark, triangulated_landmark, 1e-9)
751 
752  # Add some noise and try again: result should be ~ (4.995, 0.499167, 1.19814)
753  measurements_noisy = gtsam.Point2Vector()
754  measurements_noisy.append(measurements[0] - np.array([0.1, 0.5]))
755  measurements_noisy.append(measurements[1] - np.array([-0.2, 0.3]))
756 
757  triangulated_landmark = gtsam.triangulatePoint3(
758  self.triangulation_poses,
759  Cal3_S2(sharedCal),
760  measurements_noisy,
761  rank_tol=1e-9,
762  optimize=True)
763 
764  self.gtsamAssertEquals(self.landmark, triangulated_landmark, 1e-2)
765 
767  """Ensure triangulation with a robust model works."""
768  sharedCal = Cal3_S2(1500, 1200, 0, 640, 480)
769 
770  # landmark ~5 meters infront of camera
771  landmark = Point3(5, 0.5, 1.2)
772 
773  pose1 = Pose3(UPRIGHT, Point3(0, 0, 1))
774  pose2 = pose1 * Pose3(Rot3(), Point3(1, 0, 0))
775  pose3 = pose1 * Pose3(Rot3.Ypr(0.1, 0.2, 0.1), Point3(0.1, -2, -0.1))
776 
777  camera1 = PinholeCameraCal3_S2(pose1, sharedCal)
778  camera2 = PinholeCameraCal3_S2(pose2, sharedCal)
779  camera3 = PinholeCameraCal3_S2(pose3, sharedCal)
780 
781  z1: Point2 = camera1.project(landmark)
782  z2: Point2 = camera2.project(landmark)
783  z3: Point2 = camera3.project(landmark)
784 
785  poses = gtsam.Pose3Vector([pose1, pose2, pose3])
786  measurements = gtsam.Point2Vector([z1, z2, z3])
787 
788  # noise free, so should give exactly the landmark
789  actual = gtsam.triangulatePoint3(poses,
790  sharedCal,
791  measurements,
792  rank_tol=1e-9,
793  optimize=False)
794  self.assertTrue(np.allclose(landmark, actual, atol=1e-2))
795 
796  # Add outlier
797  measurements[0] += Point2(100, 120) # very large pixel noise!
798 
799  # now estimate does not match landmark
800  actual2 = gtsam.triangulatePoint3(poses,
801  sharedCal,
802  measurements,
803  rank_tol=1e-9,
804  optimize=False)
805  # DLT is surprisingly robust, but still off (actual error is around 0.26m)
806  self.assertTrue(np.linalg.norm(landmark - actual2) >= 0.2)
807  self.assertTrue(np.linalg.norm(landmark - actual2) <= 0.5)
808 
809  # Again with nonlinear optimization
810  actual3 = gtsam.triangulatePoint3(poses,
811  sharedCal,
812  measurements,
813  rank_tol=1e-9,
814  optimize=True)
815  # result from nonlinear (but non-robust optimization) is close to DLT and still off
816  self.assertTrue(np.allclose(actual2, actual3, atol=0.1))
817 
818  # Again with nonlinear optimization, this time with robust loss
822  actual4 = gtsam.triangulatePoint3(poses,
823  sharedCal,
824  measurements,
825  rank_tol=1e-9,
826  optimize=True,
827  model=model)
828  # using the Huber loss we now have a quite small error!! nice!
829  self.assertTrue(np.allclose(landmark, actual4, atol=0.05))
830 
832  """Check safe triangulation function."""
833  pose1, pose2 = self.poses
834 
835  K1 = Cal3_S2(1500, 1200, 0, 640, 480)
836  # create first camera. Looking along X-axis, 1 meter above ground plane (x-y)
837  camera1 = PinholeCameraCal3_S2(pose1, K1)
838 
839  # create second camera 1 meter to the right of first camera
840  K2 = Cal3_S2(1600, 1300, 0, 650, 440)
841  camera2 = PinholeCameraCal3_S2(pose2, K2)
842 
843  # 1. Project two landmarks into two cameras and triangulate
844  z1 = camera1.project(self.landmark)
845  z2 = camera2.project(self.landmark)
846 
847  cameras = CameraSetCal3_S2()
848  cameras.append(camera1)
849  cameras.append(camera2)
850 
851  measurements = gtsam.Point2Vector()
852  measurements.append(z1)
853  measurements.append(z2)
854 
855  landmarkDistanceThreshold = 10 # landmark is closer than that
856  # all default except landmarkDistanceThreshold:
857  params = TriangulationParameters(1.0, False, landmarkDistanceThreshold)
858  actual: TriangulationResult = gtsam.triangulateSafe(
859  cameras, measurements, params)
860  self.gtsamAssertEquals(actual.get(), self.landmark, 1e-2)
861  self.assertTrue(actual.valid())
862 
863  landmarkDistanceThreshold = 4 # landmark is farther than that
864  params2 = TriangulationParameters(1.0, False,
865  landmarkDistanceThreshold)
866  actual = gtsam.triangulateSafe(cameras, measurements, params2)
867  self.assertTrue(actual.farPoint())
868 
869  # 3. Add a slightly rotated third camera above with a wrong measurement
870  # (OUTLIER)
871  pose3 = pose1 * Pose3(Rot3.Ypr(0.1, 0.2, 0.1), Point3(0.1, -2, -.1))
872  K3 = Cal3_S2(700, 500, 0, 640, 480)
873  camera3 = PinholeCameraCal3_S2(pose3, K3)
874  z3 = camera3.project(self.landmark)
875 
876  cameras.append(camera3)
877  measurements.append(z3 + Point2(10, -10))
878 
879  landmarkDistanceThreshold = 10 # landmark is closer than that
880  outlierThreshold = 100 # loose, the outlier is going to pass
881  params3 = TriangulationParameters(1.0, False,
882  landmarkDistanceThreshold,
883  outlierThreshold)
884  actual = gtsam.triangulateSafe(cameras, measurements, params3)
885  self.assertTrue(actual.valid())
886 
887  # now set stricter threshold for outlier rejection
888  outlierThreshold = 5 # tighter, the outlier is not going to pass
889  params4 = TriangulationParameters(1.0, False,
890  landmarkDistanceThreshold,
891  outlierThreshold)
892  actual = gtsam.triangulateSafe(cameras, measurements, params4)
893  self.assertTrue(actual.outlier())
894 
895 
896 if __name__ == "__main__":
897  unittest.main()
gtsam::internal::rotation
Rot3 rotation(const Pose3 &pose, OptionalJacobian< 3, 6 > H)
Definition: slam/expressions.h:80
gtsam.examples.DogLegOptimizerExample.int
int
Definition: DogLegOptimizerExample.py:111
gtsam::CameraSetCal3_S2
CameraSet< PinholeCamera< Cal3_S2 > > CameraSetCal3_S2
Definition: triangulation.h:758
test_backwards_compatibility.TestBackwardsCompatibility.test_outliers_and_far_landmarks
None test_outliers_and_far_landmarks(self)
Definition: test_backwards_compatibility.py:831
gtsam::Pose3Vector
std::vector< Pose3 > Pose3Vector
Definition: Pose3.h:428
test_backwards_compatibility.TestBackwardsCompatibility.test_align
None test_align(self)
Definition: test_backwards_compatibility.py:363
gtsam::Point3Pairs
std::vector< Point3Pair > Point3Pairs
Definition: Point3.h:45
gtsam::noiseModel::mEstimator::Huber::Create
static shared_ptr Create(double k, const ReweightScheme reweight=Block)
Definition: LossFunctions.cpp:203
test_backwards_compatibility.TestBackwardsCompatibility.test_TriangulationExample
None test_TriangulationExample(self)
Definition: test_backwards_compatibility.py:734
inverse
const EIGEN_DEVICE_FUNC InverseReturnType inverse() const
Definition: ArrayCwiseUnaryOps.h:411
gtsam::GaussNewtonOptimizer
Definition: GaussNewtonOptimizer.h:38
gtsam::gtsfm::Keypoints
Definition: DsfTrackGenerator.h:40
optimize
int optimize(const SfmData &db, const NonlinearFactorGraph &graph, const Values &initial, bool separateCalibration=false)
Definition: timeSFMBAL.h:64
list
Definition: pytypes.h:2166
test_backwards_compatibility.TestBackwardsCompatibility.test_align_poses3_scaled_squares
def test_align_poses3_scaled_squares(self)
Definition: test_backwards_compatibility.py:660
gtsam::noiseModel::Robust::Create
static shared_ptr Create(const RobustModel::shared_ptr &robust, const NoiseModel::shared_ptr noise)
Definition: NoiseModel.cpp:740
test_backwards_compatibility.TestBackwardsCompatibility.test_Cal3Unified_triangulation_rectify
def test_Cal3Unified_triangulation_rectify(self)
Definition: test_backwards_compatibility.py:93
test_backwards_compatibility.TestBackwardsCompatibility.test_constructorBetweenFactorPose2s
None test_constructorBetweenFactorPose2s(self)
Definition: test_backwards_compatibility.py:428
gtsam.FixedLagSmootherKeyTimestampMap
Definition: python/gtsam/__init__.py:43
gtsam::SfmTrack2d
Track containing 2D measurements associated with a single 3D point. Note: Equivalent to gtsam....
Definition: SfmTrack.h:42
gtsam::symbol_shorthand
Definition: inference/Symbol.h:147
gtsam::noiseModel::Diagonal::Sigmas
static shared_ptr Sigmas(const Vector &sigmas, bool smart=true)
Definition: NoiseModel.cpp:291
X
#define X
Definition: icosphere.cpp:20
gtsam::utils.test_case.GtsamTestCase.gtsamAssertEquals
def gtsamAssertEquals(self, actual, expected, tol=1e-9)
Definition: test_case.py:18
test_backwards_compatibility.TestBackwardsCompatibility.test_Cal3Fisheye_triangulation_rectify
def test_Cal3Fisheye_triangulation_rectify(self)
Definition: test_backwards_compatibility.py:76
test_backwards_compatibility.TestBackwardsCompatibility.test_align_squares
def test_align_squares(self)
Definition: test_backwards_compatibility.py:409
test_backwards_compatibility.TestBackwardsCompatibility.test_align_poses2_along_straight_line
None test_align_poses2_along_straight_line(self)
Definition: test_backwards_compatibility.py:482
gtsam::gtsfm
Definition: DsfTrackGenerator.cpp:27
gtsam.MatchIndicesMap
MatchIndicesMap
Definition: python/gtsam/__init__.py:36
gtsam::KeyVector
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition: Key.h:92
test_backwards_compatibility.TestBackwardsCompatibility.fisheye_cameras
fisheye_cameras
Definition: test_backwards_compatibility.py:47
gtsam::CameraSet
A set of cameras, all with their own calibration.
Definition: CameraSet.h:36
test_backwards_compatibility.TestBackwardsCompatibility.stereographic
stereographic
Definition: test_backwards_compatibility.py:54
test_backwards_compatibility.TestBackwardsCompatibility.test_sfm_track_2d_constructor
None test_sfm_track_2d_constructor(self)
Definition: test_backwards_compatibility.py:174
test_backwards_compatibility.TestBackwardsCompatibility.test_find_karcher_mean_identity
def test_find_karcher_mean_identity(self)
Definition: test_backwards_compatibility.py:321
test_backwards_compatibility.TestBackwardsCompatibility.test_factor
def test_factor(self)
Definition: test_backwards_compatibility.py:333
gtsam::range
Double_ range(const Point2_ &p, const Point2_ &q)
Definition: slam/expressions.h:30
test_backwards_compatibility.TestBackwardsCompatibility.landmark
landmark
Definition: test_backwards_compatibility.py:74
gtsam.SfmMeasurementVector
SfmMeasurementVector
Definition: python/gtsam/__init__.py:35
gtsam::GaussianFactorGraph
Definition: GaussianFactorGraph.h:73
gtsam::TriangulationParameters
Definition: triangulation.h:558
test_backwards_compatibility.TestBackwardsCompatibility.test_find
def test_find(self)
Definition: test_backwards_compatibility.py:309
test_backwards_compatibility.TestBackwardsCompatibility.test_align_poses3_along_straight_line
def test_align_poses3_along_straight_line(self)
Definition: test_backwards_compatibility.py:590
gtsam::FindKarcherMean
T FindKarcherMean(std::initializer_list< T > &&rotations)
Definition: KarcherMeanFactor-inl.h:55
test_backwards_compatibility.TestBackwardsCompatibility.test_FixedLagSmootherExample
def test_FixedLagSmootherExample(self)
Definition: test_backwards_compatibility.py:182
test_backwards_compatibility.TestBackwardsCompatibility.unified_cameras
unified_cameras
Definition: test_backwards_compatibility.py:58
test_backwards_compatibility.TestBackwardsCompatibility.test_track_generation
None test_track_generation(self)
Definition: test_backwards_compatibility.py:110
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_backwards_compatibility.TestBackwardsCompatibility.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_backwards_compatibility.py:698
gtsam::PinholeCamera
Definition: PinholeCamera.h:33
gtsam::NonlinearFactorGraph
Definition: NonlinearFactorGraph.h:55
gtsam::Rot3Vector
std::vector< Rot3, Eigen::aligned_allocator< Rot3 > > Rot3Vector
std::vector of Rot3s, used in Matlab wrapper
Definition: Rot3.h:564
gtsam::BatchFixedLagSmoother
Definition: nonlinear/BatchFixedLagSmoother.h:29
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
BetweenFactorPose2
BetweenFactor< Pose2 > BetweenFactorPose2
Definition: serialization.cpp:49
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::Pose3Pairs
std::vector< std::pair< Pose3, Pose3 > > Pose3Pairs
Definition: Pose3.h:425
gtsam::Point2
Vector2 Point2
Definition: Point2.h:32
test_backwards_compatibility.TestBackwardsCompatibility.origin
origin
Definition: test_backwards_compatibility.py:44
gtsam.KeypointsVector
KeypointsVector
Definition: python/gtsam/__init__.py:37
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
gtsam::Rot2
Definition: Rot2.h:35
test_backwards_compatibility.TestBackwardsCompatibility.poses
poses
Definition: test_backwards_compatibility.py:45
test_backwards_compatibility.TestBackwardsCompatibility.test_align_poses2_scaled_squares
def test_align_poses2_scaled_squares(self)
Definition: test_backwards_compatibility.py:552
gtsam::Values
Definition: Values.h:65
gtsam::testing::compose
T compose(const T &t1, const T &t2)
Definition: lieProxies.h:39
gtsam::Cal3Unified
Calibration of a omni-directional camera with mirror + lens radial distortion.
Definition: Cal3Unified.h:45
test_backwards_compatibility.TestBackwardsCompatibility.test_ordering
def test_ordering(self)
Definition: test_backwards_compatibility.py:279
test_backwards_compatibility.TestBackwardsCompatibility.unified_measurements
unified_measurements
Definition: test_backwards_compatibility.py:59
test_backwards_compatibility.TestBackwardsCompatibility
Definition: test_backwards_compatibility.py:31
gtsam::testing::between
T between(const T &t1, const T &t2)
Definition: lieProxies.h:36
gtsam::Point2Vector
std::vector< Point2, Eigen::aligned_allocator< Point2 > > Point2Vector
Definition: Point2.h:49
gtsam::utils.test_case.GtsamTestCase
Definition: test_case.py:15
gtsam::IndexPair
Small utility class for representing a wrappable pairs of ints.
Definition: DSFMap.h:117
test_backwards_compatibility.TestBackwardsCompatibility.test_triangulation_robust_three_poses
None test_triangulation_robust_three_poses(self)
Definition: test_backwards_compatibility.py:766
gtsam::Cal3_S2
The most common 5DOF 3D->2D calibration.
Definition: Cal3_S2.h:34
gtsam::Point3
Vector3 Point3
Definition: Point3.h:38
gtsam::BetweenFactorPose2s
std::vector< BetweenFactor< Pose2 >::shared_ptr > BetweenFactorPose2s
Definition: dataset.h:212
gtsam::ShonanAveragingParameters2
ShonanAveragingParameters< 2 > ShonanAveragingParameters2
Definition: ShonanAveraging.h:104
gtsam::Point2Pairs
std::vector< Point2Pair > Point2Pairs
Definition: Point2.h:38
gtsam::Pose2Pairs
std::vector< Pose2Pair > Pose2Pairs
Definition: Pose2.h:362
len
size_t len(handle h)
Get the length of a Python object.
Definition: pytypes.h:2446
test_backwards_compatibility.TestBackwardsCompatibility.test_align_poses2_along_straight_line_gauge
def test_align_poses2_along_straight_line_gauge(self)
Definition: test_backwards_compatibility.py:517
test_backwards_compatibility.TestBackwardsCompatibility.setUp
def setUp(self)
Definition: test_backwards_compatibility.py:34
gtsam::gtsfm::tracksFromPairwiseMatches
std::vector< SfmTrack2d > tracksFromPairwiseMatches(const MatchIndicesMap &matches, const KeypointsVector &keypoints, bool verbose)
Creates a list of tracks from 2d point correspondences.
Definition: DsfTrackGenerator.cpp:103
test_backwards_compatibility.TestBackwardsCompatibility.triangulation_poses
triangulation_poses
Set up two camera poses Looking along X-axis, 1 meter above ground plane (x-y)
Definition: test_backwards_compatibility.py:69
gtsam::Ordering
Definition: inference/Ordering.h:33
test_backwards_compatibility.TestBackwardsCompatibility.fisheye_measurements
fisheye_measurements
Definition: test_backwards_compatibility.py:48
test_backwards_compatibility.TestBackwardsCompatibility.test_align_poses3_along_straight_line_gauge
def test_align_poses3_along_straight_line_gauge(self)
Definition: test_backwards_compatibility.py:625
gtsam::Pose2
Definition: Pose2.h:39
gtsam::ShonanAveraging2
Definition: ShonanAveraging.h:428


gtsam
Author(s):
autogenerated on Fri Nov 1 2024 03:39:26