Topic/prefactorization

.github/workflows/build.yml

@@ -26,7 +26,7 @@ jobs:
     strategy:
       fail-fast: false
       matrix:
-        os: [ubuntu-18.04, ubuntu-20.04, macos-latest, windows-latest]
+        os: [ubuntu-20.04, ubuntu-22.04, macos-latest, windows-latest]
         build-type: [Debug, RelWithDebInfo]
         compiler: [gcc, clang]
         exclude:

benchmarks/CMakeLists.txt

@@ -57,6 +57,7 @@ addBenchmark(BasicEigen)
 addBenchmark(Decomposition)
 addBenchmark(LinearSystemSolving)
 
+addBenchmark(SolverPreDecomposition problemAdaptors.cpp)
 addBenchmark(Solvers problemAdaptors.cpp)
 addBenchmark(SolversWarmStart problemAdaptors.cpp)
 addBenchmark(BoxAndSingleConstraintSolver)

benchmarks/LinearSystemSolving.cpp

@@ -93,7 +93,6 @@ BENCHMARK(BM_TriangularInverse_Transpose)->Apply(testSizes)->Unit(benchmark::kMi
       invA.col(i1).head(i1 - 1) = -invA(i1, i1) * A.row(i1).head(i1 - 1).transpose();
       invA.col(i + 1).head(i) = invA.topLeftCorner(i, i) * invA.col(i + 1).head(i);
     }
-    A.template triangularView<Eigen::Lower>().transpose().solveInPlace(invA);
   }
 }
 BENCHMARK(BM_TriangularInverse_Transpose_ByHand)->Apply(testSizes)->Unit(benchmark::kMicrosecond);

benchmarks/SolverPreDecomposition.cpp

