From 809923a6f2e5b6713124f8a0f5f8981a8e8f7d05 Mon Sep 17 00:00:00 2001 From: senselessdev1 Date: Fri, 15 Jul 2022 23:32:03 -0400 Subject: [PATCH 01/30] Start GTSFM's DSF port to C++ --- gtsam/sfm/DsfTrackGenerator.h | 180 +++++++++++++++++++++++++++++ gtsam/sfm/sfm.i | 47 ++++++++ python/CMakeLists.txt | 11 +- python/gtsam/preamble/sfm.h | 5 + python/gtsam/specializations/sfm.h | 6 + 5 files changed, 246 insertions(+), 3 deletions(-) create mode 100644 gtsam/sfm/DsfTrackGenerator.h diff --git a/gtsam/sfm/DsfTrackGenerator.h b/gtsam/sfm/DsfTrackGenerator.h new file mode 100644 index 000000000..5a3e6661e --- /dev/null +++ b/gtsam/sfm/DsfTrackGenerator.h @@ -0,0 +1,180 @@ +/* ---------------------------------------------------------------------------- + + * 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.h + * @date May 2022 + * @author John Lambert + * @brief Identifies connected components in the keypoint matched graph. + */ + +#pragma once +#include +#include + +#include + +#include +#include +#include +#include +#include + +namespace gtsam { + +typedef DSFMap DSFMapIndexPair; +typedef std::pair ImagePair; +typedef Eigen::MatrixX2i CorrespondenceIndices; // N x 2 array + + +using KeypointCoordinates = Eigen::MatrixX2d; + +struct Keypoints +{ + KeypointCoordinates coordinates; + // typedef'd for Eigen::VectorXd + boost::optional scales; + boost::optional responses; + + Keypoints(const gtsam::KeypointCoordinates& coordinates): coordinates(coordinates) {}; // boost::none +}; + + + + + +// TODO: THIS CLASSNAME IS DUPLICATED in SfmTrack.h, conflicts +// @param camera index +// @param 2d measurement +// Implemented as named tuple, instead of std::pair +struct NamedSfmMeasurement +{ + size_t i; + Point2 uv; + + NamedSfmMeasurement(size_t i, Point2 uv) : i(i), uv(uv) {} +}; + + +class SfmTrack2d +{ + private: + std::vector 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. + // + // returns boolean result of the validation. + bool validate_unique_cameras() + { + std::vector track_cam_idxs; + for (auto & measurement: measurements_) + { + track_cam_idxs.emplace_back(measurement.i); + } + auto i = std::adjacent_find(track_cam_idxs.begin(), track_cam_idxs.end()); + bool all_cameras_unique = (i == track_cam_idxs.end()); + return all_cameras_unique; + } +}; + +using MatchIndicesMap = std::map; +using KeypointsList = std::vector; +using KeypointsVector = std::vector; // TODO: prefer KeypointsSet? + + +class DsfTrackGenerator +{ + + public: + 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). + std::vector generate_tracks_from_pairwise_matches( + const MatchIndicesMap matches_dict, + const KeypointsList keypoints_list) + { + std::vector track_2d_list; + + // TODO: put these checks into the Keypoints class. + // check to ensure dimensions of coordinates are correct + //dims_valid = all([kps.coordinates.ndim == 2 for kps in keypoints_list]) + //if not dims_valid: + // raise Exception("Dimensions for Keypoint coordinates incorrect. Array needs to be 2D") + + std::cout << "[SfmTrack2d] Starting Union-Find..." << std::endl; + // Generate the DSF to form tracks + DSFMapIndexPair dsf; + + // for DSF finally + // measurement_idxs represented by k's + for (const auto& [pair_idxs, corr_idxs]: matches_dict) { + size_t i1 = pair_idxs.first; + size_t i2 = pair_idxs.second; + for (size_t k = 0; k < corr_idxs.rows(); k++) + { + size_t k1 = corr_idxs(k,0); + size_t k2 = corr_idxs(k,1); + dsf.merge(IndexPair(i1, k1), IndexPair(i2, k2)); + } + } + + std::cout << "[SfmTrack2d] Union-Find Complete" << std::endl; + const std::map > key_sets = dsf.sets(); + + size_t erroneous_track_count = 0; + // create a landmark map: 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) { + // Initialize track from measurements + SfmTrack2d track_2d; + + for (const auto& index_pair: index_pair_set) + { + // camera_idx is represented by i + // measurement_idx is represented by k + size_t i = index_pair.i(); + size_t k = index_pair.j(); + // add measurement in 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.validate_unique_cameras()) + { + track_2d_list.emplace_back(track_2d); + } else { + erroneous_track_count += 1; + } + } + + double erroneous_track_pct = key_sets.size() > 0 ? erroneous_track_count / key_sets.size() * 100 : std::nan("1"); + // TODO: 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: return the Transitivity failure percentage here. + return track_2d_list; + } +}; + +} // namespace gtsam + diff --git a/gtsam/sfm/sfm.i b/gtsam/sfm/sfm.i index 83bd07b13..84988f06c 100644 --- a/gtsam/sfm/sfm.i +++ b/gtsam/sfm/sfm.i @@ -4,6 +4,53 @@ namespace gtsam { +#include +class GtsfmData +{ + GtsfmData(const int numberImages); + void write_points(std::vector& images, const std::string save_dir); + void write_images(std::vector& images, const std::string save_dir); +}; + +#include + +class MatchIndicesMap { + MatchIndicesMap(); + MatchIndicesMap(const gtsam::MatchIndicesMap& other); + + size_t size() const; + bool empty() const; + void clear(); + gtsam::CorrespondenceIndices at(const pair& keypair) const; +}; + + +class KeypointsList { + KeypointsList(); + KeypointsList(const gtsam::KeypointsList& other); + void push_back(const gtsam::Keypoints& keypoints); + size_t size() const; + bool empty() const; + void clear(); + gtsam::Keypoints at(const size_t& index) const; +}; + + +class Keypoints +{ + Keypoints(const gtsam::KeypointCoordinates& coordinates); + gtsam::KeypointCoordinates coordinates; +}; // check if this should be a method + + +class DsfTrackGenerator { + DsfTrackGenerator(); + std::vector generate_tracks_from_pairwise_matches( + const gtsam::MatchIndicesMap matches_dict, + const gtsam::KeypointsList keypoints_list); +}; + + #include #include #include diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index c14f02dda..91cb75edd 100644 --- a/python/CMakeLists.txt +++ b/python/CMakeLists.txt @@ -51,7 +51,9 @@ set(ignore gtsam::BinaryMeasurementsUnit3 gtsam::BinaryMeasurementsRot3 gtsam::DiscreteKey - gtsam::KeyPairDoubleMap) + gtsam::KeyPairDoubleMap + gtsam::MatchIndicesMap + gtsam::KeypointsList) set(interface_headers ${PROJECT_SOURCE_DIR}/gtsam/gtsam.i @@ -145,8 +147,11 @@ if(GTSAM_UNSTABLE_BUILD_PYTHON) gtsam::CameraSetCal3Bundler gtsam::CameraSetCal3Unified gtsam::CameraSetCal3Fisheye - gtsam::KeyPairDoubleMap) - + gtsam::KeyPairDoubleMap + gtsam::MatchIndicesMap + gtsam::KeypointsList) + + pybind_wrap(${GTSAM_PYTHON_UNSTABLE_TARGET} # target ${PROJECT_SOURCE_DIR}/gtsam_unstable/gtsam_unstable.i # interface_header "gtsam_unstable.cpp" # generated_cpp diff --git a/python/gtsam/preamble/sfm.h b/python/gtsam/preamble/sfm.h index 8ff0ea82e..a51e963dd 100644 --- a/python/gtsam/preamble/sfm.h +++ b/python/gtsam/preamble/sfm.h @@ -24,3 +24,8 @@ PYBIND11_MAKE_OPAQUE( std::vector>); PYBIND11_MAKE_OPAQUE( std::vector>); +PYBIND11_MAKE_OPAQUE( + std::vector); + + +PYBIND11_MAKE_OPAQUE(gtsam::MatchIndicesMap); \ No newline at end of file diff --git a/python/gtsam/specializations/sfm.h b/python/gtsam/specializations/sfm.h index 311b2c59b..297aee262 100644 --- a/python/gtsam/specializations/sfm.h +++ b/python/gtsam/specializations/sfm.h @@ -18,6 +18,12 @@ py::bind_vector > >( py::bind_vector > >( m_, "BinaryMeasurementsRot3"); py::bind_map(m_, "KeyPairDoubleMap"); +py::bind_map(m_, "MatchIndicesMap"); + +py::bind_vector< + std::vector >( + m_, "KeypointsList"); + py::bind_vector< std::vector >( From 6e13456db45380bcb99e68e1139e8a9b26b9b4f4 Mon Sep 17 00:00:00 2001 From: senselessdev1 Date: Fri, 15 Jul 2022 23:35:00 -0400 Subject: [PATCH 02/30] Add Python unit test --- gtsam/sfm/DsfTrackGenerator.h | 2 +- python/gtsam/tests/test_DsfTrackGenerator.py | 34 ++++++++++++++++++++ 2 files changed, 35 insertions(+), 1 deletion(-) create mode 100644 python/gtsam/tests/test_DsfTrackGenerator.py diff --git a/gtsam/sfm/DsfTrackGenerator.h b/gtsam/sfm/DsfTrackGenerator.h index 5a3e6661e..e26d60769 100644 --- a/gtsam/sfm/DsfTrackGenerator.h +++ b/gtsam/sfm/DsfTrackGenerator.h @@ -11,7 +11,7 @@ /** * @file DsfTrackGenerator.h - * @date May 2022 + * @date July 2022 * @author John Lambert * @brief Identifies connected components in the keypoint matched graph. */ diff --git a/python/gtsam/tests/test_DsfTrackGenerator.py b/python/gtsam/tests/test_DsfTrackGenerator.py new file mode 100644 index 000000000..88b12ce3c --- /dev/null +++ b/python/gtsam/tests/test_DsfTrackGenerator.py @@ -0,0 +1,34 @@ +""" + +Authors: John Lambert +""" + +import unittest + +import numpy as np + +import gtsam +from gtsam import DsfTrackGenerator, Keypoints, KeypointsList, MatchIndicesMap +from gtsam.utils.test_case import GtsamTestCase + + +class TestDsfTrackGenerator(GtsamTestCase): + """Tests for DsfTrackGenerator.""" + + def test_track_generation(self) -> None: + """ """ + kps_i0 = Keypoints(coordinates=np.array([[0,0],[1,1]])) + kps_i1 = Keypoints(coordinates=np.array([[2,2],[3,3],[4,4]])) + kps_i2 = Keypoints(coordinates=np.array([[5,5],[6,6]])) + + keypoints_list = KeypointsList() + keypoints_list.append(kps_i0) + keypoints_list.append(kps_i1) + keypoints_list.append(kps_i2) + + matches_dict = MatchIndicesMap() + matches_dict[(0,0)] = np.array([[0,0],[1,1]]) + matches_dict[(1,1)] = np.array([[2,2],[3,3],[4,4]]) + import pdb; pdb.set_trace() + + tracks = DsfTrackGenerator.generate_tracks_from_pairwise_matches(matches_dict, keypoints_list) From 629b9cd9555e6e4fd1cd24ace80b90710e1dcb91 Mon Sep 17 00:00:00 2001 From: senselessdev1 Date: Sat, 16 Jul 2022 00:26:57 -0400 Subject: [PATCH 03/30] clean up docstrings --- gtsam/sfm/DsfTrackGenerator.h | 40 ++++++++------------ python/gtsam/tests/test_DsfTrackGenerator.py | 13 ++++--- 2 files changed, 22 insertions(+), 31 deletions(-) diff --git a/gtsam/sfm/DsfTrackGenerator.h b/gtsam/sfm/DsfTrackGenerator.h index e26d60769..095faa72b 100644 --- a/gtsam/sfm/DsfTrackGenerator.h +++ b/gtsam/sfm/DsfTrackGenerator.h @@ -48,13 +48,9 @@ struct Keypoints }; - - - -// TODO: THIS CLASSNAME IS DUPLICATED in SfmTrack.h, conflicts // @param camera index // @param 2d measurement -// Implemented as named tuple, instead of std::pair +// Implemented as named tuple, instead of std::pair (like SfmMeasurement in SfmTrack.h) struct NamedSfmMeasurement { size_t i; @@ -93,7 +89,7 @@ class SfmTrack2d using MatchIndicesMap = std::map; using KeypointsList = std::vector; -using KeypointsVector = std::vector; // TODO: prefer KeypointsSet? +using KeypointsVector = std::vector; // TODO(johnwlambert): prefer KeypointsSet? class DsfTrackGenerator @@ -113,25 +109,20 @@ class DsfTrackGenerator { std::vector track_2d_list; - // TODO: put these checks into the Keypoints class. - // check to ensure dimensions of coordinates are correct - //dims_valid = all([kps.coordinates.ndim == 2 for kps in keypoints_list]) - //if not dims_valid: - // raise Exception("Dimensions for Keypoint coordinates incorrect. Array needs to be 2D") - std::cout << "[SfmTrack2d] Starting Union-Find..." << std::endl; - // Generate the DSF to form tracks + // Generate the DSF to form tracks. DSFMapIndexPair dsf; - // for DSF finally - // measurement_idxs represented by k's for (const auto& [pair_idxs, corr_idxs]: matches_dict) { + // 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)); } } @@ -140,24 +131,23 @@ class DsfTrackGenerator const std::map > key_sets = dsf.sets(); size_t erroneous_track_count = 0; - // create a landmark map: a list of tracks - // Each track will be represented as a list of (camera_idx, measurements) + // 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) { - // Initialize track from measurements + // Initialize track from measurements. SfmTrack2d track_2d; for (const auto& index_pair: index_pair_set) { - // camera_idx is represented by i - // measurement_idx is represented by k + // 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 in this track + // 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 + // 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()) { track_2d_list.emplace_back(track_2d); @@ -167,11 +157,11 @@ class DsfTrackGenerator } double erroneous_track_pct = key_sets.size() > 0 ? erroneous_track_count / key_sets.size() * 100 : std::nan("1"); - // TODO: restrict decimal places to 2 decimals. + // 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: return the Transitivity failure percentage here. + // TODO(johnwlambert): return the Transitivity failure percentage here. return track_2d_list; } }; diff --git a/python/gtsam/tests/test_DsfTrackGenerator.py b/python/gtsam/tests/test_DsfTrackGenerator.py index 88b12ce3c..5546453a6 100644 --- a/python/gtsam/tests/test_DsfTrackGenerator.py +++ b/python/gtsam/tests/test_DsfTrackGenerator.py @@ -16,19 +16,20 @@ class TestDsfTrackGenerator(GtsamTestCase): """Tests for DsfTrackGenerator.""" def test_track_generation(self) -> None: - """ """ - kps_i0 = Keypoints(coordinates=np.array([[0,0],[1,1]])) - kps_i1 = Keypoints(coordinates=np.array([[2,2],[3,3],[4,4]])) - kps_i2 = Keypoints(coordinates=np.array([[5,5],[6,6]])) + """Ensures that DSF generates two tracks from measurements in 3 images (H=200,W=400).""" + kps_i0 = Keypoints(coordinates=np.array([[10.,20],[30,40]])) + kps_i1 = Keypoints(coordinates=np.array([[50.,60],[70,80],[90,100]])) + kps_i2 = Keypoints(coordinates=np.array([[110.,120],[130,140]])) keypoints_list = KeypointsList() keypoints_list.append(kps_i0) keypoints_list.append(kps_i1) keypoints_list.append(kps_i2) + # For each image pair (i1,i2), we provide a (K,2) matrix of corresponding image indices (k1,k2). matches_dict = MatchIndicesMap() - matches_dict[(0,0)] = np.array([[0,0],[1,1]]) - matches_dict[(1,1)] = np.array([[2,2],[3,3],[4,4]]) + matches_dict[(0,1)] = np.array([[0,0],[1,1]]) + matches_dict[(1,2)] = np.array([[2,0],[1,1]]) import pdb; pdb.set_trace() tracks = DsfTrackGenerator.generate_tracks_from_pairwise_matches(matches_dict, keypoints_list) From 2434d3a41eac380ed3cd6d60e1d3671fb90bbcb6 Mon Sep 17 00:00:00 2001 From: senselessdev1 Date: Sat, 16 Jul 2022 01:23:54 -0400 Subject: [PATCH 04/30] Fix unwrapped SfmTrack2d, and instantiate generator before calling its method --- gtsam/sfm/DsfTrackGenerator.h | 4 ++-- gtsam/sfm/sfm.i | 14 +++++++++++--- python/gtsam/tests/test_DsfTrackGenerator.py | 16 ++++++++-------- 3 files changed, 21 insertions(+), 13 deletions(-) diff --git a/gtsam/sfm/DsfTrackGenerator.h b/gtsam/sfm/DsfTrackGenerator.h index 095faa72b..619ca40ca 100644 --- a/gtsam/sfm/DsfTrackGenerator.h +++ b/gtsam/sfm/DsfTrackGenerator.h @@ -104,8 +104,8 @@ class DsfTrackGenerator // set elements from camera index of a detection and the index of that detection in that camera's keypoint list, // i.e. (i,k). std::vector generate_tracks_from_pairwise_matches( - const MatchIndicesMap matches_dict, - const KeypointsList keypoints_list) + const MatchIndicesMap& matches_dict, + const KeypointsList& keypoints_list) { std::vector track_2d_list; diff --git a/gtsam/sfm/sfm.i b/gtsam/sfm/sfm.i index 84988f06c..89fc2b281 100644 --- a/gtsam/sfm/sfm.i +++ b/gtsam/sfm/sfm.i @@ -43,11 +43,19 @@ class Keypoints }; // check if this should be a method +class SfmTrack2d +{ + void addMeasurement(const gtsam::NamedSfmMeasurement &m); + std::vector measurements(); + bool validate_unique_cameras(); +}; + + class DsfTrackGenerator { DsfTrackGenerator(); - std::vector generate_tracks_from_pairwise_matches( - const gtsam::MatchIndicesMap matches_dict, - const gtsam::KeypointsList keypoints_list); + std::vector generate_tracks_from_pairwise_matches( + const gtsam::MatchIndicesMap& matches_dict, + const gtsam::KeypointsList& keypoints_list); }; diff --git a/python/gtsam/tests/test_DsfTrackGenerator.py b/python/gtsam/tests/test_DsfTrackGenerator.py index 5546453a6..51665df7a 100644 --- a/python/gtsam/tests/test_DsfTrackGenerator.py +++ b/python/gtsam/tests/test_DsfTrackGenerator.py @@ -1,4 +1,4 @@ -""" +"""Unit tests for track generation using a Disjoint Set Forest data structure. Authors: John Lambert """ @@ -17,9 +17,9 @@ class TestDsfTrackGenerator(GtsamTestCase): def test_track_generation(self) -> None: """Ensures that DSF generates two tracks from measurements in 3 images (H=200,W=400).""" - kps_i0 = Keypoints(coordinates=np.array([[10.,20],[30,40]])) - kps_i1 = Keypoints(coordinates=np.array([[50.,60],[70,80],[90,100]])) - kps_i2 = Keypoints(coordinates=np.array([[110.,120],[130,140]])) + 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]])) keypoints_list = KeypointsList() keypoints_list.append(kps_i0) @@ -28,8 +28,8 @@ class TestDsfTrackGenerator(GtsamTestCase): # For each image pair (i1,i2), we provide a (K,2) matrix of corresponding image indices (k1,k2). matches_dict = MatchIndicesMap() - matches_dict[(0,1)] = np.array([[0,0],[1,1]]) - matches_dict[(1,2)] = np.array([[2,0],[1,1]]) - import pdb; pdb.set_trace() + matches_dict[(0, 1)] = np.array([[0, 0], [1, 1]]) + matches_dict[(1, 2)] = np.array([[2, 0], [1, 1]]) - tracks = DsfTrackGenerator.generate_tracks_from_pairwise_matches(matches_dict, keypoints_list) + tracks = DsfTrackGenerator().generate_tracks_from_pairwise_matches(matches_dict, keypoints_list) + print(tracks[0]) From 6e45be3d98ecc9bbd52ebd7be256e021cf01d7a3 Mon Sep 17 00:00:00 2001 From: senselessdev1 Date: Sat, 16 Jul 2022 16:09:21 -0400 Subject: [PATCH 05/30] fix all wrapper issues --- gtsam/sfm/DsfTrackGenerator.h | 16 ++++++++------ gtsam/sfm/sfm.i | 15 ++++++------- python/gtsam/tests/test_DsfTrackGenerator.py | 23 +++++++++++++++++++- 3 files changed, 38 insertions(+), 16 deletions(-) diff --git a/gtsam/sfm/DsfTrackGenerator.h b/gtsam/sfm/DsfTrackGenerator.h index 619ca40ca..6483bae97 100644 --- a/gtsam/sfm/DsfTrackGenerator.h +++ b/gtsam/sfm/DsfTrackGenerator.h @@ -21,6 +21,7 @@ #include #include +#include #include #include @@ -34,9 +35,10 @@ typedef DSFMap DSFMapIndexPair; typedef std::pair ImagePair; typedef Eigen::MatrixX2i CorrespondenceIndices; // N x 2 array - +//struct Keypoints; using KeypointCoordinates = Eigen::MatrixX2d; + struct Keypoints { KeypointCoordinates coordinates; @@ -47,6 +49,10 @@ struct Keypoints Keypoints(const gtsam::KeypointCoordinates& coordinates): coordinates(coordinates) {}; // boost::none }; +using KeypointsList = std::vector; +using KeypointsVector = std::vector; // TODO(johnwlambert): prefer KeypointsSet? +using MatchIndicesMap = std::map; + // @param camera index // @param 2d measurement @@ -54,9 +60,9 @@ struct Keypoints struct NamedSfmMeasurement { size_t i; - Point2 uv; + gtsam::Point2 uv; - NamedSfmMeasurement(size_t i, Point2 uv) : i(i), uv(uv) {} + NamedSfmMeasurement(size_t i, gtsam::Point2 uv) : i(i), uv(uv) {} }; @@ -87,10 +93,6 @@ class SfmTrack2d } }; -using MatchIndicesMap = std::map; -using KeypointsList = std::vector; -using KeypointsVector = std::vector; // TODO(johnwlambert): prefer KeypointsSet? - class DsfTrackGenerator { diff --git a/gtsam/sfm/sfm.i b/gtsam/sfm/sfm.i index 89fc2b281..0374d99b3 100644 --- a/gtsam/sfm/sfm.i +++ b/gtsam/sfm/sfm.i @@ -4,14 +4,6 @@ namespace gtsam { -#include -class GtsfmData -{ - GtsfmData(const int numberImages); - void write_points(std::vector& images, const std::string save_dir); - void write_images(std::vector& images, const std::string save_dir); -}; - #include class MatchIndicesMap { @@ -43,6 +35,13 @@ class Keypoints }; // check if this should be a method +class NamedSfmMeasurement +{ + size_t i; + gtsam::Point2 uv; + NamedSfmMeasurement(size_t i, gtsam::Point2 uv); +}; + class SfmTrack2d { void addMeasurement(const gtsam::NamedSfmMeasurement &m); diff --git a/python/gtsam/tests/test_DsfTrackGenerator.py b/python/gtsam/tests/test_DsfTrackGenerator.py index 51665df7a..953167dca 100644 --- a/python/gtsam/tests/test_DsfTrackGenerator.py +++ b/python/gtsam/tests/test_DsfTrackGenerator.py @@ -32,4 +32,25 @@ class TestDsfTrackGenerator(GtsamTestCase): matches_dict[(1, 2)] = np.array([[2, 0], [1, 1]]) tracks = DsfTrackGenerator().generate_tracks_from_pairwise_matches(matches_dict, keypoints_list) - print(tracks[0]) + + 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 + + # 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 + + # 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 From 361e3f601c79c3a467a17392db3e40fb1e9db443 Mon Sep 17 00:00:00 2001 From: senselessdev1 Date: Sat, 16 Jul 2022 22:29:01 -0400 Subject: [PATCH 06/30] Remove C++17 decompositions, and address comments --- gtsam/sfm/DsfTrackGenerator.h | 42 +++++++++++++++----- gtsam/sfm/sfm.i | 2 +- python/gtsam/tests/test_DsfTrackGenerator.py | 18 +++++---- 3 files changed, 43 insertions(+), 19 deletions(-) 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 From 51fb3750e8b82691d0686bddbb1f61934a23238f Mon Sep 17 00:00:00 2001 From: senselessdev1 Date: Sun, 17 Jul 2022 00:31:42 -0400 Subject: [PATCH 07/30] remove stray breakpoint --- python/gtsam/tests/test_DsfTrackGenerator.py | 3 --- 1 file changed, 3 deletions(-) diff --git a/python/gtsam/tests/test_DsfTrackGenerator.py b/python/gtsam/tests/test_DsfTrackGenerator.py index cb643bdb9..b9ac22397 100644 --- a/python/gtsam/tests/test_DsfTrackGenerator.py +++ b/python/gtsam/tests/test_DsfTrackGenerator.py @@ -32,9 +32,6 @@ 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. From fc35096a1d833dd1d4bf08fc475f86b10c7d49a8 Mon Sep 17 00:00:00 2001 From: John Lambert Date: Tue, 19 Jul 2022 00:34:20 -0400 Subject: [PATCH 08/30] Update style to have bracket on same lines, and autoformat --- gtsam/sfm/DsfTrackGenerator.h | 186 ++++++++++++++++------------------ 1 file changed, 90 insertions(+), 96 deletions(-) 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 From e6ccf977125794c0abba8ced4a45751cc5488a86 Mon Sep 17 00:00:00 2001 From: senselessdev1 Date: Sun, 24 Jul 2022 02:54:07 -0400 Subject: [PATCH 09/30] hasUniqueCameras to camel case --- gtsam/sfm/DsfTrackGenerator.h | 188 ++++++++++++++++------------------ gtsam/sfm/sfm.i | 16 +-- 2 files changed, 99 insertions(+), 105 deletions(-) diff --git a/gtsam/sfm/DsfTrackGenerator.h b/gtsam/sfm/DsfTrackGenerator.h index c67215a91..18c96653a 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,24 +68,23 @@ 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. // // returns boolean result of the validation. - bool has_unique_cameras() + bool hasUniqueCameras() { 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.hasUniqueCameras()) + { + 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 diff --git a/gtsam/sfm/sfm.i b/gtsam/sfm/sfm.i index 2a683a032..b46b308eb 100644 --- a/gtsam/sfm/sfm.i +++ b/gtsam/sfm/sfm.i @@ -17,6 +17,13 @@ class MatchIndicesMap { }; +class Keypoints +{ + Keypoints(const gtsam::KeypointCoordinates& coordinates); + gtsam::KeypointCoordinates coordinates; +}; // check if this should be a method + + class KeypointsList { KeypointsList(); KeypointsList(const gtsam::KeypointsList& other); @@ -28,13 +35,6 @@ class KeypointsList { }; -class Keypoints -{ - Keypoints(const gtsam::KeypointCoordinates& coordinates); - gtsam::KeypointCoordinates coordinates; -}; // check if this should be a method - - class NamedSfmMeasurement { size_t i; @@ -46,7 +46,7 @@ class SfmTrack2d { void addMeasurement(const gtsam::NamedSfmMeasurement &m); std::vector measurements(); - bool has_unique_cameras(); + bool hasUniqueCameras(); }; From 720145ae672274493fbc4f9b1b8718608141adb1 Mon Sep 17 00:00:00 2001 From: senselessdev1 Date: Sun, 24 Jul 2022 03:07:04 -0400 Subject: [PATCH 10/30] remove unnecessary extra typedef --- gtsam/sfm/DsfTrackGenerator.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam/sfm/DsfTrackGenerator.h b/gtsam/sfm/DsfTrackGenerator.h index 18c96653a..5ab87a7df 100644 --- a/gtsam/sfm/DsfTrackGenerator.h +++ b/gtsam/sfm/DsfTrackGenerator.h @@ -31,7 +31,6 @@ namespace gtsam { typedef DSFMap DSFMapIndexPair; -typedef std::pair ImagePair; typedef Eigen::MatrixX2i CorrespondenceIndices; // N x 2 array //struct Keypoints; @@ -49,7 +48,8 @@ struct Keypoints { using KeypointsList = std::vector; using KeypointsVector = std::vector; // TODO(johnwlambert): prefer KeypointsSet? -using MatchIndicesMap = std::map; +// Mapping from each image pair to (N,2) array representing indices of matching keypoints. +using MatchIndicesMap = std::map; // @param camera index From a2109a24379bf3cbdebb3d60396eb158d806c22c Mon Sep 17 00:00:00 2001 From: senselessdev1 Date: Mon, 26 Sep 2022 10:56:44 -0400 Subject: [PATCH 11/30] pass in IndexPair arguments --- python/gtsam/tests/test_DsfTrackGenerator.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/python/gtsam/tests/test_DsfTrackGenerator.py b/python/gtsam/tests/test_DsfTrackGenerator.py index b9ac22397..74a7fe5dc 100644 --- a/python/gtsam/tests/test_DsfTrackGenerator.py +++ b/python/gtsam/tests/test_DsfTrackGenerator.py @@ -28,8 +28,8 @@ class TestDsfTrackGenerator(GtsamTestCase): # For each image pair (i1,i2), we provide a (K,2) matrix of corresponding image indices (k1,k2). matches_dict = MatchIndicesMap() - matches_dict[(0, 1)] = np.array([[0, 0], [1, 1]]) - matches_dict[(1, 2)] = np.array([[2, 0], [1, 1]]) + matches_dict[gtsam.IndexPair(0, 1)] = np.array([[0, 0], [1, 1]]) + matches_dict[gtsam.IndexPair(1, 2)] = np.array([[2, 0], [1, 1]]) tracks = DsfTrackGenerator().generate_tracks_from_pairwise_matches(matches_dict, keypoints_list) assert len(tracks) == 3 From 2434dc8ab732da5dce37cb7e26c5387ee7d837b0 Mon Sep 17 00:00:00 2001 From: senselessdev1 Date: Mon, 26 Sep 2022 11:36:39 -0400 Subject: [PATCH 12/30] add to preamble/specialization --- gtsam/sfm/DsfTrackGenerator.h | 4 ++++ gtsam/sfm/sfm.i | 12 +++++++++++- python/CMakeLists.txt | 3 ++- python/gtsam/preamble/sfm.h | 3 ++- python/gtsam/specializations/sfm.h | 3 +++ python/gtsam/tests/test_DsfTrackGenerator.py | 6 +++--- 6 files changed, 25 insertions(+), 6 deletions(-) diff --git a/gtsam/sfm/DsfTrackGenerator.h b/gtsam/sfm/DsfTrackGenerator.h index 5ab87a7df..16dddcbaa 100644 --- a/gtsam/sfm/DsfTrackGenerator.h +++ b/gtsam/sfm/DsfTrackGenerator.h @@ -94,6 +94,10 @@ class SfmTrack2d { } }; +using SfmTrack2dVector = std::vector; + + + /** * @brief Generates point tracks from connected components in the keypoint matches graph. */ diff --git a/gtsam/sfm/sfm.i b/gtsam/sfm/sfm.i index b46b308eb..6c3f1e8eb 100644 --- a/gtsam/sfm/sfm.i +++ b/gtsam/sfm/sfm.i @@ -49,10 +49,20 @@ class SfmTrack2d bool hasUniqueCameras(); }; +class SfmTrack2dVector { + SfmTrack2dVector(); + SfmTrack2dVector(const gtsam::SfmTrack2dVector& other); + void push_back(const gtsam::SfmTrack2d& keypoints); + size_t size() const; + bool empty() const; + void clear(); + gtsam::SfmTrack2d at(const size_t& index) const; +}; + class DsfTrackGenerator { DsfTrackGenerator(); - std::vector generate_tracks_from_pairwise_matches( + const gtsam::SfmTrack2dVector generate_tracks_from_pairwise_matches( const gtsam::MatchIndicesMap& matches_dict, const gtsam::KeypointsList& keypoints_list); }; diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index ffb52ad11..e8cc1f10f 100644 --- a/python/CMakeLists.txt +++ b/python/CMakeLists.txt @@ -152,7 +152,8 @@ if(GTSAM_UNSTABLE_BUILD_PYTHON) gtsam::CameraSetCal3Fisheye gtsam::KeyPairDoubleMap gtsam::MatchIndicesMap - gtsam::KeypointsList) + gtsam::KeypointsList + gtsam::SfmTrack2dVector) pybind_wrap(${GTSAM_PYTHON_UNSTABLE_TARGET} # target diff --git a/python/gtsam/preamble/sfm.h b/python/gtsam/preamble/sfm.h index a51e963dd..fa4458c69 100644 --- a/python/gtsam/preamble/sfm.h +++ b/python/gtsam/preamble/sfm.h @@ -26,6 +26,7 @@ PYBIND11_MAKE_OPAQUE( std::vector>); PYBIND11_MAKE_OPAQUE( std::vector); - +PYBIND11_MAKE_OPAQUE( + std::vector); PYBIND11_MAKE_OPAQUE(gtsam::MatchIndicesMap); \ No newline at end of file diff --git a/python/gtsam/specializations/sfm.h b/python/gtsam/specializations/sfm.h index 297aee262..9eaf0f7fd 100644 --- a/python/gtsam/specializations/sfm.h +++ b/python/gtsam/specializations/sfm.h @@ -37,3 +37,6 @@ py::bind_vector< std::vector>>( m_, "SfmMeasurementVector" ); +py::bind_vector< + std::vector >( + m_, "SfmTrack2dVector"); \ No newline at end of file diff --git a/python/gtsam/tests/test_DsfTrackGenerator.py b/python/gtsam/tests/test_DsfTrackGenerator.py index 74a7fe5dc..41394f17a 100644 --- a/python/gtsam/tests/test_DsfTrackGenerator.py +++ b/python/gtsam/tests/test_DsfTrackGenerator.py @@ -8,7 +8,7 @@ import unittest import numpy as np import gtsam -from gtsam import DsfTrackGenerator, Keypoints, KeypointsList, MatchIndicesMap +from gtsam import DsfTrackGenerator, IndexPair, Keypoints, KeypointsList, MatchIndicesMap from gtsam.utils.test_case import GtsamTestCase @@ -28,8 +28,8 @@ class TestDsfTrackGenerator(GtsamTestCase): # For each image pair (i1,i2), we provide a (K,2) matrix of corresponding image indices (k1,k2). matches_dict = MatchIndicesMap() - matches_dict[gtsam.IndexPair(0, 1)] = np.array([[0, 0], [1, 1]]) - matches_dict[gtsam.IndexPair(1, 2)] = np.array([[2, 0], [1, 1]]) + 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(matches_dict, keypoints_list) assert len(tracks) == 3 From a3069a3fbc36828bd982128bebff7669fc297ef6 Mon Sep 17 00:00:00 2001 From: senselessdev1 Date: Mon, 26 Sep 2022 11:58:07 -0400 Subject: [PATCH 13/30] remove opaque on sfmtrack2d --- python/gtsam/preamble/sfm.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/python/gtsam/preamble/sfm.h b/python/gtsam/preamble/sfm.h index fa4458c69..fe49aadc1 100644 --- a/python/gtsam/preamble/sfm.h +++ b/python/gtsam/preamble/sfm.h @@ -26,7 +26,7 @@ PYBIND11_MAKE_OPAQUE( std::vector>); PYBIND11_MAKE_OPAQUE( std::vector); -PYBIND11_MAKE_OPAQUE( - std::vector); +// PYBIND11_MAKE_OPAQUE( +// std::vector); PYBIND11_MAKE_OPAQUE(gtsam::MatchIndicesMap); \ No newline at end of file From fa17c509107e2ee7a9bee36a3fe12a9a7b55b0e3 Mon Sep 17 00:00:00 2001 From: John Lambert Date: Mon, 26 Sep 2022 18:41:59 -0400 Subject: [PATCH 14/30] Add SfmTrack2dVector to ignore list --- python/CMakeLists.txt | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index e8cc1f10f..3266cf867 100644 --- a/python/CMakeLists.txt +++ b/python/CMakeLists.txt @@ -53,7 +53,8 @@ set(ignore gtsam::DiscreteKey gtsam::KeyPairDoubleMap gtsam::MatchIndicesMap - gtsam::KeypointsList) + gtsam::KeypointsList + gtsam::SfmTrack2dVector) set(interface_headers ${PROJECT_SOURCE_DIR}/gtsam/gtsam.i From 96aaff4bc6b42b824a584c3086311766f8a3daa3 Mon Sep 17 00:00:00 2001 From: senselessdev1 Date: Mon, 26 Sep 2022 20:42:03 -0400 Subject: [PATCH 15/30] wrap vector of NamedSfmMeasurement --- gtsam/sfm/DsfTrackGenerator.h | 2 +- gtsam/sfm/sfm.i | 12 +++++++++++- python/CMakeLists.txt | 6 ++++-- python/gtsam/preamble/sfm.h | 5 ++++- python/gtsam/specializations/sfm.h | 6 +++++- 5 files changed, 25 insertions(+), 6 deletions(-) diff --git a/gtsam/sfm/DsfTrackGenerator.h b/gtsam/sfm/DsfTrackGenerator.h index 16dddcbaa..9a0c350bd 100644 --- a/gtsam/sfm/DsfTrackGenerator.h +++ b/gtsam/sfm/DsfTrackGenerator.h @@ -95,7 +95,7 @@ class SfmTrack2d { }; using SfmTrack2dVector = std::vector; - +using NamedSfmMeasurementVector = std::vector; /** diff --git a/gtsam/sfm/sfm.i b/gtsam/sfm/sfm.i index 6c3f1e8eb..155a551bd 100644 --- a/gtsam/sfm/sfm.i +++ b/gtsam/sfm/sfm.i @@ -42,6 +42,16 @@ class NamedSfmMeasurement NamedSfmMeasurement(size_t i, gtsam::Point2 uv); }; +class NamedSfmMeasurementVector { + NamedSfmMeasurementVector(); + NamedSfmMeasurementVector(const gtsam::NamedSfmMeasurementVector& other); + void push_back(const gtsam::NamedSfmMeasurement& measurement); + size_t size() const; + bool empty() const; + void clear(); + gtsam::NamedSfmMeasurement at(const size_t& index) const; +}; + class SfmTrack2d { void addMeasurement(const gtsam::NamedSfmMeasurement &m); @@ -52,7 +62,7 @@ class SfmTrack2d class SfmTrack2dVector { SfmTrack2dVector(); SfmTrack2dVector(const gtsam::SfmTrack2dVector& other); - void push_back(const gtsam::SfmTrack2d& keypoints); + void push_back(const gtsam::SfmTrack2d& track); size_t size() const; bool empty() const; void clear(); diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index 3266cf867..21f9f951d 100644 --- a/python/CMakeLists.txt +++ b/python/CMakeLists.txt @@ -54,7 +54,8 @@ set(ignore gtsam::KeyPairDoubleMap gtsam::MatchIndicesMap gtsam::KeypointsList - gtsam::SfmTrack2dVector) + gtsam::SfmTrack2dVector + gtsam::NamedSfmMeasurementVector) set(interface_headers ${PROJECT_SOURCE_DIR}/gtsam/gtsam.i @@ -154,7 +155,8 @@ if(GTSAM_UNSTABLE_BUILD_PYTHON) gtsam::KeyPairDoubleMap gtsam::MatchIndicesMap gtsam::KeypointsList - gtsam::SfmTrack2dVector) + gtsam::SfmTrack2dVector + gtsam::NamedSfmMeasurementVector) pybind_wrap(${GTSAM_PYTHON_UNSTABLE_TARGET} # target diff --git a/python/gtsam/preamble/sfm.h b/python/gtsam/preamble/sfm.h index fe49aadc1..429b794b8 100644 --- a/python/gtsam/preamble/sfm.h +++ b/python/gtsam/preamble/sfm.h @@ -29,4 +29,7 @@ PYBIND11_MAKE_OPAQUE( // PYBIND11_MAKE_OPAQUE( // std::vector); -PYBIND11_MAKE_OPAQUE(gtsam::MatchIndicesMap); \ No newline at end of file +PYBIND11_MAKE_OPAQUE(gtsam::MatchIndicesMap); + +PYBIND11_MAKE_OPAQUE( + std::vector); \ No newline at end of file diff --git a/python/gtsam/specializations/sfm.h b/python/gtsam/specializations/sfm.h index 9eaf0f7fd..1bbb93570 100644 --- a/python/gtsam/specializations/sfm.h +++ b/python/gtsam/specializations/sfm.h @@ -39,4 +39,8 @@ py::bind_vector< ); py::bind_vector< std::vector >( - m_, "SfmTrack2dVector"); \ No newline at end of file + m_, "SfmTrack2dVector"); + +py::bind_vector< + std::vector >( + m_, "NamedSfmMeasurementVector"); From 6eb4ada2fdcb9c4d2ade763514207d5bb724982b Mon Sep 17 00:00:00 2001 From: senselessdev1 Date: Tue, 27 Sep 2022 10:28:57 -0400 Subject: [PATCH 16/30] add more methods for accessing members, getting size --- gtsam/sfm/DsfTrackGenerator.h | 15 +++++++++++++++ 1 file changed, 15 insertions(+) diff --git a/gtsam/sfm/DsfTrackGenerator.h b/gtsam/sfm/DsfTrackGenerator.h index 9a0c350bd..b65a4fad6 100644 --- a/gtsam/sfm/DsfTrackGenerator.h +++ b/gtsam/sfm/DsfTrackGenerator.h @@ -73,11 +73,26 @@ class SfmTrack2d { std::vector measurements_; public: + // Default constructor. + SfmTrack2d() = default; + + // Constructor from measurements. + SfmTrack2d(std::vector &measurements) : measurements_(measurements) {} + + // Add a measurement to the track. void addMeasurement(const NamedSfmMeasurement &m) { measurements_.emplace_back(m); } + + /// The measurement at index `idx` + NamedSfmMeasurement measurement(size_t idx) const { return measurements_[idx]; } + + // Return all measurements in the track. std::vector measurements() {return measurements_; } + /// Total number of measurements in this track. + size_t numberMeasurements() const { return measurements_.size(); } + // @brief Validates the track by checking that no two measurements are from the same camera. // // returns boolean result of the validation. From cafab61fecb8fcdf14065c8de1fd8a340504b10c Mon Sep 17 00:00:00 2001 From: senselessdev1 Date: Tue, 27 Sep 2022 10:29:36 -0400 Subject: [PATCH 17/30] expand tests --- gtsam/sfm/sfm.i | 4 +++ python/gtsam/preamble/sfm.h | 5 ---- python/gtsam/tests/test_DsfTrackGenerator.py | 26 +++++++++++++++++++- 3 files changed, 29 insertions(+), 6 deletions(-) diff --git a/gtsam/sfm/sfm.i b/gtsam/sfm/sfm.i index 155a551bd..6fd380b3c 100644 --- a/gtsam/sfm/sfm.i +++ b/gtsam/sfm/sfm.i @@ -54,8 +54,12 @@ class NamedSfmMeasurementVector { class SfmTrack2d { + SfmTrack2d(); + SfmTrack2d(std::vector &measurements); + size_t numberMeasurements() const; void addMeasurement(const gtsam::NamedSfmMeasurement &m); std::vector measurements(); + gtsam::NamedSfmMeasurement measurement(size_t idx) const; bool hasUniqueCameras(); }; diff --git a/python/gtsam/preamble/sfm.h b/python/gtsam/preamble/sfm.h index 429b794b8..91c9fc418 100644 --- a/python/gtsam/preamble/sfm.h +++ b/python/gtsam/preamble/sfm.h @@ -17,7 +17,6 @@ PYBIND11_MAKE_OPAQUE( std::vector); - PYBIND11_MAKE_OPAQUE( std::vector); PYBIND11_MAKE_OPAQUE( @@ -26,10 +25,6 @@ PYBIND11_MAKE_OPAQUE( std::vector>); PYBIND11_MAKE_OPAQUE( std::vector); -// PYBIND11_MAKE_OPAQUE( -// std::vector); - PYBIND11_MAKE_OPAQUE(gtsam::MatchIndicesMap); - PYBIND11_MAKE_OPAQUE( std::vector); \ No newline at end of file diff --git a/python/gtsam/tests/test_DsfTrackGenerator.py b/python/gtsam/tests/test_DsfTrackGenerator.py index 41394f17a..3d4188284 100644 --- a/python/gtsam/tests/test_DsfTrackGenerator.py +++ b/python/gtsam/tests/test_DsfTrackGenerator.py @@ -8,7 +8,16 @@ import unittest import numpy as np import gtsam -from gtsam import DsfTrackGenerator, IndexPair, Keypoints, KeypointsList, MatchIndicesMap +from gtsam import ( + DsfTrackGenerator, + IndexPair, + Keypoints, + KeypointsList, + MatchIndicesMap, + NamedSfmMeasurement, + NamedSfmMeasurementVector, + SfmTrack2d, +) from gtsam.utils.test_case import GtsamTestCase @@ -39,6 +48,7 @@ class TestDsfTrackGenerator(GtsamTestCase): 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].numberMeasurements() == 2 # Verify track 1. assert np.allclose(tracks[1].measurements()[0].uv, np.array([30.0, 40.0])) @@ -47,9 +57,23 @@ class TestDsfTrackGenerator(GtsamTestCase): assert tracks[1].measurements()[0].i == 0 assert tracks[1].measurements()[1].i == 1 assert tracks[1].measurements()[2].i == 2 + assert tracks[1].numberMeasurements() == 3 # 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].numberMeasurements() == 2 + + +class TestSfmTrack2d(GtsamTestCase): + """Tests for SfmTrack2d.""" + + def test_sfm_track_2d_constructor(self) -> None: + """ """ + measurements = NamedSfmMeasurementVector() + measurements.append(NamedSfmMeasurement(i=0, uv=np.array([10.0, 20.0]))) + track = SfmTrack2d(measurements=measurements) + track.measurement(0) + track.numberMeasurements() == 1 From 4d690efdebead9dd9449feecb4aa11318bf5fba6 Mon Sep 17 00:00:00 2001 From: senselessdev1 Date: Wed, 5 Oct 2022 22:45:03 -0400 Subject: [PATCH 18/30] add docimentation for Keypoints class --- gtsam/sfm/DsfTrackGenerator.h | 14 ++++++++++++-- 1 file changed, 12 insertions(+), 2 deletions(-) diff --git a/gtsam/sfm/DsfTrackGenerator.h b/gtsam/sfm/DsfTrackGenerator.h index b65a4fad6..05ad61390 100644 --- a/gtsam/sfm/DsfTrackGenerator.h +++ b/gtsam/sfm/DsfTrackGenerator.h @@ -37,17 +37,27 @@ typedef Eigen::MatrixX2i CorrespondenceIndices; // N x 2 array using KeypointCoordinates = Eigen::MatrixX2d; +// Output of detections in an image. +// Coordinate system convention: +// 1. The x coordinate denotes the horizontal direction (+ve direction towards the right). +// 2. The y coordinate denotes the vertical direction (+ve direction downwards). +// 3. Origin is at the top left corner of the image. struct Keypoints { + + // The (x, y) coordinates of the features, of shape Nx2. KeypointCoordinates coordinates; - // typedef'd for Eigen::VectorXd + + // Optional scale of the detections, of shape N. + // Note: gtsam::Vector is typedef'd for Eigen::VectorXd. boost::optional scales; + + /// Optional confidences/responses for each detection, of shape N. boost::optional responses; Keypoints(const gtsam::KeypointCoordinates& coordinates): coordinates(coordinates) {}; // boost::none }; using KeypointsList = std::vector; -using KeypointsVector = std::vector; // TODO(johnwlambert): prefer KeypointsSet? // Mapping from each image pair to (N,2) array representing indices of matching keypoints. using MatchIndicesMap = std::map; From 4f406650f73f90811f100c7e0b4f212d23836aad Mon Sep 17 00:00:00 2001 From: senselessdev1 Date: Wed, 12 Oct 2022 21:29:18 -0400 Subject: [PATCH 19/30] rename KeypointsList -> KeypointsVector --- gtsam/sfm/DsfTrackGenerator.h | 4 ++-- gtsam/sfm/sfm.i | 8 ++++---- python/CMakeLists.txt | 4 ++-- python/gtsam/specializations/sfm.h | 2 +- python/gtsam/tests/test_DsfTrackGenerator.py | 4 ++-- 5 files changed, 11 insertions(+), 11 deletions(-) diff --git a/gtsam/sfm/DsfTrackGenerator.h b/gtsam/sfm/DsfTrackGenerator.h index 05ad61390..a16ed3522 100644 --- a/gtsam/sfm/DsfTrackGenerator.h +++ b/gtsam/sfm/DsfTrackGenerator.h @@ -57,7 +57,7 @@ struct Keypoints { Keypoints(const gtsam::KeypointCoordinates& coordinates): coordinates(coordinates) {}; // boost::none }; -using KeypointsList = std::vector; +using KeypointsVector = std::vector; // Mapping from each image pair to (N,2) array representing indices of matching keypoints. using MatchIndicesMap = std::map; @@ -143,7 +143,7 @@ class DsfTrackGenerator { // @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) { + const KeypointsVector& keypoints_list) { std::vector track_2d_list; std::cout << "[SfmTrack2d] Starting Union-Find..." << std::endl; diff --git a/gtsam/sfm/sfm.i b/gtsam/sfm/sfm.i index 6fd380b3c..9613410a7 100644 --- a/gtsam/sfm/sfm.i +++ b/gtsam/sfm/sfm.i @@ -24,9 +24,9 @@ class Keypoints }; // check if this should be a method -class KeypointsList { - KeypointsList(); - KeypointsList(const gtsam::KeypointsList& other); +class KeypointsVector { + KeypointsVector(); + KeypointsVector(const gtsam::KeypointsVector& other); void push_back(const gtsam::Keypoints& keypoints); size_t size() const; bool empty() const; @@ -78,7 +78,7 @@ class DsfTrackGenerator { DsfTrackGenerator(); const gtsam::SfmTrack2dVector generate_tracks_from_pairwise_matches( const gtsam::MatchIndicesMap& matches_dict, - const gtsam::KeypointsList& keypoints_list); + const gtsam::KeypointsVector& keypoints_list); }; diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index 21f9f951d..600aeecc9 100644 --- a/python/CMakeLists.txt +++ b/python/CMakeLists.txt @@ -53,7 +53,7 @@ set(ignore gtsam::DiscreteKey gtsam::KeyPairDoubleMap gtsam::MatchIndicesMap - gtsam::KeypointsList + gtsam::KeypointsVector gtsam::SfmTrack2dVector gtsam::NamedSfmMeasurementVector) @@ -154,7 +154,7 @@ if(GTSAM_UNSTABLE_BUILD_PYTHON) gtsam::CameraSetCal3Fisheye gtsam::KeyPairDoubleMap gtsam::MatchIndicesMap - gtsam::KeypointsList + gtsam::KeypointsVector gtsam::SfmTrack2dVector gtsam::NamedSfmMeasurementVector) diff --git a/python/gtsam/specializations/sfm.h b/python/gtsam/specializations/sfm.h index 1bbb93570..63f7f1a48 100644 --- a/python/gtsam/specializations/sfm.h +++ b/python/gtsam/specializations/sfm.h @@ -22,7 +22,7 @@ py::bind_map(m_, "MatchIndicesMap"); py::bind_vector< std::vector >( - m_, "KeypointsList"); + m_, "KeypointsVector"); py::bind_vector< diff --git a/python/gtsam/tests/test_DsfTrackGenerator.py b/python/gtsam/tests/test_DsfTrackGenerator.py index 3d4188284..563a01165 100644 --- a/python/gtsam/tests/test_DsfTrackGenerator.py +++ b/python/gtsam/tests/test_DsfTrackGenerator.py @@ -12,7 +12,7 @@ from gtsam import ( DsfTrackGenerator, IndexPair, Keypoints, - KeypointsList, + KeypointsVector, MatchIndicesMap, NamedSfmMeasurement, NamedSfmMeasurementVector, @@ -30,7 +30,7 @@ class TestDsfTrackGenerator(GtsamTestCase): kps_i1 = Keypoints(coordinates=np.array([[50.0, 60], [70, 80], [90, 100]])) kps_i2 = Keypoints(coordinates=np.array([[110.0, 120], [130, 140]])) - keypoints_list = KeypointsList() + keypoints_list = KeypointsVector() keypoints_list.append(kps_i0) keypoints_list.append(kps_i1) keypoints_list.append(kps_i2) From 7dee1af5b4dec1ef1a558669e18e39d7ed638f5a Mon Sep 17 00:00:00 2001 From: senselessdev1 Date: Wed, 12 Oct 2022 23:31:45 -0400 Subject: [PATCH 20/30] add verbose flag for logging messages during DSF track generation --- gtsam/sfm/DsfTrackGenerator.h | 19 +++++++++++-------- gtsam/sfm/sfm.i | 3 ++- python/gtsam/tests/test_DsfTrackGenerator.py | 4 +++- 3 files changed, 16 insertions(+), 10 deletions(-) diff --git a/gtsam/sfm/DsfTrackGenerator.h b/gtsam/sfm/DsfTrackGenerator.h index a16ed3522..960f1622c 100644 --- a/gtsam/sfm/DsfTrackGenerator.h +++ b/gtsam/sfm/DsfTrackGenerator.h @@ -143,10 +143,11 @@ class DsfTrackGenerator { // @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) { + const KeypointsVector& keypoints_list, + bool verbose = false) { std::vector track_2d_list; - std::cout << "[SfmTrack2d] Starting Union-Find..." << std::endl; + if (verbose) std::cout << "[SfmTrack2d] Starting Union-Find..." << std::endl; // Generate the DSF to form tracks. DSFMapIndexPair dsf; @@ -167,7 +168,7 @@ class DsfTrackGenerator { } } - std::cout << "[SfmTrack2d] Union-Find Complete" << std::endl; + if (verbose) std::cout << "[SfmTrack2d] Union-Find Complete" << std::endl; const std::map > key_sets = dsf.sets(); // Return immediately if no sets were found. @@ -202,12 +203,14 @@ class DsfTrackGenerator { } } - 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; + if (verbose) { + 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; } diff --git a/gtsam/sfm/sfm.i b/gtsam/sfm/sfm.i index 9613410a7..ce840a1a9 100644 --- a/gtsam/sfm/sfm.i +++ b/gtsam/sfm/sfm.i @@ -78,7 +78,8 @@ class DsfTrackGenerator { DsfTrackGenerator(); const gtsam::SfmTrack2dVector generate_tracks_from_pairwise_matches( const gtsam::MatchIndicesMap& matches_dict, - const gtsam::KeypointsVector& keypoints_list); + const gtsam::KeypointsVector& keypoints_list, + bool verbose = false); }; diff --git a/python/gtsam/tests/test_DsfTrackGenerator.py b/python/gtsam/tests/test_DsfTrackGenerator.py index 563a01165..721584cff 100644 --- a/python/gtsam/tests/test_DsfTrackGenerator.py +++ b/python/gtsam/tests/test_DsfTrackGenerator.py @@ -40,7 +40,9 @@ 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(matches_dict, keypoints_list) + tracks = DsfTrackGenerator().generate_tracks_from_pairwise_matches(matches_dict, + keypoints_list, + verbose=True) assert len(tracks) == 3 # Verify track 0. From f9971f504988f891f9ffae02ba9e94009d2ce2a6 Mon Sep 17 00:00:00 2001 From: senselessdev1 Date: Thu, 13 Oct 2022 01:07:41 -0400 Subject: [PATCH 21/30] use gtsam.gtsfm namespace for new API --- gtsam/sfm/DsfTrackGenerator.h | 8 +++- gtsam/sfm/sfm.i | 44 ++++++++++---------- python/CMakeLists.txt | 16 +++---- python/gtsam/preamble/sfm.h | 6 +-- python/gtsam/specializations/sfm.h | 22 ++++------ python/gtsam/tests/test_DsfTrackGenerator.py | 7 +--- 6 files changed, 51 insertions(+), 52 deletions(-) diff --git a/gtsam/sfm/DsfTrackGenerator.h b/gtsam/sfm/DsfTrackGenerator.h index 960f1622c..47578d6ea 100644 --- a/gtsam/sfm/DsfTrackGenerator.h +++ b/gtsam/sfm/DsfTrackGenerator.h @@ -30,6 +30,8 @@ namespace gtsam { +namespace gtsfm { + typedef DSFMap DSFMapIndexPair; typedef Eigen::MatrixX2i CorrespondenceIndices; // N x 2 array @@ -54,7 +56,7 @@ struct Keypoints { /// Optional confidences/responses for each detection, of shape N. boost::optional responses; - Keypoints(const gtsam::KeypointCoordinates& coordinates): coordinates(coordinates) {}; // boost::none + Keypoints(const gtsam::gtsfm::KeypointCoordinates& coordinates): coordinates(coordinates) {}; // boost::none }; using KeypointsVector = std::vector; @@ -119,7 +121,7 @@ class SfmTrack2d { } }; -using SfmTrack2dVector = std::vector; +using SfmTrack2dVector = std::vector; using NamedSfmMeasurementVector = std::vector; @@ -216,5 +218,7 @@ class DsfTrackGenerator { } }; +}///\namespace gtsfm + } // namespace gtsam diff --git a/gtsam/sfm/sfm.i b/gtsam/sfm/sfm.i index ce840a1a9..984955d23 100644 --- a/gtsam/sfm/sfm.i +++ b/gtsam/sfm/sfm.i @@ -4,34 +4,36 @@ namespace gtsam { +namespace gtsfm { + #include class MatchIndicesMap { MatchIndicesMap(); - MatchIndicesMap(const gtsam::MatchIndicesMap& other); + MatchIndicesMap(const gtsam::gtsfm::MatchIndicesMap& other); size_t size() const; bool empty() const; void clear(); - gtsam::CorrespondenceIndices at(const pair& keypair) const; + gtsam::gtsfm::CorrespondenceIndices at(const pair& keypair) const; }; class Keypoints { - Keypoints(const gtsam::KeypointCoordinates& coordinates); - gtsam::KeypointCoordinates coordinates; + Keypoints(const gtsam::gtsfm::KeypointCoordinates& coordinates); + gtsam::gtsfm::KeypointCoordinates coordinates; }; // check if this should be a method class KeypointsVector { KeypointsVector(); - KeypointsVector(const gtsam::KeypointsVector& other); - void push_back(const gtsam::Keypoints& keypoints); + KeypointsVector(const gtsam::gtsfm::KeypointsVector& other); + void push_back(const gtsam::gtsfm::Keypoints& keypoints); size_t size() const; bool empty() const; void clear(); - gtsam::Keypoints at(const size_t& index) const; + gtsam::gtsfm::Keypoints at(const size_t& index) const; }; @@ -44,44 +46,44 @@ class NamedSfmMeasurement class NamedSfmMeasurementVector { NamedSfmMeasurementVector(); - NamedSfmMeasurementVector(const gtsam::NamedSfmMeasurementVector& other); - void push_back(const gtsam::NamedSfmMeasurement& measurement); + NamedSfmMeasurementVector(const gtsam::gtsfm::NamedSfmMeasurementVector& other); + void push_back(const gtsam::gtsfm::NamedSfmMeasurement& measurement); size_t size() const; bool empty() const; void clear(); - gtsam::NamedSfmMeasurement at(const size_t& index) const; + gtsam::gtsfm::NamedSfmMeasurement at(const size_t& index) const; }; class SfmTrack2d { SfmTrack2d(); - SfmTrack2d(std::vector &measurements); + SfmTrack2d(std::vector &measurements); size_t numberMeasurements() const; - void addMeasurement(const gtsam::NamedSfmMeasurement &m); - std::vector measurements(); - gtsam::NamedSfmMeasurement measurement(size_t idx) const; + void addMeasurement(const gtsam::gtsfm::NamedSfmMeasurement &m); + std::vector measurements(); + gtsam::gtsfm::NamedSfmMeasurement measurement(size_t idx) const; bool hasUniqueCameras(); }; class SfmTrack2dVector { SfmTrack2dVector(); - SfmTrack2dVector(const gtsam::SfmTrack2dVector& other); - void push_back(const gtsam::SfmTrack2d& track); + SfmTrack2dVector(const gtsam::gtsfm::SfmTrack2dVector& other); + void push_back(const gtsam::gtsfm::SfmTrack2d& track); size_t size() const; bool empty() const; void clear(); - gtsam::SfmTrack2d at(const size_t& index) const; + gtsam::gtsfm::SfmTrack2d at(const size_t& index) const; }; class DsfTrackGenerator { DsfTrackGenerator(); - const gtsam::SfmTrack2dVector generate_tracks_from_pairwise_matches( - const gtsam::MatchIndicesMap& matches_dict, - const gtsam::KeypointsVector& keypoints_list, + const gtsam::gtsfm::SfmTrack2dVector generate_tracks_from_pairwise_matches( + const gtsam::gtsfm::MatchIndicesMap& matches_dict, + const gtsam::gtsfm::KeypointsVector& keypoints_list, bool verbose = false); }; - +}///\namespace gtsfm #include #include diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index 600aeecc9..398ee2788 100644 --- a/python/CMakeLists.txt +++ b/python/CMakeLists.txt @@ -52,10 +52,10 @@ set(ignore gtsam::BinaryMeasurementsRot3 gtsam::DiscreteKey gtsam::KeyPairDoubleMap - gtsam::MatchIndicesMap - gtsam::KeypointsVector - gtsam::SfmTrack2dVector - gtsam::NamedSfmMeasurementVector) + gtsam::gtsfm::MatchIndicesMap + gtsam::gtsfm::KeypointsVector + gtsam::gtsfm::SfmTrack2dVector + gtsam::gtsfm::NamedSfmMeasurementVector) set(interface_headers ${PROJECT_SOURCE_DIR}/gtsam/gtsam.i @@ -153,10 +153,10 @@ if(GTSAM_UNSTABLE_BUILD_PYTHON) gtsam::CameraSetCal3Unified gtsam::CameraSetCal3Fisheye gtsam::KeyPairDoubleMap - gtsam::MatchIndicesMap - gtsam::KeypointsVector - gtsam::SfmTrack2dVector - gtsam::NamedSfmMeasurementVector) + gtsam::gtsfm::MatchIndicesMap + gtsam::gtsfm::KeypointsVector + gtsam::gtsfm::SfmTrack2dVector + gtsam::gtsfm::NamedSfmMeasurementVector) pybind_wrap(${GTSAM_PYTHON_UNSTABLE_TARGET} # target diff --git a/python/gtsam/preamble/sfm.h b/python/gtsam/preamble/sfm.h index 91c9fc418..067dd8801 100644 --- a/python/gtsam/preamble/sfm.h +++ b/python/gtsam/preamble/sfm.h @@ -24,7 +24,7 @@ PYBIND11_MAKE_OPAQUE( PYBIND11_MAKE_OPAQUE( std::vector>); PYBIND11_MAKE_OPAQUE( - std::vector); -PYBIND11_MAKE_OPAQUE(gtsam::MatchIndicesMap); + std::vector); +PYBIND11_MAKE_OPAQUE(gtsam::gtsfm::MatchIndicesMap); PYBIND11_MAKE_OPAQUE( - std::vector); \ No newline at end of file + std::vector); \ No newline at end of file diff --git a/python/gtsam/specializations/sfm.h b/python/gtsam/specializations/sfm.h index 63f7f1a48..10fb1a0ab 100644 --- a/python/gtsam/specializations/sfm.h +++ b/python/gtsam/specializations/sfm.h @@ -18,29 +18,25 @@ py::bind_vector > >( py::bind_vector > >( m_, "BinaryMeasurementsRot3"); py::bind_map(m_, "KeyPairDoubleMap"); -py::bind_map(m_, "MatchIndicesMap"); - -py::bind_vector< - std::vector >( - m_, "KeypointsVector"); - - py::bind_vector< std::vector >( m_, "SfmTracks"); - py::bind_vector< std::vector >( m_, "SfmCameras"); - py::bind_vector< std::vector>>( m_, "SfmMeasurementVector" ); -py::bind_vector< - std::vector >( - m_, "SfmTrack2dVector"); py::bind_vector< - std::vector >( + std::vector >( + m_, "SfmTrack2dVector"); +py::bind_vector< + std::vector >( m_, "NamedSfmMeasurementVector"); +py::bind_map(m_, "MatchIndicesMap"); + +py::bind_vector< + std::vector >( + m_, "KeypointsVector"); diff --git a/python/gtsam/tests/test_DsfTrackGenerator.py b/python/gtsam/tests/test_DsfTrackGenerator.py index 721584cff..f1851c221 100644 --- a/python/gtsam/tests/test_DsfTrackGenerator.py +++ b/python/gtsam/tests/test_DsfTrackGenerator.py @@ -8,14 +8,11 @@ import unittest import numpy as np import gtsam -from gtsam import ( +from gtsam import IndexPair, KeypointsVector, MatchIndicesMap, NamedSfmMeasurementVector +from gtsam.gtsam.gtsfm import ( DsfTrackGenerator, - IndexPair, Keypoints, - KeypointsVector, - MatchIndicesMap, NamedSfmMeasurement, - NamedSfmMeasurementVector, SfmTrack2d, ) from gtsam.utils.test_case import GtsamTestCase From 73fd3f9dc2eb249397909a098247b0a66da709c6 Mon Sep 17 00:00:00 2001 From: senselessdev1 Date: Thu, 13 Oct 2022 09:12:55 -0400 Subject: [PATCH 22/30] add namespace hack to prevent gtsam.gtsam.gtsfm --- python/gtsam/gtsfm.py | 4 ++++ python/gtsam/tests/test_DsfTrackGenerator.py | 2 +- 2 files changed, 5 insertions(+), 1 deletion(-) create mode 100644 python/gtsam/gtsfm.py diff --git a/python/gtsam/gtsfm.py b/python/gtsam/gtsfm.py new file mode 100644 index 000000000..afa709083 --- /dev/null +++ b/python/gtsam/gtsfm.py @@ -0,0 +1,4 @@ +# This trick is to allow direct import of sub-modules +# without this, we can only do `from gtsam.gtsam.gtsfm import X` +# with this trick, we can do `from gtsam.gtsfm import X` +from .gtsam.gtsfm import * \ No newline at end of file diff --git a/python/gtsam/tests/test_DsfTrackGenerator.py b/python/gtsam/tests/test_DsfTrackGenerator.py index f1851c221..e63025cd3 100644 --- a/python/gtsam/tests/test_DsfTrackGenerator.py +++ b/python/gtsam/tests/test_DsfTrackGenerator.py @@ -9,7 +9,7 @@ import numpy as np import gtsam from gtsam import IndexPair, KeypointsVector, MatchIndicesMap, NamedSfmMeasurementVector -from gtsam.gtsam.gtsfm import ( +from gtsam.gtsfm import ( DsfTrackGenerator, Keypoints, NamedSfmMeasurement, From cafa3c556c23db08f6eb5242480b18be1cba5253 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 22 Oct 2022 18:37:20 -0700 Subject: [PATCH 23/30] Split SfmTrack into 2 classes --- gtsam/sfm/SfmTrack.h | 62 +++++++++++++++++++++++++------- gtsam/sfm/tests/testSfmTrack.cpp | 53 +++++++++++++++++++++++++++ 2 files changed, 103 insertions(+), 12 deletions(-) create mode 100644 gtsam/sfm/tests/testSfmTrack.cpp diff --git a/gtsam/sfm/SfmTrack.h b/gtsam/sfm/SfmTrack.h index 37731b32a..471eff994 100644 --- a/gtsam/sfm/SfmTrack.h +++ b/gtsam/sfm/SfmTrack.h @@ -35,28 +35,26 @@ typedef std::pair SfmMeasurement; typedef std::pair SiftIndex; /** - * @brief An SfmTrack stores SfM measurements grouped in a track - * @addtogroup sfm + * @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. */ -struct GTSAM_EXPORT SfmTrack { - Point3 p; ///< 3D position of the point - float r, g, b; ///< RGB color of the 3D point - +struct GTSAM_EXPORT SfmTrack2d { /// The 2D image projections (id,(u,v)) std::vector measurements; - /// The feature descriptors + /// The feature descriptors (optional) std::vector siftIndices; /// @name Constructors /// @{ - explicit SfmTrack(float r = 0, float g = 0, float b = 0) - : p(0, 0, 0), r(r), g(g), b(b) {} + // Default constructor. + SfmTrack2d() = default; - explicit SfmTrack(const gtsam::Point3& pt, float r = 0, float g = 0, - float b = 0) - : p(pt), r(r), g(g), b(b) {} + // Constructor from measurements. + explicit SfmTrack2d(const std::vector& measurements) + : measurements(measurements) {} /// @} /// @name Standard Interface @@ -78,6 +76,46 @@ struct GTSAM_EXPORT SfmTrack { /// Get the SIFT feature index corresponding to the measurement at `idx` const SiftIndex& siftIndex(size_t idx) const { return siftIndices[idx]; } + /** + * @brief Validates the track by checking that no two measurements are from + * @returns boolean result of the validation. + */ + bool hasUniqueCameras() { + std::vector track_cam_indices; + for (auto& measurement : measurements) { + track_cam_indices.emplace_back(measurement.first); + } + auto i = + std::adjacent_find(track_cam_indices.begin(), track_cam_indices.end()); + bool all_cameras_unique = (i == track_cam_indices.end()); + return all_cameras_unique; + } +}; + +using SfmTrack2dVector = std::vector; + +/** + * @brief An SfmTrack stores SfM measurements grouped in a track + * @addtogroup sfm + */ +struct GTSAM_EXPORT SfmTrack : SfmTrack2d { + Point3 p; ///< 3D position of the point + float r, g, b; ///< RGB color of the 3D point + + /// @name Constructors + /// @{ + + explicit SfmTrack(float r = 0, float g = 0, float b = 0) + : p(0, 0, 0), r(r), g(g), b(b) {} + + explicit SfmTrack(const gtsam::Point3& pt, float r = 0, float g = 0, + float b = 0) + : p(pt), r(r), g(g), b(b) {} + + /// @} + /// @name Standard Interface + /// @{ + /// Get 3D point const Point3& point3() const { return p; } diff --git a/gtsam/sfm/tests/testSfmTrack.cpp b/gtsam/sfm/tests/testSfmTrack.cpp new file mode 100644 index 000000000..1b8c6bd9a --- /dev/null +++ b/gtsam/sfm/tests/testSfmTrack.cpp @@ -0,0 +1,53 @@ +/* ---------------------------------------------------------------------------- + + * 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 TestSfmTrack.cpp + * @date October 2022 + * @author Frank Dellaert + * @brief tests for SfmTrack class + */ + +#include +#include +#include +#include + +using namespace std; +using namespace gtsam; + +/* ************************************************************************* */ +TEST(SfmTrack2d, defaultConstructor) { + SfmTrack2d track; + EXPECT_LONGS_EQUAL(0, track.measurements.size()); + EXPECT_LONGS_EQUAL(0, track.siftIndices.size()); +} + +/* ************************************************************************* */ +TEST(SfmTrack2d, measurementConstructor) { + SfmTrack2d track({{0, Point2(1, 2)}, {1, Point2(3, 4)}}); + EXPECT_LONGS_EQUAL(2, track.measurements.size()); + EXPECT_LONGS_EQUAL(0, track.siftIndices.size()); +} + +/* ************************************************************************* */ +TEST(SfmTrack, construction) { + SfmTrack track(Point3(1, 2, 3), 4, 5, 6); + EXPECT(assert_equal(Point3(1, 2, 3), track.point3())); + EXPECT(assert_equal(Point3(4, 5, 6), track.rgb())); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ From 38be12eaf46885d2ae257215582ba616472b5b7c Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 22 Oct 2022 18:37:44 -0700 Subject: [PATCH 24/30] Use SfmMeasurement and SfmTrack --- gtsam/sfm/DsfTrackGenerator.h | 139 ++++++------------- gtsam/sfm/SfmTrack.h | 2 +- gtsam/sfm/sfm.i | 139 +++++++------------ python/gtsam/preamble/sfm.h | 11 +- python/gtsam/specializations/sfm.h | 26 +--- python/gtsam/tests/test_DsfTrackGenerator.py | 77 +++++----- 6 files changed, 144 insertions(+), 250 deletions(-) diff --git a/gtsam/sfm/DsfTrackGenerator.h b/gtsam/sfm/DsfTrackGenerator.h index 47578d6ea..0dae3d216 100644 --- a/gtsam/sfm/DsfTrackGenerator.h +++ b/gtsam/sfm/DsfTrackGenerator.h @@ -17,11 +17,11 @@ */ #pragma once -#include - #include #include +#include +#include #include #include #include @@ -33,19 +33,18 @@ namespace gtsam { namespace gtsfm { typedef DSFMap DSFMapIndexPair; -typedef Eigen::MatrixX2i CorrespondenceIndices; // N x 2 array +typedef Eigen::MatrixX2i CorrespondenceIndices; // N x 2 array -//struct Keypoints; +// struct Keypoints; using KeypointCoordinates = Eigen::MatrixX2d; - // Output of detections in an image. // Coordinate system convention: -// 1. The x coordinate denotes the horizontal direction (+ve direction towards the right). +// 1. The x coordinate denotes the horizontal direction (+ve direction towards +// the right). // 2. The y coordinate denotes the vertical direction (+ve direction downwards). // 3. Origin is at the top left corner of the image. struct Keypoints { - // The (x, y) coordinates of the features, of shape Nx2. KeypointCoordinates coordinates; @@ -56,80 +55,20 @@ struct Keypoints { /// Optional confidences/responses for each detection, of shape N. boost::optional responses; - Keypoints(const gtsam::gtsfm::KeypointCoordinates& coordinates): coordinates(coordinates) {}; // boost::none + Keypoints(const KeypointCoordinates& coordinates) + : coordinates(coordinates){}; // boost::none }; using KeypointsVector = std::vector; -// Mapping from each image pair to (N,2) array representing indices of matching keypoints. +// Mapping from each image pair to (N,2) array representing indices of matching +// keypoints. 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 { - size_t i; - gtsam::Point2 uv; - - NamedSfmMeasurement(size_t i, gtsam::Point2 uv) : i(i), uv(uv) {} -}; - - /** - * @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: - std::vector measurements_; - - public: - // Default constructor. - SfmTrack2d() = default; - - // Constructor from measurements. - SfmTrack2d(std::vector &measurements) : measurements_(measurements) {} - - // Add a measurement to the track. - void addMeasurement(const NamedSfmMeasurement &m) { - measurements_.emplace_back(m); - } - - /// The measurement at index `idx` - NamedSfmMeasurement measurement(size_t idx) const { return measurements_[idx]; } - - // Return all measurements in the track. - std::vector measurements() {return measurements_; } - - /// Total number of measurements in this track. - size_t numberMeasurements() const { return measurements_.size(); } - - // @brief Validates the track by checking that no two measurements are from the same camera. - // - // returns boolean result of the validation. - bool hasUniqueCameras() - { - std::vector track_cam_idxs; - for (auto & measurement : measurements_) - { - track_cam_idxs.emplace_back(measurement.i); - } - auto i = std::adjacent_find(track_cam_idxs.begin(), track_cam_idxs.end()); - bool all_cameras_unique = (i == track_cam_idxs.end()); - return all_cameras_unique; - } -}; - -using SfmTrack2dVector = std::vector; -using NamedSfmMeasurementVector = std::vector; - - -/** - * @brief Generates point tracks from connected components in the keypoint matches graph. + * @brief Generates point tracks from connected components in the keypoint + * matches graph. */ class DsfTrackGenerator { - public: /** Default constructor. */ DsfTrackGenerator() {} @@ -144,28 +83,28 @@ class DsfTrackGenerator { // 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) { + 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; + if (verbose) + 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; + const auto pair_indices = kv.first; + const auto corr_indices = 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++) - { + 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_idxs(k, 0); - size_t k2 = corr_idxs(k, 1); - // Unique keys for the DSF are (i,k), representing keypoint index in an image. + 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)); } } @@ -186,19 +125,19 @@ class DsfTrackGenerator { // 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. + 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))); + 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()) - { + // 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++; @@ -206,19 +145,19 @@ class DsfTrackGenerator { } if (verbose) { - double erroneous_track_pct = static_cast(erroneous_track_count) - / static_cast(key_sets.size()) * 100; + 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; + 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 gtsfm - -} // namespace gtsam +} // namespace gtsfm +} // namespace gtsam diff --git a/gtsam/sfm/SfmTrack.h b/gtsam/sfm/SfmTrack.h index 471eff994..1fe5aedf4 100644 --- a/gtsam/sfm/SfmTrack.h +++ b/gtsam/sfm/SfmTrack.h @@ -80,7 +80,7 @@ struct GTSAM_EXPORT SfmTrack2d { * @brief Validates the track by checking that no two measurements are from * @returns boolean result of the validation. */ - bool hasUniqueCameras() { + bool hasUniqueCameras() const { std::vector track_cam_indices; for (auto& measurement : measurements) { track_cam_indices.emplace_back(measurement.first); diff --git a/gtsam/sfm/sfm.i b/gtsam/sfm/sfm.i index 984955d23..02d3b7b5b 100644 --- a/gtsam/sfm/sfm.i +++ b/gtsam/sfm/sfm.i @@ -4,91 +4,22 @@ namespace gtsam { -namespace gtsfm { - -#include - -class MatchIndicesMap { - MatchIndicesMap(); - MatchIndicesMap(const gtsam::gtsfm::MatchIndicesMap& other); - - size_t size() const; - bool empty() const; - void clear(); - gtsam::gtsfm::CorrespondenceIndices at(const pair& keypair) const; -}; - - -class Keypoints -{ - Keypoints(const gtsam::gtsfm::KeypointCoordinates& coordinates); - gtsam::gtsfm::KeypointCoordinates coordinates; -}; // check if this should be a method - - -class KeypointsVector { - KeypointsVector(); - KeypointsVector(const gtsam::gtsfm::KeypointsVector& other); - void push_back(const gtsam::gtsfm::Keypoints& keypoints); - size_t size() const; - bool empty() const; - void clear(); - gtsam::gtsfm::Keypoints at(const size_t& index) const; -}; - - -class NamedSfmMeasurement -{ - size_t i; - gtsam::Point2 uv; - NamedSfmMeasurement(size_t i, gtsam::Point2 uv); -}; - -class NamedSfmMeasurementVector { - NamedSfmMeasurementVector(); - NamedSfmMeasurementVector(const gtsam::gtsfm::NamedSfmMeasurementVector& other); - void push_back(const gtsam::gtsfm::NamedSfmMeasurement& measurement); - size_t size() const; - bool empty() const; - void clear(); - gtsam::gtsfm::NamedSfmMeasurement at(const size_t& index) const; -}; - +#include class SfmTrack2d { + std::vector> measurements; + SfmTrack2d(); - SfmTrack2d(std::vector &measurements); + SfmTrack2d(std::vector &measurements); size_t numberMeasurements() const; - void addMeasurement(const gtsam::gtsfm::NamedSfmMeasurement &m); - std::vector measurements(); - gtsam::gtsfm::NamedSfmMeasurement measurement(size_t idx) const; + pair measurement(size_t idx) const; + pair siftIndex(size_t idx) const; + void addMeasurement(size_t idx, const gtsam::Point2& m); + gtsam::SfmMeasurement measurement(size_t idx) const; bool hasUniqueCameras(); }; -class SfmTrack2dVector { - SfmTrack2dVector(); - SfmTrack2dVector(const gtsam::gtsfm::SfmTrack2dVector& other); - void push_back(const gtsam::gtsfm::SfmTrack2d& track); - size_t size() const; - bool empty() const; - void clear(); - gtsam::gtsfm::SfmTrack2d at(const size_t& index) const; -}; - - -class DsfTrackGenerator { - DsfTrackGenerator(); - const gtsam::gtsfm::SfmTrack2dVector generate_tracks_from_pairwise_matches( - const gtsam::gtsfm::MatchIndicesMap& matches_dict, - const gtsam::gtsfm::KeypointsVector& keypoints_list, - bool verbose = false); -}; -}///\namespace gtsfm - -#include -#include -#include -class SfmTrack { +virtual class SfmTrack : gtsam::SfmTrack2d { SfmTrack(); SfmTrack(const gtsam::Point3& pt); const Point3& point3() const; @@ -99,13 +30,6 @@ class SfmTrack { double g; double b; - std::vector> measurements; - - size_t numberMeasurements() const; - pair measurement(size_t idx) const; - pair siftIndex(size_t idx) const; - void addMeasurement(size_t idx, const gtsam::Point2& m); - // enabling serialization functionality void serialize() const; @@ -113,6 +37,8 @@ class SfmTrack { bool equals(const gtsam::SfmTrack& expected, double tol) const; }; +#include +#include #include class SfmData { SfmData(); @@ -196,7 +122,7 @@ class BinaryMeasurementsRot3 { #include -// TODO(frank): copy/pasta below until we have integer template paremeters in +// TODO(frank): copy/pasta below until we have integer template parameters in // wrap! class ShonanAveragingParameters2 { @@ -391,4 +317,45 @@ class TranslationRecovery { const gtsam::BinaryMeasurementsUnit3& relativeTranslations) const; }; +namespace gtsfm { + +#include + +class MatchIndicesMap { + MatchIndicesMap(); + MatchIndicesMap(const gtsam::gtsfm::MatchIndicesMap& other); + + size_t size() const; + bool empty() const; + void clear(); + gtsam::gtsfm::CorrespondenceIndices at(const pair& keypair) const; +}; + + +class Keypoints +{ + Keypoints(const gtsam::gtsfm::KeypointCoordinates& coordinates); + gtsam::gtsfm::KeypointCoordinates coordinates; +}; // check if this should be a method + + +class KeypointsVector { + KeypointsVector(); + KeypointsVector(const gtsam::gtsfm::KeypointsVector& other); + void push_back(const gtsam::gtsfm::Keypoints& keypoints); + size_t size() const; + bool empty() const; + void clear(); + gtsam::gtsfm::Keypoints at(const size_t& index) const; +}; + +class DsfTrackGenerator { + DsfTrackGenerator(); + const gtsam::SfmTrack2dVector generate_tracks_from_pairwise_matches( + const gtsam::gtsfm::MatchIndicesMap& matches_dict, + const gtsam::gtsfm::KeypointsVector& keypoints_list, + bool verbose = false); +}; +} // namespace gtsfm + } // namespace gtsam diff --git a/python/gtsam/preamble/sfm.h b/python/gtsam/preamble/sfm.h index 067dd8801..27a4e5de9 100644 --- a/python/gtsam/preamble/sfm.h +++ b/python/gtsam/preamble/sfm.h @@ -15,16 +15,13 @@ // #include #include -PYBIND11_MAKE_OPAQUE( - std::vector); -PYBIND11_MAKE_OPAQUE( - std::vector); +PYBIND11_MAKE_OPAQUE(std::vector); +PYBIND11_MAKE_OPAQUE(std::vector); +PYBIND11_MAKE_OPAQUE(std::vector); PYBIND11_MAKE_OPAQUE( std::vector>); PYBIND11_MAKE_OPAQUE( std::vector>); PYBIND11_MAKE_OPAQUE( std::vector); -PYBIND11_MAKE_OPAQUE(gtsam::gtsfm::MatchIndicesMap); -PYBIND11_MAKE_OPAQUE( - std::vector); \ No newline at end of file +PYBIND11_MAKE_OPAQUE(gtsam::gtsfm::MatchIndicesMap); \ No newline at end of file diff --git a/python/gtsam/specializations/sfm.h b/python/gtsam/specializations/sfm.h index 10fb1a0ab..c4817f555 100644 --- a/python/gtsam/specializations/sfm.h +++ b/python/gtsam/specializations/sfm.h @@ -18,25 +18,11 @@ py::bind_vector > >( py::bind_vector > >( m_, "BinaryMeasurementsRot3"); py::bind_map(m_, "KeyPairDoubleMap"); -py::bind_vector< - std::vector >( - m_, "SfmTracks"); -py::bind_vector< - std::vector >( - m_, "SfmCameras"); -py::bind_vector< - std::vector>>( - m_, "SfmMeasurementVector" - ); +py::bind_vector>(m_, "SfmTrack2dVector"); +py::bind_vector>(m_, "SfmTracks"); +py::bind_vector>(m_, "SfmCameras"); +py::bind_vector>>( + m_, "SfmMeasurementVector"); -py::bind_vector< - std::vector >( - m_, "SfmTrack2dVector"); -py::bind_vector< - std::vector >( - m_, "NamedSfmMeasurementVector"); py::bind_map(m_, "MatchIndicesMap"); - -py::bind_vector< - std::vector >( - m_, "KeypointsVector"); +py::bind_vector>(m_, "KeypointsVector"); diff --git a/python/gtsam/tests/test_DsfTrackGenerator.py b/python/gtsam/tests/test_DsfTrackGenerator.py index e63025cd3..3452c03e8 100644 --- a/python/gtsam/tests/test_DsfTrackGenerator.py +++ b/python/gtsam/tests/test_DsfTrackGenerator.py @@ -6,15 +6,9 @@ Authors: John Lambert import unittest import numpy as np - -import gtsam -from gtsam import IndexPair, KeypointsVector, MatchIndicesMap, NamedSfmMeasurementVector -from gtsam.gtsfm import ( - DsfTrackGenerator, - Keypoints, - NamedSfmMeasurement, - SfmTrack2d, -) +from gtsam import (IndexPair, KeypointsVector, MatchIndicesMap, Point2, + SfmMeasurementVector, SfmTrack2d) +from gtsam.gtsfm import DsfTrackGenerator, Keypoints from gtsam.utils.test_case import GtsamTestCase @@ -22,48 +16,55 @@ class TestDsfTrackGenerator(GtsamTestCase): """Tests for DsfTrackGenerator.""" def test_track_generation(self) -> None: - """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]])) + """Ensures that DSF generates three tracks from measurements + in 3 images (H=200,W=400).""" + kps_i0 = Keypoints(np.array([[10.0, 20], [30, 40]])) + kps_i1 = Keypoints(np.array([[50.0, 60], [70, 80], [90, 100]])) + kps_i2 = Keypoints(np.array([[110.0, 120], [130, 140]])) keypoints_list = KeypointsVector() keypoints_list.append(kps_i0) keypoints_list.append(kps_i1) keypoints_list.append(kps_i2) - # For each image pair (i1,i2), we provide a (K,2) matrix of corresponding image indices (k1,k2). + # For each image pair (i1,i2), we provide a (K,2) matrix + # of corresponding image indices (k1,k2). matches_dict = MatchIndicesMap() 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(matches_dict, - keypoints_list, - verbose=True) + tracks = DsfTrackGenerator().generate_tracks_from_pairwise_matches( + matches_dict, + keypoints_list, + verbose=False, + ) 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].numberMeasurements() == 2 + track0 = tracks[0] + np.testing.assert_allclose(track0.measurements[0][1], Point2(10, 20)) + np.testing.assert_allclose(track0.measurements[1][1], Point2(50, 60)) + assert track0.measurements[0][0] == 0 + assert track0.measurements[1][0] == 1 + assert track0.numberMeasurements() == 2 # 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].numberMeasurements() == 3 + track1 = tracks[1] + np.testing.assert_allclose(track1.measurements[0][1], Point2(30, 40)) + np.testing.assert_allclose(track1.measurements[1][1], Point2(70, 80)) + np.testing.assert_allclose(track1.measurements[2][1], Point2(130, 140)) + assert track1.measurements[0][0] == 0 + assert track1.measurements[1][0] == 1 + assert track1.measurements[2][0] == 2 + assert track1.numberMeasurements() == 3 # 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].numberMeasurements() == 2 + track2 = tracks[2] + np.testing.assert_allclose(track2.measurements[0][1], Point2(90, 100)) + np.testing.assert_allclose(track2.measurements[1][1], Point2(110, 120)) + assert track2.measurements[0][0] == 1 + assert track2.measurements[1][0] == 2 + assert track2.numberMeasurements() == 2 class TestSfmTrack2d(GtsamTestCase): @@ -71,8 +72,12 @@ class TestSfmTrack2d(GtsamTestCase): def test_sfm_track_2d_constructor(self) -> None: """ """ - measurements = NamedSfmMeasurementVector() - measurements.append(NamedSfmMeasurement(i=0, uv=np.array([10.0, 20.0]))) + measurements = SfmMeasurementVector() + measurements.append((0, Point2(10, 20))) track = SfmTrack2d(measurements=measurements) track.measurement(0) track.numberMeasurements() == 1 + + +if __name__ == "__main__": + unittest.main() From 7e411fee514ea681f623ccb9a1ce740336704b2f Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 22 Oct 2022 19:18:37 -0700 Subject: [PATCH 25/30] Refactor into smaller functions --- gtsam/sfm/DsfTrackGenerator.h | 128 +++++++++++-------- gtsam/sfm/sfm.i | 2 +- python/gtsam/tests/test_DsfTrackGenerator.py | 2 +- 3 files changed, 77 insertions(+), 55 deletions(-) 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, From 466290cbdc5d06d1780fcaf72c52e32b6492a630 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 22 Oct 2022 19:30:15 -0700 Subject: [PATCH 26/30] 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 From ca451141c059387342a7c6ac9ec10b6d59a38732 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 22 Oct 2022 19:48:48 -0700 Subject: [PATCH 27/30] removed NamedSfmMeasurementVector --- python/CMakeLists.txt | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index 398ee2788..79a27f17f 100644 --- a/python/CMakeLists.txt +++ b/python/CMakeLists.txt @@ -54,8 +54,7 @@ set(ignore gtsam::KeyPairDoubleMap gtsam::gtsfm::MatchIndicesMap gtsam::gtsfm::KeypointsVector - gtsam::gtsfm::SfmTrack2dVector - gtsam::gtsfm::NamedSfmMeasurementVector) + gtsam::gtsfm::SfmTrack2dVector) set(interface_headers ${PROJECT_SOURCE_DIR}/gtsam/gtsam.i @@ -155,8 +154,7 @@ if(GTSAM_UNSTABLE_BUILD_PYTHON) gtsam::KeyPairDoubleMap gtsam::gtsfm::MatchIndicesMap gtsam::gtsfm::KeypointsVector - gtsam::gtsfm::SfmTrack2dVector - gtsam::gtsfm::NamedSfmMeasurementVector) + gtsam::gtsfm::SfmTrack2dVector) pybind_wrap(${GTSAM_PYTHON_UNSTABLE_TARGET} # target From f68f0dbe4142721121c0bacdfb5b8f7a082f6d60 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 23 Oct 2022 11:37:37 -0700 Subject: [PATCH 28/30] Addressed review comments - removed class in favor of free function --- gtsam/sfm/DsfTrackGenerator.cpp | 4 +-- gtsam/sfm/DsfTrackGenerator.h | 37 +++++++------------- gtsam/sfm/SfmTrack.h | 2 +- gtsam/sfm/sfm.i | 22 ++++-------- python/gtsam/tests/test_DsfTrackGenerator.py | 5 +-- 5 files changed, 26 insertions(+), 44 deletions(-) diff --git a/gtsam/sfm/DsfTrackGenerator.cpp b/gtsam/sfm/DsfTrackGenerator.cpp index 5052bca74..e82880193 100644 --- a/gtsam/sfm/DsfTrackGenerator.cpp +++ b/gtsam/sfm/DsfTrackGenerator.cpp @@ -98,7 +98,7 @@ static std::vector tracksFromDSF(const DSFMapIndexPair& dsf, * correspondence indices, from each image. * @param Length-N list of keypoints, for N images/cameras. */ -std::vector DsfTrackGenerator::tracksFromPairwiseMatches( +std::vector tracksFromPairwiseMatches( const MatchIndicesMap& matches, const KeypointsVector& keypoints, bool verbose) { // Generate the DSF to form tracks. @@ -121,7 +121,7 @@ std::vector DsfTrackGenerator::tracksFromPairwiseMatches( double erroneous_percentage = static_cast(erroneous_track_count) / static_cast(tracks2d.size()) * 100; - // TODO(johnwlambert): restrict decimal places to 2 decimals. + std::cout << std::fixed << std::setprecision(2); std::cout << "DSF Union-Find: " << erroneous_percentage; std::cout << "% of tracks discarded from multiple obs. in a single image." << std::endl; diff --git a/gtsam/sfm/DsfTrackGenerator.h b/gtsam/sfm/DsfTrackGenerator.h index dbf2e6680..b02073d8b 100644 --- a/gtsam/sfm/DsfTrackGenerator.h +++ b/gtsam/sfm/DsfTrackGenerator.h @@ -31,7 +31,6 @@ namespace gtsfm { typedef Eigen::MatrixX2i CorrespondenceIndices; // N x 2 array -// struct Keypoints; using KeypointCoordinates = Eigen::MatrixX2d; // Output of detections in an image. @@ -61,30 +60,20 @@ using KeypointsVector = std::vector; using MatchIndicesMap = std::map; /** - * @brief Generates point tracks from connected components in the keypoint - * matches graph. + * @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. */ -class DsfTrackGenerator { - public: - /** Default constructor. */ - DsfTrackGenerator() = default; - - /** - * @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); -}; +std::vector tracksFromPairwiseMatches( + const MatchIndicesMap& matches, const KeypointsVector& keypoints, + bool verbose = false); } // namespace gtsfm diff --git a/gtsam/sfm/SfmTrack.h b/gtsam/sfm/SfmTrack.h index 1fe5aedf4..e5196e750 100644 --- a/gtsam/sfm/SfmTrack.h +++ b/gtsam/sfm/SfmTrack.h @@ -77,7 +77,7 @@ struct GTSAM_EXPORT SfmTrack2d { const SiftIndex& siftIndex(size_t idx) const { return siftIndices[idx]; } /** - * @brief Validates the track by checking that no two measurements are from + * @brief Check that no two measurements are from the same camera. * @returns boolean result of the validation. */ bool hasUniqueCameras() const { diff --git a/gtsam/sfm/sfm.i b/gtsam/sfm/sfm.i index da054711e..cbcbb1191 100644 --- a/gtsam/sfm/sfm.i +++ b/gtsam/sfm/sfm.i @@ -5,18 +5,17 @@ namespace gtsam { #include -class SfmTrack2d -{ +class SfmTrack2d { std::vector> measurements; SfmTrack2d(); - SfmTrack2d(std::vector &measurements); + SfmTrack2d(const std::vector& measurements); size_t numberMeasurements() const; pair measurement(size_t idx) const; pair siftIndex(size_t idx) const; void addMeasurement(size_t idx, const gtsam::Point2& m); gtsam::SfmMeasurement measurement(size_t idx) const; - bool hasUniqueCameras(); + bool hasUniqueCameras() const; }; virtual class SfmTrack : gtsam::SfmTrack2d { @@ -331,13 +330,10 @@ class MatchIndicesMap { gtsam::gtsfm::CorrespondenceIndices at(const pair& keypair) const; }; - -class Keypoints -{ +class Keypoints { Keypoints(const gtsam::gtsfm::KeypointCoordinates& coordinates); gtsam::gtsfm::KeypointCoordinates coordinates; -}; // check if this should be a method - +}; class KeypointsVector { KeypointsVector(); @@ -349,13 +345,9 @@ class KeypointsVector { gtsam::gtsfm::Keypoints at(const size_t& index) const; }; -class DsfTrackGenerator { - DsfTrackGenerator(); - const gtsam::SfmTrack2dVector tracksFromPairwiseMatches( +gtsam::SfmTrack2dVector tracksFromPairwiseMatches( const gtsam::gtsfm::MatchIndicesMap& matches_dict, - const gtsam::gtsfm::KeypointsVector& keypoints_list, - bool verbose = false); -}; + const gtsam::gtsfm::KeypointsVector& keypoints_list, bool verbose = false); } // namespace gtsfm } // namespace gtsam diff --git a/python/gtsam/tests/test_DsfTrackGenerator.py b/python/gtsam/tests/test_DsfTrackGenerator.py index df6e60c9f..690fa22e2 100644 --- a/python/gtsam/tests/test_DsfTrackGenerator.py +++ b/python/gtsam/tests/test_DsfTrackGenerator.py @@ -5,10 +5,11 @@ Authors: John Lambert import unittest +import gtsam import numpy as np from gtsam import (IndexPair, KeypointsVector, MatchIndicesMap, Point2, SfmMeasurementVector, SfmTrack2d) -from gtsam.gtsfm import DsfTrackGenerator, Keypoints +from gtsam.gtsfm import Keypoints from gtsam.utils.test_case import GtsamTestCase @@ -33,7 +34,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().tracksFromPairwiseMatches( + tracks = gtsam.gtsfm.tracksFromPairwiseMatches( matches_dict, keypoints_list, verbose=False, From 90c990dc3323badb1d7d3cde73c7da3792a5c0da Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 23 Oct 2022 16:26:41 -0700 Subject: [PATCH 29/30] Rename of test --- .../gtsam/tests/{test_dsf_map.py => test_DSFMap.py} | 13 ++++++------- 1 file changed, 6 insertions(+), 7 deletions(-) rename python/gtsam/tests/{test_dsf_map.py => test_DSFMap.py} (88%) diff --git a/python/gtsam/tests/test_dsf_map.py b/python/gtsam/tests/test_DSFMap.py similarity index 88% rename from python/gtsam/tests/test_dsf_map.py rename to python/gtsam/tests/test_DSFMap.py index 6cae98ff5..f973f7c99 100644 --- a/python/gtsam/tests/test_dsf_map.py +++ b/python/gtsam/tests/test_DSFMap.py @@ -15,8 +15,7 @@ from __future__ import print_function import unittest from typing import Tuple -import gtsam -from gtsam import IndexPair +from gtsam import DSFMapIndexPair, IndexPair, IndexPairSetAsArray from gtsam.utils.test_case import GtsamTestCase @@ -29,10 +28,10 @@ class TestDSFMap(GtsamTestCase): def key(index_pair) -> Tuple[int, int]: return index_pair.i(), index_pair.j() - dsf = gtsam.DSFMapIndexPair() - pair1 = gtsam.IndexPair(1, 18) + dsf = DSFMapIndexPair() + pair1 = IndexPair(1, 18) self.assertEqual(key(dsf.find(pair1)), key(pair1)) - pair2 = gtsam.IndexPair(2, 2) + pair2 = IndexPair(2, 2) # testing the merge feature of dsf dsf.merge(pair1, pair2) @@ -45,7 +44,7 @@ class TestDSFMap(GtsamTestCase): k'th detected keypoint in image i. For the data below, merging such measurements into feature tracks across frames should create 2 distinct sets. """ - dsf = gtsam.DSFMapIndexPair() + dsf = DSFMapIndexPair() dsf.merge(IndexPair(0, 1), IndexPair(1, 2)) dsf.merge(IndexPair(0, 1), IndexPair(3, 4)) dsf.merge(IndexPair(4, 5), IndexPair(6, 8)) @@ -56,7 +55,7 @@ class TestDSFMap(GtsamTestCase): for i in sets: set_keys = [] s = sets[i] - for val in gtsam.IndexPairSetAsArray(s): + for val in IndexPairSetAsArray(s): set_keys.append((val.i(), val.j())) merged_sets.add(tuple(set_keys)) From 32df824920145001ada8495677af43f43fccd6a7 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 23 Oct 2022 16:27:01 -0700 Subject: [PATCH 30/30] Vectorized interface --- gtsam/sfm/DsfTrackGenerator.h | 6 +-- gtsam/sfm/SfmTrack.h | 25 ++++++++++++ gtsam/sfm/sfm.i | 6 ++- python/gtsam/tests/test_DsfTrackGenerator.py | 42 +++++++++++++------- 4 files changed, 58 insertions(+), 21 deletions(-) diff --git a/gtsam/sfm/DsfTrackGenerator.h b/gtsam/sfm/DsfTrackGenerator.h index b02073d8b..14ec2302d 100644 --- a/gtsam/sfm/DsfTrackGenerator.h +++ b/gtsam/sfm/DsfTrackGenerator.h @@ -31,8 +31,6 @@ namespace gtsfm { typedef Eigen::MatrixX2i CorrespondenceIndices; // N x 2 array -using KeypointCoordinates = Eigen::MatrixX2d; - // Output of detections in an image. // Coordinate system convention: // 1. The x coordinate denotes the horizontal direction (+ve direction towards @@ -41,7 +39,7 @@ using KeypointCoordinates = Eigen::MatrixX2d; // 3. Origin is at the top left corner of the image. struct Keypoints { // The (x, y) coordinates of the features, of shape Nx2. - KeypointCoordinates coordinates; + Eigen::MatrixX2d coordinates; // Optional scale of the detections, of shape N. // Note: gtsam::Vector is typedef'd for Eigen::VectorXd. @@ -50,7 +48,7 @@ struct Keypoints { /// Optional confidences/responses for each detection, of shape N. boost::optional responses; - Keypoints(const KeypointCoordinates& coordinates) + Keypoints(const Eigen::MatrixX2d& coordinates) : coordinates(coordinates){}; // boost::none }; diff --git a/gtsam/sfm/SfmTrack.h b/gtsam/sfm/SfmTrack.h index e5196e750..c75199374 100644 --- a/gtsam/sfm/SfmTrack.h +++ b/gtsam/sfm/SfmTrack.h @@ -22,6 +22,7 @@ #include #include +#include #include #include #include @@ -90,6 +91,30 @@ struct GTSAM_EXPORT SfmTrack2d { bool all_cameras_unique = (i == track_cam_indices.end()); return all_cameras_unique; } + + /// @} + /// @name Vectorized Interface + /// @{ + + /// @brief Return the measurements as a 2D matrix + Eigen::MatrixX2d measurementMatrix() const { + Eigen::MatrixX2d m(numberMeasurements(), 2); + for (size_t i = 0; i < numberMeasurements(); i++) { + m.row(i) = measurement(i).second; + } + return m; + } + + /// @brief Return the camera indices of the measurements + Eigen::VectorXi indexVector() const { + Eigen::VectorXi v(numberMeasurements()); + for (size_t i = 0; i < numberMeasurements(); i++) { + v(i) = measurement(i).first; + } + return v; + } + + /// @} }; using SfmTrack2dVector = std::vector; diff --git a/gtsam/sfm/sfm.i b/gtsam/sfm/sfm.i index cbcbb1191..26dc20c3e 100644 --- a/gtsam/sfm/sfm.i +++ b/gtsam/sfm/sfm.i @@ -16,6 +16,8 @@ class SfmTrack2d { void addMeasurement(size_t idx, const gtsam::Point2& m); gtsam::SfmMeasurement measurement(size_t idx) const; bool hasUniqueCameras() const; + Eigen::MatrixX2d measurementMatrix() const; + Eigen::VectorXi indexVector() const; }; virtual class SfmTrack : gtsam::SfmTrack2d { @@ -331,8 +333,8 @@ class MatchIndicesMap { }; class Keypoints { - Keypoints(const gtsam::gtsfm::KeypointCoordinates& coordinates); - gtsam::gtsfm::KeypointCoordinates coordinates; + Keypoints(const Eigen::MatrixX2d& coordinates); + Eigen::MatrixX2d coordinates; }; class KeypointsVector { diff --git a/python/gtsam/tests/test_DsfTrackGenerator.py b/python/gtsam/tests/test_DsfTrackGenerator.py index 690fa22e2..e600227c9 100644 --- a/python/gtsam/tests/test_DsfTrackGenerator.py +++ b/python/gtsam/tests/test_DsfTrackGenerator.py @@ -7,8 +7,7 @@ import unittest import gtsam import numpy as np -from gtsam import (IndexPair, KeypointsVector, MatchIndicesMap, Point2, - SfmMeasurementVector, SfmTrack2d) +from gtsam import IndexPair, KeypointsVector, MatchIndicesMap, Point2, SfmMeasurementVector, SfmTrack2d from gtsam.gtsfm import Keypoints from gtsam.utils.test_case import GtsamTestCase @@ -43,29 +42,42 @@ class TestDsfTrackGenerator(GtsamTestCase): # Verify track 0. track0 = tracks[0] + assert track0.numberMeasurements() == 2 np.testing.assert_allclose(track0.measurements[0][1], Point2(10, 20)) np.testing.assert_allclose(track0.measurements[1][1], Point2(50, 60)) assert track0.measurements[0][0] == 0 assert track0.measurements[1][0] == 1 - assert track0.numberMeasurements() == 2 + np.testing.assert_allclose( + track0.measurementMatrix(), + [ + [10, 20], + [50, 60], + ], + ) + np.testing.assert_allclose(track0.indexVector(), [0, 1]) # Verify track 1. track1 = tracks[1] - np.testing.assert_allclose(track1.measurements[0][1], Point2(30, 40)) - np.testing.assert_allclose(track1.measurements[1][1], Point2(70, 80)) - np.testing.assert_allclose(track1.measurements[2][1], Point2(130, 140)) - assert track1.measurements[0][0] == 0 - assert track1.measurements[1][0] == 1 - assert track1.measurements[2][0] == 2 - assert track1.numberMeasurements() == 3 + np.testing.assert_allclose( + track1.measurementMatrix(), + [ + [30, 40], + [70, 80], + [130, 140], + ], + ) + np.testing.assert_allclose(track1.indexVector(), [0, 1, 2]) # Verify track 2. track2 = tracks[2] - np.testing.assert_allclose(track2.measurements[0][1], Point2(90, 100)) - np.testing.assert_allclose(track2.measurements[1][1], Point2(110, 120)) - assert track2.measurements[0][0] == 1 - assert track2.measurements[1][0] == 2 - assert track2.numberMeasurements() == 2 + np.testing.assert_allclose( + track2.measurementMatrix(), + [ + [90, 100], + [110, 120], + ], + ) + np.testing.assert_allclose(track2.indexVector(), [1, 2]) class TestSfmTrack2d(GtsamTestCase):