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):