Merge pull request #1312 from borglab/dsf-gtsfm-refactor

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 <gtsam/sfm/DsfTrackGenerator.h>
+
+#include <algorithm>
+#include <iostream>
+
+namespace gtsam {
+
+namespace gtsfm {
+
+typedef DSFMap<IndexPair> 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<IndexPair>& 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<SfmTrack2d> tracksFromDSF(const DSFMapIndexPair& dsf,
+                                             const KeypointsVector& keypoints) {
+  const std::map<IndexPair, std::set<IndexPair> > 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<SfmTrack2d> 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<SfmTrack2d> 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<SfmTrack2d> 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<SfmTrack2d> 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<float>(erroneous_track_count) /
+                                  static_cast<float>(tracks2d.size()) * 100;
+
+    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;
+  }
+
+  // TODO(johnwlambert): return the Transitivity failure percentage here.
+  return tracks2d;
+}
+
+}  // namespace gtsfm
+
+}  // namespace gtsam

gtsam/sfm/DsfTrackGenerator.h

@@ -0,0 +1,78 @@
+/* ----------------------------------------------------------------------------
+
+ * 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 July 2022
+ * @author John Lambert
+ * @brief Identifies connected components in the keypoint matches graph.
+ */
+
+#pragma once
+#include <gtsam/base/DSFMap.h>
+#include <gtsam/sfm/SfmTrack.h>
+
+#include <Eigen/Core>
+#include <map>
+#include <optional>
+#include <vector>
+
+namespace gtsam {
+
+namespace gtsfm {
+
+typedef Eigen::MatrixX2i CorrespondenceIndices;  // N x 2 array
+
+// 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.
+  Eigen::MatrixX2d coordinates;
+
+  // Optional scale of the detections, of shape N.
+  // Note: gtsam::Vector is typedef'd for Eigen::VectorXd.
+  boost::optional<gtsam::Vector> scales;
+
+  /// Optional confidences/responses for each detection, of shape N.
+  boost::optional<gtsam::Vector> responses;
+
+  Keypoints(const Eigen::MatrixX2d& coordinates)
+      : coordinates(coordinates){};  // boost::none
+};
+
+using KeypointsVector = std::vector<Keypoints>;
+// Mapping from each image pair to (N,2) array representing indices of matching
+// keypoints.
+using MatchIndicesMap = std::map<IndexPair, CorrespondenceIndices>;
+
+/**
+ * @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<SfmTrack2d> tracksFromPairwiseMatches(
+    const MatchIndicesMap& matches, const KeypointsVector& keypoints,
+    bool verbose = false);
+
+}  // namespace gtsfm
+
+}  // namespace gtsam

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>
@@ -35,28 +36,26 @@ typedef std::pair<size_t, Point2> SfmMeasurement;
 typedef std::pair<size_t, size_t> SiftIndex;
 
 /**
- * @brief An SfmTrack stores SfM measurements grouped in a track
+ * @brief Track containing 2D measurements associated with a single 3D point.
- * @addtogroup sfm
+ * Note: Equivalent to gtsam.SfmTrack, but without the 3d measurement.
+ * This class holds data temporarily before 3D point is initialized.
  */
-struct GTSAM_EXPORT SfmTrack {
+struct GTSAM_EXPORT SfmTrack2d {
-  Point3 p;       ///< 3D position of the point
-  float r, g, b;  ///< RGB color of the 3D point
-
   /// The 2D image projections (id,(u,v))
   std::vector<SfmMeasurement> measurements;
 
-  /// The feature descriptors
+  /// The feature descriptors (optional)
   std::vector<SiftIndex> siftIndices;
 
   /// @name Constructors
   /// @{
 
-  explicit SfmTrack(float r = 0, float g = 0, float b = 0)
+  // Default constructor.
-      : p(0, 0, 0), r(r), g(g), b(b) {}
+  SfmTrack2d() = default;
 
-  explicit SfmTrack(const gtsam::Point3& pt, float r = 0, float g = 0,
+  // Constructor from measurements.
-                    float b = 0)
+  explicit SfmTrack2d(const std::vector<SfmMeasurement>& measurements)
-      : p(pt), r(r), g(g), b(b) {}
+      : measurements(measurements) {}
 
   /// @}
   /// @name Standard Interface
@@ -78,6 +77,70 @@ 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 Check that no two measurements are from the same camera.
+   * @returns boolean result of the validation.
+   */
+  bool hasUniqueCameras() const {
+    std::vector<int> 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;
+  }
+
+  /// @}
+  /// @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>;
+
+/**
+ * @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; }
 

gtsam/sfm/sfm.i

@@ -4,10 +4,23 @@
 
 namespace gtsam {
 
-#include <gtsam/nonlinear/NonlinearFactorGraph.h>
-#include <gtsam/nonlinear/Values.h>
 #include <gtsam/sfm/SfmTrack.h>
-class SfmTrack {
+class SfmTrack2d {
+  std::vector<pair<size_t, gtsam::Point2>> measurements;
+
+  SfmTrack2d();
+  SfmTrack2d(const std::vector<gtsam::SfmMeasurement>& measurements);
+  size_t numberMeasurements() const;
+  pair<size_t, gtsam::Point2> measurement(size_t idx) const;
+  pair<size_t, size_t> siftIndex(size_t idx) const;
+  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 {
   SfmTrack();
   SfmTrack(const gtsam::Point3& pt);
   const Point3& point3() const;
@@ -18,13 +31,6 @@ class SfmTrack {
   double g;
   double b;
 
-  std::vector<pair<size_t, gtsam::Point2>> measurements;
-
-  size_t numberMeasurements() const;
-  pair<size_t, gtsam::Point2> measurement(size_t idx) const;
-  pair<size_t, size_t> siftIndex(size_t idx) const;
-  void addMeasurement(size_t idx, const gtsam::Point2& m);
-
   // enabling serialization functionality
   void serialize() const;
 
@@ -32,6 +38,8 @@ class SfmTrack {
   bool equals(const gtsam::SfmTrack& expected, double tol) const;
 };
 
+#include <gtsam/nonlinear/Values.h>
+#include <gtsam/nonlinear/NonlinearFactorGraph.h>
 #include <gtsam/sfm/SfmData.h>
 class SfmData {
   SfmData();
@@ -115,7 +123,7 @@ class BinaryMeasurementsRot3 {
 
 #include <gtsam/sfm/ShonanAveraging.h>
 
-// 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 {
@@ -310,4 +318,38 @@ class TranslationRecovery {
       const gtsam::BinaryMeasurementsUnit3& relativeTranslations) const;
 };
 
+namespace gtsfm {
+
+#include <gtsam/sfm/DsfTrackGenerator.h>
+
+class MatchIndicesMap {
+  MatchIndicesMap();
+  MatchIndicesMap(const gtsam::gtsfm::MatchIndicesMap& other);
+
+  size_t size() const;
+  bool empty() const;
+  void clear();
+  gtsam::gtsfm::CorrespondenceIndices at(const pair<size_t, size_t>& keypair) const;
+};
+
+class Keypoints {
+  Keypoints(const Eigen::MatrixX2d& coordinates);
+  Eigen::MatrixX2d coordinates;
+};
+
+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;
+};
+
+gtsam::SfmTrack2dVector tracksFromPairwiseMatches(
+    const gtsam::gtsfm::MatchIndicesMap& matches_dict,
+    const gtsam::gtsfm::KeypointsVector& keypoints_list, bool verbose = false);
+}  // namespace gtsfm
+
 }  // namespace gtsam

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 <CppUnitLite/TestHarness.h>
+#include <gtsam/inference/Symbol.h>
+#include <gtsam/nonlinear/NonlinearFactorGraph.h>
+#include <gtsam/sfm/SfmTrack.h>
+
+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);
+}
+/* ************************************************************************* */

python/CMakeLists.txt

@@ -51,7 +51,10 @@ set(ignore
     gtsam::BinaryMeasurementsUnit3
     gtsam::BinaryMeasurementsRot3
     gtsam::DiscreteKey
-    gtsam::KeyPairDoubleMap)
+    gtsam::KeyPairDoubleMap
+    gtsam::gtsfm::MatchIndicesMap
+    gtsam::gtsfm::KeypointsVector
+    gtsam::gtsfm::SfmTrack2dVector)
 
 set(interface_headers
     ${PROJECT_SOURCE_DIR}/gtsam/gtsam.i
@@ -148,7 +151,11 @@ if(GTSAM_UNSTABLE_BUILD_PYTHON)
             gtsam::CameraSetCal3Bundler
             gtsam::CameraSetCal3Unified
             gtsam::CameraSetCal3Fisheye
-            gtsam::KeyPairDoubleMap)
+            gtsam::KeyPairDoubleMap
+            gtsam::gtsfm::MatchIndicesMap
+            gtsam::gtsfm::KeypointsVector
+            gtsam::gtsfm::SfmTrack2dVector)
+
 
     pybind_wrap(${GTSAM_PYTHON_UNSTABLE_TARGET} # target
             ${PROJECT_SOURCE_DIR}/gtsam_unstable/gtsam_unstable.i # interface_header

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 *

python/gtsam/preamble/sfm.h

@@ -15,12 +15,13 @@
 // #include <pybind11/stl.h>
 #include <pybind11/stl_bind.h>
 
-PYBIND11_MAKE_OPAQUE(
+PYBIND11_MAKE_OPAQUE(std::vector<gtsam::SfmMeasurement>);
-    std::vector<gtsam::SfmTrack>);
+PYBIND11_MAKE_OPAQUE(std::vector<gtsam::SfmTrack>);
-
+PYBIND11_MAKE_OPAQUE(std::vector<gtsam::SfmCamera>);
-PYBIND11_MAKE_OPAQUE(
-    std::vector<gtsam::SfmCamera>);
 PYBIND11_MAKE_OPAQUE(
     std::vector<gtsam::BinaryMeasurement<gtsam::Unit3>>);
 PYBIND11_MAKE_OPAQUE(
     std::vector<gtsam::BinaryMeasurement<gtsam::Rot3>>);
+PYBIND11_MAKE_OPAQUE(
+    std::vector<gtsam::gtsfm::Keypoints>);
+PYBIND11_MAKE_OPAQUE(gtsam::gtsfm::MatchIndicesMap);

python/gtsam/specializations/sfm.h

@@ -18,16 +18,11 @@ py::bind_vector<std::vector<gtsam::BinaryMeasurement<gtsam::Unit3> > >(
 py::bind_vector<std::vector<gtsam::BinaryMeasurement<gtsam::Rot3> > >(
     m_, "BinaryMeasurementsRot3");
 py::bind_map<gtsam::KeyPairDoubleMap>(m_, "KeyPairDoubleMap");
+py::bind_vector<std::vector<gtsam::SfmTrack2d>>(m_, "SfmTrack2dVector");
+py::bind_vector<std::vector<gtsam::SfmTrack>>(m_, "SfmTracks");
+py::bind_vector<std::vector<gtsam::SfmCamera>>(m_, "SfmCameras");
+py::bind_vector<std::vector<std::pair<size_t, gtsam::Point2>>>(
+    m_, "SfmMeasurementVector");
 
-py::bind_vector<
+py::bind_map<gtsam::gtsfm::MatchIndicesMap>(m_, "MatchIndicesMap");
-    std::vector<gtsam::SfmTrack> >(
+py::bind_vector<std::vector<gtsam::gtsfm::Keypoints>>(m_, "KeypointsVector");
-    m_, "SfmTracks");
-
-py::bind_vector<
-    std::vector<gtsam::SfmCamera> >(
-    m_, "SfmCameras");
-
-py::bind_vector<
-    std::vector<std::pair<size_t, gtsam::Point2>>>(
-        m_, "SfmMeasurementVector"
-    );

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 DSFMapIndexPair, IndexPair, IndexPairSetAsArray
-from gtsam import IndexPair
 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()
+        dsf = DSFMapIndexPair()
-        pair1 = gtsam.IndexPair(1, 18)
+        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))
 

python/gtsam/tests/test_DsfTrackGenerator.py

@@ -0,0 +1,96 @@
+"""Unit tests for track generation using a Disjoint Set Forest data structure.
+
+Authors: John Lambert
+"""
+
+import unittest
+
+import gtsam
+import numpy as np
+from gtsam import IndexPair, KeypointsVector, MatchIndicesMap, Point2, SfmMeasurementVector, SfmTrack2d
+from gtsam.gtsfm import Keypoints
+from gtsam.utils.test_case import GtsamTestCase
+
+
+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(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).
+        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 = gtsam.gtsfm.tracksFromPairwiseMatches(
+            matches_dict,
+            keypoints_list,
+            verbose=False,
+        )
+        assert len(tracks) == 3
+
+        # 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
+        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.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.measurementMatrix(),
+            [
+                [90, 100],
+                [110, 120],
+            ],
+        )
+        np.testing.assert_allclose(track2.indexVector(), [1, 2])
+
+
+class TestSfmTrack2d(GtsamTestCase):
+    """Tests for SfmTrack2d."""
+
+    def test_sfm_track_2d_constructor(self) -> None:
+        """ """
+        measurements = SfmMeasurementVector()
+        measurements.append((0, Point2(10, 20)))
+        track = SfmTrack2d(measurements=measurements)
+        track.measurement(0)
+        track.numberMeasurements() == 1
+
+
+if __name__ == "__main__":
+    unittest.main()