Fully serializable expression factors

gtsam/nonlinear/ExpressionFactor.h

@@ -46,19 +46,10 @@ public:
   typedef boost::shared_ptr<ExpressionFactor<T> > shared_ptr;
 
   /// Constructor
-  ExpressionFactor(const SharedNoiseModel& noiseModel, //
+  ExpressionFactor(const SharedNoiseModel& noiseModel,  //
-      const T& measurement, const Expression<T>& expression) :
+                   const T& measurement, const Expression<T>& expression)
-      measurement_(measurement), expression_(expression) {
+      : NoiseModelFactor(noiseModel), measurement_(measurement) {
-    if (!noiseModel)
+    initialize(expression);
-      throw std::invalid_argument("ExpressionFactor: no NoiseModel.");
-    if (noiseModel->dim() != Dim)
-      throw std::invalid_argument(
-          "ExpressionFactor was created with a NoiseModel of incorrect dimension.");
-    noiseModel_ = noiseModel;
-
-    // Get keys and dimensions for Jacobian matrices
-    // An Expression is assumed unmutable, so we do this now
-    boost::tie(keys_, dims_) = expression_.keysAndDims();
   }
 
   /// print relies on Testable traits being defined for T
@@ -71,7 +62,8 @@ public:
   bool equals(const NonlinearFactor& f, double tol) const {
     const ExpressionFactor* p = dynamic_cast<const ExpressionFactor*>(&f);
     return p && NoiseModelFactor::equals(f, tol) &&
-           traits<T>::Equals(measurement_, p->measurement_, tol);
+           traits<T>::Equals(measurement_, p->measurement_, tol) &&
+           dims_ == p->dims_;
   }
 
   /**
@@ -132,13 +124,84 @@ public:
         gtsam::NonlinearFactor::shared_ptr(new This(*this)));
   }
 
- protected:
+protected:
-  /// Default constructor, for serialization
+ /// Default constructor, for serialization
-  ExpressionFactor() {}
+ ExpressionFactor() {}
 
+ /// Constructor for use by SerializableExpressionFactor
+ ExpressionFactor(const SharedNoiseModel& noiseModel, const T& measurement)
+     : NoiseModelFactor(noiseModel), measurement_(measurement) {
+   // Not properly initialized yet, need to call initialize
+ }
+
+ /// Initialize with constructor arguments
+ void initialize(const Expression<T>& expression) {
+   if (!noiseModel_)
+     throw std::invalid_argument("ExpressionFactor: no NoiseModel.");
+   if (noiseModel_->dim() != Dim)
+     throw std::invalid_argument(
+         "ExpressionFactor was created with a NoiseModel of incorrect dimension.");
+   expression_ = expression;
+
+   // Get keys and dimensions for Jacobian matrices
+   // An Expression is assumed unmutable, so we do this now
+   boost::tie(keys_, dims_) = expression_.keysAndDims();
+ }
+
+private:
+ /// Serialization function
+ template <class ARCHIVE>
+ void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
+   ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(NoiseModelFactor);
+   ar& boost::serialization::make_nvp("measurement_", this->measurement_);
+ }
+
+ friend class boost::serialization::access;
 };
 // ExpressionFactor
 
+
+/**
+ * ExpressionFactor variant that supports serialization
+ * Simply overload the pure virtual method [expression] to construct an expression from keys_
+ */
+template <typename T>
+class SerializableExpressionFactor : public ExpressionFactor<T> {
+ public:
+  /// Constructor takes only two arguments, still need to call initialize
+  SerializableExpressionFactor(const SharedNoiseModel& noiseModel, const T& measurement)
+      : ExpressionFactor<T>(noiseModel, measurement) {
+  }
+
+ protected:
+
+  /// Return an expression that predicts the measurement given Values
+  virtual Expression<T> expression() const = 0;
+
+  /// Default constructor, for serialization
+  SerializableExpressionFactor() {}
+
+  /// Save to an archive: just saves the base class
+  template <class Archive>
+  void save(Archive& ar, const unsigned int /*version*/) const {
+    ar << boost::serialization::make_nvp(
+              "ExpressionFactor", boost::serialization::base_object<ExpressionFactor<T> >(*this));
+  }
+
+  /// Load from an archive, creating a valid expression using the overloaded [expression] method
+  template <class Archive>
+  void load(Archive& ar, const unsigned int /*version*/) {
+    ar >> boost::serialization::make_nvp(
+              "ExpressionFactor", boost::serialization::base_object<ExpressionFactor<T> >(*this));
+    this->initialize(expression());
+  }
+
+  // Indicate that we implement save/load separately, and be friendly to boost
+  BOOST_SERIALIZATION_SPLIT_MEMBER()
+  friend class boost::serialization::access;
+};
+// SerializableExpressionFactor
+
 /// traits
 template <typename T>
 struct traits<ExpressionFactor<T> > : public Testable<ExpressionFactor<T> > {};

gtsam/sam/BearingFactor.h

@@ -27,16 +27,6 @@ template <class T, class Archive>
 void serialize(Archive& ar, gtsam::NonlinearFactor& factor,
                const unsigned int version) {}
 
-// template <class T, class Archive>
-// void serialize(Archive& ar, gtsam::ExpressionFactor<T>& factor,
-//               const unsigned int version) {
-//  ar& BOOST_SERIALIZATION_NVP(factor.measurement_);
-//}
-
-// template <class Factor, class Archive>
-// void serialize(Archive& ar, Factor& factor, const unsigned int version) {
-//}
-
 }  // namespace serialization
 }  // namespace boost
 
