Merge pull request #2394 from borglab/feature/format_docs_spelling

Constrained docs

Author: dellaert

gtsam/constrained/Constrained.md

@@ -0,0 +1,96 @@
+# Constrained
+
+The `constrained` module in GTSAM provides constrained nonlinear optimization on top of factor graphs.
+It includes classes for representing constraints, building constrained problems, and solving them with penalty and augmented Lagrangian methods.
+
+## Core Problem Model
+
+- [`ConstrainedOptProblem`](doc/ConstrainedOptProblem.ipynb): Holds objective costs, equality constraints, and inequality constraints.
+- [`ConstrainedOptProblem::AuxiliaryKeyGenerator`](doc/ConstrainedOptProblem.ipynb): Generates keys for auxiliary variables used when transforming inequality constraints.
+- [`NonlinearConstraint`](doc/NonlinearConstraint.ipynb): Base class for nonlinear constraints represented as constrained `NoiseModelFactor` objects.
+
+## Equality Constraints
+
+- [`NonlinearEqualityConstraint`](doc/NonlinearEqualityConstraint.ipynb): Base class for constraints of the form `h(x) = 0`.
+- [`ExpressionEqualityConstraint<T>`](doc/NonlinearEqualityConstraint.ipynb): Equality constraint from an expression and right-hand side.
+- [`ZeroCostConstraint`](doc/NonlinearEqualityConstraint.ipynb): Equality constraint that enforces zero residual on a cost factor.
+- [`NonlinearEqualityConstraints`](doc/NonlinearEqualityConstraint.ipynb): Container graph for equality constraints.
+
+## Inequality Constraints
+
+- [`NonlinearInequalityConstraint`](doc/NonlinearInequalityConstraint.ipynb): Base class for constraints of the form `g(x) <= 0`.
+- [`ScalarExpressionInequalityConstraint`](doc/NonlinearInequalityConstraint.ipynb): Scalar expression-based inequality constraint.
+- [`NonlinearInequalityConstraints`](doc/NonlinearInequalityConstraint.ipynb): Container graph for inequality constraints.
+- [`InequalityPenaltyFunction`](doc/InequalityPenaltyFunction.ipynb): Interface for ramp-like penalty mappings used with inequality constraints.
+  Derived classes:
+  - [`RampFunction`](doc/InequalityPenaltyFunction.ipynb)
+  - [`SmoothRampPoly2`](doc/InequalityPenaltyFunction.ipynb)
+  - [`SmoothRampPoly3`](doc/InequalityPenaltyFunction.ipynb)
+  - [`SoftPlusFunction`](doc/InequalityPenaltyFunction.ipynb)
+
+## Optimizers
+
+- [`ConstrainedOptimizerParams`](doc/ConstrainedOptimizer.ipynb), [`ConstrainedOptimizerState`](doc/ConstrainedOptimizer.ipynb), [`ConstrainedOptimizer`](doc/ConstrainedOptimizer.ipynb): Shared base interfaces and iteration state for constrained solvers.
+- [`PenaltyOptimizerParams`](doc/PenaltyOptimizer.ipynb), [`PenaltyOptimizerState`](doc/PenaltyOptimizer.ipynb), [`PenaltyOptimizer`](doc/PenaltyOptimizer.ipynb): Penalty method solver and its parameters/state.
+- [`AugmentedLagrangianParams`](doc/AugmentedLagrangianOptimizer.ipynb), [`AugmentedLagrangianState`](doc/AugmentedLagrangianOptimizer.ipynb), [`AugmentedLagrangianOptimizer`](doc/AugmentedLagrangianOptimizer.ipynb): Augmented Lagrangian solver and its parameters/state.
+
+## How the Pieces Fit Together
+
+For a new user, it helps to think in two phases:
+
+1. Build a constrained problem.
+2. Run a constrained solver on that problem.
+
+Inequality constraints can use different smooth penalty shapes via
+`InequalityPenaltyFunction` (ramp, smooth polynomial ramps, or softplus),
+which controls behavior near the active constraint boundary.
+
+### 1) Build the Problem
+
+This stage is about modeling: you separate what you want to minimize
+(objective terms) from what must hold (constraints), then combine them into a
+single `ConstrainedOptProblem` object that the solvers can consume.
+
+```mermaid
+flowchart TB
+  User["User-defined model"]
+  Costs["Objective terms<br/>NonlinearFactorGraph"]
+  Eq["Equality constraints<br/>NonlinearEqualityConstraint(s)<br/>h(x)=0"]
+  Ineq["Inequality constraints<br/>NonlinearInequalityConstraint(s)<br/>g(x)<=0"]
+  Problem["ConstrainedOptProblem"]
+
+  User --> Costs
+  User --> Eq
+  User --> Ineq
+  Costs --> Problem
+  Eq --> Problem
+  Ineq --> Problem
+```
+
+### 2) Solve the Problem
+
+This stage is algorithmic: pick a constrained solver, form iterative
+unconstrained subproblems internally, and solve those subproblems with a
+standard nonlinear optimizer until constraint violation and cost are reduced.
+
+```mermaid
+flowchart TB
+  Problem["ConstrainedOptProblem"]
+  Choose{"Choose constrained solver"}
+  Penalty["PenaltyOptimizer"]
+  AL["AugmentedLagrangianOptimizer"]
+  PenFunc["InequalityPenaltyFunction<br/>(ramp / smooth ramp / softplus)"]
+  Sub["Iterative unconstrained subproblems"]
+  LM["Nonlinear optimizer<br/>(Levenberg-Marquardt by default)"]
+  Result["Optimized Values<br/>+ cost and violation metrics"]
+
+  Problem --> Choose
+  Choose --> Penalty
+  Choose --> AL
+  PenFunc --> Penalty
+  PenFunc --> AL
+  Penalty --> Sub
+  AL --> Sub
+  Sub --> LM
+  LM --> Result
+```

