// ** Temporary version
int
ClpPdco::pdco( ClpPdcoBase * stuff, Options &options, Info &info, Outfo &outfo)
{
// D1, D2 are positive-definite diagonal matrices defined from d1, d2.
// In particular, d2 indicates the accuracy required for
// satisfying each row of Ax = b.
//
// D1 and D2 (via d1 and d2) provide primal and dual regularization
// respectively. They ensure that the primal and dual solutions
// (x,r) and (y,z) are unique and bounded.
//
// A scalar d1 is equivalent to d1 = ones(n,1), D1 = diag(d1).
// A scalar d2 is equivalent to d2 = ones(m,1), D2 = diag(d2).
// Typically, d1 = d2 = 1e-4.
// These values perturb phi(x) only slightly (by about 1e-8) and request
// that A*x = b be satisfied quite accurately (to about 1e-8).
// Set d1 = 1e-4, d2 = 1 for least-squares problems with bound constraints.
// The problem is then
//
// minimize phi(x) + 1/2 norm(d1*x)^2 + 1/2 norm(A*x - b)^2
// subject to bl <= x <= bu.
//
// More generally, d1 and d2 may be n and m vectors containing any positive
// values (preferably not too small, and typically no larger than 1).
// Bigger elements of d1 and d2 improve the stability of the solver.
//
// At an optimal solution, if x(j) is on its lower or upper bound,
// the corresponding z(j) is positive or negative respectively.
// If x(j) is between its bounds, z(j) = 0.
// If bl(j) = bu(j), x(j) is fixed at that value and z(j) may have
// either sign.
//
// Also, r and y satisfy r = D2 y, so that Ax + D2^2 y = b.
// Thus if d2(i) = 1e-4, the i-th row of Ax = b will be satisfied to
// approximately 1e-8. This determines how large d2(i) can safely be.
//
//
// EXTERNAL FUNCTIONS:
// options = pdcoSet; provided with pdco.m
// [obj,grad,hess] = pdObj( x ); provided by user
// y = pdMat( name,mode,m,n,x ); provided by user if pdMat
// is a string, not a matrix
//
// INPUT ARGUMENTS:
// pdObj is a string containing the name of a function pdObj.m
// or a function_handle for such a function
// such that [obj,grad,hess] = pdObj(x) defines
// obj = phi(x) : a scalar,
// grad = gradient of phi(x) : an n-vector,
// hess = diag(Hessian of phi): an n-vector.
// Examples:
// If phi(x) is the linear function c"x, pdObj should return
// [obj,grad,hess] = [c"*x, c, zeros(n,1)].
// If phi(x) is the entropy function E(x) = sum x(j) log x(j),
// [obj,grad,hess] = [E(x), log(x)+1, 1./x].
// pdMat may be an ifexplicit m x n matrix A (preferably sparse!),
// or a string containing the name of a function pdMat.m
// or a function_handle for such a function
// such that y = pdMat( name,mode,m,n,x )
// returns y = A*x (mode=1) or y = A"*x (mode=2).
// The input parameter "name" will be the string pdMat.
// b is an m-vector.
// bl is an n-vector of lower bounds. Non-existent bounds
// may be represented by bl(j) = -Inf or bl(j) <= -1e+20.
// bu is an n-vector of upper bounds. Non-existent bounds
// may be represented by bu(j) = Inf or bu(j) >= 1e+20.
// d1, d2 may be positive scalars or positive vectors (see above).
// options is a structure that may be set and altered by pdcoSet
// (type help pdcoSet).
// x0, y0, z0 provide an initial solution.
// xsize, zsize are estimates of the biggest x and z at the solution.
// They are used to scale (x,y,z). Good estimates
// should improve the performance of the barrier method.
//
//
// OUTPUT ARGUMENTS:
// x is the primal solution.
// y is the dual solution associated with Ax + D2 r = b.
// z is the dual solution associated with bl <= x <= bu.
// inform = 0 if a solution is found;
// = 1 if too many iterations were required;
// = 2 if the linesearch failed too often.
// PDitns is the number of Primal-Dual Barrier iterations required.
// CGitns is the number of Conjugate-Gradient iterations required
// if an iterative solver is used (LSQR).
// time is the cpu time used.
//----------------------------------------------------------------------
// PRIVATE FUNCTIONS:
// pdxxxbounds
// pdxxxdistrib
// pdxxxlsqr
// pdxxxlsqrmat
// pdxxxmat
// pdxxxmerit
// pdxxxresid1
// pdxxxresid2
// pdxxxstep
//
// GLOBAL VARIABLES:
// global pdDDD1 pdDDD2 pdDDD3
//
//
// NOTES:
// The matrix A should be reasonably well scaled: norm(A,inf) =~ 1.
// The vector b and objective phi(x) may be of any size, but ensure that
// xsize and zsize are reasonably close to norm(x,inf) and norm(z,inf)
// at the solution.
//
// The files defining pdObj and pdMat
// must not be called Fname.m or Aname.m!!
//
//
// AUTHOR:
// Michael Saunders, Systems Optimization Laboratory (SOL),
// Stanford University, Stanford, California, USA.
// [email protected]
//
// CONTRIBUTORS:
// Byunggyoo Kim, SOL, Stanford University.
// [email protected]
//
// DEVELOPMENT:
// 20 Jun 1997: Original version of pdsco.m derived from pdlp0.m.
// 29 Sep 2002: Original version of pdco.m derived from pdsco.m.
// Introduced D1, D2 in place of gamma*I, delta*I
// and allowed for general bounds bl <= x <= bu.
// 06 Oct 2002: Allowed for fixed variabes: bl(j) = bu(j) for any j.
// 15 Oct 2002: Eliminated some work vectors (since m, n might be LARGE).
// Modularized residuals, linesearch
// 16 Oct 2002: pdxxx..., pdDDD... names rationalized.
// pdAAA eliminated (global copy of A).
// Aname is now used directly as an ifexplicit A or a function.
// NOTE: If Aname is a function, it now has an extra parameter.
// 23 Oct 2002: Fname and Aname can now be function handles.
// 01 Nov 2002: Bug fixed in feval in pdxxxmat.
//-----------------------------------------------------------------------
// global pdDDD1 pdDDD2 pdDDD3
double inf = 1.0e30;
double eps = 1.0e-15;
double atolold = -1.0, r3ratio = -1.0, Pinf, Dinf, Cinf, Cinf0;
printf("\n --------------------------------------------------------");
printf("\n pdco.m Version of 01 Nov 2002");
printf("\n Primal-dual barrier method to minimize a convex function");
printf("\n subject to linear constraints Ax + r = b, bl <= x <= bu");
printf("\n --------------------------------------------------------\n");
int m = numberRows_;
int n = numberColumns_;
bool ifexplicit = true;
CoinDenseVector<double> b(m, rhs_);
CoinDenseVector<double> x(n, x_);
CoinDenseVector<double> y(m, y_);
CoinDenseVector<double> z(n, dj_);
//delete old arrays
delete [] rhs_;
delete [] x_;
delete [] y_;
delete [] dj_;
rhs_ = NULL;
x_ = NULL;
y_ = NULL;
dj_ = NULL;
// Save stuff so available elsewhere
pdcoStuff_ = stuff;
double normb = b.infNorm();
double normx0 = x.infNorm();
double normy0 = y.infNorm();
double normz0 = z.infNorm();
printf("\nmax |b | = %8g max |x0| = %8g", normb , normx0);
printf( " xsize = %8g", xsize_);
printf("\nmax |y0| = %8g max |z0| = %8g", normy0, normz0);
printf( " zsize = %8g", zsize_);
//---------------------------------------------------------------------
// Initialize.
//---------------------------------------------------------------------
//true = 1;
//false = 0;
//zn = zeros(n,1);
//int nb = n + m;
int CGitns = 0;
int inform = 0;
//---------------------------------------------------------------------
// Only allow scalar d1, d2 for now
//---------------------------------------------------------------------
/*
if (d1_->size()==1)
d1_->resize(n, d1_->getElements()[0]); // Allow scalar d1, d2
if (d2_->size()==1)
d2->resize(m, d2->getElements()[0]); // to mean dk * unit vector
*/
assert (stuff->sizeD1() == 1);
double d1 = stuff->getD1();
double d2 = stuff->getD2();
//---------------------------------------------------------------------
// Grab input options.
//---------------------------------------------------------------------
int maxitn = options.MaxIter;
double featol = options.FeaTol;
double opttol = options.OptTol;
double steptol = options.StepTol;
int stepSame = 1; /* options.StepSame; // 1 means stepx == stepz */
double x0min = options.x0min;
double z0min = options.z0min;
double mu0 = options.mu0;
int LSproblem = options.LSproblem; // See below
int LSmethod = options.LSmethod; // 1=Cholesky 2=QR 3=LSQR
int itnlim = options.LSQRMaxIter * CoinMin(m, n);
double atol1 = options.LSQRatol1; // Initial atol
double atol2 = options.LSQRatol2; // Smallest atol,unless atol1 is smaller
double conlim = options.LSQRconlim;
//int wait = options.wait;
// LSproblem:
// 1 = dy 2 = dy shifted, DLS
// 11 = s 12 = s shifted, DLS (dx = Ds)
// 21 = dx
// 31 = 3x3 system, symmetrized by Z^{1/2}
// 32 = 2x2 system, symmetrized by X^{1/2}
//---------------------------------------------------------------------
// Set other parameters.
//---------------------------------------------------------------------
int kminor = 0; // 1 stops after each iteration
double eta = 1e-4; // Linesearch tolerance for "sufficient descent"
double maxf = 10; // Linesearch backtrack limit (function evaluations)
double maxfail = 1; // Linesearch failure limit (consecutive iterations)
double bigcenter = 1e+3; // mu is reduced if center < bigcenter.
// Parameters for LSQR.
double atolmin = eps; // Smallest atol if linesearch back-tracks
double btol = 0; // Should be small (zero is ok)
double show = false; // Controls lsqr iteration log
/*
double gamma = d1->infNorm();
double delta = d2->infNorm();
*/
double gamma = d1;
double delta = d2;
printf("\n\nx0min = %8g featol = %8.1e", x0min, featol);
printf( " d1max = %8.1e", gamma);
printf( "\nz0min = %8g opttol = %8.1e", z0min, opttol);
printf( " d2max = %8.1e", delta);
printf( "\nmu0 = %8.1e steptol = %8g", mu0 , steptol);
printf( " bigcenter= %8g" , bigcenter);
printf("\n\nLSQR:");
printf("\natol1 = %8.1e atol2 = %8.1e", atol1 , atol2 );
printf( " btol = %8.1e", btol );
printf("\nconlim = %8.1e itnlim = %8d" , conlim, itnlim);
printf( " show = %8g" , show );
// LSmethod = 3; ////// Hardwire LSQR
// LSproblem = 1; ////// and LS problem defining "dy".
/*
if wait
printf("\n\nReview parameters... then type "return"\n")
keyboard
end
*/
if (eta < 0)
printf("\n\nLinesearch disabled by eta < 0");
//---------------------------------------------------------------------
// All parameters have now been set.
//---------------------------------------------------------------------
double time = CoinCpuTime();
//bool useChol = (LSmethod == 1);
//bool useQR = (LSmethod == 2);
bool direct = (LSmethod <= 2 && ifexplicit);
char solver[7];
strncpy(solver, " LSQR", 7);
//---------------------------------------------------------------------
// Categorize bounds and allow for fixed variables by modifying b.
//---------------------------------------------------------------------
int nlow, nupp, nfix;
int *bptrs[3] = {0};
getBoundTypes(&nlow, &nupp, &nfix, bptrs );
int *low = bptrs[0];
int *upp = bptrs[1];
int *fix = bptrs[2];
int nU = n;
if (nupp == 0) nU = 1; //Make dummy vectors if no Upper bounds
//---------------------------------------------------------------------
// Get pointers to local copy of model bounds
//---------------------------------------------------------------------
CoinDenseVector<double> bl(n, columnLower_);
double *bl_elts = bl.getElements();
CoinDenseVector<double> bu(nU, columnUpper_); // this is dummy if no UB
double *bu_elts = bu.getElements();
CoinDenseVector<double> r1(m, 0.0);
double *r1_elts = r1.getElements();
CoinDenseVector<double> x1(n, 0.0);
double *x1_elts = x1.getElements();
if (nfix > 0) {
for (int k = 0; k < nfix; k++)
x1_elts[fix[k]] = bl[fix[k]];
matVecMult(1, r1, x1);
b = b - r1;
// At some stage, might want to look at normfix = norm(r1,inf);
}
//---------------------------------------------------------------------
// Scale the input data.
// The scaled variables are
// xbar = x/beta,
// ybar = y/zeta,
// zbar = z/zeta.
// Define
// theta = beta*zeta;
// The scaled function is
// phibar = ( 1 /theta) fbar(beta*xbar),
// gradient = (beta /theta) grad,
// Hessian = (beta2/theta) hess.
//---------------------------------------------------------------------
double beta = xsize_;
if (beta == 0) beta = 1; // beta scales b, x.
double zeta = zsize_;
if (zeta == 0) zeta = 1; // zeta scales y, z.
double theta = beta * zeta; // theta scales obj.
// (theta could be anything, but theta = beta*zeta makes
// scaled grad = grad/zeta = 1 approximately if zeta is chosen right.)
for (int k = 0; k < nlow; k++)
bl_elts[low[k]] = bl_elts[low[k]] / beta;
for (int k = 0; k < nupp; k++)
bu_elts[upp[k]] = bu_elts[upp[k]] / beta;
d1 = d1 * ( beta / sqrt(theta) );
d2 = d2 * ( sqrt(theta) / beta );
double beta2 = beta * beta;
b.scale( (1.0 / beta) );
y.scale( (1.0 / zeta) );
x.scale( (1.0 / beta) );
z.scale( (1.0 / zeta) );
//---------------------------------------------------------------------
// Initialize vectors that are not fully used if bounds are missing.
//---------------------------------------------------------------------
CoinDenseVector<double> rL(n, 0.0);
CoinDenseVector<double> cL(n, 0.0);
CoinDenseVector<double> z1(n, 0.0);
CoinDenseVector<double> dx1(n, 0.0);
CoinDenseVector<double> dz1(n, 0.0);
CoinDenseVector<double> r2(n, 0.0);
// Assign upper bd regions (dummy if no UBs)
CoinDenseVector<double> rU(nU, 0.0);
CoinDenseVector<double> cU(nU, 0.0);
CoinDenseVector<double> x2(nU, 0.0);
CoinDenseVector<double> z2(nU, 0.0);
CoinDenseVector<double> dx2(nU, 0.0);
CoinDenseVector<double> dz2(nU, 0.0);
//---------------------------------------------------------------------
// Initialize x, y, z, objective, etc.
//---------------------------------------------------------------------
CoinDenseVector<double> dx(n, 0.0);
CoinDenseVector<double> dy(m, 0.0);
CoinDenseVector<double> Pr(m);
CoinDenseVector<double> D(n);
double *D_elts = D.getElements();
CoinDenseVector<double> w(n);
double *w_elts = w.getElements();
CoinDenseVector<double> rhs(m + n);
//---------------------------------------------------------------------
// Pull out the element array pointers for efficiency
//---------------------------------------------------------------------
double *x_elts = x.getElements();
double *x2_elts = x2.getElements();
double *z_elts = z.getElements();
double *z1_elts = z1.getElements();
double *z2_elts = z2.getElements();
for (int k = 0; k < nlow; k++) {
x_elts[low[k]] = CoinMax( x_elts[low[k]], bl[low[k]]);
x1_elts[low[k]] = CoinMax( x_elts[low[k]] - bl[low[k]], x0min );
z1_elts[low[k]] = CoinMax( z_elts[low[k]], z0min );
}
for (int k = 0; k < nupp; k++) {
x_elts[upp[k]] = CoinMin( x_elts[upp[k]], bu[upp[k]]);
x2_elts[upp[k]] = CoinMax(bu[upp[k]] - x_elts[upp[k]], x0min );
z2_elts[upp[k]] = CoinMax(-z_elts[upp[k]], z0min );
}
//////////////////// Assume hessian is diagonal. //////////////////////
// [obj,grad,hess] = feval( Fname, (x*beta) );
x.scale(beta);
double obj = getObj(x);
CoinDenseVector<double> grad(n);
getGrad(x, grad);
CoinDenseVector<double> H(n);
getHessian(x , H);
x.scale((1.0 / beta));
//double * g_elts = grad.getElements();
double * H_elts = H.getElements();
obj /= theta; // Scaled obj.
grad = grad * (beta / theta) + (d1 * d1) * x; // grad includes x regularization.
H = H * (beta2 / theta) + (d1 * d1) ; // H includes x regularization.
/*---------------------------------------------------------------------
// Compute primal and dual residuals:
// r1 = b - Aprod(x) - d2*d2*y;
// r2 = grad - Atprod(y) + z2 - z1;
// rL = bl - x + x1;
// rU = x + x2 - bu; */
//---------------------------------------------------------------------
// [r1,r2,rL,rU,Pinf,Dinf] = ...
// pdxxxresid1( Aname,fix,low,upp, ...
// b,bl,bu,d1,d2,grad,rL,rU,x,x1,x2,y,z1,z2 );
pdxxxresid1( this, nlow, nupp, nfix, low, upp, fix,
b, bl_elts, bu_elts, d1, d2, grad, rL, rU, x, x1, x2, y, z1, z2,
r1, r2, &Pinf, &Dinf);
//---------------------------------------------------------------------
// Initialize mu and complementarity residuals:
// cL = mu*e - X1*z1.
// cU = mu*e - X2*z2.
//
// 25 Jan 2001: Now that b and obj are scaled (and hence x,y,z),
// we should be able to use mufirst = mu0 (absolute value).
// 0.1 worked poorly on StarTest1 with x0min = z0min = 0.1.
// 29 Jan 2001: We might as well use mu0 = x0min * z0min;
// so that most variables are centered after a warm start.
// 29 Sep 2002: Use mufirst = mu0*(x0min * z0min),
// regarding mu0 as a scaling of the initial center.
//---------------------------------------------------------------------
// double mufirst = mu0*(x0min * z0min);
double mufirst = mu0; // revert to absolute value
double mulast = 0.1 * opttol;
mulast = CoinMin( mulast, mufirst );
double mu = mufirst;
double center, fmerit;
pdxxxresid2( mu, nlow, nupp, low, upp, cL, cU, x1, x2,
z1, z2, ¢er, &Cinf, &Cinf0 );
fmerit = pdxxxmerit(nlow, nupp, low, upp, r1, r2, rL, rU, cL, cU );
// Initialize other things.
bool precon = true;
double PDitns = 0;
//bool converged = false;
double atol = atol1;
atol2 = CoinMax( atol2, atolmin );
atolmin = atol2;
// pdDDD2 = d2; // Global vector for diagonal matrix D2
// Iteration log.
int nf = 0;
int itncg = 0;
int nfail = 0;
printf("\n\nItn mu stepx stepz Pinf Dinf");
printf(" Cinf Objective nf center");
if (direct) {
printf("\n");
} else {
printf(" atol solver Inexact\n");
}
double regx = (d1 * x).twoNorm();
double regy = (d2 * y).twoNorm();
// regterm = twoNorm(d1.*x)^2 + norm(d2.*y)^2;
double regterm = regx * regx + regy * regy;
double objreg = obj + 0.5 * regterm;
double objtrue = objreg * theta;
printf("\n%3g ", PDitns );
printf("%6.1f%6.1f" , log10(Pinf ), log10(Dinf));
printf("%6.1f%15.7e", log10(Cinf0), objtrue );
printf(" %8.1f\n" , center );
/*
if kminor
printf("\n\nStart of first minor itn...\n");
keyboard
end
*/
//---------------------------------------------------------------------
// Main loop.
//---------------------------------------------------------------------
// Lsqr
ClpLsqr thisLsqr(this);
// while (converged) {
while(PDitns < maxitn) {
PDitns = PDitns + 1;
// 31 Jan 2001: Set atol according to progress, a la Inexact Newton.
// 07 Feb 2001: 0.1 not small enough for Satellite problem. Try 0.01.
// 25 Apr 2001: 0.01 seems wasteful for Star problem.
// Now that starting conditions are better, go back to 0.1.
double r3norm = CoinMax(Pinf, CoinMax(Dinf, Cinf));
atol = CoinMin(atol, r3norm * 0.1);
atol = CoinMax(atol, atolmin );
info.r3norm = r3norm;
//-------------------------------------------------------------------
// Define a damped Newton iteration for solving f = 0,
// keeping x1, x2, z1, z2 > 0. We eliminate dx1, dx2, dz1, dz2
// to obtain the system
//
// [-H2 A" ] [ dx ] = [ w ], H2 = H + D1^2 + X1inv Z1 + X2inv Z2,
// [ A D2^2] [ dy ] = [ r1] w = r2 - X1inv(cL + Z1 rL)
// + X2inv(cU + Z2 rU),
//
// which is equivalent to the least-squares problem
//
// min || [ D A"]dy - [ D w ] ||, D = H2^{-1/2}. (*)
// || [ D2 ] [D2inv r1] ||
//-------------------------------------------------------------------
for (int k = 0; k < nlow; k++)
H_elts[low[k]] = H_elts[low[k]] + z1[low[k]] / x1[low[k]];
for (int k = 0; k < nupp; k++)
H[upp[k]] = H[upp[k]] + z2[upp[k]] / x2[upp[k]];
w = r2;
for (int k = 0; k < nlow; k++)
w[low[k]] = w[low[k]] - (cL[low[k]] + z1[low[k]] * rL[low[k]]) / x1[low[k]];
for (int k = 0; k < nupp; k++)
w[upp[k]] = w[upp[k]] + (cU[upp[k]] + z2[upp[k]] * rU[upp[k]]) / x2[upp[k]];
if (LSproblem == 1) {
//-----------------------------------------------------------------
// Solve (*) for dy.
//-----------------------------------------------------------------
H = 1.0 / H; // H is now Hinv (NOTE!)
for (int k = 0; k < nfix; k++)
H[fix[k]] = 0;
for (int k = 0; k < n; k++)
D_elts[k] = sqrt(H_elts[k]);
thisLsqr.borrowDiag1(D_elts);
thisLsqr.diag2_ = d2;
if (direct) {
// Omit direct option for now
} else {// Iterative solve using LSQR.
//rhs = [ D.*w; r1./d2 ];
for (int k = 0; k < n; k++)
rhs[k] = D_elts[k] * w_elts[k];
for (int k = 0; k < m; k++)
rhs[n+k] = r1_elts[k] * (1.0 / d2);
double damp = 0;
if (precon) { // Construct diagonal preconditioner for LSQR
matPrecon(d2, Pr, D);
}
/*
rw(7) = precon;
info.atolmin = atolmin;
info.r3norm = fmerit; // Must be the 2-norm here.
[ dy, istop, itncg, outfo ] = ...
pdxxxlsqr( nb,m,"pdxxxlsqrmat",Aname,rw,rhs,damp, ...
atol,btol,conlim,itnlim,show,info );
thisLsqr.input->rhs_vec = &rhs;
thisLsqr.input->sol_vec = &dy;
thisLsqr.input->rel_mat_err = atol;
thisLsqr.do_lsqr(this);
*/
// New version of lsqr
int istop;
dy.clear();
show = false;
info.atolmin = atolmin;
info.r3norm = fmerit; // Must be the 2-norm here.
thisLsqr.do_lsqr( rhs, damp, atol, btol, conlim, itnlim,
show, info, dy , &istop, &itncg, &outfo, precon, Pr);
if (precon)
dy = dy * Pr;
if (!precon && itncg > 999999)
precon = true;
if (istop == 3 || istop == 7 ) // conlim or itnlim
printf("\n LSQR stopped early: istop = //%d", istop);
atolold = outfo.atolold;
atol = outfo.atolnew;
r3ratio = outfo.r3ratio;
}// LSproblem 1
// grad = pdxxxmat( Aname,2,m,n,dy ); // grad = A"dy
grad.clear();
matVecMult(2, grad, dy);
for (int k = 0; k < nfix; k++)
grad[fix[k]] = 0; // grad is a work vector
dx = H * (grad - w);
} else {
perror( "This LSproblem not yet implemented\n" );
}
//-------------------------------------------------------------------
CGitns += itncg;
//-------------------------------------------------------------------
// dx and dy are now known. Get dx1, dx2, dz1, dz2.
//-------------------------------------------------------------------
for (int k = 0; k < nlow; k++) {
dx1[low[k]] = - rL[low[k]] + dx[low[k]];
dz1[low[k]] = (cL[low[k]] - z1[low[k]] * dx1[low[k]]) / x1[low[k]];
}
for (int k = 0; k < nupp; k++) {
dx2[upp[k]] = - rU[upp[k]] - dx[upp[k]];
dz2[upp[k]] = (cU[upp[k]] - z2[upp[k]] * dx2[upp[k]]) / x2[upp[k]];
}
//-------------------------------------------------------------------
// Find the maximum step.
//--------------------------------------------------------------------
double stepx1 = pdxxxstep(nlow, low, x1, dx1 );
double stepx2 = inf;
if (nupp > 0)
stepx2 = pdxxxstep(nupp, upp, x2, dx2 );
double stepz1 = pdxxxstep( z1 , dz1 );
double stepz2 = inf;
if (nupp > 0)
stepz2 = pdxxxstep( z2 , dz2 );
double stepx = CoinMin( stepx1, stepx2 );
double stepz = CoinMin( stepz1, stepz2 );
stepx = CoinMin( steptol * stepx, 1.0 );
stepz = CoinMin( steptol * stepz, 1.0 );
if (stepSame) { // For NLPs, force same step
stepx = CoinMin( stepx, stepz ); // (true Newton method)
stepz = stepx;
}
//-------------------------------------------------------------------
// Backtracking linesearch.
//-------------------------------------------------------------------
bool fail = true;
nf = 0;
while (nf < maxf) {
nf = nf + 1;
x = x + stepx * dx;
y = y + stepz * dy;
for (int k = 0; k < nlow; k++) {
x1[low[k]] = x1[low[k]] + stepx * dx1[low[k]];
z1[low[k]] = z1[low[k]] + stepz * dz1[low[k]];
}
for (int k = 0; k < nupp; k++) {
x2[upp[k]] = x2[upp[k]] + stepx * dx2[upp[k]];
z2[upp[k]] = z2[upp[k]] + stepz * dz2[upp[k]];
}
// [obj,grad,hess] = feval( Fname, (x*beta) );
x.scale(beta);
obj = getObj(x);
getGrad(x, grad);
getHessian(x, H);
x.scale((1.0 / beta));
obj /= theta;
grad = grad * (beta / theta) + d1 * d1 * x;
H = H * (beta2 / theta) + d1 * d1;
// [r1,r2,rL,rU,Pinf,Dinf] = ...
pdxxxresid1( this, nlow, nupp, nfix, low, upp, fix,
b, bl_elts, bu_elts, d1, d2, grad, rL, rU, x, x1, x2,
y, z1, z2, r1, r2, &Pinf, &Dinf );
//double center, Cinf, Cinf0;
// [cL,cU,center,Cinf,Cinf0] = ...
pdxxxresid2( mu, nlow, nupp, low, upp, cL, cU, x1, x2, z1, z2,
¢er, &Cinf, &Cinf0);
double fmeritnew = pdxxxmerit(nlow, nupp, low, upp, r1, r2, rL, rU, cL, cU );
double step = CoinMin( stepx, stepz );
if (fmeritnew <= (1 - eta * step)*fmerit) {
fail = false;
break;
}
// Merit function didn"t decrease.
// Restore variables to previous values.
// (This introduces a little error, but save lots of space.)
x = x - stepx * dx;
y = y - stepz * dy;
for (int k = 0; k < nlow; k++) {
x1[low[k]] = x1[low[k]] - stepx * dx1[low[k]];
z1[low[k]] = z1[low[k]] - stepz * dz1[low[k]];
}
for (int k = 0; k < nupp; k++) {
x2[upp[k]] = x2[upp[k]] - stepx * dx2[upp[k]];
z2[upp[k]] = z2[upp[k]] - stepz * dz2[upp[k]];
}
// Back-track.
// If it"s the first time,
// make stepx and stepz the same.
if (nf == 1 && stepx != stepz) {
stepx = step;
} else if (nf < maxf) {
stepx = stepx / 2;
}
stepz = stepx;
}
if (fail) {
printf("\n Linesearch failed (nf too big)");
nfail += 1;
} else {
nfail = 0;
}
//-------------------------------------------------------------------
// Set convergence measures.
//--------------------------------------------------------------------
regx = (d1 * x).twoNorm();
regy = (d2 * y).twoNorm();
regterm = regx * regx + regy * regy;
objreg = obj + 0.5 * regterm;
objtrue = objreg * theta;
bool primalfeas = Pinf <= featol;
bool dualfeas = Dinf <= featol;
bool complementary = Cinf0 <= opttol;
bool enough = PDitns >= 4; // Prevent premature termination.
bool converged = primalfeas & dualfeas & complementary & enough;
//-------------------------------------------------------------------
// Iteration log.
//-------------------------------------------------------------------
char str1[100], str2[100], str3[100], str4[100], str5[100];
sprintf(str1, "\n%3g%5.1f" , PDitns , log10(mu) );
sprintf(str2, "%8.5f%8.5f" , stepx , stepz );
if (stepx < 0.0001 || stepz < 0.0001) {
sprintf(str2, " %6.1e %6.1e" , stepx , stepz );
}
sprintf(str3, "%6.1f%6.1f" , log10(Pinf) , log10(Dinf));
sprintf(str4, "%6.1f%15.7e", log10(Cinf0), objtrue );
sprintf(str5, "%3d%8.1f" , nf , center );
if (center > 99999) {
sprintf(str5, "%3d%8.1e" , nf , center );
}
printf("%s%s%s%s%s", str1, str2, str3, str4, str5);
if (direct) {
// relax
} else {
printf(" %5.1f%7d%7.3f", log10(atolold), itncg, r3ratio);
}
//-------------------------------------------------------------------
// Test for termination.
//-------------------------------------------------------------------
if (kminor) {
printf( "\nStart of next minor itn...\n");
// keyboard;
}
if (converged) {
printf("\n Converged");
break;
} else if (PDitns >= maxitn) {
printf("\n Too many iterations");
inform = 1;
break;
} else if (nfail >= maxfail) {
printf("\n Too many linesearch failures");
inform = 2;
break;
} else {
// Reduce mu, and reset certain residuals.
double stepmu = CoinMin( stepx , stepz );
stepmu = CoinMin( stepmu, steptol );
double muold = mu;
mu = mu - stepmu * mu;
if (center >= bigcenter)
mu = muold;
// mutrad = mu0*(sum(Xz)/n); // 24 May 1998: Traditional value, but
// mu = CoinMin(mu,mutrad ); // it seemed to decrease mu too much.
mu = CoinMax(mu, mulast); // 13 Jun 1998: No need for smaller mu.
// [cL,cU,center,Cinf,Cinf0] = ...
pdxxxresid2( mu, nlow, nupp, low, upp, cL, cU, x1, x2, z1, z2,
¢er, &Cinf, &Cinf0 );
fmerit = pdxxxmerit( nlow, nupp, low, upp, r1, r2, rL, rU, cL, cU );
// Reduce atol for LSQR (and SYMMLQ).
// NOW DONE AT TOP OF LOOP.
atolold = atol;
// if atol > atol2
// atolfac = (mu/mufirst)^0.25;
// atol = CoinMax( atol*atolfac, atol2 );
// end
// atol = CoinMin( atol, mu ); // 22 Jan 2001: a la Inexact Newton.
// atol = CoinMin( atol, 0.5*mu ); // 30 Jan 2001: A bit tighter
// If the linesearch took more than one function (nf > 1),
// we assume the search direction needed more accuracy
// (though this may be true only for LPs).
// 12 Jun 1998: Ask for more accuracy if nf > 2.
// 24 Nov 2000: Also if the steps are small.
// 30 Jan 2001: Small steps might be ok with warm start.
// 06 Feb 2001: Not necessarily. Reinstated tests in next line.
if (nf > 2 || CoinMin( stepx, stepz ) <= 0.01)
atol = atolold * 0.1;
}
//---------------------------------------------------------------------
// End of main loop.
//---------------------------------------------------------------------
}
for (int k = 0; k < nfix; k++)
x[fix[k]] = bl[fix[k]];
z = z1;
if (nupp > 0)
z = z - z2;
printf("\n\nmax |x| =%10.3f", x.infNorm() );
printf(" max |y| =%10.3f", y.infNorm() );
printf(" max |z| =%10.3f", z.infNorm() );
printf(" scaled");
x.scale(beta);
y.scale(zeta);
z.scale(zeta); // Unscale x, y, z.
printf( "\nmax |x| =%10.3f", x.infNorm() );
printf(" max |y| =%10.3f", y.infNorm() );
printf(" max |z| =%10.3f", z.infNorm() );
printf(" unscaled\n");
time = CoinCpuTime() - time;
char str1[100], str2[100];
sprintf(str1, "\nPDitns =%10g", PDitns );
sprintf(str2, "itns =%10d", CGitns );
// printf( [str1 " " solver str2] );
printf(" time =%10.1f\n", time);
/*
pdxxxdistrib( abs(x),abs(z) ); // Private function
if (wait)
keyboard;
*/
//-----------------------------------------------------------------------
// End function pdco.m
//-----------------------------------------------------------------------
/* printf("Solution x values:\n\n");
for (int k=0; k<n; k++)
printf(" %d %e\n", k, x[k]);
*/
// Print distribution
double thresh[9] = { 0.00000001, 0.0000001, 0.000001, 0.00001, 0.0001, 0.001, 0.01, 0.1, 1.00001};
int counts[9] = {0};
for (int ij = 0; ij < n; ij++) {
for (int j = 0; j < 9; j++) {
if(x[ij] < thresh[j]) {
counts[j] += 1;
break;
}
}
}
printf ("Distribution of Solution Values\n");
for (int j = 8; j > 1; j--)
printf(" %g to %g %d\n", thresh[j-1], thresh[j], counts[j]);
printf(" Less than %g %d\n", thresh[2], counts[0]);
return inform;
}
void stereoCalibThread::monoCalibRun()
{
while(imagePortInLeft.getInputCount()==0 && imagePortInRight.getInputCount()==0)
{
fprintf(stdout, "Connect one camera.. \n");
Time::delay(1.0);
if(isStopping())
return;
}
bool left= imagePortInLeft.getInputCount()>0?true:false;
string cameraName;
if(left)
cameraName="LEFT";
else
cameraName="RIGHT";
fprintf(stdout, "CALIBRATING %s CAMERA \n",cameraName.c_str());
int count=1;
Size boardSize, imageSize;
boardSize.width=this->boardWidth;
boardSize.height=this->boardHeight;
while (!isStopping()) {
if(left)
imageL = imagePortInLeft.read(false);
else
imageL = imagePortInRight.read(false);
if(imageL!=NULL){
bool foundL=false;
mutex->wait();
if(startCalibration>0) {
string pathImg=imageDir;
preparePath(pathImg.c_str(), pathL,pathR,count);
string iml(pathL);
imgL= (IplImage*) imageL->getIplImage();
Mat Left(imgL);
std::vector<Point2f> pointbufL;
if(boardType == "CIRCLES_GRID") {
foundL = findCirclesGridDefault(Left, boardSize, pointbufL, CALIB_CB_SYMMETRIC_GRID | CALIB_CB_CLUSTERING);
} else if(boardType == "ASYMMETRIC_CIRCLES_GRID") {
foundL = findCirclesGridDefault(Left, boardSize, pointbufL, CALIB_CB_ASYMMETRIC_GRID | CALIB_CB_CLUSTERING);
} else {
foundL = findChessboardCorners(Left, boardSize, pointbufL, CV_CALIB_CB_ADAPTIVE_THRESH | CV_CALIB_CB_FAST_CHECK | CV_CALIB_CB_NORMALIZE_IMAGE);
}
if(foundL) {
cvCvtColor(imgL,imgL,CV_RGB2BGR);
saveImage(pathImg.c_str(),imgL,count);
imageListL.push_back(iml);
Mat cL(pointbufL);
drawChessboardCorners(Left, boardSize, cL, foundL);
count++;
}
if(count>numOfPairs) {
fprintf(stdout," Running %s Camera Calibration... \n", cameraName.c_str());
monoCalibration(imageListL,this->boardWidth,this->boardHeight,this->Kleft,this->DistL);
fprintf(stdout," Saving Calibration Results... \n");
if(left)
updateIntrinsics(imgL->width,imgL->height,Kleft.at<double>(0,0),Kleft.at<double>(1,1),Kleft.at<double>(0,2),Kleft.at<double>(1,2),DistL.at<double>(0,0),DistL.at<double>(0,1),DistL.at<double>(0,2),DistL.at<double>(0,3),"CAMERA_CALIBRATION_LEFT");
else
updateIntrinsics(imgL->width,imgL->height,Kleft.at<double>(0,0),Kleft.at<double>(1,1),Kleft.at<double>(0,2),Kleft.at<double>(1,2),DistL.at<double>(0,0),DistL.at<double>(0,1),DistL.at<double>(0,2),DistL.at<double>(0,3),"CAMERA_CALIBRATION_RIGHT");
fprintf(stdout, "Calibration Results Saved in %s \n", camCalibFile.c_str());
startCalibration=0;
count=1;
imageListL.clear();
}
}
mutex->post();
ImageOf<PixelRgb>& outimL=outPortLeft.prepare();
outimL=*imageL;
outPortLeft.write();
ImageOf<PixelRgb>& outimR=outPortRight.prepare();
outimR=*imageL;
outPortRight.write();
if(foundL && startCalibration==1)
Time::delay(2.0);
cout.flush();
}
}
}
void* iswt_2d(vector<double> &swtop,int J, string nm, vector<vector<double> > &sig,int row, int col) {
// 2D Inverse Stationary Wavelet Transform
// Works In Conjunction with C++ Wavelet Library http://code.google.com/p/wavelet1d/
// (C) Rafat Hussain under GNU GPL v3
// See Project page http://code.google.com/p/wavelet1d/
// swtop [1D Output vector from swt_2d]
// J [Number of Levels]
// nm [Wavelet Name]
// sig [Output]
// row & col [Rows and cols of sig]
// I have not fully tested this code.
// References :
// 1. Nason, G.P. and Silverman, B.W. (1995) The stationary wavelet transform and some statistical applications.
// In Antoniadis, A. and Oppenheim, G. (eds) Lecture Notes in Statistics, 103, 281--300.
// 2. Pesquet, J.C.; H. Krim, H. Carfatan (1996), "Time-invariant orthonormal wavelet representations,"
// http://pdf.aminer.org/000/318/848/frequency_shift_invariant_orthonormal_wavelet_packet_representations.pdf
int rows_n =row;
int cols_n = col;
vector<double> lp1,hp1,lp2,hp2;
filtcoef(nm,lp1,hp1,lp2,hp2);
vector<double> low_pass = lp2;
vector<double> high_pass = hp2;
int lf = low_pass.size();
int sum =0;
vector<vector<double> > cLL(rows_n, vector<double>(cols_n));
for (int iter=J; iter > 0; iter--) {
vector<vector<double> > cLH(rows_n, vector<double>(cols_n));
vector<vector<double> > cHL(rows_n, vector<double>(cols_n));
vector<vector<double> > cHH(rows_n, vector<double>(cols_n));
if (iter == J) {
for (int i = 0 ; i < rows_n; i++ ){
for (int j = 0; j < cols_n; j++){
cLL[i][j] = swtop[sum+ i * cols_n + j];
cHL[i][j] = swtop[sum+ rows_n * cols_n+ i * cols_n + j];
cLH[i][j] = swtop[sum+ 2 * rows_n * cols_n + i * cols_n + j];
cHH[i][j] = swtop[sum+ 3* rows_n * cols_n + i * cols_n + j];
}
}
sum+= 4 * rows_n * cols_n;
} else {
for (int i = 0 ; i < rows_n; i++ ){
for (int j = 0; j < cols_n; j++){
cHL[i][j] = swtop[sum+ i * cols_n + j];
cLH[i][j] = swtop[sum+ rows_n * cols_n + i * cols_n + j];
cHH[i][j] = swtop[sum+ 2* rows_n * cols_n + i * cols_n + j];
}
}
sum+= 3 * rows_n * cols_n;
}
int value = (int) pow(2.0,(double) (iter-1));
for (int count = 0; count < value; count++) {
vector<vector<double> > cLL1(rows_n/value, vector<double>(cols_n/value));
vector<vector<double> > cLH1(rows_n/value, vector<double>(cols_n/value));
vector<vector<double> > cHL1(rows_n/value, vector<double>(cols_n/value));
vector<vector<double> > cHH1(rows_n/value, vector<double>(cols_n/value));
int row1 = 0;
int col1 = 0;
for (int index_r = count; index_r < rows_n; index_r+=value){
for (int index_c = count; index_c < cols_n; index_c+=value){
cLL1[row1][col1]=cLL[index_r][index_c];
cLH1[row1][col1]=cLH[index_r][index_c];
cHL1[row1][col1]=cHL[index_r][index_c];
cHH1[row1][col1]=cHH[index_r][index_c];
col1++;
}
col1 = 0;
row1++;
}
//Implementing shift =0
unsigned int len_c = cLL1[0].size();
unsigned int len_r = cLL1.size();
vector<vector<double> > cL(rows_n,vector<double>(cols_n));
vector<vector<double> > cH(rows_n,vector<double>(cols_n));
vector<vector<double> > cLL2(len_r/2, vector<double>(len_c/2));
vector<vector<double> > cLH2(len_r/2, vector<double>(len_c/2));
vector<vector<double> > cHL2(len_r/2, vector<double>(len_c/2));
vector<vector<double> > cHH2(len_r/2, vector<double>(len_c/2));
row1=0;
col1=0;
for (int index_r = 0; index_r < len_r; index_r+=2){
for (unsigned int index_c = 0; index_c < len_c; index_c+=2){
cLL2[row1][col1]=cLL1[index_r][index_c];
cLH2[row1][col1]=cLH1[index_r][index_c];
cHL2[row1][col1]=cHL1[index_r][index_c];
cHH2[row1][col1]=cHH1[index_r][index_c];
col1++;
}
col1 = 0;
row1++;
}
vector<vector<double> > cLLu(len_r, vector<double>(len_c));
vector<vector<double> > cLHu(len_r, vector<double>(len_c));
vector<vector<double> > cHLu(len_r, vector<double>(len_c));
vector<vector<double> > cHHu(len_r, vector<double>(len_c));
int row_u = cLL2.size();
int col_u = cLL2[0].size();
upsamp2(cLL2,cLLu,2,2);
upsamp2(cLH2,cLHu,2,2);
upsamp2(cHL2,cHLu,2,2);
upsamp2(cHH2,cHHu,2,2);
//cLL2.clear();cLH2.clear();cHL2.clear();cHH2.clear();
vector<vector<double> > cLL00(len_r+lf, vector<double>(len_c + lf));
vector<vector<double> > cLH00(len_r+lf, vector<double>(len_c + lf));
vector<vector<double> > cHL00(len_r+lf, vector<double>(len_c + lf));
vector<vector<double> > cHH00(len_r+lf, vector<double>(len_c + lf));
per_ext2d(cLLu,cLL00,lf/2);
per_ext2d(cLHu,cLH00,lf/2);
per_ext2d(cHLu,cHL00,lf/2);
per_ext2d(cHHu,cHH00,lf/2);
//cLLu.clear();cLHu.clear();cHLu.clear();cHHu.clear();
//Shift By 1
int U = 2; // Upsampling Factor
vector<vector<double> > cLL3(len_r/2, vector<double>(len_c/2));
vector<vector<double> > cLH3(len_r/2, vector<double>(len_c/2));
vector<vector<double> > cHL3(len_r/2, vector<double>(len_c/2));
vector<vector<double> > cHH3(len_r/2, vector<double>(len_c/2));
col1 = 0;
row1 = 0;
for (int index_r = 1; index_r < len_r; index_r+=2){
for (unsigned int index_c = 1; index_c < len_c; index_c+=2){
cLL3[row1][col1]=cLL1[index_r][index_c];
cLH3[row1][col1]=cLH1[index_r][index_c];
cHL3[row1][col1]=cHL1[index_r][index_c];
cHH3[row1][col1]=cHH1[index_r][index_c];
col1++;
}
col1 = 0;
row1++;
}
vector<vector<double> > cLLu2(len_r, vector<double>(len_c));
vector<vector<double> > cLHu2(len_r, vector<double>(len_c));
vector<vector<double> > cHLu2(len_r, vector<double>(len_c));
vector<vector<double> > cHHu2(len_r, vector<double>(len_c));
row_u = cLL3.size();
col_u = cLL3[0].size();
upsamp2(cLL3,cLLu2,2,2);
upsamp2(cLH3,cLHu2,2,2);
upsamp2(cHL3,cHLu2,2,2);
upsamp2(cHH3,cHHu2,2,2);
//cLL3.clear();cLH3.clear();cHL3.clear();cHH3.clear();
cout << "STAGE 6" << endl;
vector<vector<double> > cLL01(len_r+lf, vector<double>(len_c + lf));
vector<vector<double> > cLH01(len_r+lf, vector<double>(len_c + lf));
vector<vector<double> > cHL01(len_r+lf, vector<double>(len_c + lf));
vector<vector<double> > cHH01(len_r+lf, vector<double>(len_c + lf));
per_ext2d(cLLu2,cLL01,lf/2);
per_ext2d(cLHu2,cLH01,lf/2);
per_ext2d(cHLu2,cHL01,lf/2);
per_ext2d(cHHu2,cHH01,lf/2);
cLLu2.clear();cLHu2.clear();cHLu2.clear();cHHu2.clear();
int rowsLL = cLL01.size();
int colsLL = cLL01[0].size();
vector<vector<double> > cLLt00(rowsLL + lf-1, vector<double>(colsLL));
vector<vector<double> > cLHt00(rowsLL + lf-1, vector<double>(colsLL));
vector<vector<double> > cHLt00(rowsLL + lf-1, vector<double>(colsLL));
vector<vector<double> > cHHt00(rowsLL + lf-1, vector<double>(colsLL));
vector<vector<double> > cLLt10(rowsLL + lf-1, vector<double>(colsLL));
vector<vector<double> > cLHt10(rowsLL + lf-1, vector<double>(colsLL));
vector<vector<double> > cHLt10(rowsLL + lf-1, vector<double>(colsLL));
vector<vector<double> > cHHt10(rowsLL + lf-1, vector<double>(colsLL));
vector<vector<double> > cLLt01(rowsLL + lf-1, vector<double>(colsLL + lf-1));
vector<vector<double> > cLHt01(rowsLL + lf-1, vector<double>(colsLL + lf-1));
vector<vector<double> > cHLt01(rowsLL + lf-1, vector<double>(colsLL + lf-1));
vector<vector<double> > cHHt01(rowsLL + lf-1, vector<double>(colsLL + lf-1));
vector<vector<double> > cLLt11(rowsLL + lf-1, vector<double>(colsLL + lf-1));
vector<vector<double> > cLHt11(rowsLL + lf-1, vector<double>(colsLL + lf-1));
vector<vector<double> > cHLt11(rowsLL + lf-1, vector<double>(colsLL + lf-1));
vector<vector<double> > cHHt11(rowsLL + lf-1, vector<double>(colsLL + lf-1));
vector<vector<double> > cLLt0(rowsLL + lf-1, vector<double>(colsLL + lf-1));
vector<vector<double> > cLLt1(rowsLL + lf-1, vector<double>(colsLL + lf-1));
vector<vector<double> > oupt0(len_r, vector<double>(len_c));
vector<vector<double> > oupt1(len_r, vector<double>(len_c));
vector<vector<double> > oupt(len_r, vector<double>(len_c));
for (int j=0; j< colsLL;j++) {
vector<double> sigLL0;
vector<double> sigLH0;
vector<double> sigHL0;
vector<double> sigHH0;
for (int i=0; i < rowsLL; i++) {
double temp01 = cLL00[i][j];
double temp02 = cLH00[i][j];
double temp03 = cHL00[i][j];
double temp04 = cHH00[i][j];
sigLL0.push_back(temp01);
sigLH0.push_back(temp02);
sigHL0.push_back(temp03);
sigHH0.push_back(temp04);
}
// First Convolution Step for LL,LH,HL,HH
vector<double> X0LL;
convfft(sigLL0, low_pass, X0LL);
vector<double> X0LH;
convfft(sigLH0, low_pass, X0LH);
vector<double> X0HL;
convfft(sigHL0, high_pass, X0HL);
vector<double> X0HH;
convfft(sigHH0, high_pass, X0HH);
int lent=X0LL.size();
for (int i=0; i < lent; i++) {
cLLt00[i][j]=X0LL[i];
cLHt00[i][j]=X0LH[i];
cHLt00[i][j]=X0HL[i];
cHHt00[i][j]=X0HH[i];
}
}
for (int i=0; i< rowsLL+lf-1;i++) {
vector<double> sigLL0;
vector<double> sigLH0;
vector<double> sigHL0;
vector<double> sigHH0;
for (int j=0; j < colsLL; j++) {
double temp01 = cLLt00[i][j];
double temp02 = cLHt00[i][j];
double temp03 = cHLt00[i][j];
double temp04 = cHHt00[i][j];
sigLL0.push_back(temp01);
sigLH0.push_back(temp02);
sigHL0.push_back(temp03);
sigHH0.push_back(temp04);
}
// Second Convolution Step for LL,LH,HL,HH
vector<double> X0LL;
convfft(sigLL0, low_pass, X0LL);
vector<double> X0LH;
convfft(sigLH0, high_pass, X0LH);
vector<double> X0HL;
convfft(sigHL0, low_pass, X0HL);
vector<double> X0HH;
convfft(sigHH0, high_pass, X0HH);
int lent=X0LL.size();
for (int j=0; j < lent; j++) {
cLLt01[i][j]=X0LL[j];
cLHt01[i][j]=X0LH[j];
cHLt01[i][j]=X0HL[j];
cHHt01[i][j]=X0HH[j];
cLLt0[i][j]=X0LL[j]+X0LH[j]+X0HL[j]+X0HH[j];
}
}
for (int j=0; j< colsLL;j++) {
vector<double> sigLL0;
vector<double> sigLH0;
vector<double> sigHL0;
vector<double> sigHH0;
for (int i=0; i < rowsLL; i++) {
double temp01 = cLL01[i][j];
double temp02 = cLH01[i][j];
double temp03 = cHL01[i][j];
double temp04 = cHH01[i][j];
sigLL0.push_back(temp01);
sigLH0.push_back(temp02);
sigHL0.push_back(temp03);
sigHH0.push_back(temp04);
}
// First Convolution Step for LL,LH,HL,HH
vector<double> X0LL;
convfft(sigLL0, low_pass, X0LL);
vector<double> X0LH;
convfft(sigLH0, low_pass, X0LH);
vector<double> X0HL;
convfft(sigHL0, high_pass, X0HL);
vector<double> X0HH;
convfft(sigHH0, high_pass, X0HH);
int lent=X0LL.size();
for (int i=0; i < lent; i++) {
cLLt10[i][j]=X0LL[i];
cLHt10[i][j]=X0LH[i];
cHLt10[i][j]=X0HL[i];
cHHt10[i][j]=X0HH[i];
}
}
for (int i=0; i< rowsLL + lf-1;i++) {
vector<double> sigLL0;
vector<double> sigLH0;
vector<double> sigHL0;
vector<double> sigHH0;
for (int j=0; j < colsLL; j++) {
double temp01 = cLLt10[i][j];
double temp02 = cLHt10[i][j];
double temp03 = cHLt10[i][j];
double temp04 = cHHt10[i][j];
sigLL0.push_back(temp01);
sigLH0.push_back(temp02);
sigHL0.push_back(temp03);
sigHH0.push_back(temp04);
}
// Second Convolution Step for LL,LH,HL,HH
vector<double> X0LL;
convfft(sigLL0, low_pass, X0LL);
vector<double> X0LH;
convfft(sigLH0, high_pass, X0LH);
vector<double> X0HL;
convfft(sigHL0, low_pass, X0HL);
vector<double> X0HH;
convfft(sigHH0, high_pass, X0HH);
int lent=X0LL.size();
for (int j=0; j < lent; j++) {
cLLt11[i][j]=X0LL[j];
cLHt11[i][j]=X0LH[j];
cHLt11[i][j]=X0HL[j];
cHHt11[i][j]=X0HH[j];
cLLt1[i][j]=X0LL[j]+X0LH[j]+X0HL[j]+X0HH[j];
}
}
for (int i=0; i < len_r;i++) {
for (int j=0; j < len_c;j++) {
oupt0[i][j]=cLLt0[lf+i-1][lf+j-1];
oupt1[i][j]=cLLt1[lf+i-1][lf+j-1];
}
}
circshift2d(oupt1,-1,-1);
for (int i=0; i < len_r;i++) {
for (int j=0; j < len_c;j++) {
double temp=oupt0[i][j]+oupt1[i][j];
temp=temp/2.0;
oupt[i][j]=temp;
}
}
row1 = 0;
col1 = 0;
cout << rows_n << cols_n << " " << oupt.size() << oupt[0].size() << endl;
for (int index_r = count; index_r < rows_n; index_r+=value){
for (int index_c = count; index_c < cols_n; index_c+=value){
cLL[index_r][index_c]=oupt[row1][col1];
col1++;
}
col1 = 0;
row1++;
}
cout << "STAGE 14" << endl;
}
if (iter == 1) {
sig=cLL;
}
}
return 0;
}
void stereoCalibThread::stereoCalibRun()
{
imageL=new ImageOf<PixelRgb>;
imageR=new ImageOf<PixelRgb>;
Stamp TSLeft;
Stamp TSRight;
bool initL=false;
bool initR=false;
int count=1;
Size boardSize, imageSize;
boardSize.width=this->boardWidth;
boardSize.height=this->boardHeight;
while (!isStopping()) {
ImageOf<PixelRgb> *tmpL = imagePortInLeft.read(false);
ImageOf<PixelRgb> *tmpR = imagePortInRight.read(false);
if(tmpL!=NULL)
{
*imageL=*tmpL;
imagePortInLeft.getEnvelope(TSLeft);
initL=true;
}
if(tmpR!=NULL)
{
*imageR=*tmpR;
imagePortInRight.getEnvelope(TSRight);
initR=true;
}
if(initL && initR && checkTS(TSLeft.getTime(),TSRight.getTime())){
bool foundL=false;
bool foundR=false;
mutex->wait();
if(startCalibration>0) {
string pathImg=imageDir;
preparePath(pathImg.c_str(), pathL,pathR,count);
string iml(pathL);
string imr(pathR);
imgL= (IplImage*) imageL->getIplImage();
imgR= (IplImage*) imageR->getIplImage();
Mat Left(imgL);
Mat Right(imgR);
std::vector<Point2f> pointbufL;
std::vector<Point2f> pointbufR;
if(boardType == "CIRCLES_GRID") {
foundL = findCirclesGridDefault(Left, boardSize, pointbufL, CALIB_CB_SYMMETRIC_GRID | CALIB_CB_CLUSTERING);
foundR = findCirclesGridDefault(Right, boardSize, pointbufR, CALIB_CB_SYMMETRIC_GRID | CALIB_CB_CLUSTERING);
} else if(boardType == "ASYMMETRIC_CIRCLES_GRID") {
foundL = findCirclesGridDefault(Left, boardSize, pointbufL, CALIB_CB_ASYMMETRIC_GRID | CALIB_CB_CLUSTERING);
foundR = findCirclesGridDefault(Right, boardSize, pointbufR, CALIB_CB_ASYMMETRIC_GRID | CALIB_CB_CLUSTERING);
} else {
foundL = findChessboardCorners( Left, boardSize, pointbufL, CV_CALIB_CB_ADAPTIVE_THRESH | CV_CALIB_CB_FAST_CHECK | CV_CALIB_CB_NORMALIZE_IMAGE);
foundR = findChessboardCorners( Right, boardSize, pointbufR, CV_CALIB_CB_ADAPTIVE_THRESH | CV_CALIB_CB_FAST_CHECK | CV_CALIB_CB_NORMALIZE_IMAGE);
}
if(foundL && foundR) {
cvCvtColor(imgL,imgL,CV_RGB2BGR);
cvCvtColor(imgR,imgR, CV_RGB2BGR);
saveStereoImage(pathImg.c_str(),imgL,imgR,count);
imageListR.push_back(imr);
imageListL.push_back(iml);
imageListLR.push_back(iml);
imageListLR.push_back(imr);
Mat cL(pointbufL);
Mat cR(pointbufR);
drawChessboardCorners(Left, boardSize, cL, foundL);
drawChessboardCorners(Right, boardSize, cR, foundR);
count++;
}
if(count>numOfPairs) {
fprintf(stdout," Running Left Camera Calibration... \n");
monoCalibration(imageListL,this->boardWidth,this->boardHeight,this->Kleft,this->DistL);
fprintf(stdout," Running Right Camera Calibration... \n");
monoCalibration(imageListR,this->boardWidth,this->boardHeight,this->Kright,this->DistR);
stereoCalibration(imageListLR, this->boardWidth,this->boardHeight,this->squareSize);
fprintf(stdout," Saving Calibration Results... \n");
updateIntrinsics(imgL->width,imgL->height,Kright.at<double>(0,0),Kright.at<double>(1,1),Kright.at<double>(0,2),Kright.at<double>(1,2),DistR.at<double>(0,0),DistR.at<double>(0,1),DistR.at<double>(0,2),DistR.at<double>(0,3),"CAMERA_CALIBRATION_RIGHT");
updateIntrinsics(imgL->width,imgL->height,Kleft.at<double>(0,0),Kleft.at<double>(1,1),Kleft.at<double>(0,2),Kleft.at<double>(1,2),DistL.at<double>(0,0),DistL.at<double>(0,1),DistL.at<double>(0,2),DistL.at<double>(0,3),"CAMERA_CALIBRATION_LEFT");
Mat Rot=Mat::eye(3,3,CV_64FC1);
Mat Tr=Mat::zeros(3,1,CV_64FC1);
updateExtrinsics(this->R,this->T,"STEREO_DISPARITY");
fprintf(stdout, "Calibration Results Saved in %s \n", camCalibFile.c_str());
startCalibration=0;
count=1;
imageListR.clear();
imageListL.clear();
imageListLR.clear();
}
}
mutex->post();
ImageOf<PixelRgb>& outimL=outPortLeft.prepare();
outimL=*imageL;
outPortLeft.write();
ImageOf<PixelRgb>& outimR=outPortRight.prepare();
outimR=*imageR;
outPortRight.write();
if(foundL && foundR && startCalibration==1)
Time::delay(2.0);
initL=initR=false;
cout.flush();
}
}
delete imageL;
delete imageR;
}
extern "C" SEXP LC_CoxPH(SEXP R_L, SEXP R_R, SEXP R_x,
SEXP R_b, SEXP R_actInds, SEXP move_X, SEXP AugL, SEXP AugR, SEXP covariates,
SEXP R_LMAXIND, SEXP R_RMININD, SEXP R_MAXUNCENSBETAIND,
SEXP R_MAXUNCENSOBSIND, SEXP R_HASUNCENSORED){
bool c_move_x = LOGICAL(move_X)[0] == TRUE;
int n = LENGTH(R_L);
int k = LENGTH(R_x);
vector<double> x(k);
vector<double> b(k);
for(int i = 0; i < k; i++){
x[i] = REAL(R_x)[i];
b[i] = REAL(R_b)[i];
}
vector<int> cL(n);
vector<int> cR(n);
vector<double> cRepVec(n);
bool leftDone,rightDone;
double leftval, rightval;
for(int i = 0; i < n; i++){
leftval = REAL(R_L)[i];
rightval = REAL(R_R)[i];
leftDone = false;
rightDone = false;
for(int j = 0; j < k; j++){
if(leftDone == false){
if(leftval == x[j]){
cL[i] = j;
leftDone = true;
}
}
if(rightDone == false){
if(rightval == x[j]){
cR[i] = j;
rightDone = true;
}
}
}
if(rightDone == false || leftDone == false){
Rprintf("problem: data does not match x! Quiting\n");
return(R_NilValue);
}
}
SEXP dims = getAttrib(covariates, R_DimSymbol);
PROTECT(dims);
int num_cov = INTEGER(dims)[1];
int cov_n = INTEGER(dims)[0];
UNPROTECT(1);
if(cov_n != n){
Rprintf("Number of covariates does not match number of observations\n");
return(R_NilValue);
}
QuadProgPP::Matrix<double> Covars(cov_n,num_cov);
for(int i = 0; i < cov_n; i++){
for(int j = 0; j < num_cov; j++)
Covars[i][j] = REAL(covariates)[i + j*cov_n];
}
int ak = LENGTH(R_actInds);
vector<int> actInds(ak);
for(int i = 0; i < ak; i++)
actInds[i] = INTEGER(R_actInds)[i] - 1;
double augl = REAL(AugL)[0];
double augr = REAL(AugR)[0];
int LMAXIND = INTEGER(R_LMAXIND)[0] - 1;
int RMININD = INTEGER(R_RMININD)[0] - 1;
double LLAGRMIN = REAL(AugL)[0];
double RLAGRMIN = REAL(AugR)[0];
int MAXUNCENSBETAIND = INTEGER(R_MAXUNCENSBETAIND)[0] - 1;
int MAXUNCENSOBSIND = INTEGER(R_MAXUNCENSOBSIND)[0] - 1;
bool HASUNCENSORED = LOGICAL(R_HASUNCENSORED)[0] == TRUE;
LogConCenPH optObj(0, x, b, cL, cR, actInds, c_move_x, augl, augr, Covars, num_cov,
LMAXIND, RMININD, LLAGRMIN, RLAGRMIN, MAXUNCENSBETAIND, MAXUNCENSOBSIND, HASUNCENSORED);
optObj.updateNu();
optObj.updateRegress();
cout << "starting llk = " <<optObj.llk() << "\n";
optObj.VEMstep();
optObj.ICMstep();
cout << "after VEM and ICM, llk = " << optObj.llk() << "\n";
cout << "k = " << k << " actInds[0] = " << optObj.actIndex[0] << " actInds[getAK()-1] = " << optObj.actIndex[optObj.getAK()-1] << "\n";
// double old_llk = R_NegInf;
// double new_llk = optObj.llk();
int inner_it = 0;
int outer_it = 0;
int max_inner_its = 2;
int max_outer_its = 2;
double outer_tol = pow(10.0, -10);
double inner_tol = pow(10.0, -12);
double outer_error = outer_tol + 1;
double reg_error = outer_tol + 1;
double inner_error;
double inner_llk, outer_llk;
int loopcount = 0;
bool start_move_x = false;
Rprintf("Reminder to Cliff: move_x turned off until updates\n");
while( (outer_it < max_outer_its) && (loopcount < 2)){
outer_it++;
outer_llk = optObj.llk();
cout << "beginning loop llk = " << outer_llk << "\n";
optObj.VEMstep();
cout << "after VEMstep, llk = " << optObj.llk() << "\n";
inner_error = inner_tol + 1;
reg_error = outer_tol + 1;
inner_it = 0;
while(inner_it < max_inner_its && inner_error > inner_tol){
inner_it++;
// if(start_move_x && c_move_x)
// optObj.updateXs();
if(reg_error > outer_tol){
reg_error = optObj.updateRegress();
cout << "after updateRegress step, llk = " << optObj.llk() << "\n";
}
inner_llk = optObj.llk();
optObj.ICMstep();
cout << "after ICM step, llk = " << optObj.llk() << "\n";
inner_error = optObj.llk() - inner_llk;
if(inner_error != inner_error)
break;
}
cout << "k = " << k << " actInds[0] = " << optObj.actIndex[0] << " actInds[getAK()-1] = " << optObj.actIndex[optObj.getAK()-1] << "\n";
cout << "before recenterBeta, llk = " << optObj.llk() <<"\n";
optObj.recenterBeta();
cout << "after recenterBeta, llk = " << optObj.llk() << "\n";
outer_error = optObj.llk() - outer_llk;
if(outer_error != outer_error){
Rprintf("Warning: undefined error. Algorithm terminated at invalid estimate. Please contact Clifford Anderson-Bergman with this dataset!\n");
break;
}
if(outer_error < 0.0001)
start_move_x = true;
if(outer_error < outer_tol){
loopcount++;
}
else
loopcount = 0;
}
optObj.makePropDist();
ak = optObj.getAK();
SEXP output = PROTECT(allocVector(VECSXP, 5) );
SEXP regressP = PROTECT(allocVector(REALSXP, num_cov) );
for(int i = 0; i < num_cov; i++)
REAL(regressP)[i] = optObj.cov_b[i];
SEXP x_out = PROTECT(allocVector(REALSXP, ak) );
SEXP b_out = PROTECT(allocVector(REALSXP, ak) );
SEXP llk_out = PROTECT(allocVector(REALSXP, 1) );
for(int i = 0; i < ak; i++){
REAL(x_out)[i] = optObj.x[optObj.actIndex[i]];
REAL(b_out)[i] = optObj.b[optObj.actIndex[i]];
}
int totValues = num_cov + ak;
SEXP Hess_out = PROTECT(allocMatrix(REALSXP, totValues, totValues));
for(int i = 0; i < totValues; i++){
for(int j = 0; j <= i; j++){
REAL(Hess_out)[i + totValues * j] = optObj.partialDerCovOrBase(i,j);
REAL(Hess_out)[j + totValues * i] = REAL(Hess_out)[i + totValues* j];
}
}
REAL(llk_out)[0] = optObj.llk();
SET_VECTOR_ELT(output, 0, x_out);
SET_VECTOR_ELT(output, 1, b_out);
SET_VECTOR_ELT(output, 2, llk_out);
SET_VECTOR_ELT(output, 3, regressP);
SET_VECTOR_ELT(output, 4, Hess_out);
UNPROTECT(6);
return(output);
}
