def test_sets(self) -> None: """Ensure that pairs are merged correctly during Union-Find. An IndexPair (i,k) representing a unique key might represent the k'th detected keypoint in image i. For the data below, merging such measurements into feature tracks across frames should create 2 distinct sets. """ dsf = gtsam.DSFMapIndexPair() dsf.merge(IndexPair(0, 1), IndexPair(1, 2)) dsf.merge(IndexPair(0, 1), IndexPair(3, 4)) dsf.merge(IndexPair(4, 5), IndexPair(6, 8)) sets = dsf.sets() merged_sets = set() for i in sets: set_keys = [] s = sets[i] for val in gtsam.IndexPairSetAsArray(s): set_keys.append((val.i(), val.j())) merged_sets.add(tuple(set_keys)) # fmt: off expected_sets = { ((0, 1), (1, 2), (3, 4)), # set 1 ((4, 5), (6, 8)) # set 2 } # fmt: on assert expected_sets == merged_sets
def test_all(self): """Test everything in DFSMap.""" def key(index_pair): return index_pair.i(), index_pair.j() dsf = gtsam.DSFMapIndexPair() pair1 = gtsam.IndexPair(1, 18) self.assertEqual(key(dsf.find(pair1)), key(pair1)) pair2 = gtsam.IndexPair(2, 2) dsf.merge(pair1, pair2) self.assertTrue(dsf.find(pair1), dsf.find(pair1))
def test_all(self) -> None: """Test everything in DFSMap.""" def key(index_pair) -> Tuple[int, int]: return index_pair.i(), index_pair.j() dsf = gtsam.DSFMapIndexPair() pair1 = gtsam.IndexPair(1, 18) self.assertEqual(key(dsf.find(pair1)), key(pair1)) pair2 = gtsam.IndexPair(2, 2) # testing the merge feature of dsf dsf.merge(pair1, pair2) self.assertEqual(key(dsf.find(pair1)), key(dsf.find(pair2)))
def test_sets(self): from gtsam import IndexPair dsf = gtsam.DSFMapIndexPair() dsf.merge(IndexPair(0, 1), IndexPair(1, 2)) dsf.merge(IndexPair(0, 1), IndexPair(3, 4)) dsf.merge(IndexPair(4, 5), IndexPair(6, 8)) sets = dsf.sets() for i in sets: s = sets[i] for val in gtsam.IndexPairSetAsArray(s): val.i() val.j()
def generate_dsf(self, enable=True): """Use dsf to find data association between landmark and landmark observation(features)""" dsf = gtsam.DSFMapIndexPair() for i in range(0, self._nrimages - 1): for j in range(i + 1, self._nrimages): matches = self.load_matches(i, j) if enable: bad_essential, matches = self.ransac_filter_keypoints( matches, i, j) if bad_essential: print( "Not enough points to generate essential matrix for image_", i, " and image_", j) continue for frame_1, keypt_1, frame_2, keypt_2 in matches: dsf.merge(gtsam.IndexPair(frame_1, keypt_1), gtsam.IndexPair(frame_2, keypt_2)) return dsf
def generate_tracks_from_pairwise_matches( matches_dict: Dict[Tuple[int, int], np.ndarray], keypoints_list: List[Keypoints], ) -> List["SfmTrack2d"]: """Factory function that creates a list of tracks from 2d point 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: pose indices for the matched pair of images val: feature indices, as array of Nx2 shape; N being nb 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() # 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) if track_2d.validate_unique_cameras(): track_2d_list += [track_2d] return track_2d_list
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