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/InequalityPenaltyFunction.cpp

@@ -0,0 +1,91 @@
+/* ----------------------------------------------------------------------------
+
+ * 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    InequalityPenaltyFunction.cpp
+ * @brief   Ramp function to compute inequality constraint violations.
+ * @author  Yetong Zhang
+ * @date    Aug 3, 2024
+ */
+
+#include <gtsam/constraint/InequalityPenaltyFunction.h>
+
+namespace gtsam {
+
+/* ********************************************************************************************* */
+InequalityPenaltyFunction::UnaryScalarFunc InequalityPenaltyFunction::function() const {
+  return [=](const double& x, OptionalJacobian<1, 1> H = {}) -> double { return (*this)(x, H); };
+}
+
+/* ********************************************************************************************* */
+double RampFunction::Ramp(const double x, OptionalJacobian<1, 1> H) {
+  if (x < 0) {
+    if (H) {
+      H->setConstant(0);
+    }
+    return 0;
+  } else {
+    if (H) {
+      H->setConstant(1);
+    }
+    return x;
+  }
+}
+
+/* ********************************************************************************************* */
+double SmoothRampPoly2::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(2 * a_ * x);
+    }
+    return a_ * pow(x, 2);
+  } else {
+    if (H) {
+      H->setOnes();
+    }
+    return x - epsilon_ / 2;
+  }
+}
+
+/* ********************************************************************************************* */
+double SmoothRampPoly3::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();
+    }
+    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/InequalityPenaltyFunction.h

@@ -0,0 +1,140 @@
+/* ----------------------------------------------------------------------------
+
+ * 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    InequalityPenaltyFunction.h
+ * @brief   Ramp function to compute inequality constraint violations.
+ * @author  Yetong Zhang
+ * @date    Aug 3, 2024
+ */
+
+#pragma once
+
+#include <gtsam/nonlinear/ExpressionFactor.h>
+#include <gtsam/nonlinear/expressions.h>
+
+namespace gtsam {
+
+/** Base class for smooth approximation of the ramp function. */
+class GTSAM_EXPORT InequalityPenaltyFunction {
+ public:
+  typedef std::shared_ptr<InequalityPenaltyFunction> shared_ptr;
+  typedef std::function<double(const double& x, OptionalJacobian<1, 1> H)>
+      UnaryScalarFunc;
+
+  /** Constructor. */
+  InequalityPenaltyFunction() {}
+
+  /** Destructor. */
+  virtual ~InequalityPenaltyFunction() {}
+
+  virtual double operator()(const double& x,
+                            OptionalJacobian<1, 1> H = {}) const = 0;
+
+  virtual UnaryScalarFunc function() const;
+};
+
+/** Ramp function f : R -> R.
+ *  f(x) =  0     for   x <= 0
+ *          x     for   x > 0
+ */
+class GTSAM_EXPORT RampFunction : public InequalityPenaltyFunction {
+ public:
+  typedef InequalityPenaltyFunction Base;
+  typedef RampFunction This;
+  typedef std::shared_ptr<This> shared_ptr;
+
+ public:
+  RampFunction() : Base() {}
+
+  virtual double operator()(const double& x,
+                            OptionalJacobian<1, 1> H = {}) const override {
+    return Ramp(x, H);
+  }
+
+  virtual UnaryScalarFunc function() const override { return Ramp; }
+
+  static double Ramp(const double x, OptionalJacobian<1, 1> H = {});
+};
+
+/** 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 GTSAM_EXPORT SmoothRampPoly2 : public InequalityPenaltyFunction {
+ public:
+  typedef InequalityPenaltyFunction Base;
+  typedef SmoothRampPoly2 This;
+  typedef std::shared_ptr<This> shared_ptr;
+
+ protected:
+  double epsilon_;
+  double a_;
+
+ public:
+  SmoothRampPoly2(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 GTSAM_EXPORT SmoothRampPoly3 : public InequalityPenaltyFunction {
+ public:
+  typedef InequalityPenaltyFunction Base;
+  typedef SmoothRampPoly3 This;
+  typedef std::shared_ptr<This> shared_ptr;
+
+ protected:
+  double epsilon_;
+  double a_;
+  double b_;
+
+ public:
+  SmoothRampPoly3(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;
+};
+
+/** Softplus function that implements f(x) = log(1 + exp(k*x)) / k. */
+class GTSAM_EXPORT SoftPlusFunction : public InequalityPenaltyFunction {
+ public:
+  typedef InequalityPenaltyFunction Base;
+  typedef SoftPlusFunction This;
+  typedef std::shared_ptr<This> shared_ptr;
+
+ 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/NonlinearConstraint.h

@@ -0,0 +1,87 @@
+/* ----------------------------------------------------------------------------
+
+ * 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   Nonlinear constraints in constrained optimization.
+ * @author  Yetong Zhang, Frank Dellaert
+ * @date    Aug 3, 2024
+ */
+
+#pragma once
+
+#include <gtsam/nonlinear/NonlinearFactor.h>
+#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#include <boost/serialization/base_object.hpp>
+#endif
+
+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 GTSAM_EXPORT 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 {
+    return cloneWithNewNoiseModel(penaltyNoise(mu));
+  }
+
+  /** 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;
+  }
+
+  const Vector sigmas() const { return noiseModel()->sigmas(); }
+
+  // return the hessian of unwhitened error function in each dimension.
+  virtual std::vector<Matrix> unwhitenedHessian(const Values& x) const {
+    throw std::runtime_error("hessian not implemented");
+  }
+
+ protected:
+  /** 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);
+  }
+
+ private:
+#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+  /** Serialization function */
+  friend class boost::serialization::access;
+  template <class ARCHIVE>
+  void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
+    ar& boost::serialization::make_nvp("NonlinearConstraint",
+                                       boost::serialization::base_object<Base>(*this));
+  }
+#endif
+};
+
+}  // 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    NonlinearEqualityConstraint-inl.h
+ * @brief   Nonlinear 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