@@ -0,0 +1,180 @@
+/* Copyright 2024 CNRS-AIST JRL, Inria */
+
+#include <array>
+#include <iostream>
+
+#include <benchmark/benchmark.h>
+
+#include <jrl-qp/GoldfarbIdnaniSolver.h>
+#include <jrl-qp/test/randomProblems.h>
+
+using namespace Eigen;
+using namespace jrl::qp;
+using namespace jrl::qp::test;
+
+template<int N, int EqPercentage>
+class ProblemFixture : public ::benchmark::Fixture
+{
+public:
+  void SetUp(const ::benchmark::State & st)
+  {
+    i = 0;
+
+    int n = static_cast<int>(st.range(0));
+    int neq = nEq(n);
+
+    for(int k = 0; k < N; ++k)
+    {
+      qpp[k] = QPProblem(randomProblem(ProblemCharacteristics(n, n, neq, 0)));
+      qpp[k].C.transposeInPlace();
+      G[k] = qpp[k].G;
+    }
+  }
+
+  void TearDown(const ::benchmark::State &) {}
+
+  int idx() const
+  {
+    int ret = i % N;
+    ++i;
+    return ret;
+  }
+
+  static int nEq(int nVar)
+  {
+    return static_cast<int>((nVar * EqPercentage) / 100);
+  }
+
+  QPProblem<> & getGIPb()
+  {
+    int i = idx();
+    qpp[i].G = G[i];
+    return qpp[i];
+  }
+
+private:
+  mutable int i = 0;
+  std::array<MatrixXd, N> G;
+  std::array<QPProblem<>, N> qpp;
+};
+
+using test0 = ProblemFixture<1000, 0>;
+using test50 = ProblemFixture<1000, 50>;
+
+BENCHMARK_DEFINE_F(test0, LLT_INTERNAL)(benchmark::State & st)
+{
+  for(auto _ : st)
+  {
+    auto & pb = getGIPb();
+    Eigen::internal::llt_inplace<double, Eigen::Lower>::blocked(pb.G);
+  }
+}
+BENCHMARK_REGISTER_F(test0, LLT_INTERNAL)->Unit(benchmark::kMicrosecond)->DenseRange(10, 100, 10);
+
+BENCHMARK_DEFINE_F(test0, LLT_FUNCTION)(benchmark::State & st)
+{
+  for(auto _ : st)
+  {
+    auto & pb = getGIPb();
+    auto llt = pb.G.llt();
+  }
+}
+BENCHMARK_REGISTER_F(test0, LLT_FUNCTION)->Unit(benchmark::kMicrosecond)->DenseRange(10, 100, 10);
+
+BENCHMARK_DEFINE_F(test0, LLT_OBJECT)(benchmark::State & st)
+{
+  for(auto _ : st)
+  {
+    auto & pb = getGIPb();
+    LLT<MatrixXd> llt(pb.G);
+  }
+}
+BENCHMARK_REGISTER_F(test0, LLT_OBJECT)->Unit(benchmark::kMicrosecond)->DenseRange(10, 100, 10);
+
+BENCHMARK_DEFINE_F(test0, LLT_PREALLOC)(benchmark::State & st)
+{
+  int n = static_cast<int>(st.range(0));
+  LLT<MatrixXd> llt(n);
+
+  for(auto _ : st)
+  {
+    auto & pb = getGIPb();
+    llt.compute(pb.G);
+  }
+}
+BENCHMARK_REGISTER_F(test0, LLT_PREALLOC)->Unit(benchmark::kMicrosecond)->DenseRange(10, 100, 10);
+
+#define BENCH_DECOMP(fixture)                                                                           \
+  BENCHMARK_DEFINE_F(fixture, NO_PRE_DECOMP)(benchmark::State & st)                                     \
+  {                                                                                                     \
+    int n = static_cast<int>(st.range(0));                                                              \
+    int neq = fixture::nEq(n);                                                                          \
+    GoldfarbIdnaniSolver solver(n, neq, false);                                                         \
+                                                                                                        \
+    for(auto _ : st)                                                                                    \
+    {                                                                                                   \
+      auto & pb = getGIPb();                                                                            \
+      solver.solve(pb.G, pb.a, pb.C, pb.l, pb.u, pb.xl, pb.xu);                                         \
+    }                                                                                                   \
+  }                                                                                                     \
+  BENCHMARK_REGISTER_F(fixture, NO_PRE_DECOMP)->Unit(benchmark::kMicrosecond)->DenseRange(10, 100, 10); \
+                                                                                                        \
+  BENCHMARK_DEFINE_F(fixture, DECOMP_INV)(benchmark::State & st)                                        \
+  {                                                                                                     \
+    int n = static_cast<int>(st.range(0));                                                              \
+    int neq = fixture::nEq(n);                                                                          \
+    GoldfarbIdnaniSolver solver(n, neq, false);                                                         \
+    MatrixXd J(n, n);                                                                                   \
+                                                                                                        \
+    for(auto _ : st)                                                                                    \
+    {                                                                                                   \
+      auto & pb = getGIPb();                                                                            \
+      Eigen::internal::llt_inplace<double, Eigen::Lower>::blocked(pb.G);                                \
+      auto L = pb.G.template triangularView<Eigen::Lower>();                                            \
+      J.setIdentity();                                                                                  \
+      L.solveInPlace(J);                                                                                \
+    }                                                                                                   \
+  }                                                                                                     \
+  BENCHMARK_REGISTER_F(fixture, DECOMP_INV)->Unit(benchmark::kMicrosecond)->DenseRange(10, 100, 10);    \
+                                                                                                        \
+  BENCHMARK_DEFINE_F(fixture, DECOMP_INVT)(benchmark::State & st)                                       \
+  {                                                                                                     \
+    int n = static_cast<int>(st.range(0));                                                              \
+    int neq = fixture::nEq(n);                                                                          \
+    GoldfarbIdnaniSolver solver(n, neq, false);                                                         \
+    MatrixXd J(n, n);                                                                                   \
+                                                                                                        \
+    for(auto _ : st)                                                                                    \
+    {                                                                                                   \
+      auto & pb = getGIPb();                                                                            \
+      Eigen::internal::llt_inplace<double, Eigen::Lower>::blocked(pb.G);                                \
+      auto L = pb.G.template triangularView<Eigen::Lower>();                                            \
+      J.setIdentity();                                                                                  \
+      L.solveInPlace(J);                                                                                \
+      J.transposeInPlace();                                                                             \
+    }                                                                                                   \
+  }                                                                                                     \
+  BENCHMARK_REGISTER_F(fixture, DECOMP_INVT)->Unit(benchmark::kMicrosecond)->DenseRange(10, 100, 10);   \
+                                                                                                        \
+  BENCHMARK_DEFINE_F(fixture, DECOMP_TINV)(benchmark::State & st)                                       \
+  {                                                                                                     \
+    int n = static_cast<int>(st.range(0));                                                              \
+    int neq = fixture::nEq(n);                                                                          \
+    GoldfarbIdnaniSolver solver(n, neq, false);                                                         \
+    MatrixXd J(n, n);                                                                                   \
+                                                                                                        \
+    for(auto _ : st)                                                                                    \
+    {                                                                                                   \
+      auto & pb = getGIPb();                                                                            \
+      Eigen::internal::llt_inplace<double, Eigen::Lower>::blocked(pb.G);                                \
+      auto L = pb.G.template triangularView<Eigen::Lower>();                                            \
+      J.setIdentity();                                                                                  \
+      L.transpose().solveInPlace(J);                                                                    \
+    }                                                                                                   \
+  }                                                                                                     \
+  BENCHMARK_REGISTER_F(fixture, DECOMP_TINV)->Unit(benchmark::kMicrosecond)->DenseRange(10, 100, 10);
+
+BENCH_DECOMP(test0)
+BENCH_DECOMP(test50)
+
+BENCHMARK_MAIN();

