Skip to content

Commit

Permalink
First version with precomputed QR of constraint matrix
Browse files Browse the repository at this point in the history
  • Loading branch information
@aescande
aescande committed Jun 13, 2024
1 parent 6ac2f40 commit 97b598c
Show file tree
Hide file tree
Showing 3 changed files with 140 additions and 54 deletions.
7 changes: 7 additions & 0 deletions include/jrl-qp/GoldfarbIdnaniSolver.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -32,6 +32,13 @@ 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
Expand Down
144 changes: 90 additions & 54 deletions src/GoldfarbIdnaniSolver.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -52,8 +52,24 @@ TerminationStatus GoldfarbIdnaniSolver::solve(MatrixRef G,
return DualSolver::solve();
}

void GoldfarbIdnaniSolver::setPrecomputedR(MatrixConstRef precompR)
{
assert(precompR.rows() == precompR.cols() || precompR.rows() == nbVar_);
auto R = work_R_.asMatrix(precompR.rows(), precompR.cols(), nbVar_);
// Set lower part of R to 0
JRLQP_DEBUG_ONLY(for(int i = 0; i < precompR.cols(); ++i) R.col(i).tail(nbVar_ - i - 1).setZero(););
}

internal::InitTermination GoldfarbIdnaniSolver::init_()
{
// Check options
if(options_.RIsGiven())
{
if(options_.gFactorization() != GFactorization::L_TINV_Q || !options_.equalityFirst())
JRLQP_LOG_COMMENT(log_, LogFlags::INPUT, "Incompatible options: RIsGiven with gFactorization or equalityFirst");
}


auto ret = (options_.gFactorization_ == GFactorization::NONE)
? Eigen::internal::llt_inplace<double, Eigen::Lower>::blocked(pb_.G)
: -1;
Expand Down Expand Up @@ -368,6 +384,11 @@ internal::TerminationType GoldfarbIdnaniSolver::processMatrixG()
J = invLT;
}
break;
case GFactorization::L_TINV_Q:
{
J = pb_.G;
}
break;
default:
assert(false);
}
Expand Down Expand Up @@ -398,65 +419,80 @@ internal::TerminationType GoldfarbIdnaniSolver::initializeComputationData()
int q = A_.nbActiveCstr();
auto N = work_R_.asMatrix(nbVar_, q, nbVar_);
auto b_act = work_bact_.asVector(q);
for(int i = 0; i < q; ++i)
{
int cstrIdx = A_[i];
switch(A_.activationStatus(cstrIdx))
{
case ActivationStatus::LOWER:
[[fallthrough]];
case ActivationStatus::EQUALITY:
N.col(i) = pb_.C.col(cstrIdx);
b_act[i] = pb_.bl(cstrIdx);
break;
case ActivationStatus::UPPER:
N.col(i) = -pb_.C.col(cstrIdx);
b_act[i] = -pb_.bu(cstrIdx);
break;
case ActivationStatus::LOWER_BOUND:
[[fallthrough]];
case ActivationStatus::FIXED:
N.col(i).setZero();
N.col(i)[cstrIdx - A_.nbCstr()] = 1;
b_act[i] = pb_.xl(cstrIdx - A_.nbCstr());
break;
case ActivationStatus::UPPER_BOUND:
N.col(i).setZero();
N.col(i)[cstrIdx - A_.nbCstr()] = -1;
b_act[i] = -pb_.xu(cstrIdx - A_.nbCstr());
break;
default:
break;
}
}

// J = L^-t
processMatrixG();

// B = L^-1 N
auto L = pb_.G.template triangularView<Eigen::Lower>();
L.solveInPlace(N); // [OPTIM] There are other possible ways to do this:
// - use solveInPlace while filling N
// - multiply N by J^T = L^-1 (this would require that multiplication by triangular matrix can be
// done in place with Eigen)

// QR in place
Eigen::Map<Eigen::VectorXd> hCoeffs(work_hCoeffs_.asVector(q).data(),
q); // Should be simply hCoeffs = work_hCoeffs_.asVector(q), but his does compile
// with householder_qr_inplace_blocked
WVector tmp = work_tmp_.asVector(nbVar_);
bool b = Eigen::internal::is_malloc_allowed();
Eigen::internal::set_is_malloc_allowed(true);
Eigen::internal::householder_qr_inplace_blocked<decltype(N), decltype(hCoeffs)>::run(N, hCoeffs, 48, tmp.data());

// J = J*Q
auto J = work_J_.asMatrix(nbVar_, nbVar_, nbVar_);
Eigen::HouseholderSequence Q(N, hCoeffs);
Q.applyThisOnTheRight(J, tmp);
Eigen::internal::set_is_malloc_allowed(b);

