rm constrainedOptProblem
yetongumich
parent
7bb76f5356
commit
8b09ef1297
|
|
@ -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 <gtsam/constraint/ConstrainedOptProblem.h>
|
||||
#include <memory>
|
||||
#include <stdexcept>
|
||||
#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<NoiseModelFactor>(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<size_t, size_t, size_t, size_t> ConstrainedOptProblem::dim() const {
|
||||
return {
|
||||
GraphDimension(costs()), eConstraints().dim(), iConstraints().dim(), initialValues().dim()};
|
||||
}
|
||||
|
||||
/* ********************************************************************************************* */
|
||||
std::tuple<double, double, double> 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<ScalarExpressionInequalityConstraint>(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<ExpressionEqualityConstraint<double>>(
|
||||
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
|
||||
|
|
@ -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 <gtsam/constraint/NonlinearEqualityConstraint.h>
|
||||
#include <gtsam/constraint/NonlinearInequalityConstraint.h>
|
||||
|
||||
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<This> 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<double, double, double> 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<size_t, size_t, size_t, size_t> 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
|
||||
|
|
@ -18,11 +18,11 @@
|
|||
|
||||
#pragma once
|
||||
|
||||
#include <gtsam/constraint/ConstrainedOptProblem.h>
|
||||
// #include <gtsam/constraint/ConstrainedOptProblem.h>
|
||||
#include <gtsam/nonlinear/expressions.h>
|
||||
#include <gtsam/slam/BetweenFactor.h>
|
||||
#include "gtsam/constraint/NonlinearEqualityConstraint.h"
|
||||
#include "gtsam/constraint/NonlinearInequalityConstraint.h"
|
||||
#include <gtsam/constraint/NonlinearEqualityConstraint.h>
|
||||
#include <gtsam/constraint/NonlinearInequalityConstraint.h>
|
||||
|
||||
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<double>(cost_noise, 0., f1));
|
||||
graph.add(ExpressionFactor<double>(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<double>(cost_noise, 0., f1));
|
||||
// graph.add(ExpressionFactor<double>(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<double>(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<double>(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<ConstrainedOptProblem>(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<ConstrainedOptProblem>(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<ExpressionEqualityConstraint<double>>(h1, 1.0, sigmas);
|
||||
return constraints;
|
||||
}
|
||||
// NonlinearEqualityConstraints EqConstraints() {
|
||||
// NonlinearEqualityConstraints constraints;
|
||||
// Vector1 sigmas(1.0);
|
||||
// Double_ h1 = x1 * x1 + x2 * x2;
|
||||
// constraints.emplace_shared<ExpressionEqualityConstraint<double>>(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<ScalarExpressionInequalityConstraint>(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<ScalarExpressionInequalityConstraint>(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<ConstrainedOptProblem>(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<ConstrainedOptProblem>(costs, e_constraints, i_constraints, init_values);
|
||||
// Values optimal_values = OptimalValues();
|
||||
|
||||
} // namespace constrained_example2
|
||||
// } // namespace constrained_example2
|
||||
|
|
|
|||
Loading…
Reference in New Issue