From 8b09ef1297503721be2e312181d31fe2265c5c18 Mon Sep 17 00:00:00 2001 From: yetongumich Date: Mon, 26 Aug 2024 16:36:32 -0400 Subject: [PATCH] rm constrainedOptProblem --- gtsam/constraint/ConstrainedOptProblem.cpp | 110 ------------ gtsam/constraint/ConstrainedOptProblem.h | 103 ----------- gtsam/constraint/tests/constrainedExample.h | 186 ++++++++++---------- 3 files changed, 93 insertions(+), 306 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 2a7d78135..000000000 --- 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 a22de51a1..000000000 --- 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 47bf771f0..2cc3ba826 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; -} +// 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; -} +// 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 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; -} +// 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 +// 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; -} +// /* ************************************************************************* */ +// /** +// * 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; -} +// 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; -} +// 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 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; -} +// 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(); +// 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_example2