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
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_align_via_Sim3_to_poses_skydio32(self) -> None:
134  """Ensure scale estimate of Sim(3) object is non-negative.
135 
136  Comes from real data (from Skydio-32 Crane Mast sequence with a SIFT front-end).
137  """
138  poses_gt = [
139  Pose3(
140  Rot3(
141  [
142  [0.696305769, -0.0106830792, -0.717665705],
143  [0.00546412488, 0.999939148, -0.00958346857],
144  [0.717724415, 0.00275160848, 0.696321772],
145  ]
146  ),
147  Point3(5.83077801, -0.94815149, 0.397751679),
148  ),
149  Pose3(
150  Rot3(
151  [
152  [0.692272397, -0.00529704529, -0.721616549],
153  [0.00634689669, 0.999979075, -0.00125157022],
154  [0.721608079, -0.0037136016, 0.692291531],
155  ]
156  ),
157  Point3(5.03853323, -0.97547405, -0.348177392),
158  ),
159  Pose3(
160  Rot3(
161  [
162  [0.945991981, -0.00633548292, -0.324128225],
163  [0.00450436485, 0.999969379, -0.00639931046],
164  [0.324158843, 0.00459370582, 0.945991552],
165  ]
166  ),
167  Point3(4.13186176, -0.956364218, -0.796029527),
168  ),
169  Pose3(
170  Rot3(
171  [
172  [0.999553623, -0.00346470207, -0.0296740626],
173  [0.00346104216, 0.999993995, -0.00017469881],
174  [0.0296744897, 7.19175654e-05, 0.999559612],
175  ]
176  ),
177  Point3(3.1113898, -0.928583423, -0.90539337),
178  ),
179  Pose3(
180  Rot3(
181  [
182  [0.967850252, -0.00144846042, 0.251522892],
183  [0.000254511591, 0.999988546, 0.00477934325],
184  [-0.251526934, -0.00456167299, 0.967839535],
185  ]
186  ),
187  Point3(2.10584013, -0.921303194, -0.809322971),
188  ),
189  Pose3(
190  Rot3(
191  [
192  [0.969854065, 0.000629052774, 0.243685716],
193  [0.000387180179, 0.999991428, -0.00412234326],
194  [-0.243686221, 0.00409242166, 0.969845508],
195  ]
196  ),
197  Point3(1.0753788, -0.913035975, -0.616584091),
198  ),
199  Pose3(
200  Rot3(
201  [
202  [0.998189342, 0.00110235337, 0.0601400045],
203  [-0.00110890447, 0.999999382, 7.55559042e-05],
204  [-0.060139884, -0.000142108649, 0.998189948],
205  ]
206  ),
207  Point3(0.029993558, -0.951495122, -0.425525143),
208  ),
209  Pose3(
210  Rot3(
211  [
212  [0.999999996, -2.62868666e-05, -8.67178281e-05],
213  [2.62791334e-05, 0.999999996, -8.91767396e-05],
214  [8.67201719e-05, 8.91744604e-05, 0.999999992],
215  ]
216  ),
217  Point3(-0.973569417, -0.936340994, -0.253464928),
218  ),
219  Pose3(
220  Rot3(
221  [
222  [0.99481227, -0.00153645011, 0.101716252],
223  [0.000916919443, 0.999980747, 0.00613725239],
224  [-0.101723724, -0.00601214847, 0.994794525],
225  ]
226  ),
227  Point3(-2.02071256, -0.955446292, -0.240707879),
228  ),
229  Pose3(
230  Rot3(
231  [
232  [0.89795602, -0.00978591184, 0.43997636],
233  [0.00645921401, 0.999938116, 0.00905779513],
234  [-0.440037771, -0.00529159974, 0.89796366],
235  ]
236  ),
237  Point3(-2.94096695, -0.939974858, 0.0934225593),
238  ),
239  Pose3(
240  Rot3(
241  [
242  [0.726299119, -0.00916784876, 0.687318077],
243  [0.00892018672, 0.999952563, 0.0039118575],
244  [-0.687321336, 0.00328981905, 0.726346444],
245  ]
246  ),
247  Point3(-3.72843416, -0.897889251, 0.685129502),
248  ),
249  Pose3(
250  Rot3(
251  [
252  [0.506756029, -0.000331706105, 0.862089858],
253  [0.00613841257, 0.999975964, -0.00322354286],
254  [-0.862068067, 0.00692541035, 0.506745885],
255  ]
256  ),
257  Point3(-4.3909926, -0.890883291, 1.43029524),
258  ),
259  Pose3(
260  Rot3(
261  [
262  [0.129316352, -0.00206958814, 0.991601896],
263  [0.00515932597, 0.999985691, 0.00141424797],
264  [-0.991590634, 0.00493310721, 0.129325179],
265  ]
266  ),
267  Point3(-4.58510846, -0.922534227, 2.36884523),
268  ),
269  Pose3(
270  Rot3(
271  [
272  [0.599853194, -0.00890004681, -0.800060263],
273  [0.00313716318, 0.999956608, -0.00877161373],
274  [0.800103615, 0.00275175707, 0.599855085],
275  ]
276  ),
277  Point3(5.71559638, 0.486863076, 0.279141372),
278  ),
279  Pose3(
280  Rot3(
281  [
282  [0.762552447, 0.000836438681, -0.646926069],
283  [0.00211337894, 0.999990607, 0.00378404105],
284  [0.646923157, -0.00425272942, 0.762543517],
285  ]
286  ),
287  Point3(5.00243443, 0.513321893, -0.466921769),
288  ),
289  Pose3(
290  Rot3(
291  [
292  [0.930381645, -0.00340164355, -0.36657678],
293  [0.00425636616, 0.999989781, 0.00152338305],
294  [0.366567852, -0.00297761145, 0.930386617],
295  ]
296  ),
297  Point3(4.05404984, 0.493385291, -0.827904571),
298  ),
299  Pose3(
300  Rot3(
301  [
302  [0.999996073, -0.00278379707, -0.000323508543],
303  [0.00278790921, 0.999905063, 0.0134941517],
304  [0.000285912831, -0.0134950006, 0.999908897],
305  ]
306  ),
307  Point3(3.04724478, 0.491451306, -0.989571061),
308  ),
309  Pose3(
310  Rot3(
311  [
312  [0.968578343, -0.002544616, 0.248695527],
313  [0.000806130148, 0.999974526, 0.00709200332],
314  [-0.248707238, -0.0066686795, 0.968555721],
315  ]
316  ),
317  Point3(2.05737869, 0.46840177, -0.546344594),
318  ),
319  Pose3(
320  Rot3(
321  [
322  [0.968827882, 0.000182770584, 0.247734722],
323  [-0.000558107079, 0.9999988, 0.00144484904],
324  [-0.24773416, -0.00153807255, 0.968826821],
325  ]
326  ),
327  Point3(1.14019947, 0.469674641, -0.0491053805),
328  ),
329  Pose3(
330  Rot3(
331  [
332  [0.991647805, 0.00197867892, 0.128960146],
333  [-0.00247518407, 0.999990129, 0.00368991165],
334  [-0.128951572, -0.00397829284, 0.991642914],
335  ]
336  ),
337  Point3(0.150270471, 0.457867448, 0.103628642),
338  ),
339  Pose3(
340  Rot3(
341  [
342  [0.992244594, 0.00477781876, -0.124208847],
343  [-0.0037682125, 0.999957938, 0.00836195891],
344  [0.124243574, -0.00782906317, 0.992220862],
345  ]
346  ),
347  Point3(-0.937954641, 0.440532658, 0.154265069),
348  ),
349  Pose3(
350  Rot3(
351  [
352  [0.999591078, 0.00215462857, -0.0285137564],
353  [-0.00183807224, 0.999936443, 0.0111234301],
354  [0.028535911, -0.0110664711, 0.999531507],
355  ]
356  ),
357  Point3(-1.95622231, 0.448914367, -0.0859439782),
358  ),
359  Pose3(
360  Rot3(
361  [
362  [0.931835342, 0.000956922238, 0.362880212],
363  [0.000941640753, 0.99998678, -0.00505501434],
364  [-0.362880252, 0.00505214382, 0.931822122],
365  ]
366  ),
367  Point3(-2.85557418, 0.434739285, 0.0793777177),
368  ),
369  Pose3(
370  Rot3(
371  [
372  [0.781615218, -0.0109886966, 0.623664238],
373  [0.00516954657, 0.999924591, 0.011139446],
374  [-0.623739616, -0.00548270158, 0.781613084],
375  ]
376  ),
377  Point3(-3.67524552, 0.444074681, 0.583718622),
378  ),
379  Pose3(
380  Rot3(
381  [
382  [0.521291761, 0.00264805046, 0.853374051],
383  [0.00659087718, 0.999952868, -0.00712898365],
384  [-0.853352707, 0.00934076542, 0.521249738],
385  ]
386  ),
387  Point3(-4.35541796, 0.413479707, 1.31179007),
388  ),
389  Pose3(
390  Rot3(
391  [
392  [0.320164205, -0.00890839482, 0.947319884],
393  [0.00458409304, 0.999958649, 0.007854118],
394  [-0.947350678, 0.00182799903, 0.320191803],
395  ]
396  ),
397  Point3(-4.71617526, 0.476674479, 2.16502998),
398  ),
399  Pose3(
400  Rot3(
401  [
402  [0.464861609, 0.0268597443, -0.884976415],
403  [-0.00947397841, 0.999633409, 0.0253631906],
404  [0.885333239, -0.00340614699, 0.464945663],
405  ]
406  ),
407  Point3(6.11772094, 1.63029238, 0.491786626),
408  ),
409  Pose3(
410  Rot3(
411  [
412  [0.691647251, 0.0216006293, -0.721912024],
413  [-0.0093228132, 0.999736395, 0.020981541],
414  [0.722174939, -0.00778156302, 0.691666308],
415  ]
416  ),
417  Point3(5.46912979, 1.68759322, -0.288499782),
418  ),
419  Pose3(
420  Rot3(
421  [
422  [0.921208931, 0.00622640471, -0.389018433],
423  [-0.00686296262, 0.999976419, -0.000246683913],
424  [0.389007724, 0.00289706631, 0.92122994],
425  ]
426  ),
427  Point3(4.70156942, 1.72186229, -0.806181015),
428  ),
429  Pose3(
430  Rot3(
431  [
432  [0.822397705, 0.00276497594, 0.568906142],
433  [0.00804891535, 0.999831556, -0.016494662],
434  [-0.568855921, 0.0181442503, 0.822236923],
435  ]
436  ),
437  Point3(-3.51368714, 1.59619714, 0.437437437),
438  ),
439  Pose3(
440  Rot3(
441  [
442  [0.726822937, -0.00545541524, 0.686803193],
443  [0.00913794245, 0.999956756, -0.00172754968],
444  [-0.686764068, 0.00753159111, 0.726841357],
445  ]
446  ),
447  Point3(-4.29737821, 1.61462527, 1.11537749),
448  ),
449  Pose3(
450  Rot3(
451  [
452  [0.402595481, 0.00697612855, 0.915351441],
453  [0.0114113638, 0.999855006, -0.0126391687],
454  [-0.915306892, 0.0155338804, 0.4024575],
455  ]
456  ),
457  Point3(-4.6516433, 1.6323107, 1.96579585),
458  ),
459  ]
460  # from estimated cameras
461  unaligned_pose_dict = {
462  2: Pose3(
463  Rot3(
464  [
465  [-0.681949, -0.568276, 0.460444],
466  [0.572389, -0.0227514, 0.819667],
467  [-0.455321, 0.822524, 0.34079],
468  ]
469  ),
470  Point3(-1.52189, 0.78906, -1.60608),
471  ),
472  4: Pose3(
473  Rot3(
474  [
475  [-0.817805393, -0.575044816, 0.022755196],
476  [0.0478829397, -0.0285875849, 0.998443776],
477  [-0.573499401, 0.81762229, 0.0509139174],
478  ]
479  ),
480  Point3(-1.22653168, 0.686485651, -1.39294168),
481  ),
482  3: Pose3(
483  Rot3(
484  [
485  [-0.783051568, -0.571905041, 0.244448085],
486  [0.314861464, -0.0255673164, 0.948793218],
487  [-0.536369743, 0.819921299, 0.200091385],
488  ]
489  ),
490  Point3(-1.37620079, 0.721408674, -1.49945316),
491  ),
492  5: Pose3(
493  Rot3(
494  [
495  [-0.818916586, -0.572896131, 0.0341415873],
496  [0.0550548476, -0.0192038786, 0.99829864],
497  [-0.571265778, 0.819402974, 0.0472670839],
498  ]
499  ),
500  Point3(-1.06370243, 0.663084159, -1.27672831),
501  ),
502  6: Pose3(
503  Rot3(
504  [
505  [-0.798825521, -0.571995242, 0.186277293],
506  [0.243311017, -0.0240196245, 0.969650869],
507  [-0.550161372, 0.819905178, 0.158360233],
508  ]
509  ),
510  Point3(-0.896250742, 0.640768239, -1.16984756),
511  ),
512  7: Pose3(
513  Rot3(
514  [
515  [-0.786416666, -0.570215296, 0.237493882],
516  [0.305475635, -0.0248440676, 0.951875732],
517  [-0.536873788, 0.821119534, 0.193724669],
518  ]
519  ),
520  Point3(-0.740385043, 0.613956842, -1.05908579),
521  ),
522  8: Pose3(
523  Rot3(
524  [
525  [-0.806252832, -0.57019757, 0.157578877],
526  [0.211046715, -0.0283979846, 0.977063375],
527  [-0.55264424, 0.821016617, 0.143234279],
528  ]
529  ),
530  Point3(-0.58333517, 0.549832698, -0.9542864),
531  ),
532  9: Pose3(
533  Rot3(
534  [
535  [-0.821191354, -0.557772774, -0.120558255],
536  [-0.125347331, -0.0297958331, 0.991665395],
537  [-0.556716092, 0.829458703, -0.0454472483],
538  ]
539  ),
540  Point3(-0.436483039, 0.55003923, -0.850733187),
541  ),
542  21: Pose3(
543  Rot3(
544  [
545  [-0.778607603, -0.575075476, 0.251114312],
546  [0.334920968, -0.0424301164, 0.941290407],
547  [-0.53065822, 0.816999316, 0.225641247],
548  ]
549  ),
550  Point3(-0.736735967, 0.571415247, -0.738663611),
551  ),
552  17: Pose3(
553  Rot3(
554  [
555  [-0.818569806, -0.573904529, 0.0240221722],
556  [0.0512889176, -0.0313725422, 0.998190969],
557  [-0.572112681, 0.818321059, 0.0551155579],
558  ]
559  ),
560  Point3(-1.36150982, 0.724829031, -1.16055631),
561  ),
562  18: Pose3(
563  Rot3(
564  [
565  [-0.812668105, -0.582027424, 0.0285417146],
566  [0.0570298244, -0.0306936169, 0.997900547],
567  [-0.579929436, 0.812589675, 0.0581366453],
568  ]
569  ),
570  Point3(-1.20484771, 0.762370343, -1.05057127),
571  ),
572  20: Pose3(
573  Rot3(
574  [
575  [-0.748446406, -0.580905382, 0.319963926],
576  [0.416860654, -0.0368374152, 0.908223651],
577  [-0.515805363, 0.813137099, 0.269727429],
578  ]
579  ),
580  Point3(569.449421, -907.892555, -794.585647),
581  ),
582  22: Pose3(
583  Rot3(
584  [
585  [-0.826878177, -0.559495019, -0.0569017041],
586  [-0.0452256802, -0.0346974602, 0.99837404],
587  [-0.560559647, 0.828107125, 0.00338702978],
588  ]
589  ),
590  Point3(-0.591431172, 0.55422253, -0.654656597),
591  ),
592  29: Pose3(
593  Rot3(
594  [
595  [-0.785759779, -0.574532433, -0.229115805],
596  [-0.246020939, -0.049553424, 0.967996981],
597  [-0.567499134, 0.81698038, -0.102409921],
598  ]
599  ),
600  Point3(69.4916073, 240.595227, -493.278045),
601  ),
602  23: Pose3(
603  Rot3(
604  [
605  [-0.783524382, -0.548569702, -0.291823276],
606  [-0.316457553, -0.051878563, 0.94718701],
607  [-0.534737468, 0.834493797, -0.132950906],
608  ]
609  ),
610  Point3(-5.93496204, 41.9304933, -3.06881633),
611  ),
612  10: Pose3(
613  Rot3(
614  [
615  [-0.766833992, -0.537641809, -0.350580824],
616  [-0.389506676, -0.0443270797, 0.919956336],
617  [-0.510147213, 0.84200736, -0.175423563],
618  ]
619  ),
620  Point3(234.185458, 326.007989, -691.769777),
621  ),
622  30: Pose3(
623  Rot3(
624  [
625  [-0.754844165, -0.559278755, -0.342662459],
626  [-0.375790683, -0.0594160018, 0.92479787],
627  [-0.537579435, 0.826847636, -0.165321923],
628  ]
629  ),
630  Point3(-5.93398168, 41.9107972, -3.07385081),
631  ),
632  }
633 
634  unaligned_pose_list = []
635  for i in range(32):
636  wTi = unaligned_pose_dict.get(i, None)
637  unaligned_pose_list.append(wTi)
638  # GT poses are the reference/target
639  rSe = align_poses_sim3_ignore_missing(aTi_list=poses_gt, bTi_list=unaligned_pose_list)
640  assert rSe.scale() >= 0
641 
642 
643 def align_poses_sim3_ignore_missing(aTi_list: List[Optional[Pose3]], bTi_list: List[Optional[Pose3]]) -> Similarity3:
644  """Align by similarity transformation, but allow missing estimated poses in the input.
645 
646  Note: this is a wrapper for align_poses_sim3() that allows for missing poses/dropped cameras.
647  This is necessary, as align_poses_sim3() requires a valid pose for every input pair.
648 
649  We force SIM(3) alignment rather than SE(3) alignment.
650  We assume the two trajectories are of the exact same length.
651 
652  Args:
653  aTi_list: reference poses in frame "a" which are the targets for alignment
654  bTi_list: input poses which need to be aligned to frame "a"
655 
656  Returns:
657  aSb: Similarity(3) object that aligns the two pose graphs.
658  """
659  assert len(aTi_list) == len(bTi_list)
660 
661  # only choose target poses for which there is a corresponding estimated pose
662  corresponding_aTi_list = []
663  valid_camera_idxs = []
664  valid_bTi_list = []
665  for i, bTi in enumerate(bTi_list):
666  if bTi is not None:
667  valid_camera_idxs.append(i)
668  valid_bTi_list.append(bTi)
669  corresponding_aTi_list.append(aTi_list[i])
670 
671  aSb = align_poses_sim3(aTi_list=corresponding_aTi_list, bTi_list=valid_bTi_list)
672  return aSb
673 
674 
675 def align_poses_sim3(aTi_list: List[Pose3], bTi_list: List[Pose3]) -> Similarity3:
676  """Align two pose graphs via similarity transformation. Note: poses cannot be missing/invalid.
677 
678  We force SIM(3) alignment rather than SE(3) alignment.
679  We assume the two trajectories are of the exact same length.
680 
681  Args:
682  aTi_list: reference poses in frame "a" which are the targets for alignment
683  bTi_list: input poses which need to be aligned to frame "a"
684 
685  Returns:
686  aSb: Similarity(3) object that aligns the two pose graphs.
687  """
688  n_to_align = len(aTi_list)
689  assert len(aTi_list) == len(bTi_list)
690  assert n_to_align >= 2, "SIM(3) alignment uses at least 2 frames"
691 
692  ab_pairs = list(zip(aTi_list, bTi_list))
693 
694  aSb = Similarity3.Align(ab_pairs)
695 
696  if np.isnan(aSb.scale()) or aSb.scale() == 0:
697  # we have run into a case where points have no translation between them (i.e. panorama).
698  # We will first align the rotations and then align the translation by using centroids.
699  # TODO: handle it in GTSAM
700 
701  # align the rotations first, so that we can find the translation between the two panoramas
702  aSb = Similarity3(aSb.rotation(), np.zeros((3,)), 1.0)
703  aTi_list_rot_aligned = [aSb.transformFrom(bTi) for bTi in bTi_list]
704 
705  # fit a single translation motion to the centroid
706  aTi_centroid = np.array([aTi.translation() for aTi in aTi_list]).mean(axis=0)
707  aTi_rot_aligned_centroid = np.array([aTi.translation() for aTi in aTi_list_rot_aligned]).mean(axis=0)
708 
709  # construct the final SIM3 transform
710  aSb = Similarity3(aSb.rotation(), aTi_centroid - aTi_rot_aligned_centroid, 1.0)
711 
712  return aSb
713 
714 
715 if __name__ == "__main__":
716  unittest.main()
test_Sim3.TestSim3.test_align_poses_scaled_squares
def test_align_poses_scaled_squares(self)
Definition: test_Sim3.py:95
list
Definition: pytypes.h:2166
test_Sim3.TestSim3.test_align_poses_along_straight_line
def test_align_poses_along_straight_line(self)
Definition: test_Sim3.py:25
gtsam::utils.test_case.GtsamTestCase.gtsamAssertEquals
def gtsamAssertEquals(self, actual, expected, tol=1e-9)
Definition: test_case.py:18
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:133
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:643
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::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:675
gtsam::utils.test_case.GtsamTestCase
Definition: test_case.py:15
gtsam::Point3
Vector3 Point3
Definition: Point3.h:38
len
size_t len(handle h)
Get the length of a Python object.
Definition: pytypes.h:2446
test_Sim3.TestSim3.test_align_poses_along_straight_line_gauge
def test_align_poses_along_straight_line_gauge(self)
Definition: test_Sim3.py:60
sampling::mean
static const Vector2 mean(20, 40)
gtsam::Similarity3
Definition: Similarity3.h:36

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