def get_average_point_color(track: SfmTrack, images: List[Image]) -> Tuple[int, int, int]: """ Args: track: 3d point/landmark and its corresponding 2d measurements in various cameras images: list of all images for this scene Returns: r: red color intensity, in range [0,255] g: green color intensity, in range [0,255] b: blue color intensity, in range [0,255] """ rgb_measurements = [] for k in range(track.number_measurements()): # process each measurement i, uv_measured = track.measurement(k) u, v = np.round(uv_measured).astype(np.int32) # ensure round did not push us out of bounds u = np.clip(u, 0, images[i].width - 1) v = np.clip(v, 0, images[i].height - 1) rgb_measurements += [images[i].value_array[v, u]] r, g, b = np.array(rgb_measurements).mean(axis=0).astype(np.uint8) return r, g, b
def compute_track_reprojection_errors( track_camera_dict: Dict[int, PinholeCameraCal3Bundler], track: SfmTrack) -> Tuple[np.ndarray, float]: """Compute reprojection errors for measurements in the tracks. Args: track_camera_dict: Dict of cameras, with camera indices as keys. track: 3d point/landmark and its corresponding 2d measurements in various cameras Returns: reprojection errors for each measurement (measured in pixels). avg_track_reproj_error: average reprojection error of all meausurements in track. """ errors = [] for k in range(track.numberMeasurements()): # process each measurement i, uv_measured = track.measurement(k) # get the camera associated with the measurement camera = track_camera_dict[i] # Project to camera uv_reprojected, success_flag = camera.projectSafe(track.point3()) # Projection error in camera if success_flag: errors.append(np.linalg.norm(uv_measured - uv_reprojected)) else: # failure in projection errors.append(np.nan) errors = np.array(errors) avg_track_reproj_error = np.nan if np.isnan(errors).all() else np.nanmean( errors) return errors, avg_track_reproj_error
def values_to_gtsfm_data(values: Values, initial_data: GtsfmData) -> GtsfmData: """Cast results from the optimization to GtsfmData object. Args: values: results of factor graph optimization. initial_data: data used to generate the factor graph; used to extract information about poses and 3d points in the graph. Returns: optimized poses and landmarks. """ result = GtsfmData(initial_data.number_images()) # add cameras for i in initial_data.get_valid_camera_indices(): result.add_camera(i, values.atPinholeCameraCal3Bundler(C(i))) # add tracks for j in range(initial_data.number_tracks()): input_track = initial_data.get_track(j) # populate the result with optimized 3D point result_track = SfmTrack(values.atPoint3(P(j))) for measurement_idx in range(input_track.number_measurements()): i, uv = input_track.measurement(measurement_idx) result_track.add_measurement(i, uv) result.add_track(result_track) return result
def apply_Sim3(self, aSb: Similarity3) -> "GtsfmData": """Assume current tracks and cameras are in frame "b", then transport them to frame "a". Returns: New GtsfmData object which has been transformed from frame a to frame b. """ bTi_list = self.get_camera_poses() aTi_list = [aSb.transformFrom(bTi) if bTi is not None else None for bTi in bTi_list] aligned_data = GtsfmData(number_images=self.number_images()) # Update the camera poses to their aligned poses, but use the previous calibration. for i, aTi in enumerate(aTi_list): if aTi is None: continue calibration = self.get_camera(i).calibration() camera_type = gtsfm_types.get_camera_class_for_calibration(calibration) aligned_data.add_camera(i, camera_type(aTi, calibration)) # Align estimated tracks to ground truth. for j in range(self.number_tracks()): # Align each 3d point track_b = self.get_track(index=j) # Place into the "a" reference frame pt_a = aSb.transformFrom(track_b.point3()) track_a = SfmTrack(pt_a) # Copy over the 2d measurements directly into the new track. for k in range(track_b.numberMeasurements()): i, uv = track_b.measurement(k) track_a.addMeasurement(i, uv) aligned_data.add_track(track_a) return aligned_data
def run( self, cameras: Dict[int, PinholeCameraCal3Bundler], corr_idxs_dict: Dict[Tuple[int, int], np.ndarray], keypoints_list: List[Keypoints], ) -> SfmData: """Perform the data association. Args: cameras: dictionary with image index as key, and camera object w/ intrinsics + extrinsics as value. corr_idxs_dict: dictionary, with key as image pair (i1,i2) and value as matching keypoint indices. keypoints_list: keypoints for each image. Returns: cameras and tracks as SfmData """ available_cams = np.array(list(cameras.keys()), dtype=np.uint32) # form few tracks randomly tracks = [] num_tracks = random.randint(5, 10) for _ in range(num_tracks): # obtain 3D points for the track randomly point_3d = np.random.rand(3, 1) # create GTSAM's SfmTrack object sfmTrack = SfmTrack(point_3d) # randomly select cameras for this track selected_cams = np.random.choice(available_cams, self.min_track_len, replace=False) # for each selected camera, randomly select a point for cam_idx in selected_cams: measurement_idx = random.randint(0, len(keypoints_list[cam_idx]) - 1) measurement = keypoints_list[cam_idx].coordinates[measurement_idx] sfmTrack.add_measurement(cam_idx, measurement) tracks.append(sfmTrack) # create the final SfmData object sfm_data = SfmData() for cam in cameras.values(): sfm_data.add_camera(cam) for track in tracks: sfm_data.add_track(track) return sfm_data
def test_select_largest_connected_component(self, graph_largest_cc_mock): """Test pruning to largest connected component according to tracks. The function under test calles the graph utility, which has been mocked and we test the call against the mocked object. """ gtsfm_data = GtsfmData(5) cam = PinholeCameraCal3Bundler(Pose3(), Cal3Bundler()) # add the same camera at all indices for i in range(gtsfm_data.number_images()): gtsfm_data.add_camera(i, cam) # add two tracks to create two connected components track_1 = SfmTrack( np.random.randn(3)) # track with 2 cameras, which will be dropped track_1.add_measurement(idx=0, m=np.random.randn(2)) track_1.add_measurement(idx=3, m=np.random.randn(2)) track_2 = SfmTrack( np.random.randn(3)) # track with 3 cameras, which will be retained track_2.add_measurement(idx=1, m=np.random.randn(2)) track_2.add_measurement(idx=2, m=np.random.randn(2)) track_2.add_measurement(idx=4, m=np.random.randn(2)) gtsfm_data.add_track(track_1) gtsfm_data.add_track(track_2) largest_component_data = gtsfm_data.select_largest_connected_component( ) # check the graph util function called with the edges defined by tracks graph_largest_cc_mock.assert_called_once_with([(0, 3), (1, 2), (1, 4), (2, 4)]) # check the expected cameras coming just from track_2 expected_camera_indices = [1, 2, 4] computed_camera_indices = largest_component_data.get_valid_camera_indices( ) self.assertListEqual(computed_camera_indices, expected_camera_indices) # check that there is just one track expected_num_tracks = 1 computed_num_tracks = largest_component_data.number_tracks() self.assertEqual(computed_num_tracks, expected_num_tracks) # check the exact track computed_track = largest_component_data.get_track(0) self.assertTrue(computed_track.equals(track_2, EQUALITY_TOLERANCE))
def triangulate_two_view_correspondences( cls, camera_i1: gtsfm_types.CAMERA_TYPE, camera_i2: gtsfm_types.CAMERA_TYPE, keypoints_i1: Keypoints, keypoints_i2: Keypoints, corr_idxs: np.ndarray, ) -> Tuple[List[SfmTrack], List[int]]: """Triangulate 2-view correspondences to form 3d tracks. Args: camera_i1: camera for 1st view. camera_i2: camera for 2nd view. keypoints_i1: keypoints for 1st view. keypoints_i2: keypoints for 2nd view. corr_idxs: indices of corresponding keypoints. Returns: Triangulated 3D points as tracks. """ camera_set = (CameraSetCal3Bundler() if isinstance( camera_i1, PinholeCameraCal3Bundler) else CameraSetCal3Fisheye()) camera_set.append(camera_i1) camera_set.append(camera_i2) tracks_3d: List[SfmTrack] = [] valid_indices: List[int] = [] for j, (idx1, idx2) in enumerate(corr_idxs): track_2d = Point2Vector() track_2d.append(keypoints_i1.coordinates[idx1]) track_2d.append(keypoints_i2.coordinates[idx2]) try: triangulated_pt = gtsam.triangulatePoint3( camera_set, track_2d, rank_tol=SVD_DLT_RANK_TOL, optimize=False) track_3d = SfmTrack(triangulated_pt) track_3d.addMeasurement(0, track_2d[0]) track_3d.addMeasurement(1, track_2d[1]) tracks_3d.append(track_3d) valid_indices.append(j) except RuntimeError: pass # logger.error(e) return tracks_3d, valid_indices
def values_to_gtsfm_data(values: Values, initial_data: GtsfmData, shared_calib: bool) -> GtsfmData: """Cast results from the optimization to GtsfmData object. Args: values: results of factor graph optimization. initial_data: data used to generate the factor graph; used to extract information about poses and 3d points in the graph. shared_calib: flag indicating if calibrations were shared between the cameras. Returns: optimized poses and landmarks. """ result = GtsfmData(initial_data.number_images()) is_fisheye_calibration = isinstance(initial_data.get_camera(0), PinholeCameraCal3Fisheye) if is_fisheye_calibration: cal3_value_extraction_lambda = lambda i: values.atCal3Fisheye( K(0 if shared_calib else i)) else: cal3_value_extraction_lambda = lambda i: values.atCal3Bundler( K(0 if shared_calib else i)) camera_class = PinholeCameraCal3Fisheye if is_fisheye_calibration else PinholeCameraCal3Bundler # add cameras for i in initial_data.get_valid_camera_indices(): result.add_camera( i, camera_class(values.atPose3(X(i)), cal3_value_extraction_lambda(i)), ) # add tracks for j in range(initial_data.number_tracks()): input_track = initial_data.get_track(j) # populate the result with optimized 3D point result_track = SfmTrack(values.atPoint3(P(j))) for measurement_idx in range(input_track.numberMeasurements()): i, uv = input_track.measurement(measurement_idx) result_track.addMeasurement(i, uv) result.add_track(result_track) return result
def colmap2gtsfm( cameras: Dict[int, ColmapCamera], images: Dict[int, ColmapImage], points3D: Dict[int, ColmapPoint3D], load_sfmtracks: bool = False, ) -> Tuple[List[Cal3Bundler], List[Pose3], List[str], Optional[List[Point3]]]: """Converts COLMAP-formatted variables to GTSfM format. Args: cameras: dictionary of COLMAP-formatted Cameras images: dictionary of COLMAP-formatted Images points3D: dictionary of COLMAP-formatted Point3Ds return_tracks (optional): whether or not to return tracks Returns: img_fnames: file names of images in images_gtsfm images_gtsfm: list of N camera poses when each image was taken cameras_gtsfm: list of N camera calibrations corresponding to the N images in images_gtsfm sfmtracks_gtsfm: tracks of points in points3D """ # Note: Assumes input cameras use `PINHOLE` model if len(images) == 0 and len(cameras) == 0: raise RuntimeError("No Image or Camera data provided to loader.") cameras_gtsfm, images_gtsfm, img_fnames = [], [], [] image_id_to_idx = { } # keeps track of discrepencies between `image_id` and List index. for idx, img in enumerate(images.values()): images_gtsfm.append(Pose3(Rot3(img.qvec2rotmat()), img.tvec).inverse()) img_fnames.append(img.name) fx, _, cx, cy = cameras[img.camera_id].params[:4] cameras_gtsfm.append(Cal3Bundler(fx, 0.0, 0.0, cx, cy)) image_id_to_idx[img.id] = idx if len(points3D) == 0 and load_sfmtracks: raise RuntimeError("No SfMTrack data provided to loader.") sfmtracks_gtsfm = None if len(points3D) > 0 and load_sfmtracks: sfmtracks_gtsfm = [] for point3D in points3D.values(): sfmtrack = SfmTrack(point3D.xyz) for (image_id, point2d_idx) in zip(point3D.image_ids, point3D.point2D_idxs): sfmtrack.addMeasurement(image_id_to_idx[image_id], images[image_id].xys[point2d_idx]) sfmtracks_gtsfm.append(sfmtrack) return img_fnames, images_gtsfm, cameras_gtsfm, sfmtracks_gtsfm
def add_track(self, track: SfmTrack) -> bool: """Add a track, after checking if all the cameras in the track are already added. Args: track: track to add. Returns: Flag indicating the success of adding operation. """ # check if all cameras are already added for j in range(track.number_measurements()): i, _ = track.measurement(j) if i not in self._cameras: return False self._tracks.append(track) return True
def test_data(self): """Test functions in SfmData""" # Create new track with 3 measurements i1, i2, i3 = 3, 5, 6 uv_i1 = Point2(21.23, 45.64) # translating along X-axis uv_i2 = Point2(45.7, 45.64) uv_i3 = Point2(68.35, 45.64) # add measurements and arbitrary point to the track measurements = [(i1, uv_i1), (i2, uv_i2), (i3, uv_i3)] pt = Point3(1.0, 6.0, 2.0) track2 = SfmTrack(pt) track2.addMeasurement(i1, uv_i1) track2.addMeasurement(i2, uv_i2) track2.addMeasurement(i3, uv_i3) self.data.addTrack(self.tracks) self.data.addTrack(track2) # Number of tracks in SfmData is 2 self.assertEqual(self.data.numberTracks(), 2) # camera idx of first measurement of second track corresponds to i1 cam_idx, img_measurement = self.data.track(1).measurement(0) self.assertEqual(cam_idx, i1)
def test_pick_cameras(self): """Test picking cameras.""" obj = copy.deepcopy(EXAMPLE_DATA) # add a new track with just camera 0 and 2 track_to_add = SfmTrack(np.array([0, -2.0, 5.0])) track_to_add.add_measurement(idx=0, m=np.array([20.0, 5.0])) track_to_add.add_measurement(idx=2, m=np.array([60.0, 50.0])) obj.add_track(track_to_add) # pick the cameras at index 0 and 2, and hence dropping camera at index 1. cams_to_pick = [0, 2] computed = GtsfmData.from_selected_cameras(obj, cams_to_pick) # test the camera has actually been dropped self.assertListEqual(computed.get_valid_camera_indices(), cams_to_pick) # test the camera objects self.assertEqual(computed.get_camera(0), obj.get_camera(0)) self.assertEqual(computed.get_camera(2), obj.get_camera(2)) # check the track self.assertEqual(computed.number_tracks(), 1) self.assertTrue( computed.get_track(0).equals(track_to_add, EQUALITY_TOLERANCE))
def test_get_track(self): """Testing getter for track.""" expected_track = SfmTrack( np.array([6.41689062, 0.38897032, -23.58628273])) expected_track.add_measurement(0, np.array([383.88000488, 15.2999897])) expected_track.add_measurement(1, np.array([559.75, 106.15000153])) computed = EXAMPLE_DATA.get_track(1) # comparing just the point because track equality is failing np.testing.assert_allclose(computed.point3(), expected_track.point3())
def test_add_track_with_valid_cameras(self): """Testing track addition when all cameras in track are already present.""" gtsfm_data = copy.deepcopy(EXAMPLE_DATA) # add a track on camera #0 and #1, which exists in the data track_to_add = SfmTrack(np.array([0, -2.0, 5.0])) track_to_add.add_measurement(idx=0, m=np.array([20.0, 5.0])) track_to_add.add_measurement(idx=1, m=np.array([60.0, 50.0])) self.assertTrue(gtsfm_data.add_track(track_to_add))
def test_add_track_with_nonexistant_cameras(self): """Testing track addition where some cameras are not in tracks, resulting in failure.""" gtsfm_data = copy.deepcopy(EXAMPLE_DATA) # add a track on camera #0 and #1, which exists in the data track_to_add = SfmTrack(np.array([0, -2.0, 5.0])) track_to_add.add_measurement(idx=0, m=np.array([20.0, 5.0])) track_to_add.add_measurement(idx=3, m=np.array( [60.0, 50.0])) # this camera does not exist self.assertFalse(gtsfm_data.add_track(track_to_add))
def test_compute_track_reprojection_errors(): """Ensure that reprojection error is computed properly within a track. # For camera 0: # [13] = [10,0,3] [1,0,0 | 0] [1] # [24] = [0,10,4] * [0,1,0 | 0] *[2] # [1] = [0, 0,1] [0,0,1 | 0] [1] # [1] # For camera 1: # [-7] = [10,0,3] [1,0,0 |-2] [1] # [44] = [0,10,4] * [0,1,0 | 2] *[2] # [1] = [0, 0,1] [0,0,1 | 0] [1] # [1] """ wTi0 = Pose3(Rot3.RzRyRx(0, 0, 0), np.zeros((3, 1))) wTi1 = Pose3(Rot3.RzRyRx(0, 0, 0), np.array([2, -2, 0])) f = 10 k1 = 0 k2 = 0 u0 = 3 v0 = 4 K0 = Cal3Bundler(f, k1, k2, u0, v0) K1 = Cal3Bundler(f, k1, k2, u0, v0) track_camera_dict = { 0: PinholeCameraCal3Bundler(wTi0, K0), 1: PinholeCameraCal3Bundler(wTi1, K1) } triangulated_pt = np.array([1, 2, 1]) track_3d = SfmTrack(triangulated_pt) # in camera 0 track_3d.addMeasurement(idx=0, m=np.array([13, 24])) # in camera 1 track_3d.addMeasurement(idx=1, m=np.array( [-8, 43])) # should be (-7,44), 1 px error in each dim errors, avg_track_reproj_error = reproj_utils.compute_track_reprojection_errors( track_camera_dict, track_3d) expected_errors = np.array([0, np.sqrt(2)]) np.testing.assert_allclose(errors, expected_errors) assert avg_track_reproj_error == np.sqrt(2) / 2
def test_get_average_point_color(self): """Ensure 3d point color is computed as mean of RGB per 2d measurement.""" # random point; 2d measurements below are dummy locations (not actual projection) triangulated_pt = np.array([1, 2, 1]) track_3d = SfmTrack(triangulated_pt) # in camera 0 track_3d.addMeasurement(idx=0, m=np.array([130, 80])) # in camera 1 track_3d.addMeasurement(idx=1, m=np.array([10, 60])) img0 = np.zeros((100, 200, 3), dtype=np.uint8) img0[80, 130] = np.array([40, 50, 60]) img1 = np.zeros((100, 200, 3), dtype=np.uint8) img1[60, 10] = np.array([60, 70, 80]) images = {0: Image(img0), 1: Image(img1)} r, g, b = image_utils.get_average_point_color(track_3d, images) self.assertEqual(r, 50) self.assertEqual(g, 60) self.assertEqual(b, 70)
def test_sfmTrack_roundtrip(self): obj = SfmTrack(Point3(1, 1, 0)) obj.add_measurement(0, Point2(-1, 5)) obj.add_measurement(1, Point2(6, 2)) self.assertEqualityOnPickleRoundtrip(obj)
class TestSfmData(GtsamTestCase): """Tests for SfmData and SfmTrack modules.""" def setUp(self): """Initialize SfmData and SfmTrack""" self.data = SfmData() # initialize SfmTrack with 3D point self.tracks = SfmTrack() def test_tracks(self): """Test functions in SfmTrack""" # measurement is of format (camera_idx, imgPoint) # create arbitrary camera indices for two cameras i1, i2 = 4, 5 # create arbitrary image measurements for cameras i1 and i2 uv_i1 = Point2(12.6, 82) # translating point uv_i1 along X-axis uv_i2 = Point2(24.88, 82) # add measurements to the track self.tracks.addMeasurement(i1, uv_i1) self.tracks.addMeasurement(i2, uv_i2) # Number of measurements in the track is 2 self.assertEqual(self.tracks.numberMeasurements(), 2) # camera_idx in the first measurement of the track corresponds to i1 cam_idx, img_measurement = self.tracks.measurement(0) self.assertEqual(cam_idx, i1) np.testing.assert_array_almost_equal(Point3(0., 0., 0.), self.tracks.point3()) def test_data(self): """Test functions in SfmData""" # Create new track with 3 measurements i1, i2, i3 = 3, 5, 6 uv_i1 = Point2(21.23, 45.64) # translating along X-axis uv_i2 = Point2(45.7, 45.64) uv_i3 = Point2(68.35, 45.64) # add measurements and arbitrary point to the track measurements = [(i1, uv_i1), (i2, uv_i2), (i3, uv_i3)] pt = Point3(1.0, 6.0, 2.0) track2 = SfmTrack(pt) track2.addMeasurement(i1, uv_i1) track2.addMeasurement(i2, uv_i2) track2.addMeasurement(i3, uv_i3) self.data.addTrack(self.tracks) self.data.addTrack(track2) # Number of tracks in SfmData is 2 self.assertEqual(self.data.numberTracks(), 2) # camera idx of first measurement of second track corresponds to i1 cam_idx, img_measurement = self.data.track(1).measurement(0) self.assertEqual(cam_idx, i1) def test_Balbianello(self): """ Check that we can successfully read a bundler file and create a factor graph from it """ # The structure where we will save the SfM data filename = gtsam.findExampleDataFile("Balbianello.out") sfm_data = SfmData.FromBundlerFile(filename) # Check number of things self.assertEqual(5, sfm_data.numberCameras()) self.assertEqual(544, sfm_data.numberTracks()) track0 = sfm_data.track(0) self.assertEqual(3, track0.numberMeasurements()) # Check projection of a given point self.assertEqual(0, track0.measurement(0)[0]) camera0 = sfm_data.camera(0) expected = camera0.project(track0.point3()) actual = track0.measurement(0)[1] self.gtsamAssertEquals(expected, actual, 1) # We share *one* noiseModel between all projection factors model = gtsam.noiseModel.Isotropic.Sigma(2, 1.0) # one pixel in u and v # Convert to NonlinearFactorGraph graph = sfm_data.sfmFactorGraph(model) self.assertEqual(1419, graph.size()) # regression # Get initial estimate values = gtsam.initialCamerasAndPointsEstimate(sfm_data) self.assertEqual(549, values.size()) # regression
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])
def setUp(self): """Initialize SfmData and SfmTrack""" self.data = SfmData() # initialize SfmTrack with 3D point self.tracks = SfmTrack()
def triangulate( self, track_2d: SfmTrack2d ) -> Tuple[Optional[SfmTrack], Optional[float], bool]: """Triangulates 3D point according to the configured triangulation mode. Args: track: feature track from which measurements are to be extracted Returns: track with inlier measurements and 3D landmark. None returned if triangulation fails or has high error. avg_track_reproj_error: reprojection error of 3d triangulated point to each image plane Note: this may be "None" if the 3d point could not be triangulated successfully due to a cheirality exception or insufficient number of RANSAC inlier measurements is_cheirality_failure: boolean representing whether the selected 2d measurements lead to a cheirality exception upon triangulation """ if self.mode in [ TriangulationParam.RANSAC_SAMPLE_UNIFORM, TriangulationParam.RANSAC_SAMPLE_BIASED_BASELINE, TriangulationParam.RANSAC_TOPK_BASELINES, ]: best_inliers = self.execute_ransac_variant(track_2d) elif self.mode == TriangulationParam.NO_RANSAC: best_inliers = np.ones(len(track_2d.measurements), dtype=bool) # all marked as inliers inlier_idxs = (np.where(best_inliers)[0]).tolist() is_cheirality_failure = False if len(inlier_idxs) < 2: return None, None, is_cheirality_failure inlier_track = track_2d.select_subset(inlier_idxs) camera_track, measurement_track = self.extract_measurements( inlier_track) try: triangulated_pt = gtsam.triangulatePoint3( camera_track, measurement_track, rank_tol=SVD_DLT_RANK_TOL, optimize=True, ) except RuntimeError: is_cheirality_failure = True return None, None, is_cheirality_failure # compute reprojection errors for each measurement reproj_errors = self.compute_track_reprojection_errors( inlier_track.measurements, triangulated_pt) # all the measurements should have error < threshold if not np.all(reproj_errors < self.reproj_error_thresh): return None, reproj_errors.mean(), is_cheirality_failure track_3d = SfmTrack(triangulated_pt) for i, uv in inlier_track.measurements: track_3d.add_measurement(i, uv) avg_track_reproj_error = reproj_errors.mean() return track_3d, avg_track_reproj_error, is_cheirality_failure
def test_get_ortho_axis_alignment_transform(self) -> None: """Tests the get_ortho_axis_alignment_transform() function with a GtsfmData object containing 3 camera frustums and 6 points in the point cloud. All points lie on z=0 plane. All frustums lie on z=2 plane and look down on the z=0 plane. sample_data: output_data: y y | o | | | o | o | c c | c | ------------- x ==> --o--c-----c--o-- x o | c | | o o | | c = point at (xi,yi,0) with a camera frustum at (xi,yi,2) o = point at (xi,yi,0) """ sample_data = GtsfmData(number_images=3) default_intrinsics = Cal3Bundler(fx=100, k1=0, k2=0, u0=0, v0=0) # Add 3 camera frustums to sample_data (looking down at z=0 plane) cam_translations = np.array([[-1, 1, 2], [1, 1, 2], [1, -1, 2]]) for i in range(len(cam_translations)): camera = PinholeCameraCal3Bundler( Pose3(Rot3(), cam_translations[i, :]), default_intrinsics) sample_data.add_camera(i, camera) # Add 6 tracks to sample_data # fmt: off points3d = np.array([ [1, 1, 0], [-1, 1, 0], [-2, 2, 0], [-1, -1, 0], [1, -1, 0], [2, -2, 0], [5, 5, 0] # represents an outlier in this set of points ]) # fmt: on for pt_3d in points3d: sample_data.add_track(SfmTrack(pt_3d)) # Apply alignment transformation to sample_data walignedTw = ellipsoid_utils.get_ortho_axis_alignment_transform( sample_data) walignedSw = Similarity3(R=walignedTw.rotation(), t=walignedTw.translation(), s=1.0) sample_data = sample_data.apply_Sim3(walignedSw) # Verify correct 3d points. computed_3d_points = np.array([ sample_data.get_track(i).point3() for i in range(sample_data.number_tracks()) ]) expected_3d_points = np.array([ [0, np.sqrt(2), 0], [-np.sqrt(2), 0, 0], [-2 * np.sqrt(2), 0, 0], [0, -np.sqrt(2), 0], [np.sqrt(2), 0, 0], [2 * np.sqrt(2), 0, 0], [0, 5 * np.sqrt(2), 0], ]) npt.assert_almost_equal(computed_3d_points, expected_3d_points, decimal=3) # Verify correct camera poses. expected_wTi_list = [ Pose3(walignedTw.rotation(), np.array([-np.sqrt(2), 0, 2])), Pose3(walignedTw.rotation(), np.array([0, np.sqrt(2), 2])), Pose3(walignedTw.rotation(), np.array([np.sqrt(2), 0, 2])), ] computed_wTi_list = sample_data.get_camera_poses() for wTi_computed, wTi_expected in zip(computed_wTi_list, expected_wTi_list): assert wTi_computed.equals(wTi_expected, tol=1e-9)
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 triangulate( self, track_2d: SfmTrack2d ) -> Tuple[Optional[SfmTrack], Optional[float], TriangulationExitCode]: """Triangulates 3D point according to the configured triangulation mode. Args: track: feature track from which measurements are to be extracted Returns: track with inlier measurements and 3D landmark. None returned if triangulation fails or has high error. avg_track_reproj_error: reprojection error of 3d triangulated point to each image plane Note: this may be "None" if the 3d point could not be triangulated successfully due to a cheirality exception or insufficient number of RANSAC inlier measurements is_cheirality_failure: boolean representing whether the selected 2d measurements lead to a cheirality exception upon triangulation """ # Check if we will run RANSAC, or not. if self.options.mode in [ TriangulationSamplingMode.RANSAC_SAMPLE_UNIFORM, TriangulationSamplingMode.RANSAC_SAMPLE_BIASED_BASELINE, TriangulationSamplingMode.RANSAC_TOPK_BASELINES, ]: best_inliers = self.execute_ransac_variant(track_2d) elif self.options.mode == TriangulationSamplingMode.NO_RANSAC: best_inliers = np.ones(len(track_2d.measurements), dtype=bool) # all marked as inliers # Verify we have at least 2 inliers. inlier_idxs = (np.where(best_inliers)[0]).tolist() if len(inlier_idxs) < 2: return None, None, TriangulationExitCode.INLIERS_UNDERCONSTRAINED # Extract keypoint measurements corresponding to inlier indices. inlier_track = track_2d.select_subset(inlier_idxs) track_cameras, track_measurements = self.extract_measurements( inlier_track) # Exit if we do not have at least 2 measurements in cameras with estimated poses. if track_cameras is None: return None, None, TriangulationExitCode.POSES_UNDERCONSTRAINED # Triangulate and check for cheirality failure from GTSAM. try: triangulated_pt = gtsam.triangulatePoint3( track_cameras, track_measurements, rank_tol=SVD_DLT_RANK_TOL, optimize=True, ) except RuntimeError: return None, None, TriangulationExitCode.CHEIRALITY_FAILURE # Compute reprojection errors for each measurement. reproj_errors, avg_track_reproj_error = reproj_utils.compute_point_reprojection_errors( self.track_camera_dict, triangulated_pt, inlier_track.measurements) # Check that all measurements are within reprojection error threshold. if not np.all( reproj_errors.flatten() < self.options.reproj_error_threshold): return None, avg_track_reproj_error, TriangulationExitCode.EXCEEDS_REPROJ_THRESH # Create a gtsam.SfmTrack with the triangulated 3d point and associated 2d measurements. track_3d = SfmTrack(triangulated_pt) for i, uv in inlier_track.measurements: track_3d.addMeasurement(i, uv) return track_3d, avg_track_reproj_error, TriangulationExitCode.SUCCESS
def add_dummy_measurements_to_track(track: SfmTrack) -> SfmTrack: """Add some dummy 2d measurements in three views in cameras 0,1,3.""" track.addMeasurement(0, np.array([100, 200])) track.addMeasurement(1, np.array([300, 400])) track.addMeasurement(3, np.array([500, 600])) return track
def test_align_via_Sim3_to_poses(self) -> None: """Ensure that alignment of a SFM result to ground truth camera poses works correctly. Consider a simple example, wih 3 estimated poses and 2 points. When fitting the Similarity(3), all correspondences should have no noise, and alignment should be exact. GT: =========================================== | . (pose 3) . X . . | . (pose 2). . (pose 0) . .(pose 1) . --X . . ----X . . --- X . . | | | Estimate: ===================================== | . (pose 3) | . | X . . | | . . (pose 0) | .(pose 1) . | X . . --- X . . | --------------------------- | | """ dummy_calibration = Cal3Bundler(fx=900, k1=0, k2=0, u0=100, v0=100) # fmt: off wTi_list_gt = [ Pose3(Rot3(), np.array([3, 0, 0])), # wTi0 Pose3(Rot3(), np.array([0, 0, 0])), # wTi1 Pose3(Rot3(), np.array([0, -3, 0])), # wTi2 Pose3(Rot3(), np.array([0, 3, 0])), # wTi3 ] # points_gt = [ # np.array([1, 1, 0]), # np.array([3, 3, 0]) # ] # pose graph is scaled by a factor of 2, and shifted also. wTi_list_est = [ Pose3(Rot3(), np.array([8, 2, 0])), # wTi0 Pose3(Rot3(), np.array([2, 2, 0])), # wTi1 None, # wTi2 Pose3(Rot3(), np.array([2, 8, 0])), # wTi3 ] points_est = [np.array([4, 4, 0]), np.array([8, 8, 0])] # fmt: on def add_dummy_measurements_to_track(track: SfmTrack) -> SfmTrack: """Add some dummy 2d measurements in three views in cameras 0,1,3.""" track.addMeasurement(0, np.array([100, 200])) track.addMeasurement(1, np.array([300, 400])) track.addMeasurement(3, np.array([500, 600])) return track sfm_result = GtsfmData(number_images=4) gt_gtsfm_data = GtsfmData(number_images=4) for gtsfm_data, wTi_list in zip([sfm_result, gt_gtsfm_data], [wTi_list_est, wTi_list_gt]): for i, wTi in enumerate(wTi_list): if wTi is None: continue gtsfm_data.add_camera( i, PinholeCameraCal3Bundler(wTi, dummy_calibration)) for pt in points_est: track = SfmTrack(pt) track = add_dummy_measurements_to_track(track) gtsfm_data.add_track(track) aligned_sfm_result = sfm_result.align_via_Sim3_to_poses( wTi_list_ref=gt_gtsfm_data.get_camera_poses()) # tracks and poses should match GT now, after applying estimated scale and shift. assert aligned_sfm_result == gt_gtsfm_data # 3d points from tracks should now match the GT. assert np.allclose( aligned_sfm_result.get_track(0).point3(), np.array([1.0, 1.0, 0.0])) assert np.allclose( aligned_sfm_result.get_track(1).point3(), np.array([3.0, 3.0, 0.0]))
def test_point_cloud_cameras_locked(self) -> None: """Tests the get_ortho_axis_alignment_transform() function with a GtsfmData object containing 11 point cloud points and 12 camera frustums from the door-12 dataset. Determines if the points and frustums are properly "locked" in with one another before and after the alignment transformation is applied. """ sample_data = GtsfmData(number_images=12) # Instantiate OlssonLoader to read camera poses from door12 dataset. wTi_list = self.loader._wTi_list # Add 12 camera frustums to sample_data. default_intrinsics = Cal3Bundler(fx=100, k1=0, k2=0, u0=0, v0=0) for idx, pose in enumerate(wTi_list): camera = PinholeCameraCal3Bundler(pose, default_intrinsics) sample_data.add_camera(idx, camera) # fmt: off # These points are taken directly from the first 11 points generated by GTSFM on the door12 dataset (without # any separate alignment transformation being applied) points_3d = np.array( [[-1.4687794397729077, -1.4966178675020756, 14.583277665978546], [-1.6172612359102505, -1.0951470733744013, 14.579095414379562], [-3.2190882723771783, -4.463465966172758, 14.444076631000476], [-0.6754206497590093, -1.1132530165104157, 14.916222213341355], [-1.5514099044537981, -1.305810425894855, 14.584788688422206], [-1.551319353347404, -1.304881682597853, 14.58246449772602], [-1.9055918588057448, -1.192867982227922, 14.446379510423219], [-1.5936792439193013, -1.4398818807488012, 14.587749795933021], [-1.5937405395983737, -1.4401641027442411, 14.588167699143174], [-1.6599318889904735, -1.2273604755959784, 14.57861988411431], [2.1935589900444867, 1.6233406628428935, 12.610234497076608]]) # fmt: on # Add all point cloud points to sample_data for point_3d in points_3d: sample_data.add_track(SfmTrack(point_3d)) camera_translations = np.array( [pose.translation() for pose in sample_data.get_camera_poses()]) initial_relative_distances = scipy.spatial.distance.cdist( camera_translations, points_3d, metric="euclidean") # Apply alignment transformation to sample_data walignedTw = ellipsoid_utils.get_ortho_axis_alignment_transform( sample_data) walignedSw = Similarity3(R=walignedTw.rotation(), t=walignedTw.translation(), s=1.0) sample_data = sample_data.apply_Sim3(walignedSw) # Aggregate the final, transformed points num_tracks = sample_data.number_tracks() transformed_points_3d = [ np.array(sample_data.get_track(i).point3()) for i in range(num_tracks) ] transformed_points_3d = np.array(transformed_points_3d) transformed_camera_translations = np.array( [pose.translation() for pose in sample_data.get_camera_poses()]) final_relative_distances = scipy.spatial.distance.cdist( transformed_camera_translations, transformed_points_3d, metric="euclidean") npt.assert_almost_equal(final_relative_distances, initial_relative_distances, decimal=3)