def test_eq_check_with_missing_measurements(self) -> None: """Tests the __eq__ function with one track having subset of measurements of the other.""" track_1 = SfmTrack2d(SAMPLE_MEASUREMENTS) # dropping the last measurement track_2 = SfmTrack2d(SAMPLE_MEASUREMENTS[:3]) self.assertNotEqual(track_1, track_2) self.assertNotEqual(track_2, track_1)
def test_eq_check_with_different_measurements(self) -> None: """Tests the __eq__ function with one measurement having different value of the 2d point.""" track_1 = SfmTrack2d(SAMPLE_MEASUREMENTS) # changing the value of the last measurement old_measurement = SAMPLE_MEASUREMENTS[-1] track_2 = SfmTrack2d( SAMPLE_MEASUREMENTS[:3] + [SfmMeasurement(old_measurement.i, np.random.rand(2))]) self.assertNotEqual(track_1, track_2) self.assertNotEqual(track_2, track_1)
def test_eq_check_with_same_measurements(self) -> None: """Tests the __eq__ function with the same set of measurements but with different ordering.""" # construct two tracks with different ordering of measurements track_1 = SfmTrack2d(SAMPLE_MEASUREMENTS) track_2 = SfmTrack2d([ SAMPLE_MEASUREMENTS[0], SAMPLE_MEASUREMENTS[3], SAMPLE_MEASUREMENTS[1], SAMPLE_MEASUREMENTS[2], ]) self.assertEqual(track_1, track_2)
def __runWithCheiralityException(self, obj: Point3dInitializer) -> bool: """Run the initialization in a a-cheiral setup, and check that the result is a None track.""" cameras = obj.track_camera_dict # flip the cameras first yaw = np.pi camera_flip_pose = Pose3(Rot3.RzRyRx(yaw, 0, 0), np.zeros((3, 1))) flipped_cameras = { i: PinholeCameraCal3Bundler(cam.pose().compose(camera_flip_pose), cam.calibration()) for i, cam in cameras.items() } obj_with_flipped_cameras = Point3dInitializer( flipped_cameras, obj.mode, obj.reproj_error_thresh, obj.num_ransac_hypotheses, ) sfm_track = obj_with_flipped_cameras.triangulate( SfmTrack2d(MEASUREMENTS)) return sfm_track is None
def classify_tracks3d_with_gt_cameras( tracks: List[SfmTrack], cameras_gt: List[PinholeCameraCal3Bundler], reproj_error_thresh_px: float = 3 ) -> List[TriangulationExitCode]: """Classifies the 3D tracks w.r.t ground truth cameras by performing triangulation and collecting exit codes. Args: tracks: list of 3d tracks, of length J. cameras_gt: cameras with GT params. reproj_error_thresh_px (optional): Reprojection error threshold (in pixels) for a track to be considered an all-inlier one. Defaults to 3. Returns: The triangulation exit code for each input track, as list of length J (same as input). """ # convert the 3D tracks to 2D tracks tracks_2d: List[SfmTrack2d] = [] for track_3d in tracks: num_measurements = track_3d.numberMeasurements() measurements: List[SfmMeasurement] = [] for k in range(num_measurements): i, uv = track_3d.measurement(k) measurements.append(SfmMeasurement(i, uv)) tracks_2d.append(SfmTrack2d(measurements)) return classify_tracks2d_with_gt_cameras(tracks_2d, cameras_gt, reproj_error_thresh_px)
def __runWithSingleOutlier(self, obj: Point3dInitializer) -> bool: """Run the initialization for a track with all inlier measurements except one, and checks for correctness of the estimated point.""" sfm_track = obj.triangulate(SfmTrack2d(get_track_with_one_outlier())) point3d = sfm_track.point3() return np.array_equal(point3d, LANDMARK_POINT)
def testSimpleTriangulationOnDoorDataset(self): """Test the tracks of the door dataset using simple triangulation initialization. Using computed tracks with ground truth camera params. Expecting failures on 2 tracks which have incorrect matches.""" with open(DOOR_TRACKS_PATH, "rb") as handle: tracks = pickle.load(handle) loader = FolderLoader(DOOR_DATASET_PATH, image_extension="JPG") camera_dict = { i: PinholeCameraCal3Bundler(loader.get_camera_pose(i), loader.get_camera_intrinsics(i)) for i in range(len(loader)) } initializer = Point3dInitializer(camera_dict, TriangulationParam.NO_RANSAC, reproj_error_thresh=1e5) # tracks which have expected failures # (both tracks have incorrect measurements) expected_failures = [ SfmTrack2d(measurements=[ SfmMeasurement(i=1, uv=np.array([1252.22729492, 1487.29431152])), SfmMeasurement(i=2, uv=np.array([1170.96679688, 1407.35876465])), SfmMeasurement(i=4, uv=np.array([263.32104492, 1489.76965332 ])), ]), SfmTrack2d(measurements=[ SfmMeasurement(i=6, uv=np.array([1142.34545898, 735.92169189 ])), SfmMeasurement(i=7, uv=np.array([1179.84155273, 763.04095459 ])), SfmMeasurement(i=9, uv=np.array([216.54107666, 774.74017334])), ]), ] for track_2d in tracks: triangulated_track = initializer.triangulate(track_2d) if triangulated_track is None: # assert we have failures which are already expected self.assertIn(track_2d, expected_failures)
def __runWithTwoMeasurements(self, obj: Point3dInitializer) -> bool: """Run the initialization with a track with all correct measurements, and checks for correctness of the recovered 3D point.""" sfm_track = obj.triangulate(SfmTrack2d(MEASUREMENTS[:2])) point3d = sfm_track.point3() return np.allclose(point3d, LANDMARK_POINT)
def test_generate_tracks_from_pairwise_matches_no_duplicates(self) -> None: """Tests that the tracks are being merged and mapped correctly.""" dummy_keypoints_list = get_dummy_keypoints_list() dummy_matches_dict = get_dummy_matches() tracks = SfmTrack2d.generate_tracks_from_pairwise_matches( dummy_matches_dict, dummy_keypoints_list) # len(track) value for toy case strictly self.assertEqual(len(tracks), 4, "tracks incorrectly mapped")
def __runWithDuplicateMeasurements(self, obj: Point3dInitializer) -> bool: """Run the initialization for a track with all inlier measurements except one, and checks for correctness of the estimated point.""" sfm_track = obj.triangulate( SfmTrack2d(get_track_with_duplicate_measurements())) point3d = sfm_track.point3() return np.allclose(point3d, LANDMARK_POINT, atol=1, rtol=1e-1)
def generate_measurement_pairs(track: SfmTrack2d) -> List[Tuple[int, ...]]: """ Extract all possible measurement pairs in a track for triangulation. Args: track: feature track from which measurements are to be extracted Returns: measurement_idxs: all possible matching measurement indices in a given track """ num_track_measurements = track.number_measurements() all_measurement_idxs = range(num_track_measurements) measurement_pair_idxs = list( itertools.combinations(all_measurement_idxs, NUM_SAMPLES_PER_RANSAC_HYPOTHESIS)) return measurement_pair_idxs
def test_generate_tracks_from_pairwise_matches_with_duplicates( self, ) -> None: """Tests that the tracks are being filtered correctly. Removes tracks that have two measurements in a single image. """ dummy_keypoints_list = get_dummy_keypoints_list() dummy_matches_dict = get_dummy_matches() malformed_matches_dict = copy.deepcopy(dummy_matches_dict) # add erroneous correspondence malformed_matches_dict[(1, 1)] = np.array([[0, 3]]) tracks = SfmTrack2d.generate_tracks_from_pairwise_matches( malformed_matches_dict, dummy_keypoints_list) # check that the length of the observation list corresponding to each key # is the same. Only good tracks will remain self.assertEqual(len(tracks), 4, "Tracks not filtered correctly")
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 testSimpleTriangulationWithOutlierMeasurements(self): sfm_track = self.simple_triangulation_initializer.triangulate( SfmTrack2d(get_track_with_one_outlier())) self.assertIsNone(sfm_track)
def __runWithOneMeasurement(self, obj: Point3dInitializer) -> bool: """Run the initialization with a track with all correct measurements, and checks for a None track as a result.""" sfm_track = obj.triangulate(SfmTrack2d(MEASUREMENTS[:1])) return sfm_track is None
def run( self, cameras: Dict[int, PinholeCameraCal3Bundler], corr_idxs_dict: Dict[Tuple[int, int], np.ndarray], keypoints_list: List[Keypoints], ) -> Tuple[SfmData, Dict[str, Any]]: """Perform the data association. Args: cameras: dictionary, with image index -> camera mapping. 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. """ # generate tracks for 3D points using pairwise correspondences tracks_2d = SfmTrack2d.generate_tracks_from_pairwise_matches( corr_idxs_dict, keypoints_list) # metrics on tracks w/o triangulation check num_tracks_2d = len(tracks_2d) track_lengths = list(map(lambda x: x.number_measurements(), tracks_2d)) mean_2d_track_length = np.mean(track_lengths) logger.debug("[Data association] input number of tracks: %s", num_tracks_2d) logger.debug("[Data association] input avg. track length: %s", mean_2d_track_length) # initializer of 3D landmark for each track point3d_initializer = Point3dInitializer( cameras, self.mode, self.reproj_error_thresh, self.num_ransac_hypotheses, ) num_tracks_w_cheirality_exceptions = 0 per_accepted_track_avg_errors = [] per_rejected_track_avg_errors = [] # form SFMdata object after triangulation triangulated_data = SfmData() for track_2d in tracks_2d: # triangulate and filter based on reprojection error sfm_track, avg_track_reproj_error, is_cheirality_failure = point3d_initializer.triangulate( track_2d) if is_cheirality_failure: num_tracks_w_cheirality_exceptions += 1 if sfm_track is not None and self.__validate_track(sfm_track): triangulated_data.add_track(sfm_track) per_accepted_track_avg_errors.append(avg_track_reproj_error) else: per_rejected_track_avg_errors.append(avg_track_reproj_error) num_accepted_tracks = triangulated_data.number_tracks() accepted_tracks_ratio = num_accepted_tracks / len(tracks_2d) track_cheirality_failure_ratio = num_tracks_w_cheirality_exceptions / len( tracks_2d) # TODO: improve dropped camera handling num_cameras = len(cameras.keys()) expected_camera_indices = np.arange(num_cameras) # add cameras to landmark_map for i, cam in cameras.items(): if i != expected_camera_indices[i]: raise RuntimeError("Some cameras must have been dropped ") triangulated_data.add_camera(cam) mean_3d_track_length, median_3d_track_length, track_lengths_3d = SfmResult( triangulated_data, None).get_track_length_statistics() logger.debug("[Data association] output number of tracks: %s", num_accepted_tracks) logger.debug("[Data association] output avg. track length: %s", mean_3d_track_length) # dump the 3d point cloud before Bundle Adjustment for offline visualization points_3d = [ list(triangulated_data.track(j).point3()) for j in range(num_accepted_tracks) ] # bin edges are halfway between each integer track_lengths_histogram, _ = np.histogram(track_lengths_3d, bins=np.linspace( -0.5, 10.5, 12)) # min possible track len is 2, above 10 is improbable histogram_dict = { f"num_len_{i}_tracks": int(track_lengths_histogram[i]) for i in range(2, 11) } data_assoc_metrics = { "mean_2d_track_length": np.round(mean_2d_track_length, 3), "accepted_tracks_ratio": np.round(accepted_tracks_ratio, 3), "track_cheirality_failure_ratio": np.round(track_cheirality_failure_ratio, 3), "num_accepted_tracks": num_accepted_tracks, "3d_tracks_length": { "median": median_3d_track_length, "mean": mean_3d_track_length, "min": int(track_lengths_3d.min()), "max": int(track_lengths_3d.max()), "track_lengths_histogram": histogram_dict, }, "mean_accepted_track_avg_error": np.array(per_accepted_track_avg_errors).mean(), "per_rejected_track_avg_errors": per_rejected_track_avg_errors, "per_accepted_track_avg_errors": per_accepted_track_avg_errors, "points_3d": points_3d, } return triangulated_data, data_assoc_metrics
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 run( self, num_images: int, cameras: Dict[int, PinholeCameraCal3Bundler], corr_idxs_dict: Dict[Tuple[int, int], np.ndarray], keypoints_list: List[Keypoints], images: Optional[List[Image]] = None ) -> Tuple[GtsfmData, Dict[str, Any]]: """Perform the data association. Args: num_images: Number of images in the scene. cameras: dictionary, with image index -> camera mapping. corr_idxs_dict: dictionary, with key as image pair (i1,i2) and value as matching keypoint indices. keypoints_list: keypoints for each image. images: a list of all images in scene (optional and only for track patch visualization) viz_patch_sz: width and height of patches, if if dumping/visualizing a patch for each 2d track measurement Returns: Cameras and tracks as GtsfmData. """ # generate tracks for 3D points using pairwise correspondences tracks_2d = SfmTrack2d.generate_tracks_from_pairwise_matches( corr_idxs_dict, keypoints_list) if self.save_track_patches_viz and images is not None: io_utils.save_track_visualizations(tracks_2d, images, save_dir=os.path.join( "plots", "tracks_2d")) # metrics on tracks w/o triangulation check num_tracks_2d = len(tracks_2d) track_lengths = list(map(lambda x: x.number_measurements(), tracks_2d)) mean_2d_track_length = np.mean(track_lengths) logger.debug("[Data association] input number of tracks: %s", num_tracks_2d) logger.debug("[Data association] input avg. track length: %s", mean_2d_track_length) # initializer of 3D landmark for each track point3d_initializer = Point3dInitializer( cameras, self.mode, self.reproj_error_thresh, self.num_ransac_hypotheses, ) num_tracks_w_cheirality_exceptions = 0 per_accepted_track_avg_errors = [] per_rejected_track_avg_errors = [] # form GtsfmData object after triangulation triangulated_data = GtsfmData(num_images) # add all cameras for i, camera in cameras.items(): triangulated_data.add_camera(i, camera) # add valid tracks where triangulation is successful for track_2d in tracks_2d: # triangulate and filter based on reprojection error sfm_track, avg_track_reproj_error, is_cheirality_failure = point3d_initializer.triangulate( track_2d) if is_cheirality_failure: num_tracks_w_cheirality_exceptions += 1 if avg_track_reproj_error is not None: # need no more than 3 significant figures in json report avg_track_reproj_error = np.round(avg_track_reproj_error, 3) if sfm_track is not None and self.__validate_track(sfm_track): triangulated_data.add_track(sfm_track) per_accepted_track_avg_errors.append(avg_track_reproj_error) else: per_rejected_track_avg_errors.append(avg_track_reproj_error) track_cheirality_failure_ratio = num_tracks_w_cheirality_exceptions / len( tracks_2d) # pick only the largest connected component connected_data = triangulated_data.select_largest_connected_component() num_accepted_tracks = connected_data.number_tracks() accepted_tracks_ratio = num_accepted_tracks / len(tracks_2d) mean_3d_track_length, median_3d_track_length = connected_data.get_track_length_statistics( ) track_lengths_3d = connected_data.get_track_lengths() logger.debug("[Data association] output number of tracks: %s", num_accepted_tracks) logger.debug("[Data association] output avg. track length: %s", np.round(mean_3d_track_length, 2)) # dump the 3d point cloud before Bundle Adjustment for offline visualization points_3d = [ list(connected_data.get_track(j).point3()) for j in range(num_accepted_tracks) ] # bin edges are halfway between each integer track_lengths_histogram, _ = np.histogram(track_lengths_3d, bins=np.linspace( -0.5, 10.5, 12)) # min possible track len is 2, above 10 is improbable histogram_dict = { f"num_len_{i}_tracks": int(track_lengths_histogram[i]) for i in range(2, 11) } data_assoc_metrics = { "mean_2d_track_length": np.round(mean_2d_track_length, 3), "accepted_tracks_ratio": np.round(accepted_tracks_ratio, 3), "track_cheirality_failure_ratio": np.round(track_cheirality_failure_ratio, 3), "num_accepted_tracks": num_accepted_tracks, "3d_tracks_length": { "median": median_3d_track_length, "mean": mean_3d_track_length, "min": int(track_lengths_3d.min()) if track_lengths_3d.size > 0 else None, "max": int(track_lengths_3d.max()) if track_lengths_3d.size > 0 else None, "track_lengths_histogram": histogram_dict, }, "mean_accepted_track_avg_error": np.array(per_accepted_track_avg_errors).mean(), "per_rejected_track_avg_errors": per_rejected_track_avg_errors, "per_accepted_track_avg_errors": per_accepted_track_avg_errors, "points_3d": points_3d, } return connected_data, data_assoc_metrics
def run(self, matches_dict: Dict[Tuple[int, int], np.ndarray], keypoints_list: List[Keypoints]) -> List[SfmTrack2d]: """Estimate tracks from feature correspondences. Creates a disjoint-set forest (DSF) and 2d tracks from pairwise matches. We create a singleton for union-find set elements from camera index of a detection and the index of that detection in that camera's keypoint list, i.e. (i,k). Args: matches_dict: Dict of pairwise matches of type: key: indices for the matched pair of images val: feature indices, as array of Nx2 shape; N being number of features. A row is (feature_idx1, feature_idx2). keypoints_list: List of keypoints for each image. Returns: list of all valid SfmTrack2d generated by the matches. """ # check to ensure dimensions of coordinates are correct dims_valid = all([kps.coordinates.ndim == 2 for kps in keypoints_list]) if not dims_valid: raise Exception( "Dimensions for Keypoint coordinates incorrect. Array needs to be 2D" ) # Generate the DSF to form tracks dsf = gtsam.DSFMapIndexPair() track_2d_list = [] # for DSF finally # measurement_idxs represented by ks for (i1, i2), k_pairs in matches_dict.items(): for (k1, k2) in k_pairs: dsf.merge(gtsam.IndexPair(i1, k1), gtsam.IndexPair(i2, k2)) key_set = dsf.sets() erroneous_track_count = 0 # create a landmark map: a list of tracks # Each track is represented as a list of (camera_idx, measurements) for set_id in key_set: index_pair_set = key_set[ set_id] # key_set is a wrapped C++ map, so this unusual syntax is required # Initialize track from measurements track_measurements = [] for index_pair in gtsam.IndexPairSetAsArray(index_pair_set): # camera_idx is represented by i # measurement_idx is represented by k i = index_pair.i() k = index_pair.j() # add measurement in this track track_measurements += [ SfmMeasurement(i, keypoints_list[i].coordinates[k]) ] track_2d = SfmTrack2d(track_measurements) # Skip erroneous track that had repeated measurements within the same image (i.e., violates transitivity). # This is an expected result from an incorrect correspondence slipping through. if track_2d.validate_unique_cameras(): track_2d_list += [track_2d] else: erroneous_track_count += 1 erroneous_track_pct = erroneous_track_count / len( key_set) * 100 if len(key_set) > 0 else np.NaN logger.info( f"DSF Union-Find: {erroneous_track_pct:.2f}% of tracks discarded from multiple obs. in a single image." ) return track_2d_list