12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

add serialization

release/4.3a0
yetongumich 2024-08-06 10:41:18 -04:00
parent 59f6f081c5
commit ba50bd7ad1
3 changed files with 103 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
gtsam/constraint/NonlinearConstraint.h
View File

@ -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

51
gtsam/constraint/NonlinearEqualityConstraint.h
View File

@ -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

35
gtsam/constraint/NonlinearInequalityConstraint.h
View File

@ -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