Pt2dr cElHomographie::Direct(const Pt2dr & aP) const
{
return Pt2dr(mHX(aP),mHY(aP))/ mHZ(aP);
}
Pt2d<Fonc_Num> cElHomographie::Direct(Pt2d<Fonc_Num> aP) const
{
Symb_FNum aFZ = mHZ(aP);
return Pt2d<Fonc_Num>(mHX(aP)/aFZ,mHY(aP)/aFZ);
}
void simpleQPSolve(Matrix mH, Vector vG, Matrix mA, Vector vB, // in
Vector vP, Vector vLambda, int *info) // out
{
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;
}
