From 07bb930dbb0866a1c47d4e81b6bc3d3611f29797 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <frank@skyd.io>
Date: Sat, 11 Jul 2015 13:52:27 -0700
Subject: [PATCH] Now serializes noise model

---
 gtsam/sam/BearingFactor.h             | 13 +++++++++----
 gtsam/sam/tests/testBearingFactor.cpp | 14 ++++++++++++--
 2 files changed, 21 insertions(+), 6 deletions(-)

diff --git a/gtsam/sam/BearingFactor.h b/gtsam/sam/BearingFactor.h
index 37414e097..9d5105604 100644
--- a/gtsam/sam/BearingFactor.h
+++ b/gtsam/sam/BearingFactor.h
@@ -56,10 +56,10 @@ class BearingFactor : public ExpressionFactor<Measured> {
   GTSAM_CONCEPT_TESTABLE_TYPE(Measured)
   GTSAM_CONCEPT_POSE_TYPE(Pose)
 
+ public:
   /// Default constructor
   BearingFactor() {}
 
- public:
   /// primary constructor
   BearingFactor(Key poseKey, Key pointKey, const Measured& measured,
                 const SharedNoiseModel& model)
@@ -83,9 +83,9 @@ class BearingFactor : public ExpressionFactor<Measured> {
   template <class ARCHIVE>
   void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
     ar& boost::serialization::make_nvp(
-        "Factor", boost::serialization::base_object<Factor>(*this));
-    //    ar& BOOST_SERIALIZATION_NVP(this->noiseModel_);
-    //    ar& BOOST_SERIALIZATION_NVP(measurement_);
+        "NoiseModelFactor",
+        boost::serialization::base_object<NoiseModelFactor>(*this));
+    ar& boost::serialization::make_nvp("measurement_", this->measurement_);
   }
 
   template <class Archive>
@@ -94,4 +94,9 @@ class BearingFactor : public ExpressionFactor<Measured> {
 
 };  // BearingFactor
 
+/// traits
+template <class Pose, class Point, class Measured>
+struct traits<BearingFactor<Pose, Point, Measured> >
+    : public Testable<BearingFactor<Pose, Point, Measured> > {};
+
 }  // namespace gtsam
diff --git a/gtsam/sam/tests/testBearingFactor.cpp b/gtsam/sam/tests/testBearingFactor.cpp
index b12a88c9a..c3cb55b0d 100644
--- a/gtsam/sam/tests/testBearingFactor.cpp
+++ b/gtsam/sam/tests/testBearingFactor.cpp
@@ -21,6 +21,7 @@
 #include <gtsam/geometry/Pose3.h>
 #include <gtsam/nonlinear/factorTesting.h>
 #include <gtsam/base/serialization.h>
+#include <gtsam/base/serializationTestHelpers.h>
 
 #include <CppUnitLite/TestHarness.h>
 
@@ -31,19 +32,28 @@ using namespace gtsam;
 static SharedNoiseModel model(noiseModel::Unit::Create(1));
 
 typedef BearingFactor<Pose2, Point2> BearingFactor2D;
-typedef BearingFactor<Pose3, Point3> BearingFactor3D;
+typedef BearingFactor<Pose3, Point3, Unit3> BearingFactor3D;
 
 Key poseKey(1);
 Key pointKey(2);
 
+GTSAM_CONCEPT_TESTABLE_INST(BearingFactor2D)
+
+/* ************************************************************************* */
+// Export Noisemodels
+// See http://www.boost.org/doc/libs/1_32_0/libs/serialization/doc/special.html
+BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Unit, "gtsam_noiseModel_Unit");
+
 /* ************************************************************************* */
 TEST(BearingFactor, 2D) {
   // Create a factor
   double measurement(10.0);
-  BearingFactor<Pose2, Point2> factor(poseKey, pointKey, measurement, model);
+  BearingFactor2D factor(poseKey, pointKey, measurement, model);
   std::string serialized = serializeXML(factor);
   cout << serialized << endl;
 
+  EXPECT(serializationTestHelpers::equalsObj<BearingFactor2D>(factor));
+
   // Set the linearization point
   Values values;
   values.insert(poseKey, Pose2(1.0, 2.0, 0.57));