template <typename PointT> void
pcl::SampleConsensusModelCircle2D<PointT>::optimizeModelCoefficients (
const std::vector<int> &inliers, const Eigen::VectorXf &model_coefficients, Eigen::VectorXf &optimized_coefficients)
{
boost::mutex::scoped_lock lock (tmp_mutex_);
const int n_unknowns = 3; // 3 unknowns
// Needs a set of valid model coefficients
if (model_coefficients.size () != n_unknowns)
{
PCL_ERROR ("[pcl::SampleConsensusModelCircle2D::optimizeModelCoefficients] Invalid number of model coefficients given (%lu)!\n", (unsigned long)model_coefficients.size ());
optimized_coefficients = model_coefficients;
return;
}
// Need at least 3 samples
if (inliers.size () <= 3)
{
PCL_ERROR ("[pcl::SampleConsensusModelCircle2D::optimizeModelCoefficients] Not enough inliers found to support a model (%lu)! Returning the same coefficients.\n", (unsigned long)inliers.size ());
optimized_coefficients = model_coefficients;
return;
}
tmp_inliers_ = &inliers;
int m = inliers.size ();
double *fvec = new double[m];
int iwa[n_unknowns];
int lwa = m * n_unknowns + 5 * n_unknowns + m;
double *wa = new double[lwa];
// Set the initial solution
double x[n_unknowns];
for (int d = 0; d < n_unknowns; ++d)
x[d] = model_coefficients[d]; // initial guess
// Set tol to the square root of the machine. Unless high solutions are required, these are the recommended settings.
double tol = sqrt (dpmpar (1));
// Optimize using forward-difference approximation LM
int info = lmdif1 (&pcl::SampleConsensusModelCircle2D<PointT>::functionToOptimize, this, m, n_unknowns, x, fvec, tol, iwa, wa, lwa);
// Compute the L2 norm of the residuals
PCL_DEBUG ("[pcl::SampleConsensusModelCircle2D::optimizeModelCoefficients] LM solver finished with exit code %i, having a residual norm of %g. \nInitial solution: %g %g %g \nFinal solution: %g %g %g\n",
info, enorm (m, fvec), model_coefficients[0], model_coefficients[1], model_coefficients[2], x[0], x[1], x[2]);
optimized_coefficients = Eigen::Vector3f (x[0], x[1], x[2]);
free (wa); free (fvec);
}
/** \brief Recompute the plane coefficients using the given inlier set and return them to the user.
* @note: these are the coefficients of the circle model after refinement (eg. after SVD)
* \param inliers the data inliers found as supporting the model
* \param refit_coefficients the resultant recomputed coefficients after non-linear optimization
*/
void
SACModelCircle2D::refitModel (const std::vector<int> &inliers, std::vector<double> &refit_coefficients)
{
if (inliers.size () == 0)
{
ROS_ERROR ("[SACModelCircle2D::RefitModel] Cannot re-fit 0 inliers!");
refit_coefficients = model_coefficients_;
return;
}
if (model_coefficients_.size () == 0)
{
ROS_WARN ("[SACModelCircle2D::RefitModel] Initial model coefficients have not been estimated yet - proceeding without an initial solution!");
best_inliers_ = indices_;
}
tmp_inliers_ = &inliers;
int m = inliers.size ();
double *fvec = new double[m];
int n = 3; // 3 unknowns
int iwa[n];
int lwa = m * n + 5 * n + m;
double *wa = new double[lwa];
// Set the initial solution
double x[3] = {0.0, 0.0, 0.0};
if ((int)model_coefficients_.size () == n)
for (int d = 0; d < n; d++)
x[d] = model_coefficients_.at (d);
// Set tol to the square root of the machine. Unless high solutions are required, these are the recommended settings.
double tol = sqrt (dpmpar (1));
// Optimize using forward-difference approximation LM
int info = lmdif1 (&sample_consensus::SACModelCircle2D::functionToOptimize, this, m, n, x, fvec, tol, iwa, wa, lwa);
// Compute the L2 norm of the residuals
ROS_DEBUG ("LM solver finished with exit code %i, having a residual norm of %g. \nInitial solution: %g %g %g \nFinal solution: %g %g %g",
info, enorm (m, fvec), model_coefficients_.at (0), model_coefficients_.at (1), model_coefficients_.at (2), x[0], x[1], x[2]);
refit_coefficients.resize (n);
for (int d = 0; d < n; d++)
refit_coefficients[d] = x[d];
free (wa); free (fvec);
}
/* Subroutine */ void lmpar(int n, double *r__, int ldr,
const int *ipvt, const double *diag, const double *qtb, double delta,
double *par, double *x, double *sdiag, double *wa1,
double *wa2)
{
/* Initialized data */
#define p1 .1
#define p001 .001
/* System generated locals */
int r_dim1, r_offset, i__1, i__2;
double d__1, d__2;
/* Local variables */
int i__, j, k, l;
double fp;
int jm1, jp1;
double sum, parc, parl;
int iter;
double temp, paru, dwarf;
int nsing;
double gnorm;
double dxnorm;
/* ********** */
/* subroutine lmpar */
/* given an m by n matrix a, an n by n nonsingular diagonal */
/* matrix d, an m-vector b, and a positive number delta, */
/* the problem is to determine a value for the parameter */
/* par such that if x solves the system */
/* a*x = b , sqrt(par)*d*x = 0 , */
/* in the least squares sense, and dxnorm is the euclidean */
/* norm of d*x, then either par is zero and */
/* (dxnorm-delta) .le. 0.1*delta , */
/* or par is positive and */
/* abs(dxnorm-delta) .le. 0.1*delta . */
/* this subroutine completes the solution of the problem */
/* if it is provided with the necessary information from the */
/* qr factorization, with column pivoting, of a. that is, if */
/* a*p = q*r, where p is a permutation matrix, q has orthogonal */
/* columns, and r is an upper triangular matrix with diagonal */
/* elements of nonincreasing magnitude, then lmpar expects */
/* the full upper triangle of r, the permutation matrix p, */
/* and the first n components of (q transpose)*b. on output */
/* lmpar also provides an upper triangular matrix s such that */
/* t t t */
/* p *(a *a + par*d*d)*p = s *s . */
/* s is employed within lmpar and may be of separate interest. */
/* only a few iterations are generally needed for convergence */
/* of the algorithm. if, however, the limit of 10 iterations */
/* is reached, then the output par will contain the best */
/* value obtained so far. */
/* the subroutine statement is */
/* subroutine lmpar(n,r,ldr,ipvt,diag,qtb,delta,par,x,sdiag, */
/* wa1,wa2) */
/* where */
/* n is a positive integer input variable set to the order of r. */
/* r is an n by n array. on input the full upper triangle */
/* must contain the full upper triangle of the matrix r. */
/* on output the full upper triangle is unaltered, and the */
/* strict lower triangle contains the strict upper triangle */
/* (transposed) of the upper triangular matrix s. */
/* ldr is a positive integer input variable not less than n */
/* which specifies the leading dimension of the array r. */
/* ipvt is an integer input array of length n which defines the */
/* permutation matrix p such that a*p = q*r. column j of p */
/* is column ipvt(j) of the identity matrix. */
/* diag is an input array of length n which must contain the */
/* diagonal elements of the matrix d. */
/* qtb is an input array of length n which must contain the first */
/* n elements of the vector (q transpose)*b. */
/* delta is a positive input variable which specifies an upper */
/* bound on the euclidean norm of d*x. */
/* par is a nonnegative variable. on input par contains an */
/* initial estimate of the levenberg-marquardt parameter. */
/* on output par contains the final estimate. */
/* x is an output array of length n which contains the least */
/* squares solution of the system a*x = b, sqrt(par)*d*x = 0, */
/* for the output par. */
/* sdiag is an output array of length n which contains the */
/* diagonal elements of the upper triangular matrix s. */
/* wa1 and wa2 are work arrays of length n. */
/* subprograms called */
/* minpack-supplied ... dpmpar,enorm,qrsolv */
/* fortran-supplied ... dabs,dmax1,dmin1,dsqrt */
/* argonne national laboratory. minpack project. march 1980. */
/* burton s. garbow, kenneth e. hillstrom, jorge j. more */
/* ********** */
/* Parameter adjustments */
--wa2;
--wa1;
--sdiag;
--x;
--qtb;
--diag;
--ipvt;
r_dim1 = ldr;
r_offset = 1 + r_dim1 * 1;
r__ -= r_offset;
/* Function Body */
/* dwarf is the smallest positive magnitude. */
dwarf = dpmpar(2);
/* compute and store in x the gauss-newton direction. if the */
/* jacobian is rank-deficient, obtain a least squares solution. */
nsing = n;
i__1 = n;
for (j = 1; j <= i__1; ++j) {
wa1[j] = qtb[j];
if (r__[j + j * r_dim1] == 0. && nsing == n) {
nsing = j - 1;
}
if (nsing < n) {
wa1[j] = 0.;
}
/* L10: */
}
if (nsing < 1) {
goto L50;
}
i__1 = nsing;
for (k = 1; k <= i__1; ++k) {
j = nsing - k + 1;
wa1[j] /= r__[j + j * r_dim1];
temp = wa1[j];
jm1 = j - 1;
if (jm1 < 1) {
goto L30;
}
i__2 = jm1;
for (i__ = 1; i__ <= i__2; ++i__) {
wa1[i__] -= r__[i__ + j * r_dim1] * temp;
/* L20: */
}
L30:
/* L40: */
;
}
L50:
i__1 = n;
for (j = 1; j <= i__1; ++j) {
l = ipvt[j];
x[l] = wa1[j];
/* L60: */
}
/* initialize the iteration counter. */
/* evaluate the function at the origin, and test */
/* for acceptance of the gauss-newton direction. */
iter = 0;
i__1 = n;
for (j = 1; j <= i__1; ++j) {
wa2[j] = diag[j] * x[j];
/* L70: */
}
dxnorm = enorm(n, &wa2[1]);
fp = dxnorm - delta;
if (fp <= p1 * delta) {
goto L220;
}
/* if the jacobian is not rank deficient, the newton */
/* step provides a lower bound, parl, for the zero of */
/* the function. otherwise set this bound to zero. */
parl = 0.;
if (nsing < n) {
goto L120;
}
i__1 = n;
for (j = 1; j <= i__1; ++j) {
l = ipvt[j];
wa1[j] = diag[l] * (wa2[l] / dxnorm);
/* L80: */
}
i__1 = n;
for (j = 1; j <= i__1; ++j) {
sum = 0.;
jm1 = j - 1;
if (jm1 < 1) {
goto L100;
}
i__2 = jm1;
for (i__ = 1; i__ <= i__2; ++i__) {
sum += r__[i__ + j * r_dim1] * wa1[i__];
/* L90: */
}
L100:
wa1[j] = (wa1[j] - sum) / r__[j + j * r_dim1];
/* L110: */
}
temp = enorm(n, &wa1[1]);
parl = fp / delta / temp / temp;
L120:
/* calculate an upper bound, paru, for the zero of the function. */
i__1 = n;
for (j = 1; j <= i__1; ++j) {
sum = 0.;
i__2 = j;
for (i__ = 1; i__ <= i__2; ++i__) {
sum += r__[i__ + j * r_dim1] * qtb[i__];
/* L130: */
}
l = ipvt[j];
wa1[j] = sum / diag[l];
/* L140: */
}
gnorm = enorm(n, &wa1[1]);
paru = gnorm / delta;
if (paru == 0.) {
paru = dwarf / min(delta,p1);
}
/* if the input par lies outside of the interval (parl,paru), */
/* set par to the closer endpoint. */
*par = max(*par,parl);
*par = min(*par,paru);
if (*par == 0.) {
*par = gnorm / dxnorm;
}
/* beginning of an iteration. */
L150:
++iter;
/* evaluate the function at the current value of par. */
if (*par == 0.) {
/* Computing MAX */
d__1 = dwarf, d__2 = p001 * paru;
*par = max(d__1,d__2);
}
temp = sqrt(*par);
i__1 = n;
for (j = 1; j <= i__1; ++j) {
wa1[j] = temp * diag[j];
/* L160: */
}
qrsolv(n, &r__[r_offset], ldr, &ipvt[1], &wa1[1], &qtb[1], &x[1], &sdiag[
1], &wa2[1]);
i__1 = n;
for (j = 1; j <= i__1; ++j) {
wa2[j] = diag[j] * x[j];
/* L170: */
}
dxnorm = enorm(n, &wa2[1]);
temp = fp;
fp = dxnorm - delta;
/* if the function is small enough, accept the current value */
/* of par. also test for the exceptional cases where parl */
/* is zero or the number of iterations has reached 10. */
if (abs(fp) <= p1 * delta || (parl == 0. && fp <= temp && temp < 0.) ||
iter == 10) {
goto L220;
}
/* compute the newton correction. */
i__1 = n;
for (j = 1; j <= i__1; ++j) {
l = ipvt[j];
wa1[j] = diag[l] * (wa2[l] / dxnorm);
/* L180: */
}
i__1 = n;
for (j = 1; j <= i__1; ++j) {
wa1[j] /= sdiag[j];
temp = wa1[j];
jp1 = j + 1;
if (n < jp1) {
goto L200;
}
i__2 = n;
for (i__ = jp1; i__ <= i__2; ++i__) {
wa1[i__] -= r__[i__ + j * r_dim1] * temp;
/* L190: */
}
L200:
/* L210: */
;
}
temp = enorm(n, &wa1[1]);
parc = fp / delta / temp / temp;
/* depending on the sign of the function, update parl or paru. */
if (fp > 0.) {
parl = max(parl,*par);
}
if (fp < 0.) {
paru = min(paru,*par);
}
/* compute an improved estimate for par. */
/* Computing MAX */
d__1 = parl, d__2 = *par + parc;
*par = max(d__1,d__2);
/* end of an iteration. */
goto L150;
L220:
/* termination. */
if (iter == 0) {
*par = 0.;
}
return;
/* last card of subroutine lmpar. */
} /* lmpar_ */
/* Subroutine */ void r1updt(int m, int n, double *s, int
ls, const double *u, double *v, double *w, int *sing)
{
/* Initialized data */
#define p5 .5
#define p25 .25
/* System generated locals */
int i__1, i__2;
double d__1, d__2;
/* Local variables */
int i__, j, l, jj, nm1;
double tan__;
int nmj;
double cos__, sin__, tau, temp, giant, cotan;
/* ********** */
/* subroutine r1updt */
/* given an m by n lower trapezoidal matrix s, an m-vector u, */
/* and an n-vector v, the problem is to determine an */
/* orthogonal matrix q such that */
/* t */
/* (s + u*v )*q */
/* is again lower trapezoidal. */
/* this subroutine determines q as the product of 2*(n - 1) */
/* transformations */
/* gv(n-1)*...*gv(1)*gw(1)*...*gw(n-1) */
/* where gv(i), gw(i) are givens rotations in the (i,n) plane */
/* which eliminate elements in the i-th and n-th planes, */
/* respectively. q itself is not accumulated, rather the */
/* information to recover the gv, gw rotations is returned. */
/* the subroutine statement is */
/* subroutine r1updt(m,n,s,ls,u,v,w,sing) */
/* where */
/* m is a positive integer input variable set to the number */
/* of rows of s. */
/* n is a positive integer input variable set to the number */
/* of columns of s. n must not exceed m. */
/* s is an array of length ls. on input s must contain the lower */
/* trapezoidal matrix s stored by columns. on output s contains */
/* the lower trapezoidal matrix produced as described above. */
/* ls is a positive integer input variable not less than */
/* (n*(2*m-n+1))/2. */
/* u is an input array of length m which must contain the */
/* vector u. */
/* v is an array of length n. on input v must contain the vector */
/* v. on output v(i) contains the information necessary to */
/* recover the givens rotation gv(i) described above. */
/* w is an output array of length m. w(i) contains information */
/* necessary to recover the givens rotation gw(i) described */
/* above. */
/* sing is a logical output variable. sing is set true if any */
/* of the diagonal elements of the output s are zero. otherwise */
/* sing is set false. */
/* subprograms called */
/* minpack-supplied ... dpmpar */
/* fortran-supplied ... dabs,dsqrt */
/* argonne national laboratory. minpack project. march 1980. */
/* burton s. garbow, kenneth e. hillstrom, jorge j. more, */
/* john l. nazareth */
/* ********** */
/* Parameter adjustments */
--w;
--u;
--v;
--s;
/* Function Body */
/* giant is the largest magnitude. */
giant = dpmpar(3);
/* initialize the diagonal element pointer. */
jj = n * ((m << 1) - n + 1) / 2 - (m - n);
/* move the nontrivial part of the last column of s into w. */
l = jj;
i__1 = m;
for (i__ = n; i__ <= i__1; ++i__) {
w[i__] = s[l];
++l;
/* L10: */
}
/* rotate the vector v into a multiple of the n-th unit vector */
/* in such a way that a spike is introduced into w. */
nm1 = n - 1;
if (nm1 < 1) {
goto L70;
}
i__1 = nm1;
for (nmj = 1; nmj <= i__1; ++nmj) {
j = n - nmj;
jj -= m - j + 1;
w[j] = 0.;
if (v[j] == 0.) {
goto L50;
}
/* determine a givens rotation which eliminates the */
/* j-th element of v. */
if ((d__1 = v[n], abs(d__1)) >= (d__2 = v[j], abs(d__2))) {
goto L20;
}
cotan = v[n] / v[j];
/* Computing 2nd power */
d__1 = cotan;
sin__ = p5 / sqrt(p25 + p25 * (d__1 * d__1));
cos__ = sin__ * cotan;
tau = 1.;
if (abs(cos__) * giant > 1.) {
tau = 1. / cos__;
}
goto L30;
L20:
tan__ = v[j] / v[n];
/* Computing 2nd power */
d__1 = tan__;
cos__ = p5 / sqrt(p25 + p25 * (d__1 * d__1));
sin__ = cos__ * tan__;
tau = sin__;
L30:
/* apply the transformation to v and store the information */
/* necessary to recover the givens rotation. */
v[n] = sin__ * v[j] + cos__ * v[n];
v[j] = tau;
/* apply the transformation to s and extend the spike in w. */
l = jj;
i__2 = m;
for (i__ = j; i__ <= i__2; ++i__) {
temp = cos__ * s[l] - sin__ * w[i__];
w[i__] = sin__ * s[l] + cos__ * w[i__];
s[l] = temp;
++l;
/* L40: */
}
L50:
/* L60: */
;
}
L70:
/* add the spike from the rank 1 update to w. */
i__1 = m;
for (i__ = 1; i__ <= i__1; ++i__) {
w[i__] += v[n] * u[i__];
/* L80: */
}
/* eliminate the spike. */
*sing = FALSE_;
if (nm1 < 1) {
goto L140;
}
i__1 = nm1;
for (j = 1; j <= i__1; ++j) {
if (w[j] == 0.) {
goto L120;
}
/* determine a givens rotation which eliminates the */
/* j-th element of the spike. */
if ((d__1 = s[jj], abs(d__1)) >= (d__2 = w[j], abs(d__2))) {
goto L90;
}
cotan = s[jj] / w[j];
/* Computing 2nd power */
d__1 = cotan;
sin__ = p5 / sqrt(p25 + p25 * (d__1 * d__1));
cos__ = sin__ * cotan;
tau = 1.;
if (abs(cos__) * giant > 1.) {
tau = 1. / cos__;
}
goto L100;
L90:
tan__ = w[j] / s[jj];
/* Computing 2nd power */
d__1 = tan__;
cos__ = p5 / sqrt(p25 + p25 * (d__1 * d__1));
sin__ = cos__ * tan__;
tau = sin__;
L100:
/* apply the transformation to s and reduce the spike in w. */
l = jj;
i__2 = m;
for (i__ = j; i__ <= i__2; ++i__) {
temp = cos__ * s[l] + sin__ * w[i__];
w[i__] = -sin__ * s[l] + cos__ * w[i__];
s[l] = temp;
++l;
/* L110: */
}
/* store the information necessary to recover the */
/* givens rotation. */
w[j] = tau;
L120:
/* test for zero diagonal elements in the output s. */
if (s[jj] == 0.) {
*sing = TRUE_;
}
jj += m - j + 1;
/* L130: */
}
L140:
/* move w back into the last column of the output s. */
l = jj;
i__1 = m;
for (i__ = n; i__ <= i__1; ++i__) {
s[l] = w[i__];
++l;
/* L150: */
}
if (s[jj] == 0.) {
*sing = TRUE_;
}
return;
/* last card of subroutine r1updt. */
} /* r1updt_ */
/* Subroutine */ int lmder(minpack_funcder_mn fcn, void *p, int m, int n, double *x,
double *fvec, double *fjac, int ldfjac, double ftol,
double xtol, double gtol, int maxfev, double *
diag, int mode, double factor, int nprint,
int *nfev, int *njev, int *ipvt, double *qtf,
double *wa1, double *wa2, double *wa3, double *wa4)
{
/* Initialized data */
#define p1 .1
#define p5 .5
#define p25 .25
#define p75 .75
#define p0001 1e-4
/* System generated locals */
double d1, d2;
/* Local variables */
int i, j, l;
double par, sum;
int iter;
double temp, temp1, temp2;
int iflag;
double delta = 0.;
double ratio;
double fnorm, gnorm, pnorm, xnorm = 0., fnorm1, actred, dirder,
epsmch, prered;
int info;
/* ********** */
/* subroutine lmder */
/* the purpose of lmder is to minimize the sum of the squares of */
/* m nonlinear functions in n variables by a modification of */
/* the levenberg-marquardt algorithm. the user must provide a */
/* subroutine which calculates the functions and the jacobian. */
/* the subroutine statement is */
/* subroutine lmder(fcn,m,n,x,fvec,fjac,ldfjac,ftol,xtol,gtol, */
/* maxfev,diag,mode,factor,nprint,info,nfev, */
/* njev,ipvt,qtf,wa1,wa2,wa3,wa4) */
/* where */
/* fcn is the name of the user-supplied subroutine which */
/* calculates the functions and the jacobian. fcn must */
/* be declared in an external statement in the user */
/* calling program, and should be written as follows. */
/* subroutine fcn(m,n,x,fvec,fjac,ldfjac,iflag) */
/* integer m,n,ldfjac,iflag */
/* double precision x(n),fvec(m),fjac(ldfjac,n) */
/* ---------- */
/* if iflag = 1 calculate the functions at x and */
/* return this vector in fvec. do not alter fjac. */
/* if iflag = 2 calculate the jacobian at x and */
/* return this matrix in fjac. do not alter fvec. */
/* ---------- */
/* return */
/* end */
/* the value of iflag should not be changed by fcn unless */
/* the user wants to terminate execution of lmder. */
/* in this case set iflag to a negative integer. */
/* m is a positive integer input variable set to the number */
/* of functions. */
/* n is a positive integer input variable set to the number */
/* of variables. n must not exceed m. */
/* x is an array of length n. on input x must contain */
/* an initial estimate of the solution vector. on output x */
/* contains the final estimate of the solution vector. */
/* fvec is an output array of length m which contains */
/* the functions evaluated at the output x. */
/* fjac is an output m by n array. the upper n by n submatrix */
/* of fjac contains an upper triangular matrix r with */
/* diagonal elements of nonincreasing magnitude such that */
/* t t t */
/* p *(jac *jac)*p = r *r, */
/* where p is a permutation matrix and jac is the final */
/* calculated jacobian. column j of p is column ipvt(j) */
/* (see below) of the identity matrix. the lower trapezoidal */
/* part of fjac contains information generated during */
/* the computation of r. */
/* ldfjac is a positive integer input variable not less than m */
/* which specifies the leading dimension of the array fjac. */
/* ftol is a nonnegative input variable. termination */
/* occurs when both the actual and predicted relative */
/* reductions in the sum of squares are at most ftol. */
/* therefore, ftol measures the relative error desired */
/* in the sum of squares. */
/* xtol is a nonnegative input variable. termination */
/* occurs when the relative error between two consecutive */
/* iterates is at most xtol. therefore, xtol measures the */
/* relative error desired in the approximate solution. */
/* gtol is a nonnegative input variable. termination */
/* occurs when the cosine of the angle between fvec and */
/* any column of the jacobian is at most gtol in absolute */
/* value. therefore, gtol measures the orthogonality */
/* desired between the function vector and the columns */
/* of the jacobian. */
/* maxfev is a positive integer input variable. termination */
/* occurs when the number of calls to fcn with iflag = 1 */
/* has reached maxfev. */
/* diag is an array of length n. if mode = 1 (see */
/* below), diag is internally set. if mode = 2, diag */
/* must contain positive entries that serve as */
/* multiplicative scale factors for the variables. */
/* mode is an integer input variable. if mode = 1, the */
/* variables will be scaled internally. if mode = 2, */
/* the scaling is specified by the input diag. other */
/* values of mode are equivalent to mode = 1. */
/* factor is a positive input variable used in determining the */
/* initial step bound. this bound is set to the product of */
/* factor and the euclidean norm of diag*x if nonzero, or else */
/* to factor itself. in most cases factor should lie in the */
/* interval (.1,100.).100. is a generally recommended value. */
/* nprint is an integer input variable that enables controlled */
/* printing of iterates if it is positive. in this case, */
/* fcn is called with iflag = 0 at the beginning of the first */
/* iteration and every nprint iterations thereafter and */
/* immediately prior to return, with x, fvec, and fjac */
/* available for printing. fvec and fjac should not be */
/* altered. if nprint is not positive, no special calls */
/* of fcn with iflag = 0 are made. */
/* info is an integer output variable. if the user has */
/* terminated execution, info is set to the (negative) */
/* value of iflag. see description of fcn. otherwise, */
/* info is set as follows. */
/* info = 0 improper input parameters. */
/* info = 1 both actual and predicted relative reductions */
/* in the sum of squares are at most ftol. */
/* info = 2 relative error between two consecutive iterates */
/* is at most xtol. */
/* info = 3 conditions for info = 1 and info = 2 both hold. */
/* info = 4 the cosine of the angle between fvec and any */
/* column of the jacobian is at most gtol in */
/* absolute value. */
/* info = 5 number of calls to fcn with iflag = 1 has */
/* reached maxfev. */
/* info = 6 ftol is too small. no further reduction in */
/* the sum of squares is possible. */
/* info = 7 xtol is too small. no further improvement in */
/* the approximate solution x is possible. */
/* info = 8 gtol is too small. fvec is orthogonal to the */
/* columns of the jacobian to machine precision. */
/* nfev is an integer output variable set to the number of */
/* calls to fcn with iflag = 1. */
/* njev is an integer output variable set to the number of */
/* calls to fcn with iflag = 2. */
/* ipvt is an integer output array of length n. ipvt */
/* defines a permutation matrix p such that jac*p = q*r, */
/* where jac is the final calculated jacobian, q is */
/* orthogonal (not stored), and r is upper triangular */
/* with diagonal elements of nonincreasing magnitude. */
/* column j of p is column ipvt(j) of the identity matrix. */
/* qtf is an output array of length n which contains */
/* the first n elements of the vector (q transpose)*fvec. */
/* wa1, wa2, and wa3 are work arrays of length n. */
/* wa4 is a work array of length m. */
/* subprograms called */
/* user-supplied ...... fcn */
/* minpack-supplied ... dpmpar,enorm,lmpar,qrfac */
/* fortran-supplied ... dabs,dmax1,dmin1,dsqrt,mod */
/* argonne national laboratory. minpack project. march 1980. */
/* burton s. garbow, kenneth e. hillstrom, jorge j. more */
/* ********** */
/* epsmch is the machine precision. */
epsmch = dpmpar(1);
info = 0;
iflag = 0;
*nfev = 0;
*njev = 0;
/* check the input parameters for errors. */
if (n <= 0 || m < n || ldfjac < m || ftol < 0. || xtol < 0. ||
gtol < 0. || maxfev <= 0 || factor <= 0.) {
goto TERMINATE;
}
if (mode == 2) {
for (j = 0; j < n; ++j) {
if (diag[j] <= 0.) {
goto TERMINATE;
}
}
}
/* evaluate the function at the starting point */
/* and calculate its norm. */
iflag = (*fcn)(p, m, n, x, fvec, fjac, ldfjac, 1);
*nfev = 1;
if (iflag < 0) {
goto TERMINATE;
}
fnorm = enorm(m, fvec);
/* initialize levenberg-marquardt parameter and iteration counter. */
par = 0.;
iter = 1;
/* beginning of the outer loop. */
for (;;) {
/* calculate the jacobian matrix. */
iflag = (*fcn)(p, m, n, x, fvec, fjac, ldfjac, 2);
++(*njev);
if (iflag < 0) {
goto TERMINATE;
}
/* if requested, call fcn to enable printing of iterates. */
if (nprint > 0) {
iflag = 0;
if ((iter - 1) % nprint == 0) {
iflag = (*fcn)(p, m, n, x, fvec, fjac, ldfjac, 0);
}
if (iflag < 0) {
goto TERMINATE;
}
}
/* compute the qr factorization of the jacobian. */
qrfac(m, n, fjac, ldfjac, TRUE_, ipvt, n,
wa1, wa2, wa3);
/* on the first iteration and if mode is 1, scale according */
/* to the norms of the columns of the initial jacobian. */
if (iter == 1) {
if (mode != 2) {
for (j = 0; j < n; ++j) {
diag[j] = wa2[j];
if (wa2[j] == 0.) {
diag[j] = 1.;
}
}
}
/* on the first iteration, calculate the norm of the scaled x */
/* and initialize the step bound delta. */
for (j = 0; j < n; ++j) {
wa3[j] = diag[j] * x[j];
}
xnorm = enorm(n, wa3);
delta = factor * xnorm;
if (delta == 0.) {
delta = factor;
}
}
/* form (q transpose)*fvec and store the first n components in */
/* qtf. */
for (i = 0; i < m; ++i) {
wa4[i] = fvec[i];
}
for (j = 0; j < n; ++j) {
if (fjac[j + j * ldfjac] != 0.) {
sum = 0.;
for (i = j; i < m; ++i) {
sum += fjac[i + j * ldfjac] * wa4[i];
}
temp = -sum / fjac[j + j * ldfjac];
for (i = j; i < m; ++i) {
wa4[i] += fjac[i + j * ldfjac] * temp;
}
}
fjac[j + j * ldfjac] = wa1[j];
qtf[j] = wa4[j];
}
/* compute the norm of the scaled gradient. */
gnorm = 0.;
if (fnorm != 0.) {
for (j = 0; j < n; ++j) {
l = ipvt[j]-1;
if (wa2[l] != 0.) {
sum = 0.;
for (i = 0; i <= j; ++i) {
sum += fjac[i + j * ldfjac] * (qtf[i] / fnorm);
}
/* Computing MAX */
d1 = fabs(sum / wa2[l]);
gnorm = max(gnorm,d1);
}
}
}
/* test for convergence of the gradient norm. */
if (gnorm <= gtol) {
info = 4;
}
if (info != 0) {
goto TERMINATE;
}
/* rescale if necessary. */
if (mode != 2) {
for (j = 0; j < n; ++j) {
/* Computing MAX */
d1 = diag[j], d2 = wa2[j];
diag[j] = max(d1,d2);
}
}
/* beginning of the inner loop. */
do {
/* determine the levenberg-marquardt parameter. */
lmpar(n, fjac, ldfjac, ipvt, diag, qtf, delta,
&par, wa1, wa2, wa3, wa4);
/* store the direction p and x + p. calculate the norm of p. */
for (j = 0; j < n; ++j) {
wa1[j] = -wa1[j];
wa2[j] = x[j] + wa1[j];
wa3[j] = diag[j] * wa1[j];
}
pnorm = enorm(n, wa3);
/* on the first iteration, adjust the initial step bound. */
if (iter == 1) {
delta = min(delta,pnorm);
}
/* evaluate the function at x + p and calculate its norm. */
iflag = (*fcn)(p, m, n, wa2, wa4, fjac, ldfjac, 1);
++(*nfev);
if (iflag < 0) {
goto TERMINATE;
}
fnorm1 = enorm(m, wa4);
/* compute the scaled actual reduction. */
actred = -1.;
if (p1 * fnorm1 < fnorm) {
/* Computing 2nd power */
d1 = fnorm1 / fnorm;
actred = 1. - d1 * d1;
}
/* compute the scaled predicted reduction and */
/* the scaled directional derivative. */
for (j = 0; j < n; ++j) {
wa3[j] = 0.;
l = ipvt[j]-1;
temp = wa1[l];
for (i = 0; i <= j; ++i) {
wa3[i] += fjac[i + j * ldfjac] * temp;
}
}
temp1 = enorm(n, wa3) / fnorm;
temp2 = (sqrt(par) * pnorm) / fnorm;
prered = temp1 * temp1 + temp2 * temp2 / p5;
dirder = -(temp1 * temp1 + temp2 * temp2);
/* compute the ratio of the actual to the predicted */
/* reduction. */
ratio = 0.;
if (prered != 0.) {
ratio = actred / prered;
}
/* update the step bound. */
if (ratio <= p25) {
if (actred >= 0.) {
temp = p5;
} else {
temp = p5 * dirder / (dirder + p5 * actred);
}
if (p1 * fnorm1 >= fnorm || temp < p1) {
temp = p1;
}
/* Computing MIN */
d1 = pnorm / p1;
delta = temp * min(delta,d1);
par /= temp;
} else {
if (par == 0. || ratio >= p75) {
delta = pnorm / p5;
par = p5 * par;
}
}
/* test for successful iteration. */
if (ratio >= p0001) {
/* successful iteration. update x, fvec, and their norms. */
for (j = 0; j < n; ++j) {
x[j] = wa2[j];
wa2[j] = diag[j] * x[j];
}
for (i = 0; i < m; ++i) {
fvec[i] = wa4[i];
}
xnorm = enorm(n, wa2);
fnorm = fnorm1;
++iter;
}
/* tests for convergence. */
if (fabs(actred) <= ftol && prered <= ftol && p5 * ratio <= 1.) {
info = 1;
}
if (delta <= xtol * xnorm) {
info = 2;
}
if (fabs(actred) <= ftol && prered <= ftol && p5 * ratio <= 1. && info == 2) {
info = 3;
}
if (info != 0) {
goto TERMINATE;
}
/* tests for termination and stringent tolerances. */
if (*nfev >= maxfev) {
info = 5;
}
if (fabs(actred) <= epsmch && prered <= epsmch && p5 * ratio <= 1.) {
info = 6;
}
if (delta <= epsmch * xnorm) {
info = 7;
}
if (gnorm <= epsmch) {
info = 8;
}
if (info != 0) {
goto TERMINATE;
}
/* end of the inner loop. repeat if iteration unsuccessful. */
} while (ratio < p0001);
/* end of the outer loop. */
}
TERMINATE:
/* termination, either normal or user imposed. */
if (iflag < 0) {
info = iflag;
}
if (nprint > 0) {
(*fcn)(p, m, n, x, fvec, fjac, ldfjac, 0);
}
return info;
/* last card of subroutine lmder. */
} /* lmder_ */
/* Subroutine */ void qrfac(int m, int n, double *a, int
lda, int pivot, int *ipvt, int lipvt, double *rdiag,
double *acnorm, double *wa)
{
/* Initialized data */
#define p05 .05
/* System generated locals */
int a_dim1, a_offset, i__1, i__2, i__3;
double d__1, d__2, d__3;
/* Local variables */
int i__, j, k, jp1;
double sum;
int kmax;
double temp;
int minmn;
double epsmch;
double ajnorm;
/* ********** */
/* subroutine qrfac */
/* this subroutine uses householder transformations with column */
/* pivoting (optional) to compute a qr factorization of the */
/* m by n matrix a. that is, qrfac determines an orthogonal */
/* matrix q, a permutation matrix p, and an upper trapezoidal */
/* matrix r with diagonal elements of nonincreasing magnitude, */
/* such that a*p = q*r. the householder transformation for */
/* column k, k = 1,2,...,min(m,n), is of the form */
/* t */
/* i - (1/u(k))*u*u */
/* where u has zeros in the first k-1 positions. the form of */
/* this transformation and the method of pivoting first */
/* appeared in the corresponding linpack subroutine. */
/* the subroutine statement is */
/* subroutine qrfac(m,n,a,lda,pivot,ipvt,lipvt,rdiag,acnorm,wa) */
/* where */
/* m is a positive integer input variable set to the number */
/* of rows of a. */
/* n is a positive integer input variable set to the number */
/* of columns of a. */
/* a is an m by n array. on input a contains the matrix for */
/* which the qr factorization is to be computed. on output */
/* the strict upper trapezoidal part of a contains the strict */
/* upper trapezoidal part of r, and the lower trapezoidal */
/* part of a contains a factored form of q (the non-trivial */
/* elements of the u vectors described above). */
/* lda is a positive integer input variable not less than m */
/* which specifies the leading dimension of the array a. */
/* pivot is a logical input variable. if pivot is set true, */
/* then column pivoting is enforced. if pivot is set false, */
/* then no column pivoting is done. */
/* ipvt is an integer output array of length lipvt. ipvt */
/* defines the permutation matrix p such that a*p = q*r. */
/* column j of p is column ipvt(j) of the identity matrix. */
/* if pivot is false, ipvt is not referenced. */
/* lipvt is a positive integer input variable. if pivot is false, */
/* then lipvt may be as small as 1. if pivot is true, then */
/* lipvt must be at least n. */
/* rdiag is an output array of length n which contains the */
/* diagonal elements of r. */
/* acnorm is an output array of length n which contains the */
/* norms of the corresponding columns of the input matrix a. */
/* if this information is not needed, then acnorm can coincide */
/* with rdiag. */
/* wa is a work array of length n. if pivot is false, then wa */
/* can coincide with rdiag. */
/* subprograms called */
/* minpack-supplied ... dpmpar,enorm */
/* fortran-supplied ... dmax1,dsqrt,min0 */
/* argonne national laboratory. minpack project. march 1980. */
/* burton s. garbow, kenneth e. hillstrom, jorge j. more */
/* ********** */
/* Parameter adjustments */
--wa;
--acnorm;
--rdiag;
a_dim1 = lda;
a_offset = 1 + a_dim1 * 1;
a -= a_offset;
--ipvt;
/* Function Body */
/* epsmch is the machine precision. */
epsmch = dpmpar(1);
/* compute the initial column norms and initialize several arrays. */
i__1 = n;
for (j = 1; j <= i__1; ++j) {
acnorm[j] = enorm(m, &a[j * a_dim1 + 1]);
rdiag[j] = acnorm[j];
wa[j] = rdiag[j];
if (pivot) {
ipvt[j] = j;
}
/* L10: */
}
/* reduce a to r with householder transformations. */
minmn = min(m,n);
i__1 = minmn;
for (j = 1; j <= i__1; ++j) {
if (! (pivot)) {
goto L40;
}
/* bring the column of largest norm into the pivot position. */
kmax = j;
i__2 = n;
for (k = j; k <= i__2; ++k) {
if (rdiag[k] > rdiag[kmax]) {
kmax = k;
}
/* L20: */
}
if (kmax == j) {
goto L40;
}
i__2 = m;
for (i__ = 1; i__ <= i__2; ++i__) {
temp = a[i__ + j * a_dim1];
a[i__ + j * a_dim1] = a[i__ + kmax * a_dim1];
a[i__ + kmax * a_dim1] = temp;
/* L30: */
}
rdiag[kmax] = rdiag[j];
wa[kmax] = wa[j];
k = ipvt[j];
ipvt[j] = ipvt[kmax];
ipvt[kmax] = k;
L40:
/* compute the householder transformation to reduce the */
/* j-th column of a to a multiple of the j-th unit vector. */
i__2 = m - j + 1;
ajnorm = enorm(i__2, &a[j + j * a_dim1]);
if (ajnorm == 0.) {
goto L100;
}
if (a[j + j * a_dim1] < 0.) {
ajnorm = -ajnorm;
}
i__2 = m;
for (i__ = j; i__ <= i__2; ++i__) {
a[i__ + j * a_dim1] /= ajnorm;
/* L50: */
}
a[j + j * a_dim1] += 1.;
/* apply the transformation to the remaining columns */
/* and update the norms. */
jp1 = j + 1;
if (n < jp1) {
goto L100;
}
i__2 = n;
for (k = jp1; k <= i__2; ++k) {
sum = 0.;
i__3 = m;
for (i__ = j; i__ <= i__3; ++i__) {
sum += a[i__ + j * a_dim1] * a[i__ + k * a_dim1];
/* L60: */
}
temp = sum / a[j + j * a_dim1];
i__3 = m;
for (i__ = j; i__ <= i__3; ++i__) {
a[i__ + k * a_dim1] -= temp * a[i__ + j * a_dim1];
/* L70: */
}
if (! (pivot) || rdiag[k] == 0.) {
goto L80;
}
temp = a[j + k * a_dim1] / rdiag[k];
/* Computing MAX */
/* Computing 2nd power */
d__3 = temp;
d__1 = 0., d__2 = 1. - d__3 * d__3;
rdiag[k] *= sqrt((max(d__1,d__2)));
/* Computing 2nd power */
d__1 = rdiag[k] / wa[k];
if (p05 * (d__1 * d__1) > epsmch) {
goto L80;
}
i__3 = m - j;
rdiag[k] = enorm(i__3, &a[jp1 + k * a_dim1]);
wa[k] = rdiag[k];
L80:
/* L90: */
;
}
L100:
rdiag[j] = -ajnorm;
/* L110: */
}
return;
/* last card of subroutine qrfac. */
} /* qrfac_ */
/* Subroutine */ void dogleg(int n, const double *r__, int lr,
const double *diag, const double *qtb, double delta, double *x,
double *wa1, double *wa2)
{
/* System generated locals */
int i__1, i__2;
double d__1, d__2, d__3, d__4;
/* Local variables */
int i__, j, k, l, jj, jp1;
double sum, temp, alpha, bnorm;
double gnorm, qnorm, epsmch;
double sgnorm;
/* ********** */
/* subroutine dogleg */
/* given an m by n matrix a, an n by n nonsingular diagonal */
/* matrix d, an m-vector b, and a positive number delta, the */
/* problem is to determine the convex combination x of the */
/* gauss-newton and scaled gradient directions that minimizes */
/* (a*x - b) in the least squares sense, subject to the */
/* restriction that the euclidean norm of d*x be at most delta. */
/* this subroutine completes the solution of the problem */
/* if it is provided with the necessary information from the */
/* qr factorization of a. that is, if a = q*r, where q has */
/* orthogonal columns and r is an upper triangular matrix, */
/* then dogleg expects the full upper triangle of r and */
/* the first n components of (q transpose)*b. */
/* the subroutine statement is */
/* subroutine dogleg(n,r,lr,diag,qtb,delta,x,wa1,wa2) */
/* where */
/* n is a positive integer input variable set to the order of r. */
/* r is an input array of length lr which must contain the upper */
/* triangular matrix r stored by rows. */
/* lr is a positive integer input variable not less than */
/* (n*(n+1))/2. */
/* diag is an input array of length n which must contain the */
/* diagonal elements of the matrix d. */
/* qtb is an input array of length n which must contain the first */
/* n elements of the vector (q transpose)*b. */
/* delta is a positive input variable which specifies an upper */
/* bound on the euclidean norm of d*x. */
/* x is an output array of length n which contains the desired */
/* convex combination of the gauss-newton direction and the */
/* scaled gradient direction. */
/* wa1 and wa2 are work arrays of length n. */
/* subprograms called */
/* minpack-supplied ... dpmpar,enorm */
/* fortran-supplied ... dabs,dmax1,dmin1,dsqrt */
/* argonne national laboratory. minpack project. march 1980. */
/* burton s. garbow, kenneth e. hillstrom, jorge j. more */
/* ********** */
/* Parameter adjustments */
--wa2;
--wa1;
--x;
--qtb;
--diag;
--r__;
/* Function Body */
/* epsmch is the machine precision. */
epsmch = dpmpar(1);
/* first, calculate the gauss-newton direction. */
jj = n * (n + 1) / 2 + 1;
i__1 = n;
for (k = 1; k <= i__1; ++k) {
j = n - k + 1;
jp1 = j + 1;
jj -= k;
l = jj + 1;
sum = 0.;
if (n < jp1) {
goto L20;
}
i__2 = n;
for (i__ = jp1; i__ <= i__2; ++i__) {
sum += r__[l] * x[i__];
++l;
/* L10: */
}
L20:
temp = r__[jj];
if (temp != 0.) {
goto L40;
}
l = j;
i__2 = j;
for (i__ = 1; i__ <= i__2; ++i__) {
/* Computing MAX */
d__2 = temp, d__3 = fabs(r__[l]);
temp = max(d__2,d__3);
l = l + n - i__;
/* L30: */
}
temp = epsmch * temp;
if (temp == 0.) {
temp = epsmch;
}
L40:
x[j] = (qtb[j] - sum) / temp;
/* L50: */
}
/* test whether the gauss-newton direction is acceptable. */
i__1 = n;
for (j = 1; j <= i__1; ++j) {
wa1[j] = 0.;
wa2[j] = diag[j] * x[j];
/* L60: */
}
qnorm = enorm(n, &wa2[1]);
if (qnorm <= delta) {
/* goto L140; */
return;
}
/* the gauss-newton direction is not acceptable. */
/* next, calculate the scaled gradient direction. */
l = 1;
i__1 = n;
for (j = 1; j <= i__1; ++j) {
temp = qtb[j];
i__2 = n;
for (i__ = j; i__ <= i__2; ++i__) {
wa1[i__] += r__[l] * temp;
++l;
/* L70: */
}
wa1[j] /= diag[j];
/* L80: */
}
/* calculate the norm of the scaled gradient and test for */
/* the special case in which the scaled gradient is zero. */
gnorm = enorm(n, &wa1[1]);
sgnorm = 0.;
alpha = delta / qnorm;
if (gnorm == 0.) {
goto L120;
}
/* calculate the point along the scaled gradient */
/* at which the quadratic is minimized. */
i__1 = n;
for (j = 1; j <= i__1; ++j) {
wa1[j] = wa1[j] / gnorm / diag[j];
/* L90: */
}
l = 1;
i__1 = n;
for (j = 1; j <= i__1; ++j) {
sum = 0.;
i__2 = n;
for (i__ = j; i__ <= i__2; ++i__) {
sum += r__[l] * wa1[i__];
++l;
/* L100: */
}
wa2[j] = sum;
/* L110: */
}
temp = enorm(n, &wa2[1]);
sgnorm = gnorm / temp / temp;
/* test whether the scaled gradient direction is acceptable. */
alpha = 0.;
if (sgnorm >= delta) {
goto L120;
}
/* the scaled gradient direction is not acceptable. */
/* finally, calculate the point along the dogleg */
/* at which the quadratic is minimized. */
bnorm = enorm(n, &qtb[1]);
temp = bnorm / gnorm * (bnorm / qnorm) * (sgnorm / delta);
/* Computing 2nd power */
d__1 = sgnorm / delta;
/* Computing 2nd power */
d__2 = temp - delta / qnorm;
/* Computing 2nd power */
d__3 = delta / qnorm;
/* Computing 2nd power */
d__4 = sgnorm / delta;
temp = temp - delta / qnorm * (d__1 * d__1) + sqrt(d__2 * d__2 + (1. -
d__3 * d__3) * (1. - d__4 * d__4));
/* Computing 2nd power */
d__1 = sgnorm / delta;
alpha = delta / qnorm * (1. - d__1 * d__1) / temp;
L120:
/* form appropriate convex combination of the gauss-newton */
/* direction and the scaled gradient direction. */
temp = (1. - alpha) * min(sgnorm,delta);
i__1 = n;
for (j = 1; j <= i__1; ++j) {
x[j] = temp * wa1[j] + alpha * x[j];
/* L130: */
}
/* L140: */
return;
/* last card of subroutine dogleg. */
} /* dogleg_ */
/* Subroutine */ int hybrj(minpack_funcder_nn fcn, void *p, int n, double *x, double *
fvec, double *fjac, int ldfjac, double xtol, int
maxfev, double *diag, int mode, double factor, int
nprint, int *nfev, int *njev, double *r,
int lr, double *qtf, double *wa1, double *wa2,
double *wa3, double *wa4)
{
/* Initialized data */
#define p1 .1
#define p5 .5
#define p001 .001
#define p0001 1e-4
/* System generated locals */
int fjac_dim1, fjac_offset;
double d1, d2;
/* Local variables */
int i, j, l, jm1, iwa[1];
double sum;
int sing;
int iter;
double temp;
int iflag;
double delta;
int jeval;
int ncsuc;
double ratio;
double fnorm;
double pnorm, xnorm, fnorm1;
int nslow1, nslow2;
int ncfail;
double actred, epsmch, prered;
int info;
/* ********** */
/* subroutine hybrj */
/* the purpose of hybrj is to find a zero of a system of */
/* n nonlinear functions in n variables by a modification */
/* of the powell hybrid method. the user must provide a */
/* subroutine which calculates the functions and the jacobian. */
/* the subroutine statement is */
/* subroutine hybrj(fcn,n,x,fvec,fjac,ldfjac,xtol,maxfev,diag, */
/* mode,factor,nprint,info,nfev,njev,r,lr,qtf, */
/* wa1,wa2,wa3,wa4) */
/* where */
/* fcn is the name of the user-supplied subroutine which */
/* calculates the functions and the jacobian. fcn must */
/* be declared in an external statement in the user */
/* calling program, and should be written as follows. */
/* subroutine fcn(n,x,fvec,fjac,ldfjac,iflag) */
/* integer n,ldfjac,iflag */
/* double precision x(n),fvec(n),fjac(ldfjac,n) */
/* ---------- */
/* if iflag = 1 calculate the functions at x and */
/* return this vector in fvec. do not alter fjac. */
/* if iflag = 2 calculate the jacobian at x and */
/* return this matrix in fjac. do not alter fvec. */
/* --------- */
/* return */
/* end */
/* the value of iflag should not be changed by fcn unless */
/* the user wants to terminate execution of hybrj. */
/* in this case set iflag to a negative integer. */
/* n is a positive integer input variable set to the number */
/* of functions and variables. */
/* x is an array of length n. on input x must contain */
/* an initial estimate of the solution vector. on output x */
/* contains the final estimate of the solution vector. */
/* fvec is an output array of length n which contains */
/* the functions evaluated at the output x. */
/* fjac is an output n by n array which contains the */
/* orthogonal matrix q produced by the qr factorization */
/* of the final approximate jacobian. */
/* ldfjac is a positive integer input variable not less than n */
/* which specifies the leading dimension of the array fjac. */
/* xtol is a nonnegative input variable. termination */
/* occurs when the relative error between two consecutive */
/* iterates is at most xtol. */
/* maxfev is a positive integer input variable. termination */
/* occurs when the number of calls to fcn with iflag = 1 */
/* has reached maxfev. */
/* diag is an array of length n. if mode = 1 (see */
/* below), diag is internally set. if mode = 2, diag */
/* must contain positive entries that serve as */
/* multiplicative scale factors for the variables. */
/* mode is an integer input variable. if mode = 1, the */
/* variables will be scaled internally. if mode = 2, */
/* the scaling is specified by the input diag. other */
/* values of mode are equivalent to mode = 1. */
/* factor is a positive input variable used in determining the */
/* initial step bound. this bound is set to the product of */
/* factor and the euclidean norm of diag*x if nonzero, or else */
/* to factor itself. in most cases factor should lie in the */
/* interval (.1,100.). 100. is a generally recommended value. */
/* nprint is an integer input variable that enables controlled */
/* printing of iterates if it is positive. in this case, */
/* fcn is called with iflag = 0 at the beginning of the first */
/* iteration and every nprint iterations thereafter and */
/* immediately prior to return, with x and fvec available */
/* for printing. fvec and fjac should not be altered. */
/* if nprint is not positive, no special calls of fcn */
/* with iflag = 0 are made. */
/* info is an integer output variable. if the user has */
/* terminated execution, info is set to the (negative) */
/* value of iflag. see description of fcn. otherwise, */
/* info is set as follows. */
/* info = 0 improper input parameters. */
/* info = 1 relative error between two consecutive iterates */
/* is at most xtol. */
/* info = 2 number of calls to fcn with iflag = 1 has */
/* reached maxfev. */
/* info = 3 xtol is too small. no further improvement in */
/* the approximate solution x is possible. */
/* info = 4 iteration is not making good progress, as */
/* measured by the improvement from the last */
/* five jacobian evaluations. */
/* info = 5 iteration is not making good progress, as */
/* measured by the improvement from the last */
/* ten iterations. */
/* nfev is an integer output variable set to the number of */
/* calls to fcn with iflag = 1. */
/* njev is an integer output variable set to the number of */
/* calls to fcn with iflag = 2. */
/* r is an output array of length lr which contains the */
/* upper triangular matrix produced by the qr factorization */
/* of the final approximate jacobian, stored rowwise. */
/* lr is a positive integer input variable not less than */
/* (n*(n+1))/2. */
/* qtf is an output array of length n which contains */
/* the vector (q transpose)*fvec. */
/* wa1, wa2, wa3, and wa4 are work arrays of length n. */
/* subprograms called */
/* user-supplied ...... fcn */
/* minpack-supplied ... dogleg,dpmpar,enorm, */
/* qform,qrfac,r1mpyq,r1updt */
/* fortran-supplied ... dabs,dmax1,dmin1,mod */
/* argonne national laboratory. minpack project. march 1980. */
/* burton s. garbow, kenneth e. hillstrom, jorge j. more */
/* ********** */
/* Parameter adjustments */
--wa4;
--wa3;
--wa2;
--wa1;
--qtf;
--diag;
--fvec;
--x;
fjac_dim1 = ldfjac;
fjac_offset = 1 + fjac_dim1 * 1;
fjac -= fjac_offset;
--r;
/* Function Body */
/* epsmch is the machine precision. */
epsmch = dpmpar(1);
info = 0;
iflag = 0;
*nfev = 0;
*njev = 0;
/* check the input parameters for errors. */
if (n <= 0 || ldfjac < n || xtol < 0. || maxfev <= 0 || factor <=
0. || lr < n * (n + 1) / 2) {
goto TERMINATE;
}
if (mode == 2) {
for (j = 1; j <= n; ++j) {
if (diag[j] <= 0.) {
goto TERMINATE;
}
}
}
/* evaluate the function at the starting point */
/* and calculate its norm. */
iflag = (*fcn)(p, n, &x[1], &fvec[1], &fjac[fjac_offset], ldfjac, 1);
*nfev = 1;
if (iflag < 0) {
goto TERMINATE;
}
fnorm = enorm(n, &fvec[1]);
/* initialize iteration counter and monitors. */
iter = 1;
ncsuc = 0;
ncfail = 0;
nslow1 = 0;
nslow2 = 0;
/* beginning of the outer loop. */
for (;;) {
jeval = TRUE_;
/* calculate the jacobian matrix. */
iflag = (*fcn)(p, n, &x[1], &fvec[1], &fjac[fjac_offset], ldfjac, 2);
++(*njev);
if (iflag < 0) {
goto TERMINATE;
}
/* compute the qr factorization of the jacobian. */
qrfac(n, n, &fjac[fjac_offset], ldfjac, FALSE_, iwa, 1,
&wa1[1], &wa2[1], &wa3[1]);
/* on the first iteration and if mode is 1, scale according */
/* to the norms of the columns of the initial jacobian. */
if (iter == 1) {
if (mode != 2) {
for (j = 1; j <= n; ++j) {
diag[j] = wa2[j];
if (wa2[j] == 0.) {
diag[j] = 1.;
}
}
}
/* on the first iteration, calculate the norm of the scaled x */
/* and initialize the step bound delta. */
for (j = 1; j <= n; ++j) {
wa3[j] = diag[j] * x[j];
}
xnorm = enorm(n, &wa3[1]);
delta = factor * xnorm;
if (delta == 0.) {
delta = factor;
}
}
/* form (q transpose)*fvec and store in qtf. */
for (i = 1; i <= n; ++i) {
qtf[i] = fvec[i];
}
for (j = 1; j <= n; ++j) {
if (fjac[j + j * fjac_dim1] != 0.) {
sum = 0.;
for (i = j; i <= n; ++i) {
sum += fjac[i + j * fjac_dim1] * qtf[i];
}
temp = -sum / fjac[j + j * fjac_dim1];
for (i = j; i <= n; ++i) {
qtf[i] += fjac[i + j * fjac_dim1] * temp;
}
}
}
/* copy the triangular factor of the qr factorization into r. */
sing = FALSE_;
for (j = 1; j <= n; ++j) {
l = j;
jm1 = j - 1;
if (jm1 >= 1) {
for (i = 1; i <= jm1; ++i) {
r[l] = fjac[i + j * fjac_dim1];
l = l + n - i;
}
}
r[l] = wa1[j];
if (wa1[j] == 0.) {
sing = TRUE_;
}
}
/* accumulate the orthogonal factor in fjac. */
qform(n, n, &fjac[fjac_offset], ldfjac, &wa1[1]);
/* rescale if necessary. */
if (mode != 2) {
for (j = 1; j <= n; ++j) {
/* Computing MAX */
d1 = diag[j], d2 = wa2[j];
diag[j] = max(d1,d2);
}
}
/* beginning of the inner loop. */
for (;;) {
/* if requested, call fcn to enable printing of iterates. */
if (nprint > 0) {
iflag = 0;
if ((iter - 1) % nprint == 0) {
iflag = (*fcn)(p, n, &x[1], &fvec[1], &fjac[fjac_offset], ldfjac, 0);
}
if (iflag < 0) {
goto TERMINATE;
}
}
/* determine the direction p. */
dogleg(n, &r[1], lr, &diag[1], &qtf[1], delta, &wa1[1], &wa2[1], &wa3[1]);
/* store the direction p and x + p. calculate the norm of p. */
for (j = 1; j <= n; ++j) {
wa1[j] = -wa1[j];
wa2[j] = x[j] + wa1[j];
wa3[j] = diag[j] * wa1[j];
}
pnorm = enorm(n, &wa3[1]);
/* on the first iteration, adjust the initial step bound. */
if (iter == 1) {
delta = min(delta,pnorm);
}
/* evaluate the function at x + p and calculate its norm. */
iflag = (*fcn)(p, n, &wa2[1], &wa4[1], &fjac[fjac_offset], ldfjac, 1);
++(*nfev);
if (iflag < 0) {
goto TERMINATE;
}
fnorm1 = enorm(n, &wa4[1]);
/* compute the scaled actual reduction. */
actred = -1.;
if (fnorm1 < fnorm) {
/* Computing 2nd power */
d1 = fnorm1 / fnorm;
actred = 1. - d1 * d1;
}
/* compute the scaled predicted reduction. */
l = 1;
for (i = 1; i <= n; ++i) {
sum = 0.;
for (j = i; j <= n; ++j) {
sum += r[l] * wa1[j];
++l;
}
wa3[i] = qtf[i] + sum;
}
temp = enorm(n, &wa3[1]);
prered = 0.;
if (temp < fnorm) {
/* Computing 2nd power */
d1 = temp / fnorm;
prered = 1. - d1 * d1;
}
/* compute the ratio of the actual to the predicted */
/* reduction. */
ratio = 0.;
if (prered > 0.) {
ratio = actred / prered;
}
/* update the step bound. */
if (ratio < p1) {
ncsuc = 0;
++ncfail;
delta = p5 * delta;
} else {
ncfail = 0;
++ncsuc;
if (ratio >= p5 || ncsuc > 1) {
/* Computing MAX */
d1 = pnorm / p5;
delta = max(delta,d1);
}
if (fabs(ratio - 1.) <= p1) {
delta = pnorm / p5;
}
}
/* test for successful iteration. */
if (ratio >= p0001) {
/* successful iteration. update x, fvec, and their norms. */
for (j = 1; j <= n; ++j) {
x[j] = wa2[j];
wa2[j] = diag[j] * x[j];
fvec[j] = wa4[j];
}
xnorm = enorm(n, &wa2[1]);
fnorm = fnorm1;
++iter;
}
/* determine the progress of the iteration. */
++nslow1;
if (actred >= p001) {
nslow1 = 0;
}
if (jeval) {
++nslow2;
}
if (actred >= p1) {
nslow2 = 0;
}
/* test for convergence. */
if (delta <= xtol * xnorm || fnorm == 0.) {
info = 1;
}
if (info != 0) {
goto TERMINATE;
}
/* tests for termination and stringent tolerances. */
if (*nfev >= maxfev) {
info = 2;
}
/* Computing MAX */
d1 = p1 * delta;
if (p1 * max(d1,pnorm) <= epsmch * xnorm) {
info = 3;
}
if (nslow2 == 5) {
info = 4;
}
if (nslow1 == 10) {
info = 5;
}
if (info != 0) {
goto TERMINATE;
}
/* criterion for recalculating jacobian. */
if (ncfail == 2) {
goto TERMINATE_INNER_LOOP;
}
/* calculate the rank one modification to the jacobian */
/* and update qtf if necessary. */
for (j = 1; j <= n; ++j) {
sum = 0.;
for (i = 1; i <= n; ++i) {
sum += fjac[i + j * fjac_dim1] * wa4[i];
}
wa2[j] = (sum - wa3[j]) / pnorm;
wa1[j] = diag[j] * (diag[j] * wa1[j] / pnorm);
if (ratio >= p0001) {
qtf[j] = sum;
}
}
/* compute the qr factorization of the updated jacobian. */
r1updt(n, n, &r[1], lr, &wa1[1], &wa2[1], &wa3[1], &sing);
r1mpyq(n, n, &fjac[fjac_offset], ldfjac, &wa2[1], &wa3[1]);
r1mpyq(1, n, &qtf[1], 1, &wa2[1], &wa3[1]);
/* end of the inner loop. */
jeval = FALSE_;
}
TERMINATE_INNER_LOOP:
;
/* end of the outer loop. */
}
TERMINATE:
/* termination, either normal or user imposed. */
if (iflag < 0) {
info = iflag;
}
if (nprint > 0) {
(*fcn)(p, n, &x[1], &fvec[1], &fjac[fjac_offset], ldfjac, 0);
}
return info;
/* last card of subroutine hybrj. */
} /* hybrj_ */
