Implement constrained optimization and improve L-BFGS

This commit introduces support for constrained optimization problems
and improves the L-BFGS algorithm.

All functions will receive the position to evaluate and return an entire
state (x, f(x), grad*, hessian*) instead of treating them separately.
This simplifies the interface, while template parameters take care of
treating functions according their differentiability.

To support constrained problems, a new solver is implemented which can
wrap all existing solvers to solver the unconstrained
one via penalty method and augmented Lagrangian.

It supports now:
  min  f(x)
  s.t. c1(x) >= 0
       c2(x)  = 0

Several changes are made to L-BFGS for more robust numerical handling of
edge-cases.

Author: PatWie

BUILD

@@ -4,5 +4,6 @@ load("//:generator.bzl", "build_example", "build_test")
 
 
 build_example("simple")
+build_example("constrained_simple")
 build_test("verify")
 

README.md

@@ -1,43 +1,75 @@
 # CppNumericalSolvers (A header-only C++17 optimization library)
 
-CppNumericalSolvers stands as a robust and efficient header-only C++17
-optimization library, meticulously crafted to address numerical optimization
-challenges. This library offers a suite of powerful solvers for optimization
-problems, placing a strong emphasis on simplicity, adherence to modern C++
-standards, and seamless integration into projects.
+CppNumericalSolvers is a header-only C++17 optimization library providing a
+suite of solvers for both unconstrained and constrained optimization problems.
+The library is designed for efficiency, modern C++ compliance, and easy
+integration into existing projects. It is distributed under a permissive
+license, making it suitable for commercial use.
 
 ### Example Usage: Minimizing the Rosenbrock Function
 
-Let's delve into a straightforward example that illustrates the ease of
-utilizing CppNumericalSolvers for numerical optimization. In this instance, we
-will showcase the minimization of the classic Rosenbrock function using the
-BFGS solver.
+Let minimize of the classic Rosenbrock function using the BFGS solver.
 
 ```cpp
-using FunctionXd = cppoptlib::function::Function<double>;
+/**
+ * @brief Alias for a 2D function with first-order differentiability in cppoptlib.
+ *
+ * This defines a function template that supports differentiation, specialized for
+ * a 2-dimensional input vector.
+ */
+using Functiond2_dx = cppoptlib::function::Function<
+    double, 2, cppoptlib::function::Differentiability::First>;
 
 /**
- * @brief Definition of the Rosenbrock function for optimization.
+ * @brief Implementation of the Rosenbrock function with gradient computation.
+ *
+ * This class represents the Rosenbrock function:
+ *
+ *     f(x) = (1 - x₁)² + 100 * (x₂ - x₁²)²
  *
- * This class represents the Rosenbrock function, a classic optimization problem.
+ * It includes both function evaluation and its gradient for optimization algorithms.
+ * The function has a global minimum at (x₁, x₂) = (1, 1), where f(x) = 0.
+ *
+ * @tparam T The scalar type (e.g., double or float).
  */
-class Rosenbrock : public FunctionXd {
-  public:
-    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-
-    /**
-     * @brief Computes the value of the Rosenbrock function at a given point.
-     *
-     * @param x The input vector.
-     * @return The value of the Rosenbrock function at the given point.
-     */
-    double operator()(const Eigen::VectorXd &x) const {
-        const double t1 = (1 - x[0]);
-        const double t2 = (x[1] - x[0] * x[0]);
-        return   t1 * t1 + 100 * t2 * t2;
-    }
-
-    // Gradient and Hessian implementation can be omitted.
+class RosenbrockGradient : public Functiond2_dx {
+public:
+  /// Eigen macro for proper memory alignment.
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+  // Import necessary typedefs from the base class.
+  using typename FunctionX2_dx::state_t;
+  using typename FunctionX2_dx::scalar_t;
+  using typename FunctionX2_dx::vector_t;
+
+  /**
+   * @brief Computes the Rosenbrock function value and its gradient at a given point.
+   *
+   * @param x A 2D Eigen vector representing the input point.
+   * @return A state_t object containing the function value, input vector, and gradient.
+   */
+  state_t operator()(const vector_t &x) const override {
+    state_t state;
+
+    // Compute function value: f(x) = (1 - x₁)² + 100 * (x₂ - x₁²)²
+    const scalar_t t1 = (1 - x[0]);             // First term: (1 - x₁)
+    const scalar_t t2 = (x[1] - x[0] * x[0]);   // Second term: (x₂ - x₁²)
+
+    state.value = t1 * t1 + 100 * t2 * t2;
+    state.x = x;  // Store the input vector for reference.
+
+    // Initialize gradient vector (∇f)
+    state.gradient = vector_t::Zero(2);
+
+    // Compute partial derivatives:
+    // ∂f/∂x₁ = -2(1 - x₁) + 200(x₂ - x₁²)(-2x₁)
+    state.gradient[0] = -2 * t1 + 200 * t2 * (-2 * x[0]);
+
+    // ∂f/∂x₂ = 200(x₂ - x₁²)
+    state.gradient[1] = 200 * t2;
+
+    return state;
+  }
 };
 
 int main(int argc, char const *argv[]) {
@@ -47,40 +79,35 @@ int main(int argc, char const *argv[]) {
     // Initial guess for the solution.
     Eigen::VectorXd x(2);
     x << -1, 2;
-    std::cout << "Initial point: " << x << std::endl;
-    std::cout << "Function value at initial point: " << f(x) << std::endl;
-
-    // Check the correctness of the gradient and Hessian (against numerical implementation).
-    std::cout << "Is Gradient correctly implemented? " << cppoptlib::utils::IsGradientCorrect(f, x) << std::endl;
-    std::cout << "Is Hessian correctly implemented? " << cppoptlib::utils::IsHessianCorrect(f, x) << std::endl;
+    std::cout << "Initial point: " << x.transpose() << std::endl;
 
     // Evaluate
-    auto state = f.Eval(x);
+    auto state = f(x);
+    std::cout << "Function value at initial point: " << state.value << std::endl;
     std::cout << "Gradient at initial point: " << state.gradient << std::endl;
-    if (state.hessian) {
-        std::cout << "Hessian at initial point: " << *(state.hessian) << std::endl;
-    }
 
     // Minimize the Rosenbrock function using the BFGS solver.
     using Solver = cppoptlib::solver::Bfgs<Rosenbrock>;
     Solver solver;
-    auto [solution, solver_state] = solver.Minimize(f, x);
+    auto [solution, solver_progress] = solver.Minimize(f, x);
 
     // Display the results of the optimization.
     std::cout << "Optimal solution: " << solution.x.transpose() << std::endl;
     std::cout << "Optimal function value: " << solution.value << std::endl;
-    std::cout << "Number of iterations: " << solver_state.num_iterations << std::endl;
-    std::cout << "Solver status: " << solver_state.status << std::endl;
+    std::cout << "Number of iterations: " << solver_progress.num_iterations << std::endl;
+    std::cout << "Solver status: " << solver_progress.status << std::endl;
 
     return 0;
 }
 ```
 
