diff --git a/gtsam/sfm/DsfTrackGenerator.h b/gtsam/sfm/DsfTrackGenerator.h index 0dae3d216..86ae7131b 100644 --- a/gtsam/sfm/DsfTrackGenerator.h +++ b/gtsam/sfm/DsfTrackGenerator.h @@ -71,28 +71,13 @@ using MatchIndicesMap = std::map; class DsfTrackGenerator { public: /** Default constructor. */ - DsfTrackGenerator() {} + DsfTrackGenerator() = default; - // 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). - // @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 KeypointsVector& keypoints_list, bool verbose = false) { - std::vector track_2d_list; - - if (verbose) - std::cout << "[SfmTrack2d] Starting Union-Find..." << std::endl; - // Generate the DSF to form tracks. + /// Generate the DSF to form tracks. + static DSFMapIndexPair generateDSF(const MatchIndicesMap& matches) { DSFMapIndexPair dsf; - for (const auto& kv : matches_dict) { + for (const auto& kv : matches) { const auto pair_indices = kv.first; const auto corr_indices = kv.second; @@ -101,60 +86,97 @@ class DsfTrackGenerator { size_t i2 = pair_indices.second; for (size_t k = 0; k < corr_indices.rows(); k++) { // Measurement indices are found in a single matrix row, as (k1,k2). - size_t k1 = corr_indices(k, 0); - size_t k2 = corr_indices(k, 1); - // Unique keys for the DSF are (i,k), representing keypoint index in an - // image. - dsf.merge(IndexPair(i1, k1), IndexPair(i2, k2)); + size_t k1 = corr_indices(k, 0), k2 = corr_indices(k, 1); + // Unique key for DSF is (i,k), representing keypoint index in an image. + dsf.merge({i1, k1}, {i2, k2}); } } - if (verbose) std::cout << "[SfmTrack2d] Union-Find Complete" << std::endl; + return dsf; + } + + /// Generate a single track from a set of index pairs + static SfmTrack2d trackFromIndexPairs( + const std::set& index_pair_set, + const KeypointsVector& keypoints) { + // Initialize track from measurements. + SfmTrack2d track2d; + + for (const auto& index_pair : index_pair_set) { + // Camera index is represented by i, and measurement index is + // represented by k. + size_t i = index_pair.i(); + size_t k = index_pair.j(); + // Add measurement to this track. + track2d.addMeasurement(i, keypoints[i].coordinates.row(k)); + } + + return track2d; + } + + /// Generate tracks from the DSF. + static std::vector tracksFromDSF( + const DSFMapIndexPair& dsf, const KeypointsVector& keypoints) { const std::map > key_sets = dsf.sets(); // Return immediately if no sets were found. - if (key_sets.empty()) return track_2d_list; + if (key_sets.empty()) return {}; - size_t erroneous_track_count = 0; // Create a list of tracks. // Each track will be represented as a list of (camera_idx, measurements). + std::vector tracks2d; 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; - - for (const auto& index_pair : index_pair_set) { - // Camera index is represented by i, and measurement index is - // represented by k. - size_t i = index_pair.i(); - size_t k = index_pair.j(); - // Add measurement to this track. - track_2d.addMeasurement(i, keypoints_list[i].coordinates.row(k)); - } - - // 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.hasUniqueCameras()) { - track_2d_list.emplace_back(track_2d); - } else { - erroneous_track_count++; - } + SfmTrack2d track2d = trackFromIndexPairs(kv.second, keypoints); + tracks2d.emplace_back(track2d); } + return tracks2d; + } + + /** + * @brief 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). + * + * @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. + */ + static std::vector tracksFromPairwiseMatches( + const MatchIndicesMap& matches, const KeypointsVector& keypoints, + bool verbose = false) { + // Generate the DSF to form tracks. + if (verbose) + std::cout << "[SfmTrack2d] Starting Union-Find..." << std::endl; + DSFMapIndexPair dsf = generateDSF(matches); + if (verbose) std::cout << "[SfmTrack2d] Union-Find Complete" << std::endl; + + std::vector tracks2d = tracksFromDSF(dsf, keypoints); + + // Filter out erroneous tracks that had repeated measurements within the + // same image. This is an expected result from an incorrect correspondence + // slipping through. + std::vector validTracks; + std::copy_if( + tracks2d.begin(), tracks2d.end(), std::back_inserter(validTracks), + [](const SfmTrack2d& track2d) { return track2d.hasUniqueCameras(); }); if (verbose) { - double erroneous_track_pct = static_cast(erroneous_track_count) / - static_cast(key_sets.size()) * 100; + size_t erroneous_track_count = tracks2d.size() - validTracks.size(); + double erroneous_percentage = static_cast(erroneous_track_count) / + static_cast(tracks2d.size()) * 100; // TODO(johnwlambert): restrict decimal places to 2 decimals. - std::cout << "DSF Union-Find: " << erroneous_track_pct; + std::cout << "DSF Union-Find: " << erroneous_percentage; std::cout << "% of tracks discarded from multiple obs. in a single image." << std::endl; } + // TODO(johnwlambert): return the Transitivity failure percentage here. - return track_2d_list; + return tracks2d; } }; diff --git a/gtsam/sfm/sfm.i b/gtsam/sfm/sfm.i index 02d3b7b5b..da054711e 100644 --- a/gtsam/sfm/sfm.i +++ b/gtsam/sfm/sfm.i @@ -351,7 +351,7 @@ class KeypointsVector { class DsfTrackGenerator { DsfTrackGenerator(); - const gtsam::SfmTrack2dVector generate_tracks_from_pairwise_matches( + const gtsam::SfmTrack2dVector tracksFromPairwiseMatches( const gtsam::gtsfm::MatchIndicesMap& matches_dict, const gtsam::gtsfm::KeypointsVector& keypoints_list, bool verbose = false); diff --git a/python/gtsam/tests/test_DsfTrackGenerator.py b/python/gtsam/tests/test_DsfTrackGenerator.py index 3452c03e8..df6e60c9f 100644 --- a/python/gtsam/tests/test_DsfTrackGenerator.py +++ b/python/gtsam/tests/test_DsfTrackGenerator.py @@ -33,7 +33,7 @@ class TestDsfTrackGenerator(GtsamTestCase): matches_dict[IndexPair(0, 1)] = np.array([[0, 0], [1, 1]]) matches_dict[IndexPair(1, 2)] = np.array([[2, 0], [1, 1]]) - tracks = DsfTrackGenerator().generate_tracks_from_pairwise_matches( + tracks = DsfTrackGenerator().tracksFromPairwiseMatches( matches_dict, keypoints_list, verbose=False,