/// Given an indefinite matrix w.A, find a shift sigma
/// such that (A + sigma I) is positive definite.
/// @param sigma :: The result (shift).
/// @param d :: A solution vector to the system of linear equations
/// with the found positive defimnite matrix. The RHS vector is -w.v.
/// @param options :: The options.
/// @param inform :: The inform struct.
/// @param w :: The work struct.
void get_pd_shift(double &sigma, DoubleFortranVector &d,
const nlls_options &options, nlls_inform &inform,
more_sorensen_work &w) {
int no_shifts = 0;
bool successful_shift = false;
while (!successful_shift) {
shift_matrix(w.A, sigma, w.AplusSigma);
solve_spd(w.AplusSigma, negative(w.v), w.LtL, d, inform);
if (inform.status != NLLS_ERROR::OK) {
// reset the error calls -- handled in the code....
inform.status = NLLS_ERROR::OK;
inform.external_return = 0;
inform.external_name = "";
no_shifts = no_shifts + 1;
if (no_shifts == 10) { // too many shifts -- exit
inform.status = NLLS_ERROR::MS_TOO_MANY_SHIFTS;
return;
}
sigma = sigma + (pow(10.0, no_shifts)) * options.more_sorensen_shift;
} else {
successful_shift = true;
}
}
}
// least-squares fit a 3rd degree RPC model to the given data values in 3D
long double rpcfit33(
long double p[20], // output p coefficients
long double q[20], // output q coefficients (q[0] = 1)
long double (*x)[3], // input data positions
long double *f, // input data values
int n // number of input points
)
{
// Input: n data points/values (x,f)
// Output: a rational function p/q of degree 3
// Algorithm: minimize E(p,q) = w(x)|p(x)-q(x)f|^2
// iteratively reweighted by w=1/q^2
//
// Note: the minimization problem is linear, symmetric
// and positive definite. The solution is not 0 since
// the zero-degree coefficient of q is fixed at 1.
// Thus the system has size 39x39.
//
// Note: this algorithm fails in two cases
// 1) when the data points fit exactly to a rational function of lower
// degree (e.g., a pinhole camera)
// 2) when there are not enough data points
// In these cases the matrix of the linear problem is degenerate and
// the current solver fails, giving all-NAN, parameters.
//
// initialize q
q[0] = 1;
for (int i = 1; i < 20; i++)
q[i] = 0;
// build matrix "M" of evaluated monomials
long double M[n][39];
for (int i = 0; i < n; i++)
{
eval_mon20(M[i], x[i]);
for (int j = 1; j < 20; j++)
M[i][19+j] = -f[i] * M[i][j];
}
long double best_error_so_far = INFINITY;
int best_iteration_so_far = -1;
int number_of_iterations = 13;
for (int iteration = 0; iteration < number_of_iterations; iteration++)
{
// vector "w" of weights
long double w[n];
for (int i = 0; i < n; i++)
w[i] = pow( eval_pol20l(q, x[i]), -2);
// matrix "A" of the system
long double A[39][39] = {{0}};
for (int i = 0; i < 39; i++)
for (int j = 0; j < 39; j++)
for (int k = 0; k < n ; k++)
A[i][j] += w[k] * M[k][i] * M[k][j];
// vector "b"
long double b[39] = {0};
for (int i = 0; i < 39; i++)
for (int j = 0; j < n ; j++)
b[i] += w[j] * f[j] * M[j][i];
// solve the linear problem
long double pq[39];
solve_spd(pq, (void*)A, b, 39);
// update if it improves error
long double e = rpc33_errorpq(pq, x, f, n);
fprintf(stderr, "\terru = %Lg\n", e);
if (e < best_error_so_far)
{
best_error_so_far = e;
best_iteration_so_far = iteration;
for (int i = 0; i < 20; i++)
p[i] = pq[i];
for (int i = 1; i < 20; i++)
q[i] = pq[19+i];
}
}
fprintf(stderr, "best_iteration = %d\n", best_iteration_so_far);
fprintf(stderr, "best_error = %Lg\n", best_error_so_far);
return best_error_so_far;
}
/// Solve the trust-region subproblem using
/// the method of More and Sorensen
///
/// Using the implementation as in Algorithm 7.3.6
/// of Trust Region Methods
///
/// main output d, the soln to the TR subproblem
/// @param J :: The Jacobian.
/// @param f :: The residuals.
/// @param hf :: The Hessian (sort of).
/// @param Delta :: The raduis of the trust region.
/// @param d :: The output vector of corrections to the parameters giving the
/// solution to the TR subproblem.
/// @param nd :: The 2-norm of d.
/// @param options :: The options.
/// @param inform :: The inform struct.
/// @param w :: The work struct.
void more_sorensen(const DoubleFortranMatrix &J, const DoubleFortranVector &f,
const DoubleFortranMatrix &hf, double Delta,
DoubleFortranVector &d, double &nd,
const nlls_options &options, nlls_inform &inform,
more_sorensen_work &w) {
// The code finds
// d = arg min_p v^T p + 0.5 * p^T A p
// s.t. ||p|| \leq Delta
//
// set A and v for the model being considered here...
// Set A = J^T J
matmult_inner(J, w.A);
// add any second order information...
// so A = J^T J + HF
w.A += hf;
// now form v = J^T f
mult_Jt(J, f, w.v);
// if scaling needed, do it
if (options.scale != 0) {
apply_scaling(J, w.A, w.v, w.apply_scaling_ws, options, inform);
}
auto n = J.len2();
auto scale_back = [n, &d, &options, &w]() {
if (options.scale != 0) {
for (int i = 1; i <= n; ++i) {
d(i) = d(i) / w.apply_scaling_ws.diag(i);
}
}
};
auto local_ms_shift = options.more_sorensen_shift;
// d = -A\v
DoubleFortranVector negv = w.v;
negv *= -1.0;
solve_spd(w.A, negv, w.LtL, d, inform);
double sigma = 0.0;
if (inform.status == NLLS_ERROR::OK) {
// A is symmetric positive definite....
sigma = zero;
} else {
// reset the error calls -- handled in the code....
inform.status = NLLS_ERROR::OK;
inform.external_return = 0;
inform.external_name = "";
min_eig_symm(w.A, sigma, w.y1);
if (inform.status != NLLS_ERROR::OK) {
scale_back();
return;
}
sigma = -(sigma - local_ms_shift);
// find a shift that makes (A + sigma I) positive definite
get_pd_shift(sigma, d, options, inform, w);
if (inform.status != NLLS_ERROR::OK) {
scale_back();
return;
}
}
nd = norm2(d);
if (boost::math::isnan(nd) || boost::math::isinf(nd)) {
inform.status = NLLS_ERROR::NAN_OR_INF;
throw std::runtime_error("Step is NaN or infinite.");
}
// now, we're not in the trust region initally, so iterate....
auto sigma_shift = zero;
int no_restarts = 0;
// set 'small' in the context of the algorithm
double epsilon =
std::max(options.more_sorensen_tol * Delta, options.more_sorensen_tiny);
int it = 1;
for (; it <= options.more_sorensen_maxits; ++it) {
if (nd <= Delta + epsilon) {
// we're within the tr radius
if (fabs(sigma) < options.more_sorensen_tiny ||
fabs(nd - Delta) < epsilon) {
// we're good....exit
break;
}
if (w.y1.len() == n) {
double alpha = 0.0;
findbeta(d, w.y1, Delta, alpha, inform);
if (inform.status == NLLS_ERROR::OK) {
DoubleFortranVector tmp = w.y1;
tmp *= alpha;
d += tmp;
}
}
// also good....exit
break;
}
// w.q = R'\d
// DTRSM( "Left", "Lower", "No Transpose", "Non-unit", n, 1, one, w.LtL, n,
// w.q, n );
for (int j = 1; j <= w.LtL.len1(); ++j) {
for (int k = j + 1; k <= w.LtL.len1(); ++k) {
w.LtL(j, k) = 0.0;
}
}
w.LtL.solve(d, w.q);
auto nq = norm2(w.q);
sigma_shift = (pow((nd / nq), 2)) * ((nd - Delta) / Delta);
if (fabs(sigma_shift) < options.more_sorensen_tiny * fabs(sigma)) {
if (no_restarts < 1) {
// find a shift that makes (A + sigma I) positive definite
get_pd_shift(sigma, d, options, inform, w);
if (inform.status != NLLS_ERROR::OK) {
break;
}
no_restarts = no_restarts + 1;
} else {
// we're not going to make progress...jump out
inform.status = NLLS_ERROR::MS_NO_PROGRESS;
break;
}
} else {
sigma = sigma + sigma_shift;
}
shift_matrix(w.A, sigma, w.AplusSigma);
DoubleFortranVector negv = w.v;
negv *= -1.0;
solve_spd(w.AplusSigma, negv, w.LtL, d, inform);
if (inform.status != NLLS_ERROR::OK) {
break;
}
nd = norm2(d);
}
if (it == options.more_sorensen_maxits) {
// maxits reached, not converged
inform.status = NLLS_ERROR::MS_MAXITS;
}
scale_back();
}
