CTRSSolver.cpp File Reference

#include <stdio.h>
#include <memory.h>
#include "ObjectiveFunction.h"
#include "Matrix.h"
#include "IntPoly.h"
#include "tools.h"
#include "VectorChar.h"

Include dependency graph for CTRSSolver.cpp:

Go to the source code of this file.

Functions
void	simpleQPSolve (Matrix mH, Vector vG, Matrix mA, Vector vB, Vector vP, Vector vLambda, int *info)
void	restartSimpleQPSolve (Vector vBO, Vector vP)
Vector	L2NormMinimizer (Polynomial q, double delta, int *infoOut=NULL, int maxIter=1000, double *lambda1=NULL)
Vector	L2NormMinimizer (Polynomial q, Vector pointXk, double delta, int *infoOut=NULL, int maxIter=1000, double *lambda1=NULL)
Vector	L2NormMinimizer (Polynomial q, Vector pointXk, double delta, int *infoOut, int maxIter, double *lambda1, Vector minusG, Matrix H)
char	checkForTermination (Vector d, Vector Base, double rhoEnd)
void	initConstrainedStep (ObjectiveFunction *of)
Vector	ConstrainedL2NormMinimizer (InterPolynomial poly, int k, double delta, int *info, int iterMax, double *lambda1, Vector vOBase, ObjectiveFunction *of)
void	projectionIntoFeasibleSpace (Vector vFrom, Vector vBase, ObjectiveFunction *of)

Variables
Vector	FullLambda

Function Documentation

checkForTermination()

char checkForTermination	(	Vector	d,
		Vector	Base,
		double	rhoEnd
	)

Definition at line 56 of file CTRSSolver.cpp.

{return 0;}

ConstrainedL2NormMinimizer()

Vector ConstrainedL2NormMinimizer	(	InterPolynomial	poly,
		int	k,
		double	delta,
		int *	info,
		int	iterMax,
		double *	lambda1,
		Vector	vOBase,
		ObjectiveFunction *	of
	)

Definition at line 59 of file CTRSSolver.cpp.

{
    int dim=poly.dim();
    Matrix mH(dim,dim);
    Vector vG(dim);
    poly.gradientHessian(poly.NewtonPoints[k],vG,mH);

    if (of->isConstrained) 
        printf("Limited version! Ignoring constraints !\n");
    return L2NormMinimizer(poly, poly.NewtonPoints[k], delta, info, iterMax, lambda1, vG, mH);

//    return ConstrainedL2NormMinimizer(mH,vG,delta,info,iterMax,lambda1,vOBase+poly.NewtonPoints[k],of);
}

initConstrainedStep()

void initConstrainedStep

(

ObjectiveFunction *

of

)

Definition at line 57 of file CTRSSolver.cpp.

{ FullLambda.setSize(0); }

L2NormMinimizer() [1/3]

Vector L2NormMinimizer	(	Polynomial	q,
		double	delta,
		int *	infoOut = NULL,
		int	maxIter = 1000,
		double *	lambda1 = NULL
	)

Definition at line 307 of file UTRSSolver.cpp.

{
    return L2NormMinimizer(q, Vector::emptyVector, delta, infoOut, maxIter, lambda1);
}

L2NormMinimizer() [2/3]

Vector L2NormMinimizer	(	Polynomial	q,
		Vector	pointXk,
		double	delta,
		int *	infoOut = NULL,
		int	maxIter = 1000,
		double *	lambda1 = NULL
	)

Definition at line 297 of file UTRSSolver.cpp.

{
    int n=q.dim();
    Matrix H(n,n);
    Vector vG(n);
    q.gradientHessian(pointXk,vG,H);
    return L2NormMinimizer(q,pointXk,delta,infoOut,maxIter,lambda1,vG,H);
}

L2NormMinimizer() [3/3]

Vector L2NormMinimizer	(	Polynomial	q,
		Vector	pointXk,
		double	delta,
		int *	infoOut,
		int	maxIter,
		double *	lambda1,
		Vector	minusG,
		Matrix	H
	)

