From df4419b6093bfcc9beb8c9073dc7d8fbef691ce1 Mon Sep 17 00:00:00 2001
From: Ayush Baid <ayushrakeshbaid@gmail.com>
Date: Sat, 2 Jan 2021 17:28:22 +0530
Subject: [PATCH] adding track serialization and testable trait

---
 gtsam/slam/dataset.h                          | 55 +++++++++++++++++--
 gtsam/slam/tests/testSerializationDataset.cpp | 18 +++++-
 2 files changed, 64 insertions(+), 9 deletions(-)

diff --git a/gtsam/slam/dataset.h b/gtsam/slam/dataset.h
index a6b81eab1..89cd1f71b 100644
--- a/gtsam/slam/dataset.h
+++ b/gtsam/slam/dataset.h
@@ -29,8 +29,9 @@
 #include <gtsam/nonlinear/NonlinearFactorGraph.h>
 #include <gtsam/nonlinear/Values.h>
 #include <gtsam/linear/NoiseModel.h>
-#include <gtsam/base/types.h>
 #include <gtsam/base/Testable.h>
+#include <gtsam/base/types.h>
+
 
 #include <boost/serialization/vector.hpp>
 #include <boost/smart_ptr/shared_ptr.hpp>
@@ -224,7 +225,7 @@ struct SfmTrack {
   float r, g, b; ///< RGB color of the 3D point
   std::vector<SfmMeasurement> measurements; ///< The 2D image projections (id,(u,v))
   std::vector<SiftIndex> siftIndices;
-  
+
   /// Total number of measurements in this track
   size_t number_measurements() const {
     return measurements.size();
@@ -245,7 +246,7 @@ struct SfmTrack {
   void add_measurement(size_t idx, const gtsam::Point2& m) {
     measurements.emplace_back(idx, m);
   }
-  
+
   template<class ARCHIVE>
   void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
     ar & p;
@@ -258,14 +259,47 @@ struct SfmTrack {
 
   /// assert equality up to a tolerance
   bool equals(const SfmTrack &sfmTrack, double tol = 1e-9) const {
+    if(!p.isApprox(sfmTrack.p)){
+      return false;
+    }
+
+    // TODO: compare RGB values
+
+    // compare size of vectors
+    if(number_measurements() != sfmTrack.number_measurements() ||
+      siftIndices.size() != sfmTrack.siftIndices.size()){
+      return false;
+    }
+
+    // compare measurements (order sensitive)
+    for(size_t idx=0; idx<number_measurements(); idx++){
+      SfmMeasurement measurement = measurements[idx];
+      SfmMeasurement otherMeasurement = sfmTrack.measurements[idx];
+
+      if(measurement.first != otherMeasurement.first || !measurement.second.isApprox(otherMeasurement.second)){
+        return false;
+      }
+    }
+
     return true;
   }
 
+  /// print
+  void print(const std::string& s = "") const {
+    cout << "Track with " << measurements.size() << "measurements\n";
+  }
+
   // inline bool SfmTrack::operator == (const SfmTrack& rhs) const{
   //   return p==rhs.p;
   // }
 };
 
+/* ************************************************************************* */
+/// traits
+template<>
+struct traits<SfmTrack> : public Testable<SfmTrack> {
+};
+
 
 /// Define the structure for the camera poses
 typedef PinholeCamera<Cal3Bundler> SfmCamera;
@@ -321,15 +355,18 @@ class GTSAM_EXPORT SfmData {
     }
 
     /// print
-    void print(const std::string& s = "") const;
+    void print(const std::string& s = "") const {
+      cout << "Number of cameras = " << number_cameras() << "\n";
+      cout << "Number of tracks = " << number_tracks() << "\n";
+    }
 
   private:
     /** Serialization function */
     friend class boost::serialization::access;
     template<class Archive>
     void serialize(Archive & ar, const unsigned int /*version*/) {
-      // ar & cameras;
-      // ar & tracks;
+      ar & cameras;
+      ar & tracks;
     }
 
   // inline bool SfmData::operator == (const SfmData& rhs) const{
@@ -337,6 +374,12 @@ class GTSAM_EXPORT SfmData {
   // }
 };
 
+/* ************************************************************************* */
+/// traits
+template<>
+struct traits<SfmData> : public Testable<SfmData> {
+};
+
 /**
  * @brief This function parses a bundler output file and stores the data into a
  * SfmData structure
diff --git a/gtsam/slam/tests/testSerializationDataset.cpp b/gtsam/slam/tests/testSerializationDataset.cpp
index 38749cb48..c6fed7b4f 100644
--- a/gtsam/slam/tests/testSerializationDataset.cpp
+++ b/gtsam/slam/tests/testSerializationDataset.cpp
@@ -18,8 +18,6 @@
 
 #include <gtsam/slam/dataset.h>
 
-#include <CppUnitLite/TestHarness.h>
-
 #include <gtsam/base/serializationTestHelpers.h>
 #include <CppUnitLite/TestHarness.h>
 
@@ -39,6 +37,20 @@ TEST(dataSet, sfmDataSerialization){
   // EXPECT(equalsBinary(mydata));
 }
 
+/* ************************************************************************* */
+TEST(dataSet, sfmTrackSerialization){
+  // Test the serialization of SfmData
+  const string filename = findExampleDataFile("dubrovnik-3-7-pre");
+  SfmData mydata;
+  CHECK(readBAL(filename, mydata));
+
+  SfmTrack track = mydata.track(0);
+
+  EXPECT(equalsObj(track));
+  // EXPECT(equalsXML(mydata));
+  // EXPECT(equalsBinary(mydata));
+}
+
 /* ************************************************************************* */
 int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
-/* ************************************************************************* */
\ No newline at end of file
+/* ************************************************************************* */