From 466290cbdc5d06d1780fcaf72c52e32b6492a630 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 22 Oct 2022 19:30:15 -0700 Subject: [PATCH] Moved code to cpp file --- gtsam/sfm/DsfTrackGenerator.cpp | 136 ++++++++++++++++++++++++++++++++ gtsam/sfm/DsfTrackGenerator.h | 96 +--------------------- 2 files changed, 137 insertions(+), 95 deletions(-) create mode 100644 gtsam/sfm/DsfTrackGenerator.cpp diff --git a/gtsam/sfm/DsfTrackGenerator.cpp b/gtsam/sfm/DsfTrackGenerator.cpp new file mode 100644 index 000000000..5052bca74 --- /dev/null +++ b/gtsam/sfm/DsfTrackGenerator.cpp @@ -0,0 +1,136 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file DsfTrackGenerator.cpp + * @date October 2022 + * @author John Lambert + * @brief Identifies connected components in the keypoint matches graph. + */ + +#include + +#include +#include + +namespace gtsam { + +namespace gtsfm { + +typedef DSFMap DSFMapIndexPair; + +/// Generate the DSF to form tracks. +static DSFMapIndexPair generateDSF(const MatchIndicesMap& matches) { + DSFMapIndexPair dsf; + + for (const auto& kv : matches) { + const auto pair_indices = kv.first; + const auto corr_indices = kv.second; + + // Image pair is (i1,i2). + size_t i1 = pair_indices.first; + 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), k2 = corr_indices(k, 1); + // Unique key for DSF is (i,k), representing keypoint index in an image. + dsf.merge({i1, k1}, {i2, k2}); + } + } + + 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 {}; + + // 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) { + // Initialize track from measurements. + 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. + */ +std::vector DsfTrackGenerator::tracksFromPairwiseMatches( + const MatchIndicesMap& matches, const KeypointsVector& keypoints, + bool verbose) { + // 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) { + 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_percentage; + std::cout << "% of tracks discarded from multiple obs. in a single image." + << std::endl; + } + + // TODO(johnwlambert): return the Transitivity failure percentage here. + return tracks2d; +} + +} // namespace gtsfm + +} // namespace gtsam diff --git a/gtsam/sfm/DsfTrackGenerator.h b/gtsam/sfm/DsfTrackGenerator.h index 86ae7131b..dbf2e6680 100644 --- a/gtsam/sfm/DsfTrackGenerator.h +++ b/gtsam/sfm/DsfTrackGenerator.h @@ -18,12 +18,9 @@ #pragma once #include -#include #include #include -#include -#include #include #include #include @@ -32,7 +29,6 @@ namespace gtsam { namespace gtsfm { -typedef DSFMap DSFMapIndexPair; typedef Eigen::MatrixX2i CorrespondenceIndices; // N x 2 array // struct Keypoints; @@ -73,66 +69,6 @@ class DsfTrackGenerator { /** Default constructor. */ DsfTrackGenerator() = default; - /// Generate the DSF to form tracks. - static DSFMapIndexPair generateDSF(const MatchIndicesMap& matches) { - DSFMapIndexPair dsf; - - for (const auto& kv : matches) { - const auto pair_indices = kv.first; - const auto corr_indices = kv.second; - - // Image pair is (i1,i2). - size_t i1 = pair_indices.first; - 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), k2 = corr_indices(k, 1); - // Unique key for DSF is (i,k), representing keypoint index in an image. - dsf.merge({i1, k1}, {i2, k2}); - } - } - - 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 {}; - - // 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) { - // Initialize track from measurements. - SfmTrack2d track2d = trackFromIndexPairs(kv.second, keypoints); - tracks2d.emplace_back(track2d); - } - return tracks2d; - } - /** * @brief Creates a list of tracks from 2d point correspondences. * @@ -147,37 +83,7 @@ class DsfTrackGenerator { */ 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) { - 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_percentage; - std::cout << "% of tracks discarded from multiple obs. in a single image." - << std::endl; - } - - // TODO(johnwlambert): return the Transitivity failure percentage here. - return tracks2d; - } + bool verbose = false); }; } // namespace gtsfm