clean up and refactor NonlinearEquality and BoundingConstraint

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
  */

gtsam/constraint/NonlinearEqualityConstraint-inl.h

@@ -10,8 +10,8 @@
  * -------------------------------------------------------------------------- */
 
 /**
- * @file    EqualityConstraint-inl.h
+ * @file    NonlinearEqualityConstraint-inl.h
- * @brief   Equality constraints in constrained optimization.
+ * @brief   Nonlinear equality constraints in constrained optimization.
  * @author  Yetong Zhang, Frank Dellaert
  * @date    Aug 3, 2024 */
 

gtsam/constraint/NonlinearEqualityConstraint.cpp

@@ -10,8 +10,8 @@
  * -------------------------------------------------------------------------- */
 
 /**
- * @file    EqualityConstraint.cpp
+ * @file    NonlinearEqualityConstraint.cpp
- * @brief   Equality constraints in constrained optimization.
+ * @brief   Nonlinear equality constraints in constrained optimization.
  * @author  Yetong Zhang, Frank Dellaert
  * @date    Aug 3, 2024 */
 

gtsam/constraint/NonlinearEqualityConstraint.h

@@ -10,8 +10,8 @@
  * -------------------------------------------------------------------------- */
 
 /**
- * @file    EqualityConstraint.h
+ * @file    NonlinearEqualityConstraint.h
- * @brief   Equality constraints in constrained optimization.
+ * @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 */

gtsam/constraint/NonlinearInequalityConstraint.cpp

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

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>(
+        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;
 

gtsam/constraint/RampFunction.cpp

@@ -10,9 +10,9 @@
  * -------------------------------------------------------------------------- */
 
 /**
- * @file    NonlinearInequalityConstraint.h
+ * @file    RampFunction.cpp
- * @brief   Nonlinear inequality constraints in constrained optimization.
+ * @brief   Ramp function to compute inequality constraint violations.
- * @author  Yetong Zhang, Frank Dellaert
+ * @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();

gtsam/constraint/RampFunction.h

@@ -10,9 +10,9 @@
  * -------------------------------------------------------------------------- */
 
 /**
- * @file    NonlinearInequalityConstraint.h
+ * @file    RampFunction.h
- * @brief   Nonlinear inequality constraints in constrained optimization.
+ * @brief   Ramp function to compute inequality constraint violations.
- * @author  Yetong Zhang, Frank Dellaert
+ * @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.
+/** Ramp function approximated with a polynomial of degree 2 in [0, epsilon].
- * Function f(x) =  0                   for   x <= 0
+ * The coefficients are computed as
- *                  x^2/(2*e) + e/2     for   0 < x < epsilon
+ *      a = 1 / (2 * epsilon)
- *                  x                   for   x >= 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_;

gtsam/constraint/tests/testNonlinearEqualityConstraint.cpp

@@ -10,8 +10,8 @@
  * -------------------------------------------------------------------------- */
 
 /**
- * @file    testEqualityConstraint.cpp
+ * @file    testNonlinearEqualityConstraint.cpp
- * @brief   Equality constraints in constrained optimization.
+ * @brief   unit tests for nonlinear equality constraints
  * @author  Yetong Zhang
  * @date    Aug 3, 2024
  */

gtsam/constraint/tests/testNonlinearInequalityConstraint.cpp

@@ -10,8 +10,8 @@
  * -------------------------------------------------------------------------- */
 
 /**
- * @file    NonlinearInequalityConstraint.h
+ * @file    testNonlinearInequalityConstraint.h
- * @brief   Nonlinear inequality constraints in constrained optimization.
+ * @brief   unit tests for nonlinear inequality constraints
  * @author  Yetong Zhang
  * @date    Aug 3, 2024
  */

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 <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);
+}

gtsam/nonlinear/NonlinearEquality.h

@@ -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>;
+  typedef NonlinearEqualityConstraint Base;
-  using This = NonlinearEquality2<T>;
+  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:

gtsam/slam/BoundingConstraint.h

@@ -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 */
+  Vector unwhitenedExpr(const Values& x, OptionalMatrixVecType H = nullptr) const override {
-  bool active(const Values& c) const override {
+    if (H) {
-    // note: still active at equality to avoid zigzagging
+      double d = value(x.at<X>(this->key()), &(H->front()));
-    double x = value(c.at<X>(this->key()));
+      if (isGreaterThan_) {
-    return (isGreaterThan_) ? x <= threshold_ : x >= threshold_;
+        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 */
+  Vector unwhitenedExpr(const Values& x, OptionalMatrixVecType H = nullptr) const override {
-  bool active(const Values& c) const override {
+    X1 x1 = x.at<X1>(keys().front());
-    // note: still active at equality to avoid zigzagging
+    X2 x2 = x.at<X2>(keys().back());
-    double x = value(c.at<X1>(this->key1()), c.at<X2>(this->key2()));
+    if (H) {
-    return (isGreaterThan_) ? x <= threshold_ : x >= threshold_;
+      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) {

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, 3.0), constraint1.unwhitenedError(config), tol));
-  EXPECT(assert_equal(Vector::Constant(1,-5.0), constraint2.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, constraint3.evaluateError(pt1), tol));
-  EXPECT(assert_equal(-1.0 * I_1x1, constraint4.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, constraint3.unwhitenedError(config), tol));
-  EXPECT(assert_equal(-1.0 * I_1x1, constraint4.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);
 }