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

add GTSAM_EXPORT and refactor

release/4.3a0
yetongumich 2024-08-26 15:23:39 -04:00
parent 9b0fa1210e
commit 6caf0a3642
11 changed files with 225 additions and 174 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

28
gtsam/constraint/RampFunction.cpp → gtsam/constraint/InequalityPenaltyFunction.cpp
View File

@ -10,18 +10,23 @@
* -------------------------------------------------------------------------- */ * -------------------------------------------------------------------------- */
/** /**
* @file RampFunction.cpp * @file InequalityPenaltyFunction.cpp
* @brief Ramp function to compute inequality constraint violations. * @brief Ramp function to compute inequality constraint violations.
* @author Yetong Zhang * @author Yetong Zhang
* @date Aug 3, 2024 * @date Aug 3, 2024
*/ */
#include <gtsam/constraint/RampFunction.h> #include <gtsam/constraint/InequalityPenaltyFunction.h>
namespace gtsam { namespace gtsam {
/* ************************************************************************* */ /* ********************************************************************************************* */
double RampFunction(const double x, OptionalJacobian<1, 1> H) { InequalityPenaltyFunction::UnaryScalarFunc InequalityPenaltyFunction::function() const {
return [=](const double& x, OptionalJacobian<1, 1> H = {}) -> double { return (*this)(x, H); };
}
/* ********************************************************************************************* */
double RampFunction::Ramp(const double x, OptionalJacobian<1, 1> H) {
if (x < 0) { if (x < 0) {
if (H) { if (H) {
H->setConstant(0); H->setConstant(0);
@ -35,13 +40,8 @@ double RampFunction(const double x, OptionalJacobian<1, 1> H) {
} }
} }
/* ************************************************************************* */ /* ********************************************************************************************* */
SmoothRampFunction::UnaryScalarFunc SmoothRampFunction::function() const { double SmoothRampPoly2::operator()(const double& x, OptionalJacobian<1, 1> H) const {
return [=](const double& x, OptionalJacobian<1, 1> H = {}) -> double { return (*this)(x, H); };
}
/* ************************************************************************* */
double RampFunctionPoly2::operator()(const double& x, OptionalJacobian<1, 1> H) const {
if (x <= 0) { if (x <= 0) {
if (H) { if (H) {
H->setZero(); H->setZero();
@ -60,8 +60,8 @@ double RampFunctionPoly2::operator()(const double& x, OptionalJacobian<1, 1> H)
} }
} }
/* ************************************************************************* */ /* ********************************************************************************************* */
double RampFunctionPoly3::operator()(const double& x, OptionalJacobian<1, 1> H) const { double SmoothRampPoly3::operator()(const double& x, OptionalJacobian<1, 1> H) const {
if (x <= 0) { if (x <= 0) {
if (H) { if (H) {
H->setZero(); H->setZero();
@ -80,7 +80,7 @@ double RampFunctionPoly3::operator()(const double& x, OptionalJacobian<1, 1> H)
} }
} }
/* ************************************************************************* */ /* ********************************************************************************************* */
double SoftPlusFunction::operator()(const double& x, OptionalJacobian<1, 1> H) const { double SoftPlusFunction::operator()(const double& x, OptionalJacobian<1, 1> H) const {
if (H) { if (H) {
H->setConstant(1 / (1 + std::exp(-k_ * x))); H->setConstant(1 / (1 + std::exp(-k_ * x)));

81
gtsam/constraint/RampFunction.h → gtsam/constraint/InequalityPenaltyFunction.h
View File

@ -10,7 +10,7 @@
* -------------------------------------------------------------------------- */ * -------------------------------------------------------------------------- */
/** /**
* @file RampFunction.h * @file InequalityPenaltyFunction.h
* @brief Ramp function to compute inequality constraint violations. * @brief Ramp function to compute inequality constraint violations.
* @author Yetong Zhang * @author Yetong Zhang
* @date Aug 3, 2024 * @date Aug 3, 2024
@ -20,31 +20,49 @@
#include <gtsam/nonlinear/ExpressionFactor.h> #include <gtsam/nonlinear/ExpressionFactor.h>
#include <gtsam/nonlinear/expressions.h> #include <gtsam/nonlinear/expressions.h>
#include <memory>
namespace gtsam { namespace gtsam {
/** Base class for smooth approximation of the ramp function. */
class GTSAM_EXPORT InequalityPenaltyFunction {
public:
typedef std::shared_ptr<InequalityPenaltyFunction> shared_ptr;
typedef std::function<double(const double& x, OptionalJacobian<1, 1> H)>
UnaryScalarFunc;
/** Constructor. */
InequalityPenaltyFunction() {}
/** Destructor. */
virtual ~InequalityPenaltyFunction() {}
virtual double operator()(const double& x,
OptionalJacobian<1, 1> H = {}) const = 0;
virtual UnaryScalarFunc function() const;
};
/** Ramp function f : R -> R. /** Ramp function f : R -> R.
* f(x) = 0 for x <= 0 * f(x) = 0 for x <= 0
* x for x > 0 * x for x > 0
*/ */
double RampFunction(const double x, OptionalJacobian<1, 1> H = {}); class GTSAM_EXPORT RampFunction : public InequalityPenaltyFunction {
/** Base class for smooth approximation of the ramp function. */
class SmoothRampFunction {
public: public:
typedef std::shared_ptr<SmoothRampFunction> shared_ptr; typedef InequalityPenaltyFunction Base;
typedef std::function<double(const double& x, OptionalJacobian<1, 1> H)> UnaryScalarFunc; typedef RampFunction This;
typedef std::shared_ptr<This> shared_ptr;
/** Constructor. */ public:
SmoothRampFunction() {} RampFunction() : Base() {}
/** Destructor. */ virtual double operator()(const double& x,
virtual ~SmoothRampFunction() {} OptionalJacobian<1, 1> H = {}) const override {
return Ramp(x, H);
}
virtual double operator()(const double& x, OptionalJacobian<1, 1> H = {}) const = 0; virtual UnaryScalarFunc function() const override { return Ramp; }
UnaryScalarFunc function() const; static double Ramp(const double x, OptionalJacobian<1, 1> H = {});
}; };
/** Ramp function approximated with a polynomial of degree 2 in [0, epsilon]. /** Ramp function approximated with a polynomial of degree 2 in [0, epsilon].
@ -54,10 +72,10 @@ class SmoothRampFunction {
* a * x^2 for 0 < x < epsilon * a * x^2 for 0 < x < epsilon
* x - epsilon/2 for x >= epsilon * x - epsilon/2 for x >= epsilon
*/ */
class RampFunctionPoly2 : public SmoothRampFunction { class GTSAM_EXPORT SmoothRampPoly2 : public InequalityPenaltyFunction {
public: public:
typedef SmoothRampFunction Base; typedef InequalityPenaltyFunction Base;
typedef RampFunctionPoly2 This; typedef SmoothRampPoly2 This;
typedef std::shared_ptr<This> shared_ptr; typedef std::shared_ptr<This> shared_ptr;
protected: protected:
@ -65,9 +83,11 @@ class RampFunctionPoly2 : public SmoothRampFunction {
double a_; double a_;
public: public:
RampFunctionPoly2(const double epsilon = 1) : Base(), epsilon_(epsilon), a_(0.5 / epsilon) {} SmoothRampPoly2(const double epsilon = 1)
: Base(), epsilon_(epsilon), a_(0.5 / epsilon) {}
virtual double operator()(const double& x, OptionalJacobian<1, 1> H = {}) const override; virtual double operator()(const double& x,
OptionalJacobian<1, 1> H = {}) const override;
}; };
/** Ramp function approximated with a polynomial of degree 3 in [0, epsilon]. /** Ramp function approximated with a polynomial of degree 3 in [0, epsilon].
@ -78,10 +98,10 @@ class RampFunctionPoly2 : public SmoothRampFunction {
* a * x^3 + b * x^2 for 0 < x < epsilon * a * x^3 + b * x^2 for 0 < x < epsilon
* x for x >= epsilon * x for x >= epsilon
*/ */
class RampFunctionPoly3 : public SmoothRampFunction { class GTSAM_EXPORT SmoothRampPoly3 : public InequalityPenaltyFunction {
public: public:
typedef SmoothRampFunction Base; typedef InequalityPenaltyFunction Base;
typedef RampFunctionPoly3 This; typedef SmoothRampPoly3 This;
typedef std::shared_ptr<This> shared_ptr; typedef std::shared_ptr<This> shared_ptr;
protected: protected:
@ -90,16 +110,20 @@ class RampFunctionPoly3 : public SmoothRampFunction {
double b_; double b_;
public: public:
RampFunctionPoly3(const double epsilon = 1) SmoothRampPoly3(const double epsilon = 1)
: Base(), epsilon_(epsilon), a_(-1 / (epsilon * epsilon)), b_(2 / epsilon) {} : Base(),
epsilon_(epsilon),
a_(-1 / (epsilon * epsilon)),
b_(2 / epsilon) {}
virtual double operator()(const double& x, OptionalJacobian<1, 1> H = {}) const override; virtual double operator()(const double& x,
OptionalJacobian<1, 1> H = {}) const override;
}; };
/** Softplus function that implements f(x) = log(1 + exp(k*x)) / k. */ /** Softplus function that implements f(x) = log(1 + exp(k*x)) / k. */
class SoftPlusFunction : public SmoothRampFunction { class GTSAM_EXPORT SoftPlusFunction : public InequalityPenaltyFunction {
public: public:
typedef SmoothRampFunction Base; typedef InequalityPenaltyFunction Base;
typedef SoftPlusFunction This; typedef SoftPlusFunction This;
typedef std::shared_ptr<This> shared_ptr; typedef std::shared_ptr<This> shared_ptr;
@ -109,7 +133,8 @@ class SoftPlusFunction : public SmoothRampFunction {
public: public:
SoftPlusFunction(const double k = 1) : Base(), k_(k) {} SoftPlusFunction(const double k = 1) : Base(), k_(k) {}
virtual double operator()(const double& x, OptionalJacobian<1, 1> H = {}) const override; virtual double operator()(const double& x,
OptionalJacobian<1, 1> H = {}) const override;
}; };
} // namespace gtsam } // namespace gtsam

9
gtsam/constraint/NonlinearConstraint.h
View File

@ -31,7 +31,7 @@ namespace gtsam {
* whitenedError() returns The constraint violation vector. * whitenedError() returns The constraint violation vector.
* unwhitenedError() returns the sigma-scaled constraint violation vector. * unwhitenedError() returns the sigma-scaled constraint violation vector.
*/ */
class NonlinearConstraint : public NoiseModelFactor { class GTSAM_EXPORT NonlinearConstraint : public NoiseModelFactor {
public: public:
typedef NoiseModelFactor Base; typedef NoiseModelFactor Base;
@ -54,6 +54,13 @@ class NonlinearConstraint : public NoiseModelFactor {
return violation(x) <= tolerance; return violation(x) <= tolerance;
} }
const Vector sigmas() const { return noiseModel()->sigmas(); }
// return the hessian of unwhitened error function in each dimension.
virtual std::vector<Matrix> unwhitenedHessian(const Values& x) const {
throw std::runtime_error("hessian not implemented");
}
protected: protected:
/** Noise model used for the penalty function. */ /** Noise model used for the penalty function. */
SharedDiagonal penaltyNoise(const double mu) const { SharedDiagonal penaltyNoise(const double mu) const {

6
gtsam/constraint/NonlinearEqualityConstraint-inl.h
View File

@ -21,7 +21,7 @@
namespace gtsam { namespace gtsam {
/* ************************************************************************* */ /* ********************************************************************************************* */
template <typename T> template <typename T>
ExpressionEqualityConstraint<T>::ExpressionEqualityConstraint(const Expression<T>& expression, ExpressionEqualityConstraint<T>::ExpressionEqualityConstraint(const Expression<T>& expression,
const T& rhs, const T& rhs,
@ -31,7 +31,7 @@ ExpressionEqualityConstraint<T>::ExpressionEqualityConstraint(const Expression<T
rhs_(rhs), rhs_(rhs),
dims_(expression.keysAndDims().second) {} dims_(expression.keysAndDims().second) {}
/* ************************************************************************* */ /* ********************************************************************************************* */
template <typename T> template <typename T>
Vector ExpressionEqualityConstraint<T>::unwhitenedError(const Values& x, Vector ExpressionEqualityConstraint<T>::unwhitenedError(const Values& x,
OptionalMatrixVecType H) const { OptionalMatrixVecType H) const {
@ -45,7 +45,7 @@ Vector ExpressionEqualityConstraint<T>::unwhitenedError(const Values& x,
} }
} }
/* ************************************************************************* */ /* ********************************************************************************************* */
template <typename T> template <typename T>
NoiseModelFactor::shared_ptr ExpressionEqualityConstraint<T>::penaltyFactor(const double mu) const { NoiseModelFactor::shared_ptr ExpressionEqualityConstraint<T>::penaltyFactor(const double mu) const {
return std::make_shared<ExpressionFactor<T>>(penaltyNoise(mu), rhs_, expression_); return std::make_shared<ExpressionFactor<T>>(penaltyNoise(mu), rhs_, expression_);

16
gtsam/constraint/NonlinearEqualityConstraint.cpp
View File

@ -19,21 +19,21 @@
namespace gtsam { namespace gtsam {
/* ************************************************************************* */ /* ********************************************************************************************* */
ZeroCostConstraint::ZeroCostConstraint(const NoiseModelFactor::shared_ptr& factor) ZeroCostConstraint::ZeroCostConstraint(const NoiseModelFactor::shared_ptr& factor)
: Base(constrainedNoise(factor->noiseModel()->sigmas()), factor->keys()), factor_(factor) {} : Base(constrainedNoise(factor->noiseModel()->sigmas()), factor->keys()), factor_(factor) {}
/* ************************************************************************* */ /* ********************************************************************************************* */
Vector ZeroCostConstraint::unwhitenedError(const Values& x, OptionalMatrixVecType H) const { Vector ZeroCostConstraint::unwhitenedError(const Values& x, OptionalMatrixVecType H) const {
return factor_->unwhitenedError(x, H); return factor_->unwhitenedError(x, H);
} }
/* ************************************************************************* */ /* ********************************************************************************************* */
NoiseModelFactor::shared_ptr ZeroCostConstraint::penaltyFactor(const double mu) const { NoiseModelFactor::shared_ptr ZeroCostConstraint::penaltyFactor(const double mu) const {
return factor_->cloneWithNewNoiseModel(penaltyNoise(mu)); return factor_->cloneWithNewNoiseModel(penaltyNoise(mu));
} }
/* ************************************************************************* */ /* ********************************************************************************************* */
NonlinearEqualityConstraints NonlinearEqualityConstraints::FromCostGraph( NonlinearEqualityConstraints NonlinearEqualityConstraints::FromCostGraph(
const NonlinearFactorGraph& graph) { const NonlinearFactorGraph& graph) {
NonlinearEqualityConstraints constraints; NonlinearEqualityConstraints constraints;
@ -44,7 +44,7 @@ NonlinearEqualityConstraints NonlinearEqualityConstraints::FromCostGraph(
return constraints; return constraints;
} }
/* ************************************************************************* */ /* ********************************************************************************************* */
size_t NonlinearEqualityConstraints::dim() const { size_t NonlinearEqualityConstraints::dim() const {
size_t dimension = 0; size_t dimension = 0;
for (const auto& constraint : *this) { for (const auto& constraint : *this) {
@ -53,7 +53,7 @@ size_t NonlinearEqualityConstraints::dim() const {
return dimension; return dimension;
} }
/* ************************************************************************* */ /* ********************************************************************************************* */
Vector NonlinearEqualityConstraints::violationVector(const Values& values, bool whiten) const { Vector NonlinearEqualityConstraints::violationVector(const Values& values, bool whiten) const {
Vector violation(dim()); Vector violation(dim());
size_t start_idx = 0; size_t start_idx = 0;
@ -66,12 +66,12 @@ Vector NonlinearEqualityConstraints::violationVector(const Values& values, bool
return violation; return violation;
} }
/* ************************************************************************* */ /* ********************************************************************************************* */
double NonlinearEqualityConstraints::violationNorm(const Values& values) const { double NonlinearEqualityConstraints::violationNorm(const Values& values) const {
return violationVector(values, true).norm(); return violationVector(values, true).norm();
} }
/* ************************************************************************* */ /* ********************************************************************************************* */
NonlinearFactorGraph NonlinearEqualityConstraints::penaltyGraph(const double mu) const { NonlinearFactorGraph NonlinearEqualityConstraints::penaltyGraph(const double mu) const {
NonlinearFactorGraph graph; NonlinearFactorGraph graph;
for (const auto& constraint : *this) { for (const auto& constraint : *this) {

8
gtsam/constraint/NonlinearEqualityConstraint.h
View File

@ -26,7 +26,7 @@ namespace gtsam {
/** /**
* Equality constraint base class. * Equality constraint base class.
*/ */
class NonlinearEqualityConstraint : public NonlinearConstraint { class GTSAM_EXPORT NonlinearEqualityConstraint : public NonlinearConstraint {
public: public:
typedef NonlinearConstraint Base; typedef NonlinearConstraint Base;
typedef NonlinearEqualityConstraint This; typedef NonlinearEqualityConstraint This;
@ -52,7 +52,7 @@ class NonlinearEqualityConstraint : public NonlinearConstraint {
/** Equality constraint that force g(x) = M. */ /** Equality constraint that force g(x) = M. */
template <typename T> template <typename T>
class ExpressionEqualityConstraint : public NonlinearEqualityConstraint { class GTSAM_EXPORT ExpressionEqualityConstraint : public NonlinearEqualityConstraint {
public: public:
typedef NonlinearEqualityConstraint Base; typedef NonlinearEqualityConstraint Base;
typedef ExpressionEqualityConstraint This; typedef ExpressionEqualityConstraint This;
@ -100,7 +100,7 @@ class ExpressionEqualityConstraint : public NonlinearEqualityConstraint {
}; };
/** Equality constraint that enforce the cost factor with zero error. */ /** Equality constraint that enforce the cost factor with zero error. */
class ZeroCostConstraint : public NonlinearEqualityConstraint { class GTSAM_EXPORT ZeroCostConstraint : public NonlinearEqualityConstraint {
public: public:
typedef NonlinearEqualityConstraint Base; typedef NonlinearEqualityConstraint Base;
typedef ZeroCostConstraint This; typedef ZeroCostConstraint This;
@ -142,7 +142,7 @@ class ZeroCostConstraint : public NonlinearEqualityConstraint {
}; };
/// Container of NonlinearEqualityConstraint. /// Container of NonlinearEqualityConstraint.
class NonlinearEqualityConstraints : public FactorGraph<NonlinearEqualityConstraint> { class GTSAM_EXPORT NonlinearEqualityConstraints : public FactorGraph<NonlinearEqualityConstraint> {
public: public:
typedef std::shared_ptr<NonlinearEqualityConstraints> shared_ptr; typedef std::shared_ptr<NonlinearEqualityConstraints> shared_ptr;
typedef FactorGraph<NonlinearEqualityConstraint> Base; typedef FactorGraph<NonlinearEqualityConstraint> Base;

61
gtsam/constraint/NonlinearInequalityConstraint.cpp
View File

@ -17,11 +17,15 @@
*/ */
#include <gtsam/constraint/NonlinearInequalityConstraint.h> #include <gtsam/constraint/NonlinearInequalityConstraint.h>
#include <gtsam/constraint/RampFunction.h>
namespace gtsam { namespace gtsam {
/* ************************************************************************* */ /* ********************************************************************************************* */
Vector NonlinearInequalityConstraint::whitenedExpr(const Values& x) const {
return noiseModel()->whiten(unwhitenedExpr(x));
}
/* ********************************************************************************************* */
Vector NonlinearInequalityConstraint::unwhitenedError(const Values& x, Vector NonlinearInequalityConstraint::unwhitenedError(const Values& x,
OptionalMatrixVecType H) const { OptionalMatrixVecType H) const {
Vector error = unwhitenedExpr(x, H); Vector error = unwhitenedExpr(x, H);
@ -38,52 +42,52 @@ Vector NonlinearInequalityConstraint::unwhitenedError(const Values& x,
return error; return error;
} }
/* ************************************************************************* */ /* ********************************************************************************************* */
bool NonlinearInequalityConstraint::active(const Values& x) const { bool NonlinearInequalityConstraint::active(const Values& x) const {
return (unwhitenedExpr(x).array() >= 0).any(); return (unwhitenedExpr(x).array() >= 0).any();
} }
/* ************************************************************************* */ /* ********************************************************************************************* */
NoiseModelFactor::shared_ptr NonlinearInequalityConstraint::penaltyFactorSmooth( NoiseModelFactor::shared_ptr NonlinearInequalityConstraint::penaltyFactorCustom(
SmoothRampFunction::shared_ptr func, const double mu) const { InequalityPenaltyFunction::shared_ptr func, const double mu) const {
/// Default behavior, this function should be overriden. /// Default behavior, this function should be overriden.
return penaltyFactor(mu); return penaltyFactor(mu);
} }
/* ************************************************************************* */ /* ********************************************************************************************* */
NonlinearEqualityConstraint::shared_ptr NonlinearInequalityConstraint::createEqualityConstraint() NonlinearEqualityConstraint::shared_ptr NonlinearInequalityConstraint::createEqualityConstraint()
const { const {
/// Default behavior, this function should be overriden. /// Default behavior, this function should be overriden.
return nullptr; return nullptr;
} }
/* ************************************************************************* */ /* ********************************************************************************************* */
NoiseModelFactor::shared_ptr NonlinearInequalityConstraint::penaltyFactorEquality( NoiseModelFactor::shared_ptr NonlinearInequalityConstraint::penaltyFactorEquality(
const double mu) const { const double mu) const {
return createEqualityConstraint()->penaltyFactor(mu); return createEqualityConstraint()->penaltyFactor(mu);
} }
/* ************************************************************************* */ /* ********************************************************************************************* */
ScalarExpressionInequalityConstraint::ScalarExpressionInequalityConstraint( ScalarExpressionInequalityConstraint::ScalarExpressionInequalityConstraint(
const Double_& expression, const double& sigma) const Double_& expression, const double& sigma)
: Base(constrainedNoise(Vector1(sigma)), expression.keysAndDims().first), : Base(constrainedNoise(Vector1(sigma)), expression.keysAndDims().first),
expression_(expression), expression_(expression),
dims_(expression.keysAndDims().second) {} dims_(expression.keysAndDims().second) {}
/* ************************************************************************* */ /* ********************************************************************************************* */
ScalarExpressionInequalityConstraint::shared_ptr ScalarExpressionInequalityConstraint::GeqZero( ScalarExpressionInequalityConstraint::shared_ptr ScalarExpressionInequalityConstraint::GeqZero(
const Double_& expression, const double& sigma) { const Double_& expression, const double& sigma) {
Double_ neg_expr = Double_(0.0) - expression; Double_ neg_expr = Double_(0.0) - expression;
return std::make_shared<ScalarExpressionInequalityConstraint>(neg_expr, sigma); return std::make_shared<ScalarExpressionInequalityConstraint>(neg_expr, sigma);
} }
/* ************************************************************************* */ /* ********************************************************************************************* */
ScalarExpressionInequalityConstraint::shared_ptr ScalarExpressionInequalityConstraint::LeqZero( ScalarExpressionInequalityConstraint::shared_ptr ScalarExpressionInequalityConstraint::LeqZero(
const Double_& expression, const double& sigma) { const Double_& expression, const double& sigma) {
return std::make_shared<ScalarExpressionInequalityConstraint>(expression, sigma); return std::make_shared<ScalarExpressionInequalityConstraint>(expression, sigma);
} }
/* ************************************************************************* */ /* ********************************************************************************************* */
Vector ScalarExpressionInequalityConstraint::unwhitenedExpr(const Values& x, Vector ScalarExpressionInequalityConstraint::unwhitenedExpr(const Values& x,
OptionalMatrixVecType H) const { OptionalMatrixVecType H) const {
// Copy-paste from ExpressionFactor. // Copy-paste from ExpressionFactor.
@ -94,35 +98,38 @@ Vector ScalarExpressionInequalityConstraint::unwhitenedExpr(const Values& x,
} }
} }
/* ************************************************************************* */ /* ********************************************************************************************* */
NonlinearEqualityConstraint::shared_ptr NonlinearEqualityConstraint::shared_ptr
ScalarExpressionInequalityConstraint::createEqualityConstraint() const { ScalarExpressionInequalityConstraint::createEqualityConstraint() const {
return std::make_shared<ExpressionEqualityConstraint<double>>( return std::make_shared<ExpressionEqualityConstraint<double>>(
expression_, 0.0, noiseModel()->sigmas()); expression_, 0.0, noiseModel()->sigmas());
} }
/* ************************************************************************* */ /* ********************************************************************************************* */
NoiseModelFactor::shared_ptr ScalarExpressionInequalityConstraint::penaltyFactor( NoiseModelFactor::shared_ptr ScalarExpressionInequalityConstraint::penaltyFactor(
const double mu) const { const double mu) const {
Double_ penalty_expression(RampFunction, expression_); Double_ penalty_expression(RampFunction::Ramp, expression_);
return std::make_shared<ExpressionFactor<double>>(penaltyNoise(mu), 0.0, penalty_expression); return std::make_shared<ExpressionFactor<double>>(penaltyNoise(mu), 0.0, penalty_expression);
} }
/* ************************************************************************* */ /* ********************************************************************************************* */
NoiseModelFactor::shared_ptr ScalarExpressionInequalityConstraint::penaltyFactorSmooth( NoiseModelFactor::shared_ptr ScalarExpressionInequalityConstraint::penaltyFactorCustom(
SmoothRampFunction::shared_ptr func, const double mu) const { InequalityPenaltyFunction::shared_ptr func, const double mu) const {
if (!func) {
return penaltyFactor(mu);
}
// TODO(yetong): can we pass the functor directly to construct the expression? // TODO(yetong): can we pass the functor directly to construct the expression?
Double_ error(func->function(), expression_); Double_ error(func->function(), expression_);
return std::make_shared<ExpressionFactor<double>>(penaltyNoise(mu), 0.0, error); return std::make_shared<ExpressionFactor<double>>(penaltyNoise(mu), 0.0, error);
} }
/* ************************************************************************* */ /* ********************************************************************************************* */
NoiseModelFactor::shared_ptr ScalarExpressionInequalityConstraint::penaltyFactorEquality( NoiseModelFactor::shared_ptr ScalarExpressionInequalityConstraint::penaltyFactorEquality(
const double mu) const { const double mu) const {
return std::make_shared<ExpressionFactor<double>>(penaltyNoise(mu), 0.0, expression_); return std::make_shared<ExpressionFactor<double>>(penaltyNoise(mu), 0.0, expression_);
} }
/* ************************************************************************* */ /* ********************************************************************************************* */
size_t NonlinearInequalityConstraints::dim() const { size_t NonlinearInequalityConstraints::dim() const {
size_t dimension = 0; size_t dimension = 0;
for (const auto& constraint : *this) { for (const auto& constraint : *this) {
@ -131,7 +138,7 @@ size_t NonlinearInequalityConstraints::dim() const {
return dimension; return dimension;
} }
/* ************************************************************************* */ /* ********************************************************************************************* */
Vector NonlinearInequalityConstraints::violationVector(const Values& values, bool whiten) const { Vector NonlinearInequalityConstraints::violationVector(const Values& values, bool whiten) const {
Vector violation(dim()); Vector violation(dim());
size_t start_idx = 0; size_t start_idx = 0;
@ -144,12 +151,12 @@ Vector NonlinearInequalityConstraints::violationVector(const Values& values, boo
return violation; return violation;
} }
/* ************************************************************************* */ /* ********************************************************************************************* */
double NonlinearInequalityConstraints::violationNorm(const Values& values) const { double NonlinearInequalityConstraints::violationNorm(const Values& values) const {
return violationVector(values, true).norm(); return violationVector(values, true).norm();
} }
/* ************************************************************************* */ /* ********************************************************************************************* */
NonlinearFactorGraph NonlinearInequalityConstraints::penaltyGraph(const double mu) const { NonlinearFactorGraph NonlinearInequalityConstraints::penaltyGraph(const double mu) const {
NonlinearFactorGraph graph; NonlinearFactorGraph graph;
for (const auto& constraint : *this) { for (const auto& constraint : *this) {
@ -158,12 +165,12 @@ NonlinearFactorGraph NonlinearInequalityConstraints::penaltyGraph(const double m
return graph; return graph;
} }
/* ************************************************************************* */ /* ********************************************************************************************* */
NonlinearFactorGraph NonlinearInequalityConstraints::penaltyGraphSmooth( NonlinearFactorGraph NonlinearInequalityConstraints::penaltyGraphCustom(
SmoothRampFunction::shared_ptr func, const double mu) const { InequalityPenaltyFunction::shared_ptr func, const double mu) const {
NonlinearFactorGraph graph; NonlinearFactorGraph graph;
for (const auto& constraint : *this) { for (const auto& constraint : *this) {
graph.add(constraint->penaltyFactorSmooth(func, mu)); graph.add(constraint->penaltyFactorCustom(func, mu));
} }
return graph; return graph;
} }

23
gtsam/constraint/NonlinearInequalityConstraint.h
View File

@ -18,8 +18,8 @@
#pragma once #pragma once
#include <gtsam/constraint/InequalityPenaltyFunction.h>
#include <gtsam/constraint/NonlinearEqualityConstraint.h> #include <gtsam/constraint/NonlinearEqualityConstraint.h>
#include <gtsam/constraint/RampFunction.h>
#include <gtsam/nonlinear/expressions.h> #include <gtsam/nonlinear/expressions.h>
namespace gtsam { namespace gtsam {
@ -27,7 +27,7 @@ namespace gtsam {
/** /**
* Inequality constraint base class, enforcing g(x) <= 0. * Inequality constraint base class, enforcing g(x) <= 0.
*/ */
class NonlinearInequalityConstraint : public NonlinearConstraint { class GTSAM_EXPORT NonlinearInequalityConstraint : public NonlinearConstraint {
public: public:
typedef NonlinearConstraint Base; typedef NonlinearConstraint Base;
typedef NonlinearInequalityConstraint This; typedef NonlinearInequalityConstraint This;
@ -42,6 +42,8 @@ class NonlinearInequalityConstraint : public NonlinearConstraint {
/** Return g(x). */ /** Return g(x). */
virtual Vector unwhitenedExpr(const Values& x, OptionalMatrixVecType H = nullptr) const = 0; virtual Vector unwhitenedExpr(const Values& x, OptionalMatrixVecType H = nullptr) const = 0;
virtual Vector whitenedExpr(const Values& x) const;
/** Return ramp(g(x)). */ /** Return ramp(g(x)). */
virtual Vector unwhitenedError(const Values& x, OptionalMatrixVecType H = nullptr) const override; virtual Vector unwhitenedError(const Values& x, OptionalMatrixVecType H = nullptr) const override;
@ -51,9 +53,9 @@ class NonlinearInequalityConstraint : public NonlinearConstraint {
/** Return an equality constraint corresponding to g(x)=0. */ /** Return an equality constraint corresponding to g(x)=0. */
virtual NonlinearEqualityConstraint::shared_ptr createEqualityConstraint() const; virtual NonlinearEqualityConstraint::shared_ptr createEqualityConstraint() const;
/** Smooth approximation of the ramp function. */ /** Cost factor using a customized penalty function. */
virtual NoiseModelFactor::shared_ptr penaltyFactorSmooth(SmoothRampFunction::shared_ptr func, virtual NoiseModelFactor::shared_ptr penaltyFactorCustom(
const double mu = 1.0) const; InequalityPenaltyFunction::shared_ptr func, const double mu = 1.0) const;
/** penalty function as if the constraint is equality, 0.5 * mu * ||g(x)||^2 */ /** 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; virtual NoiseModelFactor::shared_ptr penaltyFactorEquality(const double mu = 1.0) const;
@ -73,7 +75,7 @@ class NonlinearInequalityConstraint : public NonlinearConstraint {
/** Inequality constraint that force g(x) <= 0, where g(x) is a scalar-valued /** Inequality constraint that force g(x) <= 0, where g(x) is a scalar-valued
* nonlinear function. * nonlinear function.
*/ */
class ScalarExpressionInequalityConstraint : public NonlinearInequalityConstraint { class GTSAM_EXPORT ScalarExpressionInequalityConstraint : public NonlinearInequalityConstraint {
public: public:
typedef NonlinearInequalityConstraint Base; typedef NonlinearInequalityConstraint Base;
typedef ScalarExpressionInequalityConstraint This; typedef ScalarExpressionInequalityConstraint This;
@ -111,7 +113,7 @@ class ScalarExpressionInequalityConstraint : public NonlinearInequalityConstrain
NoiseModelFactor::shared_ptr penaltyFactor(const double mu = 1.0) const override; NoiseModelFactor::shared_ptr penaltyFactor(const double mu = 1.0) const override;
/** Penalty function using a smooth approxiamtion of the ramp funciton. */ /** Penalty function using a smooth approxiamtion of the ramp funciton. */
NoiseModelFactor::shared_ptr penaltyFactorSmooth(SmoothRampFunction::shared_ptr func, NoiseModelFactor::shared_ptr penaltyFactorCustom(InequalityPenaltyFunction::shared_ptr func,
const double mu = 1.0) const override; const double mu = 1.0) const override;
/** Penalty function as if the constraint is equality, 0.5 * mu * ||g(x)/sigma||^2. */ /** Penalty function as if the constraint is equality, 0.5 * mu * ||g(x)/sigma||^2. */
@ -141,7 +143,8 @@ class ScalarExpressionInequalityConstraint : public NonlinearInequalityConstrain
}; };
/// Container of NonlinearInequalityConstraint. /// Container of NonlinearInequalityConstraint.
class NonlinearInequalityConstraints : public FactorGraph<NonlinearInequalityConstraint> { class GTSAM_EXPORT NonlinearInequalityConstraints
: public FactorGraph<NonlinearInequalityConstraint> {
public: public:
typedef FactorGraph<NonlinearInequalityConstraint> Base; typedef FactorGraph<NonlinearInequalityConstraint> Base;
typedef NonlinearInequalityConstraints This; typedef NonlinearInequalityConstraints This;
@ -161,8 +164,8 @@ class NonlinearInequalityConstraints : public FactorGraph<NonlinearInequalityCon
/** Return the penalty function corresponding to \sum_i||ramp(g_i(x))||^2. */ /** Return the penalty function corresponding to \sum_i||ramp(g_i(x))||^2. */
NonlinearFactorGraph penaltyGraph(const double mu = 1.0) const; NonlinearFactorGraph penaltyGraph(const double mu = 1.0) const;
/** Return the penalty function constructed using smooth approximations of the ramp function. */ /** Return the cost graph constructed using a customized penalty function. */
NonlinearFactorGraph penaltyGraphSmooth(SmoothRampFunction::shared_ptr func, NonlinearFactorGraph penaltyGraphCustom(InequalityPenaltyFunction::shared_ptr func,
const double mu = 1.0) const; const double mu = 1.0) const;
private: private:

141
gtsam/constraint/tests/constrainedExample.h
View File

@ -18,30 +18,25 @@
#pragma once #pragma once
#include <gtsam/base/Matrix.h> #include <gtsam/constraint/ConstrainedOptProblem.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/nonlinear/expressions.h>
#include <gtsam/slam/BetweenFactor.h> #include <gtsam/slam/BetweenFactor.h>
#include <gtsam/constraint/NonlinearEqualityConstraint.h> #include "gtsam/constraint/NonlinearEqualityConstraint.h"
#include <gtsam/constraint/NonlinearInequalityConstraint.h> #include "gtsam/constraint/NonlinearInequalityConstraint.h"
namespace constrained_example { namespace constrained_example {
using namespace gtsam; using namespace gtsam;
/// Exponential function e^x. /// Exponential function e^x.
double exp_func(const double& x, gtsam::OptionalJacobian<1, 1> H1 = {}) { inline double exp_func(const double& x, gtsam::OptionalJacobian<1, 1> H1 = {}) {
double result = exp(x); double result = exp(x);
if (H1) H1->setConstant(result); if (H1) H1->setConstant(result);
return result; return result;
} }
/// Exponential expression e^x. /// Exponential expression e^x.
Double_ exp(const Double_& x) { return Double_(exp_func, x); } inline Double_ exp(const Double_& x) { return Double_(exp_func, x); }
/// Pow functor used for pow function. /// Pow functor used for pow function.
class PowFunctor { class PowFunctor {
@ -51,24 +46,23 @@ class PowFunctor {
public: public:
PowFunctor(const double& c) : c_(c) {} PowFunctor(const double& c) : c_(c) {}
double operator()(const double& x, double operator()(const double& x, gtsam::OptionalJacobian<1, 1> H1 = {}) const {
gtsam::OptionalJacobian<1, 1> H1 = {}) const {
if (H1) H1->setConstant(c_ * pow(x, c_ - 1)); if (H1) H1->setConstant(c_ * pow(x, c_ - 1));
return pow(x, c_); return pow(x, c_);
} }
}; };
/// Pow function. /// Pow function.
Double_ pow(const Double_& x, const double& c) { inline Double_ pow(const Double_& x, const double& c) {
PowFunctor pow_functor(c); PowFunctor pow_functor(c);
return Double_(pow_functor, x); return Double_(pow_functor, x);
} }
/// Plus between Double expression and double. /// Plus between Double expression and double.
Double_ operator+(const Double_& x, const double& d) { return x + Double_(d); } inline Double_ operator+(const Double_& x, const double& d) { return x + Double_(d); }
/// Negative sign operator. /// Negative sign operator.
Double_ operator-(const Double_& x) { return Double_(0.0) - x; } inline Double_ operator-(const Double_& x) { return Double_(0.0) - x; }
/// Keys for creating expressions. /// Keys for creating expressions.
Symbol x1_key('x', 1); Symbol x1_key('x', 1);
@ -82,11 +76,11 @@ Double_ x1(x1_key), x2(x2_key);
* Constrained optimization example in L. Vandenberghe slides: * Constrained optimization example in L. Vandenberghe slides:
* https://www.seas.ucla.edu/~vandenbe/133B/lectures/nllseq.pdf * 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 * f(x) = 0.5*||x1 + e^(-x2)||^2 + 0.5*||x1^2 + 2*x2 + 1||^2
* h(x) = x1 + x1^3 + x2 + x2^2 * h(x) = x1 + x1^3 + x2 + x2^2 = 0
*/ */
namespace e_constrained_example { namespace constrained_example1 {
using namespace constrained_example; using namespace constrained_example;
NonlinearFactorGraph GetCost() { NonlinearFactorGraph Cost() {
NonlinearFactorGraph graph; NonlinearFactorGraph graph;
auto f1 = x1 + exp(-x2); auto f1 = x1 + exp(-x2);
auto f2 = pow(x1, 2.0) + 2.0 * x2 + 1.0; auto f2 = pow(x1, 2.0) + 2.0 * x2 + 1.0;
@ -96,28 +90,49 @@ NonlinearFactorGraph GetCost() {
return graph; return graph;
} }
NonlinearEqualityConstraints GetConstraints() { NonlinearEqualityConstraints EqConstraints() {
NonlinearEqualityConstraints constraints; NonlinearEqualityConstraints constraints;
Vector scale = Vector1(0.1); Vector sigmas = Vector1(1.0);
auto h1 = x1 + pow(x1, 3) + x2 + pow(x2, 2); auto h1 = x1 + pow(x1, 3) + x2 + pow(x2, 2);
constraints.push_back(NonlinearEqualityConstraint::shared_ptr( constraints.push_back(NonlinearEqualityConstraint::shared_ptr(
new ExpressionEqualityConstraint<double>(h1, 0.0, scale))); new ExpressionEqualityConstraint<double>(h1, 0.0, sigmas)));
return constraints; return constraints;
} }
NonlinearFactorGraph cost = GetCost(); Values InitValues() {
NonlinearEqualityConstraints constraints = GetConstraints(); Values values;
} // namespace e_constrained_example 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;
}
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 * 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 * f(x) = 0.5 * ||x1-1||^2 + 0.5 * ||x2-1||^2
* g(x) = 1 - x1^2 - x2^2 * h(x) = x1^2 + x2^2 - 1 = 0
* g(x) = 4*x1^2 + 0.25*x2^2 - 1 <= 0
*/ */
namespace i_constrained_example { namespace constrained_example2 {
using namespace constrained_example; using namespace constrained_example;
NonlinearFactorGraph GetCost() { NonlinearFactorGraph Cost() {
NonlinearFactorGraph graph; NonlinearFactorGraph graph;
auto cost_noise = gtsam::noiseModel::Isotropic::Sigma(1, 1.0); auto cost_noise = gtsam::noiseModel::Isotropic::Sigma(1, 1.0);
graph.addPrior(x1_key, 1.0, cost_noise); graph.addPrior(x1_key, 1.0, cost_noise);
@ -125,44 +140,42 @@ NonlinearFactorGraph GetCost() {
return graph; return graph;
} }
// InequalityConstraints GetIConstraints() { NonlinearEqualityConstraints EqConstraints() {
// InequalityConstraints i_constraints; NonlinearEqualityConstraints constraints;
// Double_ g1 = Double_(1.0) - x1 * x1 - x2 * x2; Vector1 sigmas(1.0);
// double tolerance = 0.2; Double_ h1 = x1 * x1 + x2 * x2;
// i_constraints.emplace_shared<DoubleExpressionInequality>(g1, tolerance); constraints.emplace_shared<ExpressionEqualityConstraint<double>>(h1, 1.0, sigmas);
// return i_constraints; return 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() { NonlinearInequalityConstraints IneqConstraints() {
NonlinearEqualityConstraints e_constraints; NonlinearInequalityConstraints constraints;
// Double_ h1 = x1 + x2 - Double_(1.0); Double_ g1 = 4 * x1 * x1 + 0.25 * x2 * x2 - Double_(1.0);
// double tolerance = 0.2; double sigma = 1;
// e_constraints.emplace_shared<DoubleExpressionEquality>(h1, tolerance); constraints.emplace_shared<ScalarExpressionInequalityConstraint>(g1, sigma);
return e_constraints; return constraints;
} }
NonlinearFactorGraph cost = GetCost(); Values InitValues() {
NonlinearEqualityConstraints e_constraints = GetEConstraints(); Values values;
} // namespace i_constrained_example 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;
}
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

16
gtsam/constraint/tests/testRampFunction.cpp → gtsam/constraint/tests/testInequalityPenaltyFunction.cpp
View File

@ -10,7 +10,7 @@
* -------------------------------------------------------------------------- */ * -------------------------------------------------------------------------- */
/** /**
* @file testRampFunction.h * @file testInequalityPenaltyFunction.h
* @brief unit tests for ramp functions * @brief unit tests for ramp functions
* @author Yetong Zhang * @author Yetong Zhang
* @date Aug 3, 2024 * @date Aug 3, 2024
@ -20,13 +20,14 @@
#include <gtsam/base/Testable.h> #include <gtsam/base/Testable.h>
#include <gtsam/base/TestableAssertions.h> #include <gtsam/base/TestableAssertions.h>
#include <gtsam/base/numericalDerivative.h> #include <gtsam/base/numericalDerivative.h>
#include <gtsam/constraint/RampFunction.h> #include <gtsam/constraint/InequalityPenaltyFunction.h>
using namespace gtsam; using namespace gtsam;
/* ********************************************************************************************* */
TEST(RampFunction, error_and_jacobian) { TEST(RampFunction, error_and_jacobian) {
/// Helper function for numerical Jacobian computation. /// Helper function for numerical Jacobian computation.
auto ramp_helper = [&](const double& x) { return RampFunction(x); }; auto ramp_helper = [&](const double& x) { return RampFunction::Ramp(x); };
/// Create a set of values to test the function. /// Create a set of values to test the function.
static std::vector<double> x_vec{-3.0, 0.0, 1.0, 2.0, 3.0}; static std::vector<double> x_vec{-3.0, 0.0, 1.0, 2.0, 3.0};
@ -35,7 +36,7 @@ TEST(RampFunction, error_and_jacobian) {
for (size_t i = 0; i < x_vec.size(); i++) { for (size_t i = 0; i < x_vec.size(); i++) {
double x = x_vec.at(i); double x = x_vec.at(i);
Matrix H; Matrix H;
double r = RampFunction(x, H); double r = RampFunction::Ramp(x, H);
/// Check function evaluation. /// Check function evaluation.
EXPECT_DOUBLES_EQUAL(expected_r_vec.at(i), r, 1e-9); EXPECT_DOUBLES_EQUAL(expected_r_vec.at(i), r, 1e-9);
@ -48,9 +49,10 @@ TEST(RampFunction, error_and_jacobian) {
} }
} }
/* ********************************************************************************************* */
TEST(RampFunctionPoly2, error_and_jacobian) { TEST(RampFunctionPoly2, error_and_jacobian) {
/// Helper function for numerical Jacobian computation. /// Helper function for numerical Jacobian computation.
RampFunctionPoly2 p_ramp(2.0); SmoothRampPoly2 p_ramp(2.0);
auto ramp_helper = [&](const double& x) { return p_ramp(x); }; auto ramp_helper = [&](const double& x) { return p_ramp(x); };
/// Create a set of values to test the function. /// Create a set of values to test the function.
@ -71,9 +73,10 @@ TEST(RampFunctionPoly2, error_and_jacobian) {
} }
} }
/* ********************************************************************************************* */
TEST(RampFunctionPoly3, error_and_jacobian) { TEST(RampFunctionPoly3, error_and_jacobian) {
/// Helper function for numerical Jacobian computation. /// Helper function for numerical Jacobian computation.
RampFunctionPoly3 p_ramp(2.0); SmoothRampPoly3 p_ramp(2.0);
auto ramp_helper = [&](const double& x) { return p_ramp(x); }; auto ramp_helper = [&](const double& x) { return p_ramp(x); };
/// Create a set of values to test the function. /// Create a set of values to test the function.
@ -94,6 +97,7 @@ TEST(RampFunctionPoly3, error_and_jacobian) {
} }
} }
/* ********************************************************************************************* */
TEST(SoftPlusFunction, error_and_jacobian) { TEST(SoftPlusFunction, error_and_jacobian) {
/// Helper function for numerical Jacobian computation. /// Helper function for numerical Jacobian computation.
SoftPlusFunction soft_plus(0.5); SoftPlusFunction soft_plus(0.5);

10
gtsam/constraint/tests/testNonlinearEqualityConstraint.cpp
View File

@ -22,7 +22,6 @@
#include <gtsam/base/TestableAssertions.h> #include <gtsam/base/TestableAssertions.h>
#include <gtsam/base/numericalDerivative.h> #include <gtsam/base/numericalDerivative.h>
#include <gtsam/constraint/NonlinearEqualityConstraint.h> #include <gtsam/constraint/NonlinearEqualityConstraint.h>
#include <gtsam/inference/VariableIndex.h>
#include <gtsam/nonlinear/factorTesting.h> #include <gtsam/nonlinear/factorTesting.h>
#include <gtsam/slam/BetweenFactor.h> #include <gtsam/slam/BetweenFactor.h>
@ -155,7 +154,6 @@ TEST(ExpressionEqualityConstraint, Vector2) {
EXPECT_CORRECT_FACTOR_JACOBIANS(*merit_factor, values2, 1e-7, 1e-5); EXPECT_CORRECT_FACTOR_JACOBIANS(*merit_factor, values2, 1e-7, 1e-5);
} }
// Test methods of FactorZeroErrorConstraint. // Test methods of FactorZeroErrorConstraint.
TEST(ZeroCostConstraint, BetweenFactor) { TEST(ZeroCostConstraint, BetweenFactor) {
Key x1_key = 1; Key x1_key = 1;
@ -218,9 +216,6 @@ TEST(ZeroCostConstraint, BetweenFactor) {
EXPECT_CORRECT_FACTOR_JACOBIANS(*merit_factor, values2, 1e-7, 1e-5); EXPECT_CORRECT_FACTOR_JACOBIANS(*merit_factor, values2, 1e-7, 1e-5);
} }
TEST(NonlinearEqualityConstraints, Container) { TEST(NonlinearEqualityConstraints, Container) {
NonlinearEqualityConstraints constraints; NonlinearEqualityConstraints constraints;
@ -254,12 +249,9 @@ TEST(NonlinearEqualityConstraints, Container) {
EXPECT(assert_container_equality(expected_vi_x2, vi[x2_key])); EXPECT(assert_container_equality(expected_vi_x2, vi[x2_key]));
// Check constraint violation. // Check constraint violation.
} }
TEST(NonlinearEqualityConstraints, FromCostGraph) {}
TEST(NonlinearEqualityConstraints, FromCostGraph) {
}
int main() { int main() {
TestResult tr; TestResult tr;