test_Sim3.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 Sim3 unit tests.
9 Author: John Lambert
10 """
11 # pylint: disable=no-name-in-module
12 import unittest
13 from typing import List, Optional
14 
15 import numpy as np
16 from gtsam.utils.test_case import GtsamTestCase
17 
18 import gtsam
19 from gtsam import Point3, Pose3, Rot3, Similarity3, BetweenFactorSimilarity3, NonlinearFactorGraph, Values, LevenbergMarquardtOptimizer, LevenbergMarquardtParams
20 
21 
22 class TestSim3(GtsamTestCase):
23  """Test selected Sim3 methods."""
24 
25  def test_align_poses_along_straight_line(self):
26  """Test Pose3 Align method.
27 
28  Scenario:
29  3 object poses
30  same scale (no gauge ambiguity)
31  world frame has poses rotated about x-axis (90 degree roll)
32  world and egovehicle frame translated by 15 meters w.r.t. each other
33  """
34  Rx90 = Rot3.Rx(np.deg2rad(90))
35 
36  # Create source poses (three objects o1, o2, o3 living in the egovehicle "e" frame)
37  # Suppose they are 3d cuboids detected by an onboard sensor in the egovehicle frame
38  eTo0 = Pose3(Rot3(), np.array([5, 0, 0]))
39  eTo1 = Pose3(Rot3(), np.array([10, 0, 0]))
40  eTo2 = Pose3(Rot3(), np.array([15, 0, 0]))
41 
42  eToi_list = [eTo0, eTo1, eTo2]
43 
44  # Create destination poses
45  # (same three objects, but instead living in the world/city "w" frame)
46  wTo0 = Pose3(Rx90, np.array([-10, 0, 0]))
47  wTo1 = Pose3(Rx90, np.array([-5, 0, 0]))
48  wTo2 = Pose3(Rx90, np.array([0, 0, 0]))
49 
50  wToi_list = [wTo0, wTo1, wTo2]
51 
52  we_pairs = list(zip(wToi_list, eToi_list))
53 
54  # Recover the transformation wSe (i.e. world_S_egovehicle)
55  wSe = Similarity3.Align(we_pairs)
56 
57  for wToi, eToi in zip(wToi_list, eToi_list):
58  self.gtsamAssertEquals(wToi, wSe.transformFrom(eToi))
59 
60  def test_align_poses_along_straight_line_gauge(self):
61  """Test if Pose3 Align method can account for gauge ambiguity.
62 
63  Scenario:
64  3 object poses
65  with gauge ambiguity (2x scale)
66  world frame has poses rotated about z-axis (90 degree yaw)
67  world and egovehicle frame translated by 11 meters w.r.t. each other
68  """
69  Rz90 = Rot3.Rz(np.deg2rad(90))
70 
71  # Create source poses (three objects o1, o2, o3 living in the egovehicle "e" frame)
72  # Suppose they are 3d cuboids detected by an onboard sensor in the egovehicle frame
73  eTo0 = Pose3(Rot3(), np.array([1, 0, 0]))
74  eTo1 = Pose3(Rot3(), np.array([2, 0, 0]))
75  eTo2 = Pose3(Rot3(), np.array([4, 0, 0]))
76 
77  eToi_list = [eTo0, eTo1, eTo2]
78 
79  # Create destination poses
80  # (same three objects, but instead living in the world/city "w" frame)
81  wTo0 = Pose3(Rz90, np.array([0, 12, 0]))
82  wTo1 = Pose3(Rz90, np.array([0, 14, 0]))
83  wTo2 = Pose3(Rz90, np.array([0, 18, 0]))
84 
85  wToi_list = [wTo0, wTo1, wTo2]
86 
87  we_pairs = list(zip(wToi_list, eToi_list))
88 
89  # Recover the transformation wSe (i.e. world_S_egovehicle)
90  wSe = Similarity3.Align(we_pairs)
91 
92  for wToi, eToi in zip(wToi_list, eToi_list):
93  self.gtsamAssertEquals(wToi, wSe.transformFrom(eToi))
94 
95  def test_align_poses_scaled_squares(self):
96  """Test if Pose3 Align method can account for gauge ambiguity.
97 
98  Make sure a big and small square can be aligned.
99  The u's represent a big square (10x10), and v's represents a small square (4x4).
100 
101  Scenario:
102  4 object poses
103  with gauge ambiguity (2.5x scale)
104  """
105  # 0, 90, 180, and 270 degrees yaw
106  R0 = Rot3.Rz(np.deg2rad(0))
107  R90 = Rot3.Rz(np.deg2rad(90))
108  R180 = Rot3.Rz(np.deg2rad(180))
109  R270 = Rot3.Rz(np.deg2rad(270))
110 
111  aTi0 = Pose3(R0, np.array([2, 3, 0]))
112  aTi1 = Pose3(R90, np.array([12, 3, 0]))
113  aTi2 = Pose3(R180, np.array([12, 13, 0]))
114  aTi3 = Pose3(R270, np.array([2, 13, 0]))
115 
116  aTi_list = [aTi0, aTi1, aTi2, aTi3]
117 
118  bTi0 = Pose3(R0, np.array([4, 3, 0]))
119  bTi1 = Pose3(R90, np.array([8, 3, 0]))
120  bTi2 = Pose3(R180, np.array([8, 7, 0]))
121  bTi3 = Pose3(R270, np.array([4, 7, 0]))
122 
123  bTi_list = [bTi0, bTi1, bTi2, bTi3]
124 
125  ab_pairs = list(zip(aTi_list, bTi_list))
126 
127  # Recover the transformation wSe (i.e. world_S_egovehicle)
128  aSb = Similarity3.Align(ab_pairs)
129 
130  for aTi, bTi in zip(aTi_list, bTi_list):
131  self.gtsamAssertEquals(aTi, aSb.transformFrom(bTi))
132 
133  def test_sim3_optimization(self)->None:
134  # Create a PriorFactor with a Sim3 prior
135  prior = Similarity3(Rot3.Ypr(0.1, 0.2, 0.3), Point3(1, 2, 3), 4)
136  model = gtsam.noiseModel.Isotropic.Sigma(7, 1)
137 
138  # Create graph
139  graph = NonlinearFactorGraph()
140  graph.addPriorSimilarity3(1, prior, model)
141 
142  # Create initial estimate with Identity transform
143  initial = Values()
144  initial.insert(1, Similarity3())
145 
146  # Optimize
147  params = LevenbergMarquardtParams()
148  params.setVerbosityLM("TRYCONFIG")
149  result = LevenbergMarquardtOptimizer(graph, initial).optimize()
150 
151  # After optimization, result should be prior
152  self.gtsamAssertEquals(prior, result.atSimilarity3(1), 1e-4)
153 
154  def test_sim3_optimization2(self) -> None:
155  prior = Similarity3()
156  m1 = Similarity3(Rot3.Ypr(np.pi / 4.0, 0, 0), Point3(2.0, 0, 0), 1.0)
157  m2 = Similarity3(Rot3.Ypr(np.pi / 2.0, 0, 0), Point3(np.sqrt(8) * 0.9, 0, 0), 1.0)
158  m3 = Similarity3(Rot3.Ypr(3 * np.pi / 4.0, 0, 0), Point3(np.sqrt(32) * 0.8, 0, 0), 1.0)
159  m4 = Similarity3(Rot3.Ypr(np.pi / 2.0, 0, 0), Point3(6 * 0.7, 0, 0), 1.0)
160  loop = Similarity3(1.42)
161 
162  priorNoise = gtsam.noiseModel.Isotropic.Sigma(7, 0.01)
163  betweenNoise = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 10]))
164  betweenNoise2 = gtsam.noiseModel.Diagonal.Sigmas(np.array([ 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 1.0]))
165  b1 = BetweenFactorSimilarity3(1, 2, m1, betweenNoise)
166  b2 = BetweenFactorSimilarity3(2, 3, m2, betweenNoise)
167  b3 = BetweenFactorSimilarity3(3, 4, m3, betweenNoise)
168  b4 = BetweenFactorSimilarity3(4, 5, m4, betweenNoise)
169  lc = BetweenFactorSimilarity3(5, 1, loop, betweenNoise2)
170 
171  # Create graph
172  graph = NonlinearFactorGraph()
173  graph.addPriorSimilarity3(1, prior, priorNoise)
174  graph.add(b1)
175  graph.add(b2)
176  graph.add(b3)
177  graph.add(b4)
178  graph.add(lc)
179 
180  # graph.print("Full Graph\n");
181  initial=Values()
182  initial.insert(1, prior)
183  initial.insert(2, Similarity3(Rot3.Ypr(np.pi / 2.0, 0, 0), Point3(1, 0, 0), 1.1))
184  initial.insert(3, Similarity3(Rot3.Ypr(2.0 * np.pi / 2.0, 0, 0), Point3(0.9, 1.1, 0), 1.2))
185  initial.insert(4, Similarity3(Rot3.Ypr(3.0 * np.pi / 2.0, 0, 0), Point3(0, 1, 0), 1.3))
186  initial.insert(5, Similarity3(Rot3.Ypr(4.0 * np.pi / 2.0, 0, 0), Point3(0, 0, 0), 1.0))
187 
188 
189  result = LevenbergMarquardtOptimizer(graph, initial).optimizeSafely()
190  self.gtsamAssertEquals(Similarity3(0.7), result.atSimilarity3(5), 0.4)
191 
192  def test_align_via_Sim3_to_poses_skydio32(self) -> None:
193  """Ensure scale estimate of Sim(3) object is non-negative.
194 
195  Comes from real data (from Skydio-32 Crane Mast sequence with a SIFT front-end).
196  """
197  poses_gt = [
198  Pose3(
199  Rot3(
200  [
201  [0.696305769, -0.0106830792, -0.717665705],
202  [0.00546412488, 0.999939148, -0.00958346857],
203  [0.717724415, 0.00275160848, 0.696321772],
204  ]
205  ),
206  Point3(5.83077801, -0.94815149, 0.397751679),
207  ),
208  Pose3(
209  Rot3(
210  [
211  [0.692272397, -0.00529704529, -0.721616549],
212  [0.00634689669, 0.999979075, -0.00125157022],
213  [0.721608079, -0.0037136016, 0.692291531],
214  ]
215  ),
216  Point3(5.03853323, -0.97547405, -0.348177392),
217  ),
218  Pose3(
219  Rot3(
220  [
221  [0.945991981, -0.00633548292, -0.324128225],
222  [0.00450436485, 0.999969379, -0.00639931046],
223  [0.324158843, 0.00459370582, 0.945991552],
224  ]
225  ),
226  Point3(4.13186176, -0.956364218, -0.796029527),
227  ),
228  Pose3(
229  Rot3(
230  [
231  [0.999553623, -0.00346470207, -0.0296740626],
232  [0.00346104216, 0.999993995, -0.00017469881],
233  [0.0296744897, 7.19175654e-05, 0.999559612],
234  ]
235  ),
236  Point3(3.1113898, -0.928583423, -0.90539337),
237  ),
238  Pose3(
239  Rot3(
240  [
241  [0.967850252, -0.00144846042, 0.251522892],
242  [0.000254511591, 0.999988546, 0.00477934325],
243  [-0.251526934, -0.00456167299, 0.967839535],
244  ]
245  ),
246  Point3(2.10584013, -0.921303194, -0.809322971),
247  ),
248  Pose3(
249  Rot3(
250  [
251  [0.969854065, 0.000629052774, 0.243685716],
252  [0.000387180179, 0.999991428, -0.00412234326],
253  [-0.243686221, 0.00409242166, 0.969845508],
254  ]
255  ),
256  Point3(1.0753788, -0.913035975, -0.616584091),
257  ),
258  Pose3(
259  Rot3(
260  [
261  [0.998189342, 0.00110235337, 0.0601400045],
262  [-0.00110890447, 0.999999382, 7.55559042e-05],
263  [-0.060139884, -0.000142108649, 0.998189948],
264  ]
265  ),
266  Point3(0.029993558, -0.951495122, -0.425525143),
267  ),
268  Pose3(
269  Rot3(
270  [
271  [0.999999996, -2.62868666e-05, -8.67178281e-05],
272  [2.62791334e-05, 0.999999996, -8.91767396e-05],
273  [8.67201719e-05, 8.91744604e-05, 0.999999992],
274  ]
275  ),
276  Point3(-0.973569417, -0.936340994, -0.253464928),
277  ),
278  Pose3(
279  Rot3(
280  [
281  [0.99481227, -0.00153645011, 0.101716252],
282  [0.000916919443, 0.999980747, 0.00613725239],
283  [-0.101723724, -0.00601214847, 0.994794525],
284  ]
285  ),
286  Point3(-2.02071256, -0.955446292, -0.240707879),
287  ),
288  Pose3(
289  Rot3(
290  [
291  [0.89795602, -0.00978591184, 0.43997636],
292  [0.00645921401, 0.999938116, 0.00905779513],
293  [-0.440037771, -0.00529159974, 0.89796366],
294  ]
295  ),
296  Point3(-2.94096695, -0.939974858, 0.0934225593),
297  ),
298  Pose3(
299  Rot3(
300  [
301  [0.726299119, -0.00916784876, 0.687318077],
302  [0.00892018672, 0.999952563, 0.0039118575],
303  [-0.687321336, 0.00328981905, 0.726346444],
304  ]
305  ),
306  Point3(-3.72843416, -0.897889251, 0.685129502),
307  ),
308  Pose3(
309  Rot3(
310  [
311  [0.506756029, -0.000331706105, 0.862089858],
312  [0.00613841257, 0.999975964, -0.00322354286],
313  [-0.862068067, 0.00692541035, 0.506745885],
314  ]
315  ),
316  Point3(-4.3909926, -0.890883291, 1.43029524),
317  ),
318  Pose3(
319  Rot3(
320  [
321  [0.129316352, -0.00206958814, 0.991601896],
322  [0.00515932597, 0.999985691, 0.00141424797],
323  [-0.991590634, 0.00493310721, 0.129325179],
324  ]
325  ),
326  Point3(-4.58510846, -0.922534227, 2.36884523),
327  ),
328  Pose3(
329  Rot3(
330  [
331  [0.599853194, -0.00890004681, -0.800060263],
332  [0.00313716318, 0.999956608, -0.00877161373],
333  [0.800103615, 0.00275175707, 0.599855085],
334  ]
335  ),
336  Point3(5.71559638, 0.486863076, 0.279141372),
337  ),
338  Pose3(
339  Rot3(
340  [
341  [0.762552447, 0.000836438681, -0.646926069],
342  [0.00211337894, 0.999990607, 0.00378404105],
343  [0.646923157, -0.00425272942, 0.762543517],
344  ]
345  ),
346  Point3(5.00243443, 0.513321893, -0.466921769),
347  ),
348  Pose3(
349  Rot3(
350  [
351  [0.930381645, -0.00340164355, -0.36657678],
352  [0.00425636616, 0.999989781, 0.00152338305],
353  [0.366567852, -0.00297761145, 0.930386617],
354  ]
355  ),
356  Point3(4.05404984, 0.493385291, -0.827904571),
357  ),
358  Pose3(
359  Rot3(
360  [
361  [0.999996073, -0.00278379707, -0.000323508543],
362  [0.00278790921, 0.999905063, 0.0134941517],
363  [0.000285912831, -0.0134950006, 0.999908897],
364  ]
365  ),
366  Point3(3.04724478, 0.491451306, -0.989571061),
367  ),
368  Pose3(
369  Rot3(
370  [
371  [0.968578343, -0.002544616, 0.248695527],
372  [0.000806130148, 0.999974526, 0.00709200332],
373  [-0.248707238, -0.0066686795, 0.968555721],
374  ]
375  ),
376  Point3(2.05737869, 0.46840177, -0.546344594),
377  ),
378  Pose3(
379  Rot3(
380  [
381  [0.968827882, 0.000182770584, 0.247734722],
382  [-0.000558107079, 0.9999988, 0.00144484904],
383  [-0.24773416, -0.00153807255, 0.968826821],
384  ]
385  ),
386  Point3(1.14019947, 0.469674641, -0.0491053805),
387  ),
388  Pose3(
389  Rot3(
390  [
391  [0.991647805, 0.00197867892, 0.128960146],
392  [-0.00247518407, 0.999990129, 0.00368991165],
393  [-0.128951572, -0.00397829284, 0.991642914],
394  ]
395  ),
396  Point3(0.150270471, 0.457867448, 0.103628642),
397  ),
398  Pose3(
399  Rot3(
400  [
401  [0.992244594, 0.00477781876, -0.124208847],
402  [-0.0037682125, 0.999957938, 0.00836195891],
403  [0.124243574, -0.00782906317, 0.992220862],
404  ]
405  ),
406  Point3(-0.937954641, 0.440532658, 0.154265069),
407  ),
408  Pose3(
409  Rot3(
410  [
411  [0.999591078, 0.00215462857, -0.0285137564],
412  [-0.00183807224, 0.999936443, 0.0111234301],
413  [0.028535911, -0.0110664711, 0.999531507],
414  ]
415  ),
416  Point3(-1.95622231, 0.448914367, -0.0859439782),
417  ),
418  Pose3(
419  Rot3(
420  [
421  [0.931835342, 0.000956922238, 0.362880212],
422  [0.000941640753, 0.99998678, -0.00505501434],
423  [-0.362880252, 0.00505214382, 0.931822122],
424  ]
425  ),
426  Point3(-2.85557418, 0.434739285, 0.0793777177),
427  ),
428  Pose3(
429  Rot3(
430  [
431  [0.781615218, -0.0109886966, 0.623664238],
432  [0.00516954657, 0.999924591, 0.011139446],
433  [-0.623739616, -0.00548270158, 0.781613084],
434  ]
435  ),
436  Point3(-3.67524552, 0.444074681, 0.583718622),
437  ),
438  Pose3(
439  Rot3(
440  [
441  [0.521291761, 0.00264805046, 0.853374051],
442  [0.00659087718, 0.999952868, -0.00712898365],
443  [-0.853352707, 0.00934076542, 0.521249738],
444  ]
445  ),
446  Point3(-4.35541796, 0.413479707, 1.31179007),
447  ),
448  Pose3(
449  Rot3(
450  [
451  [0.320164205, -0.00890839482, 0.947319884],
452  [0.00458409304, 0.999958649, 0.007854118],
453  [-0.947350678, 0.00182799903, 0.320191803],
454  ]
455  ),
456  Point3(-4.71617526, 0.476674479, 2.16502998),
457  ),
458  Pose3(
459  Rot3(
460  [
461  [0.464861609, 0.0268597443, -0.884976415],
462  [-0.00947397841, 0.999633409, 0.0253631906],
463  [0.885333239, -0.00340614699, 0.464945663],
464  ]
465  ),
466  Point3(6.11772094, 1.63029238, 0.491786626),
467  ),
468  Pose3(
469  Rot3(
470  [
471  [0.691647251, 0.0216006293, -0.721912024],
472  [-0.0093228132, 0.999736395, 0.020981541],
473  [0.722174939, -0.00778156302, 0.691666308],
474  ]
475  ),
476  Point3(5.46912979, 1.68759322, -0.288499782),
477  ),
478  Pose3(
479  Rot3(
480  [
481  [0.921208931, 0.00622640471, -0.389018433],
482  [-0.00686296262, 0.999976419, -0.000246683913],
483  [0.389007724, 0.00289706631, 0.92122994],
484  ]
485  ),
486  Point3(4.70156942, 1.72186229, -0.806181015),
487  ),
488  Pose3(
489  Rot3(
490  [
491  [0.822397705, 0.00276497594, 0.568906142],
492  [0.00804891535, 0.999831556, -0.016494662],
493  [-0.568855921, 0.0181442503, 0.822236923],
494  ]
495  ),
496  Point3(-3.51368714, 1.59619714, 0.437437437),
497  ),
498  Pose3(
499  Rot3(
500  [
501  [0.726822937, -0.00545541524, 0.686803193],
502  [0.00913794245, 0.999956756, -0.00172754968],
503  [-0.686764068, 0.00753159111, 0.726841357],
504  ]
505  ),
506  Point3(-4.29737821, 1.61462527, 1.11537749),
507  ),
508  Pose3(
509  Rot3(
510  [
511  [0.402595481, 0.00697612855, 0.915351441],
512  [0.0114113638, 0.999855006, -0.0126391687],
513  [-0.915306892, 0.0155338804, 0.4024575],
514  ]
515  ),
516  Point3(-4.6516433, 1.6323107, 1.96579585),
517  ),
518  ]
519  # from estimated cameras
520  unaligned_pose_dict = {
521  2: Pose3(
522  Rot3(
523  [
524  [-0.681949, -0.568276, 0.460444],
525  [0.572389, -0.0227514, 0.819667],
526  [-0.455321, 0.822524, 0.34079],
527  ]
528  ),
529  Point3(-1.52189, 0.78906, -1.60608),
530  ),
531  4: Pose3(
532  Rot3(
533  [
534  [-0.817805393, -0.575044816, 0.022755196],
535  [0.0478829397, -0.0285875849, 0.998443776],
536  [-0.573499401, 0.81762229, 0.0509139174],
537  ]
538  ),
539  Point3(-1.22653168, 0.686485651, -1.39294168),
540  ),
541  3: Pose3(
542  Rot3(
543  [
544  [-0.783051568, -0.571905041, 0.244448085],
545  [0.314861464, -0.0255673164, 0.948793218],
546  [-0.536369743, 0.819921299, 0.200091385],
547  ]
548  ),
549  Point3(-1.37620079, 0.721408674, -1.49945316),
550  ),
551  5: Pose3(
552  Rot3(
553  [
554  [-0.818916586, -0.572896131, 0.0341415873],
555  [0.0550548476, -0.0192038786, 0.99829864],
556  [-0.571265778, 0.819402974, 0.0472670839],
557  ]
558  ),
559  Point3(-1.06370243, 0.663084159, -1.27672831),
560  ),
561  6: Pose3(
562  Rot3(
563  [
564  [-0.798825521, -0.571995242, 0.186277293],
565  [0.243311017, -0.0240196245, 0.969650869],
566  [-0.550161372, 0.819905178, 0.158360233],
567  ]
568  ),
569  Point3(-0.896250742, 0.640768239, -1.16984756),
570  ),
571  7: Pose3(
572  Rot3(
573  [
574  [-0.786416666, -0.570215296, 0.237493882],
575  [0.305475635, -0.0248440676, 0.951875732],
576  [-0.536873788, 0.821119534, 0.193724669],
577  ]
578  ),
579  Point3(-0.740385043, 0.613956842, -1.05908579),
580  ),
581  8: Pose3(
582  Rot3(
583  [
584  [-0.806252832, -0.57019757, 0.157578877],
585  [0.211046715, -0.0283979846, 0.977063375],
586  [-0.55264424, 0.821016617, 0.143234279],
587  ]
588  ),
589  Point3(-0.58333517, 0.549832698, -0.9542864),
590  ),
591  9: Pose3(
592  Rot3(
593  [
594  [-0.821191354, -0.557772774, -0.120558255],
595  [-0.125347331, -0.0297958331, 0.991665395],
596  [-0.556716092, 0.829458703, -0.0454472483],
597  ]
598  ),
599  Point3(-0.436483039, 0.55003923, -0.850733187),
600  ),
601  21: Pose3(
602  Rot3(
603  [
604  [-0.778607603, -0.575075476, 0.251114312],
605  [0.334920968, -0.0424301164, 0.941290407],
606  [-0.53065822, 0.816999316, 0.225641247],
607  ]
608  ),
609  Point3(-0.736735967, 0.571415247, -0.738663611),
610  ),
611  17: Pose3(
612  Rot3(
613  [
614  [-0.818569806, -0.573904529, 0.0240221722],
615  [0.0512889176, -0.0313725422, 0.998190969],
616  [-0.572112681, 0.818321059, 0.0551155579],
617  ]
618  ),
619  Point3(-1.36150982, 0.724829031, -1.16055631),
620  ),
621  18: Pose3(
622  Rot3(
623  [
624  [-0.812668105, -0.582027424, 0.0285417146],
625  [0.0570298244, -0.0306936169, 0.997900547],
626  [-0.579929436, 0.812589675, 0.0581366453],
627  ]
628  ),
629  Point3(-1.20484771, 0.762370343, -1.05057127),
630  ),
631  20: Pose3(
632  Rot3(
633  [
634  [-0.748446406, -0.580905382, 0.319963926],
635  [0.416860654, -0.0368374152, 0.908223651],
636  [-0.515805363, 0.813137099, 0.269727429],
637  ]
638  ),
639  Point3(569.449421, -907.892555, -794.585647),
640  ),
641  22: Pose3(
642  Rot3(
643  [
644  [-0.826878177, -0.559495019, -0.0569017041],
645  [-0.0452256802, -0.0346974602, 0.99837404],
646  [-0.560559647, 0.828107125, 0.00338702978],
647  ]
648  ),
649  Point3(-0.591431172, 0.55422253, -0.654656597),
650  ),
651  29: Pose3(
652  Rot3(
653  [
654  [-0.785759779, -0.574532433, -0.229115805],
655  [-0.246020939, -0.049553424, 0.967996981],
656  [-0.567499134, 0.81698038, -0.102409921],
657  ]
658  ),
659  Point3(69.4916073, 240.595227, -493.278045),
660  ),
661  23: Pose3(
662  Rot3(
663  [
664  [-0.783524382, -0.548569702, -0.291823276],
665  [-0.316457553, -0.051878563, 0.94718701],
666  [-0.534737468, 0.834493797, -0.132950906],
667  ]
668  ),
669  Point3(-5.93496204, 41.9304933, -3.06881633),
670  ),
671  10: Pose3(
672  Rot3(
673  [
674  [-0.766833992, -0.537641809, -0.350580824],
675  [-0.389506676, -0.0443270797, 0.919956336],
676  [-0.510147213, 0.84200736, -0.175423563],
677  ]
678  ),
679  Point3(234.185458, 326.007989, -691.769777),
680  ),
681  30: Pose3(
682  Rot3(
683  [
684  [-0.754844165, -0.559278755, -0.342662459],
685  [-0.375790683, -0.0594160018, 0.92479787],
686  [-0.537579435, 0.826847636, -0.165321923],
687  ]
688  ),
689  Point3(-5.93398168, 41.9107972, -3.07385081),
690  ),
691  }
692 
693  unaligned_pose_list = []
694  for i in range(32):
695  wTi = unaligned_pose_dict.get(i, None)
696  unaligned_pose_list.append(wTi)
697  # GT poses are the reference/target
698  rSe = align_poses_sim3_ignore_missing(aTi_list=poses_gt, bTi_list=unaligned_pose_list)
699  assert rSe.scale() >= 0
700 
701 
702 def align_poses_sim3_ignore_missing(aTi_list: List[Optional[Pose3]], bTi_list: List[Optional[Pose3]]) -> Similarity3:
703  """Align by similarity transformation, but allow missing estimated poses in the input.
704 
705  Note: this is a wrapper for align_poses_sim3() that allows for missing poses/dropped cameras.
706  This is necessary, as align_poses_sim3() requires a valid pose for every input pair.
707 
708  We force SIM(3) alignment rather than SE(3) alignment.
709  We assume the two trajectories are of the exact same length.
710 
711  Args:
712  aTi_list: reference poses in frame "a" which are the targets for alignment
713  bTi_list: input poses which need to be aligned to frame "a"
714 
715  Returns:
716  aSb: Similarity(3) object that aligns the two pose graphs.
717  """
718  assert len(aTi_list) == len(bTi_list)
719 
720  # only choose target poses for which there is a corresponding estimated pose
721  corresponding_aTi_list = []
722  valid_camera_idxs = []
723  valid_bTi_list = []
724  for i, bTi in enumerate(bTi_list):
725  if bTi is not None:
726  valid_camera_idxs.append(i)
727  valid_bTi_list.append(bTi)
728  corresponding_aTi_list.append(aTi_list[i])
729 
730  aSb = align_poses_sim3(aTi_list=corresponding_aTi_list, bTi_list=valid_bTi_list)
731  return aSb
732 
733 
734 def align_poses_sim3(aTi_list: List[Pose3], bTi_list: List[Pose3]) -> Similarity3:
735  """Align two pose graphs via similarity transformation. Note: poses cannot be missing/invalid.
736 
737  We force SIM(3) alignment rather than SE(3) alignment.
738  We assume the two trajectories are of the exact same length.
739 
740  Args:
741  aTi_list: reference poses in frame "a" which are the targets for alignment
742  bTi_list: input poses which need to be aligned to frame "a"
743 
744  Returns:
745  aSb: Similarity(3) object that aligns the two pose graphs.
746  """
747  n_to_align = len(aTi_list)
748  assert len(aTi_list) == len(bTi_list)
749  assert n_to_align >= 2, "SIM(3) alignment uses at least 2 frames"
750 
751  ab_pairs = list(zip(aTi_list, bTi_list))
752 
753  aSb = Similarity3.Align(ab_pairs)
754 
755  if np.isnan(aSb.scale()) or aSb.scale() == 0:
756  # we have run into a case where points have no translation between them (i.e. panorama).
757  # We will first align the rotations and then align the translation by using centroids.
758  # TODO: handle it in GTSAM
759 
760  # align the rotations first, so that we can find the translation between the two panoramas
761  aSb = Similarity3(aSb.rotation(), np.zeros((3,)), 1.0)
762  aTi_list_rot_aligned = [aSb.transformFrom(bTi) for bTi in bTi_list]
763 
764  # fit a single translation motion to the centroid
765  aTi_centroid = np.array([aTi.translation() for aTi in aTi_list]).mean(axis=0)
766  aTi_rot_aligned_centroid = np.array([aTi.translation() for aTi in aTi_list_rot_aligned]).mean(axis=0)
767 
768  # construct the final SIM3 transform
769  aSb = Similarity3(aSb.rotation(), aTi_centroid - aTi_rot_aligned_centroid, 1.0)
770 
771  return aSb
772 
773 
774 if __name__ == "__main__":
775  unittest.main()
test_Sim3.TestSim3.test_align_poses_scaled_squares
def test_align_poses_scaled_squares(self)
Definition: test_Sim3.py:95
optimize
int optimize(const SfmData &db, const NonlinearFactorGraph &graph, const Values &initial, bool separateCalibration=false)
Definition: timeSFMBAL.h:64
list
Definition: pytypes.h:2168
test_Sim3.TestSim3.test_align_poses_along_straight_line
def test_align_poses_along_straight_line(self)
Definition: test_Sim3.py:25
gtsam::noiseModel::Diagonal::Sigmas
static shared_ptr Sigmas(const Vector &sigmas, bool smart=true)
Definition: NoiseModel.cpp:283
gtsam::utils.test_case.GtsamTestCase.gtsamAssertEquals
def gtsamAssertEquals(self, actual, expected, tol=1e-9)
Definition: test_case.py:19
gtsam::range
Double_ range(const Point2_ &p, const Point2_ &q)
Definition: slam/expressions.h:30
test_Sim3.TestSim3.test_align_via_Sim3_to_poses_skydio32
None test_align_via_Sim3_to_poses_skydio32(self)
Definition: test_Sim3.py:192
test_Sim3.align_poses_sim3_ignore_missing
Similarity3 align_poses_sim3_ignore_missing(List[Optional[Pose3]] aTi_list, List[Optional[Pose3]] bTi_list)
Definition: test_Sim3.py:702
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
gtsam::NonlinearFactorGraph
Definition: NonlinearFactorGraph.h:55
gtsam::LevenbergMarquardtOptimizer
Definition: LevenbergMarquardtOptimizer.h:35
gtsam::utils.test_case
Definition: test_case.py:1
test_Sim3.TestSim3
Definition: test_Sim3.py:22
test_Sim3.align_poses_sim3
Similarity3 align_poses_sim3(List[Pose3] aTi_list, List[Pose3] bTi_list)
Definition: test_Sim3.py:734
test_Sim3.TestSim3.test_sim3_optimization2
None test_sim3_optimization2(self)
Definition: test_Sim3.py:154
gtsam::Values
Definition: Values.h:65
gtsam::utils.test_case.GtsamTestCase
Definition: test_case.py:16
gtsam::LevenbergMarquardtParams
Definition: LevenbergMarquardtParams.h:35
gtsam::Point3
Vector3 Point3
Definition: Point3.h:38
gtsam::noiseModel::Isotropic::Sigma
static shared_ptr Sigma(size_t dim, double sigma, bool smart=true)
Definition: NoiseModel.cpp:616
len
size_t len(handle h)
Get the length of a Python object.
Definition: pytypes.h:2448
test_Sim3.TestSim3.test_align_poses_along_straight_line_gauge
def test_align_poses_along_straight_line_gauge(self)
Definition: test_Sim3.py:60
test_Sim3.TestSim3.test_sim3_optimization
None test_sim3_optimization(self)
Definition: test_Sim3.py:133
sampling::mean
static const Vector2 mean(20, 40)
gtsam::Similarity3
Definition: Similarity3.h:36

gtsam
Author(s):
autogenerated on Wed May 28 2025 03:06:03