add definition of constraints

gtsam/CMakeLists.txt

@@ -6,6 +6,7 @@ project(gtsam LANGUAGES CXX)
 set (gtsam_subdirs
     base
     basis
+    constraint
     geometry
     inference
     symbolic

gtsam/constraint/CMakeLists.txt

@@ -0,0 +1,6 @@
+# Install headers
+file(GLOB constraint_headers "*.h")
+install(FILES ${constraint_headers} DESTINATION include/gtsam/constraint)
+
+# Build tests
+add_subdirectory(tests)

gtsam/constraint/NonlinearConstraint.h

@@ -0,0 +1,65 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file    NonlinearConstraint.h
+ * @brief   Equality constraints in constrained optimization.
+ * @author  Yetong Zhang, Frank Dellaert
+ * @date    Aug 3, 2024
+ */
+
+#pragma once
+
+#include <gtsam/nonlinear/NonlinearFactor.h>
+#include "gtsam/linear/NoiseModel.h"
+
+namespace gtsam {
+
+/**
+ * Base class for nonlinear constraint.
+ * The constraint is represented as a NoiseModelFactor with Constrained noise model.
+ * whitenedError() returns The constraint violation vector.
+ * unwhitenedError() returns the sigma-scaled constraint violation vector.
+ */
+class NonlinearConstraint : public NoiseModelFactor {
+ public:
+  typedef NoiseModelFactor Base;
+
+  /** Use constructors of NoiseModelFactor. */
+  using Base::Base;
+
+  /** Destructor. */
+  virtual ~NonlinearConstraint() {}
+
+  /** Create a cost factor representing the L2 penalty function with scaling coefficient mu. */
+  virtual NoiseModelFactor::shared_ptr penaltyFactor(const double mu = 1.0) const = 0;
+
+  /** Return the norm of the constraint violation vector. */
+  virtual double violation(const Values& x) const { return sqrt(2 * error(x)); }
+
+  /**  Check if constraint violation is within tolerance. */
+  virtual bool feasible(const Values& x, const double tolerance = 1e-5) const {
+    return violation(x) <= tolerance;
+  }
+
+ protected:
+  /** Noise model used for the penalty function. */
+  SharedDiagonal penaltyNoise(const double mu) const {
+    return noiseModel::Diagonal::Sigmas(noiseModel()->sigmas() / sqrt(mu));
+  }
+
+  /** Default constrained noisemodel used for construction of constraint. */
+  static SharedNoiseModel constrainedNoise(const Vector& sigmas) {
+    return noiseModel::Constrained::MixedSigmas(1.0, sigmas);
+  }
+};
+
+}  // namespace gtsam

gtsam/constraint/NonlinearEqualityConstraint-inl.h

@@ -0,0 +1,54 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file    EqualityConstraint-inl.h
+ * @brief   Equality constraints in constrained optimization.
+ * @author  Yetong Zhang, Frank Dellaert
+ * @date    Aug 3, 2024 */
+
+#pragma once
+
+#include <gtsam/constraint/NonlinearEqualityConstraint.h>
+
+namespace gtsam {
+
+/* ************************************************************************* */
+template <typename T>
+ExpressionEqualityConstraint<T>::ExpressionEqualityConstraint(const Expression<T>& expression,
+                                                              const T& rhs,
+                                                              const Vector& sigmas)
+    : Base(constrainedNoise(sigmas), expression.keysAndDims().first),
+      expression_(expression),
+      rhs_(rhs),
+      dims_(expression.keysAndDims().second) {}
+
+/* ************************************************************************* */
+template <typename T>
+Vector ExpressionEqualityConstraint<T>::unwhitenedError(const Values& x,
+                                                        OptionalMatrixVecType H) const {
+  // Copy-paste from ExpressionFactor.
+  if (H) {
+    const T value = expression_.valueAndDerivatives(x, keys_, dims_, *H);
+    return -traits<T>::Local(value, rhs_);
+  } else {
+    const T value = expression_.value(x);
+    return -traits<T>::Local(value, rhs_);
+  }
+}
+
+/* ************************************************************************* */
+template <typename T>
+NoiseModelFactor::shared_ptr ExpressionEqualityConstraint<T>::penaltyFactor(const double mu) const {
+  return std::make_shared<ExpressionFactor<T>>(penaltyNoise(mu), rhs_, expression_);
+}
+
+}  // namespace gtsam

gtsam/constraint/NonlinearEqualityConstraint.cpp

@@ -0,0 +1,83 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file    EqualityConstraint.cpp
+ * @brief   Equality constraints in constrained optimization.
+ * @author  Yetong Zhang, Frank Dellaert
+ * @date    Aug 3, 2024 */
+
+#include <gtsam/constraint/NonlinearEqualityConstraint.h>
+
+namespace gtsam {
+
+/* ************************************************************************* */
+ZeroCostConstraint::ZeroCostConstraint(const NoiseModelFactor::shared_ptr& factor)
+    : Base(constrainedNoise(factor->noiseModel()->sigmas()), factor->keys()), factor_(factor) {}
+
+/* ************************************************************************* */
+Vector ZeroCostConstraint::unwhitenedError(const Values& x, OptionalMatrixVecType H) const {
+  return factor_->unwhitenedError(x, H);
+}
+
+/* ************************************************************************* */
+NoiseModelFactor::shared_ptr ZeroCostConstraint::penaltyFactor(const double mu) const {
+  return factor_->cloneWithNewNoiseModel(penaltyNoise(mu));
+}
+
+/* ************************************************************************* */
+NonlinearEqualityConstraints NonlinearEqualityConstraints::FromCostGraph(
+    const NonlinearFactorGraph& graph) {
+  NonlinearEqualityConstraints constraints;
+  for (const auto& factor : graph) {
+    auto noise_factor = std::dynamic_pointer_cast<NoiseModelFactor>(factor);
+    constraints.emplace_shared<ZeroCostConstraint>(noise_factor);
+  }
+  return constraints;
+}
+
+/* ************************************************************************* */
+size_t NonlinearEqualityConstraints::dim() const {
+  size_t dimension = 0;
+  for (const auto& constraint : *this) {
+    dimension += constraint->dim();
+  }
+  return dimension;
+}
+
+/* ************************************************************************* */
+Vector NonlinearEqualityConstraints::violationVector(const Values& values, bool whiten) const {
+  Vector violation(dim());
+  size_t start_idx = 0;
+  for (const auto& constraint : *this) {
+    size_t dim = constraint->dim();
+    violation.middleCols(start_idx, dim) =
+        whiten ? constraint->whitenedError(values) : constraint->unwhitenedError(values);
+    start_idx += dim;
+  }
+  return violation;
+}
+
+/* ************************************************************************* */
+double NonlinearEqualityConstraints::violationNorm(const Values& values) const {
+  return violationVector(values, true).norm();
+}
+
+/* ************************************************************************* */
+NonlinearFactorGraph NonlinearEqualityConstraints::penaltyGraph(const double mu) const {
+  NonlinearFactorGraph graph;
+  for (const auto& constraint : *this) {
+    graph.add(constraint->penaltyFactor(mu));
+  }
+  return graph;
+}
+
+}  // namespace gtsam

gtsam/constraint/NonlinearEqualityConstraint.h

@@ -0,0 +1,120 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file    EqualityConstraint.h
+ * @brief   Equality constraints in constrained optimization.
+ * @author  Yetong Zhang, Frank Dellaert
+ * @date    Aug 3, 2024 */
+
+#pragma once
+
+#include <gtsam/constraint/NonlinearConstraint.h>
+#include <gtsam/nonlinear/ExpressionFactor.h>
+#include <gtsam/nonlinear/NonlinearFactorGraph.h>
+
+namespace gtsam {
+
+/**
+ * Equality constraint base class.
+ */
+class NonlinearEqualityConstraint : public NonlinearConstraint {
+ public:
+  typedef NonlinearConstraint Base;
+  typedef NonlinearEqualityConstraint This;
+  typedef std::shared_ptr<This> shared_ptr;
+
+  /** Default constructor. */
+  using Base::Base;
+
+  /** Destructor. */
+  virtual ~NonlinearEqualityConstraint() {}
+};
+
+/** Equality constraint that force g(x) = M. */
+template <typename T>
+class ExpressionEqualityConstraint : public NonlinearEqualityConstraint {
+ public:
+  typedef NonlinearEqualityConstraint Base;
+  typedef ExpressionEqualityConstraint This;
+  typedef std::shared_ptr<This> shared_ptr;
+
+ protected:
+  Expression<T> expression_;
+  T rhs_;
+  FastVector<int> dims_;
+
+ public:
+  /**
+   * @brief Constructor.
+   *
+   * @param expression  expression representing g(x).
+   * @param tolerance   vector representing tolerance in each dimension.
+   */
+  ExpressionEqualityConstraint(const Expression<T>& expression, const T& rhs, const Vector& sigmas);
+
+  virtual Vector unwhitenedError(const Values& x, OptionalMatrixVecType H = nullptr) const override;
+
+  virtual NoiseModelFactor::shared_ptr penaltyFactor(const double mu = 1.0) const override;
+
+  const Expression<T>& expression() const { return expression_; }
+};
+
+/** Equality constraint that enforce the cost factor with zero error. */
+class ZeroCostConstraint : public NonlinearEqualityConstraint {
+ public:
+  typedef NonlinearEqualityConstraint Base;
+  typedef ZeroCostConstraint This;
+  typedef std::shared_ptr<This> shared_ptr;
+
+ protected:
+  NoiseModelFactor::shared_ptr factor_;
+
+ public:
+  /**
+   * @brief Constructor.
+   *
+   * @param factor  NoiseModel factor.
+   * @param tolerance   vector representing tolerance in each dimension.
+   */
+  ZeroCostConstraint(const NoiseModelFactor::shared_ptr& factor);
+
+  virtual Vector unwhitenedError(const Values& x, OptionalMatrixVecType H = nullptr) const override;
+
+  virtual NoiseModelFactor::shared_ptr penaltyFactor(const double mu = 1.0) const override;
+};
+
+/// Container of NonlinearEqualityConstraint.
+class NonlinearEqualityConstraints : public FactorGraph<NonlinearEqualityConstraint> {
+ public:
+  typedef std::shared_ptr<NonlinearEqualityConstraints> shared_ptr;
+  typedef FactorGraph<NonlinearEqualityConstraint> Base;
+
+ public:
+  using Base::Base;
+
+  /// Create constraints ensuring the cost of factors of a graph is zero.
+  static NonlinearEqualityConstraints FromCostGraph(const NonlinearFactorGraph& graph);
+
+  size_t dim() const;
+
+  /// Evaluate the constraint violation as a vector
+  Vector violationVector(const Values& values, bool whiten = true) const;
+
+  /// Evaluate the constraint violation (as 2-norm of the violation vector).
+  double violationNorm(const Values& values) const;
+
+  NonlinearFactorGraph penaltyGraph(const double mu = 1.0) const;
+};
+
+}  // namespace gtsam
+
+#include <gtsam/constraint/NonlinearEqualityConstraint-inl.h>

gtsam/constraint/NonlinearInequalityConstraint.cpp

@@ -0,0 +1,158 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file    NonlinearInequalityConstraint.cpp
+ * @brief   Nonlinear inequality constraints in constrained optimization.
+ * @author  Yetong Zhang, Frank Dellaert
+ * @date    Aug 3, 2024
+ */
+
+#include <gtsam/constraint/NonlinearInequalityConstraint.h>
+#include <memory>
+#include "gtsam/constraint/RampFunction.h"
+
+namespace gtsam {
+
+/* ************************************************************************* */
+Vector NonlinearInequalityConstraint::unwhitenedError(const Values& x,
+                                                      OptionalMatrixVecType H) const {
+  Vector error = unwhitenedExpr(x, H);
+  for (size_t i = 0; i < dim(); i++) {
+    if (error(i) < 0) {
+      error(i) = 0;
+      if (H) {
+        for (Matrix& m : *H) {
+          m.row(i).setZero();
+        }
+      }
+    }
+  }
+  return error;
+}
+
+/* ************************************************************************* */
+bool NonlinearInequalityConstraint::active(const Values& x) const {
+  return (unwhitenedExpr(x).array() >= 0).any();
+}
+
+/* ************************************************************************* */
+NoiseModelFactor::shared_ptr NonlinearInequalityConstraint::penaltyFactorEquality(
+    const double mu) const {
+  return createEqualityConstraint()->penaltyFactor(mu);
+}
+
+/* ************************************************************************* */
+ScalarExpressionInequalityConstraint::ScalarExpressionInequalityConstraint(
+    const Double_& expression, const double& sigma)
+    : Base(constrainedNoise(Vector1(sigma)), expression.keysAndDims().first),
+      expression_(expression),
+      dims_(expression.keysAndDims().second) {}
+
+/* ************************************************************************* */
+ScalarExpressionInequalityConstraint::shared_ptr ScalarExpressionInequalityConstraint::GeqZero(
+    const Double_& expression, const double& sigma) {
+  Double_ neg_expr = Double_(0.0) - expression;
+  return std::make_shared<ScalarExpressionInequalityConstraint>(neg_expr, sigma);
+}
+
+/* ************************************************************************* */
+ScalarExpressionInequalityConstraint::shared_ptr ScalarExpressionInequalityConstraint::LeqZero(
+    const Double_& expression, const double& sigma) {
+  return std::make_shared<ScalarExpressionInequalityConstraint>(expression, sigma);
+}
+
+/* ************************************************************************* */
+Vector ScalarExpressionInequalityConstraint::unwhitenedExpr(const Values& x,
+                                                            OptionalMatrixVecType H) const {
+  // Copy-paste from ExpressionFactor.
+  if (H) {
+    return Vector1(expression_.valueAndDerivatives(x, keys_, dims_, *H));
+  } else {
+    return Vector1(expression_.value(x));
+  }
+}
+
+/* ************************************************************************* */
+NonlinearEqualityConstraint::shared_ptr
+ScalarExpressionInequalityConstraint::createEqualityConstraint() const {
+  return std::make_shared<ExpressionEqualityConstraint<double>>(
+      expression_, 0.0, noiseModel()->sigmas());
+}
+
+/* ************************************************************************* */
+NoiseModelFactor::shared_ptr ScalarExpressionInequalityConstraint::penaltyFactor(
+    const double mu) const {
+  Double_ penalty_expression(RampFunction, expression_);
+  return std::make_shared<ExpressionFactor<double>>(penaltyNoise(mu), 0.0, penalty_expression);
+}
+
+/* ************************************************************************* */
+NoiseModelFactor::shared_ptr ScalarExpressionInequalityConstraint::penaltyFactorSmooth(
+    SmoothRampFunction::shared_ptr func, const double mu) const {
+  // TODO(yetong): can we pass the functor directly to construct the expression?
+  Double_ error(func->function(), expression_);
+  return std::make_shared<ExpressionFactor<double>>(penaltyNoise(mu), 0.0, error);
+}
+
+/* ************************************************************************* */
+NoiseModelFactor::shared_ptr ScalarExpressionInequalityConstraint::penaltyFactorEquality(
+    const double mu) const {
+  return std::make_shared<ExpressionFactor<double>>(penaltyNoise(mu), 0.0, expression_);
+}
+
+/* ************************************************************************* */
+size_t NonlinearInequalityConstraints::dim() const {
+  size_t dimension = 0;
+  for (const auto& constraint : *this) {
+    dimension += constraint->dim();
+  }
+  return dimension;
+}
+
+/* ************************************************************************* */
+Vector NonlinearInequalityConstraints::violationVector(const Values& values, bool whiten) const {
+  Vector violation(dim());
+  size_t start_idx = 0;
+  for (const auto& constraint : *this) {
+    size_t dim = constraint->dim();
+    violation.middleCols(start_idx, dim) =
+        whiten ? constraint->whitenedError(values) : constraint->unwhitenedError(values);
+    start_idx += dim;
+  }
+  return violation;
+}
+
+/* ************************************************************************* */
+double NonlinearInequalityConstraints::violationNorm(const Values& values) const {
+  return violationVector(values, true).norm();
+}
+
+/* ************************************************************************* */
+NonlinearFactorGraph NonlinearInequalityConstraints::penaltyGraph(const double mu) const {
+  NonlinearFactorGraph graph;
+  for (const auto& constraint : *this) {
+    graph.add(constraint->penaltyFactor(mu));
+  }
+  return graph;
+}
+
+/* ************************************************************************* */
+NonlinearFactorGraph NonlinearInequalityConstraints::penaltyGraphSmooth(
+    SmoothRampFunction::shared_ptr func, const double mu) const {
+  NonlinearFactorGraph graph;
+  for (const auto& constraint : *this) {
+    graph.add(constraint->penaltyFactorSmooth(func, mu));
+  }
+  return graph;
+}
+
+}  // namespace gtsam

gtsam/constraint/NonlinearInequalityConstraint.h

@@ -0,0 +1,125 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file    NonlinearInequalityConstraint.h
+ * @brief   Nonlinear inequality constraints in constrained optimization.
+ * @author  Yetong Zhang, Frank Dellaert
+ * @date    Aug 3, 2024
+ */
+
+#pragma once
+
+#include <gtsam/constraint/NonlinearEqualityConstraint.h>
+#include <gtsam/constraint/RampFunction.h>
+#include <gtsam/nonlinear/expressions.h>
+
+namespace gtsam {
+
+/**
+ * Inequality constraint base class.
+ */
+class NonlinearInequalityConstraint : public NonlinearConstraint {
+ public:
+  typedef NonlinearConstraint Base;
+  typedef NonlinearInequalityConstraint This;
+  typedef std::shared_ptr<This> shared_ptr;
+
+  /** Default constructor. */
+  using Base::Base;
+
+  /** Destructor. */
+  virtual ~NonlinearInequalityConstraint() {}
+
+  virtual Vector unwhitenedExpr(const Values& x, OptionalMatrixVecType H = nullptr) const = 0;
+
+  virtual Vector unwhitenedError(const Values& x, OptionalMatrixVecType H = nullptr) const override;
+
+  virtual bool active(const Values& x) const override;
+
+  virtual NonlinearEqualityConstraint::shared_ptr createEqualityConstraint() const = 0;
+
+  /** Smooth approximation of the ramp function. */
+  virtual NoiseModelFactor::shared_ptr penaltyFactorSmooth(SmoothRampFunction::shared_ptr func,
+                                                           const double mu = 1.0) const = 0;
+
+  /** penalty function as if the constraint is equality, 0.5 * mu * ||g(x)||^2 */
+  virtual NoiseModelFactor::shared_ptr penaltyFactorEquality(const double mu = 1.0) const;
+};
+
+/** Inequality constraint that force g(x) <= 0, where g(x) is a scalar-valued
+ * function. */
+class ScalarExpressionInequalityConstraint : public NonlinearInequalityConstraint {
+ public:
+  typedef NonlinearInequalityConstraint Base;
+  typedef ScalarExpressionInequalityConstraint This;
+  typedef std::shared_ptr<This> shared_ptr;
+
+ protected:
+  Double_ expression_;
+  FastVector<int> dims_;
+
+ public:
+  /**
+   * @brief Constructor.
+   *
+   * @param expression  expression representing g(x).
+   * @param sigma   scalar representing sigma.
+   */
+  ScalarExpressionInequalityConstraint(const Double_& expression, const double& sigma);
+
+  // Create an inequality constraint g(x)>=0.
+  static ScalarExpressionInequalityConstraint::shared_ptr GeqZero(const Double_& expression,
+                                                                  const double& sigma);
+
+  // Create an inequality constraint g(x)<=0.
+  static ScalarExpressionInequalityConstraint::shared_ptr LeqZero(const Double_& expression,
+                                                                  const double& sigma);
+
+  virtual Vector unwhitenedExpr(const Values& x, OptionalMatrixVecType H = nullptr) const override;
+
+  NonlinearEqualityConstraint::shared_ptr createEqualityConstraint() const override;
+
+  NoiseModelFactor::shared_ptr penaltyFactor(const double mu = 1.0) const override;
+
+  NoiseModelFactor::shared_ptr penaltyFactorSmooth(SmoothRampFunction::shared_ptr func,
+                                                   const double mu = 1.0) const override;
+
+  virtual NoiseModelFactor::shared_ptr penaltyFactorEquality(const double mu = 1.0) const override;
+
+  const Double_& expression() const { return expression_; }
+};
+
+/// Container of NonlinearInequalityConstraint.
+class NonlinearInequalityConstraints : public FactorGraph<NonlinearInequalityConstraint> {
+ public:
+  typedef FactorGraph<NonlinearInequalityConstraint> Base;
+  typedef NonlinearInequalityConstraints This;
+  typedef std::shared_ptr<This> shared_ptr;
+
+  using Base::Base;
+
+  /// Return the total dimension of constraints.
+  size_t dim() const;
+
+  /// Evaluate the constraint violation as a vector
+  Vector violationVector(const Values& values, bool whiten = true) const;
+
+  /// Evaluate the constraint violation (as L2 norm).
+  double violationNorm(const Values& values) const;
+
+  NonlinearFactorGraph penaltyGraph(const double mu = 1.0) const;
+
+  NonlinearFactorGraph penaltyGraphSmooth(SmoothRampFunction::shared_ptr func,
+                                          const double mu = 1.0) const;
+};
+
+}  // namespace gtsam

gtsam/constraint/RampFunction.cpp

@@ -0,0 +1,71 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file    NonlinearInequalityConstraint.h
+ * @brief   Nonlinear inequality constraints in constrained optimization.
+ * @author  Yetong Zhang, Frank Dellaert
+ * @date    Aug 3, 2024
+ */
+
+#include <gtsam/constraint/RampFunction.h>
+
+namespace gtsam {
+
+/* ************************************************************************* */
+double RampFunction(const double x, OptionalJacobian<1, 1> H) {
+  if (x < 0) {
+    if (H) {
+      H->setConstant(0);
+    }
+    return 0;
+  } else {
+    if (H) {
+      H->setConstant(1);
+    }
+    return x;
+  }
+}
+
+/* ************************************************************************* */
+SmoothRampFunction::UnaryScalarFunc SmoothRampFunction::function() const {
+  return [=](const double& x, OptionalJacobian<1, 1> H = {}) -> double { return (*this)(x, H); };
+}
+
+/* ************************************************************************* */
+double PolynomialRampFunction::operator()(const double& x, OptionalJacobian<1, 1> H) const {
+  if (x <= 0) {
+    if (H) {
+      H->setZero();
+    }
+    return 0;
+  } else if (x < epsilon_) {
+    if (H) {
+      H->setConstant(x / epsilon_);
+    }
+    return (x * x) / (2 * epsilon_) + epsilon_ / 2;
+  } else {
+    if (H) {
+      H->setOnes();
+    }
+    return x;
+  }
+}
+
+/* ************************************************************************* */
+double SoftPlusFunction::operator()(const double& x, OptionalJacobian<1, 1> H) const {
+  if (H) {
+    H->setConstant(1 / (1 + std::exp(-k_ * x)));
+  }
+  return std::log(1 + std::exp(k_ * x)) / k_;
+}
+
+}  // namespace gtsam

gtsam/constraint/RampFunction.h

@@ -0,0 +1,78 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file    NonlinearInequalityConstraint.h
+ * @brief   Nonlinear inequality constraints in constrained optimization.
+ * @author  Yetong Zhang, Frank Dellaert
+ * @date    Aug 3, 2024
+ */
+
+#pragma once
+
+#include <gtsam/nonlinear/ExpressionFactor.h>
+#include <gtsam/nonlinear/expressions.h>
+
+namespace gtsam {
+
+/// Ramp function for create penalty factors.
+double RampFunction(const double x, OptionalJacobian<1, 1> H = {});
+
+/** Base class for smooth approximation of the ramp function. */
+class SmoothRampFunction {
+ public:
+  typedef std::shared_ptr<SmoothRampFunction> shared_ptr;
+  typedef std::function<double(const double& x, OptionalJacobian<1, 1> H)> UnaryScalarFunc;
+
+  /** Constructor. */
+  SmoothRampFunction() {}
+
+  /** Destructor. */
+  virtual ~SmoothRampFunction() {}
+
+  virtual double operator()(const double& x, OptionalJacobian<1, 1> H = {}) const = 0;
+
+  UnaryScalarFunc function() const;
+};
+
+/** Ramp function approximated with a polynomial of degree 2.
+ * Function f(x) =  0                   for   x <= 0
+ *                  x^2/(2*e) + e/2     for   0 < x < epsilon
+ *                  x                   for   x >= epsilon
+ */
+class PolynomialRampFunction : public SmoothRampFunction {
+ public:
+  typedef SmoothRampFunction Base;
+
+ protected:
+  double epsilon_;
+
+ public:
+  PolynomialRampFunction(const double epsilon = 1) : Base(), epsilon_(epsilon) {}
+
+  virtual double operator()(const double& x, OptionalJacobian<1, 1> H = {}) const override;
+};
+
+/** Softplus function that implements f(x) = log(1 + exp(k*x)) / k. */
+class SoftPlusFunction : public SmoothRampFunction {
+ public:
+  typedef SmoothRampFunction Base;
+
+ protected:
+  double k_;
+
+ public:
+  SoftPlusFunction(const double k = 1) : Base(), k_(k) {}
+
+  virtual double operator()(const double& x, OptionalJacobian<1, 1> H = {}) const override;
+};
+
+}  // namespace gtsam

gtsam/constraint/tests/CMakeLists.txt

@@ -0,0 +1,9 @@
+# if GTSAM_ENABLE_BOOST_SERIALIZATION is OFF then exclude some tests
+if (NOT GTSAM_ENABLE_BOOST_SERIALIZATION)
+  # create a semicolon seperated list of files to exclude
+  set(EXCLUDE_TESTS "testSerializationConstraint.cpp")
+else()
+  set(EXCLUDE_TESTS "")
+endif()
+
+gtsamAddTestsGlob(constraint "test*.cpp" "${EXCLUDE_TESTS}" "gtsam")

gtsam/constraint/tests/constrainedExample.h

@@ -0,0 +1,168 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file    constrainedExample.h
+ * @brief   Simple constrained optimization scenarios.
+ * @author  Yetong Zhang
+ * @date    Aug 3, 2024
+ */
+
+#pragma once
+
+#include <gtsam/base/Matrix.h>
+#include <gtsam/base/Vector.h>
+#include <gtsam/inference/Symbol.h>
+#include <gtsam/nonlinear/ExpressionFactor.h>
+#include <gtsam/nonlinear/NonlinearFactor.h>
+#include <gtsam/nonlinear/expressions.h>
+#include <gtsam/slam/BetweenFactor.h>
+#include <gtsam/constraint/NonlinearEqualityConstraint.h>
+#include <gtsam/constraint/NonlinearInequalityConstraint.h>
+
+
+namespace constrained_example {
+
+using namespace gtsam;
+
+/// Exponential function e^x.
+double exp_func(const double& x, gtsam::OptionalJacobian<1, 1> H1 = {}) {
+  double result = exp(x);
+  if (H1) H1->setConstant(result);
+  return result;
+}
+
+/// Exponential expression e^x.
+Double_ exp(const Double_& x) { return Double_(exp_func, x); }
+
+/// Pow functor used for pow function.
+class PowFunctor {
+ private:
+  double c_;
+
+ public:
+  PowFunctor(const double& c) : c_(c) {}
+
+  double operator()(const double& x,
+                    gtsam::OptionalJacobian<1, 1> H1 = {}) const {
+    if (H1) H1->setConstant(c_ * pow(x, c_ - 1));
+    return pow(x, c_);
+  }
+};
+
+/// Pow function.
+Double_ pow(const Double_& x, const double& c) {
+  PowFunctor pow_functor(c);
+  return Double_(pow_functor, x);
+}
+
+/// Plus between Double expression and double.
+Double_ operator+(const Double_& x, const double& d) { return x + Double_(d); }
+
+/// Negative sign operator.
+Double_ operator-(const Double_& x) { return Double_(0.0) - x; }
+
+/// Keys for creating expressions.
+Symbol x1_key('x', 1);
+Symbol x2_key('x', 2);
+Double_ x1(x1_key), x2(x2_key);
+
+}  // namespace constrained_example
+
+/* ************************************************************************* */
+/**
+ * Constrained optimization example in L. Vandenberghe slides:
+ * https://www.seas.ucla.edu/~vandenbe/133B/lectures/nllseq.pdf
+ * f(x) = 0.5*||x1 + e^(-x2)||^2 + 0.5*||x1^2 + 2*x2 + 1||^2
+ * h(x) = x1 + x1^3 + x2 + x2^2
+ */
+namespace e_constrained_example {
+using namespace constrained_example;
+NonlinearFactorGraph GetCost() {
+  NonlinearFactorGraph graph;
+  auto f1 = x1 + exp(-x2);
+  auto f2 = pow(x1, 2.0) + 2.0 * x2 + 1.0;
+  auto cost_noise = gtsam::noiseModel::Isotropic::Sigma(1, 1.0);
+  graph.add(ExpressionFactor<double>(cost_noise, 0., f1));
+  graph.add(ExpressionFactor<double>(cost_noise, 0., f2));
+  return graph;
+}
+
+NonlinearEqualityConstraints GetConstraints() {
+  NonlinearEqualityConstraints constraints;
+  Vector scale = Vector1(0.1);
+  auto h1 = x1 + pow(x1, 3) + x2 + pow(x2, 2);
+  constraints.push_back(NonlinearEqualityConstraint::shared_ptr(
+      new ExpressionEqualityConstraint<double>(h1, 0.0, scale)));
+  return constraints;
+}
+
+NonlinearFactorGraph cost = GetCost();
+NonlinearEqualityConstraints constraints = GetConstraints();
+} // namespace e_constrained_example
+
+/* ************************************************************************* */
+/**
+ * Constrained optimization example with inequality constraints
+ * f(x) = 0.5 * ||x1-1||^2 + 0.5 * ||x2-1||^2
+ * g(x) = 1 - x1^2 - x2^2
+ */
+namespace i_constrained_example {
+using namespace constrained_example;
+NonlinearFactorGraph GetCost() {
+  NonlinearFactorGraph graph;
+  auto cost_noise = gtsam::noiseModel::Isotropic::Sigma(1, 1.0);
+  graph.addPrior(x1_key, 1.0, cost_noise);
+  graph.addPrior(x2_key, 1.0, cost_noise);
+  return graph;
+}
+
+// InequalityConstraints GetIConstraints() {
+//   InequalityConstraints i_constraints;
+//   Double_ g1 = Double_(1.0) - x1 * x1 - x2 * x2;
+//   double tolerance = 0.2;
+//   i_constraints.emplace_shared<DoubleExpressionInequality>(g1, tolerance);
+//   return i_constraints;
+// }
+
+NonlinearFactorGraph cost = GetCost();
+NonlinearEqualityConstraints e_constraints;
+// InequalityConstraints i_constraints = GetIConstraints();
+} // namespace i_constrained_example
+
+/* ************************************************************************* */
+/**
+ * Constrained optimization example with inequality constraints
+ * f(x) = 0.5 * ||x1||^2 + 0.5 * ||x2||^2
+ * h(x) = x1 + x2 - 1
+ */
+namespace e_constrained_example2 {
+using namespace constrained_example;
+NonlinearFactorGraph GetCost() {
+  NonlinearFactorGraph graph;
+  auto cost_noise = gtsam::noiseModel::Isotropic::Sigma(1, 1.0);
+  graph.addPrior(x1_key, 1.0, cost_noise);
+  // graph.addPrior(x2_key, 0.0, cost_noise);
+  // graph.emplace_shared<gtsam::BetweenFactor<double>>(x1_key, x2_key, 1.0, cost_noise);
+  return graph;
+}
+
+NonlinearEqualityConstraints GetEConstraints() {
+  NonlinearEqualityConstraints e_constraints;
+  // Double_ h1 = x1 + x2 - Double_(1.0);
+  // double tolerance = 0.2;
+  // e_constraints.emplace_shared<DoubleExpressionEquality>(h1, tolerance);
+  return e_constraints;
+}
+
+NonlinearFactorGraph cost = GetCost();
+NonlinearEqualityConstraints e_constraints = GetEConstraints();
+} // namespace i_constrained_example

gtsam/constraint/tests/testNonlinearEqualityConstraint.cpp

@@ -0,0 +1,267 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file    testEqualityConstraint.cpp
+ * @brief   Equality constraints in constrained optimization.
+ * @author  Yetong Zhang
+ * @date    Aug 3, 2024
+ */
+
+#include <CppUnitLite/Test.h>
+#include <CppUnitLite/TestHarness.h>
+#include <gtsam/base/Testable.h>
+#include <gtsam/base/TestableAssertions.h>
+#include <gtsam/base/numericalDerivative.h>
+#include <gtsam/constraint/NonlinearEqualityConstraint.h>
+#include <gtsam/inference/VariableIndex.h>
+#include <gtsam/nonlinear/factorTesting.h>
+#include <gtsam/slam/BetweenFactor.h>
+
+#include "constrainedExample.h"
+
+using namespace gtsam;
+using constrained_example::pow;
+using constrained_example::x1, constrained_example::x2;
+using constrained_example::x1_key, constrained_example::x2_key;
+
+// Test methods of DoubleExpressionEquality.
+TEST(ExpressionEqualityConstraint, double) {
+  // create constraint from double expression
+  // g(x1, x2) = x1 + x1^3 + x2 + x2^2, from Vanderbergh slides
+  Vector sigmas = Vector1(0.1);
+  auto g = x1 + pow(x1, 3) + x2 + pow(x2, 2);
+  auto constraint = ExpressionEqualityConstraint<double>(g, 0.0, sigmas);
+
+  EXPECT(constraint.noiseModel()->isConstrained());
+  EXPECT(assert_equal(sigmas, constraint.noiseModel()->sigmas()));
+
+  // Create 2 sets of values for testing.
+  Values values1, values2;
+  values1.insert(x1_key, 0.0);
+  values1.insert(x2_key, 0.0);
+  values2.insert(x1_key, 1.0);
+  values2.insert(x2_key, 1.0);
+
+  // Check that values1 are feasible.
+  EXPECT(constraint.feasible(values1));
+
+  // Check that violation evaluates as 0 at values1.
+  EXPECT(assert_equal(Vector::Zero(1), constraint.unwhitenedError(values1)));
+  EXPECT(assert_equal(Vector::Zero(1), constraint.whitenedError(values1)));
+  EXPECT(assert_equal(0.0, constraint.error(values1)));
+
+  // Check that values2 are indeed deemed infeasible.
+  EXPECT(!constraint.feasible(values2));
+
+  // Check constraint violation is indeed g(x) at values2.
+  EXPECT(assert_equal(Vector::Constant(1, 4.0), constraint.unwhitenedError(values2)));
+  EXPECT(assert_equal(Vector::Constant(1, 40), constraint.whitenedError(values2)));
+  EXPECT(assert_equal(800, constraint.error(values2)));
+
+  // Check dimension is 1 for scalar g.
+  EXPECT(constraint.dim() == 1);
+
+  // Check keys include x1, x2.
+  EXPECT(constraint.keys().size() == 2);
+  EXPECT(x1_key == *constraint.keys().begin());
+  EXPECT(x2_key == *constraint.keys().rbegin());
+
+  // Generate factor representing the term in merit function.
+  double mu = 4;
+  auto merit_factor = constraint.penaltyFactor(mu);
+
+  // Check that noise model sigma == sigmas/sqrt(mu).
+  auto expected_noise = noiseModel::Diagonal::Sigmas(sigmas / sqrt(mu));
+  EXPECT(expected_noise->equals(*merit_factor->noiseModel()));
+
+  // Check that error is equal to 0.5*mu * (g(x)+bias)^2/sigmas^2.
+  double expected_error1 = 0;  // 0.5 * 4 * ||0||_(0.1^2)^2
+  EXPECT(assert_equal(expected_error1, merit_factor->error(values1)));
+  double expected_error2 = 3200;  // 0.5 * 4 * ||4||_(0.1^2)^2
+  EXPECT(assert_equal(expected_error2, merit_factor->error(values2)));
+
+  // Check jacobian computation is correct.
+  EXPECT_CORRECT_FACTOR_JACOBIANS(*merit_factor, values1, 1e-7, 1e-5);
+  EXPECT_CORRECT_FACTOR_JACOBIANS(*merit_factor, values2, 1e-7, 1e-5);
+}
+
+// Test methods of VectorExpressionEquality.
+TEST(ExpressionEqualityConstraint, Vector2) {
+  // g(v1, v2) = v1 + v2, our own example.
+  Vector2_ x1_vec_expr(x1_key);
+  Vector2_ x2_vec_expr(x2_key);
+  auto g = x1_vec_expr + x2_vec_expr;
+  auto sigmas = Vector2(0.1, 0.5);
+  auto constraint = ExpressionEqualityConstraint<Vector2>(g, Vector2::Zero(), sigmas);
+
+  EXPECT(constraint.noiseModel()->isConstrained());
+  EXPECT(assert_equal(sigmas, constraint.noiseModel()->sigmas()));
+
+  // Create 2 sets of values for testing.
+  Values values1, values2;
+  values1.insert(x1_key, Vector2(1, 1));
+  values1.insert(x2_key, Vector2(-1, -1));
+  values2.insert(x1_key, Vector2(1, 1));
+  values2.insert(x2_key, Vector2(1, 1));
+
+  // Check that values1 are feasible.
+  EXPECT(constraint.feasible(values1));
+
+  // Check that violation evaluates as 0 at values1.
+  auto expected_violation1 = (Vector(2) << 0, 0).finished();
+  EXPECT(assert_equal(expected_violation1, constraint.unwhitenedError(values1)));
+  auto expected_scaled_violation1 = (Vector(2) << 0, 0).finished();
+  EXPECT(assert_equal(expected_scaled_violation1, constraint.whitenedError(values1)));
+
+  // Check that values2 are indeed deemed infeasible.
+  EXPECT(!constraint.feasible(values2));
+
+  // Check constraint violation is indeed g(x) at values2.
+  auto expected_violation2 = (Vector(2) << 2, 2).finished();
+  EXPECT(assert_equal(expected_violation2, constraint.unwhitenedError(values2)));
+
+  // Check scaled violation is indeed g(x)/sigmas at values2.
+  auto expected_scaled_violation2 = (Vector(2) << 20, 4).finished();
+  EXPECT(assert_equal(expected_scaled_violation2, constraint.whitenedError(values2)));
+
+  // Check dim is the dimension of the vector.
+  EXPECT(constraint.dim() == 2);
+
+  // Generate factor representing the term in merit function.
+  double mu = 4;
+  auto merit_factor = constraint.penaltyFactor(mu);
+
+  // Check that noise model sigma == sigmas/sqrt(mu).
+  auto expected_noise = noiseModel::Diagonal::Sigmas(sigmas / sqrt(mu));
+  EXPECT(expected_noise->equals(*merit_factor->noiseModel()));
+
+  // Check that error is equal to 0.5*mu*||g(x)+bias)||^2_Diag(sigmas^2).
+  double expected_error1 = 0;  // 0.5 * 4 * ||[1, 0.5]||_([0.1,0.5]^2)^2
+  EXPECT(assert_equal(expected_error1, merit_factor->error(values1)));
+  double expected_error2 = 832;  // 0.5 * 4 * ||[2, 2]||_([0.1,0.5]^2)^2
+  EXPECT(assert_equal(expected_error2, merit_factor->error(values2)));
+
+  // Check jacobian computation is correct.
+  EXPECT_CORRECT_FACTOR_JACOBIANS(*merit_factor, values1, 1e-7, 1e-5);
+  EXPECT_CORRECT_FACTOR_JACOBIANS(*merit_factor, values2, 1e-7, 1e-5);
+}
+
+
+// Test methods of FactorZeroErrorConstraint.
+TEST(ZeroCostConstraint, BetweenFactor) {
+  Key x1_key = 1;
+  Key x2_key = 2;
+  Vector sigmas = Vector2(0.5, 0.1);
+  auto noise = noiseModel::Diagonal::Sigmas(sigmas);
+
+  auto factor = std::make_shared<BetweenFactor<Vector2>>(x1_key, x2_key, Vector2(1, 1), noise);
+  auto constraint = ZeroCostConstraint(factor);
+
+  EXPECT(constraint.noiseModel()->isConstrained());
+  EXPECT(assert_equal(sigmas, constraint.noiseModel()->sigmas()));
+
+  // Create 2 sets of values for testing.
+  Values values1, values2;
+  values1.insert(x1_key, Vector2(1, 1));
+  values1.insert(x2_key, Vector2(2, 2));
+  values2.insert(x1_key, Vector2(0, 0));
+  values2.insert(x2_key, Vector2(2, 3));
+
+  // Check that values1 are feasible.
+  EXPECT(constraint.feasible(values1));
+
+  // Check that violation evaluates as 0 at values1.
+  auto expected_violation1 = (Vector(2) << 0, 0).finished();
+  EXPECT(assert_equal(expected_violation1, constraint.unwhitenedError(values1)));
+  auto expected_scaled_violation1 = (Vector(2) << 0, 0).finished();
+  EXPECT(assert_equal(expected_scaled_violation1, constraint.whitenedError(values1)));
+
+  // Check that values2 are indeed deemed infeasible.
+  EXPECT(!constraint.feasible(values2));
+
+  // Check constraint violation is indeed g(x) at values2.
+  auto expected_violation2 = (Vector(2) << 1, 2).finished();
+  EXPECT(assert_equal(expected_violation2, constraint.unwhitenedError(values2)));
+
+  // Check scaled violation is indeed g(x)/sigmas at values2.
+  auto expected_scaled_violation2 = (Vector(2) << 2, 20).finished();
+  EXPECT(assert_equal(expected_scaled_violation2, constraint.whitenedError(values2)));
+
+  // Check dim is the dimension of the vector.
+  EXPECT(constraint.dim() == 2);
+
+  // Generate factor representing the term in merit function.
+  double mu = 4;
+  auto merit_factor = constraint.penaltyFactor(mu);
+
+  // Check that noise model sigma == sigmas/sqrt(mu).
+  auto expected_noise = noiseModel::Diagonal::Sigmas(sigmas / sqrt(mu));
+  EXPECT(expected_noise->equals(*merit_factor->noiseModel()));
+
+  // Check that error is equal to 0.5*mu*||g(x)+bias)||^2_Diag(sigmas^2).
+  double expected_error1 = 0;  // 0.5 * 4 * ||[0, 0]||_([0.5,0.1]^2)^2
+  EXPECT(assert_equal(expected_error1, merit_factor->error(values1)));
+  double expected_error2 = 808;  // 0.5 * 4 * ||[1, 2]||_([0.5,0.1]^2)^2
+  EXPECT(assert_equal(expected_error2, merit_factor->error(values2)));
+
+  // Check jacobian computation is correct.
+  EXPECT_CORRECT_FACTOR_JACOBIANS(*merit_factor, values1, 1e-7, 1e-5);
+  EXPECT_CORRECT_FACTOR_JACOBIANS(*merit_factor, values2, 1e-7, 1e-5);
+}
+
+
+
+
+TEST(NonlinearEqualityConstraints, Container) {
+  NonlinearEqualityConstraints constraints;
+
+  Vector sigmas1 = Vector1(10);
+  auto g1 = x1 + pow(x1, 3) + x2 + pow(x2, 2);
+  Vector2_ x1_vec_expr(x1_key);
+  Vector2_ x2_vec_expr(x2_key);
+  auto g2 = x1_vec_expr + x2_vec_expr;
+  Vector sigmas2 = Vector2(0.1, 0.5);
+
+  constraints.emplace_shared<ExpressionEqualityConstraint<double>>(g1, 0.0, sigmas1);
+  constraints.emplace_shared<ExpressionEqualityConstraint<Vector2>>(g2, Vector2::Zero(), sigmas2);
+
+  // Check size.
+  EXPECT_LONGS_EQUAL(2, constraints.size());
+
+  // Check dimension.
+  EXPECT_LONGS_EQUAL(3, constraints.dim());
+
+  // Check keys.
+  KeySet expected_keys;
+  expected_keys.insert(x1_key);
+  expected_keys.insert(x2_key);
+  EXPECT(assert_container_equality(expected_keys, constraints.keys()));
+
+  // Check VariableIndex.
+  VariableIndex vi(constraints);
+  FactorIndices expected_vi_x1{0, 1};
+  FactorIndices expected_vi_x2{0, 1};
+  EXPECT(assert_container_equality(expected_vi_x1, vi[x1_key]));
+  EXPECT(assert_container_equality(expected_vi_x2, vi[x2_key]));
+
+  // Check constraint violation.
+
+}
+
+
+TEST(NonlinearEqualityConstraints, FromCostGraph) {
+}
+
+int main() {
+  TestResult tr;
+  return TestRegistry::runAllTests(tr);
+}

gtsam/constraint/tests/testNonlinearInequalityConstraint.cpp

@@ -0,0 +1,117 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file    NonlinearInequalityConstraint.h
+ * @brief   Nonlinear inequality constraints in constrained optimization.
+ * @author  Yetong Zhang
+ * @date    Aug 3, 2024
+ */
+
+#include <CppUnitLite/TestHarness.h>
+#include <gtsam/base/Testable.h>
+#include <gtsam/base/TestableAssertions.h>
+#include <gtsam/base/numericalDerivative.h>
+#include <gtsam/constraint/NonlinearInequalityConstraint.h>
+#include <gtsam/nonlinear/factorTesting.h>
+
+#include "constrainedExample.h"
+
+using namespace gtsam;
+using constrained_example::pow;
+using constrained_example::x1, constrained_example::x2;
+using constrained_example::x1_key, constrained_example::x2_key;
+
+// Test methods of DoubleExpressionEquality.
+TEST(NonlinearInequalityConstraint, ScalarExpressionInequalityConstraint) {
+  // create constraint from double expression
+  // g(x1, x2) = x1 + x1^3 + x2 + x2^2, from Vanderbergh slides
+  double sigma = 0.1;
+  auto g = x1 + pow(x1, 3) + x2 + pow(x2, 2);
+  auto constraint_geq = ScalarExpressionInequalityConstraint::GeqZero(g, sigma);
+  auto constraint_leq = ScalarExpressionInequalityConstraint::LeqZero(g, sigma);
+
+  // Check dimension is 1 for scalar g.
+  EXPECT_LONGS_EQUAL(1, constraint_geq->dim());
+  EXPECT_LONGS_EQUAL(1, constraint_leq->dim());
+
+  // Check keys include x1, x2.
+  KeyVector expected_keys{x1_key, x2_key};
+  EXPECT(assert_container_equality(expected_keys, constraint_leq->keys()));
+
+  // Create 3 sets of values for testing.
+  Values values1, values2, values3;
+  values1.insert(x1_key, -1.0);
+  values1.insert(x2_key, 1.0);
+  values2.insert(x1_key, 1.0);
+  values2.insert(x2_key, 1.0);
+  values3.insert(x1_key, -2.0);
+  values3.insert(x2_key, 1.0);
+
+  // Check that expression evaluation at different values.
+  EXPECT(assert_equal(Vector1(0.0), constraint_leq->unwhitenedExpr(values1)));
+  EXPECT(assert_equal(Vector1(4.0), constraint_leq->unwhitenedExpr(values2)));
+  EXPECT(assert_equal(Vector1(-8.0), constraint_leq->unwhitenedExpr(values3)));
+  EXPECT(assert_equal(Vector1(0.0), constraint_geq->unwhitenedExpr(values1)));
+  EXPECT(assert_equal(Vector1(-4.0), constraint_geq->unwhitenedExpr(values2)));
+  EXPECT(assert_equal(Vector1(8.0), constraint_geq->unwhitenedExpr(values3)));
+
+  // Check that constraint violation at different values.
+  EXPECT(assert_equal(Vector1(0.0), constraint_leq->unwhitenedError(values1)));
+  EXPECT(assert_equal(Vector1(4.0), constraint_leq->unwhitenedError(values2)));
+  EXPECT(assert_equal(Vector1(0.0), constraint_leq->unwhitenedError(values3)));
+  EXPECT(assert_equal(Vector1(0.0), constraint_geq->unwhitenedError(values1)));
+  EXPECT(assert_equal(Vector1(0.0), constraint_geq->unwhitenedError(values2)));
+  EXPECT(assert_equal(Vector1(8.0), constraint_geq->unwhitenedError(values3)));
+
+  // Check feasible.
+  EXPECT(constraint_leq->feasible(values1));
+  EXPECT(!constraint_leq->feasible(values2));
+  EXPECT(constraint_leq->feasible(values3));
+  EXPECT(constraint_geq->feasible(values1));
+  EXPECT(constraint_geq->feasible(values2));
+  EXPECT(!constraint_geq->feasible(values3));
+
+  // Check active.
+  EXPECT(constraint_leq->active(values1));
+  EXPECT(constraint_leq->active(values2));
+  EXPECT(!constraint_leq->active(values3));
+  EXPECT(constraint_geq->active(values1));
+  EXPECT(!constraint_geq->active(values2));
+  EXPECT(constraint_geq->active(values3));
+
+  // Check whitenedError of penalty factor.
+  // Expected to be sqrt(mu) / sigma * ramp(g(x))
+  double mu = 9.0;
+  auto penalty_leq = constraint_leq->penaltyFactor(mu);
+  auto penalty_geq = constraint_geq->penaltyFactor(mu);
+  EXPECT(assert_equal(Vector1(0.0), penalty_leq->whitenedError(values1)));
+  EXPECT(assert_equal(Vector1(120.0), penalty_leq->whitenedError(values2)));
+  EXPECT(assert_equal(Vector1(0.0), penalty_leq->whitenedError(values3)));
+  EXPECT(assert_equal(Vector1(0.0), penalty_geq->whitenedError(values1)));
+  EXPECT(assert_equal(Vector1(0.0), penalty_geq->whitenedError(values2)));
+  EXPECT(assert_equal(Vector1(240.0), penalty_geq->whitenedError(values3)));
+
+  // Check create equality constraint
+  auto constraint_eq1 = constraint_leq->createEqualityConstraint();
+  auto constraint_eq2 = constraint_geq->createEqualityConstraint();
+  EXPECT(assert_equal(0.0, constraint_eq1->error(values1)));
+  EXPECT(assert_equal(800.0, constraint_eq1->error(values2)));
+  EXPECT(assert_equal(3200.0, constraint_eq1->error(values3)));
+  EXPECT(assert_equal(0.0, constraint_eq2->error(values1)));
+  EXPECT(assert_equal(800.0, constraint_eq2->error(values2)));
+  EXPECT(assert_equal(3200.0, constraint_eq2->error(values3)));
+}
+
+int main() {
+  TestResult tr;
+  return TestRegistry::runAllTests(tr);
+}

gtsam/constraint/tests/testSimple.cpp

@@ -0,0 +1,37 @@
+#include <CppUnitLite/TestHarness.h>
+
+#include <gtsam/base/Testable.h>
+#include <gtsam/base/TestableAssertions.h>
+#include <gtsam/base/numericalDerivative.h>
+#include <gtsam/nonlinear/factorTesting.h>
+#include <gtsam/slam/BetweenFactor.h>
+
+#include <iostream>
+#include <functional>
+
+// Define a functor class
+class MyFunctor {
+public:
+    // Overload the function call operator
+    void operator()(int x) const {
+        std::cout << "MyFunctor called with " << x << std::endl;
+    }
+};
+
+
+TEST(InequalityConstraint, DoubleExpressionInequality) {
+    // Create an instance of the functor
+    MyFunctor functor;
+
+    // Cast the functor to std::function
+    std::function<void(int)> func = functor;
+
+    // Call the std::function object
+    func(42);
+}
+
+
+int main() {
+  TestResult tr;
+  return TestRegistry::runAllTests(tr);
+}

gtsam/nonlinear/Expression.h

@@ -34,6 +34,8 @@ namespace gtsam {
 // Forward declares
 class Values;
 template<typename T> class ExpressionFactor;
+template<typename T> class ExpressionEqualityConstraint;
+class ScalarExpressionInequalityConstraint;
 
 namespace internal {
 template<typename T> class ExecutionTrace;
@@ -206,6 +208,8 @@ protected:
   // be very selective on who can access these private methods:
   friend class ExpressionFactor<T> ;
   friend class internal::ExpressionNode<T>;
+  friend class ExpressionEqualityConstraint<T>;
+  friend class ScalarExpressionInequalityConstraint;
 
   // and add tests
   friend class ::ExpressionFactorShallowTest;