diff --git a/gtsam/sfm/DsfTrackGenerator.h b/gtsam/sfm/DsfTrackGenerator.h index 095faa72b..619ca40ca 100644 --- a/gtsam/sfm/DsfTrackGenerator.h +++ b/gtsam/sfm/DsfTrackGenerator.h @@ -104,8 +104,8 @@ class DsfTrackGenerator // set elements from camera index of a detection and the index of that detection in that camera's keypoint list, // i.e. (i,k). std::vector generate_tracks_from_pairwise_matches( - const MatchIndicesMap matches_dict, - const KeypointsList keypoints_list) + const MatchIndicesMap& matches_dict, + const KeypointsList& keypoints_list) { std::vector track_2d_list; diff --git a/gtsam/sfm/sfm.i b/gtsam/sfm/sfm.i index 84988f06c..89fc2b281 100644 --- a/gtsam/sfm/sfm.i +++ b/gtsam/sfm/sfm.i @@ -43,11 +43,19 @@ class Keypoints }; // check if this should be a method +class SfmTrack2d +{ + void addMeasurement(const gtsam::NamedSfmMeasurement &m); + std::vector measurements(); + bool validate_unique_cameras(); +}; + + class DsfTrackGenerator { DsfTrackGenerator(); - std::vector generate_tracks_from_pairwise_matches( - const gtsam::MatchIndicesMap matches_dict, - const gtsam::KeypointsList keypoints_list); + std::vector generate_tracks_from_pairwise_matches( + const gtsam::MatchIndicesMap& matches_dict, + const gtsam::KeypointsList& keypoints_list); }; diff --git a/python/gtsam/tests/test_DsfTrackGenerator.py b/python/gtsam/tests/test_DsfTrackGenerator.py index 5546453a6..51665df7a 100644 --- a/python/gtsam/tests/test_DsfTrackGenerator.py +++ b/python/gtsam/tests/test_DsfTrackGenerator.py @@ -1,4 +1,4 @@ -""" +"""Unit tests for track generation using a Disjoint Set Forest data structure. Authors: John Lambert """ @@ -17,9 +17,9 @@ class TestDsfTrackGenerator(GtsamTestCase): def test_track_generation(self) -> None: """Ensures that DSF generates two tracks from measurements in 3 images (H=200,W=400).""" - kps_i0 = Keypoints(coordinates=np.array([[10.,20],[30,40]])) - kps_i1 = Keypoints(coordinates=np.array([[50.,60],[70,80],[90,100]])) - kps_i2 = Keypoints(coordinates=np.array([[110.,120],[130,140]])) + kps_i0 = Keypoints(coordinates=np.array([[10.0, 20], [30, 40]])) + kps_i1 = Keypoints(coordinates=np.array([[50.0, 60], [70, 80], [90, 100]])) + kps_i2 = Keypoints(coordinates=np.array([[110.0, 120], [130, 140]])) keypoints_list = KeypointsList() keypoints_list.append(kps_i0) @@ -28,8 +28,8 @@ class TestDsfTrackGenerator(GtsamTestCase): # For each image pair (i1,i2), we provide a (K,2) matrix of corresponding image indices (k1,k2). matches_dict = MatchIndicesMap() - matches_dict[(0,1)] = np.array([[0,0],[1,1]]) - matches_dict[(1,2)] = np.array([[2,0],[1,1]]) - import pdb; pdb.set_trace() + matches_dict[(0, 1)] = np.array([[0, 0], [1, 1]]) + matches_dict[(1, 2)] = np.array([[2, 0], [1, 1]]) - tracks = DsfTrackGenerator.generate_tracks_from_pairwise_matches(matches_dict, keypoints_list) + tracks = DsfTrackGenerator().generate_tracks_from_pairwise_matches(matches_dict, keypoints_list) + print(tracks[0])