Refactor into smaller functions

gtsam/sfm/DsfTrackGenerator.h

@@ -71,28 +71,13 @@ using MatchIndicesMap = std::map<IndexPair, CorrespondenceIndices>;
 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<SfmTrack2d> generate_tracks_from_pairwise_matches(
-      const MatchIndicesMap& matches_dict,
-      const KeypointsVector& keypoints_list, bool verbose = false) {
-    std::vector<SfmTrack2d> 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<IndexPair>& 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<SfmTrack2d> tracksFromDSF(
+      const DSFMapIndexPair& dsf, const KeypointsVector& keypoints) {
     const std::map<IndexPair, std::set<IndexPair> > 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<SfmTrack2d> 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<SfmTrack2d> 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<SfmTrack2d> 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<SfmTrack2d> 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<float>(erroneous_track_count) /
-                                   static_cast<float>(key_sets.size()) * 100;
+      size_t erroneous_track_count = tracks2d.size() - validTracks.size();
+      double erroneous_percentage = static_cast<float>(erroneous_track_count) /
+                                    static_cast<float>(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;
   }
 };
 

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);

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,