From 76a605c5ff337d916056082aee23a07b61ac8431 Mon Sep 17 00:00:00 2001
From: Jing Dong <jdong@gatech.edu>
Date: Wed, 30 Mar 2016 12:33:13 -0400
Subject: [PATCH] more fix of smart factor serialization

---
 gtsam/geometry/triangulation.h                | 21 +++++++++++++++++++
 gtsam/slam/SmartFactorBase.h                  |  5 ++++-
 gtsam/slam/SmartProjectionFactor.h            | 19 +++++++++++++++--
 .../tests/testSmartProjectionPoseFactor.cpp   | 20 ++++++++++++++++++
 4 files changed, 62 insertions(+), 3 deletions(-)

diff --git a/gtsam/geometry/triangulation.h b/gtsam/geometry/triangulation.h
index c6e613b14..d08aef042 100644
--- a/gtsam/geometry/triangulation.h
+++ b/gtsam/geometry/triangulation.h
@@ -363,6 +363,18 @@ struct TriangulationParameters {
         << p.dynamicOutlierRejectionThreshold << std::endl;
     return os;
   }
+
+private:
+
+  /// Serialization function
+  friend class boost::serialization::access;
+  template<class ARCHIVE>
+  void serialize(ARCHIVE & ar, const unsigned int version) {
+    ar & BOOST_SERIALIZATION_NVP(rankTolerance);
+    ar & BOOST_SERIALIZATION_NVP(enableEPI);
+    ar & BOOST_SERIALIZATION_NVP(landmarkDistanceThreshold);
+    ar & BOOST_SERIALIZATION_NVP(dynamicOutlierRejectionThreshold);
+  }
 };
 
 /**
@@ -411,6 +423,15 @@ public:
       os << "no point, status = " << result.status_ << std::endl;
     return os;
   }
+
+private:
+
+  /// Serialization function
+  friend class boost::serialization::access;
+  template<class ARCHIVE>
+  void serialize(ARCHIVE & ar, const unsigned int version) {
+    ar & BOOST_SERIALIZATION_NVP(status_);
+  }
 };
 
 /// triangulateSafe: extensive checking of the outcome
diff --git a/gtsam/slam/SmartFactorBase.h b/gtsam/slam/SmartFactorBase.h
index 01a8fcf8d..010ceffca 100644
--- a/gtsam/slam/SmartFactorBase.h
+++ b/gtsam/slam/SmartFactorBase.h
@@ -29,6 +29,7 @@
 #include <gtsam/geometry/CameraSet.h>
 
 #include <boost/optional.hpp>
+#include <boost/serialization/optional.hpp>
 #include <boost/make_shared.hpp>
 #include <vector>
 
@@ -67,7 +68,7 @@ protected:
   std::vector<Z> measured_;
 
   /// @name Pose of the camera in the body frame
-  const boost::optional<Pose3> body_P_sensor_; ///< Pose of the camera in the body frame
+  boost::optional<Pose3> body_P_sensor_; ///< Pose of the camera in the body frame
   /// @}
 
   static const int Dim = traits<CAMERA>::dimension; ///< Camera dimension
@@ -392,7 +393,9 @@ private:
   template<class ARCHIVE>
   void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
     ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
+    ar & BOOST_SERIALIZATION_NVP(noiseModel_);
     ar & BOOST_SERIALIZATION_NVP(measured_);
+    ar & BOOST_SERIALIZATION_NVP(body_P_sensor_);
   }
 };
 // end class SmartFactorBase
diff --git a/gtsam/slam/SmartProjectionFactor.h b/gtsam/slam/SmartProjectionFactor.h
index 09fe84caa..631527d86 100644
--- a/gtsam/slam/SmartProjectionFactor.h
+++ b/gtsam/slam/SmartProjectionFactor.h
@@ -112,6 +112,20 @@ struct GTSAM_EXPORT SmartProjectionParams {
   void setDynamicOutlierRejectionThreshold(double dynOutRejectionThreshold) {
     triangulation.dynamicOutlierRejectionThreshold = dynOutRejectionThreshold;
   }
+
+private:
+
+  /// Serialization function
+  friend class boost::serialization::access;
+  template<class ARCHIVE>
+  void serialize(ARCHIVE & ar, const unsigned int version) {
+    ar & BOOST_SERIALIZATION_NVP(linearizationMode);
+    ar & BOOST_SERIALIZATION_NVP(degeneracyMode);
+    ar & BOOST_SERIALIZATION_NVP(triangulation);
+    ar & BOOST_SERIALIZATION_NVP(retriangulationThreshold);
+    ar & BOOST_SERIALIZATION_NVP(throwCheirality);
+    ar & BOOST_SERIALIZATION_NVP(verboseCheirality);
+  }
 };
 
 /**
@@ -535,8 +549,9 @@ private:
   template<class ARCHIVE>
   void serialize(ARCHIVE & ar, const unsigned int version) {
     ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
-    ar & BOOST_SERIALIZATION_NVP(params_.throwCheirality);
-    ar & BOOST_SERIALIZATION_NVP(params_.verboseCheirality);
+    ar & BOOST_SERIALIZATION_NVP(params_);
+    ar & BOOST_SERIALIZATION_NVP(result_);
+    ar & BOOST_SERIALIZATION_NVP(cameraPosesTriangulation_);
   }
 }
 ;
diff --git a/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp b/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp
index 1c1bc3c03..b6ecd1db6 100644
--- a/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp
+++ b/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp
@@ -1409,6 +1409,26 @@ TEST(SmartProjectionPoseFactor, serialize) {
   EXPECT(equalsBinary(factor));
 }
 
+TEST(SmartProjectionPoseFactor, serialize2) {
+  using namespace vanillaPose;
+  using namespace gtsam::serializationTestHelpers;
+  SmartProjectionParams params;
+  params.setRankTolerance(rankTol);
+  Pose3 bts;
+  SmartFactor factor(model, sharedK, bts, params);
+
+  // insert some measurments
+  vector<Key> key_view;
+  vector<Point2> meas_view;
+  key_view.push_back(Symbol('x', 1));
+  meas_view.push_back(Point2(10, 10));
+  factor.add(meas_view, key_view);
+
+  EXPECT(equalsObj(factor));
+  EXPECT(equalsXML(factor));
+  EXPECT(equalsBinary(factor));
+}
+
 /* ************************************************************************* */
 int main() {
   TestResult tr;