add serialization

gtsam/constraint/NonlinearConstraint.h

@@ -19,7 +19,9 @@
 #pragma once
 
 #include <gtsam/nonlinear/NonlinearFactor.h>
-#include "gtsam/linear/NoiseModel.h"
+#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#include <boost/serialization/base_object.hpp>
+#endif
 
 namespace gtsam {
 
@@ -40,7 +42,9 @@ class NonlinearConstraint : public NoiseModelFactor {
   virtual ~NonlinearConstraint() {}
 
   /** Create a cost factor representing the L2 penalty function with scaling coefficient mu. */
-  virtual NoiseModelFactor::shared_ptr penaltyFactor(const double mu = 1.0) const = 0;
+  virtual NoiseModelFactor::shared_ptr penaltyFactor(const double mu = 1.0) const {
+    return cloneWithNewNoiseModel(penaltyNoise(mu));
+  }
 
   /** Return the norm of the constraint violation vector. */
   virtual double violation(const Values& x) const { return sqrt(2 * error(x)); }
@@ -60,6 +64,17 @@ class NonlinearConstraint : public NoiseModelFactor {
   static SharedNoiseModel constrainedNoise(const Vector& sigmas) {
     return noiseModel::Constrained::MixedSigmas(1.0, sigmas);
   }
+
+ private:
+#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+  /** Serialization function */
+  friend class boost::serialization::access;
+  template <class ARCHIVE>
+  void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
+    ar& boost::serialization::make_nvp("NonlinearConstraint",
+                                       boost::serialization::base_object<Base>(*this));
+  }
+#endif
 };
 
 }  // namespace gtsam

gtsam/constraint/NonlinearEqualityConstraint.h

@@ -20,6 +20,9 @@
 #include <gtsam/constraint/NonlinearConstraint.h>
 #include <gtsam/nonlinear/ExpressionFactor.h>
 #include <gtsam/nonlinear/NonlinearFactorGraph.h>
+#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#include <boost/serialization/base_object.hpp>
+#endif
 
 namespace gtsam {
 
@@ -37,6 +40,17 @@ class NonlinearEqualityConstraint : public NonlinearConstraint {
 
   /** Destructor. */
   virtual ~NonlinearEqualityConstraint() {}
+
+ private:
+#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+  /** Serialization function */
+  friend class boost::serialization::access;
+  template <class ARCHIVE>
+  void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
+    ar& boost::serialization::make_nvp("NonlinearEqualityConstraint",
+                                       boost::serialization::base_object<Base>(*this));
+  }
+#endif
 };
 
 /** Equality constraint that force g(x) = M. */
@@ -66,6 +80,20 @@ class ExpressionEqualityConstraint : public NonlinearEqualityConstraint {
   virtual NoiseModelFactor::shared_ptr penaltyFactor(const double mu = 1.0) const override;
 
   const Expression<T>& expression() const { return expression_; }
+
+ private:
+#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+  /** Serialization function */
+  friend class boost::serialization::access;
+  template <class ARCHIVE>
+  void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
+    ar& boost::serialization::make_nvp("ExpressionEqualityConstraint",
+                                       boost::serialization::base_object<Base>(*this));
+    ar& BOOST_SERIALIZATION_NVP(expression_);
+    ar& BOOST_SERIALIZATION_NVP(rhs_);
+    ar& BOOST_SERIALIZATION_NVP(dims_);
+  }
+#endif
 };
 
 /** Equality constraint that enforce the cost factor with zero error. */
@@ -90,6 +118,18 @@ class ZeroCostConstraint : public NonlinearEqualityConstraint {
   virtual Vector unwhitenedError(const Values& x, OptionalMatrixVecType H = nullptr) const override;
 
   virtual NoiseModelFactor::shared_ptr penaltyFactor(const double mu = 1.0) const override;
+
+ private:
+#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+  /** Serialization function */
+  friend class boost::serialization::access;
+  template <class ARCHIVE>
+  void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
+    ar& boost::serialization::make_nvp("ZeroCostConstraint",
+                                       boost::serialization::base_object<Base>(*this));
+    ar& BOOST_SERIALIZATION_NVP(factor_);
+  }
+#endif
 };
 
 /// Container of NonlinearEqualityConstraint.
@@ -113,6 +153,17 @@ class NonlinearEqualityConstraints : public FactorGraph<NonlinearEqualityConstra
   double violationNorm(const Values& values) const;
 
   NonlinearFactorGraph penaltyGraph(const double mu = 1.0) const;
+
+ private:
+#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+  /** Serialization function */
+  friend class boost::serialization::access;
+  template <class ARCHIVE>
+  void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
+    ar& boost::serialization::make_nvp("NonlinearEqualityConstraints",
+                                       boost::serialization::base_object<Base>(*this));
+  }
+#endif
 };
 
 }  // namespace gtsam

gtsam/constraint/NonlinearInequalityConstraint.h

@@ -53,6 +53,17 @@ class NonlinearInequalityConstraint : public NonlinearConstraint {
 
   /** penalty function as if the constraint is equality, 0.5 * mu * ||g(x)||^2 */
   virtual NoiseModelFactor::shared_ptr penaltyFactorEquality(const double mu = 1.0) const;
+
+ private:
+#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+  /** Serialization function */
+  friend class boost::serialization::access;
+  template <class ARCHIVE>
+  void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
+    ar& boost::serialization::make_nvp("NonlinearInequalityConstraint",
+                                       boost::serialization::base_object<Base>(*this));
+  }
+#endif
 };
 
 /** Inequality constraint that force g(x) <= 0, where g(x) is a scalar-valued
@@ -96,6 +107,19 @@ class ScalarExpressionInequalityConstraint : public NonlinearInequalityConstrain
   virtual NoiseModelFactor::shared_ptr penaltyFactorEquality(const double mu = 1.0) const override;
 
   const Double_& expression() const { return expression_; }
+
+ private:
+#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+  /** Serialization function */
+  friend class boost::serialization::access;
+  template <class ARCHIVE>
+  void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
+    ar& boost::serialization::make_nvp("ExpressionEqualityConstraint",
+                                       boost::serialization::base_object<Base>(*this));
+    ar& BOOST_SERIALIZATION_NVP(expression_);
+    ar& BOOST_SERIALIZATION_NVP(dims_);
+  }
+#endif
 };
 
 /// Container of NonlinearInequalityConstraint.
@@ -120,6 +144,17 @@ class NonlinearInequalityConstraints : public FactorGraph<NonlinearInequalityCon
 
   NonlinearFactorGraph penaltyGraphSmooth(SmoothRampFunction::shared_ptr func,
                                           const double mu = 1.0) const;
+
+ private:
+#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+  /** Serialization function */
+  friend class boost::serialization::access;
+  template <class ARCHIVE>
+  void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
+    ar& boost::serialization::make_nvp("NonlinearInequalityConstraints",
+                                       boost::serialization::base_object<Base>(*this));
+  }
+#endif
 };
 
 }  // namespace gtsam