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
