def test_get_stats_for_sfmdata_skydio32(self) -> None: """Real data, from Skydio-32 sequence with the SIFT front-end, if Sim(3) object is corrupted with negative scale. All tracks should have NaN reprojection error, from cheirality errors from the negative scale. """ fx, k1, k2, px, py = 609.94, -8.82687675e-07, -4.25111629e-06, 507, 380 calib = Cal3Bundler(fx, k1, k2, px, py) wTi2 = Pose3( Rot3([ [0.95813172, 0.00187844098, -0.286321635], [-0.00477926501, 0.999944088, -0.00943285149], [0.286287907, 0.0104063212, 0.958087127], ]), Point3(-0.763405555, -0.266165811, -0.118153783), ) wTi3 = Pose3( Rot3([ [0.999985712, 0.00523414604, -0.00108604007], [-0.00524091331, 0.999966264, -0.00632478431], [0.00105289859, 0.00633038578, 0.999979409], ]), Point3(-0.76321922 - 0.266172481 - 0.118146617), ) wTi4 = Pose3( Rot3([ [0.963327067, 0.00797924699, 0.268211287], [-0.0070859796, 0.999965656, -0.00429831651], [-0.268236373, 0.00224014493, 0.963350523], ]), Point3(-0.76303985 - 0.266175812 - 0.11817041), ) wTi5 = Pose3( Rot3([ [0.965261642, 0.00828550155, 0.261153812], [-0.00485679326, 0.99989337, -0.0137717378], [-0.261240071, 0.0120249602, 0.965198956], ]), Point3(-0.762848965 - 0.266179234 - 0.118209764), ) wTi30 = Pose3( Rot3([ [0.761797609, -0.0189987841, 0.647536446], [0.000647893104, 0.999591701, 0.0285659033], [-0.647814775, -0.0213419024, 0.761498877], ]), Point3(-0.780003719, -0.266108015, -0.154604541), ) aligned_cameras = { 2: PinholeCameraCal3Bundler(wTi2, calib), 3: PinholeCameraCal3Bundler(wTi3, calib), 4: PinholeCameraCal3Bundler(wTi4, calib), 5: PinholeCameraCal3Bundler(wTi5, calib), 30: PinholeCameraCal3Bundler(wTi30, calib), } t0 = SfmTrack(pt=[-0.7627727, -0.26624048, -0.11879795]) t0.addMeasurement(2, [184.08586121, 441.31314087]) t0.addMeasurement(4, [18.98637581, 453.21853638]) t1 = SfmTrack(pt=[-0.76277714, -0.26603358, -0.11884205]) t1.addMeasurement(2, [213.51266479, 288.06637573]) t1.addMeasurement(4, [50.23059464, 229.30541992]) t2 = SfmTrack(pt=[-0.7633115, -0.2662322, -0.11826181]) t2.addMeasurement(2, [227.52420044, 695.15087891]) t2.addMeasurement(3, [996.67608643, 705.03125]) t3 = SfmTrack(pt=[-0.76323087, -0.26629859, -0.11836833]) t3.addMeasurement(2, [251.37863159, 702.97064209]) t3.addMeasurement(3, [537.9753418, 732.26025391]) t4 = SfmTrack(pt=[-0.70450081, -0.28115719, -0.19063382]) t4.addMeasurement(2, [253.17749023, 490.47991943]) t4.addMeasurement(3, [13.17782784, 507.57717896]) t5 = SfmTrack(pt=[-0.52781989, -0.31926005, -0.40763909]) t5.addMeasurement(2, [253.52301025, 478.41384888]) t5.addMeasurement(3, [10.92995739, 493.31018066]) t6 = SfmTrack(pt=[-0.74893948, -0.27132075, -0.1360136]) t6.addMeasurement(2, [254.64611816, 533.04730225]) t6.addMeasurement(3, [18.78449249, 557.05041504]) aligned_tracks = [t0, t1, t2, t3, t4, t5, t6] aligned_filtered_data = GtsfmData.from_cameras_and_tracks( cameras=aligned_cameras, tracks=aligned_tracks, number_images=32) metrics = metric_utils.get_stats_for_sfmdata(aligned_filtered_data, suffix="_filtered") assert metrics[0].name == "number_cameras" assert np.isclose(metrics[0]._data, np.array(5.0, dtype=np.float32)) assert metrics[1].name == "number_tracks_filtered" assert np.isclose(metrics[1]._data, np.array(7.0, dtype=np.float32)) assert metrics[2].name == "3d_track_lengths_filtered" assert metrics[2].summary == { "min": 2, "max": 2, "median": 2.0, "mean": 2.0, "stddev": 0.0, "histogram": { "1": 7 }, } assert metrics[3].name == "reprojection_errors_filtered_px" assert metrics[3].summary == { "min": np.nan, "max": np.nan, "median": np.nan, "mean": np.nan, "stddev": np.nan }
def test_align_via_Sim3_to_poses_skydio32(self) -> None: """Real data, from Skydio-32 sequence with the SIFT front-end. Tracks should have identical non-NaN reprojection error before and after alignment. """ 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), ), ] fx, k1, k2, px, py = 609.94, -8.82687675e-07, -4.25111629e-06, 507, 380 calib = Cal3Bundler(fx, k1, k2, px, py) unaligned_cameras = { 2: PinholeCameraCal3Bundler( 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), ), calib, ), 4: PinholeCameraCal3Bundler( 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), ), calib, ), 3: PinholeCameraCal3Bundler( 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), ), calib, ), 5: PinholeCameraCal3Bundler( 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), ), calib, ), 6: PinholeCameraCal3Bundler( 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), ), calib, ), 7: PinholeCameraCal3Bundler( 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), ), calib, ), 8: PinholeCameraCal3Bundler( 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), ), calib, ), 9: PinholeCameraCal3Bundler( 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), ), calib, ), 21: PinholeCameraCal3Bundler( 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), ), calib, ), 17: PinholeCameraCal3Bundler( 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), ), calib, ), 18: PinholeCameraCal3Bundler( 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), ), calib, ), 20: PinholeCameraCal3Bundler( 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), ), calib, ), 22: PinholeCameraCal3Bundler( 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), ), calib, ), 29: PinholeCameraCal3Bundler( 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), ), calib, ), 23: PinholeCameraCal3Bundler( 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), ), calib, ), 10: PinholeCameraCal3Bundler( 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), ), calib, ), 30: PinholeCameraCal3Bundler( 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), ), calib, ), } t0 = SfmTrack(pt=[-0.89190672, 1.21298076, -1.05838554]) t0.addMeasurement(2, [184.08586121, 441.31314087]) t0.addMeasurement(4, [18.98637581, 453.21853638]) t1 = SfmTrack(pt=[-0.76287111, 1.26476165, -1.22710579]) t1.addMeasurement(2, [213.51266479, 288.06637573]) t1.addMeasurement(4, [50.23059464, 229.30541992]) t2 = SfmTrack(pt=[-1.45773622, 0.86221933, -1.47515461]) t2.addMeasurement(2, [227.52420044, 695.15087891]) t2.addMeasurement(3, [996.67608643, 705.03125]) t3 = SfmTrack(pt=[-1.40486691, 0.93824916, -1.35192298]) t3.addMeasurement(2, [251.37863159, 702.97064209]) t3.addMeasurement(3, [537.9753418, 732.26025391]) t4 = SfmTrack(pt=[55.48969812, 52.24862241, 58.84578119]) t4.addMeasurement(2, [253.17749023, 490.47991943]) t4.addMeasurement(3, [13.17782784, 507.57717896]) t5 = SfmTrack(pt=[230.43166291, 206.44760657, 234.25904211]) t5.addMeasurement(2, [253.52301025, 478.41384888]) t5.addMeasurement(3, [10.92995739, 493.31018066]) t6 = SfmTrack(pt=[11.62742671, 13.43484624, 14.50306349]) t6.addMeasurement(2, [254.64611816, 533.04730225]) t6.addMeasurement(3, [18.78449249, 557.05041504]) unaligned_tracks = [t0, t1, t2, t3, t4, t5, t6] unaligned_filtered_data = GtsfmData.from_cameras_and_tracks( cameras=unaligned_cameras, tracks=unaligned_tracks, number_images=32) unaligned_metrics = metrics_utils.get_stats_for_sfmdata( unaligned_filtered_data, suffix="_filtered") aligned_filtered_data = unaligned_filtered_data.align_via_Sim3_to_poses( wTi_list_ref=poses_gt) aligned_metrics = metrics_utils.get_stats_for_sfmdata( aligned_filtered_data, suffix="_filtered") assert unaligned_metrics[3].name == "reprojection_errors_filtered_px" assert aligned_metrics[3].name == "reprojection_errors_filtered_px" # Reprojection error should be unaffected by Sim(3) alignment. for key in ["min", "max", "median", "mean", "stddev"]: assert np.isclose(unaligned_metrics[3].summary[key], aligned_metrics[3].summary[key])