From cafa3c556c23db08f6eb5242480b18be1cba5253 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Sat, 22 Oct 2022 18:37:20 -0700
Subject: [PATCH] Split SfmTrack into 2 classes

---
 gtsam/sfm/SfmTrack.h             | 62 +++++++++++++++++++++++++-------
 gtsam/sfm/tests/testSfmTrack.cpp | 53 +++++++++++++++++++++++++++
 2 files changed, 103 insertions(+), 12 deletions(-)
 create mode 100644 gtsam/sfm/tests/testSfmTrack.cpp

diff --git a/gtsam/sfm/SfmTrack.h b/gtsam/sfm/SfmTrack.h
index 37731b32a..471eff994 100644
--- a/gtsam/sfm/SfmTrack.h
+++ b/gtsam/sfm/SfmTrack.h
@@ -35,28 +35,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
- * @addtogroup sfm
+ * @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.
  */
-struct GTSAM_EXPORT SfmTrack {
-  Point3 p;       ///< 3D position of the point
-  float r, g, b;  ///< RGB color of the 3D point
-
+struct GTSAM_EXPORT SfmTrack2d {
   /// 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)
-      : p(0, 0, 0), r(r), g(g), b(b) {}
+  // Default constructor.
+  SfmTrack2d() = default;
 
-  explicit SfmTrack(const gtsam::Point3& pt, float r = 0, float g = 0,
-                    float b = 0)
-      : p(pt), r(r), g(g), b(b) {}
+  // Constructor from measurements.
+  explicit SfmTrack2d(const std::vector<SfmMeasurement>& measurements)
+      : measurements(measurements) {}
 
   /// @}
   /// @name Standard Interface
@@ -78,6 +76,46 @@ 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 Validates the track by checking that no two measurements are from
+   * @returns boolean result of the validation.
+   */
+  bool hasUniqueCameras() {
+    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;
+  }
+};
+
+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; }
 
diff --git a/gtsam/sfm/tests/testSfmTrack.cpp b/gtsam/sfm/tests/testSfmTrack.cpp
new file mode 100644
index 000000000..1b8c6bd9a
--- /dev/null
+++ b/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);
+}
+/* ************************************************************************* */