12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

adding constrained optimization problem

release/4.3a0
yetongumich 2024-08-26 15:40:19 -04:00
parent 6caf0a3642
commit 7bb76f5356
2 changed files with 213 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

110
gtsam/constraint/ConstrainedOptProblem.cpp Normal file
View File

@ -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 <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

103
gtsam/constraint/ConstrainedOptProblem.h Normal file
View File

@ -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 <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