From 59f6f081c59c81d76cddc7b8b5cef7a26766e868 Mon Sep 17 00:00:00 2001 From: yetongumich Date: Mon, 5 Aug 2024 18:02:34 -0400 Subject: [PATCH 01/12] add definition of constraints --- gtsam/CMakeLists.txt | 1 + gtsam/constraint/CMakeLists.txt | 6 + gtsam/constraint/NonlinearConstraint.h | 65 +++++ .../NonlinearEqualityConstraint-inl.h | 54 ++++ .../NonlinearEqualityConstraint.cpp | 83 ++++++ .../constraint/NonlinearEqualityConstraint.h | 120 ++++++++ .../NonlinearInequalityConstraint.cpp | 158 +++++++++++ .../NonlinearInequalityConstraint.h | 125 ++++++++ gtsam/constraint/RampFunction.cpp | 71 +++++ gtsam/constraint/RampFunction.h | 78 +++++ gtsam/constraint/tests/CMakeLists.txt | 9 + gtsam/constraint/tests/constrainedExample.h | 168 +++++++++++ .../tests/testNonlinearEqualityConstraint.cpp | 267 ++++++++++++++++++ .../testNonlinearInequalityConstraint.cpp | 117 ++++++++ gtsam/constraint/tests/testSimple.cpp | 37 +++ gtsam/nonlinear/Expression.h | 4 + 16 files changed, 1363 insertions(+) create mode 100644 gtsam/constraint/CMakeLists.txt create mode 100644 gtsam/constraint/NonlinearConstraint.h create mode 100644 gtsam/constraint/NonlinearEqualityConstraint-inl.h create mode 100644 gtsam/constraint/NonlinearEqualityConstraint.cpp create mode 100644 gtsam/constraint/NonlinearEqualityConstraint.h create mode 100644 gtsam/constraint/NonlinearInequalityConstraint.cpp create mode 100644 gtsam/constraint/NonlinearInequalityConstraint.h create mode 100644 gtsam/constraint/RampFunction.cpp create mode 100644 gtsam/constraint/RampFunction.h create mode 100644 gtsam/constraint/tests/CMakeLists.txt create mode 100644 gtsam/constraint/tests/constrainedExample.h create mode 100644 gtsam/constraint/tests/testNonlinearEqualityConstraint.cpp create mode 100644 gtsam/constraint/tests/testNonlinearInequalityConstraint.cpp create mode 100644 gtsam/constraint/tests/testSimple.cpp diff --git a/gtsam/CMakeLists.txt b/gtsam/CMakeLists.txt index 1fc8e45707..dcc350fe85 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 0000000000..e4bbd89dd6 --- /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 0000000000..d0e7aef6b3 --- /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 0000000000..a594d0b095 --- /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 0000000000..06201dc2a1 --- /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 0000000000..6d3853030f --- /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 0000000000..f02fd8648b --- /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 0000000000..7399b30e8b --- /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 0000000000..601416afd7 --- /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 0000000000..d04e2dec09 --- /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 0000000000..4f364508ca --- /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 0000000000..a8aafb9814 --- /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 0000000000..ca2725d4c4 --- /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 0000000000..a73367d689 --- /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 0000000000..800f9ec6c3 --- /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 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; From ba50bd7ad170fdef6c0737d9f858cade25a55dce Mon Sep 17 00:00:00 2001 From: yetongumich Date: Tue, 6 Aug 2024 10:41:18 -0400 Subject: [PATCH 02/12] add serialization --- gtsam/constraint/NonlinearConstraint.h | 19 ++++++- .../constraint/NonlinearEqualityConstraint.h | 51 +++++++++++++++++++ .../NonlinearInequalityConstraint.h | 35 +++++++++++++ 3 files changed, 103 insertions(+), 2 deletions(-) diff --git a/gtsam/constraint/NonlinearConstraint.h b/gtsam/constraint/NonlinearConstraint.h index d0e7aef6b3..00bf4d52b9 100644 --- a/gtsam/constraint/NonlinearConstraint.h +++ b/gtsam/constraint/NonlinearConstraint.h @@ -19,7 +19,9 @@ #pragma once #include -#include "gtsam/linear/NoiseModel.h" +#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#include +#endif namespace gtsam { @@ -40,7 +42,9 @@ class NonlinearConstraint : public NoiseModelFactor { virtual ~NonlinearConstraint() {} /** Create a cost factor representing the L2 penalty function with scaling coefficient mu. */ - virtual NoiseModelFactor::shared_ptr penaltyFactor(const double mu = 1.0) const = 0; + virtual NoiseModelFactor::shared_ptr penaltyFactor(const double mu = 1.0) const { + return cloneWithNewNoiseModel(penaltyNoise(mu)); + } /** Return the norm of the constraint violation vector. */ virtual double violation(const Values& x) const { return sqrt(2 * error(x)); } @@ -60,6 +64,17 @@ class NonlinearConstraint : public NoiseModelFactor { static SharedNoiseModel constrainedNoise(const Vector& sigmas) { return noiseModel::Constrained::MixedSigmas(1.0, sigmas); } + + private: +#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE& ar, const unsigned int /*version*/) { + ar& boost::serialization::make_nvp("NonlinearConstraint", + boost::serialization::base_object(*this)); + } +#endif }; } // namespace gtsam diff --git a/gtsam/constraint/NonlinearEqualityConstraint.h b/gtsam/constraint/NonlinearEqualityConstraint.h index 6d3853030f..9f2305fa09 100644 --- a/gtsam/constraint/NonlinearEqualityConstraint.h +++ b/gtsam/constraint/NonlinearEqualityConstraint.h @@ -20,6 +20,9 @@ #include #include #include +#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#include +#endif namespace gtsam { @@ -37,6 +40,17 @@ class NonlinearEqualityConstraint : public NonlinearConstraint { /** Destructor. */ virtual ~NonlinearEqualityConstraint() {} + + private: +#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE& ar, const unsigned int /*version*/) { + ar& boost::serialization::make_nvp("NonlinearEqualityConstraint", + boost::serialization::base_object(*this)); + } +#endif }; /** Equality constraint that force g(x) = M. */ @@ -66,6 +80,20 @@ class ExpressionEqualityConstraint : public NonlinearEqualityConstraint { virtual NoiseModelFactor::shared_ptr penaltyFactor(const double mu = 1.0) const override; const Expression& expression() const { return expression_; } + + private: +#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE& ar, const unsigned int /*version*/) { + ar& boost::serialization::make_nvp("ExpressionEqualityConstraint", + boost::serialization::base_object(*this)); + ar& BOOST_SERIALIZATION_NVP(expression_); + ar& BOOST_SERIALIZATION_NVP(rhs_); + ar& BOOST_SERIALIZATION_NVP(dims_); + } +#endif }; /** Equality constraint that enforce the cost factor with zero error. */ @@ -90,6 +118,18 @@ class ZeroCostConstraint : public NonlinearEqualityConstraint { virtual Vector unwhitenedError(const Values& x, OptionalMatrixVecType H = nullptr) const override; virtual NoiseModelFactor::shared_ptr penaltyFactor(const double mu = 1.0) const override; + + private: +#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE& ar, const unsigned int /*version*/) { + ar& boost::serialization::make_nvp("ZeroCostConstraint", + boost::serialization::base_object(*this)); + ar& BOOST_SERIALIZATION_NVP(factor_); + } +#endif }; /// Container of NonlinearEqualityConstraint. @@ -113,6 +153,17 @@ class NonlinearEqualityConstraints : public FactorGraph + void serialize(ARCHIVE& ar, const unsigned int /*version*/) { + ar& boost::serialization::make_nvp("NonlinearEqualityConstraints", + boost::serialization::base_object(*this)); + } +#endif }; } // namespace gtsam diff --git a/gtsam/constraint/NonlinearInequalityConstraint.h b/gtsam/constraint/NonlinearInequalityConstraint.h index 7399b30e8b..ac22df9434 100644 --- a/gtsam/constraint/NonlinearInequalityConstraint.h +++ b/gtsam/constraint/NonlinearInequalityConstraint.h @@ -53,6 +53,17 @@ class NonlinearInequalityConstraint : public NonlinearConstraint { /** penalty function as if the constraint is equality, 0.5 * mu * ||g(x)||^2 */ virtual NoiseModelFactor::shared_ptr penaltyFactorEquality(const double mu = 1.0) const; + + private: +#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE& ar, const unsigned int /*version*/) { + ar& boost::serialization::make_nvp("NonlinearInequalityConstraint", + boost::serialization::base_object(*this)); + } +#endif }; /** Inequality constraint that force g(x) <= 0, where g(x) is a scalar-valued @@ -96,6 +107,19 @@ class ScalarExpressionInequalityConstraint : public NonlinearInequalityConstrain virtual NoiseModelFactor::shared_ptr penaltyFactorEquality(const double mu = 1.0) const override; const Double_& expression() const { return expression_; } + + private: +#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE& ar, const unsigned int /*version*/) { + ar& boost::serialization::make_nvp("ExpressionEqualityConstraint", + boost::serialization::base_object(*this)); + ar& BOOST_SERIALIZATION_NVP(expression_); + ar& BOOST_SERIALIZATION_NVP(dims_); + } +#endif }; /// Container of NonlinearInequalityConstraint. @@ -120,6 +144,17 @@ class NonlinearInequalityConstraints : public FactorGraph + void serialize(ARCHIVE& ar, const unsigned int /*version*/) { + ar& boost::serialization::make_nvp("NonlinearInequalityConstraints", + boost::serialization::base_object(*this)); + } +#endif }; } // namespace gtsam From 80fcff1ed5af30fcbeeee902c47f0ef4bed1512f Mon Sep 17 00:00:00 2001 From: yetongumich Date: Tue, 6 Aug 2024 10:42:38 -0400 Subject: [PATCH 03/12] remove accidentally added file --- gtsam/constraint/tests/testSimple.cpp | 37 --------------------------- 1 file changed, 37 deletions(-) delete mode 100644 gtsam/constraint/tests/testSimple.cpp diff --git a/gtsam/constraint/tests/testSimple.cpp b/gtsam/constraint/tests/testSimple.cpp deleted file mode 100644 index 800f9ec6c3..0000000000 --- a/gtsam/constraint/tests/testSimple.cpp +++ /dev/null @@ -1,37 +0,0 @@ -#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); -} From 9b0fa1210e9cadb2d06f3dffc30b2712dd3aed1c Mon Sep 17 00:00:00 2001 From: yetongumich Date: Tue, 6 Aug 2024 15:42:09 -0400 Subject: [PATCH 04/12] clean up and refactor NonlinearEquality and BoundingConstraint --- gtsam/constraint/NonlinearConstraint.h | 2 +- .../NonlinearEqualityConstraint-inl.h | 4 +- .../NonlinearEqualityConstraint.cpp | 4 +- .../constraint/NonlinearEqualityConstraint.h | 19 ++- .../NonlinearInequalityConstraint.cpp | 17 ++- .../NonlinearInequalityConstraint.h | 40 ++++-- gtsam/constraint/RampFunction.cpp | 32 ++++- gtsam/constraint/RampFunction.h | 53 ++++++-- .../tests/testNonlinearEqualityConstraint.cpp | 4 +- .../testNonlinearInequalityConstraint.cpp | 4 +- gtsam/constraint/tests/testRampFunction.cpp | 124 ++++++++++++++++++ gtsam/nonlinear/NonlinearEquality.h | 74 +++++++---- gtsam/slam/BoundingConstraint.h | 65 +++++---- tests/testBoundingConstraint.cpp | 14 +- 14 files changed, 361 insertions(+), 95 deletions(-) create mode 100644 gtsam/constraint/tests/testRampFunction.cpp diff --git a/gtsam/constraint/NonlinearConstraint.h b/gtsam/constraint/NonlinearConstraint.h index 00bf4d52b9..834ef79910 100644 --- a/gtsam/constraint/NonlinearConstraint.h +++ b/gtsam/constraint/NonlinearConstraint.h @@ -11,7 +11,7 @@ /** * @file NonlinearConstraint.h - * @brief Equality constraints in constrained optimization. + * @brief Nonlinear constraints in constrained optimization. * @author Yetong Zhang, Frank Dellaert * @date Aug 3, 2024 */ diff --git a/gtsam/constraint/NonlinearEqualityConstraint-inl.h b/gtsam/constraint/NonlinearEqualityConstraint-inl.h index a594d0b095..625f1e3663 100644 --- a/gtsam/constraint/NonlinearEqualityConstraint-inl.h +++ b/gtsam/constraint/NonlinearEqualityConstraint-inl.h @@ -10,8 +10,8 @@ * -------------------------------------------------------------------------- */ /** - * @file EqualityConstraint-inl.h - * @brief Equality constraints in constrained optimization. + * @file NonlinearEqualityConstraint-inl.h + * @brief Nonlinear equality constraints in constrained optimization. * @author Yetong Zhang, Frank Dellaert * @date Aug 3, 2024 */ diff --git a/gtsam/constraint/NonlinearEqualityConstraint.cpp b/gtsam/constraint/NonlinearEqualityConstraint.cpp index 06201dc2a1..0c5b7ea11c 100644 --- a/gtsam/constraint/NonlinearEqualityConstraint.cpp +++ b/gtsam/constraint/NonlinearEqualityConstraint.cpp @@ -10,8 +10,8 @@ * -------------------------------------------------------------------------- */ /** - * @file EqualityConstraint.cpp - * @brief Equality constraints in constrained optimization. + * @file NonlinearEqualityConstraint.cpp + * @brief Nonlinear equality constraints in constrained optimization. * @author Yetong Zhang, Frank Dellaert * @date Aug 3, 2024 */ diff --git a/gtsam/constraint/NonlinearEqualityConstraint.h b/gtsam/constraint/NonlinearEqualityConstraint.h index 9f2305fa09..ab7dca2f0d 100644 --- a/gtsam/constraint/NonlinearEqualityConstraint.h +++ b/gtsam/constraint/NonlinearEqualityConstraint.h @@ -10,8 +10,8 @@ * -------------------------------------------------------------------------- */ /** - * @file EqualityConstraint.h - * @brief Equality constraints in constrained optimization. + * @file NonlinearEqualityConstraint.h + * @brief Nonlinear equality constraints in constrained optimization. * @author Yetong Zhang, Frank Dellaert * @date Aug 3, 2024 */ @@ -20,9 +20,6 @@ #include #include #include -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION -#include -#endif namespace gtsam { @@ -81,6 +78,12 @@ class ExpressionEqualityConstraint : public NonlinearEqualityConstraint { 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 */ @@ -119,6 +122,12 @@ class ZeroCostConstraint : public NonlinearEqualityConstraint { 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 */ diff --git a/gtsam/constraint/NonlinearInequalityConstraint.cpp b/gtsam/constraint/NonlinearInequalityConstraint.cpp index f02fd8648b..49003f09f3 100644 --- a/gtsam/constraint/NonlinearInequalityConstraint.cpp +++ b/gtsam/constraint/NonlinearInequalityConstraint.cpp @@ -17,8 +17,7 @@ */ #include -#include -#include "gtsam/constraint/RampFunction.h" +#include namespace gtsam { @@ -44,6 +43,20 @@ bool NonlinearInequalityConstraint::active(const Values& x) const { return (unwhitenedExpr(x).array() >= 0).any(); } +/* ************************************************************************* */ +NoiseModelFactor::shared_ptr NonlinearInequalityConstraint::penaltyFactorSmooth( + SmoothRampFunction::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 { diff --git a/gtsam/constraint/NonlinearInequalityConstraint.h b/gtsam/constraint/NonlinearInequalityConstraint.h index ac22df9434..1a1813ea37 100644 --- a/gtsam/constraint/NonlinearInequalityConstraint.h +++ b/gtsam/constraint/NonlinearInequalityConstraint.h @@ -25,7 +25,7 @@ namespace gtsam { /** - * Inequality constraint base class. + * Inequality constraint base class, enforcing g(x) <= 0. */ class NonlinearInequalityConstraint : public NonlinearConstraint { public: @@ -39,17 +39,21 @@ class NonlinearInequalityConstraint : public NonlinearConstraint { /** Destructor. */ virtual ~NonlinearInequalityConstraint() {} + /** Return g(x). */ virtual Vector unwhitenedExpr(const Values& x, OptionalMatrixVecType H = nullptr) const = 0; + /** 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; - virtual NonlinearEqualityConstraint::shared_ptr createEqualityConstraint() const = 0; + /** Return an equality constraint corresponding to g(x)=0. */ + virtual NonlinearEqualityConstraint::shared_ptr createEqualityConstraint() const; /** Smooth approximation of the ramp function. */ virtual NoiseModelFactor::shared_ptr penaltyFactorSmooth(SmoothRampFunction::shared_ptr func, - const double mu = 1.0) const = 0; + 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; @@ -67,7 +71,8 @@ class NonlinearInequalityConstraint : public NonlinearConstraint { }; /** Inequality constraint that force g(x) <= 0, where g(x) is a scalar-valued - * function. */ + * nonlinear function. + */ class ScalarExpressionInequalityConstraint : public NonlinearInequalityConstraint { public: typedef NonlinearInequalityConstraint Base; @@ -82,32 +87,45 @@ class ScalarExpressionInequalityConstraint : public NonlinearInequalityConstrain /** * @brief Constructor. * - * @param expression expression representing g(x). + * @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)>=0. + /** 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)<=0. + /** 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 penaltyFactorSmooth(SmoothRampFunction::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 */ @@ -131,17 +149,19 @@ class NonlinearInequalityConstraints : public FactorGraph H) const { +double RampFunctionPoly2::operator()(const double& x, OptionalJacobian<1, 1> H) const { if (x <= 0) { if (H) { H->setZero(); @@ -49,9 +49,29 @@ double PolynomialRampFunction::operator()(const double& x, OptionalJacobian<1, 1 return 0; } else if (x < epsilon_) { if (H) { - H->setConstant(x / epsilon_); + H->setConstant(2 * a_ * x); } - return (x * x) / (2 * epsilon_) + epsilon_ / 2; + return a_ * pow(x, 2); + } else { + if (H) { + H->setOnes(); + } + return x - epsilon_ / 2; + } +} + +/* ************************************************************************* */ +double RampFunctionPoly3::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(); diff --git a/gtsam/constraint/RampFunction.h b/gtsam/constraint/RampFunction.h index d04e2dec09..0722551c5d 100644 --- a/gtsam/constraint/RampFunction.h +++ b/gtsam/constraint/RampFunction.h @@ -10,9 +10,9 @@ * -------------------------------------------------------------------------- */ /** - * @file NonlinearInequalityConstraint.h - * @brief Nonlinear inequality constraints in constrained optimization. - * @author Yetong Zhang, Frank Dellaert + * @file RampFunction.h + * @brief Ramp function to compute inequality constraint violations. + * @author Yetong Zhang * @date Aug 3, 2024 */ @@ -20,10 +20,14 @@ #include #include +#include namespace gtsam { -/// Ramp function for create penalty factors. +/** Ramp function f : R -> R. + * f(x) = 0 for x <= 0 + * x for x > 0 + */ double RampFunction(const double x, OptionalJacobian<1, 1> H = {}); /** Base class for smooth approximation of the ramp function. */ @@ -43,20 +47,51 @@ class SmoothRampFunction { UnaryScalarFunc function() const; }; -/** Ramp function approximated with a polynomial of degree 2. +/** 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 RampFunctionPoly2 : public SmoothRampFunction { + public: + typedef SmoothRampFunction Base; + typedef RampFunctionPoly2 This; + typedef std::shared_ptr shared_ptr; + + protected: + double epsilon_; + double a_; + + public: + RampFunctionPoly2(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 - * x^2/(2*e) + e/2 for 0 < x < epsilon + * a * x^3 + b * x^2 for 0 < x < epsilon * x for x >= epsilon */ -class PolynomialRampFunction : public SmoothRampFunction { +class RampFunctionPoly3 : public SmoothRampFunction { public: typedef SmoothRampFunction Base; + typedef RampFunctionPoly3 This; + typedef std::shared_ptr shared_ptr; protected: double epsilon_; + double a_; + double b_; public: - PolynomialRampFunction(const double epsilon = 1) : Base(), epsilon_(epsilon) {} + RampFunctionPoly3(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; }; @@ -65,6 +100,8 @@ class PolynomialRampFunction : public SmoothRampFunction { class SoftPlusFunction : public SmoothRampFunction { public: typedef SmoothRampFunction Base; + typedef SoftPlusFunction This; + typedef std::shared_ptr shared_ptr; protected: double k_; diff --git a/gtsam/constraint/tests/testNonlinearEqualityConstraint.cpp b/gtsam/constraint/tests/testNonlinearEqualityConstraint.cpp index ca2725d4c4..8309de0b33 100644 --- a/gtsam/constraint/tests/testNonlinearEqualityConstraint.cpp +++ b/gtsam/constraint/tests/testNonlinearEqualityConstraint.cpp @@ -10,8 +10,8 @@ * -------------------------------------------------------------------------- */ /** - * @file testEqualityConstraint.cpp - * @brief Equality constraints in constrained optimization. + * @file testNonlinearEqualityConstraint.cpp + * @brief unit tests for nonlinear equality constraints * @author Yetong Zhang * @date Aug 3, 2024 */ diff --git a/gtsam/constraint/tests/testNonlinearInequalityConstraint.cpp b/gtsam/constraint/tests/testNonlinearInequalityConstraint.cpp index a73367d689..3950bfa03b 100644 --- a/gtsam/constraint/tests/testNonlinearInequalityConstraint.cpp +++ b/gtsam/constraint/tests/testNonlinearInequalityConstraint.cpp @@ -10,8 +10,8 @@ * -------------------------------------------------------------------------- */ /** - * @file NonlinearInequalityConstraint.h - * @brief Nonlinear inequality constraints in constrained optimization. + * @file testNonlinearInequalityConstraint.h + * @brief unit tests for nonlinear inequality constraints * @author Yetong Zhang * @date Aug 3, 2024 */ diff --git a/gtsam/constraint/tests/testRampFunction.cpp b/gtsam/constraint/tests/testRampFunction.cpp new file mode 100644 index 0000000000..2e8036d746 --- /dev/null +++ b/gtsam/constraint/tests/testRampFunction.cpp @@ -0,0 +1,124 @@ +/* ---------------------------------------------------------------------------- + + * 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 testRampFunction.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(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(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. + RampFunctionPoly2 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. + RampFunctionPoly3 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/nonlinear/NonlinearEquality.h b/gtsam/nonlinear/NonlinearEquality.h index f81486b301..c91bddd9cb 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/slam/BoundingConstraint.h b/gtsam/slam/BoundingConstraint.h index a496971e28..d35152adfa 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 = nullptr) 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 = nullptr) 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 = nullptr) 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 = nullptr, OptionalMatrixType H2 = nullptr) 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); } From 6caf0a364224355acf0c2a7a2a5e747d448bac91 Mon Sep 17 00:00:00 2001 From: yetongumich Date: Mon, 26 Aug 2024 15:23:39 -0400 Subject: [PATCH 05/12] add GTSAM_EXPORT and refactor --- ...tion.cpp => InequalityPenaltyFunction.cpp} | 28 ++-- ...Function.h => InequalityPenaltyFunction.h} | 83 +++++++---- gtsam/constraint/NonlinearConstraint.h | 9 +- .../NonlinearEqualityConstraint-inl.h | 6 +- .../NonlinearEqualityConstraint.cpp | 16 +- .../constraint/NonlinearEqualityConstraint.h | 8 +- .../NonlinearInequalityConstraint.cpp | 61 ++++---- .../NonlinearInequalityConstraint.h | 23 +-- gtsam/constraint/tests/constrainedExample.h | 137 ++++++++++-------- ....cpp => testInequalityPenaltyFunction.cpp} | 16 +- .../tests/testNonlinearEqualityConstraint.cpp | 10 +- 11 files changed, 224 insertions(+), 173 deletions(-) rename gtsam/constraint/{RampFunction.cpp => InequalityPenaltyFunction.cpp} (75%) rename gtsam/constraint/{RampFunction.h => InequalityPenaltyFunction.h} (51%) rename gtsam/constraint/tests/{testRampFunction.cpp => testInequalityPenaltyFunction.cpp} (84%) diff --git a/gtsam/constraint/RampFunction.cpp b/gtsam/constraint/InequalityPenaltyFunction.cpp similarity index 75% rename from gtsam/constraint/RampFunction.cpp rename to gtsam/constraint/InequalityPenaltyFunction.cpp index 64038a3253..8b048686ff 100644 --- a/gtsam/constraint/RampFunction.cpp +++ b/gtsam/constraint/InequalityPenaltyFunction.cpp @@ -10,18 +10,23 @@ * -------------------------------------------------------------------------- */ /** - * @file RampFunction.cpp + * @file InequalityPenaltyFunction.cpp * @brief Ramp function to compute inequality constraint violations. * @author Yetong Zhang * @date Aug 3, 2024 */ -#include +#include namespace gtsam { -/* ************************************************************************* */ -double RampFunction(const double x, OptionalJacobian<1, 1> H) { +/* ********************************************************************************************* */ +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); @@ -35,13 +40,8 @@ double RampFunction(const double x, OptionalJacobian<1, 1> H) { } } -/* ************************************************************************* */ -SmoothRampFunction::UnaryScalarFunc SmoothRampFunction::function() const { - return [=](const double& x, OptionalJacobian<1, 1> H = {}) -> double { return (*this)(x, H); }; -} - -/* ************************************************************************* */ -double RampFunctionPoly2::operator()(const double& x, OptionalJacobian<1, 1> H) const { +/* ********************************************************************************************* */ +double SmoothRampPoly2::operator()(const double& x, OptionalJacobian<1, 1> H) const { if (x <= 0) { if (H) { H->setZero(); @@ -60,8 +60,8 @@ double RampFunctionPoly2::operator()(const double& x, OptionalJacobian<1, 1> H) } } -/* ************************************************************************* */ -double RampFunctionPoly3::operator()(const double& x, OptionalJacobian<1, 1> H) const { +/* ********************************************************************************************* */ +double SmoothRampPoly3::operator()(const double& x, OptionalJacobian<1, 1> H) const { if (x <= 0) { if (H) { H->setZero(); @@ -80,7 +80,7 @@ double RampFunctionPoly3::operator()(const double& x, OptionalJacobian<1, 1> H) } } -/* ************************************************************************* */ +/* ********************************************************************************************* */ double SoftPlusFunction::operator()(const double& x, OptionalJacobian<1, 1> H) const { if (H) { H->setConstant(1 / (1 + std::exp(-k_ * x))); diff --git a/gtsam/constraint/RampFunction.h b/gtsam/constraint/InequalityPenaltyFunction.h similarity index 51% rename from gtsam/constraint/RampFunction.h rename to gtsam/constraint/InequalityPenaltyFunction.h index 0722551c5d..baafaef0bf 100644 --- a/gtsam/constraint/RampFunction.h +++ b/gtsam/constraint/InequalityPenaltyFunction.h @@ -10,7 +10,7 @@ * -------------------------------------------------------------------------- */ /** - * @file RampFunction.h + * @file InequalityPenaltyFunction.h * @brief Ramp function to compute inequality constraint violations. * @author Yetong Zhang * @date Aug 3, 2024 @@ -20,31 +20,49 @@ #include #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 */ -double RampFunction(const double x, OptionalJacobian<1, 1> H = {}); - -/** Base class for smooth approximation of the ramp function. */ -class SmoothRampFunction { +class GTSAM_EXPORT RampFunction : public InequalityPenaltyFunction { public: - typedef std::shared_ptr shared_ptr; - typedef std::function H)> UnaryScalarFunc; + typedef InequalityPenaltyFunction Base; + typedef RampFunction This; + typedef std::shared_ptr shared_ptr; - /** Constructor. */ - SmoothRampFunction() {} + public: + RampFunction() : Base() {} - /** Destructor. */ - virtual ~SmoothRampFunction() {} + virtual double operator()(const double& x, + OptionalJacobian<1, 1> H = {}) const override { + return Ramp(x, H); + } - virtual double operator()(const double& x, OptionalJacobian<1, 1> H = {}) const = 0; + virtual UnaryScalarFunc function() const override { return Ramp; } - UnaryScalarFunc function() const; + static double Ramp(const double x, OptionalJacobian<1, 1> H = {}); }; /** Ramp function approximated with a polynomial of degree 2 in [0, epsilon]. @@ -54,10 +72,10 @@ class SmoothRampFunction { * a * x^2 for 0 < x < epsilon * x - epsilon/2 for x >= epsilon */ -class RampFunctionPoly2 : public SmoothRampFunction { +class GTSAM_EXPORT SmoothRampPoly2 : public InequalityPenaltyFunction { public: - typedef SmoothRampFunction Base; - typedef RampFunctionPoly2 This; + typedef InequalityPenaltyFunction Base; + typedef SmoothRampPoly2 This; typedef std::shared_ptr shared_ptr; protected: @@ -65,9 +83,11 @@ class RampFunctionPoly2 : public SmoothRampFunction { double a_; public: - RampFunctionPoly2(const double epsilon = 1) : Base(), epsilon_(epsilon), a_(0.5 / epsilon) {} + SmoothRampPoly2(const double epsilon = 1) + : Base(), epsilon_(epsilon), a_(0.5 / epsilon) {} - virtual double operator()(const double& x, OptionalJacobian<1, 1> H = {}) const override; + virtual double operator()(const double& x, + OptionalJacobian<1, 1> H = {}) const override; }; /** Ramp function approximated with a polynomial of degree 3 in [0, epsilon]. @@ -78,10 +98,10 @@ class RampFunctionPoly2 : public SmoothRampFunction { * a * x^3 + b * x^2 for 0 < x < epsilon * x for x >= epsilon */ -class RampFunctionPoly3 : public SmoothRampFunction { +class GTSAM_EXPORT SmoothRampPoly3 : public InequalityPenaltyFunction { public: - typedef SmoothRampFunction Base; - typedef RampFunctionPoly3 This; + typedef InequalityPenaltyFunction Base; + typedef SmoothRampPoly3 This; typedef std::shared_ptr shared_ptr; protected: @@ -90,16 +110,20 @@ class RampFunctionPoly3 : public SmoothRampFunction { double b_; public: - RampFunctionPoly3(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; + 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 SoftPlusFunction : public SmoothRampFunction { +class GTSAM_EXPORT SoftPlusFunction : public InequalityPenaltyFunction { public: - typedef SmoothRampFunction Base; + typedef InequalityPenaltyFunction Base; typedef SoftPlusFunction This; typedef std::shared_ptr shared_ptr; @@ -109,7 +133,8 @@ class SoftPlusFunction : public SmoothRampFunction { public: SoftPlusFunction(const double k = 1) : Base(), k_(k) {} - virtual double operator()(const double& x, OptionalJacobian<1, 1> H = {}) const override; + virtual double operator()(const double& x, + OptionalJacobian<1, 1> H = {}) const override; }; } // namespace gtsam diff --git a/gtsam/constraint/NonlinearConstraint.h b/gtsam/constraint/NonlinearConstraint.h index 834ef79910..5588188037 100644 --- a/gtsam/constraint/NonlinearConstraint.h +++ b/gtsam/constraint/NonlinearConstraint.h @@ -31,7 +31,7 @@ namespace gtsam { * whitenedError() returns The constraint violation vector. * unwhitenedError() returns the sigma-scaled constraint violation vector. */ -class NonlinearConstraint : public NoiseModelFactor { +class GTSAM_EXPORT NonlinearConstraint : public NoiseModelFactor { public: typedef NoiseModelFactor Base; @@ -54,6 +54,13 @@ class NonlinearConstraint : public NoiseModelFactor { 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 { diff --git a/gtsam/constraint/NonlinearEqualityConstraint-inl.h b/gtsam/constraint/NonlinearEqualityConstraint-inl.h index 625f1e3663..c228cf97d4 100644 --- a/gtsam/constraint/NonlinearEqualityConstraint-inl.h +++ b/gtsam/constraint/NonlinearEqualityConstraint-inl.h @@ -21,7 +21,7 @@ namespace gtsam { -/* ************************************************************************* */ +/* ********************************************************************************************* */ template ExpressionEqualityConstraint::ExpressionEqualityConstraint(const Expression& expression, const T& rhs, @@ -31,7 +31,7 @@ ExpressionEqualityConstraint::ExpressionEqualityConstraint(const Expression Vector ExpressionEqualityConstraint::unwhitenedError(const Values& x, OptionalMatrixVecType H) const { @@ -45,7 +45,7 @@ Vector ExpressionEqualityConstraint::unwhitenedError(const Values& x, } } -/* ************************************************************************* */ +/* ********************************************************************************************* */ template NoiseModelFactor::shared_ptr ExpressionEqualityConstraint::penaltyFactor(const double mu) const { return std::make_shared>(penaltyNoise(mu), rhs_, expression_); diff --git a/gtsam/constraint/NonlinearEqualityConstraint.cpp b/gtsam/constraint/NonlinearEqualityConstraint.cpp index 0c5b7ea11c..8705f7db93 100644 --- a/gtsam/constraint/NonlinearEqualityConstraint.cpp +++ b/gtsam/constraint/NonlinearEqualityConstraint.cpp @@ -19,21 +19,21 @@ 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; @@ -44,7 +44,7 @@ NonlinearEqualityConstraints NonlinearEqualityConstraints::FromCostGraph( return constraints; } -/* ************************************************************************* */ +/* ********************************************************************************************* */ size_t NonlinearEqualityConstraints::dim() const { size_t dimension = 0; for (const auto& constraint : *this) { @@ -53,7 +53,7 @@ size_t NonlinearEqualityConstraints::dim() const { return dimension; } -/* ************************************************************************* */ +/* ********************************************************************************************* */ Vector NonlinearEqualityConstraints::violationVector(const Values& values, bool whiten) const { Vector violation(dim()); size_t start_idx = 0; @@ -66,12 +66,12 @@ Vector NonlinearEqualityConstraints::violationVector(const Values& values, bool 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) { diff --git a/gtsam/constraint/NonlinearEqualityConstraint.h b/gtsam/constraint/NonlinearEqualityConstraint.h index ab7dca2f0d..532c3ab382 100644 --- a/gtsam/constraint/NonlinearEqualityConstraint.h +++ b/gtsam/constraint/NonlinearEqualityConstraint.h @@ -26,7 +26,7 @@ namespace gtsam { /** * Equality constraint base class. */ -class NonlinearEqualityConstraint : public NonlinearConstraint { +class GTSAM_EXPORT NonlinearEqualityConstraint : public NonlinearConstraint { public: typedef NonlinearConstraint Base; typedef NonlinearEqualityConstraint This; @@ -52,7 +52,7 @@ class NonlinearEqualityConstraint : public NonlinearConstraint { /** Equality constraint that force g(x) = M. */ template -class ExpressionEqualityConstraint : public NonlinearEqualityConstraint { +class GTSAM_EXPORT ExpressionEqualityConstraint : public NonlinearEqualityConstraint { public: typedef NonlinearEqualityConstraint Base; typedef ExpressionEqualityConstraint This; @@ -100,7 +100,7 @@ class ExpressionEqualityConstraint : public NonlinearEqualityConstraint { }; /** Equality constraint that enforce the cost factor with zero error. */ -class ZeroCostConstraint : public NonlinearEqualityConstraint { +class GTSAM_EXPORT ZeroCostConstraint : public NonlinearEqualityConstraint { public: typedef NonlinearEqualityConstraint Base; typedef ZeroCostConstraint This; @@ -142,7 +142,7 @@ class ZeroCostConstraint : public NonlinearEqualityConstraint { }; /// Container of NonlinearEqualityConstraint. -class NonlinearEqualityConstraints : public FactorGraph { +class GTSAM_EXPORT NonlinearEqualityConstraints : public FactorGraph { public: typedef std::shared_ptr shared_ptr; typedef FactorGraph Base; diff --git a/gtsam/constraint/NonlinearInequalityConstraint.cpp b/gtsam/constraint/NonlinearInequalityConstraint.cpp index 49003f09f3..3404a143c7 100644 --- a/gtsam/constraint/NonlinearInequalityConstraint.cpp +++ b/gtsam/constraint/NonlinearInequalityConstraint.cpp @@ -17,11 +17,15 @@ */ #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); @@ -38,52 +42,52 @@ Vector NonlinearInequalityConstraint::unwhitenedError(const Values& x, return error; } -/* ************************************************************************* */ +/* ********************************************************************************************* */ bool NonlinearInequalityConstraint::active(const Values& x) const { return (unwhitenedExpr(x).array() >= 0).any(); } -/* ************************************************************************* */ -NoiseModelFactor::shared_ptr NonlinearInequalityConstraint::penaltyFactorSmooth( - SmoothRampFunction::shared_ptr func, const double mu) const { +/* ********************************************************************************************* */ +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. @@ -94,35 +98,38 @@ Vector ScalarExpressionInequalityConstraint::unwhitenedExpr(const Values& 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_); + Double_ penalty_expression(RampFunction::Ramp, expression_); return std::make_shared>(penaltyNoise(mu), 0.0, penalty_expression); } -/* ************************************************************************* */ -NoiseModelFactor::shared_ptr ScalarExpressionInequalityConstraint::penaltyFactorSmooth( - SmoothRampFunction::shared_ptr func, const double mu) const { +/* ********************************************************************************************* */ +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) { @@ -131,7 +138,7 @@ size_t NonlinearInequalityConstraints::dim() const { return dimension; } -/* ************************************************************************* */ +/* ********************************************************************************************* */ Vector NonlinearInequalityConstraints::violationVector(const Values& values, bool whiten) const { Vector violation(dim()); size_t start_idx = 0; @@ -144,12 +151,12 @@ Vector NonlinearInequalityConstraints::violationVector(const Values& values, boo 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) { @@ -158,12 +165,12 @@ NonlinearFactorGraph NonlinearInequalityConstraints::penaltyGraph(const double m return graph; } -/* ************************************************************************* */ -NonlinearFactorGraph NonlinearInequalityConstraints::penaltyGraphSmooth( - SmoothRampFunction::shared_ptr func, const double mu) const { +/* ********************************************************************************************* */ +NonlinearFactorGraph NonlinearInequalityConstraints::penaltyGraphCustom( + InequalityPenaltyFunction::shared_ptr func, const double mu) const { NonlinearFactorGraph graph; for (const auto& constraint : *this) { - graph.add(constraint->penaltyFactorSmooth(func, mu)); + graph.add(constraint->penaltyFactorCustom(func, mu)); } return graph; } diff --git a/gtsam/constraint/NonlinearInequalityConstraint.h b/gtsam/constraint/NonlinearInequalityConstraint.h index 1a1813ea37..658a29f2f6 100644 --- a/gtsam/constraint/NonlinearInequalityConstraint.h +++ b/gtsam/constraint/NonlinearInequalityConstraint.h @@ -18,8 +18,8 @@ #pragma once +#include #include -#include #include namespace gtsam { @@ -27,7 +27,7 @@ namespace gtsam { /** * Inequality constraint base class, enforcing g(x) <= 0. */ -class NonlinearInequalityConstraint : public NonlinearConstraint { +class GTSAM_EXPORT NonlinearInequalityConstraint : public NonlinearConstraint { public: typedef NonlinearConstraint Base; typedef NonlinearInequalityConstraint This; @@ -42,6 +42,8 @@ class NonlinearInequalityConstraint : public NonlinearConstraint { /** 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; @@ -51,9 +53,9 @@ class NonlinearInequalityConstraint : public NonlinearConstraint { /** Return an equality constraint corresponding to g(x)=0. */ virtual NonlinearEqualityConstraint::shared_ptr createEqualityConstraint() const; - /** Smooth approximation of the ramp function. */ - virtual NoiseModelFactor::shared_ptr penaltyFactorSmooth(SmoothRampFunction::shared_ptr func, - const double mu = 1.0) 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; @@ -73,7 +75,7 @@ class NonlinearInequalityConstraint : public NonlinearConstraint { /** Inequality constraint that force g(x) <= 0, where g(x) is a scalar-valued * nonlinear function. */ -class ScalarExpressionInequalityConstraint : public NonlinearInequalityConstraint { +class GTSAM_EXPORT ScalarExpressionInequalityConstraint : public NonlinearInequalityConstraint { public: typedef NonlinearInequalityConstraint Base; typedef ScalarExpressionInequalityConstraint This; @@ -111,7 +113,7 @@ class ScalarExpressionInequalityConstraint : public NonlinearInequalityConstrain NoiseModelFactor::shared_ptr penaltyFactor(const double mu = 1.0) const override; /** Penalty function using a smooth approxiamtion of the ramp funciton. */ - NoiseModelFactor::shared_ptr penaltyFactorSmooth(SmoothRampFunction::shared_ptr func, + 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. */ @@ -141,7 +143,8 @@ class ScalarExpressionInequalityConstraint : public NonlinearInequalityConstrain }; /// Container of NonlinearInequalityConstraint. -class NonlinearInequalityConstraints : public FactorGraph { +class GTSAM_EXPORT NonlinearInequalityConstraints + : public FactorGraph { public: typedef FactorGraph Base; typedef NonlinearInequalityConstraints This; @@ -161,8 +164,8 @@ class NonlinearInequalityConstraints : public FactorGraph -#include -#include -#include -#include +#include #include #include -#include -#include - +#include "gtsam/constraint/NonlinearEqualityConstraint.h" +#include "gtsam/constraint/NonlinearInequalityConstraint.h" namespace constrained_example { using namespace gtsam; /// Exponential function e^x. -double exp_func(const double& x, gtsam::OptionalJacobian<1, 1> H1 = {}) { +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. -Double_ exp(const Double_& x) { return Double_(exp_func, x); } +inline Double_ exp(const Double_& x) { return Double_(exp_func, x); } /// Pow functor used for pow function. class PowFunctor { @@ -51,24 +46,23 @@ class PowFunctor { public: PowFunctor(const double& c) : c_(c) {} - double operator()(const double& x, - gtsam::OptionalJacobian<1, 1> H1 = {}) const { + 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) { +inline 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); } +inline 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; } +inline Double_ operator-(const Double_& x) { return Double_(0.0) - x; } /// Keys for creating expressions. Symbol x1_key('x', 1); @@ -82,11 +76,11 @@ Double_ x1(x1_key), x2(x2_key); * 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 + * h(x) = x1 + x1^3 + x2 + x2^2 = 0 */ -namespace e_constrained_example { +namespace constrained_example1 { using namespace constrained_example; -NonlinearFactorGraph GetCost() { +NonlinearFactorGraph Cost() { NonlinearFactorGraph graph; auto f1 = x1 + exp(-x2); auto f2 = pow(x1, 2.0) + 2.0 * x2 + 1.0; @@ -96,28 +90,49 @@ NonlinearFactorGraph GetCost() { return graph; } -NonlinearEqualityConstraints GetConstraints() { +NonlinearEqualityConstraints EqConstraints() { NonlinearEqualityConstraints constraints; - Vector scale = Vector1(0.1); + Vector sigmas = Vector1(1.0); auto h1 = x1 + pow(x1, 3) + x2 + pow(x2, 2); constraints.push_back(NonlinearEqualityConstraint::shared_ptr( - new ExpressionEqualityConstraint(h1, 0.0, scale))); + new ExpressionEqualityConstraint(h1, 0.0, sigmas))); return constraints; } -NonlinearFactorGraph cost = GetCost(); -NonlinearEqualityConstraints constraints = GetConstraints(); -} // namespace e_constrained_example +Values InitValues() { + Values values; + values.insert(x1_key, -0.2); + values.insert(x2_key, -0.2); + return values; +} + +Values OptimalValues() { + Values values; + values.insert(x1_key, 0.0); + values.insert(x2_key, 0.0); + return values; +} + +NonlinearFactorGraph costs = Cost(); +NonlinearEqualityConstraints e_constraints = EqConstraints(); +NonlinearInequalityConstraints i_constraints; +Values init_values = InitValues(); +ConstrainedOptProblem::shared_ptr problem = + std::make_shared(costs, e_constraints, i_constraints, init_values); +Values optimal_values = OptimalValues(); +} // namespace constrained_example1 /* ************************************************************************* */ /** * Constrained optimization example with inequality constraints + * Approach a point while staying on unit circle, and within an ellipse. * f(x) = 0.5 * ||x1-1||^2 + 0.5 * ||x2-1||^2 - * g(x) = 1 - x1^2 - x2^2 + * h(x) = x1^2 + x2^2 - 1 = 0 + * g(x) = 4*x1^2 + 0.25*x2^2 - 1 <= 0 */ -namespace i_constrained_example { +namespace constrained_example2 { using namespace constrained_example; -NonlinearFactorGraph GetCost() { +NonlinearFactorGraph Cost() { NonlinearFactorGraph graph; auto cost_noise = gtsam::noiseModel::Isotropic::Sigma(1, 1.0); graph.addPrior(x1_key, 1.0, cost_noise); @@ -125,44 +140,42 @@ NonlinearFactorGraph GetCost() { 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; -// } +NonlinearEqualityConstraints EqConstraints() { + NonlinearEqualityConstraints constraints; + Vector1 sigmas(1.0); + Double_ h1 = x1 * x1 + x2 * x2; + constraints.emplace_shared>(h1, 1.0, sigmas); + return constraints; +} -NonlinearFactorGraph cost = GetCost(); -NonlinearEqualityConstraints e_constraints; -// InequalityConstraints i_constraints = GetIConstraints(); -} // namespace i_constrained_example +NonlinearInequalityConstraints IneqConstraints() { + NonlinearInequalityConstraints constraints; + Double_ g1 = 4 * x1 * x1 + 0.25 * x2 * x2 - Double_(1.0); + double sigma = 1; + constraints.emplace_shared(g1, sigma); + return constraints; +} -/* ************************************************************************* */ -/** - * 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; +Values InitValues() { + Values values; + values.insert(x1_key, -1.0); + values.insert(x2_key, 2.0); + return values; } -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; +Values OptimalValues() { + Values values; + values.insert(x1_key, 1.0 / sqrt(5)); + values.insert(x2_key, 2.0 / sqrt(5)); + return values; } -NonlinearFactorGraph cost = GetCost(); -NonlinearEqualityConstraints e_constraints = GetEConstraints(); -} // namespace i_constrained_example +NonlinearFactorGraph costs = Cost(); +NonlinearEqualityConstraints e_constraints = EqConstraints(); +NonlinearInequalityConstraints i_constraints = IneqConstraints(); +Values init_values = InitValues(); +ConstrainedOptProblem::shared_ptr problem = + std::make_shared(costs, e_constraints, i_constraints, init_values); +Values optimal_values = OptimalValues(); + +} // namespace constrained_example2 diff --git a/gtsam/constraint/tests/testRampFunction.cpp b/gtsam/constraint/tests/testInequalityPenaltyFunction.cpp similarity index 84% rename from gtsam/constraint/tests/testRampFunction.cpp rename to gtsam/constraint/tests/testInequalityPenaltyFunction.cpp index 2e8036d746..09cd17d5bf 100644 --- a/gtsam/constraint/tests/testRampFunction.cpp +++ b/gtsam/constraint/tests/testInequalityPenaltyFunction.cpp @@ -10,7 +10,7 @@ * -------------------------------------------------------------------------- */ /** - * @file testRampFunction.h + * @file testInequalityPenaltyFunction.h * @brief unit tests for ramp functions * @author Yetong Zhang * @date Aug 3, 2024 @@ -20,13 +20,14 @@ #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(x); }; + 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}; @@ -35,7 +36,7 @@ TEST(RampFunction, error_and_jacobian) { for (size_t i = 0; i < x_vec.size(); i++) { double x = x_vec.at(i); Matrix H; - double r = RampFunction(x, H); + double r = RampFunction::Ramp(x, H); /// Check function evaluation. EXPECT_DOUBLES_EQUAL(expected_r_vec.at(i), r, 1e-9); @@ -48,9 +49,10 @@ TEST(RampFunction, error_and_jacobian) { } } +/* ********************************************************************************************* */ TEST(RampFunctionPoly2, error_and_jacobian) { /// Helper function for numerical Jacobian computation. - RampFunctionPoly2 p_ramp(2.0); + SmoothRampPoly2 p_ramp(2.0); auto ramp_helper = [&](const double& x) { return p_ramp(x); }; /// Create a set of values to test the function. @@ -71,9 +73,10 @@ TEST(RampFunctionPoly2, error_and_jacobian) { } } +/* ********************************************************************************************* */ TEST(RampFunctionPoly3, error_and_jacobian) { /// Helper function for numerical Jacobian computation. - RampFunctionPoly3 p_ramp(2.0); + SmoothRampPoly3 p_ramp(2.0); auto ramp_helper = [&](const double& x) { return p_ramp(x); }; /// Create a set of values to test the function. @@ -94,6 +97,7 @@ TEST(RampFunctionPoly3, error_and_jacobian) { } } +/* ********************************************************************************************* */ TEST(SoftPlusFunction, error_and_jacobian) { /// Helper function for numerical Jacobian computation. SoftPlusFunction soft_plus(0.5); diff --git a/gtsam/constraint/tests/testNonlinearEqualityConstraint.cpp b/gtsam/constraint/tests/testNonlinearEqualityConstraint.cpp index 8309de0b33..70dabff451 100644 --- a/gtsam/constraint/tests/testNonlinearEqualityConstraint.cpp +++ b/gtsam/constraint/tests/testNonlinearEqualityConstraint.cpp @@ -22,7 +22,6 @@ #include #include #include -#include #include #include @@ -155,7 +154,6 @@ TEST(ExpressionEqualityConstraint, Vector2) { EXPECT_CORRECT_FACTOR_JACOBIANS(*merit_factor, values2, 1e-7, 1e-5); } - // Test methods of FactorZeroErrorConstraint. TEST(ZeroCostConstraint, BetweenFactor) { Key x1_key = 1; @@ -218,9 +216,6 @@ TEST(ZeroCostConstraint, BetweenFactor) { EXPECT_CORRECT_FACTOR_JACOBIANS(*merit_factor, values2, 1e-7, 1e-5); } - - - TEST(NonlinearEqualityConstraints, Container) { NonlinearEqualityConstraints constraints; @@ -254,12 +249,9 @@ TEST(NonlinearEqualityConstraints, Container) { EXPECT(assert_container_equality(expected_vi_x2, vi[x2_key])); // Check constraint violation. - } - -TEST(NonlinearEqualityConstraints, FromCostGraph) { -} +TEST(NonlinearEqualityConstraints, FromCostGraph) {} int main() { TestResult tr; From 7bb76f5356f872b4c84c3ef8ea03aa98bc2ac26b Mon Sep 17 00:00:00 2001 From: yetongumich Date: Mon, 26 Aug 2024 15:40:19 -0400 Subject: [PATCH 06/12] adding constrained optimization problem --- gtsam/constraint/ConstrainedOptProblem.cpp | 110 +++++++++++++++++++++ gtsam/constraint/ConstrainedOptProblem.h | 103 +++++++++++++++++++ 2 files changed, 213 insertions(+) create mode 100644 gtsam/constraint/ConstrainedOptProblem.cpp create mode 100644 gtsam/constraint/ConstrainedOptProblem.h diff --git a/gtsam/constraint/ConstrainedOptProblem.cpp b/gtsam/constraint/ConstrainedOptProblem.cpp new file mode 100644 index 0000000000..2a7d781350 --- /dev/null +++ b/gtsam/constraint/ConstrainedOptProblem.cpp @@ -0,0 +1,110 @@ +/* ---------------------------------------------------------------------------- + + * 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 ConstrainedOptProblem.h + * @brief Nonlinear constrained optimization problem. + * @author Yetong Zhang, Frank Dellaert + * @date Aug 3, 2024 + */ + +#include +#include +#include +#include "gtsam/constraint/NonlinearEqualityConstraint.h" +#include "gtsam/nonlinear/NonlinearFactor.h" + +namespace gtsam { + +/* ********************************************************************************************* */ +size_t GraphDimension(const NonlinearFactorGraph& graph) { + size_t total_dim = 0; + for (const auto& factor : graph) { + total_dim += factor->dim(); + } + return total_dim; +} + +/* ********************************************************************************************* */ +bool CheckPureCost(const NonlinearFactorGraph& graph) { + for (const auto& factor : graph) { + if (NoiseModelFactor::shared_ptr f = std::dynamic_pointer_cast(factor)) { + if (f->noiseModel()->isConstrained()) { + return false; + } + } + } + return true; +} + +/* ********************************************************************************************* */ +ConstrainedOptProblem::ConstrainedOptProblem(const NonlinearFactorGraph& costs, + const NonlinearEqualityConstraints& e_constraints, + const NonlinearInequalityConstraints& i_constraints, + const Values& values) + : costs_(costs), e_constraints_(e_constraints), i_constraints_(i_constraints), values_(values) { + if (!CheckPureCost(costs)) { + throw std::runtime_error( + "Cost contains factors with constrained noise model. They should be moved to constraints."); + } +} + +/* ********************************************************************************************* */ +std::tuple ConstrainedOptProblem::dim() const { + return { + GraphDimension(costs()), eConstraints().dim(), iConstraints().dim(), initialValues().dim()}; +} + +/* ********************************************************************************************* */ +std::tuple ConstrainedOptProblem::evaluate(const Values& values) const { + return {costs().error(values), + eConstraints().violationNorm(values), + iConstraints().violationNorm(values)}; +} + +/* ********************************************************************************************* */ +ConstrainedOptProblem ConstrainedOptProblem::auxiliaryProblem( + const AuxiliaryKeyGenerator& generator) const { + if (iConstraints().size() == 0) { + return *this; + } + + NonlinearEqualityConstraints new_e_constraints = eConstraints(); + Values new_values = initialValues(); + + size_t k = 0; + for (const auto& i_constraint : iConstraints()) { + if (ScalarExpressionInequalityConstraint::shared_ptr p = + std::dynamic_pointer_cast(i_constraint)) { + // Generate next available auxiliary key. + Key aux_key = generator.generate(k, new_values); + + // Construct auxiliary equality constraint. + Double_ aux_expr(aux_key); + Double_ equality_expr = p->expression() + aux_expr * aux_expr; + new_e_constraints.emplace_shared>( + equality_expr, 0.0, p->noiseModel()->sigmas()); + + // Compute initial value for auxiliary key. + if (!i_constraint->feasible(initialValues())) { + new_values.insert(aux_key, 0.0); + } else { + Vector gap = i_constraint->unwhitenedExpr(initialValues()); + new_values.insert(aux_key, sqrt(-gap(0))); + } + } + } + return ConstrainedOptProblem::EqConstrainedOptProblem(costs_, new_e_constraints, new_values); +} + +/* ********************************************************************************************* */ + +} // namespace gtsam \ No newline at end of file diff --git a/gtsam/constraint/ConstrainedOptProblem.h b/gtsam/constraint/ConstrainedOptProblem.h new file mode 100644 index 0000000000..a22de51a12 --- /dev/null +++ b/gtsam/constraint/ConstrainedOptProblem.h @@ -0,0 +1,103 @@ +/* ---------------------------------------------------------------------------- + + * 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 ConstrainedOptProblem.h + * @brief Nonlinear constrained optimization problem. + * @author Yetong Zhang, Frank Dellaert + * @date Aug 3, 2024 + */ + +#pragma once + +#include +#include + +namespace gtsam { + +/** Constrained optimization problem, in the form of + * argmin_x 0.5||f(X)||^2 + * s.t. h(X) = 0 + * g(X) <= 0 + * where X represents the variables, 0.5||f(X)||^2 represents the quadratic cost + * functions, h(X)=0 represents the nonlinear equality constraints, g(x)<=0 represents the + * inequality constraints. + */ +class GTSAM_EXPORT ConstrainedOptProblem { + public: + typedef ConstrainedOptProblem This; + typedef std::shared_ptr shared_ptr; + + protected: + NonlinearFactorGraph costs_; // cost function, ||f(X)||^2 + NonlinearEqualityConstraints e_constraints_; // equality constraints, h(X)=0 + NonlinearInequalityConstraints i_constraints_; // inequality constraints, g(X)<=0 + Values values_; // initial value estimates of X + + public: + /** Constructor with both equality and inequality constraints. */ + ConstrainedOptProblem(const NonlinearFactorGraph& costs, + const NonlinearEqualityConstraints& e_constraints, + const NonlinearInequalityConstraints& i_constraints, + const Values& values = Values()); + + /** Constructor with equality constraints only. */ + static ConstrainedOptProblem EqConstrainedOptProblem( + const NonlinearFactorGraph& costs, + const NonlinearEqualityConstraints& e_constraints, + const Values& values = Values()) { + return ConstrainedOptProblem(costs, e_constraints, NonlinearInequalityConstraints(), values); + } + + /** Member variable access functions. */ + const NonlinearFactorGraph& costs() const { return costs_; } + const NonlinearEqualityConstraints& eConstraints() const { return e_constraints_; } + const NonlinearInequalityConstraints& iConstraints() const { return i_constraints_; } + const Values& initialValues() const { return values_; } + + /** Evaluate cost and constraint violations. + * Return a tuple representing (cost, e-constraint violation, i-constraint violation). + */ + std::tuple evaluate(const Values& values) const; + + /** Return the dimension of the problem, as a tuple of + * total dimension of cost factors, + * total dimension of equality constraints, + * total dimension of inequality constraints, + * total dimension of variables. + */ + std::tuple dim() const; + + /** Base class to generate keys for auxiliary variables. */ + class GTSAM_EXPORT AuxiliaryKeyGenerator { + public: + AuxiliaryKeyGenerator() {} + virtual ~AuxiliaryKeyGenerator() {} + + virtual Key generate(const size_t k) const { return Symbol('u', k); } + + /** generate the next available auxiliary key that is not in values. */ + Key generate(size_t& k, const Values& values) const { + Key key = generate(k++); + while (values.exists(key)) { + key = generate(k++); + } + return key; + } + }; + + /// Equivalent equality-constrained optimization probelm with auxiliary + /// variables z. Inequality constraints g(x)<=0 are transformed into equality + /// constraints g(x)+z^2=0. + ConstrainedOptProblem auxiliaryProblem(const AuxiliaryKeyGenerator& generator) const; +}; + +} // namespace gtsam From 8b09ef1297503721be2e312181d31fe2265c5c18 Mon Sep 17 00:00:00 2001 From: yetongumich Date: Mon, 26 Aug 2024 16:36:32 -0400 Subject: [PATCH 07/12] rm constrainedOptProblem --- gtsam/constraint/ConstrainedOptProblem.cpp | 110 ----------- gtsam/constraint/ConstrainedOptProblem.h | 103 ---------- gtsam/constraint/tests/constrainedExample.h | 208 ++++++++++---------- 3 files changed, 104 insertions(+), 317 deletions(-) delete mode 100644 gtsam/constraint/ConstrainedOptProblem.cpp delete mode 100644 gtsam/constraint/ConstrainedOptProblem.h diff --git a/gtsam/constraint/ConstrainedOptProblem.cpp b/gtsam/constraint/ConstrainedOptProblem.cpp deleted file mode 100644 index 2a7d781350..0000000000 --- a/gtsam/constraint/ConstrainedOptProblem.cpp +++ /dev/null @@ -1,110 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 ConstrainedOptProblem.h - * @brief Nonlinear constrained optimization problem. - * @author Yetong Zhang, Frank Dellaert - * @date Aug 3, 2024 - */ - -#include -#include -#include -#include "gtsam/constraint/NonlinearEqualityConstraint.h" -#include "gtsam/nonlinear/NonlinearFactor.h" - -namespace gtsam { - -/* ********************************************************************************************* */ -size_t GraphDimension(const NonlinearFactorGraph& graph) { - size_t total_dim = 0; - for (const auto& factor : graph) { - total_dim += factor->dim(); - } - return total_dim; -} - -/* ********************************************************************************************* */ -bool CheckPureCost(const NonlinearFactorGraph& graph) { - for (const auto& factor : graph) { - if (NoiseModelFactor::shared_ptr f = std::dynamic_pointer_cast(factor)) { - if (f->noiseModel()->isConstrained()) { - return false; - } - } - } - return true; -} - -/* ********************************************************************************************* */ -ConstrainedOptProblem::ConstrainedOptProblem(const NonlinearFactorGraph& costs, - const NonlinearEqualityConstraints& e_constraints, - const NonlinearInequalityConstraints& i_constraints, - const Values& values) - : costs_(costs), e_constraints_(e_constraints), i_constraints_(i_constraints), values_(values) { - if (!CheckPureCost(costs)) { - throw std::runtime_error( - "Cost contains factors with constrained noise model. They should be moved to constraints."); - } -} - -/* ********************************************************************************************* */ -std::tuple ConstrainedOptProblem::dim() const { - return { - GraphDimension(costs()), eConstraints().dim(), iConstraints().dim(), initialValues().dim()}; -} - -/* ********************************************************************************************* */ -std::tuple ConstrainedOptProblem::evaluate(const Values& values) const { - return {costs().error(values), - eConstraints().violationNorm(values), - iConstraints().violationNorm(values)}; -} - -/* ********************************************************************************************* */ -ConstrainedOptProblem ConstrainedOptProblem::auxiliaryProblem( - const AuxiliaryKeyGenerator& generator) const { - if (iConstraints().size() == 0) { - return *this; - } - - NonlinearEqualityConstraints new_e_constraints = eConstraints(); - Values new_values = initialValues(); - - size_t k = 0; - for (const auto& i_constraint : iConstraints()) { - if (ScalarExpressionInequalityConstraint::shared_ptr p = - std::dynamic_pointer_cast(i_constraint)) { - // Generate next available auxiliary key. - Key aux_key = generator.generate(k, new_values); - - // Construct auxiliary equality constraint. - Double_ aux_expr(aux_key); - Double_ equality_expr = p->expression() + aux_expr * aux_expr; - new_e_constraints.emplace_shared>( - equality_expr, 0.0, p->noiseModel()->sigmas()); - - // Compute initial value for auxiliary key. - if (!i_constraint->feasible(initialValues())) { - new_values.insert(aux_key, 0.0); - } else { - Vector gap = i_constraint->unwhitenedExpr(initialValues()); - new_values.insert(aux_key, sqrt(-gap(0))); - } - } - } - return ConstrainedOptProblem::EqConstrainedOptProblem(costs_, new_e_constraints, new_values); -} - -/* ********************************************************************************************* */ - -} // namespace gtsam \ No newline at end of file diff --git a/gtsam/constraint/ConstrainedOptProblem.h b/gtsam/constraint/ConstrainedOptProblem.h deleted file mode 100644 index a22de51a12..0000000000 --- a/gtsam/constraint/ConstrainedOptProblem.h +++ /dev/null @@ -1,103 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 ConstrainedOptProblem.h - * @brief Nonlinear constrained optimization problem. - * @author Yetong Zhang, Frank Dellaert - * @date Aug 3, 2024 - */ - -#pragma once - -#include -#include - -namespace gtsam { - -/** Constrained optimization problem, in the form of - * argmin_x 0.5||f(X)||^2 - * s.t. h(X) = 0 - * g(X) <= 0 - * where X represents the variables, 0.5||f(X)||^2 represents the quadratic cost - * functions, h(X)=0 represents the nonlinear equality constraints, g(x)<=0 represents the - * inequality constraints. - */ -class GTSAM_EXPORT ConstrainedOptProblem { - public: - typedef ConstrainedOptProblem This; - typedef std::shared_ptr shared_ptr; - - protected: - NonlinearFactorGraph costs_; // cost function, ||f(X)||^2 - NonlinearEqualityConstraints e_constraints_; // equality constraints, h(X)=0 - NonlinearInequalityConstraints i_constraints_; // inequality constraints, g(X)<=0 - Values values_; // initial value estimates of X - - public: - /** Constructor with both equality and inequality constraints. */ - ConstrainedOptProblem(const NonlinearFactorGraph& costs, - const NonlinearEqualityConstraints& e_constraints, - const NonlinearInequalityConstraints& i_constraints, - const Values& values = Values()); - - /** Constructor with equality constraints only. */ - static ConstrainedOptProblem EqConstrainedOptProblem( - const NonlinearFactorGraph& costs, - const NonlinearEqualityConstraints& e_constraints, - const Values& values = Values()) { - return ConstrainedOptProblem(costs, e_constraints, NonlinearInequalityConstraints(), values); - } - - /** Member variable access functions. */ - const NonlinearFactorGraph& costs() const { return costs_; } - const NonlinearEqualityConstraints& eConstraints() const { return e_constraints_; } - const NonlinearInequalityConstraints& iConstraints() const { return i_constraints_; } - const Values& initialValues() const { return values_; } - - /** Evaluate cost and constraint violations. - * Return a tuple representing (cost, e-constraint violation, i-constraint violation). - */ - std::tuple evaluate(const Values& values) const; - - /** Return the dimension of the problem, as a tuple of - * total dimension of cost factors, - * total dimension of equality constraints, - * total dimension of inequality constraints, - * total dimension of variables. - */ - std::tuple dim() const; - - /** Base class to generate keys for auxiliary variables. */ - class GTSAM_EXPORT AuxiliaryKeyGenerator { - public: - AuxiliaryKeyGenerator() {} - virtual ~AuxiliaryKeyGenerator() {} - - virtual Key generate(const size_t k) const { return Symbol('u', k); } - - /** generate the next available auxiliary key that is not in values. */ - Key generate(size_t& k, const Values& values) const { - Key key = generate(k++); - while (values.exists(key)) { - key = generate(k++); - } - return key; - } - }; - - /// Equivalent equality-constrained optimization probelm with auxiliary - /// variables z. Inequality constraints g(x)<=0 are transformed into equality - /// constraints g(x)+z^2=0. - ConstrainedOptProblem auxiliaryProblem(const AuxiliaryKeyGenerator& generator) const; -}; - -} // namespace gtsam diff --git a/gtsam/constraint/tests/constrainedExample.h b/gtsam/constraint/tests/constrainedExample.h index 47bf771f02..2cc3ba826f 100644 --- a/gtsam/constraint/tests/constrainedExample.h +++ b/gtsam/constraint/tests/constrainedExample.h @@ -18,11 +18,11 @@ #pragma once -#include +// #include #include #include -#include "gtsam/constraint/NonlinearEqualityConstraint.h" -#include "gtsam/constraint/NonlinearInequalityConstraint.h" +#include +#include namespace constrained_example { @@ -78,104 +78,104 @@ Double_ x1(x1_key), x2(x2_key); * f(x) = 0.5*||x1 + e^(-x2)||^2 + 0.5*||x1^2 + 2*x2 + 1||^2 * h(x) = x1 + x1^3 + x2 + x2^2 = 0 */ -namespace constrained_example1 { -using namespace constrained_example; -NonlinearFactorGraph Cost() { - 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 EqConstraints() { - NonlinearEqualityConstraints constraints; - Vector sigmas = Vector1(1.0); - auto h1 = x1 + pow(x1, 3) + x2 + pow(x2, 2); - constraints.push_back(NonlinearEqualityConstraint::shared_ptr( - new ExpressionEqualityConstraint(h1, 0.0, sigmas))); - return constraints; -} - -Values InitValues() { - Values values; - values.insert(x1_key, -0.2); - values.insert(x2_key, -0.2); - return values; -} - -Values OptimalValues() { - Values values; - values.insert(x1_key, 0.0); - values.insert(x2_key, 0.0); - return values; -} - -NonlinearFactorGraph costs = Cost(); -NonlinearEqualityConstraints e_constraints = EqConstraints(); -NonlinearInequalityConstraints i_constraints; -Values init_values = InitValues(); -ConstrainedOptProblem::shared_ptr problem = - std::make_shared(costs, e_constraints, i_constraints, init_values); -Values optimal_values = OptimalValues(); -} // namespace constrained_example1 - -/* ************************************************************************* */ -/** - * Constrained optimization example with inequality constraints - * Approach a point while staying on unit circle, and within an ellipse. - * f(x) = 0.5 * ||x1-1||^2 + 0.5 * ||x2-1||^2 - * h(x) = x1^2 + x2^2 - 1 = 0 - * g(x) = 4*x1^2 + 0.25*x2^2 - 1 <= 0 - */ -namespace constrained_example2 { -using namespace constrained_example; -NonlinearFactorGraph Cost() { - 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; -} - -NonlinearEqualityConstraints EqConstraints() { - NonlinearEqualityConstraints constraints; - Vector1 sigmas(1.0); - Double_ h1 = x1 * x1 + x2 * x2; - constraints.emplace_shared>(h1, 1.0, sigmas); - return constraints; -} - -NonlinearInequalityConstraints IneqConstraints() { - NonlinearInequalityConstraints constraints; - Double_ g1 = 4 * x1 * x1 + 0.25 * x2 * x2 - Double_(1.0); - double sigma = 1; - constraints.emplace_shared(g1, sigma); - return constraints; -} - -Values InitValues() { - Values values; - values.insert(x1_key, -1.0); - values.insert(x2_key, 2.0); - return values; -} - -Values OptimalValues() { - Values values; - values.insert(x1_key, 1.0 / sqrt(5)); - values.insert(x2_key, 2.0 / sqrt(5)); - return values; -} - -NonlinearFactorGraph costs = Cost(); -NonlinearEqualityConstraints e_constraints = EqConstraints(); -NonlinearInequalityConstraints i_constraints = IneqConstraints(); -Values init_values = InitValues(); -ConstrainedOptProblem::shared_ptr problem = - std::make_shared(costs, e_constraints, i_constraints, init_values); -Values optimal_values = OptimalValues(); - -} // namespace constrained_example2 +// namespace constrained_example1 { +// using namespace constrained_example; +// NonlinearFactorGraph Cost() { +// 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 EqConstraints() { +// NonlinearEqualityConstraints constraints; +// Vector sigmas = Vector1(1.0); +// auto h1 = x1 + pow(x1, 3) + x2 + pow(x2, 2); +// constraints.push_back(NonlinearEqualityConstraint::shared_ptr( +// new ExpressionEqualityConstraint(h1, 0.0, sigmas))); +// return constraints; +// } + +// Values InitValues() { +// Values values; +// values.insert(x1_key, -0.2); +// values.insert(x2_key, -0.2); +// return values; +// } + +// Values OptimalValues() { +// Values values; +// values.insert(x1_key, 0.0); +// values.insert(x2_key, 0.0); +// return values; +// } + +// NonlinearFactorGraph costs = Cost(); +// NonlinearEqualityConstraints e_constraints = EqConstraints(); +// NonlinearInequalityConstraints i_constraints; +// Values init_values = InitValues(); +// ConstrainedOptProblem::shared_ptr problem = +// std::make_shared(costs, e_constraints, i_constraints, init_values); +// Values optimal_values = OptimalValues(); +// } // namespace constrained_example1 + +// /* ************************************************************************* */ +// /** +// * Constrained optimization example with inequality constraints +// * Approach a point while staying on unit circle, and within an ellipse. +// * f(x) = 0.5 * ||x1-1||^2 + 0.5 * ||x2-1||^2 +// * h(x) = x1^2 + x2^2 - 1 = 0 +// * g(x) = 4*x1^2 + 0.25*x2^2 - 1 <= 0 +// */ +// namespace constrained_example2 { +// using namespace constrained_example; +// NonlinearFactorGraph Cost() { +// 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; +// } + +// NonlinearEqualityConstraints EqConstraints() { +// NonlinearEqualityConstraints constraints; +// Vector1 sigmas(1.0); +// Double_ h1 = x1 * x1 + x2 * x2; +// constraints.emplace_shared>(h1, 1.0, sigmas); +// return constraints; +// } + +// NonlinearInequalityConstraints IneqConstraints() { +// NonlinearInequalityConstraints constraints; +// Double_ g1 = 4 * x1 * x1 + 0.25 * x2 * x2 - Double_(1.0); +// double sigma = 1; +// constraints.emplace_shared(g1, sigma); +// return constraints; +// } + +// Values InitValues() { +// Values values; +// values.insert(x1_key, -1.0); +// values.insert(x2_key, 2.0); +// return values; +// } + +// Values OptimalValues() { +// Values values; +// values.insert(x1_key, 1.0 / sqrt(5)); +// values.insert(x2_key, 2.0 / sqrt(5)); +// return values; +// } + +// NonlinearFactorGraph costs = Cost(); +// NonlinearEqualityConstraints e_constraints = EqConstraints(); +// NonlinearInequalityConstraints i_constraints = IneqConstraints(); +// Values init_values = InitValues(); +// ConstrainedOptProblem::shared_ptr problem = +// std::make_shared(costs, e_constraints, i_constraints, init_values); +// Values optimal_values = OptimalValues(); + +// } // namespace constrained_example2 From e482c41afd7560decc25b0cd478e8dadbb6102e9 Mon Sep 17 00:00:00 2001 From: yetongumich Date: Tue, 27 Aug 2024 16:25:37 -0400 Subject: [PATCH 08/12] add FactorGraph include --- gtsam/constraint/NonlinearConstraint.h | 1 + 1 file changed, 1 insertion(+) diff --git a/gtsam/constraint/NonlinearConstraint.h b/gtsam/constraint/NonlinearConstraint.h index 5588188037..7ee7ed771e 100644 --- a/gtsam/constraint/NonlinearConstraint.h +++ b/gtsam/constraint/NonlinearConstraint.h @@ -18,6 +18,7 @@ #pragma once +#include #include #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION #include From 2516a0a1bbf2b81393e30f47362c82c5c8763f14 Mon Sep 17 00:00:00 2001 From: yetongumich Date: Tue, 27 Aug 2024 16:34:55 -0400 Subject: [PATCH 09/12] add FactorGraph-inst --- gtsam/constraint/NonlinearConstraint.h | 1 + gtsam/constraint/NonlinearEqualityConstraint.cpp | 1 + gtsam/constraint/NonlinearInequalityConstraint.cpp | 1 + 3 files changed, 3 insertions(+) diff --git a/gtsam/constraint/NonlinearConstraint.h b/gtsam/constraint/NonlinearConstraint.h index 7ee7ed771e..da13f77c65 100644 --- a/gtsam/constraint/NonlinearConstraint.h +++ b/gtsam/constraint/NonlinearConstraint.h @@ -19,6 +19,7 @@ #pragma once #include +#include #include #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION #include diff --git a/gtsam/constraint/NonlinearEqualityConstraint.cpp b/gtsam/constraint/NonlinearEqualityConstraint.cpp index 8705f7db93..170f2317cd 100644 --- a/gtsam/constraint/NonlinearEqualityConstraint.cpp +++ b/gtsam/constraint/NonlinearEqualityConstraint.cpp @@ -16,6 +16,7 @@ * @date Aug 3, 2024 */ #include +#include namespace gtsam { diff --git a/gtsam/constraint/NonlinearInequalityConstraint.cpp b/gtsam/constraint/NonlinearInequalityConstraint.cpp index 3404a143c7..2f96f9a29d 100644 --- a/gtsam/constraint/NonlinearInequalityConstraint.cpp +++ b/gtsam/constraint/NonlinearInequalityConstraint.cpp @@ -17,6 +17,7 @@ */ #include +#include namespace gtsam { From 037c8b36436993383a9b74e0249fc006d1b1039a Mon Sep 17 00:00:00 2001 From: yetongumich Date: Sun, 17 Nov 2024 23:53:21 -0800 Subject: [PATCH 10/12] add using NonlinearFactor::error --- gtsam/constraint/NonlinearConstraint.h | 2 ++ gtsam/nonlinear/NonlinearFactor.h | 2 ++ 2 files changed, 4 insertions(+) diff --git a/gtsam/constraint/NonlinearConstraint.h b/gtsam/constraint/NonlinearConstraint.h index da13f77c65..a01430bf36 100644 --- a/gtsam/constraint/NonlinearConstraint.h +++ b/gtsam/constraint/NonlinearConstraint.h @@ -40,6 +40,8 @@ class GTSAM_EXPORT NonlinearConstraint : public NoiseModelFactor { /** Use constructors of NoiseModelFactor. */ using Base::Base; + using NonlinearFactor::error; + /** Destructor. */ virtual ~NonlinearConstraint() {} 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. From 021d1054289b5e7c44a44ef591d57d21c4bdbef5 Mon Sep 17 00:00:00 2001 From: yetongumich Date: Wed, 20 Nov 2024 11:24:44 -0800 Subject: [PATCH 11/12] fix export issues --- Using-GTSAM-EXPORT.md | 6 +++--- gtsam/constraint/NonlinearEqualityConstraint.h | 2 +- 2 files changed, 4 insertions(+), 4 deletions(-) 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/constraint/NonlinearEqualityConstraint.h b/gtsam/constraint/NonlinearEqualityConstraint.h index 532c3ab382..30e8e52d36 100644 --- a/gtsam/constraint/NonlinearEqualityConstraint.h +++ b/gtsam/constraint/NonlinearEqualityConstraint.h @@ -52,7 +52,7 @@ class GTSAM_EXPORT NonlinearEqualityConstraint : public NonlinearConstraint { /** Equality constraint that force g(x) = M. */ template -class GTSAM_EXPORT ExpressionEqualityConstraint : public NonlinearEqualityConstraint { +class ExpressionEqualityConstraint : public NonlinearEqualityConstraint { public: typedef NonlinearEqualityConstraint Base; typedef ExpressionEqualityConstraint This; From 3c81405a019eddc398a89ba4df70deb3de806297 Mon Sep 17 00:00:00 2001 From: yetongumich Date: Tue, 31 Dec 2024 14:26:31 -0800 Subject: [PATCH 12/12] addressing comments --- gtsam/CMakeLists.txt | 2 +- .../CMakeLists.txt | 2 +- .../InequalityPenaltyFunction.cpp | 2 +- .../InequalityPenaltyFunction.h | 9 + .../NonlinearConstraint.h | 0 .../NonlinearEqualityConstraint-inl.h | 2 +- .../NonlinearEqualityConstraint.cpp | 2 +- .../NonlinearEqualityConstraint.h | 10 +- .../NonlinearInequalityConstraint.cpp | 2 +- .../NonlinearInequalityConstraint.h | 4 +- .../tests/CMakeLists.txt | 0 gtsam/constrained/tests/constrainedExample.h | 73 +++++++ .../tests/testInequalityPenaltyFunction.cpp | 2 +- .../tests/testNonlinearEqualityConstraint.cpp | 2 +- .../testNonlinearInequalityConstraint.cpp | 2 +- gtsam/constraint/tests/constrainedExample.h | 181 ------------------ gtsam/nonlinear/NonlinearEquality.h | 2 +- gtsam/slam/BoundingConstraint.h | 10 +- 18 files changed, 105 insertions(+), 202 deletions(-) rename gtsam/{constraint => constrained}/CMakeLists.txt (93%) rename gtsam/{constraint => constrained}/InequalityPenaltyFunction.cpp (97%) rename gtsam/{constraint => constrained}/InequalityPenaltyFunction.h (93%) rename gtsam/{constraint => constrained}/NonlinearConstraint.h (100%) rename gtsam/{constraint => constrained}/NonlinearEqualityConstraint-inl.h (97%) rename gtsam/{constraint => constrained}/NonlinearEqualityConstraint.cpp (98%) rename gtsam/{constraint => constrained}/NonlinearEqualityConstraint.h (96%) rename gtsam/{constraint => constrained}/NonlinearInequalityConstraint.cpp (99%) rename gtsam/{constraint => constrained}/NonlinearInequalityConstraint.h (98%) rename gtsam/{constraint => constrained}/tests/CMakeLists.txt (100%) create mode 100644 gtsam/constrained/tests/constrainedExample.h rename gtsam/{constraint => constrained}/tests/testInequalityPenaltyFunction.cpp (98%) rename gtsam/{constraint => constrained}/tests/testNonlinearEqualityConstraint.cpp (99%) rename gtsam/{constraint => constrained}/tests/testNonlinearInequalityConstraint.cpp (98%) delete mode 100644 gtsam/constraint/tests/constrainedExample.h diff --git a/gtsam/CMakeLists.txt b/gtsam/CMakeLists.txt index dcc350fe85..0911ef550d 100644 --- a/gtsam/CMakeLists.txt +++ b/gtsam/CMakeLists.txt @@ -6,7 +6,7 @@ project(gtsam LANGUAGES CXX) set (gtsam_subdirs base basis - constraint + constrained geometry inference symbolic diff --git a/gtsam/constraint/CMakeLists.txt b/gtsam/constrained/CMakeLists.txt similarity index 93% rename from gtsam/constraint/CMakeLists.txt rename to gtsam/constrained/CMakeLists.txt index e4bbd89dd6..1687440765 100644 --- a/gtsam/constraint/CMakeLists.txt +++ b/gtsam/constrained/CMakeLists.txt @@ -1,6 +1,6 @@ # Install headers file(GLOB constraint_headers "*.h") -install(FILES ${constraint_headers} DESTINATION include/gtsam/constraint) +install(FILES ${constraint_headers} DESTINATION include/gtsam/constrained) # Build tests add_subdirectory(tests) diff --git a/gtsam/constraint/InequalityPenaltyFunction.cpp b/gtsam/constrained/InequalityPenaltyFunction.cpp similarity index 97% rename from gtsam/constraint/InequalityPenaltyFunction.cpp rename to gtsam/constrained/InequalityPenaltyFunction.cpp index 8b048686ff..2e331292ce 100644 --- a/gtsam/constraint/InequalityPenaltyFunction.cpp +++ b/gtsam/constrained/InequalityPenaltyFunction.cpp @@ -16,7 +16,7 @@ * @date Aug 3, 2024 */ -#include +#include namespace gtsam { diff --git a/gtsam/constraint/InequalityPenaltyFunction.h b/gtsam/constrained/InequalityPenaltyFunction.h similarity index 93% rename from gtsam/constraint/InequalityPenaltyFunction.h rename to gtsam/constrained/InequalityPenaltyFunction.h index baafaef0bf..3aef099af5 100644 --- a/gtsam/constraint/InequalityPenaltyFunction.h +++ b/gtsam/constrained/InequalityPenaltyFunction.h @@ -83,6 +83,9 @@ class GTSAM_EXPORT SmoothRampPoly2 : public InequalityPenaltyFunction { 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) {} @@ -110,6 +113,9 @@ class GTSAM_EXPORT SmoothRampPoly3 : public InequalityPenaltyFunction { double b_; public: + /** Constructor. + * @param epsilon parameter for adjusting the smoothness of the function. + */ SmoothRampPoly3(const double epsilon = 1) : Base(), epsilon_(epsilon), @@ -131,6 +137,9 @@ class GTSAM_EXPORT SoftPlusFunction : public InequalityPenaltyFunction { 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, diff --git a/gtsam/constraint/NonlinearConstraint.h b/gtsam/constrained/NonlinearConstraint.h similarity index 100% rename from gtsam/constraint/NonlinearConstraint.h rename to gtsam/constrained/NonlinearConstraint.h diff --git a/gtsam/constraint/NonlinearEqualityConstraint-inl.h b/gtsam/constrained/NonlinearEqualityConstraint-inl.h similarity index 97% rename from gtsam/constraint/NonlinearEqualityConstraint-inl.h rename to gtsam/constrained/NonlinearEqualityConstraint-inl.h index c228cf97d4..14bc42939f 100644 --- a/gtsam/constraint/NonlinearEqualityConstraint-inl.h +++ b/gtsam/constrained/NonlinearEqualityConstraint-inl.h @@ -17,7 +17,7 @@ #pragma once -#include +#include namespace gtsam { diff --git a/gtsam/constraint/NonlinearEqualityConstraint.cpp b/gtsam/constrained/NonlinearEqualityConstraint.cpp similarity index 98% rename from gtsam/constraint/NonlinearEqualityConstraint.cpp rename to gtsam/constrained/NonlinearEqualityConstraint.cpp index 170f2317cd..effc8774c1 100644 --- a/gtsam/constraint/NonlinearEqualityConstraint.cpp +++ b/gtsam/constrained/NonlinearEqualityConstraint.cpp @@ -15,7 +15,7 @@ * @author Yetong Zhang, Frank Dellaert * @date Aug 3, 2024 */ -#include +#include #include namespace gtsam { diff --git a/gtsam/constraint/NonlinearEqualityConstraint.h b/gtsam/constrained/NonlinearEqualityConstraint.h similarity index 96% rename from gtsam/constraint/NonlinearEqualityConstraint.h rename to gtsam/constrained/NonlinearEqualityConstraint.h index 30e8e52d36..017ca16077 100644 --- a/gtsam/constraint/NonlinearEqualityConstraint.h +++ b/gtsam/constrained/NonlinearEqualityConstraint.h @@ -17,7 +17,7 @@ #pragma once -#include +#include #include #include @@ -99,7 +99,10 @@ class ExpressionEqualityConstraint : public NonlinearEqualityConstraint { #endif }; -/** Equality constraint that enforce the cost factor with zero error. */ +/** 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; @@ -114,7 +117,6 @@ class GTSAM_EXPORT ZeroCostConstraint : public NonlinearEqualityConstraint { * @brief Constructor. * * @param factor NoiseModel factor. - * @param tolerance vector representing tolerance in each dimension. */ ZeroCostConstraint(const NoiseModelFactor::shared_ptr& factor); @@ -177,4 +179,4 @@ class GTSAM_EXPORT NonlinearEqualityConstraints : public FactorGraph +#include diff --git a/gtsam/constraint/NonlinearInequalityConstraint.cpp b/gtsam/constrained/NonlinearInequalityConstraint.cpp similarity index 99% rename from gtsam/constraint/NonlinearInequalityConstraint.cpp rename to gtsam/constrained/NonlinearInequalityConstraint.cpp index 2f96f9a29d..b1dcb7534c 100644 --- a/gtsam/constraint/NonlinearInequalityConstraint.cpp +++ b/gtsam/constrained/NonlinearInequalityConstraint.cpp @@ -16,7 +16,7 @@ * @date Aug 3, 2024 */ -#include +#include #include namespace gtsam { diff --git a/gtsam/constraint/NonlinearInequalityConstraint.h b/gtsam/constrained/NonlinearInequalityConstraint.h similarity index 98% rename from gtsam/constraint/NonlinearInequalityConstraint.h rename to gtsam/constrained/NonlinearInequalityConstraint.h index 658a29f2f6..9dbf1f008d 100644 --- a/gtsam/constraint/NonlinearInequalityConstraint.h +++ b/gtsam/constrained/NonlinearInequalityConstraint.h @@ -18,8 +18,8 @@ #pragma once -#include -#include +#include +#include #include namespace gtsam { diff --git a/gtsam/constraint/tests/CMakeLists.txt b/gtsam/constrained/tests/CMakeLists.txt similarity index 100% rename from gtsam/constraint/tests/CMakeLists.txt rename to gtsam/constrained/tests/CMakeLists.txt 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/constraint/tests/testInequalityPenaltyFunction.cpp b/gtsam/constrained/tests/testInequalityPenaltyFunction.cpp similarity index 98% rename from gtsam/constraint/tests/testInequalityPenaltyFunction.cpp rename to gtsam/constrained/tests/testInequalityPenaltyFunction.cpp index 09cd17d5bf..9b41dbe2c5 100644 --- a/gtsam/constraint/tests/testInequalityPenaltyFunction.cpp +++ b/gtsam/constrained/tests/testInequalityPenaltyFunction.cpp @@ -20,7 +20,7 @@ #include #include #include -#include +#include using namespace gtsam; diff --git a/gtsam/constraint/tests/testNonlinearEqualityConstraint.cpp b/gtsam/constrained/tests/testNonlinearEqualityConstraint.cpp similarity index 99% rename from gtsam/constraint/tests/testNonlinearEqualityConstraint.cpp rename to gtsam/constrained/tests/testNonlinearEqualityConstraint.cpp index 70dabff451..e09c197a31 100644 --- a/gtsam/constraint/tests/testNonlinearEqualityConstraint.cpp +++ b/gtsam/constrained/tests/testNonlinearEqualityConstraint.cpp @@ -21,7 +21,7 @@ #include #include #include -#include +#include #include #include diff --git a/gtsam/constraint/tests/testNonlinearInequalityConstraint.cpp b/gtsam/constrained/tests/testNonlinearInequalityConstraint.cpp similarity index 98% rename from gtsam/constraint/tests/testNonlinearInequalityConstraint.cpp rename to gtsam/constrained/tests/testNonlinearInequalityConstraint.cpp index 3950bfa03b..4d9034daa2 100644 --- a/gtsam/constraint/tests/testNonlinearInequalityConstraint.cpp +++ b/gtsam/constrained/tests/testNonlinearInequalityConstraint.cpp @@ -20,7 +20,7 @@ #include #include #include -#include +#include #include #include "constrainedExample.h" diff --git a/gtsam/constraint/tests/constrainedExample.h b/gtsam/constraint/tests/constrainedExample.h deleted file mode 100644 index 2cc3ba826f..0000000000 --- a/gtsam/constraint/tests/constrainedExample.h +++ /dev/null @@ -1,181 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 - -/* ************************************************************************* */ -/** - * 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 = 0 - */ -// namespace constrained_example1 { -// using namespace constrained_example; -// NonlinearFactorGraph Cost() { -// 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 EqConstraints() { -// NonlinearEqualityConstraints constraints; -// Vector sigmas = Vector1(1.0); -// auto h1 = x1 + pow(x1, 3) + x2 + pow(x2, 2); -// constraints.push_back(NonlinearEqualityConstraint::shared_ptr( -// new ExpressionEqualityConstraint(h1, 0.0, sigmas))); -// return constraints; -// } - -// Values InitValues() { -// Values values; -// values.insert(x1_key, -0.2); -// values.insert(x2_key, -0.2); -// return values; -// } - -// Values OptimalValues() { -// Values values; -// values.insert(x1_key, 0.0); -// values.insert(x2_key, 0.0); -// return values; -// } - -// NonlinearFactorGraph costs = Cost(); -// NonlinearEqualityConstraints e_constraints = EqConstraints(); -// NonlinearInequalityConstraints i_constraints; -// Values init_values = InitValues(); -// ConstrainedOptProblem::shared_ptr problem = -// std::make_shared(costs, e_constraints, i_constraints, init_values); -// Values optimal_values = OptimalValues(); -// } // namespace constrained_example1 - -// /* ************************************************************************* */ -// /** -// * Constrained optimization example with inequality constraints -// * Approach a point while staying on unit circle, and within an ellipse. -// * f(x) = 0.5 * ||x1-1||^2 + 0.5 * ||x2-1||^2 -// * h(x) = x1^2 + x2^2 - 1 = 0 -// * g(x) = 4*x1^2 + 0.25*x2^2 - 1 <= 0 -// */ -// namespace constrained_example2 { -// using namespace constrained_example; -// NonlinearFactorGraph Cost() { -// 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; -// } - -// NonlinearEqualityConstraints EqConstraints() { -// NonlinearEqualityConstraints constraints; -// Vector1 sigmas(1.0); -// Double_ h1 = x1 * x1 + x2 * x2; -// constraints.emplace_shared>(h1, 1.0, sigmas); -// return constraints; -// } - -// NonlinearInequalityConstraints IneqConstraints() { -// NonlinearInequalityConstraints constraints; -// Double_ g1 = 4 * x1 * x1 + 0.25 * x2 * x2 - Double_(1.0); -// double sigma = 1; -// constraints.emplace_shared(g1, sigma); -// return constraints; -// } - -// Values InitValues() { -// Values values; -// values.insert(x1_key, -1.0); -// values.insert(x2_key, 2.0); -// return values; -// } - -// Values OptimalValues() { -// Values values; -// values.insert(x1_key, 1.0 / sqrt(5)); -// values.insert(x2_key, 2.0 / sqrt(5)); -// return values; -// } - -// NonlinearFactorGraph costs = Cost(); -// NonlinearEqualityConstraints e_constraints = EqConstraints(); -// NonlinearInequalityConstraints i_constraints = IneqConstraints(); -// Values init_values = InitValues(); -// ConstrainedOptProblem::shared_ptr problem = -// std::make_shared(costs, e_constraints, i_constraints, init_values); -// Values optimal_values = OptimalValues(); - -// } // namespace constrained_example2 diff --git a/gtsam/nonlinear/NonlinearEquality.h b/gtsam/nonlinear/NonlinearEquality.h index c91bddd9cb..cac54f332a 100644 --- a/gtsam/nonlinear/NonlinearEquality.h +++ b/gtsam/nonlinear/NonlinearEquality.h @@ -18,7 +18,7 @@ #pragma once #include -#include +#include #include #include diff --git a/gtsam/slam/BoundingConstraint.h b/gtsam/slam/BoundingConstraint.h index d35152adfa..4c6c775a00 100644 --- a/gtsam/slam/BoundingConstraint.h +++ b/gtsam/slam/BoundingConstraint.h @@ -19,7 +19,7 @@ #include #include -#include +#include namespace gtsam { @@ -59,7 +59,7 @@ struct BoundingConstraint1: public NonlinearInequalityConstraint { virtual double value(const X& x, OptionalMatrixType H = OptionalNone) const = 0; - Vector unwhitenedExpr(const Values& x, OptionalMatrixVecType H = nullptr) const override { + Vector unwhitenedExpr(const Values& x, OptionalMatrixVecType H = {}) const override { if (H) { double d = value(x.at(this->key()), &(H->front())); if (isGreaterThan_) { @@ -75,7 +75,7 @@ struct BoundingConstraint1: public NonlinearInequalityConstraint { } /// TODO: This should be deprecated. - Vector evaluateError(const X& x, OptionalMatrixType H = nullptr) const { + Vector evaluateError(const X& x, OptionalMatrixType H = {}) const { Matrix D; double error = value(x, &D) - threshold_; if (H) { @@ -138,7 +138,7 @@ struct BoundingConstraint2: public NonlinearInequalityConstraint { OptionalMatrixType H1 = OptionalNone, OptionalMatrixType H2 = OptionalNone) const = 0; - Vector unwhitenedExpr(const Values& x, OptionalMatrixVecType H = nullptr) const override { + Vector unwhitenedExpr(const Values& x, OptionalMatrixVecType H = {}) const override { X1 x1 = x.at(keys().front()); X2 x2 = x.at(keys().back()); if (H) { @@ -158,7 +158,7 @@ struct BoundingConstraint2: public NonlinearInequalityConstraint { /// TODO: This should be deprecated. Vector evaluateError(const X1& x1, const X2& x2, - OptionalMatrixType H1 = nullptr, OptionalMatrixType H2 = nullptr) const { + OptionalMatrixType H1 = {}, OptionalMatrixType H2 = {}) const { Matrix D1, D2; double error = value(x1, x2, &D1, &D2) - threshold_; if (H1) {