void lincg(PyramidT& pyramid, PyramidT& pC,
const Array2Df& b, Array2Df& x,
const int itmax, const float tol,
Progress &ph)
{
float rdotr_curr;
float rdotr_prev;
float rdotr_best;
float alpha;
float beta;
const size_t rows = pyramid.getRows();
const size_t cols = pyramid.getCols();
const size_t n = rows*cols;
const float tol2 = tol*tol;
Array2Df x_best(cols, rows);
Array2Df r(cols, rows);
Array2Df p(cols, rows);
Array2Df Ap(cols, rows);
// bnrm2 = ||b||
const float bnrm2 = utils::dotProduct(b.data(), n);
// r = b - Ax
multiplyA(pyramid, pC, x, r); // r = A x
utils::vsub(b.data(), r.data(), r.data(), n); // r = b - r
// rdotr = r.r
rdotr_best = rdotr_curr = utils::dotProduct(r.data(), n);
// Setup initial vector
std::copy(r.begin(), r.end(), p.begin()); // p = r
std::copy(x.begin(), x.end(), x_best.begin()); // x_best = x
const float irdotr = rdotr_curr;
const float percent_sf = 100.0f/std::log(tol2*bnrm2/irdotr);
int iter = 0;
int num_backwards = 0;
for (; iter < itmax; ++iter)
{
// TEST
ph.setValue(
static_cast<int>(std::log(rdotr_curr/irdotr)*percent_sf)
);
// User requested abort
if ( ph.canceled() && iter > 0 )
{
break;
}
// Ap = A p
multiplyA(pyramid, pC, p, Ap);
// alpha = r.r / (p . Ap)
alpha = rdotr_curr / utils::dotProduct(p.data(), Ap.data(), n);
// r = r - alpha Ap
utils::vsubs(r.data(), alpha, Ap.data(), r.data(), n);
// rdotr = r.r
rdotr_prev = rdotr_curr;
rdotr_curr = utils::dotProduct(r.data(), n);
// Have we gone unstable?
if (rdotr_curr > rdotr_prev)
{
// Save where we've got to
if (num_backwards == 0 && rdotr_prev < rdotr_best)
{
rdotr_best = rdotr_prev;
std::copy(x.begin(), x.end(), x_best.begin());
}
num_backwards++;
}
else
{
num_backwards = 0;
}
// x = x + alpha * p
utils::vadds(x.data(), alpha, p.data(), x.data(), n);
// Exit if we're done
// fprintf(stderr, "iter:%d err:%f\n", iter+1, sqrtf(rdotr/bnrm2));
if (rdotr_curr/bnrm2 < tol2)
break;
if (num_backwards > NUM_BACKWARDS_CEILING)
{
// Reset
num_backwards = 0;
std::copy(x_best.begin(), x_best.end(), x.begin());
// r = Ax
multiplyA(pyramid, pC, x, r);
// r = b - r
utils::vsub(b.data(), r.data(), r.data(), n);
// rdotr = r.r
rdotr_best = rdotr_curr = utils::dotProduct(r.data(), r.size());
// p = r
std::copy(r.begin(), r.end(), p.begin());
}
else
{
// p = r + beta * p
beta = rdotr_curr/rdotr_prev;
utils::vadds(r.data(), beta, p.data(), p.data(), n);
}
}
// Use the best version we found
if (rdotr_curr > rdotr_best)
{
rdotr_curr = rdotr_best;
std::copy(x_best.begin(), x_best.end(), x.begin());
}
if (rdotr_curr/bnrm2 > tol2)
{
// Not converged
ph.setValue(
static_cast<int>(std::log(rdotr_curr/irdotr)*percent_sf)
);
if (iter == itmax)
{
std::cerr << std::endl << "pfstmo_mantiuk06: Warning: Not "\
"converged (hit maximum iterations), error = "
<< std::sqrt(rdotr_curr/bnrm2)
<< " (should be below " << tol <<")"
<< std::endl;
}
else
{
std::cerr << std::endl
<< "pfstmo_mantiuk06: Warning: Not converged "\
"(going unstable), error = "
<< std::sqrt(rdotr_curr/bnrm2)
<< " (should be below " << tol << ")"
<< std::endl;
}
}
else
{
ph.setValue( itmax );
}
}
