diff --git a/gtsam/constraint/NonlinearConstraint.h b/gtsam/constraint/NonlinearConstraint.h index d0e7aef6b..00bf4d52b 100644 --- a/gtsam/constraint/NonlinearConstraint.h +++ b/gtsam/constraint/NonlinearConstraint.h @@ -19,7 +19,9 @@ #pragma once #include -#include "gtsam/linear/NoiseModel.h" +#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#include +#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 + void serialize(ARCHIVE& ar, const unsigned int /*version*/) { + ar& boost::serialization::make_nvp("NonlinearConstraint", + boost::serialization::base_object(*this)); + } +#endif }; } // namespace gtsam diff --git a/gtsam/constraint/NonlinearEqualityConstraint.h b/gtsam/constraint/NonlinearEqualityConstraint.h index 6d3853030..9f2305fa0 100644 --- a/gtsam/constraint/NonlinearEqualityConstraint.h +++ b/gtsam/constraint/NonlinearEqualityConstraint.h @@ -20,6 +20,9 @@ #include #include #include +#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#include +#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 + void serialize(ARCHIVE& ar, const unsigned int /*version*/) { + ar& boost::serialization::make_nvp("NonlinearEqualityConstraint", + boost::serialization::base_object(*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& expression() const { return expression_; } + + private: +#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE& ar, const unsigned int /*version*/) { + ar& boost::serialization::make_nvp("ExpressionEqualityConstraint", + boost::serialization::base_object(*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 + void serialize(ARCHIVE& ar, const unsigned int /*version*/) { + ar& boost::serialization::make_nvp("ZeroCostConstraint", + boost::serialization::base_object(*this)); + ar& BOOST_SERIALIZATION_NVP(factor_); + } +#endif }; /// Container of NonlinearEqualityConstraint. @@ -113,6 +153,17 @@ class NonlinearEqualityConstraints : public FactorGraph + void serialize(ARCHIVE& ar, const unsigned int /*version*/) { + ar& boost::serialization::make_nvp("NonlinearEqualityConstraints", + boost::serialization::base_object(*this)); + } +#endif }; } // namespace gtsam diff --git a/gtsam/constraint/NonlinearInequalityConstraint.h b/gtsam/constraint/NonlinearInequalityConstraint.h index 7399b30e8..ac22df943 100644 --- a/gtsam/constraint/NonlinearInequalityConstraint.h +++ b/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 + void serialize(ARCHIVE& ar, const unsigned int /*version*/) { + ar& boost::serialization::make_nvp("NonlinearInequalityConstraint", + boost::serialization::base_object(*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 + void serialize(ARCHIVE& ar, const unsigned int /*version*/) { + ar& boost::serialization::make_nvp("ExpressionEqualityConstraint", + boost::serialization::base_object(*this)); + ar& BOOST_SERIALIZATION_NVP(expression_); + ar& BOOST_SERIALIZATION_NVP(dims_); + } +#endif }; /// Container of NonlinearInequalityConstraint. @@ -120,6 +144,17 @@ class NonlinearInequalityConstraints : public FactorGraph + void serialize(ARCHIVE& ar, const unsigned int /*version*/) { + ar& boost::serialization::make_nvp("NonlinearInequalityConstraints", + boost::serialization::base_object(*this)); + } +#endif }; } // namespace gtsam