From 9b0fa1210e9cadb2d06f3dffc30b2712dd3aed1c Mon Sep 17 00:00:00 2001 From: yetongumich Date: Tue, 6 Aug 2024 15:42:09 -0400 Subject: [PATCH] clean up and refactor NonlinearEquality and BoundingConstraint --- gtsam/constraint/NonlinearConstraint.h | 2 +- .../NonlinearEqualityConstraint-inl.h | 4 +- .../NonlinearEqualityConstraint.cpp | 4 +- .../constraint/NonlinearEqualityConstraint.h | 19 ++- .../NonlinearInequalityConstraint.cpp | 17 ++- .../NonlinearInequalityConstraint.h | 40 ++++-- gtsam/constraint/RampFunction.cpp | 32 ++++- gtsam/constraint/RampFunction.h | 57 ++++++-- .../tests/testNonlinearEqualityConstraint.cpp | 4 +- .../testNonlinearInequalityConstraint.cpp | 4 +- gtsam/constraint/tests/testRampFunction.cpp | 124 ++++++++++++++++++ gtsam/nonlinear/NonlinearEquality.h | 74 +++++++---- gtsam/slam/BoundingConstraint.h | 65 +++++---- tests/testBoundingConstraint.cpp | 14 +- 14 files changed, 363 insertions(+), 97 deletions(-) create mode 100644 gtsam/constraint/tests/testRampFunction.cpp diff --git a/gtsam/constraint/NonlinearConstraint.h b/gtsam/constraint/NonlinearConstraint.h index 00bf4d52b..834ef7991 100644 --- a/gtsam/constraint/NonlinearConstraint.h +++ b/gtsam/constraint/NonlinearConstraint.h @@ -11,7 +11,7 @@ /** * @file NonlinearConstraint.h - * @brief Equality constraints in constrained optimization. + * @brief Nonlinear constraints in constrained optimization. * @author Yetong Zhang, Frank Dellaert * @date Aug 3, 2024 */ diff --git a/gtsam/constraint/NonlinearEqualityConstraint-inl.h b/gtsam/constraint/NonlinearEqualityConstraint-inl.h index a594d0b09..625f1e366 100644 --- a/gtsam/constraint/NonlinearEqualityConstraint-inl.h +++ b/gtsam/constraint/NonlinearEqualityConstraint-inl.h @@ -10,8 +10,8 @@ * -------------------------------------------------------------------------- */ /** - * @file EqualityConstraint-inl.h - * @brief Equality constraints in constrained optimization. + * @file NonlinearEqualityConstraint-inl.h + * @brief Nonlinear equality constraints in constrained optimization. * @author Yetong Zhang, Frank Dellaert * @date Aug 3, 2024 */ diff --git a/gtsam/constraint/NonlinearEqualityConstraint.cpp b/gtsam/constraint/NonlinearEqualityConstraint.cpp index 06201dc2a..0c5b7ea11 100644 --- a/gtsam/constraint/NonlinearEqualityConstraint.cpp +++ b/gtsam/constraint/NonlinearEqualityConstraint.cpp @@ -10,8 +10,8 @@ * -------------------------------------------------------------------------- */ /** - * @file EqualityConstraint.cpp - * @brief Equality constraints in constrained optimization. + * @file NonlinearEqualityConstraint.cpp + * @brief Nonlinear equality constraints in constrained optimization. * @author Yetong Zhang, Frank Dellaert * @date Aug 3, 2024 */ diff --git a/gtsam/constraint/NonlinearEqualityConstraint.h b/gtsam/constraint/NonlinearEqualityConstraint.h index 9f2305fa0..ab7dca2f0 100644 --- a/gtsam/constraint/NonlinearEqualityConstraint.h +++ b/gtsam/constraint/NonlinearEqualityConstraint.h @@ -10,8 +10,8 @@ * -------------------------------------------------------------------------- */ /** - * @file EqualityConstraint.h - * @brief Equality constraints in constrained optimization. + * @file NonlinearEqualityConstraint.h + * @brief Nonlinear equality constraints in constrained optimization. * @author Yetong Zhang, Frank Dellaert * @date Aug 3, 2024 */ @@ -20,9 +20,6 @@ #include #include #include -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION -#include -#endif namespace gtsam { @@ -81,6 +78,12 @@ class ExpressionEqualityConstraint : public NonlinearEqualityConstraint { const Expression& expression() const { return expression_; } + /// @return a deep copy of this factor + gtsam::NonlinearFactor::shared_ptr clone() const override { + return std::static_pointer_cast( + gtsam::NonlinearFactor::shared_ptr(new This(*this))); + } + private: #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ @@ -119,6 +122,12 @@ class ZeroCostConstraint : public NonlinearEqualityConstraint { virtual NoiseModelFactor::shared_ptr penaltyFactor(const double mu = 1.0) const override; + /// @return a deep copy of this factor + gtsam::NonlinearFactor::shared_ptr clone() const override { + return std::static_pointer_cast( + gtsam::NonlinearFactor::shared_ptr(new This(*this))); + } + private: #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ diff --git a/gtsam/constraint/NonlinearInequalityConstraint.cpp b/gtsam/constraint/NonlinearInequalityConstraint.cpp index f02fd8648..49003f09f 100644 --- a/gtsam/constraint/NonlinearInequalityConstraint.cpp +++ b/gtsam/constraint/NonlinearInequalityConstraint.cpp @@ -17,8 +17,7 @@ */ #include -#include -#include "gtsam/constraint/RampFunction.h" +#include namespace gtsam { @@ -44,6 +43,20 @@ bool NonlinearInequalityConstraint::active(const Values& x) const { return (unwhitenedExpr(x).array() >= 0).any(); } +/* ************************************************************************* */ +NoiseModelFactor::shared_ptr NonlinearInequalityConstraint::penaltyFactorSmooth( + SmoothRampFunction::shared_ptr func, const double mu) const { + /// Default behavior, this function should be overriden. + return penaltyFactor(mu); +} + +/* ************************************************************************* */ +NonlinearEqualityConstraint::shared_ptr NonlinearInequalityConstraint::createEqualityConstraint() + const { + /// Default behavior, this function should be overriden. + return nullptr; +} + /* ************************************************************************* */ NoiseModelFactor::shared_ptr NonlinearInequalityConstraint::penaltyFactorEquality( const double mu) const { diff --git a/gtsam/constraint/NonlinearInequalityConstraint.h b/gtsam/constraint/NonlinearInequalityConstraint.h index ac22df943..1a1813ea3 100644 --- a/gtsam/constraint/NonlinearInequalityConstraint.h +++ b/gtsam/constraint/NonlinearInequalityConstraint.h @@ -25,7 +25,7 @@ namespace gtsam { /** - * Inequality constraint base class. + * Inequality constraint base class, enforcing g(x) <= 0. */ class NonlinearInequalityConstraint : public NonlinearConstraint { public: @@ -39,17 +39,21 @@ class NonlinearInequalityConstraint : public NonlinearConstraint { /** Destructor. */ virtual ~NonlinearInequalityConstraint() {} + /** Return g(x). */ virtual Vector unwhitenedExpr(const Values& x, OptionalMatrixVecType H = nullptr) const = 0; + /** Return ramp(g(x)). */ virtual Vector unwhitenedError(const Values& x, OptionalMatrixVecType H = nullptr) const override; + /** Return true if g(x)>=0 in any dimension. */ virtual bool active(const Values& x) const override; - virtual NonlinearEqualityConstraint::shared_ptr createEqualityConstraint() const = 0; + /** Return an equality constraint corresponding to g(x)=0. */ + virtual NonlinearEqualityConstraint::shared_ptr createEqualityConstraint() const; /** Smooth approximation of the ramp function. */ virtual NoiseModelFactor::shared_ptr penaltyFactorSmooth(SmoothRampFunction::shared_ptr func, - const double mu = 1.0) const = 0; + const double mu = 1.0) const; /** 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; @@ -67,7 +71,8 @@ class NonlinearInequalityConstraint : public NonlinearConstraint { }; /** Inequality constraint that force g(x) <= 0, where g(x) is a scalar-valued - * function. */ + * nonlinear function. + */ class ScalarExpressionInequalityConstraint : public NonlinearInequalityConstraint { public: typedef NonlinearInequalityConstraint Base; @@ -82,32 +87,45 @@ class ScalarExpressionInequalityConstraint : public NonlinearInequalityConstrain /** * @brief Constructor. * - * @param expression expression representing g(x). + * @param expression expression representing g(x) (or -g(x) for GeqZero). * @param sigma scalar representing sigma. */ ScalarExpressionInequalityConstraint(const Double_& expression, const double& sigma); - // Create an inequality constraint g(x)>=0. + /** Create an inequality constraint g(x)/sigma >= 0, internally represented as -g(x)/sigma <= 0. + */ static ScalarExpressionInequalityConstraint::shared_ptr GeqZero(const Double_& expression, const double& sigma); - // Create an inequality constraint g(x)<=0. + /** Create an inequality constraint g(x)/sigma <= 0. */ static ScalarExpressionInequalityConstraint::shared_ptr LeqZero(const Double_& expression, const double& sigma); + /** Compute g(x), or -g(x) for objects constructed from GeqZero. */ virtual Vector unwhitenedExpr(const Values& x, OptionalMatrixVecType H = nullptr) const override; + /** Equality constraint representing g(x)/sigma = 0. */ NonlinearEqualityConstraint::shared_ptr createEqualityConstraint() const override; + /** Penalty function 0.5*mu*||ramp(g(x)/sigma||^2. */ NoiseModelFactor::shared_ptr penaltyFactor(const double mu = 1.0) const override; + /** Penalty function using a smooth approxiamtion of the ramp funciton. */ NoiseModelFactor::shared_ptr penaltyFactorSmooth(SmoothRampFunction::shared_ptr func, const double mu = 1.0) const override; + /** Penalty function as if the constraint is equality, 0.5 * mu * ||g(x)/sigma||^2. */ virtual NoiseModelFactor::shared_ptr penaltyFactorEquality(const double mu = 1.0) const override; + /** Return expression g(x), or -g(x) for objects constructed from GeqZero. */ const Double_& expression() const { return expression_; } + /** return a deep copy of this factor. */ + gtsam::NonlinearFactor::shared_ptr clone() const override { + return std::static_pointer_cast( + gtsam::NonlinearFactor::shared_ptr(new This(*this))); + } + private: #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ @@ -131,17 +149,19 @@ class NonlinearInequalityConstraints : public FactorGraph H) const { +double RampFunctionPoly2::operator()(const double& x, OptionalJacobian<1, 1> H) const { if (x <= 0) { if (H) { H->setZero(); @@ -49,9 +49,29 @@ double PolynomialRampFunction::operator()(const double& x, OptionalJacobian<1, 1 return 0; } else if (x < epsilon_) { if (H) { - H->setConstant(x / epsilon_); + H->setConstant(2 * a_ * x); } - return (x * x) / (2 * epsilon_) + epsilon_ / 2; + return a_ * pow(x, 2); + } else { + if (H) { + H->setOnes(); + } + return x - epsilon_ / 2; + } +} + +/* ************************************************************************* */ +double RampFunctionPoly3::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(3 * a_ * pow(x, 2) + 2 * b_ * x); + } + return a_ * pow(x, 3) + b_ * pow(x, 2); } else { if (H) { H->setOnes(); diff --git a/gtsam/constraint/RampFunction.h b/gtsam/constraint/RampFunction.h index d04e2dec0..0722551c5 100644 --- a/gtsam/constraint/RampFunction.h +++ b/gtsam/constraint/RampFunction.h @@ -10,9 +10,9 @@ * -------------------------------------------------------------------------- */ /** - * @file NonlinearInequalityConstraint.h - * @brief Nonlinear inequality constraints in constrained optimization. - * @author Yetong Zhang, Frank Dellaert + * @file RampFunction.h + * @brief Ramp function to compute inequality constraint violations. + * @author Yetong Zhang * @date Aug 3, 2024 */ @@ -20,10 +20,14 @@ #include #include +#include namespace gtsam { -/// Ramp function for create penalty factors. +/** Ramp function f : R -> R. + * f(x) = 0 for x <= 0 + * x for x > 0 + */ double RampFunction(const double x, OptionalJacobian<1, 1> H = {}); /** Base class for smooth approximation of the ramp function. */ @@ -43,20 +47,51 @@ class SmoothRampFunction { 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 +/** Ramp function approximated with a polynomial of degree 2 in [0, epsilon]. + * The coefficients are computed as + * a = 1 / (2 * epsilon) + * Function f(x) = 0 for x <= 0 + * a * x^2 for 0 < x < epsilon + * x - epsilon/2 for x >= epsilon */ -class PolynomialRampFunction : public SmoothRampFunction { +class RampFunctionPoly2 : public SmoothRampFunction { public: typedef SmoothRampFunction Base; + typedef RampFunctionPoly2 This; + typedef std::shared_ptr shared_ptr; protected: double epsilon_; + double a_; public: - PolynomialRampFunction(const double epsilon = 1) : Base(), epsilon_(epsilon) {} + RampFunctionPoly2(const double epsilon = 1) : Base(), epsilon_(epsilon), a_(0.5 / epsilon) {} + + virtual double operator()(const double& x, OptionalJacobian<1, 1> H = {}) const override; +}; + +/** Ramp function approximated with a polynomial of degree 3 in [0, epsilon]. + * The coefficients are computed as + * a = -1 / epsilon^2 + * b = 2 / epsilon + * Function f(x) = 0 for x <= 0 + * a * x^3 + b * x^2 for 0 < x < epsilon + * x for x >= epsilon + */ +class RampFunctionPoly3 : public SmoothRampFunction { + public: + typedef SmoothRampFunction Base; + typedef RampFunctionPoly3 This; + typedef std::shared_ptr shared_ptr; + + protected: + double epsilon_; + double a_; + double b_; + + public: + RampFunctionPoly3(const double epsilon = 1) + : Base(), epsilon_(epsilon), a_(-1 / (epsilon * epsilon)), b_(2 / epsilon) {} virtual double operator()(const double& x, OptionalJacobian<1, 1> H = {}) const override; }; @@ -65,6 +100,8 @@ class PolynomialRampFunction : public SmoothRampFunction { class SoftPlusFunction : public SmoothRampFunction { public: typedef SmoothRampFunction Base; + typedef SoftPlusFunction This; + typedef std::shared_ptr shared_ptr; protected: double k_; diff --git a/gtsam/constraint/tests/testNonlinearEqualityConstraint.cpp b/gtsam/constraint/tests/testNonlinearEqualityConstraint.cpp index ca2725d4c..8309de0b3 100644 --- a/gtsam/constraint/tests/testNonlinearEqualityConstraint.cpp +++ b/gtsam/constraint/tests/testNonlinearEqualityConstraint.cpp @@ -10,8 +10,8 @@ * -------------------------------------------------------------------------- */ /** - * @file testEqualityConstraint.cpp - * @brief Equality constraints in constrained optimization. + * @file testNonlinearEqualityConstraint.cpp + * @brief unit tests for nonlinear equality constraints * @author Yetong Zhang * @date Aug 3, 2024 */ diff --git a/gtsam/constraint/tests/testNonlinearInequalityConstraint.cpp b/gtsam/constraint/tests/testNonlinearInequalityConstraint.cpp index a73367d68..3950bfa03 100644 --- a/gtsam/constraint/tests/testNonlinearInequalityConstraint.cpp +++ b/gtsam/constraint/tests/testNonlinearInequalityConstraint.cpp @@ -10,8 +10,8 @@ * -------------------------------------------------------------------------- */ /** - * @file NonlinearInequalityConstraint.h - * @brief Nonlinear inequality constraints in constrained optimization. + * @file testNonlinearInequalityConstraint.h + * @brief unit tests for nonlinear inequality constraints * @author Yetong Zhang * @date Aug 3, 2024 */ diff --git a/gtsam/constraint/tests/testRampFunction.cpp b/gtsam/constraint/tests/testRampFunction.cpp new file mode 100644 index 000000000..2e8036d74 --- /dev/null +++ b/gtsam/constraint/tests/testRampFunction.cpp @@ -0,0 +1,124 @@ +/* ---------------------------------------------------------------------------- + + * 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 testRampFunction.h + * @brief unit tests for ramp functions + * @author Yetong Zhang + * @date Aug 3, 2024 + */ + +#include +#include +#include +#include +#include + +using namespace gtsam; + +TEST(RampFunction, error_and_jacobian) { + /// Helper function for numerical Jacobian computation. + auto ramp_helper = [&](const double& x) { return RampFunction(x); }; + + /// Create a set of values to test the function. + static std::vector x_vec{-3.0, 0.0, 1.0, 2.0, 3.0}; + static std::vector expected_r_vec{0.0, 0.0, 1.0, 2.0, 3.0}; + + for (size_t i = 0; i < x_vec.size(); i++) { + double x = x_vec.at(i); + Matrix H; + double r = RampFunction(x, H); + + /// Check function evaluation. + EXPECT_DOUBLES_EQUAL(expected_r_vec.at(i), r, 1e-9); + + /// Check derivative. + if (abs(x) > 1e-6) { // function is not smooth at 0, so Jacobian is undefined. + Matrix expected_H = gtsam::numericalDerivative11(ramp_helper, x, 1e-6); + EXPECT(assert_equal(expected_H, H)); + } + } +} + +TEST(RampFunctionPoly2, error_and_jacobian) { + /// Helper function for numerical Jacobian computation. + RampFunctionPoly2 p_ramp(2.0); + auto ramp_helper = [&](const double& x) { return p_ramp(x); }; + + /// Create a set of values to test the function. + static std::vector x_vec{-3.0, 0.0, 1.0, 2.0, 3.0}; + static std::vector expected_r_vec{0.0, 0.0, 0.25, 1.0, 2.0}; + + for (size_t i = 0; i < x_vec.size(); i++) { + double x = x_vec.at(i); + Matrix H; + double r = p_ramp(x, H); + + /// Check function evaluation. + EXPECT_DOUBLES_EQUAL(expected_r_vec.at(i), r, 1e-9); + + /// Check derivative. + Matrix expected_H = gtsam::numericalDerivative11(ramp_helper, x, 1e-6); + EXPECT(assert_equal(expected_H, H, 1e-6)); + } +} + +TEST(RampFunctionPoly3, error_and_jacobian) { + /// Helper function for numerical Jacobian computation. + RampFunctionPoly3 p_ramp(2.0); + auto ramp_helper = [&](const double& x) { return p_ramp(x); }; + + /// Create a set of values to test the function. + static std::vector x_vec{-3.0, 0.0, 1.0, 2.0, 3.0}; + static std::vector expected_r_vec{0.0, 0.0, 0.75, 2.0, 3.0}; + + for (size_t i = 0; i < x_vec.size(); i++) { + double x = x_vec.at(i); + Matrix H; + double r = p_ramp(x, H); + + /// Check function evaluation. + EXPECT_DOUBLES_EQUAL(expected_r_vec.at(i), r, 1e-9); + + /// Check derivative. + Matrix expected_H = gtsam::numericalDerivative11(ramp_helper, x, 1e-6); + EXPECT(assert_equal(expected_H, H, 1e-6)); + } +} + +TEST(SoftPlusFunction, error_and_jacobian) { + /// Helper function for numerical Jacobian computation. + SoftPlusFunction soft_plus(0.5); + auto soft_plus_helper = [&](const double& x) { return soft_plus(x); }; + + /// Create a set of values to test the function. + static std::vector x_vec{-3.0, 0.0, 1.0, 2.0, 3.0}; + static std::vector expected_r_vec{ + 0.40282656, 1.38629436, 1.94815397, 2.62652338, 3.40282656}; + + for (size_t i = 0; i < x_vec.size(); i++) { + double x = x_vec.at(i); + Matrix H; + double r = soft_plus(x, H); + + /// Check function evaluation. + EXPECT_DOUBLES_EQUAL(expected_r_vec.at(i), r, 1e-6); + + /// Check derivative. + Matrix expected_H = gtsam::numericalDerivative11(soft_plus_helper, x, 1e-6); + EXPECT(assert_equal(expected_H, H, 1e-6)); + } +} + +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} diff --git a/gtsam/nonlinear/NonlinearEquality.h b/gtsam/nonlinear/NonlinearEquality.h index f81486b30..c91bddd9c 100644 --- a/gtsam/nonlinear/NonlinearEquality.h +++ b/gtsam/nonlinear/NonlinearEquality.h @@ -18,6 +18,7 @@ #pragma once #include +#include #include #include @@ -40,14 +41,11 @@ namespace gtsam { * \nosubgrouping */ template -class NonlinearEquality: public NoiseModelFactorN { +class NonlinearEquality: public NonlinearEqualityConstraint { public: typedef VALUE T; - // Provide access to the Matrix& version of evaluateError: - using NoiseModelFactor1::evaluateError; - private: // feasible value @@ -63,7 +61,7 @@ private: using This = NonlinearEquality; // typedef to base class - using Base = NoiseModelFactorN; + using Base = NonlinearEqualityConstraint; public: @@ -88,7 +86,7 @@ public: const CompareFunction &_compare = std::bind(traits::Equals, std::placeholders::_1, std::placeholders::_2, 1e-9)) : Base(noiseModel::Constrained::All(traits::GetDimension(feasible)), - j), feasible_(feasible), allow_error_(false), error_gain_(0.0), // + KeyVector{j}), feasible_(feasible), allow_error_(false), error_gain_(0.0), // compare_(_compare) { } @@ -99,10 +97,14 @@ public: const CompareFunction &_compare = std::bind(traits::Equals, std::placeholders::_1, std::placeholders::_2, 1e-9)) : Base(noiseModel::Constrained::All(traits::GetDimension(feasible)), - j), feasible_(feasible), allow_error_(true), error_gain_(error_gain), // + KeyVector{j}), feasible_(feasible), allow_error_(true), error_gain_(error_gain), // compare_(_compare) { } + Key key() const { + return keys().front(); + } + /// @} /// @name Testable /// @{ @@ -139,7 +141,7 @@ public: } /// Error function - Vector evaluateError(const T& xj, OptionalMatrixType H) const override { + Vector evaluateError(const T& xj, OptionalMatrixType H = nullptr) const { const size_t nj = traits::GetDimension(feasible_); if (allow_error_) { if (H) @@ -158,11 +160,20 @@ public: } } + Vector unwhitenedError(const Values& x, OptionalMatrixVecType H = nullptr) const override { + VALUE x1 = x.at(key()); + if (H) { + return evaluateError(x1, &(H->front())); + } else { + return evaluateError(x1); + } + } + /// Linearize is over-written, because base linearization tries to whiten GaussianFactor::shared_ptr linearize(const Values& x) const override { const T& xj = x.at(this->key()); Matrix A; - Vector b = evaluateError(xj, A); + Vector b = evaluateError(xj, &A); SharedDiagonal model = noiseModel::Constrained::All(b.size()); return GaussianFactor::shared_ptr( new JacobianFactor(this->key(), A, b, model)); @@ -206,16 +217,13 @@ struct traits> : Testable> {}; * Simple unary equality constraint - fixes a value for a variable */ template -class NonlinearEquality1: public NoiseModelFactorN { +class NonlinearEquality1: public NonlinearEqualityConstraint { public: typedef VALUE X; - // Provide access to Matrix& version of evaluateError: - using NoiseModelFactor1::evaluateError; - protected: - typedef NoiseModelFactorN Base; + typedef NonlinearEqualityConstraint Base; typedef NonlinearEquality1 This; /// Default constructor to allow for serialization @@ -240,7 +248,7 @@ public: NonlinearEquality1(const X& value, Key key, double mu = 1000.0) : Base(noiseModel::Constrained::All(traits::GetDimension(value), std::abs(mu)), - key), + KeyVector{key}), value_(value) {} ~NonlinearEquality1() override { @@ -252,14 +260,25 @@ public: gtsam::NonlinearFactor::shared_ptr(new This(*this))); } + Key key() const { return keys().front(); } + /// g(x) with optional derivative - Vector evaluateError(const X& x1, OptionalMatrixType H) const override { + Vector evaluateError(const X& x1, OptionalMatrixType H = nullptr) const { if (H) (*H) = Matrix::Identity(traits::GetDimension(x1),traits::GetDimension(x1)); // manifold equivalent of h(x)-z -> log(z,h(x)) return traits::Local(value_,x1); } + Vector unwhitenedError(const Values& x, OptionalMatrixVecType H = nullptr) const override { + X x1 = x.at(key()); + if (H) { + return evaluateError(x1, &(H->front())); + } else { + return evaluateError(x1); + } + } + /// Print void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override { @@ -298,10 +317,10 @@ struct traits > * be the same. */ template -class NonlinearEquality2 : public NoiseModelFactorN { +class NonlinearEquality2 : public NonlinearEqualityConstraint { protected: - using Base = NoiseModelFactorN; - using This = NonlinearEquality2; + typedef NonlinearEqualityConstraint Base; + typedef NonlinearEquality2 This; GTSAM_CONCEPT_MANIFOLD_TYPE(T) @@ -311,9 +330,6 @@ class NonlinearEquality2 : public NoiseModelFactorN { public: typedef std::shared_ptr> shared_ptr; - // Provide access to the Matrix& version of evaluateError: - using Base::evaluateError; - /** * Constructor @@ -323,7 +339,7 @@ class NonlinearEquality2 : public NoiseModelFactorN { */ NonlinearEquality2(Key key1, Key key2, double mu = 1e4) : Base(noiseModel::Constrained::All(traits::dimension, std::abs(mu)), - key1, key2) {} + KeyVector{key1, key2}) {} ~NonlinearEquality2() override {} /// @return a deep copy of this factor @@ -334,13 +350,23 @@ class NonlinearEquality2 : public NoiseModelFactorN { /// g(x) with optional derivative2 Vector evaluateError( - const T& x1, const T& x2, OptionalMatrixType H1, OptionalMatrixType H2) const override { + const T& x1, const T& x2, OptionalMatrixType H1 = nullptr, OptionalMatrixType H2 = nullptr) const { static const size_t p = traits::dimension; if (H1) *H1 = -Matrix::Identity(p, p); if (H2) *H2 = Matrix::Identity(p, p); return traits::Local(x1, x2); } + Vector unwhitenedError(const Values& x, OptionalMatrixVecType H = nullptr) const override { + T x1 = x.at(keys().front()); + T x2 = x.at(keys().back()); + if (H) { + return evaluateError(x1, x2, &(H->front()), &(H->back())); + } else { + return evaluateError(x1, x2); + } + } + GTSAM_MAKE_ALIGNED_OPERATOR_NEW private: diff --git a/gtsam/slam/BoundingConstraint.h b/gtsam/slam/BoundingConstraint.h index a496971e2..d35152adf 100644 --- a/gtsam/slam/BoundingConstraint.h +++ b/gtsam/slam/BoundingConstraint.h @@ -19,6 +19,7 @@ #include #include +#include namespace gtsam { @@ -30,20 +31,17 @@ namespace gtsam { * @ingroup slam */ template -struct BoundingConstraint1: public NoiseModelFactorN { +struct BoundingConstraint1: public NonlinearInequalityConstraint { typedef VALUE X; - typedef NoiseModelFactorN Base; + typedef NonlinearInequalityConstraint Base; typedef std::shared_ptr > shared_ptr; - // Provide access to the Matrix& version of evaluateError: - using Base::evaluateError; - double threshold_; bool isGreaterThan_; /// flag for greater/less than BoundingConstraint1(Key key, double threshold, bool isGreaterThan, double mu = 1000.0) : - Base(noiseModel::Constrained::All(1, mu), key), + Base(noiseModel::Constrained::All(1, mu), KeyVector{key}), threshold_(threshold), isGreaterThan_(isGreaterThan) { } @@ -51,6 +49,7 @@ struct BoundingConstraint1: public NoiseModelFactorN { inline double threshold() const { return threshold_; } inline bool isGreaterThan() const { return isGreaterThan_; } + inline Key key() const { return keys().front(); } /** * function producing a scalar value to compare to the threshold @@ -60,14 +59,23 @@ struct BoundingConstraint1: public NoiseModelFactorN { virtual double value(const X& x, OptionalMatrixType H = OptionalNone) const = 0; - /** active when constraint *NOT* met */ - bool active(const Values& c) const override { - // note: still active at equality to avoid zigzagging - double x = value(c.at(this->key())); - return (isGreaterThan_) ? x <= threshold_ : x >= threshold_; + Vector unwhitenedExpr(const Values& x, OptionalMatrixVecType H = nullptr) const override { + if (H) { + double d = value(x.at(this->key()), &(H->front())); + if (isGreaterThan_) { + H->front() *= -1; + return Vector1(threshold_ - d); + } else { + return Vector1(d - threshold_); + } + } else { + double d = value(x.at(this->key())); + return Vector1((isGreaterThan_) ? threshold_ - d : d - threshold_); + } } - Vector evaluateError(const X& x, OptionalMatrixType H) const override { + /// TODO: This should be deprecated. + Vector evaluateError(const X& x, OptionalMatrixType H = nullptr) const { Matrix D; double error = value(x, &D) - threshold_; if (H) { @@ -102,22 +110,19 @@ private: * to implement for specific systems */ template -struct BoundingConstraint2: public NoiseModelFactorN { +struct BoundingConstraint2: public NonlinearInequalityConstraint { typedef VALUE1 X1; typedef VALUE2 X2; - typedef NoiseModelFactorN Base; + typedef NonlinearInequalityConstraint Base; typedef std::shared_ptr > shared_ptr; - // Provide access to the Matrix& version of evaluateError: - using Base::evaluateError; - double threshold_; bool isGreaterThan_; /// flag for greater/less than BoundingConstraint2(Key key1, Key key2, double threshold, bool isGreaterThan, double mu = 1000.0) - : Base(noiseModel::Constrained::All(1, mu), key1, key2), + : Base(noiseModel::Constrained::All(1, mu), KeyVector{key1, key2}), threshold_(threshold), isGreaterThan_(isGreaterThan) {} ~BoundingConstraint2() override {} @@ -133,15 +138,27 @@ struct BoundingConstraint2: public NoiseModelFactorN { OptionalMatrixType H1 = OptionalNone, OptionalMatrixType H2 = OptionalNone) const = 0; - /** active when constraint *NOT* met */ - bool active(const Values& c) const override { - // note: still active at equality to avoid zigzagging - double x = value(c.at(this->key1()), c.at(this->key2())); - return (isGreaterThan_) ? x <= threshold_ : x >= threshold_; + Vector unwhitenedExpr(const Values& x, OptionalMatrixVecType H = nullptr) const override { + X1 x1 = x.at(keys().front()); + X2 x2 = x.at(keys().back()); + if (H) { + double d = value(x1, x2, &(H->front()), &(H->back())); + if (isGreaterThan_) { + H->front() *= -1; + H->back() *= -1; + return Vector1(threshold_ - d); + } else { + return Vector1(d - threshold_); + } + } else { + double d = value(x1, x2); + return Vector1((isGreaterThan_) ? threshold_ - d : d - threshold_); + } } + /// TODO: This should be deprecated. Vector evaluateError(const X1& x1, const X2& x2, - OptionalMatrixType H1, OptionalMatrixType H2) const override { + OptionalMatrixType H1 = nullptr, OptionalMatrixType H2 = nullptr) const { Matrix D1, D2; double error = value(x1, x2, &D1, &D2) - threshold_; if (H1) { diff --git a/tests/testBoundingConstraint.cpp b/tests/testBoundingConstraint.cpp index 4aa357ba2..ca0ec489d 100644 --- a/tests/testBoundingConstraint.cpp +++ b/tests/testBoundingConstraint.cpp @@ -90,8 +90,8 @@ TEST( testBoundingConstraint, unary_basics_active1 ) { EXPECT(constraint2.active(config)); EXPECT(assert_equal(Vector::Constant(1,-3.0), constraint1.evaluateError(pt2), tol)); EXPECT(assert_equal(Vector::Constant(1,-5.0), constraint2.evaluateError(pt2), tol)); - EXPECT(assert_equal(Vector::Constant(1,-3.0), constraint1.unwhitenedError(config), tol)); - EXPECT(assert_equal(Vector::Constant(1,-5.0), constraint2.unwhitenedError(config), tol)); + EXPECT(assert_equal(Vector::Constant(1, 3.0), constraint1.unwhitenedError(config), tol)); + EXPECT(assert_equal(Vector::Constant(1, 5.0), constraint2.unwhitenedError(config), tol)); EXPECT_DOUBLES_EQUAL(45.0, constraint1.error(config), tol); EXPECT_DOUBLES_EQUAL(125.0, constraint2.error(config), tol); } @@ -103,10 +103,10 @@ TEST( testBoundingConstraint, unary_basics_active2 ) { config.insert(key, pt1); EXPECT(constraint3.active(config)); EXPECT(constraint4.active(config)); - EXPECT(assert_equal(-1.0 * I_1x1, constraint3.evaluateError(pt1), tol)); - EXPECT(assert_equal(-1.0 * I_1x1, constraint4.evaluateError(pt1), tol)); - EXPECT(assert_equal(-1.0 * I_1x1, constraint3.unwhitenedError(config), tol)); - EXPECT(assert_equal(-1.0 * I_1x1, constraint4.unwhitenedError(config), tol)); + // EXPECT(assert_equal(-1.0 * I_1x1, constraint3.evaluateError(pt1), tol)); + // EXPECT(assert_equal(-1.0 * I_1x1, constraint4.evaluateError(pt1), tol)); + EXPECT(assert_equal(1.0 * I_1x1, constraint3.unwhitenedError(config), tol)); + EXPECT(assert_equal(1.0 * I_1x1, constraint4.unwhitenedError(config), tol)); EXPECT_DOUBLES_EQUAL(5.0, constraint3.error(config), tol); EXPECT_DOUBLES_EQUAL(5.0, constraint4.error(config), tol); } @@ -213,7 +213,7 @@ TEST( testBoundingConstraint, MaxDistance_basics) { config1.update(key2, pt4); EXPECT(rangeBound.active(config1)); - EXPECT(assert_equal(-1.0*I_1x1, rangeBound.unwhitenedError(config1))); + EXPECT(assert_equal(1.0*I_1x1, rangeBound.unwhitenedError(config1))); EXPECT_DOUBLES_EQUAL(0.5*mu, rangeBound.error(config1), tol); }