diff --git a/gtsam/CMakeLists.txt b/gtsam/CMakeLists.txt index 1fc8e4570..dcc350fe8 100644 --- a/gtsam/CMakeLists.txt +++ b/gtsam/CMakeLists.txt @@ -6,6 +6,7 @@ project(gtsam LANGUAGES CXX) set (gtsam_subdirs base basis + constraint geometry inference symbolic diff --git a/gtsam/constraint/CMakeLists.txt b/gtsam/constraint/CMakeLists.txt new file mode 100644 index 000000000..e4bbd89dd --- /dev/null +++ b/gtsam/constraint/CMakeLists.txt @@ -0,0 +1,6 @@ +# Install headers +file(GLOB constraint_headers "*.h") +install(FILES ${constraint_headers} DESTINATION include/gtsam/constraint) + +# Build tests +add_subdirectory(tests) diff --git a/gtsam/constraint/NonlinearConstraint.h b/gtsam/constraint/NonlinearConstraint.h new file mode 100644 index 000000000..d0e7aef6b --- /dev/null +++ b/gtsam/constraint/NonlinearConstraint.h @@ -0,0 +1,65 @@ +/* ---------------------------------------------------------------------------- + + * 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 Equality constraints in constrained optimization. + * @author Yetong Zhang, Frank Dellaert + * @date Aug 3, 2024 + */ + +#pragma once + +#include +#include "gtsam/linear/NoiseModel.h" + +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 NonlinearConstraint : public NoiseModelFactor { + public: + typedef NoiseModelFactor Base; + + /** Use constructors of NoiseModelFactor. */ + using Base::Base; + + /** 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 = 0; + + /** 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; + } + + 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); + } +}; + +} // namespace gtsam diff --git a/gtsam/constraint/NonlinearEqualityConstraint-inl.h b/gtsam/constraint/NonlinearEqualityConstraint-inl.h new file mode 100644 index 000000000..a594d0b09 --- /dev/null +++ b/gtsam/constraint/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 EqualityConstraint-inl.h + * @brief 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/constraint/NonlinearEqualityConstraint.cpp b/gtsam/constraint/NonlinearEqualityConstraint.cpp new file mode 100644 index 000000000..06201dc2a --- /dev/null +++ b/gtsam/constraint/NonlinearEqualityConstraint.cpp @@ -0,0 +1,83 @@ +/* ---------------------------------------------------------------------------- + + * 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 EqualityConstraint.cpp + * @brief Equality constraints in constrained optimization. + * @author Yetong Zhang, Frank Dellaert + * @date Aug 3, 2024 */ + +#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/constraint/NonlinearEqualityConstraint.h b/gtsam/constraint/NonlinearEqualityConstraint.h new file mode 100644 index 000000000..6d3853030 --- /dev/null +++ b/gtsam/constraint/NonlinearEqualityConstraint.h @@ -0,0 +1,120 @@ +/* ---------------------------------------------------------------------------- + + * 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 EqualityConstraint.h + * @brief 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 NonlinearEqualityConstraint : public NonlinearConstraint { + public: + typedef NonlinearConstraint Base; + typedef NonlinearEqualityConstraint This; + typedef std::shared_ptr shared_ptr; + + /** Default constructor. */ + using Base::Base; + + /** Destructor. */ + virtual ~NonlinearEqualityConstraint() {} +}; + +/** 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_; } +}; + +/** Equality constraint that enforce the cost factor with zero error. */ +class 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. + * @param tolerance vector representing tolerance in each dimension. + */ + 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; +}; + +/// Container of NonlinearEqualityConstraint. +class 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; +}; + +} // namespace gtsam + +#include diff --git a/gtsam/constraint/NonlinearInequalityConstraint.cpp b/gtsam/constraint/NonlinearInequalityConstraint.cpp new file mode 100644 index 000000000..f02fd8648 --- /dev/null +++ b/gtsam/constraint/NonlinearInequalityConstraint.cpp @@ -0,0 +1,158 @@ +/* ---------------------------------------------------------------------------- + + * 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 +#include "gtsam/constraint/RampFunction.h" + +namespace gtsam { + +/* ************************************************************************* */ +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::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, expression_); + return std::make_shared>(penaltyNoise(mu), 0.0, penalty_expression); +} + +/* ************************************************************************* */ +NoiseModelFactor::shared_ptr ScalarExpressionInequalityConstraint::penaltyFactorSmooth( + SmoothRampFunction::shared_ptr func, const double mu) const { + // 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::penaltyGraphSmooth( + SmoothRampFunction::shared_ptr func, const double mu) const { + NonlinearFactorGraph graph; + for (const auto& constraint : *this) { + graph.add(constraint->penaltyFactorSmooth(func, mu)); + } + return graph; +} + +} // namespace gtsam diff --git a/gtsam/constraint/NonlinearInequalityConstraint.h b/gtsam/constraint/NonlinearInequalityConstraint.h new file mode 100644 index 000000000..7399b30e8 --- /dev/null +++ b/gtsam/constraint/NonlinearInequalityConstraint.h @@ -0,0 +1,125 @@ +/* ---------------------------------------------------------------------------- + + * 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. + */ +class NonlinearInequalityConstraint : public NonlinearConstraint { + public: + typedef NonlinearConstraint Base; + typedef NonlinearInequalityConstraint This; + typedef std::shared_ptr shared_ptr; + + /** Default constructor. */ + using Base::Base; + + /** Destructor. */ + virtual ~NonlinearInequalityConstraint() {} + + virtual Vector unwhitenedExpr(const Values& x, OptionalMatrixVecType H = nullptr) const = 0; + + virtual Vector unwhitenedError(const Values& x, OptionalMatrixVecType H = nullptr) const override; + + virtual bool active(const Values& x) const override; + + virtual NonlinearEqualityConstraint::shared_ptr createEqualityConstraint() const = 0; + + /** Smooth approximation of the ramp function. */ + virtual NoiseModelFactor::shared_ptr penaltyFactorSmooth(SmoothRampFunction::shared_ptr func, + const double mu = 1.0) const = 0; + + /** 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; +}; + +/** Inequality constraint that force g(x) <= 0, where g(x) is a scalar-valued + * function. */ +class 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). + * @param sigma scalar representing sigma. + */ + ScalarExpressionInequalityConstraint(const Double_& expression, const double& sigma); + + // Create an inequality constraint g(x)>=0. + static ScalarExpressionInequalityConstraint::shared_ptr GeqZero(const Double_& expression, + const double& sigma); + + // Create an inequality constraint g(x)<=0. + static ScalarExpressionInequalityConstraint::shared_ptr LeqZero(const Double_& expression, + const double& sigma); + + virtual Vector unwhitenedExpr(const Values& x, OptionalMatrixVecType H = nullptr) const override; + + NonlinearEqualityConstraint::shared_ptr createEqualityConstraint() const override; + + NoiseModelFactor::shared_ptr penaltyFactor(const double mu = 1.0) const override; + + NoiseModelFactor::shared_ptr penaltyFactorSmooth(SmoothRampFunction::shared_ptr func, + const double mu = 1.0) const override; + + virtual NoiseModelFactor::shared_ptr penaltyFactorEquality(const double mu = 1.0) const override; + + const Double_& expression() const { return expression_; } +}; + +/// Container of NonlinearInequalityConstraint. +class 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; + + NonlinearFactorGraph penaltyGraph(const double mu = 1.0) const; + + NonlinearFactorGraph penaltyGraphSmooth(SmoothRampFunction::shared_ptr func, + const double mu = 1.0) const; +}; + +} // namespace gtsam diff --git a/gtsam/constraint/RampFunction.cpp b/gtsam/constraint/RampFunction.cpp new file mode 100644 index 000000000..601416afd --- /dev/null +++ b/gtsam/constraint/RampFunction.cpp @@ -0,0 +1,71 @@ +/* ---------------------------------------------------------------------------- + + * 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 + */ + +#include + +namespace gtsam { + +/* ************************************************************************* */ +double RampFunction(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; + } +} + +/* ************************************************************************* */ +SmoothRampFunction::UnaryScalarFunc SmoothRampFunction::function() const { + return [=](const double& x, OptionalJacobian<1, 1> H = {}) -> double { return (*this)(x, H); }; +} + +/* ************************************************************************* */ +double PolynomialRampFunction::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(x / epsilon_); + } + return (x * x) / (2 * epsilon_) + epsilon_ / 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/constraint/RampFunction.h b/gtsam/constraint/RampFunction.h new file mode 100644 index 000000000..d04e2dec0 --- /dev/null +++ b/gtsam/constraint/RampFunction.h @@ -0,0 +1,78 @@ +/* ---------------------------------------------------------------------------- + + * 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 + +namespace gtsam { + +/// Ramp function for create penalty factors. +double RampFunction(const double x, OptionalJacobian<1, 1> H = {}); + +/** Base class for smooth approximation of the ramp function. */ +class SmoothRampFunction { + public: + typedef std::shared_ptr shared_ptr; + typedef std::function H)> UnaryScalarFunc; + + /** Constructor. */ + SmoothRampFunction() {} + + /** Destructor. */ + virtual ~SmoothRampFunction() {} + + virtual double operator()(const double& x, OptionalJacobian<1, 1> H = {}) const = 0; + + UnaryScalarFunc function() const; +}; + +/** Ramp function approximated with a polynomial of degree 2. + * Function f(x) = 0 for x <= 0 + * x^2/(2*e) + e/2 for 0 < x < epsilon + * x for x >= epsilon + */ +class PolynomialRampFunction : public SmoothRampFunction { + public: + typedef SmoothRampFunction Base; + + protected: + double epsilon_; + + public: + PolynomialRampFunction(const double epsilon = 1) : Base(), epsilon_(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 SoftPlusFunction : public SmoothRampFunction { + public: + typedef SmoothRampFunction Base; + + protected: + double k_; + + public: + 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/constraint/tests/CMakeLists.txt b/gtsam/constraint/tests/CMakeLists.txt new file mode 100644 index 000000000..4f364508c --- /dev/null +++ b/gtsam/constraint/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/constraint/tests/constrainedExample.h b/gtsam/constraint/tests/constrainedExample.h new file mode 100644 index 000000000..a8aafb981 --- /dev/null +++ b/gtsam/constraint/tests/constrainedExample.h @@ -0,0 +1,168 @@ +/* ---------------------------------------------------------------------------- + + * 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 +#include +#include +#include +#include + + +namespace constrained_example { + +using namespace gtsam; + +/// Exponential function e^x. +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. +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. +Double_ pow(const Double_& x, const double& c) { + PowFunctor pow_functor(c); + return Double_(pow_functor, x); +} + +/// Plus between Double expression and double. +Double_ operator+(const Double_& x, const double& d) { return x + Double_(d); } + +/// Negative sign operator. +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 + +/* ************************************************************************* */ +/** + * Constrained optimization example in L. Vandenberghe slides: + * https://www.seas.ucla.edu/~vandenbe/133B/lectures/nllseq.pdf + * f(x) = 0.5*||x1 + e^(-x2)||^2 + 0.5*||x1^2 + 2*x2 + 1||^2 + * h(x) = x1 + x1^3 + x2 + x2^2 + */ +namespace e_constrained_example { +using namespace constrained_example; +NonlinearFactorGraph GetCost() { + NonlinearFactorGraph graph; + auto f1 = x1 + exp(-x2); + auto f2 = pow(x1, 2.0) + 2.0 * x2 + 1.0; + auto cost_noise = gtsam::noiseModel::Isotropic::Sigma(1, 1.0); + graph.add(ExpressionFactor(cost_noise, 0., f1)); + graph.add(ExpressionFactor(cost_noise, 0., f2)); + return graph; +} + +NonlinearEqualityConstraints GetConstraints() { + NonlinearEqualityConstraints constraints; + Vector scale = Vector1(0.1); + auto h1 = x1 + pow(x1, 3) + x2 + pow(x2, 2); + constraints.push_back(NonlinearEqualityConstraint::shared_ptr( + new ExpressionEqualityConstraint(h1, 0.0, scale))); + return constraints; +} + +NonlinearFactorGraph cost = GetCost(); +NonlinearEqualityConstraints constraints = GetConstraints(); +} // namespace e_constrained_example + +/* ************************************************************************* */ +/** + * Constrained optimization example with inequality constraints + * f(x) = 0.5 * ||x1-1||^2 + 0.5 * ||x2-1||^2 + * g(x) = 1 - x1^2 - x2^2 + */ +namespace i_constrained_example { +using namespace constrained_example; +NonlinearFactorGraph GetCost() { + NonlinearFactorGraph graph; + auto cost_noise = gtsam::noiseModel::Isotropic::Sigma(1, 1.0); + graph.addPrior(x1_key, 1.0, cost_noise); + graph.addPrior(x2_key, 1.0, cost_noise); + return graph; +} + +// InequalityConstraints GetIConstraints() { +// InequalityConstraints i_constraints; +// Double_ g1 = Double_(1.0) - x1 * x1 - x2 * x2; +// double tolerance = 0.2; +// i_constraints.emplace_shared(g1, tolerance); +// return i_constraints; +// } + +NonlinearFactorGraph cost = GetCost(); +NonlinearEqualityConstraints e_constraints; +// InequalityConstraints i_constraints = GetIConstraints(); +} // namespace i_constrained_example + +/* ************************************************************************* */ +/** + * Constrained optimization example with inequality constraints + * f(x) = 0.5 * ||x1||^2 + 0.5 * ||x2||^2 + * h(x) = x1 + x2 - 1 + */ +namespace e_constrained_example2 { +using namespace constrained_example; +NonlinearFactorGraph GetCost() { + NonlinearFactorGraph graph; + auto cost_noise = gtsam::noiseModel::Isotropic::Sigma(1, 1.0); + graph.addPrior(x1_key, 1.0, cost_noise); + // graph.addPrior(x2_key, 0.0, cost_noise); + // graph.emplace_shared>(x1_key, x2_key, 1.0, cost_noise); + return graph; +} + +NonlinearEqualityConstraints GetEConstraints() { + NonlinearEqualityConstraints e_constraints; + // Double_ h1 = x1 + x2 - Double_(1.0); + // double tolerance = 0.2; + // e_constraints.emplace_shared(h1, tolerance); + return e_constraints; +} + +NonlinearFactorGraph cost = GetCost(); +NonlinearEqualityConstraints e_constraints = GetEConstraints(); +} // namespace i_constrained_example diff --git a/gtsam/constraint/tests/testNonlinearEqualityConstraint.cpp b/gtsam/constraint/tests/testNonlinearEqualityConstraint.cpp new file mode 100644 index 000000000..ca2725d4c --- /dev/null +++ b/gtsam/constraint/tests/testNonlinearEqualityConstraint.cpp @@ -0,0 +1,267 @@ +/* ---------------------------------------------------------------------------- + + * 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 testEqualityConstraint.cpp + * @brief Equality constraints in constrained optimization. + * @author Yetong Zhang + * @date Aug 3, 2024 + */ + +#include +#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/constraint/tests/testNonlinearInequalityConstraint.cpp b/gtsam/constraint/tests/testNonlinearInequalityConstraint.cpp new file mode 100644 index 000000000..a73367d68 --- /dev/null +++ b/gtsam/constraint/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 NonlinearInequalityConstraint.h + * @brief Nonlinear inequality constraints in constrained optimization. + * @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/constraint/tests/testSimple.cpp b/gtsam/constraint/tests/testSimple.cpp new file mode 100644 index 000000000..800f9ec6c --- /dev/null +++ b/gtsam/constraint/tests/testSimple.cpp @@ -0,0 +1,37 @@ +#include + +#include +#include +#include +#include +#include + +#include +#include + +// Define a functor class +class MyFunctor { +public: + // Overload the function call operator + void operator()(int x) const { + std::cout << "MyFunctor called with " << x << std::endl; + } +}; + + +TEST(InequalityConstraint, DoubleExpressionInequality) { + // Create an instance of the functor + MyFunctor functor; + + // Cast the functor to std::function + std::function func = functor; + + // Call the std::function object + func(42); +} + + +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} diff --git a/gtsam/nonlinear/Expression.h b/gtsam/nonlinear/Expression.h index e5017390d..aa65384c2 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 @@ protected: // 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;