Definition at line 147 of file UTRSSolver.cpp.

{
// lambda1>0.0 if interior convergence

    const double theta=0.01;
//    const double kappaEasy=0.1, kappaHard=0.2;
    const double kappaEasy=0.01, kappaHard=0.02;

    double normG,lambda,lambdaCorrection, lambdaPlus, lambdaL, lambdaU, 
           uHu, alpha, normS;
    int info=0, n=minusG.sz();
    Matrix HLambda(n,n);
    MatrixTriangle L(n);
    Vector s(n), omega(n), u(n), sFinal;
    bool gIsNull, choleskyFactorAlreadyComputed=false;

//    printf("\nG= "); minusG.print();
//    printf("\nH=\n"); H.print();

    gIsNull=minusG.isNull();
    normG=minusG.euclidianNorm();
    lambda=normG/delta;
    minusG.multiply(-1.0);

    lambdaL=initLambdaL(normG,delta,H);
    lambdaU=initLambdaU(normG,delta,H);

    //    Special case: parl = paru.
    lambdaU= mmax(lambdaU,(1+kappaEasy)*lambdaL);

    lambda=mmax(lambda, lambdaL);
    lambda=mmin(lambda, lambdaU);

    while (maxIter--)
    {
        if (!choleskyFactorAlreadyComputed)
        {
            if (!H.cholesky(L, lambda, &lambdaCorrection))
            {
       //         lambdaL=mmax(mmax(lambdaL,lambda),lambdaCorrection);
                lambdaL=mmax(lambdaL,lambda+lambdaCorrection);
                lambda=mmax(sqrt(lambdaL*lambdaU), lambdaL+theta*(lambdaU-lambdaL));
                continue;
            }
        } else choleskyFactorAlreadyComputed=false;


        // cholesky factorization successful : solve Hlambda * s = -G
        s.copyFrom(minusG);
        L.solveInPlace(s);
        L.solveTransposInPlace(s);
        normS=s.euclidianNorm();

        // check for termination
#ifndef POWEL_TERMINATION
        if (condorAbs(normS-delta)<kappaEasy*delta) 
        { 
            s.multiply(delta/normS);
            info=1; 
            break; 
        }
#else
//      powell check !!!
        HLambda.copyFrom(H);
        HLambda.addUnityInPlace(lambda);
        double sHs=s.scalarProduct(HLambda.multiply(s));
        if (sqr(delta/normS-1)<kappaEasy*(1+lambda*delta*delta/sHs)) 
        {
            s.multiply(delta/normS);
            info=1; 
            break;
        }
#endif

        if (normS<delta)
        {
            // check for termination
            // interior convergence; maybe break;
            if (lambda==0) { info=1; break; }
            lambdaU=mmin(lambdaU,lambda); 
        } else lambdaL=mmax(lambdaL,lambda);

//        if (lambdaU-lambdaL<kappaEasy*(2-kappaEasy)*lambdaL) { info=3; break; };

        omega.copyFrom(s);
        L.solveInPlace(omega);
        lambdaPlus=lambda+(normS-delta)/delta*sqr(normS)/sqr(omega.euclidianNorm());
        lambdaPlus=mmax(lambdaPlus, lambdaL);
        lambdaPlus=mmin(lambdaPlus, lambdaU);

        if (normS<delta)
        {
            L.LINPACK(u);
#ifndef POWEL_TERMINATION
            HLambda.copyFrom(H);
            HLambda.addUnityInPlace(lambda);
#endif
            uHu=u.scalarProduct(HLambda.multiply(u));
            lambdaL=mmax(lambdaL,lambda-uHu);

            alpha=findAlpha(s,u,delta,q,pointXk,sFinal,minusG,H);
            // check for termination
#ifndef POWEL_TERMINATION
            if (sqr(alpha)*uHu<
                kappaHard*(s.scalarProduct(HLambda.multiply(s))   ))//  +lambda*sqr(delta)))
#else
            if (sqr(alpha)*uHu+sHs<
                kappaHard*(sHs+lambda*sqr(delta)))
#endif
            {
                s=sFinal; info=2; break;
            }
        }
        if ((normS>delta)&&(!gIsNull)) { lambda=lambdaPlus; continue; };

        if (H.cholesky(L, lambdaPlus, &lambdaCorrection)) 
        { 
            lambda=lambdaPlus; 
            choleskyFactorAlreadyComputed=true; 
            continue; 
        }

        lambdaL=mmax(lambdaL,lambdaPlus);
        // check lambdaL for interior convergence
//      if (lambdaL==0) return s;
        lambda=mmax(sqrt(lambdaL*lambdaU), lambdaL+theta*(lambdaU-lambdaL));
    }

    if (infoOut) *infoOut=info;
    if (lambda1)
    {
        if (lambda==0.0)
        {
            // calculate the value of the lowest eigenvalue of H
            // to check
            lambdaL=0; lambdaU=initLambdaU2(H);
            while (lambdaL<0.99*lambdaU)
            {
                lambda=0.5*(lambdaL+lambdaU);
                if (H.cholesky(L,-lambda)) lambdaL=lambda;
//                if (H.cholesky(L,-lambda,&lambdaCorrection)) lambdaL=lambda+lambdaCorrection;
                else lambdaU=lambda;
            }
            *lambda1=lambdaL;
        } else *lambda1=0.0;
    }
    return s;
}