include/jrl-qp/GoldfarbIdnaniSolver.h

@@ -32,6 +32,12 @@ class JRLQP_DLLAPI GoldfarbIdnaniSolver : public DualSolver
                           const VectorConstRef & xl,
                           const VectorConstRef & xu);
 
+  /** This is used to set the precomputed R factor in the QR decomposition of the initially
+   * active constraints. Should be used when options.gFactorization is GFactorization::L_TINV_Q.
+   * Use only if you know what you are doing.
+   */
+  void setPrecomputedR(MatrixConstRef precompR);
+
 protected:
   /** Structure to gather the problem definition. */
   struct Problem
@@ -74,9 +80,17 @@ class JRLQP_DLLAPI GoldfarbIdnaniSolver : public DualSolver
 
   void addInitialConstraint(const internal::SelectedConstraint & sc);
 
+  internal::TerminationType processMatrixG();
+  internal::TerminationType processInitialActiveSetWithEqualityOnly();
+  internal::TerminationType initializeComputationData();
+  internal::TerminationType initializePrimalDualPoints();
+
   mutable internal::Workspace<> work_d_;
   internal::Workspace<> work_J_;
   internal::Workspace<> work_R_;
+  internal::Workspace<> work_tmp_;
+  internal::Workspace<> work_hCoeffs_; // for initial QR decomposition
+  internal::Workspace<> work_bact_;
   Problem pb_;
 };
 

include/jrl-qp/SolverOptions.h

@@ -10,12 +10,34 @@
 
 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,
+  /** G is given as a matrix J = L^-T Q, where G = L L^T and Q is the orthonormal
+  matrix appearing in the QR decomposition of the activated constraints (see other
+  options for details). */
+  L_TINV_Q
+};
+
 /** Options for the solvers*/
 struct JRLQP_DLLAPI SolverOptions
 {
   int maxIter_ = 500;
   double bigBnd_ = 1e100;
   bool warmStart_ = false;
+  bool equalityFirst_ = false; // True if all equality constraints are given first in the constraint matrix
+  bool RIsGiven_ = false; // True when the R factor in the decomposition of some initially active constraints is given.
+                          // If equalityFirst is true, these are the equality constraints. Ignored if gFactorization !=
+                          // L_TINV_Q or equalityFirst_ = false.
+  GFactorization gFactorization_ = GFactorization::NONE;
   std::uint32_t logFlags_ = 0;
   std::ostream * logStream_ = &defaultStream_;
 
@@ -76,6 +98,36 @@ struct JRLQP_DLLAPI SolverOptions
     return *this;
   }
 
+  bool equalityFirst() const
+  {
+    return equalityFirst_;
+  }
+  SolverOptions & equalityFirst(bool eqFirst)
+  {
+    equalityFirst_ = eqFirst;
+    return *this;
+  }
+
+  bool RIsGiven() const
+  {
+    return RIsGiven_;
+  }
+  SolverOptions & RIsGiven(bool Rgiven)
+  {
+    RIsGiven_ = Rgiven;
+    return *this;
+  }
+
+  GFactorization gFactorization() const
+  {
+    return gFactorization_;
+  }
+  SolverOptions & gFactorization(GFactorization fact)
+  {
+    gFactorization_ = fact;
+    return *this;
+  }
+
   std::ostream & logStream() const
   {
     return *logStream_;