diff --git a/Using-GTSAM-EXPORT.md b/Using-GTSAM-EXPORT.md index 24a29f96b3..10dc4a8539 100644 --- a/Using-GTSAM-EXPORT.md +++ b/Using-GTSAM-EXPORT.md @@ -3,9 +3,9 @@ To create a DLL in windows, the `GTSAM_EXPORT` keyword has been created and needs to be applied to different methods and classes in the code to expose this code outside of the DLL. However, there are several intricacies that make this more difficult than it sounds. In general, if you follow the following three rules, GTSAM_EXPORT should work properly. The rest of the document also describes (1) the common error message encountered when you are not following these rules and (2) the reasoning behind these usage rules. ## Usage Rules -1. Put `GTSAM_EXPORT` in front of any function that you want exported in the DLL _if and only if_ that function is declared in a .cpp file, not just a .h file. +1. Put `GTSAM_EXPORT` in front of any function that you want exported in the DLL _if and only if_ that function is defined in a .cpp file, not just a .h file. 2. Use `GTSAM_EXPORT` in a class definition (i.e. `class GSTAM_EXPORT MyClass {...}`) only if: - * At least one of the functions inside that class is declared in a .cpp file and not just the .h file. + * At least one of the functions inside that class is defined in a .cpp file and not just the .h file. * You can `GTSAM_EXPORT` any class it inherits from as well. (Note that this implictly requires the class does not derive from a "header-only" class. Note that Eigen is a "header-only" library, so if your class derives from Eigen, _do not_ use `GTSAM_EXPORT` in the class definition!) 3. If you have defined a class using `GTSAM_EXPORT`, do not use `GTSAM_EXPORT` in any of its individual function declarations. (Note that you _can_ put `GTSAM_EXPORT` in the definition of individual functions within a class as long as you don't put `GTSAM_EXPORT` in the class definition.) 4. For template specializations, you need to add `GTSAM_EXPORT` to each individual specialization. @@ -28,7 +28,7 @@ But first, we need to understand exactly what `GTSAM_EXPORT` is. `GTSAM_EXPORT` Rule #1 doesn't seem very bad, until you combine it with rule #2 -***Compiler Rule #2*** Anything declared in a header file is not included in a DLL. +***Compiler Rule #2*** Anything defined in a header file is not included in a DLL. When these two rules are combined, you get some very confusing results. For example, a class which is completely defined in a header (e.g. Foo) cannot use `GTSAM_EXPORT` in its definition. If Foo is defined with `GTSAM_EXPORT`, then the compiler _must_ find Foo in a DLL. Because Foo is a header-only class, however, it can't find it, leading to a very confusing "I can't find this symbol" type of error. Note that the linker says it can't find the symbol even though the compiler found the header file that completely defines the class. diff --git a/gtsam/CMakeLists.txt b/gtsam/CMakeLists.txt index 1fc8e45707..0911ef550d 100644 --- a/gtsam/CMakeLists.txt +++ b/gtsam/CMakeLists.txt @@ -6,6 +6,7 @@ project(gtsam LANGUAGES CXX) set (gtsam_subdirs base basis + constrained geometry inference symbolic diff --git a/gtsam/constrained/CMakeLists.txt b/gtsam/constrained/CMakeLists.txt new file mode 100644 index 0000000000..1687440765 --- /dev/null +++ b/gtsam/constrained/CMakeLists.txt @@ -0,0 +1,6 @@ +# Install headers +file(GLOB constraint_headers "*.h") +install(FILES ${constraint_headers} DESTINATION include/gtsam/constrained) + +# Build tests +add_subdirectory(tests) diff --git a/gtsam/constrained/InequalityPenaltyFunction.cpp b/gtsam/constrained/InequalityPenaltyFunction.cpp new file mode 100644 index 0000000000..2e331292ce --- /dev/null +++ b/gtsam/constrained/InequalityPenaltyFunction.cpp @@ -0,0 +1,91 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file InequalityPenaltyFunction.cpp + * @brief Ramp function to compute inequality constraint violations. + * @author Yetong Zhang + * @date Aug 3, 2024 + */ + +#include + +namespace gtsam { + +/* ********************************************************************************************* */ +InequalityPenaltyFunction::UnaryScalarFunc InequalityPenaltyFunction::function() const { + return [=](const double& x, OptionalJacobian<1, 1> H = {}) -> double { return (*this)(x, H); }; +} + +/* ********************************************************************************************* */ +double RampFunction::Ramp(const double x, OptionalJacobian<1, 1> H) { + if (x < 0) { + if (H) { + H->setConstant(0); + } + return 0; + } else { + if (H) { + H->setConstant(1); + } + return x; + } +} + +/* ********************************************************************************************* */ +double SmoothRampPoly2::operator()(const double& x, OptionalJacobian<1, 1> H) const { + if (x <= 0) { + if (H) { + H->setZero(); + } + return 0; + } else if (x < epsilon_) { + if (H) { + H->setConstant(2 * a_ * x); + } + return a_ * pow(x, 2); + } else { + if (H) { + H->setOnes(); + } + return x - epsilon_ / 2; + } +} + +/* ********************************************************************************************* */ +double SmoothRampPoly3::operator()(const double& x, OptionalJacobian<1, 1> H) const { + if (x <= 0) { + if (H) { + H->setZero(); + } + return 0; + } else if (x < epsilon_) { + if (H) { + H->setConstant(3 * a_ * pow(x, 2) + 2 * b_ * x); + } + return a_ * pow(x, 3) + b_ * pow(x, 2); + } else { + if (H) { + H->setOnes(); + } + return x; + } +} + +/* ********************************************************************************************* */ +double SoftPlusFunction::operator()(const double& x, OptionalJacobian<1, 1> H) const { + if (H) { + H->setConstant(1 / (1 + std::exp(-k_ * x))); + } + return std::log(1 + std::exp(k_ * x)) / k_; +} + +} // namespace gtsam diff --git a/gtsam/constrained/InequalityPenaltyFunction.h b/gtsam/constrained/InequalityPenaltyFunction.h new file mode 100644 index 0000000000..3aef099af5 --- /dev/null +++ b/gtsam/constrained/InequalityPenaltyFunction.h @@ -0,0 +1,149 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file InequalityPenaltyFunction.h + * @brief Ramp function to compute inequality constraint violations. + * @author Yetong Zhang + * @date Aug 3, 2024 + */ + +#pragma once + +#include +#include + +namespace gtsam { + +/** Base class for smooth approximation of the ramp function. */ +class GTSAM_EXPORT InequalityPenaltyFunction { + public: + typedef std::shared_ptr shared_ptr; + typedef std::function H)> + UnaryScalarFunc; + + /** Constructor. */ + InequalityPenaltyFunction() {} + + /** Destructor. */ + virtual ~InequalityPenaltyFunction() {} + + virtual double operator()(const double& x, + OptionalJacobian<1, 1> H = {}) const = 0; + + virtual UnaryScalarFunc function() const; +}; + +/** Ramp function f : R -> R. + * f(x) = 0 for x <= 0 + * x for x > 0 + */ +class GTSAM_EXPORT RampFunction : public InequalityPenaltyFunction { + public: + typedef InequalityPenaltyFunction Base; + typedef RampFunction This; + typedef std::shared_ptr shared_ptr; + + public: + RampFunction() : Base() {} + + virtual double operator()(const double& x, + OptionalJacobian<1, 1> H = {}) const override { + return Ramp(x, H); + } + + virtual UnaryScalarFunc function() const override { return Ramp; } + + static double Ramp(const double x, OptionalJacobian<1, 1> H = {}); +}; + +/** Ramp function approximated with a polynomial of degree 2 in [0, epsilon]. + * The coefficients are computed as + * a = 1 / (2 * epsilon) + * Function f(x) = 0 for x <= 0 + * a * x^2 for 0 < x < epsilon + * x - epsilon/2 for x >= epsilon + */ +class GTSAM_EXPORT SmoothRampPoly2 : public InequalityPenaltyFunction { + public: + typedef InequalityPenaltyFunction Base; + typedef SmoothRampPoly2 This; + typedef std::shared_ptr shared_ptr; + + protected: + double epsilon_; + double a_; + + public: + /** Constructor. + * @param epsilon parameter for adjusting the smoothness of the function. + */ + SmoothRampPoly2(const double epsilon = 1) + : Base(), epsilon_(epsilon), a_(0.5 / epsilon) {} + + virtual double operator()(const double& x, + OptionalJacobian<1, 1> H = {}) const override; +}; + +/** Ramp function approximated with a polynomial of degree 3 in [0, epsilon]. + * The coefficients are computed as + * a = -1 / epsilon^2 + * b = 2 / epsilon + * Function f(x) = 0 for x <= 0 + * a * x^3 + b * x^2 for 0 < x < epsilon + * x for x >= epsilon + */ +class GTSAM_EXPORT SmoothRampPoly3 : public InequalityPenaltyFunction { + public: + typedef InequalityPenaltyFunction Base; + typedef SmoothRampPoly3 This; + typedef std::shared_ptr shared_ptr; + + protected: + double epsilon_; + double a_; + double b_; + + public: + /** Constructor. + * @param epsilon parameter for adjusting the smoothness of the function. + */ + SmoothRampPoly3(const double epsilon = 1) + : Base(), + epsilon_(epsilon), + a_(-1 / (epsilon * epsilon)), + b_(2 / epsilon) {} + + virtual double operator()(const double& x, + OptionalJacobian<1, 1> H = {}) const override; +}; + +/** Softplus function that implements f(x) = log(1 + exp(k*x)) / k. */ +class GTSAM_EXPORT SoftPlusFunction : public InequalityPenaltyFunction { + public: + typedef InequalityPenaltyFunction Base; + typedef SoftPlusFunction This; + typedef std::shared_ptr shared_ptr; + + protected: + double k_; + + public: + /** Constructor. + * @param k parameter for adjusting the smoothness of the function. + */ + SoftPlusFunction(const double k = 1) : Base(), k_(k) {} + + virtual double operator()(const double& x, + OptionalJacobian<1, 1> H = {}) const override; +}; + +} // namespace gtsam diff --git a/gtsam/constrained/NonlinearConstraint.h b/gtsam/constrained/NonlinearConstraint.h new file mode 100644 index 0000000000..a01430bf36 --- /dev/null +++ b/gtsam/constrained/NonlinearConstraint.h @@ -0,0 +1,91 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file NonlinearConstraint.h + * @brief Nonlinear constraints in constrained optimization. + * @author Yetong Zhang, Frank Dellaert + * @date Aug 3, 2024 + */ + +#pragma once + +#include +#include +#include +#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#include +#endif + +namespace gtsam { + +/** + * Base class for nonlinear constraint. + * The constraint is represented as a NoiseModelFactor with Constrained noise model. + * whitenedError() returns The constraint violation vector. + * unwhitenedError() returns the sigma-scaled constraint violation vector. + */ +class GTSAM_EXPORT NonlinearConstraint : public NoiseModelFactor { + public: + typedef NoiseModelFactor Base; + + /** Use constructors of NoiseModelFactor. */ + using Base::Base; + + using NonlinearFactor::error; + + /** Destructor. */ + 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 { + return cloneWithNewNoiseModel(penaltyNoise(mu)); + } + + /** Return the norm of the constraint violation vector. */ + virtual double violation(const Values& x) const { return sqrt(2 * error(x)); } + + /** Check if constraint violation is within tolerance. */ + virtual bool feasible(const Values& x, const double tolerance = 1e-5) const { + return violation(x) <= tolerance; + } + + const Vector sigmas() const { return noiseModel()->sigmas(); } + + // return the hessian of unwhitened error function in each dimension. + virtual std::vector unwhitenedHessian(const Values& x) const { + throw std::runtime_error("hessian not implemented"); + } + + protected: + /** Noise model used for the penalty function. */ + SharedDiagonal penaltyNoise(const double mu) const { + return noiseModel::Diagonal::Sigmas(noiseModel()->sigmas() / sqrt(mu)); + } + + /** Default constrained noisemodel used for construction of constraint. */ + 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/constrained/NonlinearEqualityConstraint-inl.h b/gtsam/constrained/NonlinearEqualityConstraint-inl.h new file mode 100644 index 0000000000..14bc42939f --- /dev/null +++ b/gtsam/constrained/NonlinearEqualityConstraint-inl.h @@ -0,0 +1,54 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file NonlinearEqualityConstraint-inl.h + * @brief Nonlinear equality constraints in constrained optimization. + * @author Yetong Zhang, Frank Dellaert + * @date Aug 3, 2024 */ + +#pragma once + +#include + +namespace gtsam { + +/* ********************************************************************************************* */ +template +ExpressionEqualityConstraint::ExpressionEqualityConstraint(const Expression& expression, + const T& rhs, + const Vector& sigmas) + : Base(constrainedNoise(sigmas), expression.keysAndDims().first), + expression_(expression), + rhs_(rhs), + dims_(expression.keysAndDims().second) {} + +/* ********************************************************************************************* */ +template +Vector ExpressionEqualityConstraint::unwhitenedError(const Values& x, + OptionalMatrixVecType H) const { + // Copy-paste from ExpressionFactor. + if (H) { + const T value = expression_.valueAndDerivatives(x, keys_, dims_, *H); + return -traits::Local(value, rhs_); + } else { + const T value = expression_.value(x); + return -traits::Local(value, rhs_); + } +} + +/* ********************************************************************************************* */ +template +NoiseModelFactor::shared_ptr ExpressionEqualityConstraint::penaltyFactor(const double mu) const { + return std::make_shared>(penaltyNoise(mu), rhs_, expression_); +} + +} // namespace gtsam diff --git a/gtsam/constrained/NonlinearEqualityConstraint.cpp b/gtsam/constrained/NonlinearEqualityConstraint.cpp new file mode 100644 index 0000000000..effc8774c1 --- /dev/null +++ b/gtsam/constrained/NonlinearEqualityConstraint.cpp @@ -0,0 +1,84 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file NonlinearEqualityConstraint.cpp + * @brief Nonlinear equality constraints in constrained optimization. + * @author Yetong Zhang, Frank Dellaert + * @date Aug 3, 2024 */ + +#include +#include + +namespace gtsam { + +/* ********************************************************************************************* */ +ZeroCostConstraint::ZeroCostConstraint(const NoiseModelFactor::shared_ptr& factor) + : Base(constrainedNoise(factor->noiseModel()->sigmas()), factor->keys()), factor_(factor) {} + +/* ********************************************************************************************* */ +Vector ZeroCostConstraint::unwhitenedError(const Values& x, OptionalMatrixVecType H) const { + return factor_->unwhitenedError(x, H); +} + +/* ********************************************************************************************* */ +NoiseModelFactor::shared_ptr ZeroCostConstraint::penaltyFactor(const double mu) const { + return factor_->cloneWithNewNoiseModel(penaltyNoise(mu)); +} + +/* ********************************************************************************************* */ +NonlinearEqualityConstraints NonlinearEqualityConstraints::FromCostGraph( + const NonlinearFactorGraph& graph) { + NonlinearEqualityConstraints constraints; + for (const auto& factor : graph) { + auto noise_factor = std::dynamic_pointer_cast(factor); + constraints.emplace_shared(noise_factor); + } + return constraints; +} + +/* ********************************************************************************************* */ +size_t NonlinearEqualityConstraints::dim() const { + size_t dimension = 0; + for (const auto& constraint : *this) { + dimension += constraint->dim(); + } + return dimension; +} + +/* ********************************************************************************************* */ +Vector NonlinearEqualityConstraints::violationVector(const Values& values, bool whiten) const { + Vector violation(dim()); + size_t start_idx = 0; + for (const auto& constraint : *this) { + size_t dim = constraint->dim(); + violation.middleCols(start_idx, dim) = + whiten ? constraint->whitenedError(values) : constraint->unwhitenedError(values); + start_idx += dim; + } + return violation; +} + +/* ********************************************************************************************* */ +double NonlinearEqualityConstraints::violationNorm(const Values& values) const { + return violationVector(values, true).norm(); +} + +/* ********************************************************************************************* */ +NonlinearFactorGraph NonlinearEqualityConstraints::penaltyGraph(const double mu) const { + NonlinearFactorGraph graph; + for (const auto& constraint : *this) { + graph.add(constraint->penaltyFactor(mu)); + } + return graph; +} + +} // namespace gtsam diff --git a/gtsam/constrained/NonlinearEqualityConstraint.h b/gtsam/constrained/NonlinearEqualityConstraint.h new file mode 100644 index 0000000000..017ca16077 --- /dev/null +++ b/gtsam/constrained/NonlinearEqualityConstraint.h @@ -0,0 +1,182 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file NonlinearEqualityConstraint.h + * @brief Nonlinear equality constraints in constrained optimization. + * @author Yetong Zhang, Frank Dellaert + * @date Aug 3, 2024 */ + +#pragma once + +#include +#include +#include + +namespace gtsam { + +/** + * Equality constraint base class. + */ +class GTSAM_EXPORT NonlinearEqualityConstraint : public NonlinearConstraint { + public: + typedef NonlinearConstraint Base; + typedef NonlinearEqualityConstraint This; + typedef std::shared_ptr shared_ptr; + + /** Default constructor. */ + using Base::Base; + + /** 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. */ +template +class ExpressionEqualityConstraint : public NonlinearEqualityConstraint { + public: + typedef NonlinearEqualityConstraint Base; + typedef ExpressionEqualityConstraint This; + typedef std::shared_ptr shared_ptr; + + protected: + Expression expression_; + T rhs_; + FastVector dims_; + + public: + /** + * @brief Constructor. + * + * @param expression expression representing g(x). + * @param tolerance vector representing tolerance in each dimension. + */ + ExpressionEqualityConstraint(const Expression& expression, const T& rhs, const Vector& sigmas); + + virtual Vector unwhitenedError(const Values& x, OptionalMatrixVecType H = nullptr) const override; + + virtual NoiseModelFactor::shared_ptr penaltyFactor(const double mu = 1.0) const override; + + const Expression& expression() const { return expression_; } + + /// @return a deep copy of this factor + gtsam::NonlinearFactor::shared_ptr clone() const override { + return std::static_pointer_cast( + gtsam::NonlinearFactor::shared_ptr(new This(*this))); + } + + 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. + * e.g., for a factor with unwhitened cost 2x-1, the constraint enforces the + * equlity 2x-1=0. + */ +class GTSAM_EXPORT ZeroCostConstraint : public NonlinearEqualityConstraint { + public: + typedef NonlinearEqualityConstraint Base; + typedef ZeroCostConstraint This; + typedef std::shared_ptr shared_ptr; + + protected: + NoiseModelFactor::shared_ptr factor_; + + public: + /** + * @brief Constructor. + * + * @param factor NoiseModel factor. + */ + ZeroCostConstraint(const NoiseModelFactor::shared_ptr& factor); + + virtual Vector unwhitenedError(const Values& x, OptionalMatrixVecType H = nullptr) const override; + + virtual NoiseModelFactor::shared_ptr penaltyFactor(const double mu = 1.0) const override; + + /// @return a deep copy of this factor + gtsam::NonlinearFactor::shared_ptr clone() const override { + return std::static_pointer_cast( + gtsam::NonlinearFactor::shared_ptr(new This(*this))); + } + + 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. +class GTSAM_EXPORT NonlinearEqualityConstraints : public FactorGraph { + public: + typedef std::shared_ptr shared_ptr; + typedef FactorGraph Base; + + public: + using Base::Base; + + /// Create constraints ensuring the cost of factors of a graph is zero. + static NonlinearEqualityConstraints FromCostGraph(const NonlinearFactorGraph& graph); + + size_t dim() const; + + /// Evaluate the constraint violation as a vector + Vector violationVector(const Values& values, bool whiten = true) const; + + /// Evaluate the constraint violation (as 2-norm of the violation vector). + 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 + void serialize(ARCHIVE& ar, const unsigned int /*version*/) { + ar& boost::serialization::make_nvp("NonlinearEqualityConstraints", + boost::serialization::base_object(*this)); + } +#endif +}; + +} // namespace gtsam + +#include diff --git a/gtsam/constrained/NonlinearInequalityConstraint.cpp b/gtsam/constrained/NonlinearInequalityConstraint.cpp new file mode 100644 index 0000000000..b1dcb7534c --- /dev/null +++ b/gtsam/constrained/NonlinearInequalityConstraint.cpp @@ -0,0 +1,179 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file NonlinearInequalityConstraint.cpp + * @brief Nonlinear inequality constraints in constrained optimization. + * @author Yetong Zhang, Frank Dellaert + * @date Aug 3, 2024 + */ + +#include +#include + +namespace gtsam { + +/* ********************************************************************************************* */ +Vector NonlinearInequalityConstraint::whitenedExpr(const Values& x) const { + return noiseModel()->whiten(unwhitenedExpr(x)); +} + +/* ********************************************************************************************* */ +Vector NonlinearInequalityConstraint::unwhitenedError(const Values& x, + OptionalMatrixVecType H) const { + Vector error = unwhitenedExpr(x, H); + for (size_t i = 0; i < dim(); i++) { + if (error(i) < 0) { + error(i) = 0; + if (H) { + for (Matrix& m : *H) { + m.row(i).setZero(); + } + } + } + } + return error; +} + +/* ********************************************************************************************* */ +bool NonlinearInequalityConstraint::active(const Values& x) const { + return (unwhitenedExpr(x).array() >= 0).any(); +} + +/* ********************************************************************************************* */ +NoiseModelFactor::shared_ptr NonlinearInequalityConstraint::penaltyFactorCustom( + InequalityPenaltyFunction::shared_ptr func, const double mu) const { + /// Default behavior, this function should be overriden. + return penaltyFactor(mu); +} + +/* ********************************************************************************************* */ +NonlinearEqualityConstraint::shared_ptr NonlinearInequalityConstraint::createEqualityConstraint() + const { + /// Default behavior, this function should be overriden. + return nullptr; +} + +/* ********************************************************************************************* */ +NoiseModelFactor::shared_ptr NonlinearInequalityConstraint::penaltyFactorEquality( + const double mu) const { + return createEqualityConstraint()->penaltyFactor(mu); +} + +/* ********************************************************************************************* */ +ScalarExpressionInequalityConstraint::ScalarExpressionInequalityConstraint( + const Double_& expression, const double& sigma) + : Base(constrainedNoise(Vector1(sigma)), expression.keysAndDims().first), + expression_(expression), + dims_(expression.keysAndDims().second) {} + +/* ********************************************************************************************* */ +ScalarExpressionInequalityConstraint::shared_ptr ScalarExpressionInequalityConstraint::GeqZero( + const Double_& expression, const double& sigma) { + Double_ neg_expr = Double_(0.0) - expression; + return std::make_shared(neg_expr, sigma); +} + +/* ********************************************************************************************* */ +ScalarExpressionInequalityConstraint::shared_ptr ScalarExpressionInequalityConstraint::LeqZero( + const Double_& expression, const double& sigma) { + return std::make_shared(expression, sigma); +} + +/* ********************************************************************************************* */ +Vector ScalarExpressionInequalityConstraint::unwhitenedExpr(const Values& x, + OptionalMatrixVecType H) const { + // Copy-paste from ExpressionFactor. + if (H) { + return Vector1(expression_.valueAndDerivatives(x, keys_, dims_, *H)); + } else { + return Vector1(expression_.value(x)); + } +} + +/* ********************************************************************************************* */ +NonlinearEqualityConstraint::shared_ptr +ScalarExpressionInequalityConstraint::createEqualityConstraint() const { + return std::make_shared>( + expression_, 0.0, noiseModel()->sigmas()); +} + +/* ********************************************************************************************* */ +NoiseModelFactor::shared_ptr ScalarExpressionInequalityConstraint::penaltyFactor( + const double mu) const { + Double_ penalty_expression(RampFunction::Ramp, expression_); + return std::make_shared>(penaltyNoise(mu), 0.0, penalty_expression); +} + +/* ********************************************************************************************* */ +NoiseModelFactor::shared_ptr ScalarExpressionInequalityConstraint::penaltyFactorCustom( + InequalityPenaltyFunction::shared_ptr func, const double mu) const { + if (!func) { + return penaltyFactor(mu); + } + // TODO(yetong): can we pass the functor directly to construct the expression? + Double_ error(func->function(), expression_); + return std::make_shared>(penaltyNoise(mu), 0.0, error); +} + +/* ********************************************************************************************* */ +NoiseModelFactor::shared_ptr ScalarExpressionInequalityConstraint::penaltyFactorEquality( + const double mu) const { + return std::make_shared>(penaltyNoise(mu), 0.0, expression_); +} + +/* ********************************************************************************************* */ +size_t NonlinearInequalityConstraints::dim() const { + size_t dimension = 0; + for (const auto& constraint : *this) { + dimension += constraint->dim(); + } + return dimension; +} + +/* ********************************************************************************************* */ +Vector NonlinearInequalityConstraints::violationVector(const Values& values, bool whiten) const { + Vector violation(dim()); + size_t start_idx = 0; + for (const auto& constraint : *this) { + size_t dim = constraint->dim(); + violation.middleCols(start_idx, dim) = + whiten ? constraint->whitenedError(values) : constraint->unwhitenedError(values); + start_idx += dim; + } + return violation; +} + +/* ********************************************************************************************* */ +double NonlinearInequalityConstraints::violationNorm(const Values& values) const { + return violationVector(values, true).norm(); +} + +/* ********************************************************************************************* */ +NonlinearFactorGraph NonlinearInequalityConstraints::penaltyGraph(const double mu) const { + NonlinearFactorGraph graph; + for (const auto& constraint : *this) { + graph.add(constraint->penaltyFactor(mu)); + } + return graph; +} + +/* ********************************************************************************************* */ +NonlinearFactorGraph NonlinearInequalityConstraints::penaltyGraphCustom( + InequalityPenaltyFunction::shared_ptr func, const double mu) const { + NonlinearFactorGraph graph; + for (const auto& constraint : *this) { + graph.add(constraint->penaltyFactorCustom(func, mu)); + } + return graph; +} + +} // namespace gtsam diff --git a/gtsam/constrained/NonlinearInequalityConstraint.h b/gtsam/constrained/NonlinearInequalityConstraint.h new file mode 100644 index 0000000000..9dbf1f008d --- /dev/null +++ b/gtsam/constrained/NonlinearInequalityConstraint.h @@ -0,0 +1,183 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file NonlinearInequalityConstraint.h + * @brief Nonlinear inequality constraints in constrained optimization. + * @author Yetong Zhang, Frank Dellaert + * @date Aug 3, 2024 + */ + +#pragma once + +#include +#include +#include + +namespace gtsam { + +/** + * Inequality constraint base class, enforcing g(x) <= 0. + */ +class GTSAM_EXPORT NonlinearInequalityConstraint : public NonlinearConstraint { + public: + typedef NonlinearConstraint Base; + typedef NonlinearInequalityConstraint This; + typedef std::shared_ptr shared_ptr; + + /** Default constructor. */ + using Base::Base; + + /** Destructor. */ + virtual ~NonlinearInequalityConstraint() {} + + /** Return g(x). */ + virtual Vector unwhitenedExpr(const Values& x, OptionalMatrixVecType H = nullptr) const = 0; + + virtual Vector whitenedExpr(const Values& x) const; + + /** Return ramp(g(x)). */ + virtual Vector unwhitenedError(const Values& x, OptionalMatrixVecType H = nullptr) const override; + + /** Return true if g(x)>=0 in any dimension. */ + virtual bool active(const Values& x) const override; + + /** Return an equality constraint corresponding to g(x)=0. */ + virtual NonlinearEqualityConstraint::shared_ptr createEqualityConstraint() const; + + /** Cost factor using a customized penalty function. */ + virtual NoiseModelFactor::shared_ptr penaltyFactorCustom( + InequalityPenaltyFunction::shared_ptr func, const double mu = 1.0) const; + + /** 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 + * nonlinear function. + */ +class GTSAM_EXPORT ScalarExpressionInequalityConstraint : public NonlinearInequalityConstraint { + public: + typedef NonlinearInequalityConstraint Base; + typedef ScalarExpressionInequalityConstraint This; + typedef std::shared_ptr shared_ptr; + + protected: + Double_ expression_; + FastVector dims_; + + public: + /** + * @brief Constructor. + * + * @param expression expression representing g(x) (or -g(x) for GeqZero). + * @param sigma scalar representing sigma. + */ + ScalarExpressionInequalityConstraint(const Double_& expression, const double& sigma); + + /** Create an inequality constraint g(x)/sigma >= 0, internally represented as -g(x)/sigma <= 0. + */ + static ScalarExpressionInequalityConstraint::shared_ptr GeqZero(const Double_& expression, + const double& sigma); + + /** Create an inequality constraint g(x)/sigma <= 0. */ + static ScalarExpressionInequalityConstraint::shared_ptr LeqZero(const Double_& expression, + const double& sigma); + + /** Compute g(x), or -g(x) for objects constructed from GeqZero. */ + virtual Vector unwhitenedExpr(const Values& x, OptionalMatrixVecType H = nullptr) const override; + + /** Equality constraint representing g(x)/sigma = 0. */ + NonlinearEqualityConstraint::shared_ptr createEqualityConstraint() const override; + + /** Penalty function 0.5*mu*||ramp(g(x)/sigma||^2. */ + NoiseModelFactor::shared_ptr penaltyFactor(const double mu = 1.0) const override; + + /** Penalty function using a smooth approxiamtion of the ramp funciton. */ + NoiseModelFactor::shared_ptr penaltyFactorCustom(InequalityPenaltyFunction::shared_ptr func, + const double mu = 1.0) const override; + + /** Penalty function as if the constraint is equality, 0.5 * mu * ||g(x)/sigma||^2. */ + virtual NoiseModelFactor::shared_ptr penaltyFactorEquality(const double mu = 1.0) const override; + + /** Return expression g(x), or -g(x) for objects constructed from GeqZero. */ + const Double_& expression() const { return expression_; } + + /** return a deep copy of this factor. */ + gtsam::NonlinearFactor::shared_ptr clone() const override { + return std::static_pointer_cast( + gtsam::NonlinearFactor::shared_ptr(new This(*this))); + } + + 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. +class GTSAM_EXPORT NonlinearInequalityConstraints + : public FactorGraph { + public: + typedef FactorGraph Base; + typedef NonlinearInequalityConstraints This; + typedef std::shared_ptr shared_ptr; + + using Base::Base; + + /** Return the total dimension of constraints. */ + size_t dim() const; + + /** Evaluate the constraint violation as a vector. */ + Vector violationVector(const Values& values, bool whiten = true) const; + + /** Evaluate the constraint violation (as L2 norm). */ + double violationNorm(const Values& values) const; + + /** Return the penalty function corresponding to \sum_i||ramp(g_i(x))||^2. */ + NonlinearFactorGraph penaltyGraph(const double mu = 1.0) const; + + /** Return the cost graph constructed using a customized penalty function. */ + NonlinearFactorGraph penaltyGraphCustom(InequalityPenaltyFunction::shared_ptr func, + 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("NonlinearInequalityConstraints", + boost::serialization::base_object(*this)); + } +#endif +}; + +} // namespace gtsam diff --git a/gtsam/constrained/tests/CMakeLists.txt b/gtsam/constrained/tests/CMakeLists.txt new file mode 100644 index 0000000000..4f364508ca --- /dev/null +++ b/gtsam/constrained/tests/CMakeLists.txt @@ -0,0 +1,9 @@ +# if GTSAM_ENABLE_BOOST_SERIALIZATION is OFF then exclude some tests +if (NOT GTSAM_ENABLE_BOOST_SERIALIZATION) + # create a semicolon seperated list of files to exclude + set(EXCLUDE_TESTS "testSerializationConstraint.cpp") +else() + set(EXCLUDE_TESTS "") +endif() + +gtsamAddTestsGlob(constraint "test*.cpp" "${EXCLUDE_TESTS}" "gtsam") diff --git a/gtsam/constrained/tests/constrainedExample.h b/gtsam/constrained/tests/constrainedExample.h new file mode 100644 index 0000000000..0b708a41fc --- /dev/null +++ b/gtsam/constrained/tests/constrainedExample.h @@ -0,0 +1,73 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file constrainedExample.h + * @brief Simple constrained optimization scenarios. + * @author Yetong Zhang + * @date Aug 3, 2024 + */ + +#pragma once + +// #include +#include +#include +#include +#include + +namespace constrained_example { + +using namespace gtsam; + +/// Exponential function e^x. +inline double exp_func(const double& x, gtsam::OptionalJacobian<1, 1> H1 = {}) { + double result = exp(x); + if (H1) H1->setConstant(result); + return result; +} + +/// Exponential expression e^x. +inline Double_ exp(const Double_& x) { return Double_(exp_func, x); } + +/// Pow functor used for pow function. +class PowFunctor { + private: + double c_; + + public: + PowFunctor(const double& c) : c_(c) {} + + double operator()(const double& x, gtsam::OptionalJacobian<1, 1> H1 = {}) const { + if (H1) H1->setConstant(c_ * pow(x, c_ - 1)); + return pow(x, c_); + } +}; + +/// Pow function. +inline Double_ pow(const Double_& x, const double& c) { + PowFunctor pow_functor(c); + return Double_(pow_functor, x); +} + +/// Plus between Double expression and double. +inline Double_ operator+(const Double_& x, const double& d) { return x + Double_(d); } + +/// Negative sign operator. +inline Double_ operator-(const Double_& x) { return Double_(0.0) - x; } + +/// Keys for creating expressions. +Symbol x1_key('x', 1); +Symbol x2_key('x', 2); +Double_ x1(x1_key), x2(x2_key); + +} // namespace constrained_example + diff --git a/gtsam/constrained/tests/testInequalityPenaltyFunction.cpp b/gtsam/constrained/tests/testInequalityPenaltyFunction.cpp new file mode 100644 index 0000000000..9b41dbe2c5 --- /dev/null +++ b/gtsam/constrained/tests/testInequalityPenaltyFunction.cpp @@ -0,0 +1,128 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file testInequalityPenaltyFunction.h + * @brief unit tests for ramp functions + * @author Yetong Zhang + * @date Aug 3, 2024 + */ + +#include +#include +#include +#include +#include + +using namespace gtsam; + +/* ********************************************************************************************* */ +TEST(RampFunction, error_and_jacobian) { + /// Helper function for numerical Jacobian computation. + auto ramp_helper = [&](const double& x) { return RampFunction::Ramp(x); }; + + /// Create a set of values to test the function. + static std::vector x_vec{-3.0, 0.0, 1.0, 2.0, 3.0}; + static std::vector expected_r_vec{0.0, 0.0, 1.0, 2.0, 3.0}; + + for (size_t i = 0; i < x_vec.size(); i++) { + double x = x_vec.at(i); + Matrix H; + double r = RampFunction::Ramp(x, H); + + /// Check function evaluation. + EXPECT_DOUBLES_EQUAL(expected_r_vec.at(i), r, 1e-9); + + /// Check derivative. + if (abs(x) > 1e-6) { // function is not smooth at 0, so Jacobian is undefined. + Matrix expected_H = gtsam::numericalDerivative11(ramp_helper, x, 1e-6); + EXPECT(assert_equal(expected_H, H)); + } + } +} + +/* ********************************************************************************************* */ +TEST(RampFunctionPoly2, error_and_jacobian) { + /// Helper function for numerical Jacobian computation. + SmoothRampPoly2 p_ramp(2.0); + auto ramp_helper = [&](const double& x) { return p_ramp(x); }; + + /// Create a set of values to test the function. + static std::vector x_vec{-3.0, 0.0, 1.0, 2.0, 3.0}; + static std::vector expected_r_vec{0.0, 0.0, 0.25, 1.0, 2.0}; + + for (size_t i = 0; i < x_vec.size(); i++) { + double x = x_vec.at(i); + Matrix H; + double r = p_ramp(x, H); + + /// Check function evaluation. + EXPECT_DOUBLES_EQUAL(expected_r_vec.at(i), r, 1e-9); + + /// Check derivative. + Matrix expected_H = gtsam::numericalDerivative11(ramp_helper, x, 1e-6); + EXPECT(assert_equal(expected_H, H, 1e-6)); + } +} + +/* ********************************************************************************************* */ +TEST(RampFunctionPoly3, error_and_jacobian) { + /// Helper function for numerical Jacobian computation. + SmoothRampPoly3 p_ramp(2.0); + auto ramp_helper = [&](const double& x) { return p_ramp(x); }; + + /// Create a set of values to test the function. + static std::vector x_vec{-3.0, 0.0, 1.0, 2.0, 3.0}; + static std::vector expected_r_vec{0.0, 0.0, 0.75, 2.0, 3.0}; + + for (size_t i = 0; i < x_vec.size(); i++) { + double x = x_vec.at(i); + Matrix H; + double r = p_ramp(x, H); + + /// Check function evaluation. + EXPECT_DOUBLES_EQUAL(expected_r_vec.at(i), r, 1e-9); + + /// Check derivative. + Matrix expected_H = gtsam::numericalDerivative11(ramp_helper, x, 1e-6); + EXPECT(assert_equal(expected_H, H, 1e-6)); + } +} + +/* ********************************************************************************************* */ +TEST(SoftPlusFunction, error_and_jacobian) { + /// Helper function for numerical Jacobian computation. + SoftPlusFunction soft_plus(0.5); + auto soft_plus_helper = [&](const double& x) { return soft_plus(x); }; + + /// Create a set of values to test the function. + static std::vector x_vec{-3.0, 0.0, 1.0, 2.0, 3.0}; + static std::vector expected_r_vec{ + 0.40282656, 1.38629436, 1.94815397, 2.62652338, 3.40282656}; + + for (size_t i = 0; i < x_vec.size(); i++) { + double x = x_vec.at(i); + Matrix H; + double r = soft_plus(x, H); + + /// Check function evaluation. + EXPECT_DOUBLES_EQUAL(expected_r_vec.at(i), r, 1e-6); + + /// Check derivative. + Matrix expected_H = gtsam::numericalDerivative11(soft_plus_helper, x, 1e-6); + EXPECT(assert_equal(expected_H, H, 1e-6)); + } +} + +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} diff --git a/gtsam/constrained/tests/testNonlinearEqualityConstraint.cpp b/gtsam/constrained/tests/testNonlinearEqualityConstraint.cpp new file mode 100644 index 0000000000..e09c197a31 --- /dev/null +++ b/gtsam/constrained/tests/testNonlinearEqualityConstraint.cpp @@ -0,0 +1,259 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file testNonlinearEqualityConstraint.cpp + * @brief unit tests for nonlinear equality constraints + * @author Yetong Zhang + * @date Aug 3, 2024 + */ + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "constrainedExample.h" + +using namespace gtsam; +using constrained_example::pow; +using constrained_example::x1, constrained_example::x2; +using constrained_example::x1_key, constrained_example::x2_key; + +// Test methods of DoubleExpressionEquality. +TEST(ExpressionEqualityConstraint, double) { + // create constraint from double expression + // g(x1, x2) = x1 + x1^3 + x2 + x2^2, from Vanderbergh slides + Vector sigmas = Vector1(0.1); + auto g = x1 + pow(x1, 3) + x2 + pow(x2, 2); + auto constraint = ExpressionEqualityConstraint(g, 0.0, sigmas); + + EXPECT(constraint.noiseModel()->isConstrained()); + EXPECT(assert_equal(sigmas, constraint.noiseModel()->sigmas())); + + // Create 2 sets of values for testing. + Values values1, values2; + values1.insert(x1_key, 0.0); + values1.insert(x2_key, 0.0); + values2.insert(x1_key, 1.0); + values2.insert(x2_key, 1.0); + + // Check that values1 are feasible. + EXPECT(constraint.feasible(values1)); + + // Check that violation evaluates as 0 at values1. + EXPECT(assert_equal(Vector::Zero(1), constraint.unwhitenedError(values1))); + EXPECT(assert_equal(Vector::Zero(1), constraint.whitenedError(values1))); + EXPECT(assert_equal(0.0, constraint.error(values1))); + + // Check that values2 are indeed deemed infeasible. + EXPECT(!constraint.feasible(values2)); + + // Check constraint violation is indeed g(x) at values2. + EXPECT(assert_equal(Vector::Constant(1, 4.0), constraint.unwhitenedError(values2))); + EXPECT(assert_equal(Vector::Constant(1, 40), constraint.whitenedError(values2))); + EXPECT(assert_equal(800, constraint.error(values2))); + + // Check dimension is 1 for scalar g. + EXPECT(constraint.dim() == 1); + + // Check keys include x1, x2. + EXPECT(constraint.keys().size() == 2); + EXPECT(x1_key == *constraint.keys().begin()); + EXPECT(x2_key == *constraint.keys().rbegin()); + + // Generate factor representing the term in merit function. + double mu = 4; + auto merit_factor = constraint.penaltyFactor(mu); + + // Check that noise model sigma == sigmas/sqrt(mu). + auto expected_noise = noiseModel::Diagonal::Sigmas(sigmas / sqrt(mu)); + EXPECT(expected_noise->equals(*merit_factor->noiseModel())); + + // Check that error is equal to 0.5*mu * (g(x)+bias)^2/sigmas^2. + double expected_error1 = 0; // 0.5 * 4 * ||0||_(0.1^2)^2 + EXPECT(assert_equal(expected_error1, merit_factor->error(values1))); + double expected_error2 = 3200; // 0.5 * 4 * ||4||_(0.1^2)^2 + EXPECT(assert_equal(expected_error2, merit_factor->error(values2))); + + // Check jacobian computation is correct. + EXPECT_CORRECT_FACTOR_JACOBIANS(*merit_factor, values1, 1e-7, 1e-5); + EXPECT_CORRECT_FACTOR_JACOBIANS(*merit_factor, values2, 1e-7, 1e-5); +} + +// Test methods of VectorExpressionEquality. +TEST(ExpressionEqualityConstraint, Vector2) { + // g(v1, v2) = v1 + v2, our own example. + Vector2_ x1_vec_expr(x1_key); + Vector2_ x2_vec_expr(x2_key); + auto g = x1_vec_expr + x2_vec_expr; + auto sigmas = Vector2(0.1, 0.5); + auto constraint = ExpressionEqualityConstraint(g, Vector2::Zero(), sigmas); + + EXPECT(constraint.noiseModel()->isConstrained()); + EXPECT(assert_equal(sigmas, constraint.noiseModel()->sigmas())); + + // Create 2 sets of values for testing. + Values values1, values2; + values1.insert(x1_key, Vector2(1, 1)); + values1.insert(x2_key, Vector2(-1, -1)); + values2.insert(x1_key, Vector2(1, 1)); + values2.insert(x2_key, Vector2(1, 1)); + + // Check that values1 are feasible. + EXPECT(constraint.feasible(values1)); + + // Check that violation evaluates as 0 at values1. + auto expected_violation1 = (Vector(2) << 0, 0).finished(); + EXPECT(assert_equal(expected_violation1, constraint.unwhitenedError(values1))); + auto expected_scaled_violation1 = (Vector(2) << 0, 0).finished(); + EXPECT(assert_equal(expected_scaled_violation1, constraint.whitenedError(values1))); + + // Check that values2 are indeed deemed infeasible. + EXPECT(!constraint.feasible(values2)); + + // Check constraint violation is indeed g(x) at values2. + auto expected_violation2 = (Vector(2) << 2, 2).finished(); + EXPECT(assert_equal(expected_violation2, constraint.unwhitenedError(values2))); + + // Check scaled violation is indeed g(x)/sigmas at values2. + auto expected_scaled_violation2 = (Vector(2) << 20, 4).finished(); + EXPECT(assert_equal(expected_scaled_violation2, constraint.whitenedError(values2))); + + // Check dim is the dimension of the vector. + EXPECT(constraint.dim() == 2); + + // Generate factor representing the term in merit function. + double mu = 4; + auto merit_factor = constraint.penaltyFactor(mu); + + // Check that noise model sigma == sigmas/sqrt(mu). + auto expected_noise = noiseModel::Diagonal::Sigmas(sigmas / sqrt(mu)); + EXPECT(expected_noise->equals(*merit_factor->noiseModel())); + + // Check that error is equal to 0.5*mu*||g(x)+bias)||^2_Diag(sigmas^2). + double expected_error1 = 0; // 0.5 * 4 * ||[1, 0.5]||_([0.1,0.5]^2)^2 + EXPECT(assert_equal(expected_error1, merit_factor->error(values1))); + double expected_error2 = 832; // 0.5 * 4 * ||[2, 2]||_([0.1,0.5]^2)^2 + EXPECT(assert_equal(expected_error2, merit_factor->error(values2))); + + // Check jacobian computation is correct. + EXPECT_CORRECT_FACTOR_JACOBIANS(*merit_factor, values1, 1e-7, 1e-5); + EXPECT_CORRECT_FACTOR_JACOBIANS(*merit_factor, values2, 1e-7, 1e-5); +} + +// Test methods of FactorZeroErrorConstraint. +TEST(ZeroCostConstraint, BetweenFactor) { + Key x1_key = 1; + Key x2_key = 2; + Vector sigmas = Vector2(0.5, 0.1); + auto noise = noiseModel::Diagonal::Sigmas(sigmas); + + auto factor = std::make_shared>(x1_key, x2_key, Vector2(1, 1), noise); + auto constraint = ZeroCostConstraint(factor); + + EXPECT(constraint.noiseModel()->isConstrained()); + EXPECT(assert_equal(sigmas, constraint.noiseModel()->sigmas())); + + // Create 2 sets of values for testing. + Values values1, values2; + values1.insert(x1_key, Vector2(1, 1)); + values1.insert(x2_key, Vector2(2, 2)); + values2.insert(x1_key, Vector2(0, 0)); + values2.insert(x2_key, Vector2(2, 3)); + + // Check that values1 are feasible. + EXPECT(constraint.feasible(values1)); + + // Check that violation evaluates as 0 at values1. + auto expected_violation1 = (Vector(2) << 0, 0).finished(); + EXPECT(assert_equal(expected_violation1, constraint.unwhitenedError(values1))); + auto expected_scaled_violation1 = (Vector(2) << 0, 0).finished(); + EXPECT(assert_equal(expected_scaled_violation1, constraint.whitenedError(values1))); + + // Check that values2 are indeed deemed infeasible. + EXPECT(!constraint.feasible(values2)); + + // Check constraint violation is indeed g(x) at values2. + auto expected_violation2 = (Vector(2) << 1, 2).finished(); + EXPECT(assert_equal(expected_violation2, constraint.unwhitenedError(values2))); + + // Check scaled violation is indeed g(x)/sigmas at values2. + auto expected_scaled_violation2 = (Vector(2) << 2, 20).finished(); + EXPECT(assert_equal(expected_scaled_violation2, constraint.whitenedError(values2))); + + // Check dim is the dimension of the vector. + EXPECT(constraint.dim() == 2); + + // Generate factor representing the term in merit function. + double mu = 4; + auto merit_factor = constraint.penaltyFactor(mu); + + // Check that noise model sigma == sigmas/sqrt(mu). + auto expected_noise = noiseModel::Diagonal::Sigmas(sigmas / sqrt(mu)); + EXPECT(expected_noise->equals(*merit_factor->noiseModel())); + + // Check that error is equal to 0.5*mu*||g(x)+bias)||^2_Diag(sigmas^2). + double expected_error1 = 0; // 0.5 * 4 * ||[0, 0]||_([0.5,0.1]^2)^2 + EXPECT(assert_equal(expected_error1, merit_factor->error(values1))); + double expected_error2 = 808; // 0.5 * 4 * ||[1, 2]||_([0.5,0.1]^2)^2 + EXPECT(assert_equal(expected_error2, merit_factor->error(values2))); + + // Check jacobian computation is correct. + EXPECT_CORRECT_FACTOR_JACOBIANS(*merit_factor, values1, 1e-7, 1e-5); + EXPECT_CORRECT_FACTOR_JACOBIANS(*merit_factor, values2, 1e-7, 1e-5); +} + +TEST(NonlinearEqualityConstraints, Container) { + NonlinearEqualityConstraints constraints; + + Vector sigmas1 = Vector1(10); + auto g1 = x1 + pow(x1, 3) + x2 + pow(x2, 2); + Vector2_ x1_vec_expr(x1_key); + Vector2_ x2_vec_expr(x2_key); + auto g2 = x1_vec_expr + x2_vec_expr; + Vector sigmas2 = Vector2(0.1, 0.5); + + constraints.emplace_shared>(g1, 0.0, sigmas1); + constraints.emplace_shared>(g2, Vector2::Zero(), sigmas2); + + // Check size. + EXPECT_LONGS_EQUAL(2, constraints.size()); + + // Check dimension. + EXPECT_LONGS_EQUAL(3, constraints.dim()); + + // Check keys. + KeySet expected_keys; + expected_keys.insert(x1_key); + expected_keys.insert(x2_key); + EXPECT(assert_container_equality(expected_keys, constraints.keys())); + + // Check VariableIndex. + VariableIndex vi(constraints); + FactorIndices expected_vi_x1{0, 1}; + FactorIndices expected_vi_x2{0, 1}; + EXPECT(assert_container_equality(expected_vi_x1, vi[x1_key])); + EXPECT(assert_container_equality(expected_vi_x2, vi[x2_key])); + + // Check constraint violation. +} + +TEST(NonlinearEqualityConstraints, FromCostGraph) {} + +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} diff --git a/gtsam/constrained/tests/testNonlinearInequalityConstraint.cpp b/gtsam/constrained/tests/testNonlinearInequalityConstraint.cpp new file mode 100644 index 0000000000..4d9034daa2 --- /dev/null +++ b/gtsam/constrained/tests/testNonlinearInequalityConstraint.cpp @@ -0,0 +1,117 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file testNonlinearInequalityConstraint.h + * @brief unit tests for nonlinear inequality constraints + * @author Yetong Zhang + * @date Aug 3, 2024 + */ + +#include +#include +#include +#include +#include +#include + +#include "constrainedExample.h" + +using namespace gtsam; +using constrained_example::pow; +using constrained_example::x1, constrained_example::x2; +using constrained_example::x1_key, constrained_example::x2_key; + +// Test methods of DoubleExpressionEquality. +TEST(NonlinearInequalityConstraint, ScalarExpressionInequalityConstraint) { + // create constraint from double expression + // g(x1, x2) = x1 + x1^3 + x2 + x2^2, from Vanderbergh slides + double sigma = 0.1; + auto g = x1 + pow(x1, 3) + x2 + pow(x2, 2); + auto constraint_geq = ScalarExpressionInequalityConstraint::GeqZero(g, sigma); + auto constraint_leq = ScalarExpressionInequalityConstraint::LeqZero(g, sigma); + + // Check dimension is 1 for scalar g. + EXPECT_LONGS_EQUAL(1, constraint_geq->dim()); + EXPECT_LONGS_EQUAL(1, constraint_leq->dim()); + + // Check keys include x1, x2. + KeyVector expected_keys{x1_key, x2_key}; + EXPECT(assert_container_equality(expected_keys, constraint_leq->keys())); + + // Create 3 sets of values for testing. + Values values1, values2, values3; + values1.insert(x1_key, -1.0); + values1.insert(x2_key, 1.0); + values2.insert(x1_key, 1.0); + values2.insert(x2_key, 1.0); + values3.insert(x1_key, -2.0); + values3.insert(x2_key, 1.0); + + // Check that expression evaluation at different values. + EXPECT(assert_equal(Vector1(0.0), constraint_leq->unwhitenedExpr(values1))); + EXPECT(assert_equal(Vector1(4.0), constraint_leq->unwhitenedExpr(values2))); + EXPECT(assert_equal(Vector1(-8.0), constraint_leq->unwhitenedExpr(values3))); + EXPECT(assert_equal(Vector1(0.0), constraint_geq->unwhitenedExpr(values1))); + EXPECT(assert_equal(Vector1(-4.0), constraint_geq->unwhitenedExpr(values2))); + EXPECT(assert_equal(Vector1(8.0), constraint_geq->unwhitenedExpr(values3))); + + // Check that constraint violation at different values. + EXPECT(assert_equal(Vector1(0.0), constraint_leq->unwhitenedError(values1))); + EXPECT(assert_equal(Vector1(4.0), constraint_leq->unwhitenedError(values2))); + EXPECT(assert_equal(Vector1(0.0), constraint_leq->unwhitenedError(values3))); + EXPECT(assert_equal(Vector1(0.0), constraint_geq->unwhitenedError(values1))); + EXPECT(assert_equal(Vector1(0.0), constraint_geq->unwhitenedError(values2))); + EXPECT(assert_equal(Vector1(8.0), constraint_geq->unwhitenedError(values3))); + + // Check feasible. + EXPECT(constraint_leq->feasible(values1)); + EXPECT(!constraint_leq->feasible(values2)); + EXPECT(constraint_leq->feasible(values3)); + EXPECT(constraint_geq->feasible(values1)); + EXPECT(constraint_geq->feasible(values2)); + EXPECT(!constraint_geq->feasible(values3)); + + // Check active. + EXPECT(constraint_leq->active(values1)); + EXPECT(constraint_leq->active(values2)); + EXPECT(!constraint_leq->active(values3)); + EXPECT(constraint_geq->active(values1)); + EXPECT(!constraint_geq->active(values2)); + EXPECT(constraint_geq->active(values3)); + + // Check whitenedError of penalty factor. + // Expected to be sqrt(mu) / sigma * ramp(g(x)) + double mu = 9.0; + auto penalty_leq = constraint_leq->penaltyFactor(mu); + auto penalty_geq = constraint_geq->penaltyFactor(mu); + EXPECT(assert_equal(Vector1(0.0), penalty_leq->whitenedError(values1))); + EXPECT(assert_equal(Vector1(120.0), penalty_leq->whitenedError(values2))); + EXPECT(assert_equal(Vector1(0.0), penalty_leq->whitenedError(values3))); + EXPECT(assert_equal(Vector1(0.0), penalty_geq->whitenedError(values1))); + EXPECT(assert_equal(Vector1(0.0), penalty_geq->whitenedError(values2))); + EXPECT(assert_equal(Vector1(240.0), penalty_geq->whitenedError(values3))); + + // Check create equality constraint + auto constraint_eq1 = constraint_leq->createEqualityConstraint(); + auto constraint_eq2 = constraint_geq->createEqualityConstraint(); + EXPECT(assert_equal(0.0, constraint_eq1->error(values1))); + EXPECT(assert_equal(800.0, constraint_eq1->error(values2))); + EXPECT(assert_equal(3200.0, constraint_eq1->error(values3))); + EXPECT(assert_equal(0.0, constraint_eq2->error(values1))); + EXPECT(assert_equal(800.0, constraint_eq2->error(values2))); + EXPECT(assert_equal(3200.0, constraint_eq2->error(values3))); +} + +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} diff --git a/gtsam/nonlinear/Expression.h b/gtsam/nonlinear/Expression.h index e5017390de..aa65384c29 100644 --- a/gtsam/nonlinear/Expression.h +++ b/gtsam/nonlinear/Expression.h @@ -34,6 +34,8 @@ namespace gtsam { // Forward declares class Values; template class ExpressionFactor; +template class ExpressionEqualityConstraint; +class ScalarExpressionInequalityConstraint; namespace internal { template class ExecutionTrace; @@ -206,6 +208,8 @@ class Expression { // be very selective on who can access these private methods: friend class ExpressionFactor ; friend class internal::ExpressionNode; + friend class ExpressionEqualityConstraint; + friend class ScalarExpressionInequalityConstraint; // and add tests friend class ::ExpressionFactorShallowTest; diff --git a/gtsam/nonlinear/NonlinearEquality.h b/gtsam/nonlinear/NonlinearEquality.h index f81486b301..cac54f332a 100644 --- a/gtsam/nonlinear/NonlinearEquality.h +++ b/gtsam/nonlinear/NonlinearEquality.h @@ -18,6 +18,7 @@ #pragma once #include +#include #include #include @@ -40,14 +41,11 @@ namespace gtsam { * \nosubgrouping */ template -class NonlinearEquality: public NoiseModelFactorN { +class NonlinearEquality: public NonlinearEqualityConstraint { public: typedef VALUE T; - // Provide access to the Matrix& version of evaluateError: - using NoiseModelFactor1::evaluateError; - private: // feasible value @@ -63,7 +61,7 @@ class NonlinearEquality: public NoiseModelFactorN { using This = NonlinearEquality; // typedef to base class - using Base = NoiseModelFactorN; + using Base = NonlinearEqualityConstraint; public: @@ -88,7 +86,7 @@ class NonlinearEquality: public NoiseModelFactorN { const CompareFunction &_compare = std::bind(traits::Equals, std::placeholders::_1, std::placeholders::_2, 1e-9)) : Base(noiseModel::Constrained::All(traits::GetDimension(feasible)), - j), feasible_(feasible), allow_error_(false), error_gain_(0.0), // + KeyVector{j}), feasible_(feasible), allow_error_(false), error_gain_(0.0), // compare_(_compare) { } @@ -99,10 +97,14 @@ class NonlinearEquality: public NoiseModelFactorN { const CompareFunction &_compare = std::bind(traits::Equals, std::placeholders::_1, std::placeholders::_2, 1e-9)) : Base(noiseModel::Constrained::All(traits::GetDimension(feasible)), - j), feasible_(feasible), allow_error_(true), error_gain_(error_gain), // + KeyVector{j}), feasible_(feasible), allow_error_(true), error_gain_(error_gain), // compare_(_compare) { } + Key key() const { + return keys().front(); + } + /// @} /// @name Testable /// @{ @@ -139,7 +141,7 @@ class NonlinearEquality: public NoiseModelFactorN { } /// Error function - Vector evaluateError(const T& xj, OptionalMatrixType H) const override { + Vector evaluateError(const T& xj, OptionalMatrixType H = nullptr) const { const size_t nj = traits::GetDimension(feasible_); if (allow_error_) { if (H) @@ -158,11 +160,20 @@ class NonlinearEquality: public NoiseModelFactorN { } } + Vector unwhitenedError(const Values& x, OptionalMatrixVecType H = nullptr) const override { + VALUE x1 = x.at(key()); + if (H) { + return evaluateError(x1, &(H->front())); + } else { + return evaluateError(x1); + } + } + /// Linearize is over-written, because base linearization tries to whiten GaussianFactor::shared_ptr linearize(const Values& x) const override { const T& xj = x.at(this->key()); Matrix A; - Vector b = evaluateError(xj, A); + Vector b = evaluateError(xj, &A); SharedDiagonal model = noiseModel::Constrained::All(b.size()); return GaussianFactor::shared_ptr( new JacobianFactor(this->key(), A, b, model)); @@ -206,16 +217,13 @@ struct traits> : Testable> {}; * Simple unary equality constraint - fixes a value for a variable */ template -class NonlinearEquality1: public NoiseModelFactorN { +class NonlinearEquality1: public NonlinearEqualityConstraint { public: typedef VALUE X; - // Provide access to Matrix& version of evaluateError: - using NoiseModelFactor1::evaluateError; - protected: - typedef NoiseModelFactorN Base; + typedef NonlinearEqualityConstraint Base; typedef NonlinearEquality1 This; /// Default constructor to allow for serialization @@ -240,7 +248,7 @@ class NonlinearEquality1: public NoiseModelFactorN { NonlinearEquality1(const X& value, Key key, double mu = 1000.0) : Base(noiseModel::Constrained::All(traits::GetDimension(value), std::abs(mu)), - key), + KeyVector{key}), value_(value) {} ~NonlinearEquality1() override { @@ -252,14 +260,25 @@ class NonlinearEquality1: public NoiseModelFactorN { gtsam::NonlinearFactor::shared_ptr(new This(*this))); } + Key key() const { return keys().front(); } + /// g(x) with optional derivative - Vector evaluateError(const X& x1, OptionalMatrixType H) const override { + Vector evaluateError(const X& x1, OptionalMatrixType H = nullptr) const { if (H) (*H) = Matrix::Identity(traits::GetDimension(x1),traits::GetDimension(x1)); // manifold equivalent of h(x)-z -> log(z,h(x)) return traits::Local(value_,x1); } + Vector unwhitenedError(const Values& x, OptionalMatrixVecType H = nullptr) const override { + X x1 = x.at(key()); + if (H) { + return evaluateError(x1, &(H->front())); + } else { + return evaluateError(x1); + } + } + /// Print void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override { @@ -298,10 +317,10 @@ struct traits > * be the same. */ template -class NonlinearEquality2 : public NoiseModelFactorN { +class NonlinearEquality2 : public NonlinearEqualityConstraint { protected: - using Base = NoiseModelFactorN; - using This = NonlinearEquality2; + typedef NonlinearEqualityConstraint Base; + typedef NonlinearEquality2 This; GTSAM_CONCEPT_MANIFOLD_TYPE(T) @@ -311,9 +330,6 @@ class NonlinearEquality2 : public NoiseModelFactorN { public: typedef std::shared_ptr> shared_ptr; - // Provide access to the Matrix& version of evaluateError: - using Base::evaluateError; - /** * Constructor @@ -323,7 +339,7 @@ class NonlinearEquality2 : public NoiseModelFactorN { */ NonlinearEquality2(Key key1, Key key2, double mu = 1e4) : Base(noiseModel::Constrained::All(traits::dimension, std::abs(mu)), - key1, key2) {} + KeyVector{key1, key2}) {} ~NonlinearEquality2() override {} /// @return a deep copy of this factor @@ -334,13 +350,23 @@ class NonlinearEquality2 : public NoiseModelFactorN { /// g(x) with optional derivative2 Vector evaluateError( - const T& x1, const T& x2, OptionalMatrixType H1, OptionalMatrixType H2) const override { + const T& x1, const T& x2, OptionalMatrixType H1 = nullptr, OptionalMatrixType H2 = nullptr) const { static const size_t p = traits::dimension; if (H1) *H1 = -Matrix::Identity(p, p); if (H2) *H2 = Matrix::Identity(p, p); return traits::Local(x1, x2); } + Vector unwhitenedError(const Values& x, OptionalMatrixVecType H = nullptr) const override { + T x1 = x.at(keys().front()); + T x2 = x.at(keys().back()); + if (H) { + return evaluateError(x1, x2, &(H->front()), &(H->back())); + } else { + return evaluateError(x1, x2); + } + } + GTSAM_MAKE_ALIGNED_OPERATOR_NEW private: diff --git a/gtsam/nonlinear/NonlinearFactor.h b/gtsam/nonlinear/NonlinearFactor.h index 725117748e..3830532aed 100644 --- a/gtsam/nonlinear/NonlinearFactor.h +++ b/gtsam/nonlinear/NonlinearFactor.h @@ -280,6 +280,8 @@ class GTSAM_EXPORT NoiseModelFactor: public NonlinearFactor { */ double weight(const Values& c) const; + using NonlinearFactor::error; + /** * Calculate the error of the factor. * This is the log-likelihood, e.g. \f$ 0.5(h(x)-z)^2/\sigma^2 \f$ in case of Gaussian. diff --git a/gtsam/slam/BoundingConstraint.h b/gtsam/slam/BoundingConstraint.h index a496971e28..4c6c775a00 100644 --- a/gtsam/slam/BoundingConstraint.h +++ b/gtsam/slam/BoundingConstraint.h @@ -19,6 +19,7 @@ #include #include +#include namespace gtsam { @@ -30,20 +31,17 @@ namespace gtsam { * @ingroup slam */ template -struct BoundingConstraint1: public NoiseModelFactorN { +struct BoundingConstraint1: public NonlinearInequalityConstraint { typedef VALUE X; - typedef NoiseModelFactorN Base; + typedef NonlinearInequalityConstraint Base; typedef std::shared_ptr > shared_ptr; - // Provide access to the Matrix& version of evaluateError: - using Base::evaluateError; - double threshold_; bool isGreaterThan_; /// flag for greater/less than BoundingConstraint1(Key key, double threshold, bool isGreaterThan, double mu = 1000.0) : - Base(noiseModel::Constrained::All(1, mu), key), + Base(noiseModel::Constrained::All(1, mu), KeyVector{key}), threshold_(threshold), isGreaterThan_(isGreaterThan) { } @@ -51,6 +49,7 @@ struct BoundingConstraint1: public NoiseModelFactorN { inline double threshold() const { return threshold_; } inline bool isGreaterThan() const { return isGreaterThan_; } + inline Key key() const { return keys().front(); } /** * function producing a scalar value to compare to the threshold @@ -60,14 +59,23 @@ struct BoundingConstraint1: public NoiseModelFactorN { virtual double value(const X& x, OptionalMatrixType H = OptionalNone) const = 0; - /** active when constraint *NOT* met */ - bool active(const Values& c) const override { - // note: still active at equality to avoid zigzagging - double x = value(c.at(this->key())); - return (isGreaterThan_) ? x <= threshold_ : x >= threshold_; + Vector unwhitenedExpr(const Values& x, OptionalMatrixVecType H = {}) const override { + if (H) { + double d = value(x.at(this->key()), &(H->front())); + if (isGreaterThan_) { + H->front() *= -1; + return Vector1(threshold_ - d); + } else { + return Vector1(d - threshold_); + } + } else { + double d = value(x.at(this->key())); + return Vector1((isGreaterThan_) ? threshold_ - d : d - threshold_); + } } - Vector evaluateError(const X& x, OptionalMatrixType H) const override { + /// TODO: This should be deprecated. + Vector evaluateError(const X& x, OptionalMatrixType H = {}) const { Matrix D; double error = value(x, &D) - threshold_; if (H) { @@ -102,22 +110,19 @@ struct BoundingConstraint1: public NoiseModelFactorN { * to implement for specific systems */ template -struct BoundingConstraint2: public NoiseModelFactorN { +struct BoundingConstraint2: public NonlinearInequalityConstraint { typedef VALUE1 X1; typedef VALUE2 X2; - typedef NoiseModelFactorN Base; + typedef NonlinearInequalityConstraint Base; typedef std::shared_ptr > shared_ptr; - // Provide access to the Matrix& version of evaluateError: - using Base::evaluateError; - double threshold_; bool isGreaterThan_; /// flag for greater/less than BoundingConstraint2(Key key1, Key key2, double threshold, bool isGreaterThan, double mu = 1000.0) - : Base(noiseModel::Constrained::All(1, mu), key1, key2), + : Base(noiseModel::Constrained::All(1, mu), KeyVector{key1, key2}), threshold_(threshold), isGreaterThan_(isGreaterThan) {} ~BoundingConstraint2() override {} @@ -133,15 +138,27 @@ struct BoundingConstraint2: public NoiseModelFactorN { OptionalMatrixType H1 = OptionalNone, OptionalMatrixType H2 = OptionalNone) const = 0; - /** active when constraint *NOT* met */ - bool active(const Values& c) const override { - // note: still active at equality to avoid zigzagging - double x = value(c.at(this->key1()), c.at(this->key2())); - return (isGreaterThan_) ? x <= threshold_ : x >= threshold_; + Vector unwhitenedExpr(const Values& x, OptionalMatrixVecType H = {}) const override { + X1 x1 = x.at(keys().front()); + X2 x2 = x.at(keys().back()); + if (H) { + double d = value(x1, x2, &(H->front()), &(H->back())); + if (isGreaterThan_) { + H->front() *= -1; + H->back() *= -1; + return Vector1(threshold_ - d); + } else { + return Vector1(d - threshold_); + } + } else { + double d = value(x1, x2); + return Vector1((isGreaterThan_) ? threshold_ - d : d - threshold_); + } } + /// TODO: This should be deprecated. Vector evaluateError(const X1& x1, const X2& x2, - OptionalMatrixType H1, OptionalMatrixType H2) const override { + OptionalMatrixType H1 = {}, OptionalMatrixType H2 = {}) const { Matrix D1, D2; double error = value(x1, x2, &D1, &D2) - threshold_; if (H1) { diff --git a/tests/testBoundingConstraint.cpp b/tests/testBoundingConstraint.cpp index 4aa357ba2d..ca0ec489dc 100644 --- a/tests/testBoundingConstraint.cpp +++ b/tests/testBoundingConstraint.cpp @@ -90,8 +90,8 @@ TEST( testBoundingConstraint, unary_basics_active1 ) { EXPECT(constraint2.active(config)); EXPECT(assert_equal(Vector::Constant(1,-3.0), constraint1.evaluateError(pt2), tol)); EXPECT(assert_equal(Vector::Constant(1,-5.0), constraint2.evaluateError(pt2), tol)); - EXPECT(assert_equal(Vector::Constant(1,-3.0), constraint1.unwhitenedError(config), tol)); - EXPECT(assert_equal(Vector::Constant(1,-5.0), constraint2.unwhitenedError(config), tol)); + EXPECT(assert_equal(Vector::Constant(1, 3.0), constraint1.unwhitenedError(config), tol)); + EXPECT(assert_equal(Vector::Constant(1, 5.0), constraint2.unwhitenedError(config), tol)); EXPECT_DOUBLES_EQUAL(45.0, constraint1.error(config), tol); EXPECT_DOUBLES_EQUAL(125.0, constraint2.error(config), tol); } @@ -103,10 +103,10 @@ TEST( testBoundingConstraint, unary_basics_active2 ) { config.insert(key, pt1); EXPECT(constraint3.active(config)); EXPECT(constraint4.active(config)); - EXPECT(assert_equal(-1.0 * I_1x1, constraint3.evaluateError(pt1), tol)); - EXPECT(assert_equal(-1.0 * I_1x1, constraint4.evaluateError(pt1), tol)); - EXPECT(assert_equal(-1.0 * I_1x1, constraint3.unwhitenedError(config), tol)); - EXPECT(assert_equal(-1.0 * I_1x1, constraint4.unwhitenedError(config), tol)); + // EXPECT(assert_equal(-1.0 * I_1x1, constraint3.evaluateError(pt1), tol)); + // EXPECT(assert_equal(-1.0 * I_1x1, constraint4.evaluateError(pt1), tol)); + EXPECT(assert_equal(1.0 * I_1x1, constraint3.unwhitenedError(config), tol)); + EXPECT(assert_equal(1.0 * I_1x1, constraint4.unwhitenedError(config), tol)); EXPECT_DOUBLES_EQUAL(5.0, constraint3.error(config), tol); EXPECT_DOUBLES_EQUAL(5.0, constraint4.error(config), tol); } @@ -213,7 +213,7 @@ TEST( testBoundingConstraint, MaxDistance_basics) { config1.update(key2, pt4); EXPECT(rangeBound.active(config1)); - EXPECT(assert_equal(-1.0*I_1x1, rangeBound.unwhitenedError(config1))); + EXPECT(assert_equal(1.0*I_1x1, rangeBound.unwhitenedError(config1))); EXPECT_DOUBLES_EQUAL(0.5*mu, rangeBound.error(config1), tol); }