-This example demonstrates the usage of the BFGS solver to minimize the
-Rosenbrock function. You can easily adapt this code for your specific
-optimization problem by defining your objective function and selecting an
-appropriate solver from CppNumericalSolvers. Dive into the implementation for
-more details on available solvers and advanced usage.
+You can easily adapt this code for your specific optimization problem by
+defining your objective function and selecting an appropriate solver from
+CppNumericalSolvers. Dive into the implementation for more details on available
+solvers and advanced usage.
+
+
+See the examples for a constrained problem.
 
 ### References
 

include/cppoptlib/constrained_function.h

@@ -0,0 +1,208 @@
+// Copyright 2025 Amazon.com, Inc. or its affiliates. All Rights Reserved.
+#ifndef INCLUDE_CPPOPTLIB_CONSTRAINED_FUNCTION_H_
+#define INCLUDE_CPPOPTLIB_CONSTRAINED_FUNCTION_H_
+
+#include <Eigen/Core>
+#include <array>
+#include <functional>
+#include <string>
+#include <utility>
+
+#include "function.h"
+namespace cppoptlib::function {
+
+template <class function_t, std::size_t NumConstraints>
+struct ConstrainedFunction;
+
+template <class cfunction_t>
+struct ConstrainedState : public State<cfunction_t> {
+  static_assert(cfunction_t::NumConstraints > -1,
+                "need a constrained function");
+
+  using state_t = ConstrainedState<cfunction_t>;
+
+  typename cfunction_t::scalar_t value = 0;
+  typename cfunction_t::vector_t x;
+  typename cfunction_t::vector_t gradient;
+
+  std::array<typename cfunction_t::scalar_t, cfunction_t::NumConstraints>
+      lagrange_multipliers;
+  std::array<typename cfunction_t::scalar_t, cfunction_t::NumConstraints>
+      violations;
+  typename cfunction_t::scalar_t penalty = 0;
+
+  ConstrainedState() {
+    lagrange_multipliers.fill(typename cfunction_t::scalar_t(0));
+    violations.fill(typename cfunction_t::scalar_t(0));
+    penalty = typename cfunction_t::scalar_t(10);
+  }
+
+  ConstrainedState(const state_t &rhs) { CopyState(rhs); }  // nolint
+
+  ConstrainedState operator=(const state_t &rhs) {
+    CopyState(rhs);
+    return *this;
+  }
+
+  void CopyState(const state_t &rhs) {
+    value = rhs.value;
+    x = rhs.x.eval();
+    gradient = rhs.gradient.eval();
+    penalty = rhs.penalty;
+    lagrange_multipliers = rhs.lagrange_multipliers;
+    violations = rhs.violations;
+  }
+
+  State<typename cfunction_t::unconstrained_function_t> AsUnconstrained()
+      const {
+    State<typename cfunction_t::unconstrained_function_t> state;
+    state.value = value;
+    state.x = x.eval();
+    state.gradient = gradient.eval();
+    return state;
+  }
+};
+
+template <class cfunction_t>
+class UnconstrainedFunctionAdapter
+    : public cfunction_t::unconstrained_function_t {
+ public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+  UnconstrainedFunctionAdapter(cfunction_t original,
+                               typename cfunction_t::state_t state)
+      : original(original), state(state) {}
+
+  typename cfunction_t::unconstrained_function_t::state_t operator()(
+      const typename cfunction_t::types::vector_t &x) const override {
+    const typename cfunction_t::state_t inner =
+        original(x, state.lagrange_multipliers, state.penalty);
+    typename cfunction_t::unconstrained_function_t::state_t outer;
+    outer.value = inner.value;
+    outer.x = inner.x;
+    outer.gradient = inner.gradient;
+    return outer;
+  }
+
+ private:
+  cfunction_t original;
+  typename cfunction_t::state_t state;
+};
+
+template <class function_t, std::size_t TNumConstraints>
+struct ConstrainedFunction {
+  static constexpr int NumConstraints = TNumConstraints;
+
+  using types = typename function_t::types;
+  using scalar_t = typename function_t::types::scalar_t;
+  using vector_t = typename function_t::types::vector_t;
+  using matrix_t = typename function_t::types::matrix_t;
+  using unconstrained_function_t = function_t;
+
+ public:
+  using state_t =
+      ConstrainedState<ConstrainedFunction<function_t, NumConstraints>>;
+  static constexpr Differentiability diff_level = function_t::diff_level;
+  ConstrainedFunction(
+      const function_t *objective,
+      const std::array<const function_t *, TNumConstraints> &constraints)
+      : objective_(objective), constraints_(constraints) {}
+  state_t operator()(const typename types::vector_t &x,
+                     std::array<scalar_t, TNumConstraints> lagrange_multipliers,
+                     scalar_t penalty) const {
+    const typename function_t::state_t objective_state =
+        objective_->operator()(x);
+
+    state_t constrained_state;
+    constrained_state.x = objective_state.x;
+    constrained_state.value = objective_state.value;
+    constrained_state.gradient = objective_state.gradient;
+
+    // Sum augmented penalties for hard constraints.
+    for (std::size_t i = 0; i < TNumConstraints; ++i) {
+      const typename function_t::state_t constraint_state =
+          constraints_[i]->operator()(x);
+      const scalar_t cost = constraint_state.value;
+      const scalar_t violation = cost;
+
+      const scalar_t lambda = lagrange_multipliers[i];
+      const scalar_t aug_cost =
+          violation + lambda * violation +
+          static_cast<scalar_t>(0.5) * penalty * violation * violation;
+      constrained_state.value += aug_cost;
+      // Augmented gradient (only active if the constraint is violated).
+      const scalar_t a = scalar_t(1) + lambda + penalty * violation;
+      const typename types::vector_t scaled_local_grad =
+          a * constraint_state.gradient;
+      typename types::vector_t aug_grad = (cost > scalar_t(0))
+                                              ? scaled_local_grad
+                                              : types::vector_t::Zero(x.size());
+      constrained_state.gradient += aug_grad;
+      constrained_state.violations[i] = violation;
+    }
+
+    return constrained_state;
+  }
+
+  const function_t *objective_;
+  std::array<const function_t *, TNumConstraints> constraints_;
+};
+
+template <typename function_t>
+class ZeroConstraint : public function_t {
+ public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+  // Construct with the original constraint function.
+  explicit ZeroConstraint() {}
+
+  typename function_t::state_t operator()(
+      const typename function_t::vector_t &x) const override {
+    // Evaluate the original constraint: c(x)
+    typename function_t::state_t state = constraint_(x);
+    // Save the original constraint value.
+    typename function_t::scalar_t c_val = state.value;
+    // Transform to squared penalty: f(x) = c(x)^2, with gradient 2*c(x)*c'(x).
+    state.value = c_val * c_val;
+    state.gradient = 2 * c_val * state.gradient;
+    return state;
+  }
+
+ private:
+  function_t constraint_;
+};
+
+template <typename function_t>
+class NonNegativeConstraint : public function_t {
+ public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+  // Construct with the original constraint function.
+  explicit NonNegativeConstraint() {}
+
+  typename function_t::state_t operator()(
+      const typename function_t::vector_t &x) const override {
+    // Evaluate the original constraint: c(x)
+    typename function_t::state_t state = constraint_(x);
+    // For inequality constraints, we only penalize when c(x) < 0.
+    if (state.value >= 1e-7) {
+      // Constraint satisfied; no penalty.
+      state.value = 0;
+      state.gradient.setZero();
+    } else {
+      // Constraint violated; penalty = ( - c(x) )^2.
+      typename function_t::scalar_t violation = -state.value;
+      state.value = violation * violation;
+      // Chain rule: gradient = 2*violation * (-c'(x))
+      state.gradient = 2 * violation * (-state.gradient);
+    }
+    return state;
+  }
+
+ private:
+  function_t constraint_;
+};
+
+}  // namespace cppoptlib::function
+
+#endif  //  INCLUDE_CPPOPTLIB_CONSTRAINED_FUNCTION_H_