addressing comments

gtsam/CMakeLists.txt

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

gtsam/constrained/CMakeLists.txt

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

gtsam/constrained/InequalityPenaltyFunction.cpp

@@ -16,7 +16,7 @@
  * @date    Aug 3, 2024
  */
 
-#include <gtsam/constraint/InequalityPenaltyFunction.h>
+#include <gtsam/constrained/InequalityPenaltyFunction.h>
 
 namespace gtsam {
 

gtsam/constrained/InequalityPenaltyFunction.h

@@ -83,6 +83,9 @@ class GTSAM_EXPORT SmoothRampPoly2 : public InequalityPenaltyFunction {
   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) {}
 
@@ -110,6 +113,9 @@ class GTSAM_EXPORT SmoothRampPoly3 : public InequalityPenaltyFunction {
   double b_;
 
  public:
+  /** Constructor.
+   * @param epsilon parameter for adjusting the smoothness of the function.
+   */
   SmoothRampPoly3(const double epsilon = 1)
       : Base(),
         epsilon_(epsilon),
@@ -131,6 +137,9 @@ class GTSAM_EXPORT SoftPlusFunction : public InequalityPenaltyFunction {
   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,

gtsam/constrained/NonlinearEqualityConstraint-inl.h

@@ -17,7 +17,7 @@
 
 #pragma once
 
-#include <gtsam/constraint/NonlinearEqualityConstraint.h>
+#include <gtsam/constrained/NonlinearEqualityConstraint.h>
 
 namespace gtsam {
 

gtsam/constrained/NonlinearEqualityConstraint.cpp

@@ -15,7 +15,7 @@
  * @author  Yetong Zhang, Frank Dellaert
  * @date    Aug 3, 2024 */
 
-#include <gtsam/constraint/NonlinearEqualityConstraint.h>
+#include <gtsam/constrained/NonlinearEqualityConstraint.h>
 #include <gtsam/inference/FactorGraph-inst.h>
 
 namespace gtsam {

gtsam/constrained/NonlinearEqualityConstraint.h

@@ -17,7 +17,7 @@
 
 #pragma once
 
-#include <gtsam/constraint/NonlinearConstraint.h>
+#include <gtsam/constrained/NonlinearConstraint.h>
 #include <gtsam/nonlinear/ExpressionFactor.h>
 #include <gtsam/nonlinear/NonlinearFactorGraph.h>
 
@@ -99,7 +99,10 @@ class ExpressionEqualityConstraint : public NonlinearEqualityConstraint {
 #endif
 };
 
-/** Equality constraint that enforce the cost factor with zero error. */
+/** Equality constraint that enforce the cost factor with zero error. 
+ * e.g., for a factor with unwhitened cost 2x-1, the constraint enforces the
+ * equlity 2x-1=0.
+ */
 class GTSAM_EXPORT ZeroCostConstraint : public NonlinearEqualityConstraint {
  public:
   typedef NonlinearEqualityConstraint Base;
@@ -114,7 +117,6 @@ class GTSAM_EXPORT ZeroCostConstraint : public NonlinearEqualityConstraint {
    * @brief Constructor.
    *
    * @param factor  NoiseModel factor.
-   * @param tolerance   vector representing tolerance in each dimension.
    */
   ZeroCostConstraint(const NoiseModelFactor::shared_ptr& factor);
 
@@ -177,4 +179,4 @@ class GTSAM_EXPORT NonlinearEqualityConstraints : public FactorGraph<NonlinearEq
 
 }  // namespace gtsam
 
-#include <gtsam/constraint/NonlinearEqualityConstraint-inl.h>
+#include <gtsam/constrained/NonlinearEqualityConstraint-inl.h>

gtsam/constrained/NonlinearInequalityConstraint.cpp

@@ -16,7 +16,7 @@
  * @date    Aug 3, 2024
  */
 
-#include <gtsam/constraint/NonlinearInequalityConstraint.h>
+#include <gtsam/constrained/NonlinearInequalityConstraint.h>
 #include <gtsam/inference/FactorGraph-inst.h>
 
 namespace gtsam {

gtsam/constrained/NonlinearInequalityConstraint.h

@@ -18,8 +18,8 @@
 
 #pragma once
 
-#include <gtsam/constraint/InequalityPenaltyFunction.h>
+#include <gtsam/constrained/InequalityPenaltyFunction.h>
-#include <gtsam/constraint/NonlinearEqualityConstraint.h>
+#include <gtsam/constrained/NonlinearEqualityConstraint.h>
 #include <gtsam/nonlinear/expressions.h>
 
 namespace gtsam {

gtsam/constrained/tests/constrainedExample.h

@@ -0,0 +1,73 @@
+/* ----------------------------------------------------------------------------
+
+ * 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/constrained/ConstrainedOptProblem.h>
+#include <gtsam/nonlinear/expressions.h>
+#include <gtsam/slam/BetweenFactor.h>
+#include <gtsam/constrained/NonlinearEqualityConstraint.h>
+#include <gtsam/constrained/NonlinearInequalityConstraint.h>
+
+namespace constrained_example {
+
+using namespace gtsam;
+
+/// Exponential function e^x.
+inline 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.
+inline 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.
+inline Double_ pow(const Double_& x, const double& c) {
+  PowFunctor pow_functor(c);
+  return Double_(pow_functor, x);
+}
+
+/// Plus between Double expression and double.
+inline Double_ operator+(const Double_& x, const double& d) { return x + Double_(d); }
+
+/// Negative sign operator.
+inline 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
+

gtsam/constrained/tests/testInequalityPenaltyFunction.cpp

@@ -20,7 +20,7 @@
 #include <gtsam/base/Testable.h>
 #include <gtsam/base/TestableAssertions.h>
 #include <gtsam/base/numericalDerivative.h>
-#include <gtsam/constraint/InequalityPenaltyFunction.h>
+#include <gtsam/constrained/InequalityPenaltyFunction.h>
 
 using namespace gtsam;
 

gtsam/constrained/tests/testNonlinearEqualityConstraint.cpp

@@ -21,7 +21,7 @@
 #include <gtsam/base/Testable.h>
 #include <gtsam/base/TestableAssertions.h>
 #include <gtsam/base/numericalDerivative.h>
-#include <gtsam/constraint/NonlinearEqualityConstraint.h>
+#include <gtsam/constrained/NonlinearEqualityConstraint.h>
 #include <gtsam/nonlinear/factorTesting.h>
 #include <gtsam/slam/BetweenFactor.h>
 

gtsam/constrained/tests/testNonlinearInequalityConstraint.cpp

@@ -20,7 +20,7 @@
 #include <gtsam/base/Testable.h>
 #include <gtsam/base/TestableAssertions.h>
 #include <gtsam/base/numericalDerivative.h>
-#include <gtsam/constraint/NonlinearInequalityConstraint.h>
+#include <gtsam/constrained/NonlinearInequalityConstraint.h>
 #include <gtsam/nonlinear/factorTesting.h>
 
 #include "constrainedExample.h"

gtsam/constraint/tests/constrainedExample.h

@@ -1,181 +0,0 @@
-/* ----------------------------------------------------------------------------
-
- * 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/constraint/ConstrainedOptProblem.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.
-inline 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.
-inline 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.
-inline Double_ pow(const Double_& x, const double& c) {
-  PowFunctor pow_functor(c);
-  return Double_(pow_functor, x);
-}
-
-/// Plus between Double expression and double.
-inline Double_ operator+(const Double_& x, const double& d) { return x + Double_(d); }
-
-/// Negative sign operator.
-inline 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 = 0
- */
-// namespace constrained_example1 {
-// using namespace constrained_example;
-// NonlinearFactorGraph Cost() {
-//   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 EqConstraints() {
-//   NonlinearEqualityConstraints constraints;
-//   Vector sigmas = Vector1(1.0);
-//   auto h1 = x1 + pow(x1, 3) + x2 + pow(x2, 2);
-//   constraints.push_back(NonlinearEqualityConstraint::shared_ptr(
-//       new ExpressionEqualityConstraint<double>(h1, 0.0, sigmas)));
-//   return constraints;
-// }
-
-// Values InitValues() {
-//   Values values;
-//   values.insert(x1_key, -0.2);
-//   values.insert(x2_key, -0.2);
-//   return values;
-// }
-
-// Values OptimalValues() {
-//   Values values;
-//   values.insert(x1_key, 0.0);
-//   values.insert(x2_key, 0.0);
-//   return values;
-// }
-
-// NonlinearFactorGraph costs = Cost();
-// NonlinearEqualityConstraints e_constraints = EqConstraints();
-// NonlinearInequalityConstraints i_constraints;
-// Values init_values = InitValues();
-// ConstrainedOptProblem::shared_ptr problem =
-//     std::make_shared<ConstrainedOptProblem>(costs, e_constraints, i_constraints, init_values);
-// Values optimal_values = OptimalValues();
-// }  // namespace constrained_example1
-
-// /* ************************************************************************* */
-// /**
-//  * Constrained optimization example with inequality constraints
-//  * Approach a point while staying on unit circle, and within an ellipse.
-//  * f(x) = 0.5 * ||x1-1||^2 + 0.5 * ||x2-1||^2
-//  * h(x) = x1^2 + x2^2 - 1 = 0
-//  * g(x) = 4*x1^2 + 0.25*x2^2 - 1 <= 0
-//  */
-// namespace constrained_example2 {
-// using namespace constrained_example;
-// NonlinearFactorGraph Cost() {
-//   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;
-// }
-
-// NonlinearEqualityConstraints EqConstraints() {
-//   NonlinearEqualityConstraints constraints;
-//   Vector1 sigmas(1.0);
-//   Double_ h1 = x1 * x1 + x2 * x2;
-//   constraints.emplace_shared<ExpressionEqualityConstraint<double>>(h1, 1.0, sigmas);
-//   return constraints;
-// }
-
-// NonlinearInequalityConstraints IneqConstraints() {
-//   NonlinearInequalityConstraints constraints;
-//   Double_ g1 = 4 * x1 * x1 + 0.25 * x2 * x2 - Double_(1.0);
-//   double sigma = 1;
-//   constraints.emplace_shared<ScalarExpressionInequalityConstraint>(g1, sigma);
-//   return constraints;
-// }
-
-// Values InitValues() {
-//   Values values;
-//   values.insert(x1_key, -1.0);
-//   values.insert(x2_key, 2.0);
-//   return values;
-// }
-
-// Values OptimalValues() {
-//   Values values;
-//   values.insert(x1_key, 1.0 / sqrt(5));
-//   values.insert(x2_key, 2.0 / sqrt(5));
-//   return values;
-// }
-
-// NonlinearFactorGraph costs = Cost();
-// NonlinearEqualityConstraints e_constraints = EqConstraints();
-// NonlinearInequalityConstraints i_constraints = IneqConstraints();
-// Values init_values = InitValues();
-// ConstrainedOptProblem::shared_ptr problem =
-//     std::make_shared<ConstrainedOptProblem>(costs, e_constraints, i_constraints, init_values);
-// Values optimal_values = OptimalValues();
-
-// }  // namespace constrained_example2

gtsam/nonlinear/NonlinearEquality.h

@@ -18,7 +18,7 @@
 #pragma once
 
 #include <gtsam/nonlinear/NonlinearFactor.h>
-#include <gtsam/constraint/NonlinearEqualityConstraint.h>
+#include <gtsam/constrained/NonlinearEqualityConstraint.h>
 #include <gtsam/base/Testable.h>
 #include <gtsam/base/Manifold.h>
 

gtsam/slam/BoundingConstraint.h

@@ -19,7 +19,7 @@
 
 #include <gtsam/base/Lie.h>
 #include <gtsam/nonlinear/NonlinearFactor.h>
-#include <gtsam/constraint/NonlinearInequalityConstraint.h>
+#include <gtsam/constrained/NonlinearInequalityConstraint.h>
 
 namespace gtsam {
 
@@ -59,7 +59,7 @@ struct BoundingConstraint1: public NonlinearInequalityConstraint {
   virtual double value(const X& x, OptionalMatrixType H =
       OptionalNone) const = 0;
 
-  Vector unwhitenedExpr(const Values& x, OptionalMatrixVecType H = nullptr) const override {
+  Vector unwhitenedExpr(const Values& x, OptionalMatrixVecType H = {}) const override {
     if (H) {
       double d = value(x.at<X>(this->key()), &(H->front()));
       if (isGreaterThan_) {
@@ -75,7 +75,7 @@ struct BoundingConstraint1: public NonlinearInequalityConstraint {
   }
 
   /// TODO: This should be deprecated.
-  Vector evaluateError(const X& x, OptionalMatrixType H = nullptr) const {
+  Vector evaluateError(const X& x, OptionalMatrixType H = {}) const {
     Matrix D;
     double error = value(x, &D) - threshold_;
     if (H) {
@@ -138,7 +138,7 @@ struct BoundingConstraint2: public NonlinearInequalityConstraint {
       OptionalMatrixType H1 = OptionalNone,
       OptionalMatrixType H2 = OptionalNone) const = 0;
 
-  Vector unwhitenedExpr(const Values& x, OptionalMatrixVecType H = nullptr) const override {
+  Vector unwhitenedExpr(const Values& x, OptionalMatrixVecType H = {}) const override {
     X1 x1 = x.at<X1>(keys().front());
     X2 x2 = x.at<X2>(keys().back());
     if (H) {
@@ -158,7 +158,7 @@ struct BoundingConstraint2: public NonlinearInequalityConstraint {
 
   /// TODO: This should be deprecated.
   Vector evaluateError(const X1& x1, const X2& x2,
-      OptionalMatrixType H1 = nullptr, OptionalMatrixType H2 = nullptr) const {
+      OptionalMatrixType H1 = {}, OptionalMatrixType H2 = {}) const {
     Matrix D1, D2;
     double error = value(x1, x2, &D1, &D2) - threshold_;
     if (H1) {