projectionIntoFeasibleSpace()

void projectionIntoFeasibleSpace	(	Vector	vFrom,
		Vector	vBase,
		ObjectiveFunction *	of
	)

Definition at line 74 of file CTRSSolver.cpp.

{
    vBase.copyFrom(vFrom);
}

restartSimpleQPSolve()

void restartSimpleQPSolve	(	Vector	vBO,
		Vector	vP
	)

Definition at line 363 of file QPSolver.cpp.

{
    if (bQRFailed_QP) { vP.zero(); return; }
    int i,k=0, *ii=vi_QP, nc2=vi_QP.sz();
    double *lambda=vLastLambda_QP, *b=vBO;
    Vector vTmp(nc2);
    for (i=0; i<nc2; i++)
    {
        if (lambda[i]!=0.0)
        {
            b[k]=b[ii[i]];
            k++;
        }
    }
    vBO.setSize(k);
    vBO.permutIn(vTmp,viPermut_QP);
    mR_QP.solveInPlace(vTmp);
    mQ_QP.multiply(vP,vTmp);
}

simpleQPSolve()

void simpleQPSolve	(	Matrix	mH,
		Vector	vG,
		Matrix	mA,
		Vector	vB,
		Vector	vP,
		Vector	vLambda,
		int *	info
	)

Definition at line 105 of file QPSolver.cpp.

