From a8c63925d8709857e82f763f5b2983cd38c0f608 Mon Sep 17 00:00:00 2001
From: Adrien Escande <adrien.escande@inria.fr>
Date: Mon, 5 Feb 2024 16:24:13 +0100
Subject: [PATCH] Adding more factorization options

---
 include/jrl-qp/SolverOptions.h | 26 ++++++++++++++------
 src/GoldfarbIdnaniSolver.cpp   | 45 ++++++++++++++++++++++++++++------
 tests/OptionsTest.cpp          | 22 ++++++++++++++---
 3 files changed, 73 insertions(+), 20 deletions(-)

diff --git a/include/jrl-qp/SolverOptions.h b/include/jrl-qp/SolverOptions.h
index 4dcd110..8563dd7 100644
--- a/include/jrl-qp/SolverOptions.h
+++ b/include/jrl-qp/SolverOptions.h
@@ -10,16 +10,26 @@
 
 namespace jrl::qp
 {
+/** Factorization of G */
+enum class GFactorization
+{
+  /** No factorization */
+  NONE,
+  /** G is given as the lower triangular matrix L such that G = L L^T */
+  L,
+  /** G is given as the lower triangular matrix invL such that G^-1 = invL^T invL */
+  L_INV,
+  /** G is given as the lower triangular matrix invL such that G^-1 = invL invL^T */
+  L_TINV
+};
+
 /** Options for the solvers*/
 struct JRLQP_DLLAPI SolverOptions
 {
   int maxIter_ = 500;
   double bigBnd_ = 1e100;
   bool warmStart_ = false;
-  /** If true, the input matrix G must contain the lower triangular matrix L
-   * such that G = L L^T. The upper part of the matrix is ignored.
-   */
-  bool factorizedG_ = false;
+  GFactorization gFactorization_ = GFactorization::NONE;
   std::uint32_t logFlags_ = 0;
   std::ostream * logStream_ = &defaultStream_;
 
@@ -80,13 +90,13 @@ struct JRLQP_DLLAPI SolverOptions
     return *this;
   }
 
-  bool factorizedG() const
+  GFactorization gFactorization() const
   {
-    return factorizedG_;
+    return gFactorization_;
   }
-  SolverOptions & factorizedG(bool fact)
+  SolverOptions & gFactorization(GFactorization fact)
   {
-    factorizedG_ = fact;
+    gFactorization_ = fact;
     return *this;
   }
 
diff --git a/src/GoldfarbIdnaniSolver.cpp b/src/GoldfarbIdnaniSolver.cpp
index c2dc8cd..c2221bb 100644
--- a/src/GoldfarbIdnaniSolver.cpp
+++ b/src/GoldfarbIdnaniSolver.cpp
@@ -55,20 +55,49 @@ TerminationStatus GoldfarbIdnaniSolver::solve(MatrixRef G,
 
 internal::InitTermination GoldfarbIdnaniSolver::init_()
 {
-  auto ret = (!options_.factorizedG_) ? Eigen::internal::llt_inplace<double, Eigen::Lower>::blocked(pb_.G) : -1;
-  auto L = pb_.G.template triangularView<Eigen::Lower>();
+  auto ret = (options_.gFactorization_ == GFactorization::NONE)
+                 ? Eigen::internal::llt_inplace<double, Eigen::Lower>::blocked(pb_.G)
+                 : -1;
 
   if(ret >= 0) return TerminationStatus::NON_POS_HESSIAN;
 
-  // J = L^-t
   auto J = work_J_.asMatrix(nbVar_, nbVar_, nbVar_);
-  J.setIdentity();
-  L.transpose().solveInPlace(J);
+  auto x = work_x_.asVector(nbVar_);
 
+  // J = L^-t
   // x = -G^-1 * a
-  auto x = work_x_.asVector(nbVar_);
-  x = L.solve(pb_.a);
-  L.transpose().solveInPlace(x); // possible [OPTIM]: J already contains L^-T
+  switch(options_.gFactorization_)
+  {
+    case GFactorization::NONE:
+      [[fallthrough]];
+    case GFactorization::L:
+    {
+      auto L = pb_.G.template triangularView<Eigen::Lower>();
+      J.setIdentity();
+      L.transpose().solveInPlace(J);
+      x = L.solve(pb_.a);
+      L.transpose().solveInPlace(x); // possible [OPTIM]: J already contains L^-T
+    }
+    break;
+    case GFactorization::L_INV:
+    {
+      auto invL = pb_.G.template triangularView<Eigen::Lower>();
+      J = invL.transpose();
+      x = invL * pb_.a;
+      x = J.template triangularView<Eigen::Upper>() * x;
+    }
+    break;
+    case GFactorization::L_TINV:
+    {
+      auto invLT = pb_.G.template triangularView<Eigen::Upper>();
+      J = invLT;
+      x = invLT.transpose() * pb_.a;
+      x = invLT * x;
+    }
+    break;
+    default:
+      assert(false);
+  }
   x = -x;
   f_ = 0.5 * pb_.a.dot(x);
 
diff --git a/tests/OptionsTest.cpp b/tests/OptionsTest.cpp
index 202c4ed..7e6b757 100644
--- a/tests/OptionsTest.cpp
+++ b/tests/OptionsTest.cpp
@@ -21,17 +21,31 @@ TEST_CASE("Test FactorizedG")
 
     auto llt = pb.G.llt();
     Eigen::MatrixXd L = llt.matrixL();
+    Eigen::MatrixXd invL = L.inverse();
+    Eigen::MatrixXd invLT = invL.transpose();
 
-    options.factorizedG(true);
+    options.gFactorization(jrl::qp::GFactorization::NONE);
+    solver.options(options);
+    solver.solve(pb.G, pb.a, pb.C, pb.l, pb.u, pb.xl, pb.xu);
+    Eigen::VectorXd x0 = solver.solution();
+
+    options.gFactorization(jrl::qp::GFactorization::L);
     solver.options(options);
     solver.solve(L, pb.a, pb.C, pb.l, pb.u, pb.xl, pb.xu);
     Eigen::VectorXd x1 = solver.solution();
 
-    options.factorizedG(false);
+    options.gFactorization(jrl::qp::GFactorization::L_INV);
     solver.options(options);
-    solver.solve(pb.G, pb.a, pb.C, pb.l, pb.u, pb.xl, pb.xu);
+    solver.solve(invL, pb.a, pb.C, pb.l, pb.u, pb.xl, pb.xu);
     Eigen::VectorXd x2 = solver.solution();
 
-    FAST_CHECK_UNARY(x1.isApprox(x2));
+    options.gFactorization(jrl::qp::GFactorization::L_TINV);
+    solver.options(options);
+    solver.solve(invLT, pb.a, pb.C, pb.l, pb.u, pb.xl, pb.xu);
+    Eigen::VectorXd x3 = solver.solution();
+
+    FAST_CHECK_UNARY(x1.isApprox(x0));
+    FAST_CHECK_UNARY(x2.isApprox(x0));
+    FAST_CHECK_UNARY(x3.isApprox(x0));
   }
 }
\ No newline at end of file