void LDLDecomposition<T>::getPseudoInverse(MatrixT& Ainv) const
{
Ainv.resize(LDL.n,LDL.n);
VectorT temp(LDL.n,Zero),y,x;
for(int i=0;i<LDL.n;i++) {
temp(i)=One;
LBackSub(temp,y);
for(int j=0;j<y.n;j++) {
if(!FuzzyZero(LDL(j,j),zeroTolerance))
y(j) = y(j)/LDL(j,j);
else
y(j) = 0.0;
}
LTBackSub(y,x);
//fill in a column
for(int j=0;j<LDL.n;j++)
Ainv(j,i)=x(j);
temp(i)=Zero;
}
T tol = Ainv.maxAbsElement()*Epsilon;
for(int i=0;i<LDL.n;i++)
for(int j=0;j<i;j++) {
if(!FuzzyEquals(Ainv(i,j),Ainv(j,i),tol))
LOG4CXX_INFO(KrisLibrary::logger(),Ainv);
Assert(FuzzyEquals(Ainv(i,j),Ainv(j,i),tol));
Ainv(i,j)=Ainv(j,i) = 0.5*(Ainv(i,j)+Ainv(j,i));
}
}
void LDLDecomposition<T>::DBackSub(const VectorT& b, VectorT& x) const
{
x.resize(b.n);
Assert(b.n==x.n);
for(int i=0;i<x.n;i++) {
if(!FuzzyZero(LDL(i,i),zeroTolerance))
x(i) = b(i)/LDL(i,i);
else {
if(!FuzzyZero(b(i),zeroTolerance))
cerr<<"LDLDecomposition::DBackSub(): Warning, zero on the diagonal"<<endl;
x(i) = 0;
}
}
}
void LDLDecomposition<T>::set(const MatrixT& A)
{
Assert(A.m == A.n);
LDL.resize(A.n,A.n);
int i,j,k;
T sum;
for(i=0;i<A.n;i++) {
sum = A(i,i);
for(k=0;k<i;k++) sum -= LDL(k,k)*Sqr(LDL(i,k));
LDL(i,i) = sum;
if(FuzzyZero(sum,zeroTolerance)) {
/*
if(verbose >= 1)
LOG4CXX_ERROR(KrisLibrary::logger(),"Warning: LDLt decomposition has a zero element on diagonal "<<i);
*/
}
for(j=i+1;j<A.n;j++) {
sum = A(i,j);
for(int k=0;k<i;k++) sum -= LDL(k,k)*LDL(i,k)*LDL(j,k);
if(LDL(i,i) == 0) {
if(!FuzzyZero(sum,zeroTolerance)) {
if(verbose >= 1) LOG4CXX_ERROR(KrisLibrary::logger(),"LDLDecomposition: Zero diagonal, what to do with sum "<<sum<<"?");
sum = 0;
}
}
else
sum /= LDL(i,i);
LDL(j,i) = LDL(i,j) = sum;
}
}
/*
MatrixT L,LD,LDLt;
VectorT D;
getL(L);
getD(D);
//LOG4CXX_INFO(KrisLibrary::logger(),"A: "); A.print();
//LOG4CXX_INFO(KrisLibrary::logger(),"L: "); L.print();
//LOG4CXX_INFO(KrisLibrary::logger(),"D: "); D.print();
LD = L;
for(int i=0;i<A.n;i++)
LD.scaleCol(i,LDL(i,i));
LDLt.mulTransposeB(LD,L);
//LOG4CXX_INFO(KrisLibrary::logger(),"LDLt: "); LDLt.print();
LDLt -= A;
LOG4CXX_ERROR(KrisLibrary::logger(),"Max error in LDL "<<LDLt.maxAbsElement());
*/
}
void LDLDecomposition<T>::mulD(const Vector& x,Vector& y) const
{
int n=LDL.n;
Assert(x.n == n);
y.resize(n);
for(int i=0;i<n;i++) y(i) = x(i)*LDL(i,i);
}
void LDLDecomposition<T>::set(const MatrixT& A)
{
Assert(A.m == A.n);
LDL.resize(A.n,A.n);
int i,j,k;
T sum;
for(i=0;i<A.n;i++) {
sum = A(i,i);
for(k=0;k<i;k++) sum -= LDL(k,k)*Sqr(LDL(i,k));
LDL(i,i) = sum;
if(FuzzyZero(sum,zeroTolerance)) {
cerr<<"Warning: LDLt decomposition has a zero element on diagonal "<<i<<endl;
}
for(j=i+1;j<A.n;j++) {
sum = A(i,j);
for(int k=0;k<i;k++) sum -= LDL(k,k)*LDL(i,k)*LDL(j,k);
if(LDL(i,i) == 0) {
if(sum != 0) {
cerr<<"Zero diagonal, what to do with sum "<<sum<<endl;
sum = 0;
}
}
else
sum /= LDL(i,i);
LDL(j,i) = LDL(i,j) = sum;
}
}
/*
MatrixT L,LD,LDLt;
VectorT D;
getL(L);
getD(D);
//cout<<"A: "; A.print();
//cout<<"L: "; L.print();
//cout<<"D: "; D.print();
LD = L;
for(int i=0;i<A.n;i++)
LD.scaleCol(i,LDL(i,i));
LDLt.mulTransposeB(LD,L);
//cout<<"LDLt: "; LDLt.print();
LDLt -= A;
cout<<"Max error in LDL "<<LDLt.maxAbsElement()<<endl;
*/
}
void LDLDecomposition<T>::update(const VectorT& _x)
{
VectorT x = _x; //make a copy, we'll change it
int n=LDL.n;
Assert(x.n == n);
T alpha=1;
for(int i=0;i<n;i++) {
T deltai = LDL(i,i);
T temp = alpha + Sqr(x(i))/deltai;
deltai = deltai*temp;
T gamma = x(i)/deltai;
deltai = deltai / alpha;
alpha = temp;
LDL(i,i) = deltai;
for(int k=i+1;k<n;k++) {
x(k) -= x(i)*LDL(k,i);
LDL(k,i) += gamma*x(k);
}
}
}
void LDLDecomposition<T>::mulLT(const Vector& x,Vector& y) const
{
int n=LDL.n;
Assert(x.n == n);
y.resize(n);
for(int i=0;i<n;i++) {
Real sum = x(i); //Lii = 1
for(int j=i+1;j<n;j++)
sum += LDL(j,i)*x(j);
y(i) = sum;
}
}
void LDLDecomposition<T>::getL(MatrixT& L) const
{
Assert(LDL.m == LDL.n);
L.resize(LDL.m,LDL.n);
for(int i=0;i<LDL.n;i++) {
L(i,i) = One;
for(int j=0;j<i;j++)
L(i,j) = LDL(i,j);
for(int j=i+1;j<LDL.n;j++)
L(i,j) = Zero;
}
}
bool LDLDecomposition<T>::DBackSub(const VectorT& b, VectorT& x) const
{
bool res=true;
x.resize(b.n);
Assert(b.n==x.n);
for(int i=0;i<x.n;i++) {
if(!FuzzyZero(LDL(i,i),zeroTolerance))
x(i) = b(i)/LDL(i,i);
else {
if(!FuzzyZero(b(i),zeroTolerance)) {
if(verbose >= 1)
LOG4CXX_ERROR(KrisLibrary::logger(),"LDLDecomposition::DBackSub(): Warning, zero on the diagonal, b("<<i<<")="<<b(i));
res = false;
x(i) = Sign(b(i))*Inf;
}
else
x(i) = 0;
}
}
return res;
}
bool LDLDecomposition<T>::downdate(const VectorT& _x)
{
VectorT x = _x; //make a copy, we'll change it
int n=LDL.n;
Assert(x.n == n);
T alpha=1;
for(int i=0;i<n;i++) {
T deltai = LDL(i,i);
T temp = alpha - Sqr(x(i))/deltai;
deltai = deltai*temp;
if(deltai == 0) return false;
T gamma = x(i)/deltai;
deltai = deltai / alpha;
alpha = temp;
LDL(i,i) = deltai;
for(int k=i+1;k<n;k++) {
x(k) -= x(i)*LDL(k,i);
LDL(k,i) -= gamma*x(k);
}
}
return true;
}
void LDL_solve(const DenseMatrix &A, const DenseMatrix &b, DenseMatrix &x)
{
DenseMatrix L = DenseMatrix(A.nrows(), A.ncols());
DenseMatrix D = DenseMatrix(A.nrows(), A.ncols());
DenseMatrix x_ = DenseMatrix(b.nrows(), b.ncols());
if (not is_symmetric_dense(A))
throw SymEngineException("Matrix must be symmetric");
LDL(A, L, D);
forward_substitution(L, b, x);
diagonal_solve(D, x, x_);
transpose_dense(L, D);
back_substitution(D, x_, x);
}
void LDL_solve(const DenseMatrix &A, const DenseMatrix &b, DenseMatrix &x)
{
DenseMatrix L = DenseMatrix(A.nrows(), A.ncols());
DenseMatrix D = DenseMatrix(A.nrows(), A.ncols());
DenseMatrix x_ = DenseMatrix(b.nrows(), 1);
if (!is_symmetric_dense(A))
throw std::runtime_error("Matrix must be symmetric");
LDL(A, L, D);
forward_substitution(L, b, x);
diagonal_solve(D, x, x_);
transpose_dense(L, D);
back_substitution(D, x_, x);
}
void LDL( DistMatrix<F,STAR,STAR>& A, bool conjugate )
{ LDL( A.Matrix(), conjugate ); }
void Initialize
( const AffineLPProblem<Matrix<Real>,Matrix<Real>>& problem,
AffineLPSolution<Matrix<Real>>& solution,
bool primalInit,
bool dualInit,
bool standardShift )
{
EL_DEBUG_CSE
const Int m = problem.A.Height();
const Int n = problem.A.Width();
const Int k = problem.G.Height();
if( primalInit )
{
if( solution.x.Height() != n || solution.x.Width() != 1 )
LogicError("x was of the wrong size");
if( solution.s.Height() != k || solution.s.Width() != 1 )
LogicError("s was of the wrong size");
}
if( dualInit )
{
if( solution.y.Height() != m || solution.y.Width() != 1 )
LogicError("y was of the wrong size");
if( solution.z.Height() != k || solution.z.Width() != 1 )
LogicError("z was of the wrong size");
}
if( primalInit && dualInit )
{
// TODO(poulson): Perform a consistency check
return;
}
// Form the KKT matrix
// ===================
Matrix<Real> J, ones;
Ones( ones, k, 1 );
KKT( problem.A, problem.G, ones, ones, J );
// Factor the KKT matrix
// =====================
Matrix<Real> dSub;
Permutation p;
LDL( J, dSub, p, false );
AffineLPResidual<Matrix<Real>> residual;
Matrix<Real> u, d;
Zeros( residual.dualConic, k, 1 );
if( !primalInit )
{
// Minimize || G x - h ||^2, s.t. A x = b by solving
//
// | 0 A^T G^T | | x | | 0 |
// | A 0 0 | | u | = | b |,
// | G 0 -I | | -s | | h |
//
// where 'u' is an unused dummy variable.
Zeros( residual.dualEquality, n, 1 );
residual.primalEquality = problem.b;
residual.primalEquality *= -1;
residual.primalConic = problem.h;
residual.primalConic *= -1;
KKTRHS
( residual.dualEquality,
residual.primalEquality,
residual.primalConic,
residual.dualConic,
ones, d );
ldl::SolveAfter( J, dSub, p, d, false );
ExpandCoreSolution( m, n, k, d, solution.x, u, solution.s );
solution.s *= -1;
}
if( !dualInit )
{
// Minimize || z ||^2, s.t. A^T y + G^T z + c = 0 by solving
//
// | 0 A^T G^T | | u | | -c |
// | A 0 0 | | y | = | 0 |,
// | G 0 -I | | z | | 0 |
//
// where 'u' is an unused dummy variable.
residual.dualEquality = problem.c;
Zeros( residual.primalEquality, m, 1 );
Zeros( residual.primalConic, k, 1 );
KKTRHS
( residual.dualEquality,
residual.primalEquality,
residual.primalConic,
residual.dualConic,
ones, d );
ldl::SolveAfter( J, dSub, p, d, false );
ExpandCoreSolution( m, n, k, d, u, solution.y, solution.z );
}
const Real epsilon = limits::Epsilon<Real>();
const Real sNorm = Nrm2( solution.s );
const Real zNorm = Nrm2( solution.z );
const Real gammaPrimal = Sqrt(epsilon)*Max(sNorm,Real(1));
const Real gammaDual = Sqrt(epsilon)*Max(zNorm,Real(1));
if( standardShift )
{
// alpha_p := min { alpha : s + alpha*e >= 0 }
// -------------------------------------------
const auto sMinPair = VectorMinLoc( solution.s );
const Real alphaPrimal = -sMinPair.value;
if( alphaPrimal >= Real(0) && primalInit )
RuntimeError("initialized s was non-positive");
// alpha_d := min { alpha : z + alpha*e >= 0 }
// -------------------------------------------
const auto zMinPair = VectorMinLoc( solution.z );
const Real alphaDual = -zMinPair.value;
if( alphaDual >= Real(0) && dualInit )
RuntimeError("initialized z was non-positive");
if( alphaPrimal >= -gammaPrimal )
Shift( solution.s, alphaPrimal+1 );
if( alphaDual >= -gammaDual )
Shift( solution.z, alphaDual+1 );
}
else
{
LowerClip( solution.s, gammaPrimal );
LowerClip( solution.z, gammaDual );
}
}
