From 38be12eaf46885d2ae257215582ba616472b5b7c Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 22 Oct 2022 18:37:44 -0700 Subject: [PATCH] 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()