From 7bb76f5356f872b4c84c3ef8ea03aa98bc2ac26b Mon Sep 17 00:00:00 2001 From: yetongumich Date: Mon, 26 Aug 2024 15:40:19 -0400 Subject: [PATCH] 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 000000000..2a7d78135 --- /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 000000000..a22de51a1 --- /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