diff --git a/gtsam/geometry/Similarity3.cpp b/gtsam/geometry/Similarity3.cpp index ea7a6d0498..e8d6e75106 100644 --- a/gtsam/geometry/Similarity3.cpp +++ b/gtsam/geometry/Similarity3.cpp @@ -40,6 +40,8 @@ static Point3Pairs subtractCentroids(const Point3Pairs &abPointPairs, } /// Form inner products x and y and calculate scale. +// We force the scale to be a non-negative quantity +// (see Section 10.1 of https://ethaneade.com/lie_groups.pdf) static double calculateScale(const Point3Pairs &d_abPointPairs, const Rot3 &aRb) { double x = 0, y = 0; @@ -50,7 +52,7 @@ static double calculateScale(const Point3Pairs &d_abPointPairs, y += da.transpose() * da_prime; x += da_prime.transpose() * da_prime; } - const double s = y / x; + const double s = std::fabs(y / x); return s; } diff --git a/python/gtsam/tests/test_Sim3.py b/python/gtsam/tests/test_Sim3.py index 001321e2cd..c00a36435e 100644 --- a/python/gtsam/tests/test_Sim3.py +++ b/python/gtsam/tests/test_Sim3.py @@ -10,6 +10,7 @@ """ # pylint: disable=no-name-in-module import unittest +from typing import List, Optional import numpy as np @@ -129,6 +130,587 @@ def test_align_poses_scaled_squares(self): for aTi, bTi in zip(aTi_list, bTi_list): self.gtsamAssertEquals(aTi, aSb.transformFrom(bTi)) + def test_align_via_Sim3_to_poses_skydio32(self) -> None: + """Ensure scale estimate of Sim(3) object is non-negative. + + Comes from real data (from Skydio-32 Crane Mast sequence with a SIFT front-end). + """ + poses_gt = [ + Pose3( + Rot3( + [ + [0.696305769, -0.0106830792, -0.717665705], + [0.00546412488, 0.999939148, -0.00958346857], + [0.717724415, 0.00275160848, 0.696321772], + ] + ), + Point3(5.83077801, -0.94815149, 0.397751679), + ), + Pose3( + Rot3( + [ + [0.692272397, -0.00529704529, -0.721616549], + [0.00634689669, 0.999979075, -0.00125157022], + [0.721608079, -0.0037136016, 0.692291531], + ] + ), + Point3(5.03853323, -0.97547405, -0.348177392), + ), + Pose3( + Rot3( + [ + [0.945991981, -0.00633548292, -0.324128225], + [0.00450436485, 0.999969379, -0.00639931046], + [0.324158843, 0.00459370582, 0.945991552], + ] + ), + Point3(4.13186176, -0.956364218, -0.796029527), + ), + Pose3( + Rot3( + [ + [0.999553623, -0.00346470207, -0.0296740626], + [0.00346104216, 0.999993995, -0.00017469881], + [0.0296744897, 7.19175654e-05, 0.999559612], + ] + ), + Point3(3.1113898, -0.928583423, -0.90539337), + ), + Pose3( + Rot3( + [ + [0.967850252, -0.00144846042, 0.251522892], + [0.000254511591, 0.999988546, 0.00477934325], + [-0.251526934, -0.00456167299, 0.967839535], + ] + ), + Point3(2.10584013, -0.921303194, -0.809322971), + ), + Pose3( + Rot3( + [ + [0.969854065, 0.000629052774, 0.243685716], + [0.000387180179, 0.999991428, -0.00412234326], + [-0.243686221, 0.00409242166, 0.969845508], + ] + ), + Point3(1.0753788, -0.913035975, -0.616584091), + ), + Pose3( + Rot3( + [ + [0.998189342, 0.00110235337, 0.0601400045], + [-0.00110890447, 0.999999382, 7.55559042e-05], + [-0.060139884, -0.000142108649, 0.998189948], + ] + ), + Point3(0.029993558, -0.951495122, -0.425525143), + ), + Pose3( + Rot3( + [ + [0.999999996, -2.62868666e-05, -8.67178281e-05], + [2.62791334e-05, 0.999999996, -8.91767396e-05], + [8.67201719e-05, 8.91744604e-05, 0.999999992], + ] + ), + Point3(-0.973569417, -0.936340994, -0.253464928), + ), + Pose3( + Rot3( + [ + [0.99481227, -0.00153645011, 0.101716252], + [0.000916919443, 0.999980747, 0.00613725239], + [-0.101723724, -0.00601214847, 0.994794525], + ] + ), + Point3(-2.02071256, -0.955446292, -0.240707879), + ), + Pose3( + Rot3( + [ + [0.89795602, -0.00978591184, 0.43997636], + [0.00645921401, 0.999938116, 0.00905779513], + [-0.440037771, -0.00529159974, 0.89796366], + ] + ), + Point3(-2.94096695, -0.939974858, 0.0934225593), + ), + Pose3( + Rot3( + [ + [0.726299119, -0.00916784876, 0.687318077], + [0.00892018672, 0.999952563, 0.0039118575], + [-0.687321336, 0.00328981905, 0.726346444], + ] + ), + Point3(-3.72843416, -0.897889251, 0.685129502), + ), + Pose3( + Rot3( + [ + [0.506756029, -0.000331706105, 0.862089858], + [0.00613841257, 0.999975964, -0.00322354286], + [-0.862068067, 0.00692541035, 0.506745885], + ] + ), + Point3(-4.3909926, -0.890883291, 1.43029524), + ), + Pose3( + Rot3( + [ + [0.129316352, -0.00206958814, 0.991601896], + [0.00515932597, 0.999985691, 0.00141424797], + [-0.991590634, 0.00493310721, 0.129325179], + ] + ), + Point3(-4.58510846, -0.922534227, 2.36884523), + ), + Pose3( + Rot3( + [ + [0.599853194, -0.00890004681, -0.800060263], + [0.00313716318, 0.999956608, -0.00877161373], + [0.800103615, 0.00275175707, 0.599855085], + ] + ), + Point3(5.71559638, 0.486863076, 0.279141372), + ), + Pose3( + Rot3( + [ + [0.762552447, 0.000836438681, -0.646926069], + [0.00211337894, 0.999990607, 0.00378404105], + [0.646923157, -0.00425272942, 0.762543517], + ] + ), + Point3(5.00243443, 0.513321893, -0.466921769), + ), + Pose3( + Rot3( + [ + [0.930381645, -0.00340164355, -0.36657678], + [0.00425636616, 0.999989781, 0.00152338305], + [0.366567852, -0.00297761145, 0.930386617], + ] + ), + Point3(4.05404984, 0.493385291, -0.827904571), + ), + Pose3( + Rot3( + [ + [0.999996073, -0.00278379707, -0.000323508543], + [0.00278790921, 0.999905063, 0.0134941517], + [0.000285912831, -0.0134950006, 0.999908897], + ] + ), + Point3(3.04724478, 0.491451306, -0.989571061), + ), + Pose3( + Rot3( + [ + [0.968578343, -0.002544616, 0.248695527], + [0.000806130148, 0.999974526, 0.00709200332], + [-0.248707238, -0.0066686795, 0.968555721], + ] + ), + Point3(2.05737869, 0.46840177, -0.546344594), + ), + Pose3( + Rot3( + [ + [0.968827882, 0.000182770584, 0.247734722], + [-0.000558107079, 0.9999988, 0.00144484904], + [-0.24773416, -0.00153807255, 0.968826821], + ] + ), + Point3(1.14019947, 0.469674641, -0.0491053805), + ), + Pose3( + Rot3( + [ + [0.991647805, 0.00197867892, 0.128960146], + [-0.00247518407, 0.999990129, 0.00368991165], + [-0.128951572, -0.00397829284, 0.991642914], + ] + ), + Point3(0.150270471, 0.457867448, 0.103628642), + ), + Pose3( + Rot3( + [ + [0.992244594, 0.00477781876, -0.124208847], + [-0.0037682125, 0.999957938, 0.00836195891], + [0.124243574, -0.00782906317, 0.992220862], + ] + ), + Point3(-0.937954641, 0.440532658, 0.154265069), + ), + Pose3( + Rot3( + [ + [0.999591078, 0.00215462857, -0.0285137564], + [-0.00183807224, 0.999936443, 0.0111234301], + [0.028535911, -0.0110664711, 0.999531507], + ] + ), + Point3(-1.95622231, 0.448914367, -0.0859439782), + ), + Pose3( + Rot3( + [ + [0.931835342, 0.000956922238, 0.362880212], + [0.000941640753, 0.99998678, -0.00505501434], + [-0.362880252, 0.00505214382, 0.931822122], + ] + ), + Point3(-2.85557418, 0.434739285, 0.0793777177), + ), + Pose3( + Rot3( + [ + [0.781615218, -0.0109886966, 0.623664238], + [0.00516954657, 0.999924591, 0.011139446], + [-0.623739616, -0.00548270158, 0.781613084], + ] + ), + Point3(-3.67524552, 0.444074681, 0.583718622), + ), + Pose3( + Rot3( + [ + [0.521291761, 0.00264805046, 0.853374051], + [0.00659087718, 0.999952868, -0.00712898365], + [-0.853352707, 0.00934076542, 0.521249738], + ] + ), + Point3(-4.35541796, 0.413479707, 1.31179007), + ), + Pose3( + Rot3( + [ + [0.320164205, -0.00890839482, 0.947319884], + [0.00458409304, 0.999958649, 0.007854118], + [-0.947350678, 0.00182799903, 0.320191803], + ] + ), + Point3(-4.71617526, 0.476674479, 2.16502998), + ), + Pose3( + Rot3( + [ + [0.464861609, 0.0268597443, -0.884976415], + [-0.00947397841, 0.999633409, 0.0253631906], + [0.885333239, -0.00340614699, 0.464945663], + ] + ), + Point3(6.11772094, 1.63029238, 0.491786626), + ), + Pose3( + Rot3( + [ + [0.691647251, 0.0216006293, -0.721912024], + [-0.0093228132, 0.999736395, 0.020981541], + [0.722174939, -0.00778156302, 0.691666308], + ] + ), + Point3(5.46912979, 1.68759322, -0.288499782), + ), + Pose3( + Rot3( + [ + [0.921208931, 0.00622640471, -0.389018433], + [-0.00686296262, 0.999976419, -0.000246683913], + [0.389007724, 0.00289706631, 0.92122994], + ] + ), + Point3(4.70156942, 1.72186229, -0.806181015), + ), + Pose3( + Rot3( + [ + [0.822397705, 0.00276497594, 0.568906142], + [0.00804891535, 0.999831556, -0.016494662], + [-0.568855921, 0.0181442503, 0.822236923], + ] + ), + Point3(-3.51368714, 1.59619714, 0.437437437), + ), + Pose3( + Rot3( + [ + [0.726822937, -0.00545541524, 0.686803193], + [0.00913794245, 0.999956756, -0.00172754968], + [-0.686764068, 0.00753159111, 0.726841357], + ] + ), + Point3(-4.29737821, 1.61462527, 1.11537749), + ), + Pose3( + Rot3( + [ + [0.402595481, 0.00697612855, 0.915351441], + [0.0114113638, 0.999855006, -0.0126391687], + [-0.915306892, 0.0155338804, 0.4024575], + ] + ), + Point3(-4.6516433, 1.6323107, 1.96579585), + ), + ] + # from estimated cameras + unaligned_pose_dict = { + 2: Pose3( + Rot3( + [ + [-0.681949, -0.568276, 0.460444], + [0.572389, -0.0227514, 0.819667], + [-0.455321, 0.822524, 0.34079], + ] + ), + Point3(-1.52189, 0.78906, -1.60608), + ), + 4: Pose3( + Rot3( + [ + [-0.817805393, -0.575044816, 0.022755196], + [0.0478829397, -0.0285875849, 0.998443776], + [-0.573499401, 0.81762229, 0.0509139174], + ] + ), + Point3(-1.22653168, 0.686485651, -1.39294168), + ), + 3: Pose3( + Rot3( + [ + [-0.783051568, -0.571905041, 0.244448085], + [0.314861464, -0.0255673164, 0.948793218], + [-0.536369743, 0.819921299, 0.200091385], + ] + ), + Point3(-1.37620079, 0.721408674, -1.49945316), + ), + 5: Pose3( + Rot3( + [ + [-0.818916586, -0.572896131, 0.0341415873], + [0.0550548476, -0.0192038786, 0.99829864], + [-0.571265778, 0.819402974, 0.0472670839], + ] + ), + Point3(-1.06370243, 0.663084159, -1.27672831), + ), + 6: Pose3( + Rot3( + [ + [-0.798825521, -0.571995242, 0.186277293], + [0.243311017, -0.0240196245, 0.969650869], + [-0.550161372, 0.819905178, 0.158360233], + ] + ), + Point3(-0.896250742, 0.640768239, -1.16984756), + ), + 7: Pose3( + Rot3( + [ + [-0.786416666, -0.570215296, 0.237493882], + [0.305475635, -0.0248440676, 0.951875732], + [-0.536873788, 0.821119534, 0.193724669], + ] + ), + Point3(-0.740385043, 0.613956842, -1.05908579), + ), + 8: Pose3( + Rot3( + [ + [-0.806252832, -0.57019757, 0.157578877], + [0.211046715, -0.0283979846, 0.977063375], + [-0.55264424, 0.821016617, 0.143234279], + ] + ), + Point3(-0.58333517, 0.549832698, -0.9542864), + ), + 9: Pose3( + Rot3( + [ + [-0.821191354, -0.557772774, -0.120558255], + [-0.125347331, -0.0297958331, 0.991665395], + [-0.556716092, 0.829458703, -0.0454472483], + ] + ), + Point3(-0.436483039, 0.55003923, -0.850733187), + ), + 21: Pose3( + Rot3( + [ + [-0.778607603, -0.575075476, 0.251114312], + [0.334920968, -0.0424301164, 0.941290407], + [-0.53065822, 0.816999316, 0.225641247], + ] + ), + Point3(-0.736735967, 0.571415247, -0.738663611), + ), + 17: Pose3( + Rot3( + [ + [-0.818569806, -0.573904529, 0.0240221722], + [0.0512889176, -0.0313725422, 0.998190969], + [-0.572112681, 0.818321059, 0.0551155579], + ] + ), + Point3(-1.36150982, 0.724829031, -1.16055631), + ), + 18: Pose3( + Rot3( + [ + [-0.812668105, -0.582027424, 0.0285417146], + [0.0570298244, -0.0306936169, 0.997900547], + [-0.579929436, 0.812589675, 0.0581366453], + ] + ), + Point3(-1.20484771, 0.762370343, -1.05057127), + ), + 20: Pose3( + Rot3( + [ + [-0.748446406, -0.580905382, 0.319963926], + [0.416860654, -0.0368374152, 0.908223651], + [-0.515805363, 0.813137099, 0.269727429], + ] + ), + Point3(569.449421, -907.892555, -794.585647), + ), + 22: Pose3( + Rot3( + [ + [-0.826878177, -0.559495019, -0.0569017041], + [-0.0452256802, -0.0346974602, 0.99837404], + [-0.560559647, 0.828107125, 0.00338702978], + ] + ), + Point3(-0.591431172, 0.55422253, -0.654656597), + ), + 29: Pose3( + Rot3( + [ + [-0.785759779, -0.574532433, -0.229115805], + [-0.246020939, -0.049553424, 0.967996981], + [-0.567499134, 0.81698038, -0.102409921], + ] + ), + Point3(69.4916073, 240.595227, -493.278045), + ), + 23: Pose3( + Rot3( + [ + [-0.783524382, -0.548569702, -0.291823276], + [-0.316457553, -0.051878563, 0.94718701], + [-0.534737468, 0.834493797, -0.132950906], + ] + ), + Point3(-5.93496204, 41.9304933, -3.06881633), + ), + 10: Pose3( + Rot3( + [ + [-0.766833992, -0.537641809, -0.350580824], + [-0.389506676, -0.0443270797, 0.919956336], + [-0.510147213, 0.84200736, -0.175423563], + ] + ), + Point3(234.185458, 326.007989, -691.769777), + ), + 30: Pose3( + Rot3( + [ + [-0.754844165, -0.559278755, -0.342662459], + [-0.375790683, -0.0594160018, 0.92479787], + [-0.537579435, 0.826847636, -0.165321923], + ] + ), + Point3(-5.93398168, 41.9107972, -3.07385081), + ), + } + + unaligned_pose_list = [] + for i in range(32): + wTi = unaligned_pose_dict.get(i, None) + unaligned_pose_list.append(wTi) + # GT poses are the reference/target + rSe = align_poses_sim3_ignore_missing(aTi_list=poses_gt, bTi_list=unaligned_pose_list) + assert rSe.scale() >= 0 + + +def align_poses_sim3_ignore_missing(aTi_list: List[Optional[Pose3]], bTi_list: List[Optional[Pose3]]) -> Similarity3: + """Align by similarity transformation, but allow missing estimated poses in the input. + + Note: this is a wrapper for align_poses_sim3() that allows for missing poses/dropped cameras. + This is necessary, as align_poses_sim3() requires a valid pose for every input pair. + + We force SIM(3) alignment rather than SE(3) alignment. + We assume the two trajectories are of the exact same length. + + Args: + aTi_list: reference poses in frame "a" which are the targets for alignment + bTi_list: input poses which need to be aligned to frame "a" + + Returns: + aSb: Similarity(3) object that aligns the two pose graphs. + """ + assert len(aTi_list) == len(bTi_list) + + # only choose target poses for which there is a corresponding estimated pose + corresponding_aTi_list = [] + valid_camera_idxs = [] + valid_bTi_list = [] + for i, bTi in enumerate(bTi_list): + if bTi is not None: + valid_camera_idxs.append(i) + valid_bTi_list.append(bTi) + corresponding_aTi_list.append(aTi_list[i]) + + aSb = align_poses_sim3(aTi_list=corresponding_aTi_list, bTi_list=valid_bTi_list) + return aSb + + +def align_poses_sim3(aTi_list: List[Pose3], bTi_list: List[Pose3]) -> Similarity3: + """Align two pose graphs via similarity transformation. Note: poses cannot be missing/invalid. + + We force SIM(3) alignment rather than SE(3) alignment. + We assume the two trajectories are of the exact same length. + + Args: + aTi_list: reference poses in frame "a" which are the targets for alignment + bTi_list: input poses which need to be aligned to frame "a" + + Returns: + aSb: Similarity(3) object that aligns the two pose graphs. + """ + n_to_align = len(aTi_list) + assert len(aTi_list) == len(bTi_list) + assert n_to_align >= 2, "SIM(3) alignment uses at least 2 frames" + + ab_pairs = Pose3Pairs(list(zip(aTi_list, bTi_list))) + + aSb = Similarity3.Align(ab_pairs) + + if np.isnan(aSb.scale()) or aSb.scale() == 0: + # we have run into a case where points have no translation between them (i.e. panorama). + # We will first align the rotations and then align the translation by using centroids. + # TODO: handle it in GTSAM + + # align the rotations first, so that we can find the translation between the two panoramas + aSb = Similarity3(aSb.rotation(), np.zeros((3,)), 1.0) + aTi_list_rot_aligned = [aSb.transformFrom(bTi) for bTi in bTi_list] + + # fit a single translation motion to the centroid + aTi_centroid = np.array([aTi.translation() for aTi in aTi_list]).mean(axis=0) + aTi_rot_aligned_centroid = np.array([aTi.translation() for aTi in aTi_list_rot_aligned]).mean(axis=0) + + # construct the final SIM3 transform + aSb = Similarity3(aSb.rotation(), aTi_centroid - aTi_rot_aligned_centroid, 1.0) + + return aSb + if __name__ == "__main__": unittest.main()