clean up and refactor NonlinearEquality and BoundingConstraint
							parent
							
								
									80fcff1ed5
								
							
						
					
					
						commit
						9b0fa1210e
					
				|  | @ -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 | ||||
|  */ | ||||
|  |  | |||
|  | @ -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 */ | ||||
| 
 | ||||
|  |  | |||
|  | @ -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 */ | ||||
| 
 | ||||
|  |  | |||
|  | @ -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 <gtsam/constraint/NonlinearConstraint.h> | ||||
| #include <gtsam/nonlinear/ExpressionFactor.h> | ||||
| #include <gtsam/nonlinear/NonlinearFactorGraph.h> | ||||
| #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION | ||||
| #include <boost/serialization/base_object.hpp> | ||||
| #endif | ||||
| 
 | ||||
| namespace gtsam { | ||||
| 
 | ||||
|  | @ -81,6 +78,12 @@ class ExpressionEqualityConstraint : public NonlinearEqualityConstraint { | |||
| 
 | ||||
|   const Expression<T>& 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>( | ||||
|         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>( | ||||
|         gtsam::NonlinearFactor::shared_ptr(new This(*this))); | ||||
|   } | ||||
| 
 | ||||
|  private: | ||||
| #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION | ||||
|   /** Serialization function */ | ||||
|  |  | |||
|  | @ -17,8 +17,7 @@ | |||
|  */ | ||||
| 
 | ||||
| #include <gtsam/constraint/NonlinearInequalityConstraint.h> | ||||
| #include <memory> | ||||
| #include "gtsam/constraint/RampFunction.h" | ||||
| #include <gtsam/constraint/RampFunction.h> | ||||
| 
 | ||||
| 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 { | ||||
|  |  | |||
|  | @ -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>( | ||||
|         gtsam::NonlinearFactor::shared_ptr(new This(*this))); | ||||
|   } | ||||
| 
 | ||||
|  private: | ||||
| #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION | ||||
|   /** Serialization function */ | ||||
|  | @ -131,17 +149,19 @@ class NonlinearInequalityConstraints : public FactorGraph<NonlinearInequalityCon | |||
| 
 | ||||
|   using Base::Base; | ||||
| 
 | ||||
|   /// Return the total dimension of constraints.
 | ||||
|   /** Return the total dimension of constraints. */ | ||||
|   size_t dim() const; | ||||
| 
 | ||||
|   /// Evaluate the constraint violation as a vector
 | ||||
|   /** Evaluate the constraint violation as a vector. */ | ||||
|   Vector violationVector(const Values& values, bool whiten = true) const; | ||||
| 
 | ||||
|   /// Evaluate the constraint violation (as L2 norm).
 | ||||
|   /** Evaluate the constraint violation (as L2 norm). */ | ||||
|   double violationNorm(const Values& values) const; | ||||
| 
 | ||||
|   /** Return the penalty function corresponding to \sum_i||ramp(g_i(x))||^2. */ | ||||
|   NonlinearFactorGraph penaltyGraph(const double mu = 1.0) const; | ||||
| 
 | ||||
|   /** Return the penalty function constructed using smooth approximations of the ramp function. */ | ||||
|   NonlinearFactorGraph penaltyGraphSmooth(SmoothRampFunction::shared_ptr func, | ||||
|                                           const double mu = 1.0) const; | ||||
| 
 | ||||
|  |  | |||
|  | @ -10,9 +10,9 @@ | |||
|  * -------------------------------------------------------------------------- */ | ||||
| 
 | ||||
| /**
 | ||||
|  * @file    NonlinearInequalityConstraint.h | ||||
|  * @brief   Nonlinear inequality constraints in constrained optimization. | ||||
|  * @author  Yetong Zhang, Frank Dellaert | ||||
|  * @file    RampFunction.cpp | ||||
|  * @brief   Ramp function to compute inequality constraint violations. | ||||
|  * @author  Yetong Zhang | ||||
|  * @date    Aug 3, 2024 | ||||
|  */ | ||||
| 
 | ||||
|  | @ -41,7 +41,7 @@ SmoothRampFunction::UnaryScalarFunc SmoothRampFunction::function() const { | |||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| double PolynomialRampFunction::operator()(const double& x, OptionalJacobian<1, 1> 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(); | ||||
|  |  | |||
|  | @ -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 <gtsam/nonlinear/ExpressionFactor.h> | ||||
| #include <gtsam/nonlinear/expressions.h> | ||||
| #include <memory> | ||||
| 
 | ||||
| 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<This> 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<This> 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<This> shared_ptr; | ||||
| 
 | ||||
|  protected: | ||||
|   double k_; | ||||
|  |  | |||
|  | @ -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 | ||||
|  */ | ||||
|  |  | |||
|  | @ -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 | ||||
|  */ | ||||
|  |  | |||
|  | @ -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 <CppUnitLite/TestHarness.h> | ||||
| #include <gtsam/base/Testable.h> | ||||
| #include <gtsam/base/TestableAssertions.h> | ||||
| #include <gtsam/base/numericalDerivative.h> | ||||
| #include <gtsam/constraint/RampFunction.h> | ||||
| 
 | ||||
| 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<double> x_vec{-3.0, 0.0, 1.0, 2.0, 3.0}; | ||||
|   static std::vector<double> 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<double, double, 1>(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<double> x_vec{-3.0, 0.0, 1.0, 2.0, 3.0}; | ||||
|   static std::vector<double> 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<double, double, 1>(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<double> x_vec{-3.0, 0.0, 1.0, 2.0, 3.0}; | ||||
|   static std::vector<double> 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<double, double, 1>(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<double> x_vec{-3.0, 0.0, 1.0, 2.0, 3.0}; | ||||
|   static std::vector<double> 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<double, double, 1>(soft_plus_helper, x, 1e-6); | ||||
|     EXPECT(assert_equal(expected_H, H, 1e-6)); | ||||
|   } | ||||
| } | ||||
| 
 | ||||
| int main() { | ||||
|   TestResult tr; | ||||
|   return TestRegistry::runAllTests(tr); | ||||
| } | ||||
|  | @ -18,6 +18,7 @@ | |||
| #pragma once | ||||
| 
 | ||||
| #include <gtsam/nonlinear/NonlinearFactor.h> | ||||
| #include <gtsam/constraint/NonlinearEqualityConstraint.h> | ||||
| #include <gtsam/base/Testable.h> | ||||
| #include <gtsam/base/Manifold.h> | ||||
| 
 | ||||
|  | @ -40,14 +41,11 @@ namespace gtsam { | |||
|  * \nosubgrouping | ||||
|  */ | ||||
| template<class VALUE> | ||||
| class NonlinearEquality: public NoiseModelFactorN<VALUE> { | ||||
| class NonlinearEquality: public NonlinearEqualityConstraint { | ||||
| 
 | ||||
| public: | ||||
|   typedef VALUE T; | ||||
| 
 | ||||
|   // Provide access to the Matrix& version of evaluateError:
 | ||||
|   using NoiseModelFactor1<VALUE>::evaluateError; | ||||
| 
 | ||||
| private: | ||||
| 
 | ||||
|   // feasible value
 | ||||
|  | @ -63,7 +61,7 @@ private: | |||
|   using This = NonlinearEquality<VALUE>; | ||||
| 
 | ||||
|   // typedef to base class
 | ||||
|   using Base = NoiseModelFactorN<VALUE>; | ||||
|   using Base = NonlinearEqualityConstraint; | ||||
| 
 | ||||
| public: | ||||
| 
 | ||||
|  | @ -88,7 +86,7 @@ public: | |||
|       const CompareFunction &_compare = std::bind(traits<T>::Equals, | ||||
|           std::placeholders::_1, std::placeholders::_2, 1e-9)) : | ||||
|       Base(noiseModel::Constrained::All(traits<T>::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<T>::Equals, | ||||
|           std::placeholders::_1, std::placeholders::_2, 1e-9)) : | ||||
|       Base(noiseModel::Constrained::All(traits<T>::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<T>::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<VALUE>(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<T>(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<NonlinearEquality<VALUE>> : Testable<NonlinearEquality<VALUE>> {}; | |||
|  * Simple unary equality constraint - fixes a value for a variable | ||||
|  */ | ||||
| template<class VALUE> | ||||
| class NonlinearEquality1: public NoiseModelFactorN<VALUE> { | ||||
| class NonlinearEquality1: public NonlinearEqualityConstraint { | ||||
| 
 | ||||
| public: | ||||
|   typedef VALUE X; | ||||
| 
 | ||||
|   // Provide access to Matrix& version of evaluateError:
 | ||||
|   using NoiseModelFactor1<VALUE>::evaluateError; | ||||
| 
 | ||||
| protected: | ||||
|   typedef NoiseModelFactorN<VALUE> Base; | ||||
|   typedef NonlinearEqualityConstraint Base; | ||||
|   typedef NonlinearEquality1<VALUE> 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<X>::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<X>::GetDimension(x1),traits<X>::GetDimension(x1)); | ||||
|     // manifold equivalent of h(x)-z -> log(z,h(x))
 | ||||
|     return traits<X>::Local(value_,x1); | ||||
|   } | ||||
| 
 | ||||
|   Vector unwhitenedError(const Values& x, OptionalMatrixVecType H = nullptr) const override { | ||||
|     X x1 = x.at<X>(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<NonlinearEquality1<VALUE> > | |||
|  * be the same. | ||||
|  */ | ||||
| template <class T> | ||||
| class NonlinearEquality2 : public NoiseModelFactorN<T, T> { | ||||
| class NonlinearEquality2 : public NonlinearEqualityConstraint { | ||||
|  protected: | ||||
|   using Base = NoiseModelFactorN<T, T>; | ||||
|   using This = NonlinearEquality2<T>; | ||||
|   typedef NonlinearEqualityConstraint Base; | ||||
|   typedef NonlinearEquality2<T> This; | ||||
| 
 | ||||
|   GTSAM_CONCEPT_MANIFOLD_TYPE(T) | ||||
| 
 | ||||
|  | @ -311,9 +330,6 @@ class NonlinearEquality2 : public NoiseModelFactorN<T, T> { | |||
|  public: | ||||
|   typedef std::shared_ptr<NonlinearEquality2<T>> shared_ptr; | ||||
| 
 | ||||
|   // Provide access to the Matrix& version of evaluateError:
 | ||||
|   using Base::evaluateError; | ||||
| 
 | ||||
| 
 | ||||
|   /**
 | ||||
|    * Constructor | ||||
|  | @ -323,7 +339,7 @@ class NonlinearEquality2 : public NoiseModelFactorN<T, T> { | |||
|    */ | ||||
|   NonlinearEquality2(Key key1, Key key2, double mu = 1e4) | ||||
|       : Base(noiseModel::Constrained::All(traits<T>::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<T, T> { | |||
| 
 | ||||
|   /// 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<T>::dimension; | ||||
|     if (H1) *H1 = -Matrix::Identity(p, p); | ||||
|     if (H2) *H2 = Matrix::Identity(p, p); | ||||
|     return traits<T>::Local(x1, x2); | ||||
|   } | ||||
| 
 | ||||
|   Vector unwhitenedError(const Values& x, OptionalMatrixVecType H = nullptr) const override { | ||||
|     T x1 = x.at<T>(keys().front()); | ||||
|     T x2 = x.at<T>(keys().back()); | ||||
|     if (H) { | ||||
|       return evaluateError(x1, x2, &(H->front()), &(H->back())); | ||||
|     } else { | ||||
|       return evaluateError(x1, x2); | ||||
|     } | ||||
|   } | ||||
| 
 | ||||
|   GTSAM_MAKE_ALIGNED_OPERATOR_NEW | ||||
| 
 | ||||
|  private: | ||||
|  |  | |||
|  | @ -19,6 +19,7 @@ | |||
| 
 | ||||
| #include <gtsam/base/Lie.h> | ||||
| #include <gtsam/nonlinear/NonlinearFactor.h> | ||||
| #include <gtsam/constraint/NonlinearInequalityConstraint.h> | ||||
| 
 | ||||
| namespace gtsam { | ||||
| 
 | ||||
|  | @ -30,20 +31,17 @@ namespace gtsam { | |||
|  * @ingroup slam | ||||
|  */ | ||||
| template<class VALUE> | ||||
| struct BoundingConstraint1: public NoiseModelFactorN<VALUE> { | ||||
| struct BoundingConstraint1: public NonlinearInequalityConstraint { | ||||
|   typedef VALUE X; | ||||
|   typedef NoiseModelFactorN<VALUE> Base; | ||||
|   typedef NonlinearInequalityConstraint Base; | ||||
|   typedef std::shared_ptr<BoundingConstraint1<VALUE> > 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<VALUE> { | |||
| 
 | ||||
|   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<VALUE> { | |||
|   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<X>(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<X>(this->key()), &(H->front())); | ||||
|       if (isGreaterThan_) { | ||||
|         H->front() *= -1; | ||||
|         return Vector1(threshold_ - d); | ||||
|       } else { | ||||
|         return Vector1(d - threshold_); | ||||
|       } | ||||
|     } else { | ||||
|       double d = value(x.at<X>(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<class VALUE1, class VALUE2> | ||||
| struct BoundingConstraint2: public NoiseModelFactorN<VALUE1, VALUE2> { | ||||
| struct BoundingConstraint2: public NonlinearInequalityConstraint { | ||||
|   typedef VALUE1 X1; | ||||
|   typedef VALUE2 X2; | ||||
| 
 | ||||
|   typedef NoiseModelFactorN<VALUE1, VALUE2> Base; | ||||
|   typedef NonlinearInequalityConstraint Base; | ||||
|   typedef std::shared_ptr<BoundingConstraint2<VALUE1, VALUE2> > 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<VALUE1, VALUE2> { | |||
|       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<X1>(this->key1()), c.at<X2>(this->key2())); | ||||
|     return (isGreaterThan_) ? x <= threshold_ : x >= threshold_; | ||||
|   Vector unwhitenedExpr(const Values& x, OptionalMatrixVecType H = nullptr) const override { | ||||
|     X1 x1 = x.at<X1>(keys().front()); | ||||
|     X2 x2 = x.at<X2>(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) { | ||||
|  |  | |||
|  | @ -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); | ||||
| } | ||||
| 
 | ||||
|  |  | |||
		Loading…
	
		Reference in New Issue