From e5cd8c6dae83e5dfa29eb01b5f8d1b8ac68dbfea Mon Sep 17 00:00:00 2001
From: cbeall <cbeall@nuro.ai>
Date: Thu, 15 Nov 2018 11:06:32 -0800
Subject: [PATCH] Fix serialization of AttitudeFactor and add roundtrip test

---
 gtsam/navigation/AttitudeFactor.h             | 26 ++++++++++-------
 gtsam/navigation/tests/testAttitudeFactor.cpp | 29 +++++++++++++++++++
 2 files changed, 45 insertions(+), 10 deletions(-)

diff --git a/gtsam/navigation/AttitudeFactor.h b/gtsam/navigation/AttitudeFactor.h
index 4ae6078e9..23cd5c477 100644
--- a/gtsam/navigation/AttitudeFactor.h
+++ b/gtsam/navigation/AttitudeFactor.h
@@ -55,6 +55,14 @@ public:
   /** vector of errors */
   Vector attitudeError(const Rot3& p,
       OptionalJacobian<2,3> H = boost::none) const;
+
+  /** Serialization function */
+  friend class boost::serialization::access;
+  template<class ARCHIVE>
+  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
+    ar & boost::serialization::make_nvp("nZ_", const_cast<Unit3&>(nZ_));
+    ar & boost::serialization::make_nvp("bRef_", const_cast<Unit3&>(bRef_));
+  }
 };
 
 /**
@@ -123,11 +131,10 @@ private:
   friend class boost::serialization::access;
   template<class ARCHIVE>
   void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
-    ar
-        & boost::serialization::make_nvp("NoiseModelFactor1",
-            boost::serialization::base_object<Base>(*this));
-    ar & BOOST_SERIALIZATION_NVP(nZ_);
-    ar & BOOST_SERIALIZATION_NVP(bRef_);
+    ar & boost::serialization::make_nvp("NoiseModelFactor1",
+        boost::serialization::base_object<Base>(*this));
+    ar & boost::serialization::make_nvp("AttitudeFactor",
+        boost::serialization::base_object<AttitudeFactor>(*this));
   }
 
 public:
@@ -210,11 +217,10 @@ private:
   friend class boost::serialization::access;
   template<class ARCHIVE>
   void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
-    ar
-        & boost::serialization::make_nvp("NoiseModelFactor1",
-            boost::serialization::base_object<Base>(*this));
-    ar & BOOST_SERIALIZATION_NVP(nZ_);
-    ar & BOOST_SERIALIZATION_NVP(bRef_);
+    ar & boost::serialization::make_nvp("NoiseModelFactor1",
+        boost::serialization::base_object<Base>(*this));
+    ar & boost::serialization::make_nvp("AttitudeFactor",
+        boost::serialization::base_object<AttitudeFactor>(*this));
   }
 
 public:
diff --git a/gtsam/navigation/tests/testAttitudeFactor.cpp b/gtsam/navigation/tests/testAttitudeFactor.cpp
index f7f0aa9ad..70b78c916 100644
--- a/gtsam/navigation/tests/testAttitudeFactor.cpp
+++ b/gtsam/navigation/tests/testAttitudeFactor.cpp
@@ -19,6 +19,8 @@
 #include <gtsam/navigation/AttitudeFactor.h>
 #include <gtsam/base/Testable.h>
 #include <gtsam/base/numericalDerivative.h>
+#include <gtsam/base/serialization.h>
+#include <gtsam/base/serializationTestHelpers.h>
 #include <CppUnitLite/TestHarness.h>
 
 using namespace std;
@@ -57,6 +59,22 @@ TEST( Rot3AttitudeFactor, Constructor ) {
   EXPECT(assert_equal(expectedH, actualH, 1e-8));
 }
 
+/* ************************************************************************* */
+// Export Noisemodels
+// See http://www.boost.org/doc/libs/1_32_0/libs/serialization/doc/special.html
+BOOST_CLASS_EXPORT(gtsam::noiseModel::Isotropic);
+
+/* ************************************************************************* */
+TEST(Rot3AttitudeFactor, Serialization) {
+  Unit3 nDown(0, 0, -1);
+  SharedNoiseModel model = noiseModel::Isotropic::Sigma(2, 0.25);
+  Rot3AttitudeFactor factor(0, nDown, model);
+
+  EXPECT(serializationTestHelpers::equalsObj(factor));
+  EXPECT(serializationTestHelpers::equalsXML(factor));
+  EXPECT(serializationTestHelpers::equalsBinary(factor));
+}
+
 // *************************************************************************
 TEST( Pose3AttitudeFactor, Constructor ) {
 
@@ -90,6 +108,17 @@ TEST( Pose3AttitudeFactor, Constructor ) {
   EXPECT(assert_equal(expectedH, actualH, 1e-8));
 }
 
+/* ************************************************************************* */
+TEST(Pose3AttitudeFactor, Serialization) {
+  Unit3 nDown(0, 0, -1);
+  SharedNoiseModel model = noiseModel::Isotropic::Sigma(2, 0.25);
+  Pose3AttitudeFactor factor(0, nDown, model);
+
+  EXPECT(serializationTestHelpers::equalsObj(factor));
+  EXPECT(serializationTestHelpers::equalsXML(factor));
+  EXPECT(serializationTestHelpers::equalsBinary(factor));
+}
+
 // *************************************************************************
 int main() {
   TestResult tr;