/* Give a least-squares solution to the system of linear equations given in
the jacobian and errvec arrays. Return result in solution.
This uses the method of solving the normal equations.
*/
void SolveLeastSquares(double *solution, int rows, int cols, double **jacobian,
double *errvec, double **sqarray)
{
int r, c, i;
double sum;
assert(rows >= cols);
/* Multiply Jacobian transpose by Jacobian, and put result in sqarray. */
for (r = 0; r < cols; r++)
for (c = 0; c < cols; c++) {
sum = 0.0;
for (i = 0; i < rows; i++)
sum += jacobian[i][r] * jacobian[i][c];
sqarray[r][c] = sum;
}
/* Multiply transpose of Jacobian by errvec, and put result in solution. */
for (c = 0; c < cols; c++) {
sum = 0.0;
for (i = 0; i < rows; i++)
sum += jacobian[i][c] * errvec[i];
solution[c] = sum;
}
/* Now, solve square system of equations. */
SolveLinearSystem(solution, sqarray, cols);
}
/* Apply the method developed by Matthew Brown (see BMVC 02 paper) to
fit a 3D quadratic function through the DOG function values around
the location (s,r,c), i.e., (scale,row,col), at which a peak has
been detected. Return the interpolated peak position by setting
the vector "offset", which gives offset from position (s,r,c). The
returned function value is the interpolated DOG magnitude at this peak.
*/
float FitQuadratic(float offset[3], Image *dogs, int s, int r, int c)
{
float g[3], **dog0, **dog1, **dog2;
static float **H = NULL;
/* First time through, allocate space for Hessian matrix, H. */
if (H == NULL)
H = AllocMatrix(3, 3, PERM_POOL);
/* Select the dog images at peak scale, dog1, as well as the scale
below, dog0, and scale above, dog2.
*/
dog0 = dogs[s-1]->pixels;
dog1 = dogs[s]->pixels;
dog2 = dogs[s+1]->pixels;
/* Fill in the values of the gradient from pixel differences. */
g[0] = (dog2[r][c] - dog0[r][c]) / 2.0;
g[1] = (dog1[r+1][c] - dog1[r-1][c]) / 2.0;
g[2] = (dog1[r][c+1] - dog1[r][c-1]) / 2.0;
/* Fill in the values of the Hessian from pixel differences. */
H[0][0] = dog0[r][c] - 2.0 * dog1[r][c] + dog2[r][c];
H[1][1] = dog1[r-1][c] - 2.0 * dog1[r][c] + dog1[r+1][c];
H[2][2] = dog1[r][c-1] - 2.0 * dog1[r][c] + dog1[r][c+1];
H[0][1] = H[1][0] = ((dog2[r+1][c] - dog2[r-1][c]) -
(dog0[r+1][c] - dog0[r-1][c])) / 4.0;
H[0][2] = H[2][0] = ((dog2[r][c+1] - dog2[r][c-1]) -
(dog0[r][c+1] - dog0[r][c-1])) / 4.0;
H[1][2] = H[2][1] = ((dog1[r+1][c+1] - dog1[r+1][c-1]) -
(dog1[r-1][c+1] - dog1[r-1][c-1])) / 4.0;
/* Solve the 3x3 linear sytem, Hx = -g. Result gives peak offset.
Note that SolveLinearSystem destroys contents of H.
*/
offset[0] = - g[0];
offset[1] = - g[1];
offset[2] = - g[2];
SolveLinearSystem(offset, H, 3);
/* Also return value of DOG at peak location using initial value plus
0.5 times linear interpolation with gradient to peak position
(this is correct for a quadratic approximation).
*/
return (dog1[r][c] + 0.5 * DotProd(offset, g, 3));
}
void BackwardEulerIvpOdeSolver::CalculateNextYValue(AbstractOdeSystem* pAbstractOdeSystem,
double timeStep,
double time,
std::vector<double>& rCurrentYValues,
std::vector<double>& rNextYValues)
{
// Check the size of the ODE system matches the solvers expected
assert(mSizeOfOdeSystem == pAbstractOdeSystem->GetNumberOfStateVariables());
unsigned counter = 0;
// const double eps = 1e-6 * rCurrentGuess[0]; // Our tolerance (should use min(guess) perhaps?)
const double eps = 1e-6; // JonW tolerance
double norm = 2*eps;
std::vector<double> current_guess(mSizeOfOdeSystem);
current_guess.assign(rCurrentYValues.begin(), rCurrentYValues.end());
while (norm > eps)
{
// Calculate Jacobian and mResidual for current guess
ComputeResidual(pAbstractOdeSystem, timeStep, time, rCurrentYValues, current_guess);
ComputeJacobian(pAbstractOdeSystem, timeStep, time, rCurrentYValues, current_guess);
// // Update norm (our style)
// norm = ComputeNorm(mResidual);
// Solve Newton linear system
SolveLinearSystem();
// Update norm (JonW style)
norm = ComputeNorm(mUpdate);
// Update current guess
for (unsigned i=0; i<mSizeOfOdeSystem; i++)
{
current_guess[i] -= mUpdate[i];
}
counter++;
assert(counter < 20); // avoid infinite loops
}
rNextYValues.assign(current_guess.begin(), current_guess.end());
}
int CalcSphereCenter (const Point<3> ** pts, Point<3> & c)
{
Vec3d row1 (*pts[0], *pts[1]);
Vec3d row2 (*pts[0], *pts[2]);
Vec3d row3 (*pts[0], *pts[3]);
Vec3d rhs(0.5 * (row1*row1),
0.5 * (row2*row2),
0.5 * (row3*row3));
Transpose (row1, row2, row3);
Vec3d sol;
if (SolveLinearSystem (row1, row2, row3, rhs, sol))
{
(*testout) << "CalcSphereCenter: degenerated" << endl;
return 1;
}
c = *pts[0] + sol;
return 0;
}
/**
* Use Newton's method to solve the given cell for the next timestep.
*
* @param rCell the cell to solve
* @param time the current time
* @param rCurrentGuess the current guess at a solution. Will be updated on exit.
*/
void Solve(CELLTYPE &rCell,
double time,
double rCurrentGuess[SIZE])
{
unsigned counter = 0;
const double eps = 1e-6; // JonW tolerance
// check that the initial guess that was given gives a valid residual
rCell.ComputeResidual(time, rCurrentGuess, mResidual.data());
double norm_of_residual = norm_inf(mResidual);
assert(!std::isnan(norm_of_residual));
double norm_of_update = 0.0; //Properly initialised in the loop
do
{
// Calculate Jacobian for current guess
rCell.ComputeJacobian(time, rCurrentGuess, mJacobian);
// Solve Newton linear system for mUpdate, given mJacobian and mResidual
SolveLinearSystem();
// Update norm (JonW style)
norm_of_update = norm_inf(mUpdate);
// Update current guess and recalculate residual
for (unsigned i=0; i<SIZE; i++)
{
rCurrentGuess[i] -= mUpdate[i];
}
double norm_of_previous_residual = norm_of_residual;
rCell.ComputeResidual(time, rCurrentGuess, mResidual.data());
norm_of_residual = norm_inf(mResidual);
if (norm_of_residual > norm_of_previous_residual && norm_of_update > eps)
{
//Second part of guard:
//Note that if norm_of_update < eps (converged) then it's
//likely that both the residual and the previous residual were
//close to the root.
//Work out where the biggest change in the guess has happened.
double relative_change_max = 0.0;
unsigned relative_change_direction = 0;
for (unsigned i=0; i<SIZE; i++)
{
double relative_change = fabs(mUpdate[i]/rCurrentGuess[i]);
if (relative_change > relative_change_max)
{
relative_change_max = relative_change;
relative_change_direction = i;
}
}
if (relative_change_max > 1.0)
{
//Only walk 0.2 of the way in that direction (put back 0.8)
rCurrentGuess[relative_change_direction] += 0.8*mUpdate[relative_change_direction];
rCell.ComputeResidual(time, rCurrentGuess, mResidual.data());
norm_of_residual = norm_inf(mResidual);
WARNING("Residual increasing and one direction changing radically - back tracking in that direction");
}
}
counter++;
// avoid infinite loops
if (counter > 15)
{
#define COVERAGE_IGNORE
EXCEPTION("Newton method diverged in CardiacNewtonSolver::Solve()");
#undef COVERAGE_IGNORE
}
}
while (norm_of_update > eps);
#define COVERAGE_IGNORE
#ifndef NDEBUG
if (norm_of_residual > 2e-10)
{ //This line is for correlation - in case we use norm_of_residual as convergence criterion
WARN_ONCE_ONLY("Newton iteration terminated because update vector norm is small, but residual norm is not small.");
}
#endif // NDEBUG
#undef COVERAGE_IGNORE
}
