diff --git a/gtsam/sfm/DsfTrackGenerator.h b/gtsam/sfm/DsfTrackGenerator.h index 6483bae97..c67215a91 100644 --- a/gtsam/sfm/DsfTrackGenerator.h +++ b/gtsam/sfm/DsfTrackGenerator.h @@ -13,7 +13,7 @@ * @file DsfTrackGenerator.h * @date July 2022 * @author John Lambert - * @brief Identifies connected components in the keypoint matched graph. + * @brief Identifies connected components in the keypoint matches graph. */ #pragma once @@ -66,6 +66,11 @@ struct NamedSfmMeasurement }; +/** + * @brief Track containing 2D measurements associated with a single 3D point. + * Note: Equivalent to gtsam.SfmTrack, but without the 3d measurement. + * This class holds data temporarily before 3D point is initialized. + */ class SfmTrack2d { private: @@ -80,7 +85,7 @@ class SfmTrack2d // @brief Validates the track by checking that no two measurements are from the same camera. // // returns boolean result of the validation. - bool validate_unique_cameras() + bool has_unique_cameras() { std::vector track_cam_idxs; for (auto & measurement: measurements_) @@ -93,18 +98,25 @@ class SfmTrack2d } }; - +/** + * @brief Generates point tracks from connected components in the keypoint matches graph. + */ class DsfTrackGenerator { public: + /** Default constructor. */ DsfTrackGenerator() {} // 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, + // 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). + // @param Map from (i1,i2) image pair indices to (K,2) matrix, for K + // correspondence indices, from each image. + // @param Length-N list of keypoints, for N images/cameras. std::vector generate_tracks_from_pairwise_matches( const MatchIndicesMap& matches_dict, const KeypointsList& keypoints_list) @@ -115,7 +127,10 @@ class DsfTrackGenerator // Generate the DSF to form tracks. DSFMapIndexPair dsf; - for (const auto& [pair_idxs, corr_idxs]: matches_dict) { + for (const auto& kv: matches_dict) { + const auto pair_idxs = kv.first; + const auto corr_idxs = kv.second; + // Image pair is (i1,i2). size_t i1 = pair_idxs.first; size_t i2 = pair_idxs.second; @@ -132,10 +147,16 @@ class DsfTrackGenerator std::cout << "[SfmTrack2d] Union-Find Complete" << std::endl; const std::map > key_sets = dsf.sets(); + // Return immediately if no sets were found. + if (key_sets.empty()) return track_2d_list; + size_t erroneous_track_count = 0; // Create a list of tracks. // Each track will be represented as a list of (camera_idx, measurements). - for (const auto& [set_id, index_pair_set]: key_sets) { + for (const auto& kv: key_sets) { + const auto set_id = kv.first; + const auto index_pair_set = kv.second; + // Initialize track from measurements. SfmTrack2d track_2d; @@ -150,15 +171,16 @@ class DsfTrackGenerator // Skip erroneous track that had repeated measurements within the same image. // This is an expected result from an incorrect correspondence slipping through. - if (track_2d.validate_unique_cameras()) + if (track_2d.has_unique_cameras()) { track_2d_list.emplace_back(track_2d); } else { - erroneous_track_count += 1; + erroneous_track_count++; } } - double erroneous_track_pct = key_sets.size() > 0 ? erroneous_track_count / key_sets.size() * 100 : std::nan("1"); + double erroneous_track_pct = static_cast(erroneous_track_count) + / static_cast(key_sets.size()) * 100; // TODO(johnwlambert): restrict decimal places to 2 decimals. std::cout << "DSF Union-Find: " << erroneous_track_pct; std::cout << "% of tracks discarded from multiple obs. in a single image." << std::endl; diff --git a/gtsam/sfm/sfm.i b/gtsam/sfm/sfm.i index 0374d99b3..2a683a032 100644 --- a/gtsam/sfm/sfm.i +++ b/gtsam/sfm/sfm.i @@ -46,7 +46,7 @@ class SfmTrack2d { void addMeasurement(const gtsam::NamedSfmMeasurement &m); std::vector measurements(); - bool validate_unique_cameras(); + bool has_unique_cameras(); }; diff --git a/python/gtsam/tests/test_DsfTrackGenerator.py b/python/gtsam/tests/test_DsfTrackGenerator.py index 953167dca..cb643bdb9 100644 --- a/python/gtsam/tests/test_DsfTrackGenerator.py +++ b/python/gtsam/tests/test_DsfTrackGenerator.py @@ -16,7 +16,7 @@ class TestDsfTrackGenerator(GtsamTestCase): """Tests for DsfTrackGenerator.""" def test_track_generation(self) -> None: - """Ensures that DSF generates two tracks from measurements in 3 images (H=200,W=400).""" + """Ensures that DSF generates three tracks from measurements in 3 images (H=200,W=400).""" 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]])) @@ -32,25 +32,27 @@ class TestDsfTrackGenerator(GtsamTestCase): matches_dict[(1, 2)] = np.array([[2, 0], [1, 1]]) tracks = DsfTrackGenerator().generate_tracks_from_pairwise_matches(matches_dict, keypoints_list) + import pdb + pdb.set_trace() assert len(tracks) == 3 # Verify track 0. assert np.allclose(tracks[0].measurements()[0].uv, np.array([10.0, 20.0])) assert np.allclose(tracks[0].measurements()[1].uv, np.array([50.0, 60.0])) - assert tracks[0].measurements()[0].i, 0 - assert tracks[0].measurements()[1].i, 1 + assert tracks[0].measurements()[0].i == 0 + assert tracks[0].measurements()[1].i == 1 # Verify track 1. assert np.allclose(tracks[1].measurements()[0].uv, np.array([30.0, 40.0])) assert np.allclose(tracks[1].measurements()[1].uv, np.array([70.0, 80.0])) assert np.allclose(tracks[1].measurements()[2].uv, np.array([130.0, 140.0])) - assert tracks[1].measurements()[0].i, 0 - assert tracks[1].measurements()[1].i, 1 - assert tracks[1].measurements()[2].i, 2 + assert tracks[1].measurements()[0].i == 0 + assert tracks[1].measurements()[1].i == 1 + assert tracks[1].measurements()[2].i == 2 # Verify track 2. assert np.allclose(tracks[2].measurements()[0].uv, np.array([90.0, 100.0])) assert np.allclose(tracks[2].measurements()[1].uv, np.array([110.0, 120.0])) - assert tracks[2].measurements()[0].i, 1 - assert tracks[2].measurements()[1].i, 2 + assert tracks[2].measurements()[0].i == 1 + assert tracks[2].measurements()[1].i == 2