Adding serialization support to be used for GT-SFM (#650)

* adding serialization and other functions to enable testing

* adding track serialization and testable trait

* improving formatting

* fixing variable names and comments

* adding serialization functions to wrapper

* fixing xml serialization issues

* reverting SfmTrack to struct

* printing out the 3d point

* adding equals function to wrapper

* adding inline comment for round trip

gtsam/gtsam.i

@@ -2768,6 +2768,12 @@ class SfmTrack {
   pair<size_t, gtsam::Point2> measurement(size_t idx) const;
   pair<size_t, size_t> siftIndex(size_t idx) const;
   void add_measurement(size_t idx, const gtsam::Point2& m);
+
+  // enabling serialization functionality
+  void serialize() const;
+
+  // enabling function to compare objects
+  bool equals(const gtsam::SfmTrack& expected, double tol) const;
 };
 
 class SfmData {
@@ -2778,6 +2784,12 @@ class SfmData {
   gtsam::SfmTrack track(size_t idx) const;
   void add_track(const gtsam::SfmTrack& t) ;
   void add_camera(const gtsam::SfmCamera& cam);
+
+  // enabling serialization functionality
+  void serialize() const;
+
+  // enabling function to compare objects
+  bool equals(const gtsam::SfmData& expected, double tol) const;
 };
 
 gtsam::SfmData readBal(string filename);

gtsam/slam/dataset.h

@@ -29,6 +29,8 @@
 #include <gtsam/nonlinear/NonlinearFactorGraph.h>
 #include <gtsam/nonlinear/Values.h>
 #include <gtsam/linear/NoiseModel.h>
+#include <gtsam/base/serialization.h>
+#include <gtsam/base/Testable.h>
 #include <gtsam/base/types.h>
 
 #include <boost/smart_ptr/shared_ptr.hpp>
@@ -222,7 +224,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();
@@ -243,6 +245,73 @@ struct SfmTrack {
   void add_measurement(size_t idx, const gtsam::Point2& m) {
     measurements.emplace_back(idx, m);
   }
+
+  /** Serialization function */
+  friend class boost::serialization::access;
+  template<class ARCHIVE>
+  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
+    ar & BOOST_SERIALIZATION_NVP(p);
+    ar & BOOST_SERIALIZATION_NVP(r);
+    ar & BOOST_SERIALIZATION_NVP(g);
+    ar & BOOST_SERIALIZATION_NVP(b);
+    ar & BOOST_SERIALIZATION_NVP(measurements);
+    ar & BOOST_SERIALIZATION_NVP(siftIndices);
+  }
+
+  /// assert equality up to a tolerance
+  bool equals(const SfmTrack &sfmTrack, double tol = 1e-9) const {
+    // check the 3D point
+    if (!p.isApprox(sfmTrack.p)) {
+      return false;
+    }
+
+    // check the RGB values
+    if (r!=sfmTrack.r || g!=sfmTrack.g || b!=sfmTrack.b) {
+      return false;
+    }
+
+    // compare size of vectors for measurements and siftIndices
+    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;
+      }
+    }
+
+    // compare sift indices (order sensitive)
+    for (size_t idx = 0; idx < siftIndices.size(); ++idx) {
+      SiftIndex index = siftIndices[idx];
+      SiftIndex otherIndex = sfmTrack.siftIndices[idx];
+
+      if (index.first != otherIndex.first ||
+          index.second != otherIndex.second) {
+        return false;
+      }
+    }
+
+    return true;
+  }
+
+  /// print
+  void print(const std::string& s = "") const {
+    cout << "Track with " << measurements.size();
+    cout << " measurements of point " << p << "\n";
+  }
+};
+
+/* ************************************************************************* */
+/// traits
+template<>
+struct traits<SfmTrack> : public Testable<SfmTrack> {
 };
 
 
@@ -269,13 +338,62 @@ struct SfmData {
     return tracks[idx];
   }
   /// Add a track to SfmData
-  void add_track(const SfmTrack& t)  {
+  void add_track(const SfmTrack& t) {
     tracks.push_back(t);
   }
   /// Add a camera to SfmData
-  void add_camera(const SfmCamera& cam){
+  void add_camera(const SfmCamera& cam) {
     cameras.push_back(cam);
   }
+
+  /** Serialization function */
+  friend class boost::serialization::access;
+  template<class Archive>
+  void serialize(Archive & ar, const unsigned int /*version*/) {
+    ar & BOOST_SERIALIZATION_NVP(cameras);
+    ar & BOOST_SERIALIZATION_NVP(tracks);
+  }
+
+  /// @}
+  /// @name Testable
+  /// @{
+
+  /// assert equality up to a tolerance
+  bool equals(const SfmData &sfmData, double tol = 1e-9) const {
+    // check number of cameras and tracks
+    if (number_cameras() != sfmData.number_cameras() ||
+        number_tracks() != sfmData.number_tracks()) {
+      return false;
+    }
+
+    // check each camera
+    for (size_t i = 0; i < number_cameras(); ++i) {
+      if (!camera(i).equals(sfmData.camera(i), tol)) {
+        return false;
+      }
+    }
+
+    // check each track
+    for (size_t j = 0; j < number_tracks(); ++j) {
+      if (!track(j).equals(sfmData.track(j), tol)) {
+        return false;
+      }
+    }
+
+    return true;
+  }
+
+  /// print
+  void print(const std::string& s = "") const {
+    cout << "Number of cameras = " << number_cameras() << "\n";
+    cout << "Number of tracks = " << number_tracks() << "\n";
+  }
+};
+
+/* ************************************************************************* */
+/// traits
+template<>
+struct traits<SfmData> : public Testable<SfmData> {
 };
 
 /**

gtsam/slam/tests/testSerializationDataset.cpp

@@ -0,0 +1,58 @@
+/* ----------------------------------------------------------------------------
+
+ * 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 testSerializationDataset.cpp
+ * @brief serialization tests for dataset.cpp
+ * @author Ayush Baid
+ * @date Jan 1, 2021
+ */
+
+#include <gtsam/slam/dataset.h>
+
+#include <gtsam/base/serializationTestHelpers.h>
+#include <CppUnitLite/TestHarness.h>
+
+using namespace std;
+using namespace gtsam;
+using namespace gtsam::serializationTestHelpers;
+
+/* ************************************************************************* */
+TEST(dataSet, sfmDataSerialization) {
+  // Test the serialization of SfmData
+  const string filename = findExampleDataFile("dubrovnik-3-7-pre");
+  SfmData mydata;
+  CHECK(readBAL(filename, mydata));
+
+  // round-trip equality check on serialization and subsequent deserialization
+  EXPECT(equalsObj(mydata));
+  EXPECT(equalsXML(mydata));
+  EXPECT(equalsBinary(mydata));
+}
+
+/* ************************************************************************* */
+TEST(dataSet, sfmTrackSerialization) {
+  // Test the serialization of SfmTrack
+  const string filename = findExampleDataFile("dubrovnik-3-7-pre");
+  SfmData mydata;
+  CHECK(readBAL(filename, mydata));
+
+  SfmTrack track = mydata.track(0);
+
+  // round-trip equality check on serialization and subsequent deserialization
+  EXPECT(equalsObj(track));
+  EXPECT(equalsXML(track));
+  EXPECT(equalsBinary(track));
+}
+
+/* ************************************************************************* */
+int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
+/* ************************************************************************* */