// Set lower part of R to 0
JRLQP_DEBUG_ONLY(for(int i = 0; i < q; ++i) N.col(i).tail(nbVar_ - i - 1).setZero(););
if(options_.RIsGiven() && options_.equalityFirst())
{
assert(options_.gFactorization() == GFactorization::L_TINV_Q);
for(int i = 0; i < q; ++i)
{
int cstrIdx = A_[i];
b_act[i] = pb_.bl(cstrIdx);
}
}
else
{
for(int i = 0; i < q; ++i)
{
int cstrIdx = A_[i];
// We have here the code for any kind of activation status while for now only
// equality constraints can be activated before reaching this part.
switch(A_.activationStatus(cstrIdx))
{
case ActivationStatus::LOWER:
[[fallthrough]];
case ActivationStatus::EQUALITY:
N.col(i) = pb_.C.col(cstrIdx);
b_act[i] = pb_.bl(cstrIdx);
break;
case ActivationStatus::UPPER:
N.col(i) = -pb_.C.col(cstrIdx);
b_act[i] = -pb_.bu(cstrIdx);
break;
case ActivationStatus::LOWER_BOUND:
[[fallthrough]];
case ActivationStatus::FIXED:
N.col(i).setZero();
N.col(i)[cstrIdx - A_.nbCstr()] = 1;
b_act[i] = pb_.xl(cstrIdx - A_.nbCstr());
break;
case ActivationStatus::UPPER_BOUND:
N.col(i).setZero();
N.col(i)[cstrIdx - A_.nbCstr()] = -1;
b_act[i] = -pb_.xu(cstrIdx - A_.nbCstr());
break;
default:
break;
}
}

// B = L^-1 N
auto L = pb_.G.template triangularView<Eigen::Lower>();
L.solveInPlace(N); // [OPTIM] There are other possible ways to do this:
// - use solveInPlace while filling N
// - multiply N by J^T = L^-1 (this would require that multiplication by triangular matrix can
// be done in place with Eigen)

// QR in place
Eigen::Map<Eigen::VectorXd> hCoeffs(work_hCoeffs_.asVector(q).data(),
q); // Should be simply hCoeffs = work_hCoeffs_.asVector(q), but his does
// compile with householder_qr_inplace_blocked
WVector tmp = work_tmp_.asVector(nbVar_);
bool b = Eigen::internal::is_malloc_allowed();
Eigen::internal::set_is_malloc_allowed(true);
Eigen::internal::householder_qr_inplace_blocked<decltype(N), decltype(hCoeffs)>::run(N, hCoeffs, 48, tmp.data());

// J = J*Q
Eigen::HouseholderSequence Q(N, hCoeffs);
Q.applyThisOnTheRight(J, tmp);
Eigen::internal::set_is_malloc_allowed(b);

// Set lower part of R to 0
JRLQP_DEBUG_ONLY(for(int i = 0; i < q; ++i) N.col(i).tail(nbVar_ - i - 1).setZero(););
}

JRLQP_LOG(log_, LogFlags::INIT | LogFlags::NO_ITER, N, J, b_act);

Expand Down
43 changes: 43 additions & 0 deletions tests/OptionsTest.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -101,4 +101,47 @@ TEST_CASE("Test EqualityFirst")
//FAST_CHECK_UNARY(x3.isApprox(x0));
//FAST_CHECK_UNARY(x4.isApprox(x0));
}
}

TEST_CASE("Precomputed R")
{
const int nbVar = 7;
const int neq = 4;
const int nIneq = 8;
jrl::qp::GoldfarbIdnaniSolver solver(nbVar, neq+nIneq, false);
jrl::qp::SolverOptions options;

for(int i = 0; i < 10; ++i)
{
auto pb = QPProblem(randomProblem(ProblemCharacteristics(nbVar, nbVar, neq, nIneq)));
pb.C.transposeInPlace();

for(int i = 0; i < neq; ++i)
{
REQUIRE_EQ(pb.l[i], pb.u[i]);
}

auto llt = pb.G.llt();

Eigen::MatrixXd J = Eigen::MatrixXd::Identity(nbVar, nbVar);
Eigen::VectorXd tmp(nbVar);
llt.matrixL().transpose().solveInPlace(J);
Eigen::MatrixXd B = J.template triangularView<Eigen::Lower>().transpose() * pb.C.leftCols(neq);
Eigen::HouseholderQR<Eigen::MatrixXd> qr(B);
qr.householderQ().applyThisOnTheRight(J, tmp);

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_TINV_Q);
options.RIsGiven(true);
solver.options(options);
solver.setPrecomputedR(Eigen::MatrixXd(qr.matrixQR().triangularView<Eigen::Upper>()));
solver.solve(J, pb.a, pb.C, pb.l, pb.u, pb.xl, pb.xu);
Eigen::VectorXd x1 = solver.solution();

FAST_CHECK_UNARY(x1.isApprox(x0));
}
}

0 comments on commit 97b598c

Please sign in to comment.