add definition of constraints
yetongumich
parent
c6bd3f8e32
commit
59f6f081c5
|
|
@ -6,6 +6,7 @@ project(gtsam LANGUAGES CXX)
|
|||
set (gtsam_subdirs
|
||||
base
|
||||
basis
|
||||
constraint
|
||||
geometry
|
||||
inference
|
||||
symbolic
|
||||
|
|
|
|||
|
|
@ -0,0 +1,6 @@
|
|||
# Install headers
|
||||
file(GLOB constraint_headers "*.h")
|
||||
install(FILES ${constraint_headers} DESTINATION include/gtsam/constraint)
|
||||
|
||||
# Build tests
|
||||
add_subdirectory(tests)
|
||||
|
|
@ -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 <gtsam/nonlinear/NonlinearFactor.h>
|
||||
#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
|
||||
|
|
@ -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 <gtsam/constraint/NonlinearEqualityConstraint.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/* ************************************************************************* */
|
||||
template <typename T>
|
||||
ExpressionEqualityConstraint<T>::ExpressionEqualityConstraint(const Expression<T>& expression,
|
||||
const T& rhs,
|
||||
const Vector& sigmas)
|
||||
: Base(constrainedNoise(sigmas), expression.keysAndDims().first),
|
||||
expression_(expression),
|
||||
rhs_(rhs),
|
||||
dims_(expression.keysAndDims().second) {}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template <typename T>
|
||||
Vector ExpressionEqualityConstraint<T>::unwhitenedError(const Values& x,
|
||||
OptionalMatrixVecType H) const {
|
||||
// Copy-paste from ExpressionFactor.
|
||||
if (H) {
|
||||
const T value = expression_.valueAndDerivatives(x, keys_, dims_, *H);
|
||||
return -traits<T>::Local(value, rhs_);
|
||||
} else {
|
||||
const T value = expression_.value(x);
|
||||
return -traits<T>::Local(value, rhs_);
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template <typename T>
|
||||
NoiseModelFactor::shared_ptr ExpressionEqualityConstraint<T>::penaltyFactor(const double mu) const {
|
||||
return std::make_shared<ExpressionFactor<T>>(penaltyNoise(mu), rhs_, expression_);
|
||||
}
|
||||
|
||||
} // namespace gtsam
|
||||
|
|
@ -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 <gtsam/constraint/NonlinearEqualityConstraint.h>
|
||||
|
||||
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<NoiseModelFactor>(factor);
|
||||
constraints.emplace_shared<ZeroCostConstraint>(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
|
||||
|
|
@ -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 <gtsam/constraint/NonlinearConstraint.h>
|
||||
#include <gtsam/nonlinear/ExpressionFactor.h>
|
||||
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/**
|
||||
* Equality constraint base class.
|
||||
*/
|
||||
class NonlinearEqualityConstraint : public NonlinearConstraint {
|
||||
public:
|
||||
typedef NonlinearConstraint Base;
|
||||
typedef NonlinearEqualityConstraint This;
|
||||
typedef std::shared_ptr<This> shared_ptr;
|
||||
|
||||
/** Default constructor. */
|
||||
using Base::Base;
|
||||
|
||||
/** Destructor. */
|
||||
virtual ~NonlinearEqualityConstraint() {}
|
||||
};
|
||||
|
||||
/** Equality constraint that force g(x) = M. */
|
||||
template <typename T>
|
||||
class ExpressionEqualityConstraint : public NonlinearEqualityConstraint {
|
||||
public:
|
||||
typedef NonlinearEqualityConstraint Base;
|
||||
typedef ExpressionEqualityConstraint This;
|
||||
typedef std::shared_ptr<This> shared_ptr;
|
||||
|
||||
protected:
|
||||
Expression<T> expression_;
|
||||
T rhs_;
|
||||
FastVector<int> dims_;
|
||||
|
||||
public:
|
||||
/**
|
||||
* @brief Constructor.
|
||||
*
|
||||
* @param expression expression representing g(x).
|
||||
* @param tolerance vector representing tolerance in each dimension.
|
||||
*/
|
||||
ExpressionEqualityConstraint(const Expression<T>& 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<T>& 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<This> 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<NonlinearEqualityConstraint> {
|
||||
public:
|
||||
typedef std::shared_ptr<NonlinearEqualityConstraints> shared_ptr;
|
||||
typedef FactorGraph<NonlinearEqualityConstraint> 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 <gtsam/constraint/NonlinearEqualityConstraint-inl.h>
|
||||
|
|
@ -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 <gtsam/constraint/NonlinearInequalityConstraint.h>
|
||||
#include <memory>
|
||||
#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<ScalarExpressionInequalityConstraint>(neg_expr, sigma);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
ScalarExpressionInequalityConstraint::shared_ptr ScalarExpressionInequalityConstraint::LeqZero(
|
||||
const Double_& expression, const double& sigma) {
|
||||
return std::make_shared<ScalarExpressionInequalityConstraint>(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<ExpressionEqualityConstraint<double>>(
|
||||
expression_, 0.0, noiseModel()->sigmas());
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
NoiseModelFactor::shared_ptr ScalarExpressionInequalityConstraint::penaltyFactor(
|
||||
const double mu) const {
|
||||
Double_ penalty_expression(RampFunction, expression_);
|
||||
return std::make_shared<ExpressionFactor<double>>(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<ExpressionFactor<double>>(penaltyNoise(mu), 0.0, error);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
NoiseModelFactor::shared_ptr ScalarExpressionInequalityConstraint::penaltyFactorEquality(
|
||||
const double mu) const {
|
||||
return std::make_shared<ExpressionFactor<double>>(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
|
||||
|
|
@ -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 <gtsam/constraint/NonlinearEqualityConstraint.h>
|
||||
#include <gtsam/constraint/RampFunction.h>
|
||||
#include <gtsam/nonlinear/expressions.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/**
|
||||
* Inequality constraint base class.
|
||||
*/
|
||||
class NonlinearInequalityConstraint : public NonlinearConstraint {
|
||||
public:
|
||||
typedef NonlinearConstraint Base;
|
||||
typedef NonlinearInequalityConstraint This;
|
||||
typedef std::shared_ptr<This> 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<This> shared_ptr;
|
||||
|
||||
protected:
|
||||
Double_ expression_;
|
||||
FastVector<int> 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<NonlinearInequalityConstraint> {
|
||||
public:
|
||||
typedef FactorGraph<NonlinearInequalityConstraint> Base;
|
||||
typedef NonlinearInequalityConstraints This;
|
||||
typedef std::shared_ptr<This> 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
|
||||
|
|
@ -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 <gtsam/constraint/RampFunction.h>
|
||||
|
||||
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
|
||||
|
|
@ -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 <gtsam/nonlinear/ExpressionFactor.h>
|
||||
#include <gtsam/nonlinear/expressions.h>
|
||||
|
||||
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<SmoothRampFunction> shared_ptr;
|
||||
typedef std::function<double(const double& x, OptionalJacobian<1, 1> 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
|
||||
|
|
@ -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")
|
||||
|
|
@ -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 <gtsam/base/Matrix.h>
|
||||
#include <gtsam/base/Vector.h>
|
||||
#include <gtsam/inference/Symbol.h>
|
||||
#include <gtsam/nonlinear/ExpressionFactor.h>
|
||||
#include <gtsam/nonlinear/NonlinearFactor.h>
|
||||
#include <gtsam/nonlinear/expressions.h>
|
||||
#include <gtsam/slam/BetweenFactor.h>
|
||||
#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 = {}) {
|
||||
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<double>(cost_noise, 0., f1));
|
||||
graph.add(ExpressionFactor<double>(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<double>(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<DoubleExpressionInequality>(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<gtsam::BetweenFactor<double>>(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<DoubleExpressionEquality>(h1, tolerance);
|
||||
return e_constraints;
|
||||
}
|
||||
|
||||
NonlinearFactorGraph cost = GetCost();
|
||||
NonlinearEqualityConstraints e_constraints = GetEConstraints();
|
||||
} // namespace i_constrained_example
|
||||
|
|
@ -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 <CppUnitLite/Test.h>
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
#include <gtsam/base/Testable.h>
|
||||
#include <gtsam/base/TestableAssertions.h>
|
||||
#include <gtsam/base/numericalDerivative.h>
|
||||
#include <gtsam/constraint/NonlinearEqualityConstraint.h>
|
||||
#include <gtsam/inference/VariableIndex.h>
|
||||
#include <gtsam/nonlinear/factorTesting.h>
|
||||
#include <gtsam/slam/BetweenFactor.h>
|
||||
|
||||
#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<double>(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<Vector2>(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<BetweenFactor<Vector2>>(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<ExpressionEqualityConstraint<double>>(g1, 0.0, sigmas1);
|
||||
constraints.emplace_shared<ExpressionEqualityConstraint<Vector2>>(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);
|
||||
}
|
||||
|
|
@ -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 <CppUnitLite/TestHarness.h>
|
||||
#include <gtsam/base/Testable.h>
|
||||
#include <gtsam/base/TestableAssertions.h>
|
||||
#include <gtsam/base/numericalDerivative.h>
|
||||
#include <gtsam/constraint/NonlinearInequalityConstraint.h>
|
||||
#include <gtsam/nonlinear/factorTesting.h>
|
||||
|
||||
#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);
|
||||
}
|
||||
|
|
@ -0,0 +1,37 @@
|
|||
#include <CppUnitLite/TestHarness.h>
|
||||
|
||||
#include <gtsam/base/Testable.h>
|
||||
#include <gtsam/base/TestableAssertions.h>
|
||||
#include <gtsam/base/numericalDerivative.h>
|
||||
#include <gtsam/nonlinear/factorTesting.h>
|
||||
#include <gtsam/slam/BetweenFactor.h>
|
||||
|
||||
#include <iostream>
|
||||
#include <functional>
|
||||
|
||||
// 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<void(int)> func = functor;
|
||||
|
||||
// Call the std::function object
|
||||
func(42);
|
||||
}
|
||||
|
||||
|
||||
int main() {
|
||||
TestResult tr;
|
||||
return TestRegistry::runAllTests(tr);
|
||||
}
|
||||
|
|
@ -34,6 +34,8 @@ namespace gtsam {
|
|||
// Forward declares
|
||||
class Values;
|
||||
template<typename T> class ExpressionFactor;
|
||||
template<typename T> class ExpressionEqualityConstraint;
|
||||
class ScalarExpressionInequalityConstraint;
|
||||
|
||||
namespace internal {
|
||||
template<typename T> class ExecutionTrace;
|
||||
|
|
@ -206,6 +208,8 @@ protected:
|
|||
// be very selective on who can access these private methods:
|
||||
friend class ExpressionFactor<T> ;
|
||||
friend class internal::ExpressionNode<T>;
|
||||
friend class ExpressionEqualityConstraint<T>;
|
||||
friend class ScalarExpressionInequalityConstraint;
|
||||
|
||||
// and add tests
|
||||
friend class ::ExpressionFactorShallowTest;
|
||||
|
|
|
|||
Loading…
Reference in New Issue