2 GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
3 Atlanta, Georgia 30332-0415
6 See LICENSE for the license information
13 from typing
import List, Optional
19 from gtsam
import Point3, Pose3, Rot3, Similarity3
23 """Test selected Sim3 methods."""
26 """Test Pose3 Align method.
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
34 Rx90 = Rot3.Rx(np.deg2rad(90))
39 eTo1 =
Pose3(
Rot3(), np.array([10, 0, 0]))
40 eTo2 =
Pose3(
Rot3(), np.array([15, 0, 0]))
42 eToi_list = [eTo0, eTo1, eTo2]
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]))
50 wToi_list = [wTo0, wTo1, wTo2]
52 we_pairs =
list(zip(wToi_list, eToi_list))
55 wSe = Similarity3.Align(we_pairs)
57 for wToi, eToi
in zip(wToi_list, eToi_list):
61 """Test if Pose3 Align method can account for gauge ambiguity.
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
69 Rz90 = Rot3.Rz(np.deg2rad(90))
77 eToi_list = [eTo0, eTo1, eTo2]
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]))
85 wToi_list = [wTo0, wTo1, wTo2]
87 we_pairs =
list(zip(wToi_list, eToi_list))
90 wSe = Similarity3.Align(we_pairs)
92 for wToi, eToi
in zip(wToi_list, eToi_list):
96 """Test if Pose3 Align method can account for gauge ambiguity.
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).
103 with gauge ambiguity (2.5x scale)
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))
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]))
116 aTi_list = [aTi0, aTi1, aTi2, aTi3]
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]))
123 bTi_list = [bTi0, bTi1, bTi2, bTi3]
125 ab_pairs =
list(zip(aTi_list, bTi_list))
128 aSb = Similarity3.Align(ab_pairs)
130 for aTi, bTi
in zip(aTi_list, bTi_list):
134 """Ensure scale estimate of Sim(3) object is non-negative.
136 Comes from real data (from Skydio-32 Crane Mast sequence with a SIFT front-end).
142 [0.696305769, -0.0106830792, -0.717665705],
143 [0.00546412488, 0.999939148, -0.00958346857],
144 [0.717724415, 0.00275160848, 0.696321772],
147 Point3(5.83077801, -0.94815149, 0.397751679),
152 [0.692272397, -0.00529704529, -0.721616549],
153 [0.00634689669, 0.999979075, -0.00125157022],
154 [0.721608079, -0.0037136016, 0.692291531],
157 Point3(5.03853323, -0.97547405, -0.348177392),
162 [0.945991981, -0.00633548292, -0.324128225],
163 [0.00450436485, 0.999969379, -0.00639931046],
164 [0.324158843, 0.00459370582, 0.945991552],
167 Point3(4.13186176, -0.956364218, -0.796029527),
172 [0.999553623, -0.00346470207, -0.0296740626],
173 [0.00346104216, 0.999993995, -0.00017469881],
174 [0.0296744897, 7.19175654e-05, 0.999559612],
177 Point3(3.1113898, -0.928583423, -0.90539337),
182 [0.967850252, -0.00144846042, 0.251522892],
183 [0.000254511591, 0.999988546, 0.00477934325],
184 [-0.251526934, -0.00456167299, 0.967839535],
187 Point3(2.10584013, -0.921303194, -0.809322971),
192 [0.969854065, 0.000629052774, 0.243685716],
193 [0.000387180179, 0.999991428, -0.00412234326],
194 [-0.243686221, 0.00409242166, 0.969845508],
197 Point3(1.0753788, -0.913035975, -0.616584091),
202 [0.998189342, 0.00110235337, 0.0601400045],
203 [-0.00110890447, 0.999999382, 7.55559042e-05],
204 [-0.060139884, -0.000142108649, 0.998189948],
207 Point3(0.029993558, -0.951495122, -0.425525143),
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],
217 Point3(-0.973569417, -0.936340994, -0.253464928),
222 [0.99481227, -0.00153645011, 0.101716252],
223 [0.000916919443, 0.999980747, 0.00613725239],
224 [-0.101723724, -0.00601214847, 0.994794525],
227 Point3(-2.02071256, -0.955446292, -0.240707879),
232 [0.89795602, -0.00978591184, 0.43997636],
233 [0.00645921401, 0.999938116, 0.00905779513],
234 [-0.440037771, -0.00529159974, 0.89796366],
237 Point3(-2.94096695, -0.939974858, 0.0934225593),
242 [0.726299119, -0.00916784876, 0.687318077],
243 [0.00892018672, 0.999952563, 0.0039118575],
244 [-0.687321336, 0.00328981905, 0.726346444],
247 Point3(-3.72843416, -0.897889251, 0.685129502),
252 [0.506756029, -0.000331706105, 0.862089858],
253 [0.00613841257, 0.999975964, -0.00322354286],
254 [-0.862068067, 0.00692541035, 0.506745885],
257 Point3(-4.3909926, -0.890883291, 1.43029524),
262 [0.129316352, -0.00206958814, 0.991601896],
263 [0.00515932597, 0.999985691, 0.00141424797],
264 [-0.991590634, 0.00493310721, 0.129325179],
267 Point3(-4.58510846, -0.922534227, 2.36884523),
272 [0.599853194, -0.00890004681, -0.800060263],
273 [0.00313716318, 0.999956608, -0.00877161373],
274 [0.800103615, 0.00275175707, 0.599855085],
277 Point3(5.71559638, 0.486863076, 0.279141372),
282 [0.762552447, 0.000836438681, -0.646926069],
283 [0.00211337894, 0.999990607, 0.00378404105],
284 [0.646923157, -0.00425272942, 0.762543517],
287 Point3(5.00243443, 0.513321893, -0.466921769),
292 [0.930381645, -0.00340164355, -0.36657678],
293 [0.00425636616, 0.999989781, 0.00152338305],
294 [0.366567852, -0.00297761145, 0.930386617],
297 Point3(4.05404984, 0.493385291, -0.827904571),
302 [0.999996073, -0.00278379707, -0.000323508543],
303 [0.00278790921, 0.999905063, 0.0134941517],
304 [0.000285912831, -0.0134950006, 0.999908897],
307 Point3(3.04724478, 0.491451306, -0.989571061),
312 [0.968578343, -0.002544616, 0.248695527],
313 [0.000806130148, 0.999974526, 0.00709200332],
314 [-0.248707238, -0.0066686795, 0.968555721],
317 Point3(2.05737869, 0.46840177, -0.546344594),
322 [0.968827882, 0.000182770584, 0.247734722],
323 [-0.000558107079, 0.9999988, 0.00144484904],
324 [-0.24773416, -0.00153807255, 0.968826821],
327 Point3(1.14019947, 0.469674641, -0.0491053805),
332 [0.991647805, 0.00197867892, 0.128960146],
333 [-0.00247518407, 0.999990129, 0.00368991165],
334 [-0.128951572, -0.00397829284, 0.991642914],
337 Point3(0.150270471, 0.457867448, 0.103628642),
342 [0.992244594, 0.00477781876, -0.124208847],
343 [-0.0037682125, 0.999957938, 0.00836195891],
344 [0.124243574, -0.00782906317, 0.992220862],
347 Point3(-0.937954641, 0.440532658, 0.154265069),
352 [0.999591078, 0.00215462857, -0.0285137564],
353 [-0.00183807224, 0.999936443, 0.0111234301],
354 [0.028535911, -0.0110664711, 0.999531507],
357 Point3(-1.95622231, 0.448914367, -0.0859439782),
362 [0.931835342, 0.000956922238, 0.362880212],
363 [0.000941640753, 0.99998678, -0.00505501434],
364 [-0.362880252, 0.00505214382, 0.931822122],
367 Point3(-2.85557418, 0.434739285, 0.0793777177),
372 [0.781615218, -0.0109886966, 0.623664238],
373 [0.00516954657, 0.999924591, 0.011139446],
374 [-0.623739616, -0.00548270158, 0.781613084],
377 Point3(-3.67524552, 0.444074681, 0.583718622),
382 [0.521291761, 0.00264805046, 0.853374051],
383 [0.00659087718, 0.999952868, -0.00712898365],
384 [-0.853352707, 0.00934076542, 0.521249738],
387 Point3(-4.35541796, 0.413479707, 1.31179007),
392 [0.320164205, -0.00890839482, 0.947319884],
393 [0.00458409304, 0.999958649, 0.007854118],
394 [-0.947350678, 0.00182799903, 0.320191803],
397 Point3(-4.71617526, 0.476674479, 2.16502998),
402 [0.464861609, 0.0268597443, -0.884976415],
403 [-0.00947397841, 0.999633409, 0.0253631906],
404 [0.885333239, -0.00340614699, 0.464945663],
407 Point3(6.11772094, 1.63029238, 0.491786626),
412 [0.691647251, 0.0216006293, -0.721912024],
413 [-0.0093228132, 0.999736395, 0.020981541],
414 [0.722174939, -0.00778156302, 0.691666308],
417 Point3(5.46912979, 1.68759322, -0.288499782),
422 [0.921208931, 0.00622640471, -0.389018433],
423 [-0.00686296262, 0.999976419, -0.000246683913],
424 [0.389007724, 0.00289706631, 0.92122994],
427 Point3(4.70156942, 1.72186229, -0.806181015),
432 [0.822397705, 0.00276497594, 0.568906142],
433 [0.00804891535, 0.999831556, -0.016494662],
434 [-0.568855921, 0.0181442503, 0.822236923],
437 Point3(-3.51368714, 1.59619714, 0.437437437),
442 [0.726822937, -0.00545541524, 0.686803193],
443 [0.00913794245, 0.999956756, -0.00172754968],
444 [-0.686764068, 0.00753159111, 0.726841357],
447 Point3(-4.29737821, 1.61462527, 1.11537749),
452 [0.402595481, 0.00697612855, 0.915351441],
453 [0.0114113638, 0.999855006, -0.0126391687],
454 [-0.915306892, 0.0155338804, 0.4024575],
457 Point3(-4.6516433, 1.6323107, 1.96579585),
461 unaligned_pose_dict = {
465 [-0.681949, -0.568276, 0.460444],
466 [0.572389, -0.0227514, 0.819667],
467 [-0.455321, 0.822524, 0.34079],
470 Point3(-1.52189, 0.78906, -1.60608),
475 [-0.817805393, -0.575044816, 0.022755196],
476 [0.0478829397, -0.0285875849, 0.998443776],
477 [-0.573499401, 0.81762229, 0.0509139174],
480 Point3(-1.22653168, 0.686485651, -1.39294168),
485 [-0.783051568, -0.571905041, 0.244448085],
486 [0.314861464, -0.0255673164, 0.948793218],
487 [-0.536369743, 0.819921299, 0.200091385],
490 Point3(-1.37620079, 0.721408674, -1.49945316),
495 [-0.818916586, -0.572896131, 0.0341415873],
496 [0.0550548476, -0.0192038786, 0.99829864],
497 [-0.571265778, 0.819402974, 0.0472670839],
500 Point3(-1.06370243, 0.663084159, -1.27672831),
505 [-0.798825521, -0.571995242, 0.186277293],
506 [0.243311017, -0.0240196245, 0.969650869],
507 [-0.550161372, 0.819905178, 0.158360233],
510 Point3(-0.896250742, 0.640768239, -1.16984756),
515 [-0.786416666, -0.570215296, 0.237493882],
516 [0.305475635, -0.0248440676, 0.951875732],
517 [-0.536873788, 0.821119534, 0.193724669],
520 Point3(-0.740385043, 0.613956842, -1.05908579),
525 [-0.806252832, -0.57019757, 0.157578877],
526 [0.211046715, -0.0283979846, 0.977063375],
527 [-0.55264424, 0.821016617, 0.143234279],
530 Point3(-0.58333517, 0.549832698, -0.9542864),
535 [-0.821191354, -0.557772774, -0.120558255],
536 [-0.125347331, -0.0297958331, 0.991665395],
537 [-0.556716092, 0.829458703, -0.0454472483],
540 Point3(-0.436483039, 0.55003923, -0.850733187),
545 [-0.778607603, -0.575075476, 0.251114312],
546 [0.334920968, -0.0424301164, 0.941290407],
547 [-0.53065822, 0.816999316, 0.225641247],
550 Point3(-0.736735967, 0.571415247, -0.738663611),
555 [-0.818569806, -0.573904529, 0.0240221722],
556 [0.0512889176, -0.0313725422, 0.998190969],
557 [-0.572112681, 0.818321059, 0.0551155579],
560 Point3(-1.36150982, 0.724829031, -1.16055631),
565 [-0.812668105, -0.582027424, 0.0285417146],
566 [0.0570298244, -0.0306936169, 0.997900547],
567 [-0.579929436, 0.812589675, 0.0581366453],
570 Point3(-1.20484771, 0.762370343, -1.05057127),
575 [-0.748446406, -0.580905382, 0.319963926],
576 [0.416860654, -0.0368374152, 0.908223651],
577 [-0.515805363, 0.813137099, 0.269727429],
580 Point3(569.449421, -907.892555, -794.585647),
585 [-0.826878177, -0.559495019, -0.0569017041],
586 [-0.0452256802, -0.0346974602, 0.99837404],
587 [-0.560559647, 0.828107125, 0.00338702978],
590 Point3(-0.591431172, 0.55422253, -0.654656597),
595 [-0.785759779, -0.574532433, -0.229115805],
596 [-0.246020939, -0.049553424, 0.967996981],
597 [-0.567499134, 0.81698038, -0.102409921],
600 Point3(69.4916073, 240.595227, -493.278045),
605 [-0.783524382, -0.548569702, -0.291823276],
606 [-0.316457553, -0.051878563, 0.94718701],
607 [-0.534737468, 0.834493797, -0.132950906],
610 Point3(-5.93496204, 41.9304933, -3.06881633),
615 [-0.766833992, -0.537641809, -0.350580824],
616 [-0.389506676, -0.0443270797, 0.919956336],
617 [-0.510147213, 0.84200736, -0.175423563],
620 Point3(234.185458, 326.007989, -691.769777),
625 [-0.754844165, -0.559278755, -0.342662459],
626 [-0.375790683, -0.0594160018, 0.92479787],
627 [-0.537579435, 0.826847636, -0.165321923],
630 Point3(-5.93398168, 41.9107972, -3.07385081),
634 unaligned_pose_list = []
636 wTi = unaligned_pose_dict.get(i,
None)
637 unaligned_pose_list.append(wTi)
640 assert rSe.scale() >= 0
644 """Align by similarity transformation, but allow missing estimated poses in the input.
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.
649 We force SIM(3) alignment rather than SE(3) alignment.
650 We assume the two trajectories are of the exact same length.
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"
657 aSb: Similarity(3) object that aligns the two pose graphs.
659 assert len(aTi_list) ==
len(bTi_list)
662 corresponding_aTi_list = []
663 valid_camera_idxs = []
665 for i, bTi
in enumerate(bTi_list):
667 valid_camera_idxs.append(i)
668 valid_bTi_list.append(bTi)
669 corresponding_aTi_list.append(aTi_list[i])
671 aSb =
align_poses_sim3(aTi_list=corresponding_aTi_list, bTi_list=valid_bTi_list)
676 """Align two pose graphs via similarity transformation. Note: poses cannot be missing/invalid.
678 We force SIM(3) alignment rather than SE(3) alignment.
679 We assume the two trajectories are of the exact same length.
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"
686 aSb: Similarity(3) object that aligns the two pose graphs.
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"
692 ab_pairs =
list(zip(aTi_list, bTi_list))
694 aSb = Similarity3.Align(ab_pairs)
696 if np.isnan(aSb.scale())
or aSb.scale() == 0:
702 aSb =
Similarity3(aSb.rotation(), np.zeros((3,)), 1.0)
703 aTi_list_rot_aligned = [aSb.transformFrom(bTi)
for bTi
in bTi_list]
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)
710 aSb =
Similarity3(aSb.rotation(), aTi_centroid - aTi_rot_aligned_centroid, 1.0)
715 if __name__ ==
"__main__":