Add regularization and number of backtracks to diagnostics

SleipnirGroup · Jan 7, 2025 · 3262d5a · 3262d5a
1 parent 5af8519
commit 3262d5a
Show file tree

Hide file tree

Showing 5 changed files with 103 additions and 33 deletions.
diff --git a/src/optimization/RegularizedLDLT.hpp b/src/optimization/RegularizedLDLT.hpp
@@ -115,7 +115,7 @@ class RegularizedLDLT {
   }
 
   /**
-   * Solve the system of equations using a regularized LDLT factorization.
+   * Solves the system of equations using a regularized LDLT factorization.
    *
    * @param rhs Right-hand side of the system.
    */
@@ -124,6 +124,11 @@ class RegularizedLDLT {
     return m_solver.solve(rhs);
   }
 
+  /**
+   * Returns the Hessian regularization factor.
+   */
+  double HessianRegularization() const { return m_δOld; }
+
  private:
   Solver m_solver;
 

diff --git a/src/optimization/solver/InteriorPoint.cpp b/src/optimization/solver/InteriorPoint.cpp
@@ -24,6 +24,7 @@
 #include "sleipnir/util/Print.hpp"
 #include "sleipnir/util/Spy.hpp"
 #include "sleipnir/util/small_vector.hpp"
+#include "util/PrintIterationDiagnostics.hpp"
 #include "util/ScopeExit.hpp"
 #include "util/ToMilliseconds.hpp"
 
