Vectorized interface

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<gtsam::Vector> responses;
 
-  Keypoints(const KeypointCoordinates& coordinates)
+  Keypoints(const Eigen::MatrixX2d& coordinates)
       : coordinates(coordinates){};  // boost::none
 };
 

gtsam/sfm/SfmTrack.h

@@ -22,6 +22,7 @@
 #include <gtsam/geometry/Point2.h>
 #include <gtsam/geometry/Point3.h>
 
+#include <Eigen/Core>
 #include <string>
 #include <utility>
 #include <vector>
@@ -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<SfmTrack2d>;

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 {

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