@@ -47,10 +37,10 @@ namespace gtsam {
  * @addtogroup SAM
  */
 template <class Pose, class Point, class Measured = typename Pose::Rotation>
-class BearingFactor : public ExpressionFactor<Measured> {
+class BearingFactor : public SerializableExpressionFactor<Measured> {
  private:
   typedef BearingFactor<Pose, Point> This;
-  typedef ExpressionFactor<Measured> Base;
+  typedef SerializableExpressionFactor<Measured> Base;
 
   /** concept check by type */
   GTSAM_CONCEPT_TESTABLE_TYPE(Measured)
@@ -61,37 +51,28 @@ class BearingFactor : public ExpressionFactor<Measured> {
   BearingFactor() {}
 
   /// primary constructor
-  BearingFactor(Key poseKey, Key pointKey, const Measured& measured,
+  BearingFactor(Key poseKey, Key pointKey, const Measured& measured, const SharedNoiseModel& model)
-                const SharedNoiseModel& model)
+      : Base(model, measured) {
-      : Base(model, measured,
+    this->keys_.push_back(poseKey);
-             Expression<Measured>(Expression<Pose>(poseKey), &Pose::bearing,
+    this->keys_.push_back(pointKey);
-                                  Expression<Point>(pointKey))) {}
+    this->initialize(expression());
+  }
 
   virtual ~BearingFactor() {}
 
   /** print contents */
   void print(const std::string& s = "",
              const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
-    std::cout << s << "BearingFactor, bearing = ";
+    std::cout << s << "BearingFactor" << std::endl;
-    this->measurement_.print();
+    Base::print(s, keyFormatter);
-    Base::print("", keyFormatter);
   }
 
  private:
-  /** Serialization function */
+  // Return an expression
-  friend class boost::serialization::access;
+  virtual Expression<Measured> expression() const {
-  template <class ARCHIVE>
+    return Expression<Measured>(Expression<Pose>(this->keys_[0]), &Pose::bearing,
-  void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
+                                Expression<Point>(this->keys_[1]));
-    ar& boost::serialization::make_nvp(
-        "NoiseModelFactor",
-        boost::serialization::base_object<NoiseModelFactor>(*this));
-    ar& boost::serialization::make_nvp("measurement_", this->measurement_);
   }
-
-  template <class Archive>
-  friend void boost::serialization::serialize(Archive& ar, Base& factor,
-                                              const unsigned int version);
-
 };  // BearingFactor
 
 /// traits

gtsam/sam/tests/testBearingFactor.cpp

@@ -29,30 +29,41 @@ using namespace std;
 using namespace gtsam;
 
 // Create a noise model for the pixel error
-static SharedNoiseModel model(noiseModel::Unit::Create(1));
+static SharedNoiseModel model(noiseModel::Isotropic::Sigma(1, 0.5));
-
-typedef BearingFactor<Pose2, Point2> BearingFactor2D;
-typedef BearingFactor<Pose3, Point3, Unit3> BearingFactor3D;
 
 Key poseKey(1);
 Key pointKey(2);
 
+typedef BearingFactor<Pose2, Point2> BearingFactor2D;
+double measurement2D(10.0);
+BearingFactor2D factor2D(poseKey, pointKey, measurement2D, model);
 GTSAM_CONCEPT_TESTABLE_INST(BearingFactor2D)
 
+typedef BearingFactor<Pose3, Point3, Unit3> BearingFactor3D;
+
 /* ************************************************************************* */
 // 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");
+BOOST_CLASS_EXPORT(gtsam::noiseModel::Isotropic);
+//BOOST_CLASS_EXPORT(BearingFactor2D);
+//BOOST_CLASS_EXPORT(ExpressionFactor<Rot2>);
+//BOOST_CLASS_EXPORT_GUID(gtsam::ExpressionFactor<Rot2>, "ExpressionFactorRot2")
+
+/* ************************************************************************* */
+TEST(BearingFactor, Serialization2D) {
+  EXPECT(serializationTestHelpers::equalsObj<BearingFactor2D>(factor2D));
+  EXPECT(serializationTestHelpers::equalsXML<BearingFactor2D>(factor2D));
+  EXPECT(serializationTestHelpers::equalsBinary<BearingFactor2D>(factor2D));
+}
 
 /* ************************************************************************* */
 TEST(BearingFactor, 2D) {
-  // Create a factor
+  // Serialize the factor
-  double measurement(10.0);
+  std::string serialized = serializeXML(factor2D);
-  BearingFactor2D factor(poseKey, pointKey, measurement, model);
-  std::string serialized = serializeXML(factor);
-  cout << serialized << endl;
 
-  EXPECT(serializationTestHelpers::equalsObj<BearingFactor2D>(factor));
+  // And de-serialize it
+  BearingFactor2D factor;
+  deserializeXML(serialized, factor);
 
   // Set the linearization point
   Values values;
@@ -63,15 +74,17 @@ TEST(BearingFactor, 2D) {
 }
 
 /* ************************************************************************* */
-// TODO(frank): this is broken: (non-existing) Pose3::bearing should yield a Unit3
+// TODO(frank): this is broken: (non-existing) Pose3::bearing should yield a
-//TEST(BearingFactor, 3D) {
+// Unit3
+// TEST(BearingFactor, 3D) {
 //  // Create a factor
 //  Rot3 measurement;
 //  BearingFactor<Pose3, Point3> factor(poseKey, pointKey, measurement, model);
 //
 //  // Set the linearization point
 //  Values values;
-//  values.insert(poseKey, Pose3(Rot3::RzRyRx(0.2, -0.3, 1.75), Point3(1.0, 2.0, -3.0)));
+//  values.insert(poseKey, Pose3(Rot3::RzRyRx(0.2, -0.3, 1.75), Point3(1.0, 2.0,
+//  -3.0)));
 //  values.insert(pointKey, Point3(-2.0, 11.0, 1.0));
 //
 //  EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-5, 1e-5);