gtsam/constrained/ConstrainedOptProblem.h

@@ -28,18 +28,19 @@ namespace gtsam {
  *    s.t.     h(X) = 0
  *             g(X) <= 0
  * where X represents the variables, 0.5||f(X)||^2 represents the quadratic cost
- * functions, h(X)=0 represents the nonlinear equality constraints, g(x)<=0 represents the
- * inequality constraints.
+ * functions, h(X)=0 represents the nonlinear equality constraints, g(x)<=0
+ * represents the inequality constraints.
  */
 class GTSAM_EXPORT ConstrainedOptProblem {
  public:
   typedef ConstrainedOptProblem This;
   typedef std::shared_ptr<This> shared_ptr;
 
  protected:
-  NonlinearFactorGraph costs_;                    // cost function, ||f(X)||^2
-  NonlinearEqualityConstraints eqConstraints_;    // equality constraints, h(X)=0
-  NonlinearInequalityConstraints ineqConstraints_;  // inequality constraints, g(X)<=0
+  NonlinearFactorGraph costs_;                  // cost function, ||f(X)||^2
+  NonlinearEqualityConstraints eqConstraints_;  // equality constraints, h(X)=0
+  NonlinearInequalityConstraints
+      ineqConstraints_;  // inequality constraints, g(X)<=0
 
  public:
   /** Default constructor. */
@@ -57,16 +58,22 @@ class GTSAM_EXPORT ConstrainedOptProblem {
   static ConstrainedOptProblem EqConstrainedOptProblem(
       const NonlinearFactorGraph& costs,
       const NonlinearEqualityConstraints& eqConstraints) {
-    return ConstrainedOptProblem(costs, eqConstraints, NonlinearInequalityConstraints());
+    return ConstrainedOptProblem(costs, eqConstraints,
+                                 NonlinearInequalityConstraints());
   }
 
   /** Member variable access functions. */
   const NonlinearFactorGraph& costs() const { return costs_; }
-  const NonlinearEqualityConstraints& eConstraints() const { return eqConstraints_; }
-  const NonlinearInequalityConstraints& iConstraints() const { return ineqConstraints_; }
+  const NonlinearEqualityConstraints& eConstraints() const {
+    return eqConstraints_;
+  }
+  const NonlinearInequalityConstraints& iConstraints() const {
+    return ineqConstraints_;
+  }
 
   /** Evaluate cost and constraint violations.
-   * Return a tuple representing (cost, e-constraint violation, i-constraint violation).
+   * Return a tuple representing (cost, e-constraint violation, i-constraint
+   * violation).
    */
   std::tuple<double, double, double> evaluate(const Values& values) const;
 

gtsam/constrained/NonlinearConstraint.h

@@ -18,8 +18,8 @@
 
 #pragma once
 
-#include <gtsam/inference/FactorGraph.h>
 #include <gtsam/inference/FactorGraph-inst.h>
+#include <gtsam/inference/FactorGraph.h>
 #include <gtsam/nonlinear/NonlinearFactor.h>
 #if GTSAM_ENABLE_BOOST_SERIALIZATION
 #include <boost/serialization/base_object.hpp>
@@ -29,8 +29,8 @@ namespace gtsam {
 
 /**
  * Base class for nonlinear constraint.
- * The constraint is represented as a NoiseModelFactor with Constrained noise model.
- * whitenedError() returns The constraint violation vector.
+ * 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 {
@@ -45,8 +45,10 @@ class GTSAM_EXPORT NonlinearConstraint : public NoiseModelFactor {
   /** 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 {
+  /** 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));
   }
 
@@ -71,7 +73,7 @@ class GTSAM_EXPORT NonlinearConstraint : public NoiseModelFactor {
     return noiseModel::Diagonal::Sigmas(noiseModel()->sigmas() / sqrt(mu));
   }
 
-  /** Default constrained noisemodel used for construction of constraint. */
+  /** Default constrained noise model used for construction of constraint. */
   static SharedNoiseModel constrainedNoise(const Vector& sigmas) {
     return noiseModel::Constrained::MixedSigmas(1.0, sigmas);
   }
@@ -82,8 +84,8 @@ class GTSAM_EXPORT NonlinearConstraint : public NoiseModelFactor {
   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));
+    ar& boost::serialization::make_nvp(
+        "NonlinearConstraint", boost::serialization::base_object<Base>(*this));
   }
 #endif
 };

gtsam/constrained/NonlinearEqualityConstraint-inl.h

@@ -21,20 +21,19 @@
 
 namespace gtsam {
 
-/* ********************************************************************************************* */
+/* ************************************************************************* */
 template <typename T>
-ExpressionEqualityConstraint<T>::ExpressionEqualityConstraint(const Expression<T>& expression,
-                                                              const T& rhs,
-                                                              const Vector& sigmas)
+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 {
+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);
@@ -45,10 +44,12 @@ Vector ExpressionEqualityConstraint<T>::unwhitenedError(const Values& x,
   }
 }
 
-/* ********************************************************************************************* */
+/* ************************************************************************* */
 template <typename T>
-NoiseModelFactor::shared_ptr ExpressionEqualityConstraint<T>::penaltyFactor(const double mu) const {
-  return std::make_shared<ExpressionFactor<T>>(penaltyNoise(mu), rhs_, expression_);
+NoiseModelFactor::shared_ptr ExpressionEqualityConstraint<T>::penaltyFactor(
+    const double mu) const {
+  return std::make_shared<ExpressionFactor<T>>(penaltyNoise(mu), rhs_,
+                                               expression_);
 }
 
 }  // namespace gtsam

gtsam/constrained/NonlinearEqualityConstraint.cpp

@@ -20,21 +20,25 @@
 
 namespace gtsam {
 
-/* ********************************************************************************************* */
-ZeroCostConstraint::ZeroCostConstraint(const NoiseModelFactor::shared_ptr& factor)
-    : Base(constrainedNoise(factor->noiseModel()->sigmas()), factor->keys()), factor_(factor) {}
+/* ************************************************************************* */
+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 {
+/* ************************************************************************* */
+Vector ZeroCostConstraint::unwhitenedError(const Values& x,
+                                           OptionalMatrixVecType H) const {
   return factor_->unwhitenedError(x, H);
 }
 
-/* ********************************************************************************************* */
-NoiseModelFactor::shared_ptr ZeroCostConstraint::penaltyFactor(const double mu) const {
+/* ************************************************************************* */
+NoiseModelFactor::shared_ptr ZeroCostConstraint::penaltyFactor(
+    const double mu) const {
   return factor_->cloneWithNewNoiseModel(penaltyNoise(mu));
 }
 
-/* ********************************************************************************************* */
+/* ************************************************************************* */
 NonlinearEqualityConstraints NonlinearEqualityConstraints::FromCostGraph(
     const NonlinearFactorGraph& graph) {
   NonlinearEqualityConstraints constraints;
@@ -45,7 +49,7 @@ NonlinearEqualityConstraints NonlinearEqualityConstraints::FromCostGraph(
   return constraints;
 }
 
-/* ********************************************************************************************* */
+/* ************************************************************************* */
 size_t NonlinearEqualityConstraints::dim() const {
   size_t dimension = 0;
   for (const auto& constraint : *this) {
@@ -54,26 +58,29 @@ size_t NonlinearEqualityConstraints::dim() const {
   return dimension;
 }
 
-/* ********************************************************************************************* */
-Vector NonlinearEqualityConstraints::violationVector(const Values& values, bool whiten) const {
+/* ************************************************************************* */
+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.segment(start_idx, dim) =
-        whiten ? constraint->whitenedError(values) : constraint->unwhitenedError(values);
+        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 NonlinearEqualityConstraints::penaltyGraph(
+    const double mu) const {
   NonlinearFactorGraph graph;
   for (const auto& constraint : *this) {
     graph.add(constraint->penaltyFactor(mu));