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