add definition of constraints

Using-GTSAM-EXPORT.md

@@ -3,9 +3,9 @@
 To create a DLL in windows, the `GTSAM_EXPORT` keyword has been created and needs to be applied to different methods and classes in the code to expose this code outside of the DLL.  However, there are several intricacies that make this more difficult than it sounds.  In general, if you follow the following three rules, GTSAM_EXPORT should work properly.  The rest of the document also describes (1) the common error message encountered when you are not following these rules and (2) the reasoning behind these usage rules.
 
 ## Usage Rules
-1.  Put `GTSAM_EXPORT` in front of any function that you want exported in the DLL _if and only if_ that function is declared in a .cpp file, not just a .h file.
+1.  Put `GTSAM_EXPORT` in front of any function that you want exported in the DLL _if and only if_ that function is defined in a .cpp file, not just a .h file.
 2.  Use `GTSAM_EXPORT` in a class definition (i.e. `class GSTAM_EXPORT MyClass {...}`) only if:
-    * At least one of the functions inside that class is declared in a .cpp file and not just the .h file.
+    * At least one of the functions inside that class is defined in a .cpp file and not just the .h file.
     * You can `GTSAM_EXPORT` any class it inherits from as well.  (Note that this implictly requires the class does not derive from a "header-only" class.  Note that Eigen is a "header-only" library, so if your class derives from Eigen, _do not_ use `GTSAM_EXPORT` in the class definition!) 
 3.  If you have defined a class using `GTSAM_EXPORT`, do not use `GTSAM_EXPORT` in any of its individual function declarations.  (Note that you _can_ put `GTSAM_EXPORT` in the definition of individual functions within a class as long as you don't put `GTSAM_EXPORT` in the class definition.)
 4. For template specializations, you need to add `GTSAM_EXPORT` to each individual specialization.
@@ -28,7 +28,7 @@ But first, we need to understand exactly what `GTSAM_EXPORT` is.  `GTSAM_EXPORT`
 
 Rule #1 doesn't seem very bad, until you combine it with rule #2
 
-***Compiler Rule #2*** Anything declared in a header file is not included in a DLL.
+***Compiler Rule #2*** Anything defined in a header file is not included in a DLL.
 
 When these two rules are combined, you get some very confusing results.  For example, a class which is completely defined in a header (e.g. Foo) cannot use `GTSAM_EXPORT` in its definition.  If Foo is defined with `GTSAM_EXPORT`, then the compiler _must_ find Foo in a DLL.  Because Foo is a header-only class, however, it can't find it, leading to a very confusing "I can't find this symbol" type of error.  Note that the linker says it can't find the symbol even though the compiler found the header file that completely defines the class.
 

gtsam/CMakeLists.txt

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

gtsam/constrained/CMakeLists.txt

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

gtsam/constrained/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/constrained/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/constrained/InequalityPenaltyFunction.h

@@ -0,0 +1,149 @@
+/* ----------------------------------------------------------------------------
+
+ * 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:
+  /** Constructor.
+   * @param epsilon parameter for adjusting the smoothness of the function.
+   */
+  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:
+  /** Constructor.
+   * @param epsilon parameter for adjusting the smoothness of the function.
+   */
+  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:
+  /** Constructor.
+   * @param k parameter for adjusting the smoothness of the function.
+   */
+  SoftPlusFunction(const double k = 1) : Base(), k_(k) {}
+
+  virtual double operator()(const double& x,
+                            OptionalJacobian<1, 1> H = {}) const override;
+};
+
+}  // namespace gtsam

gtsam/constrained/NonlinearConstraint.h

@@ -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    NonlinearConstraint.h
+ * @brief   Nonlinear constraints in constrained optimization.
+ * @author  Yetong Zhang, Frank Dellaert
+ * @date    Aug 3, 2024
+ */
+
+#pragma once
+
+#include <gtsam/inference/FactorGraph.h>
+#include <gtsam/inference/FactorGraph-inst.h>
+#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;
+
+  using NonlinearFactor::error;
+
+  /** 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