@@ -786,19 +787,12 @@ void InteriorPoint(std::span<Variable> decisionVariables,
 
     const auto innerIterEndTime = std::chrono::steady_clock::now();
 
-    // Diagnostics for current iteration
     if (config.diagnostics) {
-      if (iterations % 20 == 0) {
-        sleipnir::println("{:^4}   {:^9}  {:^13}  {:^13}  {:^13}", "iter",
-                          "time (ms)", "error", "cost", "infeasibility");
-        sleipnir::println("{:=^61}", "");
-      }
-
-      sleipnir::println("{:4}{}  {:9.3f}  {:13e}  {:13e}  {:13e}", iterations,
-                        feasibilityRestoration ? "r" : " ",
-                        ToMilliseconds(innerIterEndTime - innerIterStartTime),
-                        E_0, f.Value(),
-                        c_e.lpNorm<1>() + (c_i - s).lpNorm<1>());
+      PrintIterationDiagnostics(iterations, feasibilityRestoration,
+                                innerIterEndTime - innerIterStartTime, E_0,
+                                f.Value(),
+                                c_e.lpNorm<1>() + (c_i - s).lpNorm<1>(),
+                                solver.HessianRegularization(), α);
     }
 
     ++iterations;

diff --git a/src/optimization/solver/Newton.cpp b/src/optimization/solver/Newton.cpp
@@ -19,6 +19,7 @@
 #include "sleipnir/optimization/SolverExitCondition.hpp"
 #include "sleipnir/util/Print.hpp"
 #include "sleipnir/util/Spy.hpp"
+#include "util/PrintIterationDiagnostics.hpp"
 #include "util/ScopeExit.hpp"
 #include "util/ToMilliseconds.hpp"
 
@@ -250,17 +251,10 @@ void Newton(std::span<Variable> decisionVariables, Variable& f,
 
     const auto innerIterEndTime = std::chrono::steady_clock::now();
 
-    // Diagnostics for current iteration
     if (config.diagnostics) {
-      if (iterations % 20 == 0) {
-        sleipnir::println("{:^4}   {:^9}  {:^13}  {:^13}  {:^13}", "iter",
-                          "time (ms)", "error", "cost", "infeasibility");
-        sleipnir::println("{:=^61}", "");
-      }
-
-      sleipnir::println("{:4}   {:9.3f}  {:13e}  {:13e}  {:13e}", iterations,
-                        ToMilliseconds(innerIterEndTime - innerIterStartTime),
-                        E_0, f.Value(), 0.0);
+      PrintIterationDiagnostics(
+          iterations, false, innerIterEndTime - innerIterStartTime, E_0,
+          f.Value(), 0.0, solver.HessianRegularization(), α);
     }
 
     ++iterations;

diff --git a/src/optimization/solver/SQP.cpp b/src/optimization/solver/SQP.cpp
@@ -23,6 +23,7 @@
 #include "sleipnir/util/Print.hpp"
 #include "sleipnir/util/Spy.hpp"
 #include "sleipnir/util/small_vector.hpp"
+#include "util/PrintIterationDiagnostics.hpp"
 #include "util/ScopeExit.hpp"
 #include "util/ToMilliseconds.hpp"
 
@@ -520,17 +521,10 @@ void SQP(std::span<Variable> decisionVariables,
 
     const auto innerIterEndTime = std::chrono::steady_clock::now();
 
-    // Diagnostics for current iteration
     if (config.diagnostics) {
-      if (iterations % 20 == 0) {
-        sleipnir::println("{:^4}   {:^9}  {:^13}  {:^13}  {:^13}", "iter",
-                          "time (ms)", "error", "cost", "infeasibility");
-        sleipnir::println("{:=^61}", "");
-      }
-
-      sleipnir::println("{:4}   {:9.3f}  {:13e}  {:13e}  {:13e}", iterations,
-                        ToMilliseconds(innerIterEndTime - innerIterStartTime),
-                        E_0, f.Value(), c_e.lpNorm<1>());
+      PrintIterationDiagnostics(
+          iterations, false, innerIterEndTime - innerIterStartTime, E_0,
+          f.Value(), c_e.lpNorm<1>(), solver.HessianRegularization(), α);
     }
 
     ++iterations;

diff --git a/src/util/PrintIterationDiagnostics.hpp b/src/util/PrintIterationDiagnostics.hpp
@@ -0,0 +1,83 @@
+// Copyright (c) Sleipnir contributors
+
+#pragma once
+
+#include <chrono>
+#include <cmath>
+#include <ranges>
+#include <string>
+
+#include "sleipnir/util/Print.hpp"
+#include "sleipnir/util/small_vector.hpp"
+#include "util/ToMilliseconds.hpp"
+
+namespace sleipnir {
+
+/**
+ * Prints diagnostics for the current iteration.
+ *
+ * @param iterations Number of iterations.
+ * @param feasibilityRestoration Whether solver is in feasibility restoration
+ *     mode.
+ * @param time The iteration duration.
+ * @param error The error.
+ * @param cost The cost.
+ * @param infeasibility The infeasibility.
+ * @param δ The Hessian regularization factor.
+ * @param α The step size.
+ */
+template <typename Rep, typename Period = std::ratio<1>>
+void PrintIterationDiagnostics(int iterations, bool feasibilityRestoration,
+                               const std::chrono::duration<Rep, Period>& time,
+                               double error, double cost, double infeasibility,
+                               double δ, double α) {
+  if (iterations % 20 == 0) {
+    sleipnir::println("{:^4}   {:^9}  {:^13}  {:^13}  {:^13}  {:^4}  {:^10}",
+                      "iter", "time (ms)", "error", "cost", "infeasibility",
+                      "reg", "backtracks");
+    sleipnir::println("{:=^79}", "");
+  }
+
+  sleipnir::print("{:4}{}  {:9.3f}  {:13e}  {:13e}  {:13e}  ", iterations,
+                  feasibilityRestoration ? "r" : " ", ToMilliseconds(time),
+                  error, cost, infeasibility);
+
+  // Print regularization
+  if (δ == 0.0) {
+    sleipnir::print(" 0  ");
+  } else {
+    int exponent = std::log10(δ);
+
+    if (exponent == 0) {
+      sleipnir::print(" 1  ");
+    } else if (exponent == 1) {
+      sleipnir::print("10  ");
+    } else {
+      // Gather regularization exponent digits
+      int n = std::abs(exponent);
+      small_vector<int> digits;
+      do {
+        digits.emplace_back(n % 10);
+        n /= 10;
+      } while (n > 0);
+
+      std::string reg = "10";
+
+      // Append regularization exponent
+      if (exponent < 0) {
+        reg += "⁻";
+      }
+      constexpr const char* strs[] = {"⁰", "¹", "²", "³", "⁴",
+                                      "⁵", "⁶", "⁷", "⁸", "⁹"};
+      for (const auto& digit : std::ranges::views::reverse(digits)) {
+        reg += strs[digit];
+      }
+
+      sleipnir::print("{:<4}", reg);
+    }
+  }
+
+  sleipnir::println("  {:10d}", static_cast<int>(-std::log2(α)));
+}
+
+}  // namespace sleipnir