diff --git a/gtsam/sfm/DsfTrackGenerator.h b/gtsam/sfm/DsfTrackGenerator.h index c67215a91..9da588c55 100644 --- a/gtsam/sfm/DsfTrackGenerator.h +++ b/gtsam/sfm/DsfTrackGenerator.h @@ -17,7 +17,6 @@ */ #pragma once -#include #include #include @@ -39,8 +38,7 @@ typedef Eigen::MatrixX2i CorrespondenceIndices; // N x 2 array using KeypointCoordinates = Eigen::MatrixX2d; -struct Keypoints -{ +struct Keypoints { KeypointCoordinates coordinates; // typedef'd for Eigen::VectorXd boost::optional scales; @@ -57,8 +55,7 @@ using MatchIndicesMap = std::map; // @param camera index // @param 2d measurement // Implemented as named tuple, instead of std::pair (like SfmMeasurement in SfmTrack.h) -struct NamedSfmMeasurement -{ +struct NamedSfmMeasurement { size_t i; gtsam::Point2 uv; @@ -71,16 +68,15 @@ struct NamedSfmMeasurement * Note: Equivalent to gtsam.SfmTrack, but without the 3d measurement. * This class holds data temporarily before 3D point is initialized. */ -class SfmTrack2d -{ - private: - std::vector measurements_; +class SfmTrack2d { + private: + std::vector measurements_; - public: - void addMeasurement(const NamedSfmMeasurement &m) { - measurements_.emplace_back(m); - } - std::vector measurements() {return measurements_; } + public: + void addMeasurement(const NamedSfmMeasurement &m) { + measurements_.emplace_back(m); + } + std::vector measurements() {return measurements_; } // @brief Validates the track by checking that no two measurements are from the same camera. // @@ -88,7 +84,7 @@ class SfmTrack2d bool has_unique_cameras() { std::vector track_cam_idxs; - for (auto & measurement: measurements_) + for (auto & measurement : measurements_) { track_cam_idxs.emplace_back(measurement.i); } @@ -101,93 +97,91 @@ class SfmTrack2d /** * @brief Generates point tracks from connected components in the keypoint matches graph. */ -class DsfTrackGenerator -{ +class DsfTrackGenerator { - public: - /** Default constructor. */ - 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, - // 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) - { - std::vector track_2d_list; + // 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 KeypointsList& keypoints_list) { + std::vector track_2d_list; - std::cout << "[SfmTrack2d] Starting Union-Find..." << std::endl; - // Generate the DSF to form tracks. - DSFMapIndexPair dsf; - - for (const auto& kv: matches_dict) { - const auto pair_idxs = kv.first; - const auto corr_idxs = kv.second; + std::cout << "[SfmTrack2d] Starting Union-Find..." << std::endl; + // Generate the DSF to form tracks. + DSFMapIndexPair dsf; - // Image pair is (i1,i2). - size_t i1 = pair_idxs.first; - size_t i2 = pair_idxs.second; - for (size_t k = 0; k < corr_idxs.rows(); k++) - { - // Measurement indices are found in a single matrix row, as (k1,k2). - size_t k1 = corr_idxs(k,0); - size_t k2 = corr_idxs(k,1); - // Unique keys for the DSF are (i,k), representing keypoint index in an image. - dsf.merge(IndexPair(i1, k1), IndexPair(i2, k2)); - } + 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; + for (size_t k = 0; k < corr_idxs.rows(); k++) + { + // Measurement indices are found in a single matrix row, as (k1,k2). + size_t k1 = corr_idxs(k, 0); + size_t k2 = corr_idxs(k, 1); + // Unique keys for the DSF are (i,k), representing keypoint index in an image. + dsf.merge(IndexPair(i1, k1), IndexPair(i2, k2)); } - - 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& 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(NamedSfmMeasurement(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.has_unique_cameras()) - { - track_2d_list.emplace_back(track_2d); - } else { - erroneous_track_count++; - } - } - - 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; - - // TODO(johnwlambert): return the Transitivity failure percentage here. - return track_2d_list; } + + 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& 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(NamedSfmMeasurement(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.has_unique_cameras()) + { + track_2d_list.emplace_back(track_2d); + } else { + erroneous_track_count++; + } + } + + 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; + + // TODO(johnwlambert): return the Transitivity failure percentage here. + return track_2d_list; + } }; } // namespace gtsam