{
    const double tolRelFeasibility=1e-6;
//     int *info=NULL;
    int dim=mA.nColumn(), nc=mA.nLine(), ncr, i,j,k, lastJ=-2, *ii;
    MatrixTriangle M(dim);
    Matrix mAR, mZ(dim,dim-1), mHZ(dim,dim-1), mZHZ(dim-1,dim-1);
    Vector vTmp(mmax(nc,dim)), vYB(dim), vD(dim), vTmp2(dim), vTmp3(nc), vLast(dim), 
           vBR_QP, vLambdaScale(nc);
    VectorChar vLB(nc);
    double *lambda=vLambda, *br, *b=vB, violationMax, violationMax2,
           *rlambda,ax,al,dviolation, **a=mA, **ar=mAR, mymin, mymax, maxb, *llambda,
           **r,t, *scaleLambda=vLambdaScale;
    char finished=0, feasible=0, *lb=vLB;

    if (info) *info=0;

    // remove lines which are null
    k=0; vi_QP.setSize(nc); maxb=0.0;
    for (i=0; i<nc; i++)
    {
        mymin=INF; mymax=-INF;
        for (j=0; j<dim; j++)
        {
            mymin=mmin(mymin,a[i][j]);
            mymax=mmax(mymax,a[i][j]);
            if ((mymin!=mymax)||(mymin!=0.0)||(mymax!=0.0)) break;
        }
        if ((mymin!=mymax)||(mymin!=0.0)||(mymax!=0.0))
        {
            lambda[k]=lambda[i];
            maxb=mmax(maxb,condorAbs(b[i]));
            scaleLambda[k]=mA.euclidianNorm(i);
            vi_QP[k]=i;
            k++;
        }
    }
    nc=k; vi_QP.setSize(nc); ii=vi_QP; maxb=(1.0+maxb)*tolRelFeasibility;
    vLast.zero(); 
    
    for (i=0; i<nc; i++) if (lambda[i]!=0.0) lb[i]=2; else lb[i]=1;

    while (!finished)
    {
        finished=1;
        mAR.setSize(dim,dim); mAR.zero(); ar=mAR;
        vBR_QP.setSize(mmin(nc,dim)); br=vBR_QP;
        ncr=0;
        for (i=0; i<nc; i++)
            if (lambda[i]!=0.0)
            {
//                mAR.setLines(ncr,mA,ii[i],1);
                k=ii[i];
                t=scaleLambda[ncr];
                for (j=0; j<dim; j++) ar[ncr][j]=a[k][j]*t;
                br[ncr]=b[ii[i]]*t;
                ncr++;
            }
        mAR.setSize(ncr,dim);
        vBR_QP.setSize(ncr);
        vLastLambda_QP.copyFrom(vLambda); llambda=vLastLambda_QP;

        if (ncr==0)
        {
            // compute step
            vYB.copyFrom(vG);
            vYB.multiply(-1.0);
            if (mH.cholesky(M))
            {
                M.solveInPlace(vYB);
                M.solveTransposInPlace(vYB);
            } else
            {
                printf("warning: cholesky factorisation failed.\n");
                if (info) *info=2;
            }
            vLambda.zero();
        } else
        {
            Matrix mAR2=mAR.clone(), mQ2;
            MatrixTriangle mR2;
            mAR2.QR(mQ2,mR2);

            mAR.QR(mQ_QP,mR_QP, viPermut_QP); // content of mAR is destroyed here !
            bQRFailed_QP=0;

            r=mR_QP;
            for (i=0; i<ncr; i++)
                if (r[i][i]==0.0)
                {
                    // one constraint has been wrongly added.
                    bQRFailed_QP=1;
                    QPReconstructLambda(vLambda,vLambdaScale); vP.copyFrom(vLast); return; 
                }

            for (i=0; i<ncr; i++)
                if (viPermut_QP[i]!=i)
                {
                  //  printf("whoups.\n");
                }
            if (ncr<dim)
            {
                mQ_QP.getSubMatrix(mZ,0,ncr);

                // Z^t H Z
                mH.multiply(mHZ,mZ);
                mZ.transposeAndMultiply(mZHZ,mHZ);
                mQ_QP.setSize(dim,ncr);            
            }

            // form Yb
            vBR_QP.permutIn(vTmp,viPermut_QP);
            mR_QP.solveInPlace(vTmp);
            mQ_QP.multiply(vYB,vTmp);

            if (ncr<dim)
            {

                // ( vG + H vYB )^t Z : result in vD
                
                mH.multiply(vTmp,vYB);
                vTmp+=vG;
                vTmp.transposeAndMultiply(vD,mZ);

                // calculate current delta (result in vD)
                vD.multiply(-1.0);
                if (mZHZ.cholesky(M))
                {
                    M.solveInPlace(vD);
                    M.solveTransposInPlace(vD);
                }
                else
                {
                    printf("warning: cholesky factorisation failed.\n");
                    if (info) *info=2;
                };

                // evaluate vX* (result in vYB):
                mZ.multiply(vTmp, vD);
                vYB+=vTmp;
            }

            // evaluate vG* (result in vTmp2)
            mH.multiply(vTmp2,vYB);
            vTmp2+=vG;

            // evaluate lambda star (result in vTmp):
            mQ2.transposeAndMultiply(vTmp,vTmp2);
            mR2.solveTransposInPlace(vTmp);

            // evaluate lambda star (result in vTmp):
            mQ_QP.transposeAndMultiply(vTmp3,vTmp2);
            mR_QP.solveTransposInPlace(vTmp3);
            vTmp3.permutOut(vTmp,viPermut_QP);
            rlambda=vTmp;

            ncr=0;
            for (i=0; i<nc; i++)
                if (lambda[i]!=0.0)
                {
                    lambda[i]=rlambda[ncr];
                    ncr++;
                }
        } // end of test on ncr==0

        // find the most violated constraint j among non-active Linear constraints:
        j=-1;
        if (nc>0)
        {
            k=-1; violationMax=-INF; violationMax2=-INF;
            for (i=0; i<nc; i++)
            {
                if (lambda[i]<=0.0) // test to see if this constraint is not active 
                {
                    ax=mA.scalarProduct(ii[i],vYB);
                    dviolation=b[ii[i]]-ax;
                    if (llambda[i]==0.0)
                    {
                        // the constraint was not active this round
                        // thus, it can enter the competition for the next 
                        // active constraint

                        if (dviolation>maxb)
                        {
                            // the constraint should be activated
                            if (dviolation>violationMax2) 
                                { k=i; violationMax2=dviolation; }
                            al=mA.scalarProduct(ii[i],vLast)-ax; 
                            if (al>0.0) // test to see if we are going closer
                            {  
                                dviolation/=al;
                                if (dviolation>violationMax ) 
                                    { j=i; violationMax =dviolation; }
                            }
                        }
                    } else
                    {
                        lb[i]--;
                        if (feasible) 
                        {
                            if (lb[i]==0) 
                            {
                                vLambda.copyFrom(vLastLambda_QP);
                                if (lastJ>=0) lambda[lastJ]=0.0;
                                QPReconstructLambda(vLambda,vLambdaScale); 
                                vP.copyFrom(vYB); 
                                return; 
                            }
                        } else
                        {
                            if (lb[i]==0)
                            {
                                if (info) *info=1;
                                QPReconstructLambda(vLambda,vLambdaScale); 
                                vP.zero(); 
                                return;
                            }
                        }
                        finished=0;  // this constraint was wrongly activated.
                        lambda[i]=0.0;
                    }
                }
            }

            // !!! the order the tests is important here !!!
            if ((j==-1)&&(!feasible)) 
            { 
                feasible=1; 
                for (i=0; i<nc; i++) if (llambda[i]!=0.0) lb[i]=2; else lb[i]=1;
            }
            if (j==-1) { j=k; violationMax=violationMax2; } // change j to k after feasible is set
            if (ncr==mmin(dim,nc)) { 
                if (feasible) { // feasible must have been checked before
                    QPReconstructLambda(vLambda,vLambdaScale); vP.copyFrom(vYB); return; 
                } else
                {
                    if (info) *info=1;
                    QPReconstructLambda(vLambda,vLambdaScale); vP.zero(); return; 
                }
            }
            // activation of constraint only if ncr<mmin(dim,nc)
            if (j>=0) { lambda[j]=1e-5; finished=0; } // we need to activate a new constraint 
//            else if (ncr==dim){ 
//                QPReconstructLambda(vLambda); vP.copyFrom(vYB); return; }
        }

        // to prevent rounding error
        if (j==lastJ) { 
//        if (0) {
            QPReconstructLambda(vLambda,vLambdaScale); vP.copyFrom(vYB); return; }
        lastJ=j;

        vLast.copyFrom(vYB);
    }
    QPReconstructLambda(vLambda,vLambdaScale); vP.copyFrom(vYB); return;
}

Variable Documentation

FullLambda

Vector FullLambda

Definition at line 38 of file CTRSSolver.cpp.